衢州职业技术学院，浙江 衢州

收稿日期：2020年11月27日；录用日期：2020年12月21日；发布日期：2020年12月28日

摘要

障碍物的检测和避免可以被视为设计移动机器人的中心问题，这项技术为机器人提供了可以在不熟悉的环境中穿越而不导致自身损坏。避障机器人可以检测路径上的障碍物并在不发生任何碰撞的情况下进行机动的设计。点集配准算法是移动机器人位姿估计和地图构建中的一个非常重要的算法，广泛应用于模型重建、多视角配准、即时定位与建图(SLAM)等领域，ICP算法已得到国内外研究学者的高度评价。本文比较了迭代最近点(ICP)和基于群集的双向迭代最近点(IDCPBoC)算法。基于以上两种算法，对机器人进行姿态估计，并对移动机器人避障实验过程进行了分析。密集传感器方法(DSM)可构建环境图，IDCPBoC算法可以更好地避免障碍，IDCPBoC算法的准确性和收敛性优于ICP算法，且IDCPBoC算法运行时间较少，因此实时性能也大大提高了。

关键词

移动机器人，位姿估计，ICP算法，IDCPBoC算法，即时定位与建图

Research on Optimization and Design of Pose Estimation Algorithm in the Process for Simultaneous Localization and Mapping

Youhong Xu, Yun Zhou, Xiaoping Liu

Quzhou College of Technology, Quzhou Zhejiang

Received: Nov. 27^th, 2020; accepted: Dec. 21^st, 2020; published: Dec. 28^th, 2020

ABSTRACT

Obstacle detection and avoidance can be considered as the central issue in designing mobile robots. This technology provides the robots with senses which can use to traverse in unfamiliar environments without damaging itself. In this paper, an Obstacle Avoiding Robot is designed which can detect obstacles in its path and maneuver around them without making any collision. It is a robot vehicle that works on Arduino Microcontroller and employs three ultrasonic distance sensors to detect obstacles. The point set registration algorithm is a very important algorithm in mobile robot pose estimation and map construction. It is widely used in the fields of model reconstruction, multi-view registration, simultaneous localization and mapping (SLAM), etc. The ICP algorithm has been studied at home and abroad. Scholars attach great importance to it. This paper compares the iterative closest point (ICP) and cluster-based iterative bidirectional closest point (IDCPBoC) algorithms. The two algorithms are used to estimate the pose of the robot; the obstacle avoidance experiment process of the mobile robot is analyzed; the environment map is constructed using the DSM (dense sensor method) method; the IDCPBoC algorithm can better avoid obstacles. The accuracy and convergence of the IDCPBoC algorithm are better than the ICP algorithm. Because the IDCPBoC algorithm runs less time, the real-time performance is also greatly improved.

Keywords:Mobile Robot, Pose Estimation, ICP Algorithm, IDCPBoC Algorithm, SLAM

Copyright © 2020 by author(s) and Hans Publishers Inc.

This work is licensed under the Creative Commons Attribution International License (CC BY 4.0).

http://creativecommons.org/licenses/by/4.0/

1. 引言

移动机器人是机器人的重要研究领域，国内外专家学者很早就开始移动机器人的研究。世界上第一台真正意义上的移动机器人是斯坦福研究院(SRI)的人工智能中心于1966年到1972年研制的Shakey，通过无线通讯系统由二台计算机控制，可以进行简单的自主导航。虽然Shakey只能解决简单的感知、运动规划和控制问题，但它却是当时将人工智能(Artificial Intelligence, AI)应用于机器人的最为成功的研究平台，它证实了许多通常属于AI领域的严肃的科学结论。从20世纪70年代末开始，随着计算机的应用和传感技术的发展，以及新的机器人导航算法的不断推出，移动机器人研究开始进入快车道。移动机器人智能的一个重要标志就是自主导航，而实现机器人自主导航有个基本要求——避障。下面让我们来了解一下移动机器人的避障，避障是指移动机器人根据采集的障碍物的状态信息，在行走过程中通过传感器感知到妨碍其通行的静态和动态物体时，按照一定的方法进行有效地避障，最后达到目标点。实现避障与导航的必要条件是环境感知，在未知或者是部分未知的环境下避障需要通过传感器获取周围环境信息，包括障碍物的尺寸、形状和位置等信息，因此传感器技术在移动机器人避障中起着十分重要的作用。

机器人导航过程中的定位问题，就是确定移动机器人在工作环境中相对于全局坐标的位置及其本身的姿态，位姿(位置和姿态)估计问题是移动机器人研究的一个核心问题。精确的位姿估计对于移动机器人的定位、自动地图生成、路径规划和控制、目标检测和跟踪等具有重要意义。在二维环境中，移动机器人的位姿通，位姿估计 [1] [2] 在计算机控制领域扮演着十分重要的角色，在使用视觉传感器估计机器人位姿进行控制、机器人导航、增强现实以及其它方面都有着极大的应用。位姿估计这一过程的基础是找到现实世界和图像投影之间的对应点。然后根据这些点对的类型，如2D-2D，2D-3D，3D-3D等，采取相应的位姿估计方法。在移动机器人位姿估计和地图构建领域，三维点云数据的配准对解决模型重建、SLAM、图像拼接、相机定位等问题有比较显著的优势 [3] [4]，对于三维点集配准问题，国内外学者做了大量的研究，如点标记法(Point Signature)、主曲率方法(Principal Curvature)、旋转图像(Spin Image)、遗传算法(Generic Algorithm)等，这些算法能够很好解决三维配准问题，Besl和Mckay在1992年提出一种迭代最近点(Iterative Closest Point, ICP)，它是基于自由形态曲面的配准算法 [5]。即时定位与建图(simultaneous localization and mapping, SLAM) [6] 对移动机器人和VR/AR耳机等应用至关重要，这些应用通常都是在室内进行的，而GPS或惯性测量装置要么无效，要么不够精确。系统需要映射它的环境来确定它在该环境中的位置和姿势，而姿势反过来又影响映射，有多种方法来做SLAM的传感，其中经常会听到的是激光雷达作为主要的传感输入。随着ICP算法的广泛应用，针对ICP算法在帧间配准中对初始值依赖性大的问题，李永锋等 [7] 提出了一种新的由粗到精的帧间配准改进ICP算法，算法通过泰勒展开简化相机位姿模型，利用求和思想建立粗配准模型；邱世聪等 [5] 针对传统ICP算法所存在的对初始点云位置要求高、算法效率低等局限性，对算法进行研究改进，改进结合K-近邻搜索和法向量估计；刘亚彬等 [8] 探讨了基于总体最小二乘和抗差估计的点云数据三维配准方法，并基于多元总体最小二乘理论研究改进了的迭代最近点算法(ICP)，有效提高配准的效率和稳健性。本文对迭代最近点(ICP)及基于聚类的迭代双向最近点(IDCPBoC)算法进行比较研究，基于以上两种算法对机器人进行位姿估计研究，有效满足移动机器人位姿估计算法的实时性及精度要求。

2. 机器人避障

避障是指移动机器人在行走过程中，通过传感器感知到在其规划路线上存在静态或动态障碍物时，按照一定的算法实时更新路径，绕过障碍物，最后达到目标点 [9]。实现避障与导航的必要条件是环境感知，在未知或者是部分未知的环境下避障需要通过传感器获取周围环境信息，包括障碍物的尺寸、形状和位置等信息，因此传感器技术在移动机器人避障中起着十分重要的作用。机器人避障需使用的传感器有激光雷达、深度相机、超声波传感器、物理碰撞、跌落检测等。目前市面上常见的机器人避障基本都采用到激光雷达，但如果仅使用激光雷达作为唯一的一种避障传感器，是无法在一些复杂场所胜任避障工作的，必须要为机器人配备其它的传感器作为补充，比如：超声波传感器，它的成本非常低，实施简单，可识别透明物体，缺点是检测距离近，三维轮廓识别精度不好，所以对桌腿等复杂轮廓的物体识别不好，但是它可以识别玻璃、镜面等物体，避障设置和结果网格地图的无碰撞路径(Ryther and Madsen, 2009)如图1所示。

图1. 避障设置和结果网格地图的无碰撞路径(Ryther and Madsen, 2009)

常用的计算机视觉方案也有很多种，比如双目视觉，基于TOF的深度相机，基于结构光的深度相机等。深度相机可以同时获得RGB图和深度图，不管是基于TOF还是结构光，在室外强光环境下效果都并不太理想，因为它们都是需要主动发光的。像基于结构光的深度相机，发射出的光会生成相对随机但又固定的斑点图样，这些光斑打在物体上后，因为与摄像头距离不同，被摄像头捕捉到的位置也不相同，之后先计算拍到的图的斑点与标定的标准图案在不同位置的偏移，利用摄像头位置、传感器大小等参数就可以计算出物体与摄像头的距离。

双目视觉 [10] [11] 的测距本质上也是三角测距法，由于两个摄像头的位置不同，就像我们人的两只眼睛一样，看到的物体不一样。两个摄像头看到的同一个点，在成像的时候会有不同的像素位置，此时通过三角测距就可以测出这个点的距离。与结构光方法不同的是，结构光计算的点是主动发出的、已知确定的，而双目算法计算的点一般是利用算法抓取到的图像特征，如SIFT或SURF特征等，SIFT和SURF特征提取示例如图2所示。这样通过特征计算出来的是稀疏图。要做良好的避障，稀疏图还是不太够的，我们需要获得的是稠密的点云图，整个场景的深度信息。稠密匹配的算法大致可以分为两类，局部算法和全局算法。局部算法使用像素局部的信息来计算其深度，而全局算法采用图像中的所有信息进行计算。一般来说，局部算法的速度更快，但全局算法的精度更高。

图2. SIFT和SURF特征提取

这两类各有很多种不同方式的具体算法实现，能过它们的输出我们可以估算出整个场景中的深度信息，这个深度信息可以帮助我们寻找地图场景中的可行走区域以及障碍物 [12] [13]。整个的输出类似于激光雷达输出的3D点云图，但是相比来讲得到信息会更丰富，视觉同激光相比优点是价格低很多，缺点也比较明显，测量精度要差一些，对计算能力的要求也高很多。

3. ICP算法

点云数据能够以较小的存储成本获得物体准确的拓扑结构和几何结构，因而获得越来越广泛的关注。在实际的采集过程中，因为被测物体尺寸过大，物体表面被遮挡或者三维扫描设备的扫描角度等因素，单次的扫描往往得不到物体完整的几何信息。因此，为了获得被测物体的完整几何信息，就需要将不同视角即不同参考坐标下的两组或者多组点云统一到统一坐标系下，进行点云的配准。在配准算法中，研究者使用最多的是ICP算法。

ICP算法的基本原理是：分别在带匹配的目标点云P和源点云Q中，按照一定的约束条件，找到最邻近点 $\left(p_i,q_i\right)$，然后计算出最优匹配参数R和t，使得误差函数最小。误差函数为 $f\left(R,t\right)$ 为：

$f\left(R,t\right)={{\displaystyle {\sum }_{i+1}^N{q}_i-\left(R{P}_i+T\right)}}^2$ (1)

其中n为最邻近点对的个数， $p_i$ 为目标点云P中的一点， $q_i$ 为源点云Q中与 $p_i$ 对应的最近点，R为旋转矩阵，t为平移向量。

ICP算法步骤：

1) 在目标点云P中取点集 $p_i\in P$ ；

2) 找出源点云Q中的对应点集 $q_i\in Q$，使得 $\Vert q_i-p_i\Vert =\min$ ；

3) 计算旋转矩阵R和平移矩阵t，使得误差函数最小；

4) 对 $p_i$ 使用上一步求得的旋转矩阵R和平移矩阵t进行旋转和平移变换，的到新的对应点集

${p^{\prime }}_i=\left({p^{\prime }}_i=R{p}_i+t\right)$

5) 计算 $p_i$ 与对应点集 $q_i$ 的平均距离；

6) 如果d小于某一给定的阈值或者大于预设的最大迭代次数，则停止迭代计算。否则返回第2步，直到满足收敛条件为止。

4. DCP算法

ICP算法存在局部极小值问题，对ICP的最近点迭代对应规则进行了修改，提出双向最近点迭代算法(dual closest point, DCP) [14] [15]，找寻相邻时刻数据点中的对应点的步骤为：

1) $p_i$ 在点集 $M^{\prime }$ 中寻找对应点 ${p^{\prime }}_i$ ；

2) ${p^{\prime }}_i$ 在点集 $D^{\prime }$ 中寻找对应点 ${p^{″}}_i$。

3) 设 $l_1$ 和 $l_2$ 分别为 $\Vert p_i{p^{\prime }}_i\Vert$ 、 $\Vert {p^{\prime }}_i{p^{″}}_i\Vert$ 的较大和较小者，若 $l_1\pm l_2<\lambda \left(\lambda >1\right)$，则认为 ${p^{\prime }}_i$ 是 $p_i$ 的对应点；相反， ${p^{\prime }}_i$ 不是 $p_i$ 的对应点。在本文中， $\lambda =1.2$。

ICP算法DCP算法比较实验结果如图3所示。

图3. 仿真比较实验

5. IDCPBoC算法

IDCP算法相对于ICP算法比较复杂度，因此，本文对基于聚类的迭代双向最近点(iterative dual closest point based on clustering, IDCPBoC)算法进行研究。

本文采用LMS200激光测距仪，首先对获得的扫描数据进行滤波，假设经滤波后得到的点集为 $D^{\prime }$ ( $p_i\in D^{\prime }$，大小为 ${N^{″}}_D$ )。再对 $D^{\prime }$ 进行粗聚类。由于激光扫描点的密度不均匀，因此在粗聚类时采用动态阈值。其算法为：① 令第1个数据点 $p_0\in C_1$ 属于第1个类；② 计算 $p_i$ 到 $p_{i+1}$ 的距离l；③ 若 $l\prec T_{i+1}$，则 $p_{i\text{+}1}\in C_j$ ；反之，置 $j=j+1$，$p_{i\text{+}1}\in C_j$，$d_{i+1}$ 为 $p_{i\text{+}1}$ 到原点的距离，④ 若 $i\prec {N^{″}}_D-1,i=i+1$，返回②，反之，结束。动态阀值为：

$T_{i+1}\{\begin{array}{l}30d_{i+1}\prec 1000\text{mm}\\ 0.03d_{i+1}{d}_{i+1}\ge 1000\text{mm}\end{array}$ (2)

6. 实验分析

为评估算法性能，在室内走廊环境下进行，机器人采用双轮差速及万向舵轮的驱动方式，两轮为差动驱动轮，后轮为万向支撑轮，激光测距仪LMS200安装在机器人的前部，通过传感器每隔300 ms扫描一次周边地形，机器人如图4所示。在避障过程中移动机器人的最大速度和最小速度分别为0.9 m/s和0.5 m/s。基于上述ICP及IDCPBoC算法匹配相邻两帧数据，对移动机器人进行相对位姿估计，图5为两种算法的配准后的残差的RMS值，评价移动机器人重要的性能指标之一是实时性问题，所以图6为两种算法运行时间T_min。通过实验分析，IDCPBoC的算法收敛情况优于ICP算法，且IDCPBoC的算法运行时间较小，基于ICP算法和IDCPBoC算法定位移动机器人的运动轨迹分别如图7和图8所示，并基于并同时采用DSM (dense sensor method)方法构建环境地图，可以发现IDCPBoC算法能更好的规避障碍物。

图4. 移动机器人

图5. 配准误差的均方根(RMS)

图6. 两种算法运行时间T_min

图7. ICP避障结果

图8. IDCPBoC避障结果

7. 结论

IDCPBoC的算法精度及收敛情况均优于ICP算法，且评价移动机器人重要的性能指标之一是实时性问题，通过实验分析发现IDCPBoC算法运行时间较小，因此实时性大大提高，并基于并同时采用Dense Sensor Method方法进行SLAM，可以发现IDCPBoC算法能更好的规避障碍物。该项目中完成的工作可作为进一步改进的基础，以提高在各种环境中障碍物检测的准确性和适应性。将来，该项目打算测试整合不同类型的传感器来弥补彼此的劣势，例如当超声传感器可能无法正确识别环境噪声和温度或气压变化的环境中的障碍物时，成像传感器可能会很有用。精度可以通过包括用于自动调节空气声速的电子气压计来增加确定到障碍物的距离的能力，另外，添加蓝牙设备可以提供远程更改代码中控制参数的灵活性。

致谢

诚挚感谢衢州市科技计划指导性项目(2020017)和衢职院校级科研项目(QZYY2007)给予资助，感谢优秀的合作教师。

文章引用

徐友洪,吴宏良,李剑萍. 即时定位与建图过程中位姿估计算法优化与设计研究
Research on Optimization and Design of Pose Estimation Algorithm in the Process for Simultaneous Localization and Mapping[J]. 计算机科学与应用, 2020, 10(12): 2331-2338. https://doi.org/10.12677/CSA.2020.1012246

参考文献