一种基于全局位置估计误差的路标探索策略

马健，俞扬

(南京大学计算机软件新技术国家重点实验室，江苏南京 210023)

由于自主移动机器人在日常生活服务[1]、危险环境作业[2]、太空探索[3]等领域有广泛的应用，对机器人的自主行走和环境感知的研究吸引了越来越多的研究者关注。路标的发现及其位置的精确估计自主移动机器人在环境中的定位起到关键作用。当路标的位置信息已知时，通过对视线范围内路标的鉴别和距离的估计，机器人即可计算出自己的所在位置。因此如何有效地在未知环境中探索和估计路标的位置是自主移动机器人研究的重要问题。

在完全未知环境中自主探索和估计路标的方法主要基于同时定位与地图构建(simultaneous localization and mapping，SLAM)技术。SLAM最先由Self和Cheeseman[4]提出，它迭代的通过路标信息修正自身位置，同时通过自身位置的估计来修正路标信息，使得在机器人的移动过程中能够逐渐提高其自身位置和路标的估计精度。由于其重要的理论与应用价值，同时定位与地图构建(SLAM)被认为是自主移动机器人实现真正自主运动的关键，是智能移动机器人进入人类日常生活的必要基础，也成为近年来机器人和人工智能领域中一个非常活跃的研究热点[5-7]。

在以往的研究中，移动机器人在未知环境中游走，并在移动过程中使用SLAM技术对路标进行估计[8]。常用的游走方式包括随机游走或启发式的探索策略，目标是最大限度地、尽快地引导机器人向未探测区域运动，尽量避免在已探测区域内运动，从而尽快减少未探测的区域。然而，基于SLAM的路标估计方法需要机器人重复探测已探测过的环境来提高对自身位置和路标位置的估计精度[9]，过快移向未探测区域将使得已探测路标的精度过低，导致较大的定位误差。

本文提出一种以全局定位误差最小化为指导的探索策略，进一步提高路标位置的估计精度。本文中还将全局定位信息引入SLAM的位置估计中，有效地校正当移动变化过大时(例如速度发生变化或转弯时)SLAM估计的误差。在模拟环境中的初步实验验证了本文方法的有效性。

1 路径规划相关工作

1.1 同时定位与地图构建相关算法

同时定位与地图构建的一个重要支撑技术是概率统计学。以贝叶斯滤波器为基本原理，研究人员先后提出了扩展卡尔曼滤波的SLAM算法(EKFSLAM)[7]期望值最大化的SLAM算法(EMEKF)[8]粒子滤波和扩展卡尔曼滤波的SLAM算法(FastSLAM)[9]等。EKF-SLAM 算法是用高斯分布表示后验概率的贝叶斯滤波[10]。该算法作了3个近似。首先，运动模型近似为线性的，在环境为静态的条件下，地图特征状态不变，仅有机器人状态发生变化。根据扩展卡尔曼滤波状态方程线性的要求，机器人的非线性运动模型近似为一个带有高斯噪声的线性函数;其次，观测模型近似为线性的。最后，初始的不确定量近似为高斯分布。EKF-SLAM方法已被广泛地研究和应用。Dissanayake等[11]通过在路边架设雷达点路标并人工驾驶室外移动汽车，验证了卡尔曼滤波在同时定位与地图构建方面的有效性。Castellanos等[12]利用扩展卡尔曼滤波研究室内环境的同时定位与地图构建。悉尼大学的Williams等[13]将它应用于水下环境。

EM-SLAM算法是一种由最大似然估计法发展而来的统计算法，由Thrun等[14]提出。应用EM算法进行地图构建的主要观点是当机器人路径已知时，确定地图是相对简单的;同样，对于一张给定的地图，确定机器人位姿的概率估计也相对简单。求解时交替执行2个步骤，分别称为期望步骤(expectation step，E步骤)和最大值化步骤(maximization step，M步骤).在E步骤中，根据当前最大可能性地图估计各个时间点机器人位置的概率估计，在M步骤中，根据在E步骤所计算得到的位置估计最大可能性地图。通过不断重复，机器人位置估计和地图估计的精确性不断提高。

针对EKF-SLAM方法在单峰高斯假设下难以解决数据对应问题、无法构建环行环境地图的缺陷，以及EM-SLAM方法需要多次遍历数据集、难以递增地构建环境地图的缺陷，Hahnel等[15]提出了基于粒子滤波和卡尔曼滤波的混合SLAM方法，也称为FastSLAM。FastSLAM算法的基本思想是通过样本集合表示概率密度。每一个样本是关于状态真值的一个离散猜测，一组样本描述出概率分布的代表性特性。这种基于样本的表示方法使得粒子滤波可以表示各种概率密度，适用于在线的、非线性、非高斯分布的实时估计。FastSLAM面向路标特征地图，将机器人位姿估计和地图特征估计相分离，用粒子样本描述机器人路径的概率分布，每一个粒子对应于一个可能的机器人路径，每个粒子中维护着一张地图，以该粒子所对应的机器人路径为条件，利用扩展卡尔曼滤波估计地图中各个特征的高斯分布。通过将粒子滤波和扩展卡尔曼滤波的有机结合，FastSLAM极大地减少了粒子需求数[10]。

1.2 室内未知环境的路径规划

室内未知环境的路径规划算法可分为3类:1)随机遍历策略，如迂回往复式、内外螺旋式[18];2)沿边学习加局部路径规划方法，主要应用算法为细胞分解法，应用的局部路径规划方法有人工势场法、完全应激式算法、模糊逻辑算法[17];3)漫步式探测路径规划[19]，机器人根据自身传感器探测周围环境信息，并在可视区域根据一定的算法生成局部运动的规划目标，将机器人的路径规划归结为逐步寻找下一步最佳视点的递归。采用此种策略的完全遍历规划方法有强化学习方法、基于随机树结构的方法、生物激励的神经网络方法、探测路径规划[13]。

随机遍历策略有迂回往复式、内外螺旋式、随机转向式、时间转向式、模板模型法、随机局部遍历的方法。这些方法的共同点是:1)不采用现在通用的效益函数;2)规划算法简单，方便低成本软硬件设计实现。总的来说，在不采用效益评价函数，如工作时间、能量损耗、重复覆盖率等的前提下，可以达到长时间上的大范围覆盖未知环境。简单的随机遍历策略广泛应用于清洁机器人，如科沃斯地宝系列机器人。迂回往复法也经常作为其他算法的底层策略，或算法判断后所采取的方法[16]。

沿边学习加局部路径规划方法中机器人在沿边学习的过程中可以获取室内未知环境的全局信息进行建模，而后采用全局视角和局部路径规划相结合的方法，遍历整个室内的未知环境。其思想是机器人首先沿着未知室内环境边沿行走一周，然后选择某个位置向四周观察，确定合适的探测区域前进，并以此循环下去，典型的算法有MTCP[17]算法。进行局部路径规划时，机器人可以建立虚拟子目标，主要应用算法为细胞分解法，该方法在机器人进行沿边学习之后，算法将环境分解为多个细胞，每个细胞设置一个基点，机器人走遍了基点则完成了遍历。人工势场法、完全应激式算法、模糊逻辑算法以及启发式函数也可以作为路径评价的判断标准。

虽然沿边学习方法可以获得部分环境信息，但是由于一方面此方法需要对环境进行建模，另一方面传感器信息不完全，因此该方法有局限性。而漫步式路径规划算法则可以避免这些缺点，其方法有强化学习法、基于随机树结构的方法、基于生物激励的神经网络方法和探测路径规划方法。

对于探测路径规划方法，其规划序列为首先选择一组下一步观测视点的候选位置，然后通过效用函数评估这些候选位置，选择使效用函数最大化的位置作为下一个观测视点。探测路径规划主要包括2个方面:候选视点的生成和评价。在机器人进行探测时，首先应当考虑的是信息收益和路径成本。

候选点的生成算法一般为前沿(Frontier)理论、随机生成候选点理论以及这2种方法的混合算法。前沿理论基本思想是将下一步探测视点放置在已探测环境与未探测环境的交界线上，从而期望获得最大化信息收益。随机生成的方法是在传感器扫描区域的圆或者圆环上以随机的方式生成一定数量的候选视点。对候选视点的评价中，信息收益是一项重要指标，一般有2种方法进行衡量:一种是直接衡量传感器探测空间的未知区域的几何信息法;另一种是采用熵的概率信息法[16]。此外，路径成本也是效益评价函数里的常见参数，如何衡量定位不确定性，成为了目前研究的热点。

2 本文方法

2.1 基于路标的全局位置估计探索策略

本文提出的室内未知环境机器人路径规划方法属于漫步式探测路径规划，其目的是最小化全局定位误差。机器人从室内某一初始位置出发后先随机游走直至发现一定数量的路标，而后选择下一步的探测路径。本文的思想是首先选择一组下一步观测视点的候选位置，然后通过效益评价函数评估这些候选位置，选择使效益评价函数最大化的位置作为下一个观测位置。由于在整个可达空间中搜索下一个最佳探测规划位置意味着巨大的计算量，为减少计算量，一般对候选探测位置作一定的约束以提高计算效率。因此，探测规划研究的讨论主要集中在2个方面，一个是候选位置的生成，另一个是候选位置的评估。

本文方法将以当前自主移动机器人所在位置为圆心、r为半径的圆上随机生成的N个可达空间内的位置作为候选视点，而后分别预估自主移动机器人在这N个候选位置对应的N条路径上可能观测到的新路标的个数及位置、新路标的个数与该条路径预估新探测的环境面积的比例、已经探测的路标个数、已经探测的环境面积的比例。新探测的路标的位置则用随机的方法进行估计。候选位置的评价采用效用函数对候选视点进行量化评估，该方法考虑了全局定位误差最小化，并对较大转弯角度进行惩罚，以使机器人能够在降低全局定位误差的同时较为平滑地游走。该效用函数为

式(1)的目标函数表示全局定位误差，其中i表示需要估计位置的路标，m表示环境中需要估计位置的路标的个数，α表示机器人一步转弯的角度，est_again_location(i)表示结合全局位置估计和卡尔曼滤波的SLAM算法得到的第i个路标的估计坐标，est_location(i)表示上次得到的第i个路标的估计坐标，这2个函数的具体估计方法见3.2小节。punish(α)表示转弯角度的惩罚项，本文简单地用机器人转弯时的角度的弧度制大小表示。

机器人一步优化路径规划的算法流程如下:

1)以预测的机器人所在位置为圆心、R为半径的圆上随机找N个在面板范围内的点;

2)分别预估机器人在这N条路径上会观测到的新路标的个数及位置;

3)通过模拟机器人在这N条路径上游走，分别计算每条路径对应的效用函数值;

4)选取效用函数值最小路径为下一步探测路径。

2.2 全局位置估计和卡尔曼滤波的SLAM算法

在传统的SLAM问题的相关的算法中，SLAM是一个滤波问题，是根据系统的初始状态和从0到t时刻的观测信息与控制信息估计系统的当前状态。卡尔曼滤波算法就属于这种思想，卡尔曼滤波器已经被广泛认为是实现SLAM的一种基本方法。然而，在实际情况中，当移动机器人移动变化过大时(例如速度发生变化或转弯时)，使用传统的SLAM相关算法，机器人的位置和真实的位置的误差会比较大，机器人转弯越急误差可能越大。基于此本文提出结合全局位置估计和卡尔曼滤波的SLAM算法，由于全局定位信息更少地依赖历史运动轨迹，与卡尔曼滤波等传统结合使用不仅可以减小机器人游走时的估计误差，而且可以有效降低机器人在转弯时估计的误差。

不失一般性，本文假设机器人所在的真实物理位置为 (xi，yi)，i∈ {1，2，...，n}，选取的已观测到的路标的真实位置为(xti，yti)，路标的估计位置为(xsi，ysi)，假设需要估计的机器人的位置为(xs，ys)，优化的目标函数为

记机器人与第i个路标的真实距离为

这样，本文有n个这样的式子，分别将第i个式子减去第n个式子得到n－1个如下的等式:

这是一个最小二乘问题，记A为n－1个式(3)的右边部分，即n－1个[2(xsi－xsn)2(ysi－ysn)]，则A为(n－1)×2的矩阵。记x为所求的[xsys]T，B为n－1个式(4)的左边部分，即B为(n－1)×1的矩阵。需要估计的机器人的位置的最小二乘的最后结果 x'= [xs'，ys']T=(ATA)－1ATB 。在机器人每一步行走过程中，本文用上述方法得出的估计位置和卡尔曼滤波得出的估计位置的平均值作为机器人最终的估计位置。

3 仿真实验

3.1 实验设置

本文对2种规划方法做了对比仿真，实验随机选择候选点运动和一步最优规划算法。机器人运动模型为本实验室全方位移动机器人模型，假设其可以感知半径10 m范围内的路标.环境大小为80 m×80 m的正方形，其中随机分布了30个路标，游走前机器人位于坐标(30 m，20 m)处，与x轴正方向成45o夹角，2种游走策略下自主移动机器人运行步数都是1 000步。对于本文的方法，在环境中均匀选取100个点，以估计全局误差。

3.2 实验结果

图1给出了不同探索策略的机器人从相同位置和角度出发在相等的运行步数下生成的运动路径。

图1 运动轨迹与探索的路标结果对比Fig.1 The comparison of trajectory and road signs

其中，略大的点表示环境中的路标，虚线表示自主移动机器人真实的游走路线，实线表示自主移动机器人分别使用随机游走策略和本文的方法的游走路线，略小的点表示估计的路标的位置，圆圈表示算法预计的路标的估计误差。实验结果表明:1)使用本文的游走策略探测到的路标更多，本文的游走策略未探测的路标有6个，而使用随机游走后未探测到的路标有13个;2)在观测到的路标上，使用随机游走策略的最大误差更大;3)使用本文策略后自主移动机器人游走的路线更平滑，在机器人位置的估计问题上，本文采用新算法后位置估计的误差有较为显著的减小。

3.2.1 全局路标定位误差比较

图2给出了自主移动机器人2种游走策略下的全局位置估计误差的比较结果。其中横坐标表示的是机器人游走的步数，纵坐标表示2种游走策略对应的在每一步的全局位置估计误差。由于机器人游走开始时会随机游走一段至发现一定数量的路标，所以横坐标不是从0开始，而是从发现一定数量的路标的时刻180开始的。机器人随机选择候选点的全局误差的均值和方差分别是1.821 2和1.062 4，而机器人采用基于路标的全局位置估计探索策略后的全局误差的均值和方差分别是 1.532 0和0.943 2。

图2 2种游走策略下的全局位置估计误差比较结果Fig.2 The global position estimation error under two kinds of migration strategy

3.2.2 自主移动机器人自身定位误差比较

图3给出了卡尔曼滤波的SLAM算法和结合全局位置估计、卡尔曼滤波的SLAM算法在机器人使用本文的游走策略时每一步的机器人定位误差的比较结果。其中横坐标表示的是机器人游走的步数，纵坐标表示每种算法在每一步对机器人自身位置估计误差的平均值。从图中可以看出在机器人游走初期，机器人探测到的路标不多，基于卡尔曼滤波的SLAM算法产生的误差和本文的方法的位置估计误差差别不大。同时，全局位置估计的误差变化较大，从图中可以看出，自主移动机器人在180～500步之间位置估计误差抖动较大。但随着机器人游走步数的增大，机器人探测到的路标增多，本文方法产生的误差明显减小。图中误差图有峰值出现，出现的原因是由于机器人转弯时的误差比平时大，从图中可以看出本文方法在机器人转弯时的误差比卡尔曼滤波的SLAM算法产生的误差明显小。卡尔曼滤波的SLAM算法产生的误差的均值和方差分别为1.046 7和0.842 7，结合全局位置估计和卡尔曼滤波的SLAM算法产生的误差的均值和方差分别为0.791 0 和0.409 1。

图3 2种SLAM算法的机器人定位误差比较Fig.3 The robot positioning error under two kinds of SLAM algorithm

3.2.3 2种游走策略下移动机器人耗时的比较

由于移动机器人采用基于路标的全局位置估计探索策略后，每次转弯等动作时需要较多的时间规划下一步的游走路径，所以相对于随机游走，移动机器人游走的耗时增加。实验表明移动机器人采用路标的全局位置估计探索策略时耗时为237.588 2 s，而移动机器人随机游走时耗时为81.959 9 s。

实验结果表明，相对于卡尔曼滤波的SLAM算法，采用结合全局位置估计和卡尔曼滤波的SLAM算法后机器人自身位置估计的精度有明显提高，但耗时相对增加。改变初始条件，多次实验，每次的误差数据不尽相同，但得到的结论相同。

4 结束语

采用基于路标的全局位置估计探索策略后，相对于机器人随机游走，机器人的全局定位误差有了较为明显的减小。结合全局位置估计和卡尔曼滤波的SLAM算法相对于基于卡尔曼滤波的SLAM算法，可以有效地减小机器人位置的估计误差。更多的实验分析以及机器人多步优化路径规划问题等，值得进一步研究。

[1]DURRANT-WHYTE H F.An autonomous guided vehicle for cargo handling applications[J].International Journal of Robotics Research，1996，15(5):407-440.

[2]MONTEMERLO M，PINEAU J，ROY N，et al.Experiences with a mobile robotic guide for the elderly[C]//Proceedings of the 18th National Conference on Artificial Intelligence.Edmonton，Canada.2002:587-592.

[3]HUNTSBERGER T，AGHAZARIAN H，CHENG Y，et al.Rover autonomy for long range navigation and science data acquisition on planetary surfaces[C]//Proceedings of IEEE International Conference on Roboticsand Automation.Washington，DC，2002:3161-3168.

[4]SMITH R，SELF M，Cheeseman P.Estimating uncertain spatial relationships in robotics[M].Autonomous Robot Vehicles，1990:167-193.

[5]LI C，HUANG Y，KANG Y，et al.Monocular SLAM using verticalstraightlineswith inverse-depth representation[C]//Proceedings of 7th IEEE World Congress on Intelligent Control and Automation.Chongqing，China，2008:3015-3020.

[6]THRUN S，BURGARD W，FOX D.A probabilistic approach to concurrent mapping and localization for mobile robots[J].Autonomous Robots，1998，5(3/4):253-271.

[7]GRISETTI G，STACHNISS C，BURGARD W.Improved techniques for grid mapping with Rao-Blackwellized particle filters[J].IEEE Transactions on Robotics，2007，23(1):34-46.

[8]张恒，樊晓平.移动机器人同步定位与地图构建过程中的轨迹规划研究[J].机器人，2006，28(3):285-290.ZHANG Heng，FAN Xiaoping.Mobile robot trajectory planning in simultaneous localization and mapping problem[J].Robot，2006，28(3):285-290.

[9]NEWMAN P.On the structure and solution of the simultaneous localisation and map building problem[D].Sydney:University of Sydney，1999:1-5.

[10]熊蓉.室内未知环境线段特征地图构建[D].杭州:浙江大学，2009:10-23.XIONG Rong.Line feature map building for unknown indoor environments[D].Hangzhou:Zhejiang University，2009:10-23.

[11]DISSANAYAKE M W M G，NEWMAN P，CLARK S，et al.A solution to the simultaneous localization and map building(SLAM)problem[J].IEEE Transactions on Robotics and Automation，2001，17(3):229-241.

[12]CASTELLANOS J A，MONTIEL J M M，NEIRA J，et al.The SPmap:A probabilistic framework for simultaneous localization and map building[J].IEEE Transactions on Robotics and Automation，1999，15(5):948-952.

[13]WILLIAMS S，DISSANAYAKE G，DURRANT-WHYTE H F.Towards terrain-aided navigation for underwater robotics[J].Advanced Robotics，2001，15(5):533-549.

[14]THRUN S.Probabilistic algorithms in robotics[J].AI Magazine，2000，21(4):93-109.

[15]HAHNEL D，BURGARD W，FOX D，et al.An efficient FastSLAM algorithm for generating maps of large-scale cyclic environments from raw laser range measurements[C]//Proceedings of 2003 IEEE/RSJ International Conference on Intelligent Robots and Systems.Las Vegas，NV，2003:206-211.

[16]李一波，张庆涛.室内未知环境遍历路径规划算法综

述[J].计算机科学，2012，39(11A):334-338.

LI Yibo，ZHANG Qingtao.Review of coverage path planning arithmetic in unknown indoor environment[J].Computer Science，2012，39(11A):334-338.

[17]ULRICH I，MONDADA F，NICOUD J D.Autonomous vacuum cleaner[J].Robotics and Autonomous Systems，1997，19(3):233-245.

[18]OH J S，PARK J B，CHOI Y H.Complete coverage navigation of clean robot based on triangular cell map[J].IEEE Transactions on Industrial Electronics，2004，51(3):718-726.

[19]JEBARI I，BAZEILLE S，FILLIAT D.Informatics in control，automation and robotics[M].Berlin:Springer，2012:224-237.