CN113111081A

CN113111081A - 一种弱特征环境下的移动机器人建图方法

Info

Publication number: CN113111081A
Application number: CN202110408720.1A
Authority: CN
Inventors: 彭倍; 曾双耀; 邵继业
Original assignee: Sichuan Artigent Robotics Equipment Co ltd
Current assignee: Sichuan Artigent Robotics Equipment Co ltd
Priority date: 2021-04-16
Filing date: 2021-04-16
Publication date: 2021-07-13

Abstract

本发明公开一种弱特征环境下的移动机器人建图方法，应用于移动机器人领域，针对现有技术在弱特征环境下的建图精度过低的问题，本发明基于图优化SLAM算法对扫描匹配进行改进，能有效的提高移动机器人的建图质量，具体的：在图优化SLAM中，机器人的位姿作为顶点，位姿之间的关系构成边(edge)，一旦图构成了，通过改进的扫描匹配算法调整机器人的位姿去尽量满足这些边构成约束，从而得到最终的建图；采用本发明的方法能明显改善弱特征环境建图精度。

Description

一种弱特征环境下的移动机器人建图方法

技术领域

本发明属于移动机器人领域，特别涉及一种移动机器人建图技术。

背景技术

目前室内自主移动机器人大多数采用码盘，陀螺仪，IMU，激光雷达等传感器采集相应信息通过融合算法来估算机器人实时的姿态。码盘类似于汽车的里程计，通过计算轮毂转动的圈数来推算机器人运动的路程，但是机器人在运动过程中难免受到打滑，轮胎磨损等因素的影响导致码盘检测误差，并且会随着机器人运动逐渐累加，无法有效消除误差积累。陀螺仪和IMU(Inertial measurement unit惯性测量单元)由于电子器件的电气特性会出现零漂(零漂是输入为0的时候的输出)的现象，并且会随着时间误差逐渐累积，同样无法有效消除误差积累。

在类似监所等环境中，由于环境相似度大，即呈现弱特征环境，对机器人有效建图提出了很大的挑战，弱特征环境即环境中存在少量相似的直线特征，但是角点特征较少，特征点的相似会直接导致出现建图漂移或错误。

发明内容

为解决上述技术问题，本发明提出一种弱特征环境下的移动机器人建图方法，通过将器人位姿当做顶点，位姿关系当做边，使得顶点满足边构成的约束，得到最终的建图。

本发明采用的技术方案为：一种弱特征环境下的移动机器人建图方法，将机器人位姿作为顶点，位姿关系作为边，通过优化顶点满足边构成的约束，完成建图。

具体包括以下步骤：

S1、通过传感器采集机器人的位姿数据；

S2、机器人运动至下一位置，使用体素网格滤波器对点云数据进行采集；

S3、采用改进的扫描匹配算法对采集到的点云数据进行计算，若扫描匹配结果为已知子地图，则对已知子地图进行优化，然后执行步骤S4；否则，***新的位姿点，更新子地图，执行步骤S4；

S4、判断建图是否完成，若完成，则结束；否则返回步骤S2。

步骤S3具体包括以下分步骤：

S31、取两帧点云数据，将两帧点云数据各自均分为k组，并计算两帧点云数据各自的质心，若质心在所划分组内，则将这一组移除；

S32、从经步骤S31处理后的两帧点云数据各取一组构成矩阵W，计算矩阵W的旋转矩阵与平移矩阵；

S33、根据旋转矩阵与平移矩阵计算两帧点云的距离，若距离小于设定的阈值，则说明两组匹配，否则不匹配；

S34、重复步骤S32-S33，直至完成所有组的匹配，判断匹配组数是否满足要求，若是则匹配成功，则对已知子地图进行优化，然后执行步骤S4；否则，***新的位姿点，更新子地图，执行步骤S4。

步骤S32具体为：若矩阵W满秩，则由奇异值分解计算算矩阵W的旋转矩阵与平移矩阵；否则，通过

使得E(R，t)最小，迭代求解算矩阵W的旋转矩阵与平移矩阵，其中，R表示矩阵W的旋转矩阵，t表示矩阵W的平移矩阵，p_i表示第p组第i个点云数据，q_i表示第q组第j个点云数据，n表示每组的点云个数。

步骤S34所述***新的位姿点，更新子地图，具体为：当匹配失败时，则判断机器人目前所处的位置为新的位姿点，不在已知的子地图上，则***新的位姿点，更新子地图。

本发明的有益效果：本发明的方法通过将机器人的位姿作为一个节点或顶点(vetex)，位姿之间的关系构成边(edge)，然后采用改进的扫描匹配算，调整机器人的位姿去尽量满足这些边构成的约束，从而得到最终的建图；本发明的方法具备以下优点：

1、针对弱特征环境建图精度相对于一些开源的SLAM算法有明显改善；

2、由于无须迭代，算法时间复杂度较低，针对建图实时性较好。

附图说明

图1为图优化SLAM算法信息矩阵获取过程示意图；

其中，图1(a)为观测地标m₁的示意图，图1(b)为机器人从x₁运动到x₂的示意图，图1(c)为运动几步之后的示意图；

图2为本发明的方法流程图；

图3为本发明实施例提供的仿真环境；

图4为本发明实施例提供的Gmapping算法建图结果；

图5为本发明实施例提供的Hector-SLAM算法建图结果；

图6为本发明实施例提供的Cartographer-SLAM算法建图结果；

图7为采用本发明方法的建图结果；

图8为本发明方法与现有技术的位姿误差对比；

图9为本发明方法与现有技术的角度误差对比。

具体实施方式

为便于本领域技术人员理解本发明的技术内容，下面结合附图对本发明内容进一步阐释。

本发明提供一种弱特征环境下移动机器人建图方法。主要是基于图优化SLAM(Simultaneous Localization and Mapping)算法对扫描匹配进行改进，能有效的提高移动机器人的建图质量。在图优化SLAM中，机器人的位姿是一个节点或顶点(vetex)，位姿之间的关系构成边(edge)，比如t’+1时刻和t’时刻之间的odometry(里程计)关系构成边，或者由视觉计算出来的位姿转换矩阵也可以构成边。一旦图构成了，就要调整机器人的位姿去尽量满足这些边构成的约束。如图1所示为图优化SLAM算法信息矩阵获取过程示意图，其中图1(a)为观测地标m₁的示意图，图1(b)为机器人从x₁运动到x₂的示意图，图1(c)为运动几步之后的示意图；图优化SLAM能分解成两个任务：

1、构建图。机器人位姿当做顶点，位姿关系当做边，称为前端。

2、优化图。调整机器人位姿顶点尽量满足边的约束，称为后端。

图优化过程中：先堆积数据，机器人位姿为构建的顶点。边是位姿之间的关系，可以是编码器数据计算的位姿，也可以是通过ICP匹配计算出来的位姿，还可以是闭环检测的位姿关系。

由于室内环境下的基于ICP匹配的算法比较复杂，并且ICP算法高度依赖初始估计，限制了其实际应用范围，在此提出一种基于激光雷达的改进扫描匹配算法。由于构建图方法不是算法消耗和精度提升的主要部分，本算法只针对优化图中扫描匹配进行优化，算法流程图如图2所示；包括如下步骤：

S1：激光雷达、里程计等传感器采集数据输入。本领域技术人员应知本步骤中激光雷达采集点云数据，里程计采集运动距离数据等。

S2：使用图优化SLAM算法进行图形构建(前端)，机器人运动至下一位置，使用体素网格滤波器对点云数据进行采集。

S3：改进的扫描匹配算法：

S3.1：取两帧的点云数据，分别记为P点集和Q点集，P点集和Q点集各自分成k组，根据

计算出各组质心，并移除质心。N_k表示第k组的点云数，x_i表示第k组第i个点云数据坐标。

S3.2：计算矩阵W(由公式(5)可得)，如果W满秩，则由奇异值分解计算R和t，即旋转矩阵R＝VU^T且平移矩阵t＝p_i-Rq_i；否则，通过

使得E(R，t)最小，迭代求解R和t。

其中，p_i表示第p组第i个点云数据，q_i表示第q组第j个点云数据；

由奇异值分解法求解R和t推导如下，设任意两组点云为：

使下式最小即可求得R和t：

其中，μ_q和μ_p分别是Q点云集与P点云集的质心。展开上式并化简得：

q′_i、p′_i分别为原始点云减去对应质心的点。上式只与R有关，因此寻求使得上式最小的R值。R是正交矩阵，因此R^TR＝1，上式的最小值等价于求下式的最大值：

用矩阵的迹表示：

其中

tr(·)表示矩阵的迹。对W进行奇异值分解得到W＝UΛV^T，W＝UΛV^T为W的奇值分解，其中U和V均为单位正交阵，T表示矩阵的转置。U称为左奇异矩阵；V称为右奇异矩阵；Λ仅在主对角线上有值，称为奇异值，其它元素均为0。

假设有一变量K＝VU^T，因此KW＝VU^TUΛV^T＝VΛV^T，故KW是一个正定对称的矩阵，根据定理可知，对正定对称矩阵A，对任意正交矩阵B，都有tr(A)≥tr(BA)，因此，R^T＝VU^T。故有：

S3.3：求取距离

若d小于阈值，则说明两组匹配，则匹配组数增加；否则，该组不匹配。这里的阈值的取值依据为：例如对同一物体在不同位置或角度进行扫描，两组点云匹配度大于95％以上，则说明此时的阈值较为合理，选取不同物体重复上述过程，选择匹配度最合适的阈值。

S3.4：判断是否所有组均完成匹配，若是，判断匹配的成功率是否满足要求，若是，代表成功，否则，失败；若并非所有组均完成了匹配，则继续匹配。

S4：根据S3.4判断扫描匹配结果是否成功。若是，则对已知匹配的子地图进行优化；否则，***新的位姿点，更新子地图。

步骤S4中判是否所有组均完成匹配，比如匹配率设置为95％，若所有组的匹配率达到95％，则表示匹配成功，否则匹配失败。

当匹配失败时，则判断机器人目前所处的位置为新的位姿点，不在已知的子地图上，由于建图过程是连续的，本发明通过已经建立好的已知子地图上的位姿点可以推断出新得位姿点所处的位置。

S5：判断建图是否完成。若是，则结束算法；否则，转S2。本步骤中依据实际环境特征是否均扫描完成作为判断建图是否完成的依据。

本实施例在操作***是Ubuntu16.04，仿真软件是Gazebo7的条件下进行，建立如图3所示仿真环境，黑色圆表示带有RPLIDAR A1激光雷达的机器人模型，***表示障碍物墙体，主要是模拟一个特征点少，特征相似且直线段较明显的弱特征环境(例如监所环境的长通道)。

参数设置：提取机器人先后两帧经步骤S1、S2处理过的点云数据，设置机器人的扫描角度为200°，该型号的激光雷达每隔1°扫描一个特征点，因此根据步骤S3.1将组数分为10组，每个组20个相邻的点云数据，根据

计算质心坐标，若计算的质心正好在这20个点中某一个，则除去该质心，根据步骤S3.2计算W矩阵，判断W矩阵是否非奇异的，若是，则旋转矩阵R＝VU^T且平移矩阵t＝u_pi-Ru_qj；否则，根据公式E(R，t)迭代计算求解R和t。最后根据步骤S3.3计算d，若d在阈值0.012cm内，则表示两组点云匹配成功，否则不成功。后续步骤遍历先后两帧点云组数据。最后根据步骤S4和S5判断即可。

分别使用开源的算法Gmapping、Hector-SLAM、Cartographer-SLAM和改进的图优化SLAM算法进行建图分析，结果如下：

如图4至图7所示，Gmapping算法在建图后期会出现漂移现象，而Hector算法在建图中期出现断层，直接导致建图失败，Cartographer-SLAM算法相对前面两种算法来说效果较好，改进的算法效果最好。

统计位姿误差和角度误差分别如图8和图9所示。Gmapping算法的误差随时间的增大而增大，是由于后期出现漂移所致；Hector-SLAM算法的误差从一开始就很大，无法应对弱特征的环境；Cartographer-SLAM算法是开源算法中效果最好的，但是相对改进的算法来说误差的稳定性以及建图效果均相对差一些，因此，本算法提出的改进扫描匹配的算法能够在弱特征环境下建图起到较好的效果。

图8和图9中，标t表示时间，单位：秒；纵坐标表示位姿误差，即x方向和y方向的平方和的绝对平方根，单位：厘米；Improved Proposal表示改进的Graph SLAM算法。

本发明主要是对类似监所长走廊环境下，角点特征较少，环境相似度大的情况，针对基本的Graph SLAM的ICP算法复杂度大，实际应用不理想的情况，提出一种改进的扫描匹配算法，通过扫描点分组，奇异值分解计算旋转矩阵以及平移矩阵，通过仿真验证，建图效果得到改善，说明扫描匹配算法是可行的。

本领域的普通技术人员将会意识到，这里所述的实施例是为了帮助读者理解本发明的原理，应被理解为本发明的保护范围并不局限于这样的特别陈述和实施例。对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的权利要求范围之内。

Claims

1.一种弱特征环境下的移动机器人建图方法，其特征在于，将机器人位姿作为顶点，位姿关系作为边，通过优化顶点满足边构成的约束，完成建图。

2.根据权利要求1所述的一种弱特征环境下的移动机器人建图方法，其特征在于，具体包括以下步骤：

S1、通过传感器采集机器人的位姿数据；

S4、判断建图是否完成，若完成，则结束；否则返回步骤S2。

3.根据权利要求2所述的一种弱特征环境下的移动机器人建图方法，其特征在于，步骤S3具体包括以下分步骤：

4.根据权利要求3所述的一种弱特征环境下的移动机器人建图方法，其特征在于，步骤S32具体为：若矩阵W满秩，则由奇异值分解计算算矩阵W的旋转矩阵与平移矩阵；否则，通过

5.根据权利要求4所述的一种弱特征环境下的移动机器人建图方法，其特征在于，步骤S34所述***新的位姿点，更新子地图，具体为：当匹配失败时，则判断机器人目前所处的位置为新的位姿点，不在已知的子地图上，则***新的位姿点，更新子地图。