[发明专利]一种融合ND和ICP匹配的移动机器人重定位方法

权利要求书

1.一种融合ND和ICP匹配的移动机器人重定位方法，其特征在于，所述方法具体包括如下步骤：

S1、获取激光帧的初始点云，并对激光帧初始点云进行滤波，获得激光帧点云S；

S2、获取栅格导航地图的撒阵列粒子集，基于地图点云的ND模型计算撒阵列粒子集中各撒阵列粒子的ND概率得分，提取前m个最高ND概率得分的撒阵列粒子；

S3、基于m个撒阵列粒子构建矩阵集T2，基于各矩阵将激光帧点云S换到栅格地图，得到变换后的m个激光帧点云S″；

S4、将m个激光帧点云S”和点云地图M'进行ICP匹配，输出前n个误差较小的撒阵列粒子、对应的变换矩阵T_e；

S5、计算n个撒阵列粒子的ND概率得分，输出最高得分撒阵列粒子对应的变换矩阵T_e，基于该变化矩阵T_e来计算机器人的当前位姿；

撒阵列粒子集的获取方法具体如下：

遍历栅格导航地图中的像素点，若当前像素值≥254，即且所述像素点的周围距离阈值内的像素值均≥254，将所述像素坐标作为撒阵列粒子保存到撒阵列粒子集。

2.如权利要求1所述融合ND和ICP匹配的移动机器人重定位方法，其特征在于，撒阵列粒子的ND概率得分获取方法具体如下：

基于撒阵列粒子集来构建矩阵集T1，矩阵集T1中包括K个矩阵，基于各矩阵将激光帧点云S换到栅格导航地图上，形成K个激光帧点云S'；

基于地图点云的ND模型来计算各帧激光帧点云S'的ND概率得分，即为帧激光帧点云S'对应撒阵列粒子的ND概率得分。

3.如权利要求1或2所述融合ND和ICP匹配的移动机器人重定位方法，其特征在于，地图点云的ND模型的构建方法具体包括如下步骤：

S31、将栅格导航地图转化为点云地图M，即获取障碍物在地图中的坐标，并对点云地图M进行滤波，生成滤波后的点云地图M'；

S32、针对点云地图M'来构建地图点云的ND模型，即二维正态分布概率模型。

4.如权利要求1或2所述融合ND和ICP匹配的移动机器人重定位方法，其特征在于，障碍物在地图中的坐标的获取过程具体如下：

遍历栅格导航地图像素，将像素值为0的像素点通过公式(1)和公式(2)进行坐标变换，即将像素坐标(r,c)转化为地图坐标(x,y)，把点添加到点云地图M，坐标变换过程具体如下：

x＝c*res+origin_x (1)

y＝(height-r-1)*res+origin_y (2)

其中，res为地图分辨率，height为像素表示的方形地图宽度，(origin_x,origin_y)为地图坐标系的偏移值。