自主移动机器人即时定位与地图构建方法研究

曾品善，妙全兴，徐 磊

(1.武警工程大学 信息工程系，陕西 西安710086;2.武警警官学院 信息工程系，四川 成都610213)

在Leonard、Durrant［1］等人提出的关于自主移动机器人导航3个基本问题“Where am I?Where am I going?How should I get there?”中，第一个问题就是关于自主移动机器人的定位问题。自主移动机器人的即时定位与地图构建(Simultaneous Localization and Mapping，SLAM)，也称为并发建图与定位(Concurrent Mapping and Localization，CML)。可以描述为:机器人在未知环境中从一个未知的位置开始移动，在移动过程中根据自身携带的传感器观测数据逐步构建周围环境地图，同时在所构建地图基础上对机器人的位姿进行估计和更新，把机器人的即时定位与增量式地图构建融为一体，从而实现机器人的自主定位和导航。

即时定位与地图构建(SLAM)是自主移动机器人实现真正自主运动的关键，因此SLAM被视为自主移动机器人领域的“圣杯”，是当今机器人研究中的一个热点问题。Smith Self和Cheesman［2］最早提出了SLAM问题，并将定位与地图构建问题整合在一起。在SLAM问题中，为了实现精确定位，机器人需要通过位置相对确定的环境特征来不断校正自身位置;而另一方面，为了确定环境特征的位置，机器人需要知道自身所在的精确位置。这类似于一个“鸡生蛋，蛋生鸡”的问题，二者互为前提。

1 SLAM研究的主要问题

1.1 不确定信息的描述和处理方法

自主移动机器人是通过对机器人位姿和环境中路标的递归估计来进行即时定位与地图构建的。根据系统的初始状态和从0～t时刻的观测信息与控制信息，自主移动机器人需要对各个时刻的不确定信息进行处理。不确定信息的描述和处理包括以下几个问题:(1)在地图和位置的表示中，如何描述运动和感知信息的不确定性。(2)在迭代过程中，如何处理旧信息与新信息的关系，连续更新地图和位置。(3)如何依据不确定的信息进行决策。

对于在自主移动机器人SLAM中遇到的不确定信息，研究人员已经提出了多种度量方法来进行处理，如概率度量、模糊度量、信任度量、可能性度量和证据理论度量等［3］。由于概率度量和模糊度量相较于其他的方法，能够更好地适应不确定模型，在实际应用中鲁棒性也较好，因此目前在SLAM中使用较多的主要就是这两种方法。但是这两种方法仍然有一些缺点，如概率度量的计算量较大，且算法需建立在一定假设条件之上，模糊度量在参数设定上需要通过大量的实验经验等。

1.2 数据关联

数据关联技术是指把来自一个或多个传感器的测量数据与已有的测量数据集合进行关联分析，确定它们是否来自同一数据源。对于分析后未关联的测量，它们可能来自新的目标，也可能是由于噪声或杂波产生的干扰。数据关联在SLAM中主要完成3个任务:新环境特征的检测、环境特征的匹配和地图的匹配。自主移动机器人周围环境的复杂程度直接影响着数据关联的计算量，一般的数据关联技术难以满足SLAM的实时性要求。在SLAM领域，最常用的数据关联方法是最近邻法(Nearest Neighbor，NN)，该方法计算简单，但对于距离较近的2个环境特征可能会出现关联错误。数据关联正确与否直接决定了机器人定位与地图构建的准确性，错误的数据关联结果会导致此后对特征的预测发生错误，甚至会导致定位与制图误差的发散。

2 SLAM的实现方法

2.1 扩展卡尔曼滤波

自主移动机器人在进行即时定位与地图构建时涉及到对未知且不确定的环境信息和传感器噪声的处理，一般使用以概率论为基础的卡尔曼滤波(Kalman Filter，KF)来描述此类问题。KF是一种最优线性递归估计算法，它能从一系列的不完全包含噪声的测量中估计动态系统的状态。基本的KF必须应用在符合高斯分布的离散时间线性动态系统中，然而在许多实际情况中，系统的状态方程和观测方程却是非线性的，这就需要对KF进行改进以应用于时间非线性的动态系统中。

扩展卡尔曼滤波(Extended Kalman Filter，EKF)是一种较常用的非线性滤波方法，它是利用最常使用的线性化技术泰勒级数展开，将非线性滤波问题转化为近似的线性滤波问题基本KF，然后利用线性滤波理论求解，得到非线性滤波问题的次优滤波算法。EKF由预测和更新两步组成，是一种经典的滤波方式，也是自主移动机器人SLAM中常见的一种滤波算法。但是由于在滤波过程中不可避免地引入了线性化误差，只有当系统的状态和观测模型都接近线性时，滤波结果才可能接近真实值，否则线性化误差会严重影响滤波精度，最终导致滤波器发散，从而使地图构建发生错误。

2.2 不敏卡尔曼滤波

不敏卡尔曼滤波(Unscented Kalman Filter，UKF)与EKF类似，也是应用卡尔曼滤波框架处理非线性高斯系统的另一种滤波算法。UKF的理论基础是UT变换(Unscented Transformation)，通过对确定性Sigma点的采样来近似非线性函数的概率密度分布，而不需要对非线性状态和观测模型进行线性化处理。这些Sigma采样点能够完全体现高斯概率密度函数的真实均值和协方差［4］。与EKF相比，在计算复杂度方面几乎相同，但UKF不需要计算雅克比矩阵。更重要的是当系统非线性程度高时UKF可以取得更好的结果。

2.3 粒子滤波

粒子滤波(Particle Filter，PF)是基于顺序重要采样(Sequential Importance Sampling，SIS)的一种滤波方法，通过一组从概率密度函数上随机抽取的加权样本(粒子)来逼近后验概率，因此粒子滤波适用于任何状态空间模型，不受非高斯和非线性模型的影响，理论上足够多的粒子可以取得良好的逼近效果。

粒子滤波的思想是通过寻找粒子集和粒子对应权值组成的随机采样数据集合s(k)来对概率密度函数进行近似，以有限样本点的求和运算代替积分运算，从而获得状态最小方差估计。用数学语言描述如下:对平稳随机过程，假定k－1时刻系统的后验概率密度为，依据一定原则选取n个随机样本点，k时刻获得测量信息后，经过状态和时间更新过程，n个粒子的后验概率密度可近似为。随着粒子数目的增加，粒子的概率密度函数逐渐逼近状态的概率密度函数，粒子滤波估计即达到最优贝叶斯估计的效果。粒子滤波算法使非线性滤波问题摆脱了随机量必须满足高斯分布的制约条件，因此近年来该算法在SLAM问题上得到成功应用［5］。

对于粒子滤波算法来说，如何选择较好的粒子分布预测函数是关键问题。一个好的粒子分布预测函数，不仅能够减少重采样，降低退化效应，还能提高预测精度，使粒子的分布更接近状态的真实后验概率分布。

2.4 基于视觉的SLAM算法

目前SLAM中使用的传感器大部分是声纳和激光雷达，但声纳传感器成本低且精度不高，激光雷达精度高却成本较高。另一类应用较多的传感器是视觉传感器，视觉传感器的视觉图像中包含了丰富的环境信息，运用一定的先验信息或目标识别算法，可以得到意义更明确的地图［6］。常用的视觉传感器的配置方式有单目摄像头和双目摄像头两种。双目摄像头利用立体视觉的原理能够得到图像中物体的精确深度信息，但它的缺点是成本较高，并且由于要对两幅图像中的同一物体进行精确匹配，造成过程处理的时间过长。单目摄像头成本低廉，通过结合EKF等滤波算法，在加入一定约束条件下可以快速获取环境信息，因此得到了广泛应用。

3 结束语

近年来研究者们对自主移动机器人SLAM问题进行了大量的研究，一方面对现有的SLAM算法进行改进，以提高算法的性能和可行性，另一方面积极结合不断更新的机器人传感器设备，提出新的SLAM算法。但是在以下几个方面还需要进行更进一步的研究:(1)实时性。通过提高算法的效率和进一步降低算法的时间和空间复杂度，从软件上提升SLAM问题的实时性。同时开发专用高速高精度传感器和构图芯片或板卡，通过硬件设备来提高处理速度。(2)三维SLAM。目前SLAM问题还大量局限于二维静态环境的研究与应用，而现实中的环境通常是三维动态的。将SLAM的研究与应用从二维静态环境扩展到三维动态环境具有重要的意义。(3)多机器人协作SLAM。基于多机器人协作的SLAM通过机器人之间的信息互补与共享，能够弥补单机器人定位不准确和构图精度不高等问题，同时提高了自主移动机器人在复杂地形下完成繁重任务的能力。

［1］LEONARD J，DURRANT W H.Mobile robot localization by tracking geometric beacons［J］.IEEE Transactions on Robotics and Automation，1991，7(3):376－382.

［2］SMITH R，SELF M，CHEESEMAN P.Estimating uncertain spatial relationships in robotics［J］.Autonomous Robot Vehicles，1990，8(5):167－193.

［3］ 王卫华，陈卫东，席裕庚.基于不确定信息的移动机器人地图创建研究进展［J］.机器人，2001，23(6):563－568.

［4］ 潘泉，杨峰，叶亮，等.一类非线性滤波器——UKF综述［J］.控制与决策，2005，20(5):481－489.

［5］ 胡士强，敬忠良.粒子滤波算法综述［J］.控制与决策，2005，20(4):361－365.

［6］ 朱磊，樊继壮，赵杰，等.未知环境下的移动机器人SLAM方法［J］.华中科技大学学报:自然科学版，2011，39(7):9－13.