基于激光雷达的移动机器人同步定位与地图构建方法

摘要

本发明公开的基于激光雷达的移动机器人同步定位与地图构建方法，其特征在于，包括以下步骤：步骤1、点云匹配；步骤2、抽样；步骤3、更新权重；步骤4、重采样；步骤5、更新地图。本发明基于激光雷达的移动机器人同步定位与地图构建方法，着重提供良好的点云初始状态，从采样点的选取和匹配策略方面对激光雷达快速点云配准算法进行了改进，利用主成分分析法对相邻帧之间的点云进行粗配准，再采用改进点到线迭代最近点配准算法校正粗配准结果完成精确配准。同时在重采样算法中，在多次复制大权重粒子集合的情况下引入小权重粒子集合，改善粒子多样性缺乏，提高了移动机器人定位精度。

说明书

技术领域

本发明属于移动机器人定位方法技术领域，具体涉及一种基于激光雷达的移动机器人同步定位与地图构建方法。

背景技术

智能化机器人在面临复杂不确定地形环境如何确定自身位置，提供优质的地图具有重要的理论研究价值和实际应用意义。同步定位与地图构建(SLAM：SimultaneousLocalization And Mapping)利用机器人携带的传感器在未知环境下观测目标，根据机器人与目标之间的相对位置和里程信息来估计机器人与目标点的全局坐标。SLAM问题一直是机器人研究中的一个基础且重要的开放性研究热点。

求解SLAM问题的方法大致可分为基于概率估计的方法和基于非概率估计的方法两大类。基于概率估计的方法主要有扩展卡尔曼滤波(EKF：Extended Kalman Filter)、FastSLAM与粒子滤波(PF：Particle Filter)等。粒子滤波的优点在于解决如状态递归估计或高维非线性、非高斯时变系统的概率推理等复杂问题。粒子滤波作为一种有效解决SLAM问题的方法仍然存在粒子退化问题。后来，Murphy等人提出采用Rao-Blackwillisedparticle filter(RBPF)求解SLAM问题。

RBPF-SLAM是一种基于粒子滤波的SLAM算法，利用粒子来表示机器人的位置和姿态。它广泛应用于机器人同步定位和地图构建。SLAM问题可以描述为“估计”-“观测”-“校正”过程：机器人在一个未知环境中从未知的初始位置，利用系统的运动模型和里程计获取自身的运动信息和系统的观测模型，感知环境信息来估计机器人的轨迹，构建增量的环境地图。

发明内容

本发明的目的在于提供一种基于激光雷达的移动机器人同步定位与地图构建方法，解决了现有SLAM算法中移动机器人定位精度低、构建地图不准确的问题。

本发明所采用的技术方案是：基于激光雷达的移动机器人同步定位与地图构建方法，包括以下步骤：

步骤1、点云匹配：当机器人在移动过程中获得来自激光雷达的扫描数据时，采用点到线的改进ICP配准算法，对机器人位姿进行精确估计；

步骤2、抽样：对上一代第i个粒子

步骤3、更新权重：根据权重计算公式计算出每个粒子

步骤4、重采样：当Neff

步骤5、更新地图：根据第i个粒子表示的移动机器人位姿和当前观测情况，对第i个粒子构建的地图进行更新。

本发明的特点还在于，

步骤1中点到线的改进ICP配准算法的具体步骤为：

步骤1.1、设置距离误差阈值δ；

步骤1.2、将当前帧的点云数据根据初始位姿投影到参考坐标系中；

步骤1.3、对于点云中的每一个点p

步骤1.4、通过下式(4)计算配准误差：

式(4)中，索引i和j分别代表点云p和q中的点，R和T分别为旋转和平移矩阵，n

步骤1.5、当配准误差E小于误差阈值δ，停止配准迭代，否则返回步骤1.2。

步骤2中对上一代第i个粒子

式(5)中，

步骤3中根据权重计算公式式(6)计算出每个粒子

式(6)中，

步骤4具体为：

步骤4.1、设定重采样门限阈值N

步骤4.2、对权重大于阈值w

步骤4.3、对权重小于阈值w

步骤4.4、分别对步骤4.2和步骤4.3产生的两部分粒子进行排序，得到新的粒子集

步骤4.5、对步骤4.4得到的新的粒子集进行权重归一化后对其进行后验均值估计，即得到粒子滤波估计的最终状态。

本发明的有益效果是：本发明基于激光雷达的移动机器人同步定位与地图构建方法，着重提供良好的点云初始状态，从采样点的选取和匹配策略方面对激光雷达快速点云配准算法进行了改进，利用主成分分析法对相邻帧之间的点云进行粗配准，再采用改进点到线迭代最近点配准算法校正粗配准结果完成精确配准。同时在重采样算法中，在多次复制大权重粒子集合的情况下引入小权重粒子集合，改善粒子多样性缺乏，提高了移动机器人定位精度。

附图说明

图1是本发明基于激光雷达的移动机器人同步定位与地图构建方法中粒子滤波重采样算法的流程示意图；

图2a)是本发明实施例中原始点云分布图，图2b)是本发明实施例中传统ICP配准后点云分布图，图2c)是本发明实施例中改进ICP配准后点云分布图；

图3是本发明实施例中改进重采样粒子滤波、MSV算法和传统重采样粒子滤波三种算法滤波状态估计曲线图；

图4是本发明实施例中改进重采样粒子滤波、MSV算法和传统重采样粒子滤波三种算法滤波均方根误差曲线图；

图5a)是本发明实施例中传统重采样滤波后粒子分布图，图5b)是本发明实施例中MSV算法滤波后粒子分布图，图5c)是本发明实施例中改进重采样滤波后粒子分布图；

图6a)是本发明实施例中地图构建的室外走廊真实环境，图6b)是本发明实施例中地图构建的室内真实环境；

图7a)是本发明实施例中传统重采样粒子滤波算法在实验室外走廊环境下构建地图效果图，图7b)是本发明实施例中MSV算法在实验室外走廊环境下构建地图效果图，图7c)是本发明实施例中改进重采样与点云配准算法在实验室外走廊环境下构建地图效果图；

图8a)是本发明实施例中传统重采样粒子滤波算法在实验室环境下构建地图效果图，图8b)是本发明实施例中MSV算法在实验室环境下构建地图效果图，图8c)是本发明实施例中改进重采样与点云配准算法在实验室环境下构建地图效果图。

具体实施方式

下面结合附图以及具体实施方式对本发明进行详细说明。

本发明提供了一种基于激光雷达的移动机器人同步定位与地图构建方法，具体按照如下步骤实施：

步骤1，点云匹配：当机器人在移动过程中获得来自激光雷达的扫描数据时，采用改进点云配准算法，对机器人位姿进行精确估计。

ICP算法配准的目标是通过一个三维变换矩阵将源点云集p＝{p

求解误差函数的最小值，获得下一次位姿变换估计值：

上式中

PCA(Principal Component Analysis)是一种有效的简化检测数据集方法，可以减少数据集维数，反映数据集对方差贡献的最大特征。因此，相似度大的两片点云，通过PCA算法只要把其参考坐标系调整到一致，即可达到粗配准目的，粗配准为点到线ICP点云精配准提供了良好的点云初始变换。

传统ICP配准算法采用两点间欧氏距离最近点作为配准点，易受到噪声的干扰。因此，本发明在对应点匹配中，将最近的点度量从两点之间的欧氏距离改变为到两点连线的垂直距离，对点云粗配准结果进行点到线ICP算法精确配准校正以提高匹配的精度和收敛速度，实现移动机器人的精确定位。点到线ICP解决曲面匹配问题，可以表示如下：给出一个参考S

上式中n

基于点到线的改进ICP配准算法步骤：

步骤1.1，设置距离误差阈值δ，改进算法采用点到线的距离作为误差；

步骤1.2，将当前帧的点云数据根据初始位姿投影到参考坐标系中；

步骤1.3，对于点云中的每一个点p

步骤1.4，计算配准误差，误差计算采用下式：

步骤1.5，当误差小于误差阈值δ，停止配准迭代，否则返回步骤1.2。

步骤2，抽样：对上一代第i个粒子

式(5)中，

步骤3，更新权重：根据权重计算公式计算出每个粒子

式(6)中，

步骤4，如图1所示，重采样：当Neff

优化的重采样算法对于粒子滤波来说既可保持样本的有效性和多样性，还可提高滤波器的性能。传统重采样方法完全丢弃了权值较小的粒子,多次迭代后削弱了粒子的多样性，导致收敛精度不足。针对该问题本发明提出一种改进重采样方法，将所有粒子按权值进行分类，根据预先设定的粒子权值阈值wt，在大权重和小权重粒子集合内分别进行采样，最后对新的粒子集进行权重归一化，进入下一步滤波过程。与传统重采样方法相比，改进重采样着重于在大权重集合内进行采样，大量复制粒子，同时在小权重集合内采用随机重采样方法获取粒子，在保证高权重粒子数的同时低权重粒子的选取机会增大，增加粒子多样性，进而提高机器人状态的估计精度。

改进重采样算法步骤如下：

步骤4.1，设定重采样门限阈值N

步骤4.2，对权重大于阈值w

步骤4.3，对权重小于阈值w

步骤4.4，分别对步骤4.2和步骤4.3产生的两部分粒子进行排序，得到新的粒子集

步骤4.5，对新的粒子集进行权重归一化，完成后面的滤波过程。

步骤5，更新地图：根据第i个粒子表示的移动机器人位姿和当前观测情况，对第i个粒子构建的地图进行更新。

为验证本发明的有益效果：从算法配准精度、估计性能以及算法运行时间进行了性能仿真分析，同时将本发明算法与传统重采样传统粒子滤波以及文献所提MSV算法进行了性能对比。

选取典型的单变量非平稳模型进行仿真分析，该模型具有典型的非线性特征，且后验分布具有双峰特性，其状态方程和观测方程分别为：

其中，k为状态时间，x(k)为状态向量，z(k)为观测向量，v(k-1)和n(k)是零均值高斯随机变量，其方差分别为Q(k-1)和R(k)。

对目标位置估计误差性能采用均方根误差(RMSE)评价：

其中x(k)和

用激光雷达数据集对原始ICP算法和本发明改进ICP算法进行点云匹配仿真实验，原点云数据与目标点云数据的配准结果如图2所示，图2(a)为原始点云分布，图2(b)为传统ICP配准后点云分布，图2(c)为本发明改进ICP算法配准后点云分布。由图2中箭头所指区域可知，图2(b)图中目标点云和源点云交叉部分较多，完全覆盖面积较少，图2(c)图中目标点云完全覆盖源云的面积较大，因此，本发明改进配准算法的点云配准准确度更高。在机器人定位过程中，优良的配准算法可以进一步提高激光雷达点云的配准精度，从而实现移动机器人的精确定位。

此外，本发明对传统ICP算法和改进ICP算法的运行时间进行了比较，对于相同点云数据集，传统ICP算法进行点云配准运行时间需要12.537s，而本发明不但改进了精配准算法，同时增加了粗配准，改进精配准算法与原始ICP配准算法相比，算法运行时间大大减小，因此加上粗配准时间依旧比原始ICP精配准算法运行时间短，总共时间为8.063s，比传统ICP算法直接配准时间节省了4.474s，由此说明改进算法的实时性效果更好。

对改进重采样粒子滤波、MSV算法和传统重采样粒子滤波三种算法进行了性能对比。仿真中Q(k-1)和R(k)分别取10和1，状态初始值x(0)＝0.1，N取100次，粒子数设定为100。图3是三种粒子滤波算法对状态估计值的仿真对比，图4是三种粒子滤波算法状态估计均方根误差。由图3和图4可知，本发明提出的改进重采样粒子滤波状态估计值与真实值接近，均方根误差均小于其它两种算法，说明本发明所提算法提高了粒子的多样性和估计精度。

图5(a)、图5(b)、图5(c)依次为传统重采样算法、MSV算法和本发明改进重采样粒子滤波三种算法滤波后粒子分布情况。由图5可知改进重采样粒子滤波算法拟合精度高，大部分粒子分布紧密靠近真值，只存在个别粒子样本发散，可以有效提高地图估计精度。

为了进一步分析改进算法的实时性，对传统重采样、MSV算法和本发明改进重采样粒子滤波三种算法在相同初始粒子数情况下进行多次仿真实验，统计算法运行时间均值，如表1所示。可以看出，在相同粒子个数下，改进算法的运行时间低于传统算法以及MSV算法，验证了本发明所改进重采样粒子滤波算法在地图构建具有一定的实时性。

表1 三种算法运行时间对比

Table 1 Comparison of running time between traditional algorithm andimproved algorithm

在实际情况下，机器人的工作环境比仿真环境更加复杂，环境特征更加丰富，存在行人等不确定因素，为了验证改进算法在这些情况下的有效性，分别采用传统重采样算法、MSV算法、本发明改进重采样算法以及融合本发明改进重采样与配准算法，基于Turtlebot机器人对西安理工大学教五楼室内和室外走廊环境分别进行了SLAM试验。构建地图时所需的粒子个数如表2所示，每个环境重复10次试验。实验场景一是室外走廊，实验场景二为室内，由于室内障碍物较多环境复杂，所以构建地图所需粒子数多于室外所需粒子数。

表2 构建地图所需粒子数

Table 2 Number of particles required to construct a map

图6(a)和图6(b)依次为进行地图构建的室外走廊和室内真实环境。采用RBPF-SLAM算法构建的网格图如图7和图8所示。由图7和图8圈标记地方可以看出，本发明所提改进算法构建出来的地图与真实场景一致，原算法构建的地图在拐角处存在偏移和重影的问题，而改进算法构建的地图更清晰、更整齐。对比图7(a)、图7(b)和图7(c)，图7(a)中传统算法没有构建出障碍物完整的形状，图7(b)中MSV算法对障碍物的构建不够完整，地图中灭火器和电梯口位置构建不清晰，而图7(c)中本发明改进重采样与点云配准算法构建的地图信息更加精确的反映环境信息，线条较为清晰，表示障碍物对应所在位置的单元栅格状态更为确定，同时对于电梯口等拐角位置信息的构建更加完整。

由图8中间部分箭头所标注的圈为室内摆放的障碍物，可以看出室内靠墙两边的书桌位置以及中间矩形障碍物的形状被清晰的构建出来，图8(c)所构建的障碍物效果明显优于图8(a)和图8(b),所以本发明所提改进算法对于障碍物的构建效果提升也很明显，可以建立正确且一致性良好的环境地图，算法的定位精度得到了一定程度上的改善。因为原始SLAM算法由于在粒子滤波重采样过程中，注重复制大权值粒子和频繁的重采样导致粒子多样性丧失、粒子退化，导致机器人位姿与估计位姿不重合，构建地图过程中出现地图倾斜与墙体重影，MSV算法采用计算最小采样方差的方式，使得重采样前后粒子分布保持一致，差异达到最小化，来避免一些权值较小但是可能是真实值的粒子被删除，缓解了样本贫化问题，但是计算复杂度增加，粒子分布比较集中，只能达到有限精度，而改进算法克服了原始算法和MSV算法的缺点，并且由表2可以看出，改进算法用于地图构建，用更少的粒子数，构建出更加精确的地图，减小了算法计算复杂度，进一步验证了改进算法构建环境地图的有效性。