CN111060888B - 一种融合icp和似然域模型的移动机器人重定位方法 - Google Patents

Abstract

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

Description

一种融合ICP和似然域模型的移动机器人重定位方法

技术领域

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

背景技术

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

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

发明内容

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

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

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

S2、获取栅格导航地图的撒阵列粒子集，撒阵列粒子集存在K个撒阵列粒子，基于撒阵列粒子集来构建矩阵集T1，矩阵集T1中包括K个矩阵，赋予矩阵集T1中各矩阵一系列的旋转角，形成旋转矩阵集T2，基于各旋转矩阵将激光帧点云S变换到栅格导航地图，形成K个激光帧点云S'，各旋转矩阵对应为机器人各种位姿；

S3、通过似然域模型计算出各位姿的概率得分，提取最高概率得分的m个位姿；

S4、将m个位姿对应的旋转矩阵构建旋转矩阵集T2′，基于各旋转矩阵将激光帧点云S变换到栅格导航地图，形成m个激光帧点云S″

S5、将m个激光帧点云S″分别与地图点云M'进行ICP匹配，提取前n个最小误差旋转矩阵对应位姿、及其对应的AL对齐点云及变换矩阵Tc；

S6、通过似然域模型计算n个位姿的概率得分，获取最高得分位姿对应的变换矩阵Tc，基于变换矩阵Tc来计算机器人当前的位姿。

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

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

进一步的，点云地图的获取方法具体如下：

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

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

检测像素值为0的像素点，基于如下公式进行坐标变换，将像素坐标(r,c)转化为地图坐标(x,y)，把点添加到点云地图M，坐标变换过程具体如下：

x＝c*res+origin_x

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

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

进一步的，其特征在于，位姿的概率得分获取方法具体如下：

基于位姿对应的旋转矩阵将激光帧点云S转换到栅格导航地图，形成新激光帧点云；

基于似然域模型计算新激光帧点云中单点的概率得分，对概率得分进行n次方指数放大(例如更新前的概率得分为p_i，更新后的概率得分为p_i ⁿ)，更新新激光帧点云中单点的概率得分，将新激光帧点云中各激光点的概率得分进行累加，即获取新激光帧点云对应位姿的概率得分。

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

附图说明

图1为本发明实施例提供的融合ICP和似然域模型的移动机器人重定位方法流程图。

具体实施方式

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

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

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

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

获取当前原始激光帧，使用统计概率滤波器和体素滤波器剔除点云中的离群点，降低点云密度，得到激光帧点云S，激光雷达采样率一般很高，一帧激光点云有数千个点，在似然域和ICP匹配过程中很耗计算时间，使用体素滤波器，在保证点云微观特征的基础上降低点云的密度，提高匹配效率。

S2、获取栅格导航地图的撒阵列粒子集，撒阵列粒子集存在K个撒阵列粒子，基于撒阵列粒子集来构建矩阵集T1，矩阵集T1中包括K个矩阵，基于设定的角度步进值Δδ∈[0,π/2]，从0度到360度赋予矩阵集T1中各矩阵一系列的旋转角，将矩阵集T1转化为旋转矩阵集T2，基于各旋转矩阵将激光帧点云S变换到栅格导航地图，形成激光帧点云S'，各旋转矩阵对应为机器人各种位姿，筛选出最优位姿；

在本发明实施例中，撒阵列粒子集的构建方法具体如下：

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

S3、通过似然域模型计算出各位姿的似然域概率得分，提取最高概率得分的m个位姿；

在本发明实施例中，似然域模型假设激光雷达测量有3种情形：随机噪声、检测失败和随机测量。对随机噪声使用测量为z，概率值为exp(-(z*z)/2σ²)的高斯分布建模；随机测量使用一个测量范围在[0,Z_max]，概率值为1/Z_max的均匀分布建模；检测失败使用一个以z_max为中心的概率值为p_max＝1很窄的均匀分布建模；通过公式(1)计算出一个激光帧点云S'中单激光点位姿的概率得分，由于点位姿的概率得分值过小，令各点位姿的概率得分p_z+＝p_z*p_z*p_z，该激光帧点云S'所有激光点得分概率累加，即获取激光帧点云S'对应位姿的似然域概率得分，似然域概率得分p_z的计算公式如下：

p_z＝z_hit·exp(-(z*z)/2σ²)+z_rand/z_max   (1)

z_max、z_rand、z_hit分别是自设定的权重，分别对应最大测量概率的权重，随机测量和高斯测量，z表示激光帧点云S'的点距地图中最近障碍物的距离，σ为自设定的高斯分布的标准差，p_z是激光帧中一个激光点在地图中与地图障碍物匹配的概率，似然域模型的计算类似查表的过程，速度很快，因此设置相对较小的阵列间距，获取较多的地图阵列点，以获得更加精准的位姿。

S4、将m个位姿对应的旋转矩阵构建旋转矩阵集T2′，基于各旋转矩阵将激光帧点云S变换到栅格导航地图，形成m个激光帧点云S″；

S5、将m个激光帧点云S″分别和地图点云M'进行ICP匹配，提取前n个最小误差旋转矩阵对应位姿、及其对应的AL对齐点云及变换矩阵Tc；

在本发明实施例中，点云地图的获取方法具体包括如下步骤：

将栅格导航地图转化为点云地图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'。

在本发明实施例中，将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构建变换矩阵Tc，将激光帧点云S1通过矩阵Tc变换为AL对齐点云，最后计算激光帧点云S1和地图点云M1的匹配误差

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

S6、计算n个位姿的似然域概率得分，获取最高得分位姿对应的变换矩阵Tc，基于变换矩阵Tc来计算机器人当前的位姿。

将前n个最小误差位姿对应的旋转矩阵来构建旋转矩阵集T2″，将基于旋转矩阵集T2″中各旋转矩阵将激光帧点云S变换到栅格导航地图，形成n个激光帧点云S″′，计算激光帧点云S″′对应位姿的似然域概率得分，获取最高得分激光帧点云S″′对应的位姿，及其变换矩阵Tc，机器人最优的位姿矩阵即为Tc*T2′，由前面3x3的旋转矩阵解出机器人的旋转角yaw，其第4列为机器人的位置向量，得出最终重定位结果(x,y,yaw)。

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

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

Claims (5)

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

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

S2、获取栅格导航地图的撒阵列粒子集，撒阵列粒子集存在K个撒阵列粒子，基于撒阵列粒子集来构建矩阵集T1，矩阵集T1中包括K个矩阵，构建矩阵集T1的旋转矩阵集T2，基于各旋转矩阵将激光帧点云S变换到栅格导航地图，形成K个激光帧点云S'，各旋转矩阵对应为机器人各种位姿；

S3、通过似然域模型计算出各位姿的概率得分，提取最高概率得分的m个位姿；

S4、将m个位姿对应的旋转矩阵构建旋转矩阵集T2′，基于各旋转矩阵将激光帧点云S变换到栅格导航地图，形成m个激光帧点云S″

S5、将m个激光帧点云S″分别与地图点云M'进行ICP匹配，提取前n个最小误差旋转矩阵对应位姿、及其对应的AL对齐点云及变换矩阵Tc；

S6、通过似然域模型计算n个位姿的概率得分，获取最高得分位姿对应的变换矩阵Tc，基于变换矩阵Tc来计算机器人当前的位姿。

2.如权利要求1所述融合ICP和似然域模型的移动机器人重定位方法，其特征在于，撒阵列粒子集的构建方法具体如下：

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

3.如权利要求1所述融合ICP和似然域模型的移动机器人重定位方法，其特征在于，点云地图的获取方法具体如下：

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

4.如权利要求1所述融合ICP和似然域模型的移动机器人重定位方法，其特征在于，障碍物在地图中的坐标获取方法具体如下：

检测像素值为0的像素点，基于如下公式进行坐标变换，将像素坐标(r,c)转化为地图坐标(x,y)，把点添加到点云地图M，坐标变换过程具体如下：

x＝c*res+origin_x

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

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

5.如权利要求1至4任一权利要求所述融合ICP和似然域模型的移动机器人重定位方法，其特征在于，位姿的概率得分获取方法具体如下：

基于位姿对应的旋转矩阵将激光帧点云S转换到栅格导航地图，形成新激光帧点云；

基于似然域模型计算新激光帧点云中单点的概率得分，对概率得分进行n次方指数放大，更新新激光帧点云中单点的概率得分，将新激光帧点云中各激光点的概率得分进行累加，即获取新激光帧点云对应位姿的概率得分。

Images