CN111123279B - 一种融合nd和icp匹配的移动机器人重定位方法 - Google Patents

Abstract

本发明适用于机器人定位技术领域，提供了一种融合ND和IPC匹配的移动机器人重定位方法，包括：对激光帧初始点云进行滤波，获得激光帧点云S；基于地图点云的ND模型计算撒阵列粒子集中各撒阵列粒子的ND概率得分，提取前m个最高ND概率得分的撒阵列粒子；基于m个撒阵列粒子构建矩阵集T2，基于各矩阵将激光帧点云S换到栅格地图，得到m个激光帧点云S″；将m个激光帧点云S″和分别地图点云M'进行ICP匹配，输出前n个误差较小的撒阵列粒子及对应的变换矩阵T_e；计算n个撒阵列粒子的ND概率得分，输出最高得分撒阵列粒子对应的变换矩阵T_e，基于该变化矩阵T_e来计算机器人的当前位姿。仅通过用激光雷达进行定位，降低定位的成本的同时，实现了高精度的重定位。

Description

一种融合ND和ICP匹配的移动机器人重定位方法

技术领域

本发明属于机器人定位技术领域，提供了一种融合ND和ICP匹配的移动机器人重定位方法。

背景技术

随着移动机器人在服务业和仓储物流领域有着越来越广泛的应用，其自主定位导航技术也越发重要。现主要的技术有磁导航、视觉导航和激光导航，其中磁导航工作轨迹固定，路径需要铺设电磁导线或磁条。视觉导航轨迹较为自由，但是环境中多使用二维码或特殊图形码等标记物，还需要定期维护。激光导航定位精准，路径灵活多变，适应多种现场环境，因此成为了移动机器人主流的定位导航方式，但是也显露出一系列问题，如机器人的重启或关机、打滑漂移和人为移动等，导致机器人绑架和定位失败问题，此时重定位技术就显得越发重要。

为了解决上述问题，现有的方案主要包含如下三种：3)AMCL定位。在地图中生成随机粒子，计算粒子权重，重采样剔除权重低的粒子，保存和复制权重高的粒子，多次迭代后粒子群收敛于真实机器人位姿。2)机器人回到初始位置。定位失败后，人为将机器人推到初始位置，然后重新开机。3)人为标记机器人新位姿，根据导航地图和真实环境，人为标记机器人的大致位置。这些定位方法都需要人工参与，无法保证重定位的精度。

发明内容

本发明实施例提供一种融合ND和ICP匹配的移动机器人重定位方法，仅通过激光雷达来实现了高精度的重定位。

本发明是这样实现的，一种融合ND和ICP匹配的移动机器人重定位方法，所述方法具体包括如下步骤：

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

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

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

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

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

进一步的，撒阵列粒子的ND概率得分获取方法具体如下：

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

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

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

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

进一步的，地图点云的ND模型的构建方法具体包括如下步骤：

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

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

进一步的，障碍物在地图中的坐标的获取过程具体如下：

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

x＝c*res+origin_x

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

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

本发明提供的融合ND和ICP匹配的移动机器人重定位方法具有如下有益效果：仅通过用激光雷达进行定位，不依赖其它传感器和安装环境，降低定位的成本的同时，实现了高精度的重定位。

附图说明

图1为本发明实施例提供的融合ND和ICP匹配的移动机器人重定位方法流程图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

本发明要解决的是机器人出现打滑漂移、人为移动、重启或关机导致的定位失败问题，实现移动机器人的高精度定位。

图1为本发明实施例提供的融合ND和ICP匹配的移动机器人重定位方法流程图，该方法具体包括如下步骤：

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

获取当前原始激光帧，使用统计概率滤波器和体素滤波器剔除点云中的离群点，降低点云密度，得到激光帧点云S。

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

在本发明实施例中，撒阵列粒子集的获取方法具体如下：撒阵列粒子集中的撒阵列粒子是机器人在地图中的假设位置，机器人在导航地图中应处于白色的自由区域，并且周围0.3米范围内没有黑色障碍物；该步骤遍历栅格导航地图中的像素点，步进值为预设的阵列点距离值，如果当前像素值≥254，即且该像素点的周围距离阈值内(0.3米)的像素值均≥254，将此点像素坐标保存到撒阵列粒子集中。

S3、基于地图点云的ND模型来计算各帧激光帧点云S'的ND概率得分，提取前m个最高ND概率得分对应的撒阵列粒子，点云地图中标注了障碍物的地图坐标；

在本发明实施例中，地图点云的ND模型的构建方法具体包括如下步骤：

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

将栅格导航地图转化为点云地图M：遍历栅格导航地图像素，栅格导航地图为8位单通道灰度图像，根据像素值分为三种状态：0代表的黑色障碍物区域，像素值>＝254代表的白色自由区域，像素值小于254并大于0代表的灰色空闲区域。如果像素值为0，使用如下公式进行坐标变换，将像素坐标(r,c)转化为地图坐标(x,y)，把点添加到点云地图M，坐标变换过程具体如下：

x＝c*res+origin_x

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

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

地图点云滤波：地图点云M的点数多、密度大，还存在明显的一些离群点，容易造成误匹配和匹配效率低,因此使用统计概率滤波器和体素滤波提，剔除点云地图M中的离群点，并且在保证点云微观特征的基础上降低点云的密度，提高匹配效率，最终得到点云地图M'。

S32、针对点云地图M'来构建地图点云的ND模型，即二维正态分布概率模型，其构建方法具体如下：

将点云地图M'栅格化，计算每个栅格内点集q_i的均值μ_i和协方差Σ_i。

其中，i为栅格点的数量。基于均值μ_i和协方差Σ_i得到栅格q的二维正态分布概率密度函数P_i，二维正态分布概率密度函数P_i的计算公式具体如下：

在本发明实施例中，各激光帧点云S'的ND概率得分计算过程具体如下：

遍历激光帧点云S'的点集，判断每个激光点所在的栅格，计算各栅格的二维正态分布概率值，各栅格的二维正态分布概率值累乘得到激光帧点云S'与点云地图M'的匹配总得分，匹配总得分采用似然函数来计算，似然函数值最大的m个撒阵列粒子作为初步最优位姿，具体似然函数公式如下：

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

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

将m个激光帧点云S”分别与地图点云M'进行ICP匹配，单个激光帧点云S”与地图点云M'的ICP匹配过程具体如下：

首先分别计算激光帧点云S”及地图点云M'点云的重心，然后去重心化得到激光帧点云S1和地图点云M1，在地图点云M1分别查找离激光帧点云S1各点最近的点，所有的点构成地图点云M2，将地图点云M2和激光帧点云S1中的点分别定义为m_2i和s_i，i∈[0,N]，激光帧点云S1和地图点云M2中都存在N个点，地图点云M2和激光帧点云S1中所有点的均值分别为m_2mean,s_mean；

构建最小二乘目标函数

使用SVD分解求解最优变换R，t＝m_2mean-Rs_mean，由R和t构建变换矩阵T_e，将激光帧点云S1通过矩阵T_e变换为AL对齐点云，最后计算激光帧点云S1和地图点云M1的匹配误差

如果误差不满足阈值要求，重新进行上述匹配过程，如果满足阈值，停止迭代，返回AL对齐点云、误差error和变换矩阵T_e。

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

基于n个撒阵列粒子来构建矩阵集T3，阵集T3中含有n个矩阵，基于各矩阵将激光帧点云S换到栅格导航地图上，形成n个激光帧点云S″′，遍历激光帧点云S″′的点集，计算各激光帧点云S″′的ND概率得分，输出最高ND概率得分中对应的撒阵列粒子，及其对应的变换矩阵T_e，最优的位姿矩阵即为Te*T2，转化为向量得到机器人的位姿(x,y,theta)。

本发明提供的融合ND和ICP匹配的移动机器人重定位方法具有如下有益效果：仅通过用激光雷达进行定位，不依赖其它传感器和安装环境，降低定位的成本的同时，实现了高精度的重定位。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (4)

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)为地图坐标系的偏移值。

Images