CN108917759A - 基于多层次地图匹配的移动机器人位姿纠正算法 - Google Patents

Abstract

本发明公开了一种基于多层次地图匹配的移动机器人位姿纠正算法，属于机器人和计算机图形学技术领域。本发明所述算法利用同步定位与构图算法建立全局栅格地图，结合当前观测与栅格地图的匹配关系利用AMCL算法纠正航迹推算出的位姿，从而得到相对准确的全局位姿信息；同时，本发明把栅格地图转换为之对应的全局目标点云地图，将机器人实时观测的激光点云与目标点云地图配准，以进一步修正全局位姿。本发明所述方法可以获得准确的全局位姿信息，减少长距离定位误差积累，避免现有粒子滤波技术中因粒子空间有限导致求解的位姿不够准确以及ICP算法求解的精度与效率对初始位姿过于依赖的缺点，实现了高效且准确的位姿求解。

Description

基于多层次地图匹配的移动机器人位姿纠正算法

技术领域

本发明属于机器人和计算机图形学技术领域，具体涉及一种基于多层次地图匹配的移动机器人位姿纠正算法。

背景技术

随着人工智能的不断发展，机器人的应用越来越广泛。为保证机器人能适应不同复杂的环境，因此对机器人的智能化提出了更高的要求。移动机器人的自主导航定位技术是机器人技术的核心，而其中定位是要解决的首要问题，吸引了众多研究人员的热切关注。定位问题可以分为两个子问题，一是机器人初始位置已知的局部位置跟踪问题，二是初始位置未知的全局定位问题。局部位置跟踪可以通过采集里程计、惯性导航单元信息进行航迹推算来完成，但存在很大的误差积累。激光雷达、摄像头、GPS等传感器可以得到机器人的全局位姿信息，但是摄像头和GPS对环境依赖大，如，摄像头对光线要求较高，GPS要求周围遮挡物较少。激光雷达具有精度高、环境适应能力强等特点，被广泛应用于机器人领域。早期的机器人定位算法主要包括扩展卡尔曼滤波、马尔可夫定位、多假设跟踪、粒子滤波等算法。虽然上述算法可以获得机器人的定位信息，但是计算效率和定位精度不高，不适用于一些特殊的应用环境。例如，变电站巡检、工厂物体抓取等工作需要可靠的感知传感器和高精度定位算法。因此，对移动机器人高精度定位研究具有很大的意义。以往机器人定位研究工作主要有以下几种方法：

(1)Fox提出了可尔可夫定位算法，已知移动机器人采用动作和传感器观测求解机器人在某个环境空间的位姿可信度，从而求解出最好的位姿。

(2)Leonard提出把扩展卡尔曼滤波算法应用于移动机器人定位，它其实是马尔可夫定位的一种特殊情况，但用一阶矩和二阶矩表示置信度，即均值和协方差。它适用于地标一致性可以绝对肯定的情况。

(3)Dellaert提出了蒙特卡洛定位算法，通过把合适的概率运动和感知模型代入粒子滤波算法中，它使用粒子滤波估计机器人位姿的后验，适应用于局部定位和全局定位，也能解决部分机器人绑架问题。之后，有学者将自适应机制引入蒙特卡洛算法，减少了计算时间。

(4)Minguez提出采用迭代最近点算法估计移动机器人位移。此算法对里程计输出的位姿进行了有效的纠正，提高了移动机器人长距离移动的定位精度。

粒子滤波算法的运用可以对非线性非高斯问题进行评估，迭代最近点算法可以减少里程计误差的积累。以上方法都在一定程度上解决了机器人定位的问题，但依然存在很多局限，主要表现在：

(1)定位精度不高。由于车轮打滑、传感器温度漂移带来的误差，以上方法在利用二维激光的情况下只能得到一个粗略的位姿估计，不能满足一些高精度定位需求的场合。目前，机器人定位多数是基于概率的方法估计最优的机器人位姿。蒙特卡洛算法由于粒子数量有限、粒子退化、栅格地图分辨率有限等问题，造成了估计得到局部最优位姿，而错过了全局最优位姿。迭代最近点算法在有一个较优的初始解情况下，可以保证全局最优，但如果采用的相邻激光点云进行匹配，这样就不可避免的引出了全局误差积累。

(2)计算效率与定位精度不能兼顾。当以上方法单独使用时可以保证机器人运行的时效性，但是定位精度不高。如蒙特卡洛算法想要提高定位精度，最直接的方式就是增加粒子数，虽然增加粒子数可以提高位姿估计的精度，但会增加计算成本，影响机器人运行的流畅性。迭代最近点算法的引入，可以增加机器人定位的准确性，但如果初始迭代参数选择不准确、关联点对搜索算法效率不高、不能很好的剔除伪点对就会造成算法求解迭代次数增加，求解结果陷入局部最优，反而会降低机器人运行的效率。

发明内容

本发明的目的是解决现有的定位算法在求解机器人位姿存在较大误差，误差会随距离增加而积累，且难以保证实时性的问题，提供一种基于多层次地图匹配的移动机器人位姿纠正算法。

本发明所提出的技术问题是这样解决的：

一种基于多层次地图匹配的移动机器人位姿纠正算法，包括以下步骤：

步骤1.利用同步定位与构图(SLAM)算法建立机器人二维栅格地图；在机器人每次开机后，采用开源的计算机视觉的opencv函数库读取已经建好的栅格地图；

步骤2.栅格地图上的黑色像素点代表激光扫描到的物体，把相应的黑色像素点转化为图像标系下的激光点云数据格式；再将图像坐标系下的激光点云P转换至地图坐标系下，得到地图坐标系下的点云P′，即全局目标点云地图；

图像坐标系和地图坐标系之间的转换关系为：P′＝RP+t；其中R是图像坐标系到地图坐标系的旋转矩阵，t是地图坐标系原点在图像坐标系下的坐标；

步骤3.根据机器人运动学模型，利用里程计和惯性测量单元的数据融合，计算出机器人的航迹，从而得到一个没有全局信息的初始位姿估计；

步骤4.采用自适应蒙特卡洛定位(AMCL)算法将激光雷达扫描到的当前观测点云与栅格地图进行匹配，纠正机器人航迹推算出的位姿，从而得到机器人在栅格地图中的全局位姿；

步骤5.设定激光雷达的扫描间距和旋转角度差，激光雷达依次扫描并采集激光点云数据S_k，该点云位于机器人坐标系下；再采用AMCL算法输出的全局位姿作为激光点云S_k与全局目标点云地图P′的初始配准参数初始旋转矩阵和初始平移向量

步骤6.根据初始配准参数，将点云S_k和全局目标点云地图P′进行配准，得到最终纠正的机器人位姿，为旋转矩阵和平移向量

步骤6的具体步骤为：

步骤6-1.令当前迭代次数i＝1，根据和把机器人坐标系下的点云S_k旋转平移变换得到地图坐标系下的点云Q，并把点云Q和点云地图P′按照kd-tree的数据结构存储，Q＝{q_j,j＝1...N}，P′＝{p′_j,j＝1...N}，N是激光单次扫描得到点云总数；

步骤6-2.根据最近距离原则在Q中搜索p′_j的距离最近点q_j得到搜索关联点对(q_j,p′_j)；

根据最近互邻原则在点云地图P′中搜索q_j的最近点p″_j，判断p″_j与p″_j的距离是否小于预设条件d_m；

若搜索关联点对(q_j,p′_j)满足预设条件，则确定关联点对为(q_j,p′_j)；否则返回步骤6-2重新搜索；最终得到N_t对关联点对(q_m,p′_m)，m＝1…N_t，N_t为满足预设条件的关联点对的个数，每个关联点对的距离为

步骤6-3.对N_t对关联点对的距离进行排序，选择距离较小的N_o对关联点对(q_n,p′_n)，其中n＝1…N_o，N_o＝N_t×η，η为固定重叠率；

步骤6-4.基于奇异分解的方法，利用N_o对关联点对(q_n,p′_n)，计算并更新旋转矩阵和平移向量

步骤6-5.判断当前误差是否达到预设精度要求，如达到要求则进行下一步骤，end＝i；如未达到要求，令i＝i+1，返回至步骤6-1；

步骤7.把旋转矩阵转换为对应的四元数，最终得到纠正后的机器人在地图坐标系下的位置和角度。

本发明的有益效果是：

本发明对现有的机器人定位算法进行了改进，提出了基于多层次地图匹配的移动机器人位姿纠正算法。

(1)在定位精度上有很大提升。通过AMCL算法和改进的ICP算法将机器人实时观测分别与栅格地图和点云地图进行匹配，先后对机器人位姿进行纠正，减少了长距离定位误差积累，避免AMCL算法因粒子空间有限导致求解的位姿不够准确。改进的迭代最近点(ICP)算法中设计的伪点对剔除方法，提高了匹配的精度。所以本发明中采用的算法提高了机器人的全局位姿精度，拓宽了移动机器人的运用领域。

(2)在计算效率中有所改进，多种算法结合来纠正机器人位姿就必须考虑运行效率。本发明中采用改进的ICP算法间隔纠正AMCL算法输出的全局位姿，保证了两种算法组合运行的时效性。粒子数自适应的方式，从而减少概率估计的时间，且本发明中改进的ICP的初始迭代参数由AMCL算法计算得到，从而克服传统的ICP算法求解的精度与效率对初始位姿过于依赖的缺点，实现了高效且准确的位姿求解。

附图说明

图1为本发明所述基于多次层地图匹配的移动机器位姿纠正算法流程图；

图2为本发明中栅格地图转换为全局目标点云地图的效果图；

图3为本发明中里程计输出的位姿得到的点云效果图；

图4为本发明中AMCL算法输出的位姿得到的点云效果图；

图5为采用里程计输出的位姿结合传统的ICP算法得到的点云效果图；

图6为采用AMCL算法输出的位姿结合传统的ICP算法得到的点云效果图；

图7为本发明实施例采用多层次地图匹配算法得到的点云效果图；

图8为采用里程计输出的位姿和理想位姿绘制的轨迹图；

图9为采用AMCL算法输出的位姿和理想位姿绘制的轨迹图；

图10为本发明实施多层次地图匹配得到的位姿和理想位姿绘制的轨迹图。

具体实施方式

下面结合附图和实施例对本发明进行进一步的说明。

本实施例提供一种基于多层次地图匹配的移动机器人位姿纠正算法，本发明实施例采用HUSKY A200机器人进行算法验证，机器人内部装有ROS系统的工控机、运动控制模块、信息采集模块等，此外还搭载了LMS151的二维激光雷达、里程计、惯性测量单元等传感器。本发明实施例使用的栅格地图由机器人通过SLAM算法建成。机器人每前进30厘米或旋转30度采集一次数据，一共记录了17次数据，数据内容包括原始激光点云、里程计输出的位姿、AMCL算法输出的位姿。本发明进行了多次实验对比，最终验证了本发明提出的基于多层次地图匹配的移动机器位姿纠正算法相比现有的一些位姿纠正算法有明显的改进效果。算法流程图如图1所示，包括以下步骤：

步骤1.利用同步定位与构图(SLAM)算法建立机器人二维栅格地图；在机器人每次开机后，采用开源的计算机视觉的opencv函数库读取已经建好的栅格地图；

步骤2.栅格地图上的黑色像素点代表激光扫描到的物体，把相应的黑色像素点转化为图像标系下的激光点云数据格式；再将图像坐标系下的激光点云P转换至地图坐标系下，得到地图坐标系下的点云P′，即全局目标点云地图；栅格地图转换为全局目标点云地图的效果图如图2所示；

图像坐标系和地图坐标系之间的转换关系为：P′＝RP+t；其中R是图像坐标系到地图坐标系的旋转矩阵，图像坐标系到地图坐标系的旋转角度因为点云按照右手坐标系存储，所以要加负号，再转换为四元素表示x＝0，y＝0，由四元素和相应的转换函数得到旋转矩阵R；t是地图坐标系原点在图像坐标系下的坐标；其中r为图像的行数，c为图像的列数，d为图像分辨率；

通过以上步骤把栅格地图转换为全局目标点云地图，各参数的具体数值如下表所示：

步骤3.根据机器人运动学模型，利用里程计和惯性测量单元的数据融合，计算出机器人的航迹，从而得到一个没有全局信息的初始位姿估计；里程计输出的位姿得到的点云效果图如图3所示；

步骤4.采用自适应蒙特卡洛定位(AMCL)算法将激光雷达扫描到的当前观测点云与栅格地图进行匹配，纠正机器人航迹推算出的位姿，从而得到机器人在栅格地图中的全局位姿；AMCL算法输出的位姿得到的点云效果图如图4所示；

步骤5.设定激光雷达的扫描间距和旋转角度差，激光雷达依次扫描并采集激光点云数据S_k，该点云位于机器人坐标系下；再采用AMCL算法输出的全局位姿作为激光点云S_k与全局目标点云地图P′的初始配准参数初始旋转矩阵和初始平移向量

步骤6.根据初始配准参数，将点云S_k和全局目标点云地图P′进行配准，得到最终纠正的机器人位姿，为旋转矩阵和平移向量图5为采用里程计输出的位姿结合传统的ICP算法得到的点云效果图；图6为采用AMCL算法输出的位姿结合传统的ICP算法得到的点云效果图；

步骤6的具体步骤为：

步骤6-1.令当前迭代次数i＝1，根据和把机器人坐标系下的点云S_k旋转平移变换得到地图坐标系下的点云Q，并把点云Q和点云地图P′按照kd-tree的数据结构存储，Q＝{q_j,j＝1...N}，P′＝{p′_j,j＝1...N}，N是激光单次扫描得到点云总数；

步骤6-2.根据最近距离原则在Q中搜索p′_j的距离最近点q_j得到搜索关联点对(q_j,p′_j)；

根据最近互邻原则在点云地图P′中搜索q_j的最近点p″_j，判断p′_j与p″_j的距离是否小于预设条件d_m；

若搜索关联点对(q_j,p′_j)满足预设条件，则确定关联点对为(q_j,p′_j)；否则返回步骤6-2重新搜索；最终得到N_t对关联点对(q_m,p′_m)，m＝1…N_t，N_t为满足预设条件的关联点对的个数，每个关联点对的距离为

步骤6-3.对N_t对关联点对的距离进行排序，选择距离较小的N_o对关联点对(q_n,p′_n)，其中n＝1…N_o，N_o＝N_t×η，η为固定重叠率；

步骤6-4.基于奇异分解的方法，利用No对关联点对(q_n,p′_n)，计算并更新旋转矩阵和平移向量

步骤6-5.判断当前误差是否达到预设精度要求，如达到要求则进行下一步骤，end＝i；如未达到要求，令i＝i+1，返回至步骤6-1；

步骤7.把旋转矩阵转换为对应的四元数，最终得到纠正后的机器人在地图坐标系下的位置和角度。

在完成以上步骤后，得到了本发明提出的基于多层次地图匹配的移动机器位姿纠正算法的效果图，如图7所示。

下表给出了针对采集到的数据集(其中包含了扫描得到的二维激光点云数据和里程计数据和AMCL数据)。不同位姿纠正算法在运行时间上的对比。记录的时间是进行17次扫描匹配所用时间的总和。

名称	里程计+传统ICP	AMCL+传统ICP	多次层地图匹配
				运行时间	0.333s	0.309s	0.287s

由于本发明主要是纠正机器人位姿，所以对算法运行的效率要求较高。在本实验例中综合考虑了运用时间和匹配精度，改进的ICP算法选择的匹配点对数为50个，也可以根据不同环境和精度需求改变匹配对数、收敛条件等参数，使其达到实际工作要求。图3是通过里程计信息和惯性测量单元信息进行航迹推算得到的点云效果图，从图中圈出的部分可以发现推算出的位姿和全局地图偏差较大，且有些变形，说明机器人位姿出现了漂移。图4是通过AMCL算法结合栅格地图对航迹推算出的位姿进行纠正得到的点云效果图，从图中圈出的部分可以发现机器人的位姿还是出现了偏差，但相比图3有所改进。图5是使用了传统的ICP算法对航迹推算出的位姿进行纠正。从图中圈出的部分可以发现机器人得到的位姿信息比较稳定没有漂移，但是存在全局误差积累，且偏差较大。图6是使用了传统的ICP算法对AMCL算法输出的位姿进行纠正。从图中圈出的部分可以发现机器人得到的位姿信息比较稳定没有漂移，且相比图5和全局地图的偏差有所减小。图7是本发明最终改善的效果，对比前面所有的方法，匹配效果有明显的改善。图8为通过里程计得到的位姿和理想位姿绘制的轨迹图；图9为使用了AMCL算法得到的位姿和理想位姿绘制的轨迹图。图10为本发明实施多层次地图匹配得到的位姿和理想位姿绘制的轨迹图。通过图8、9和图10的效果对比，更能表明本发明提出的基于多层次地图匹配的算法对于纠正移动机器人的位姿有很好效果。

本领域的普通技术人员将会意识到，这里所述的实施例是为了帮助读者理解本发明的原理，应被理解为本发明的保护范围并不局限于这样的特别陈述和实施例。本领域的普通技术人员可以根据本发明公开的这些技术启示做出各种不脱离本发明实质的其它各种具体变形和组合，这些变形和组合仍然在本发明的保护范围内。

Claims (1)

1.一种基于多层次地图匹配的移动机器人位姿纠正算法，其特征在于，包括以下步骤：

步骤1.利用同步定位与构图算法建立机器人二维栅格地图；在机器人每次开机后，采用开源的计算机视觉的opencv函数库读取已经建好的栅格地图；

步骤2.栅格地图上的黑色像素点代表激光扫描到的物体，把相应的黑色像素点转化为图像标系下的激光点云数据格式；再将图像坐标系下的激光点云P转换至地图坐标系下，得到地图坐标系下的点云P′，即全局目标点云地图；

图像坐标系和地图坐标系之间的转换关系为：P′＝RP+t；其中R是图像坐标系到地图坐标系的旋转矩阵，t是地图坐标系原点在图像坐标系下的坐标；

步骤3.根据机器人运动学模型，利用里程计和惯性测量单元的数据融合，计算出机器人的航迹，从而得到一个没有全局信息的初始位姿估计；

步骤4.采用自适应蒙特卡洛定位算法将激光雷达扫描到的当前观测点云与栅格地图进行匹配，纠正机器人航迹推算出的位姿，从而得到机器人在栅格地图中的全局位姿；

步骤5.设定激光雷达的扫描间距和旋转角度差，激光雷达依次扫描并采集激光点云数据S_k，该点云位于机器人坐标系下；再采用AMCL算法输出的全局位姿作为激光点云S_k与全局目标点云地图P′的初始配准参数初始旋转矩阵和初始平移向量

步骤6.根据初始配准参数，将点云S_k和全局目标点云地图P′进行配准，得到最终纠正的机器人位姿，为旋转矩阵和平移向量

步骤6的具体步骤为：

步骤6-1.令当前迭代次数i＝1，根据和把机器人坐标系下的点云S_k旋转平移变换得到地图坐标系下的点云Q，并把点云Q和点云地图P′按照kd-tree的数据结构存储，Q＝{q_j,j＝1...N}，P′＝{p′_j,j＝1...N}，N是激光单次扫描得到点云总数；

步骤6-2.根据最近距离原则在Q中搜索p′_j的距离最近点q_j得到搜索关联点对(q_j,p′_j)；

根据最近互邻原则在点云地图P′中搜索q_j的最近点p″_j，判断p′_j与p″_j的距离是否小于预设条件d_m；

若搜索关联点对(q_j,p′_j)满足预设条件，则确定关联点对为(q_j,p′_j)；否则返回步骤6-2重新搜索；最终得到N_t对关联点对(q_m,p′_m)，m＝1…N_t，N_t为满足预设条件的关联点对的个数，每个关联点对的距离为

步骤6-3.对N_t对关联点对的距离进行排序，选择距离较小的N_o对关联点对(q_n,p′_n)，其中n＝1…N_o，N_o＝N_t×η，η为固定重叠率；

步骤6-4.基于奇异分解的方法，利用N_o对关联点对(q_n,p′_n)，计算并更新旋转矩阵和平移向量

步骤6-5.判断当前误差是否达到预设精度要求，如达到要求则进行下一步骤，end＝i；如未达到要求，令i＝i+1，返回至步骤6-1；

步骤7.把旋转矩阵转换为对应的四元数，最终得到纠正后的机器人在地图坐标系下的位置和角度。