基于粒子滤波的机器人主动定位算法

常天莉，黄浩晖，陈 玮

(广东工业大学 自动化学院，广东 广州 510006)

0 引 言

移动机器人定位问题[1]作为机器人系统中最重要的部分已经被研究多年。在足球机器人领域，机器人需要独自或者协同在同一坐标系系统里执行一系列任务，因此根据有限的环境信息预测机器人的当前位置是非常重要的。

过去针对移动机器人全局定位的求解问题，卡尔曼滤波定位算法(Kalman filter localization)[2]、马尔卡夫定位算法(Markov localization)[3]、以及蒙特卡罗定位算法(Monte Carlo localization)[4]等都得到了广泛的应用。然而上述定位算法都属于被动定位算法，即机器人在环境中随机移动，根据传感器随机获取的环境信息进行定位，这样就会降低系统的定位效率；对于具有多个局部相似或对称的环境特征，甚至不能求出机器人的正确位置。

针对上述问题，基于马尔科夫定位算法的主动定位算法[3]可以很好地提高机器人定位系统的性能，该算法通过选择机器人的行为使得机器人对位置估计的不确定性期望值达到最小，即机器人通过该算法可以选择有利于识别不同位置的行为。然而该算法只考虑了执行下一个行动单位时对状态环境的预估，考虑到机器人可能需要移动到较远的位置或者通过多次迭代计算才能唯一确定它的位置，这种算法的求解深度是不够的。因此学者们对主动定位算法不断进行改进[5]，通过使用启发信息，主动控制机器人的运动路径，使机器人能够区别不同的粒子簇，最终使机器人能收敛于真实的位置。

然而启发信息制定的合理性影响着机器人定位的求解效率，为了减少算法对启发信息的依赖以及增加算法的鲁棒性，本文基于改进的粒子滤波主动定位算法，研究智能体如何从带有噪声的视觉信息中快速定位，同时把算法应用到RoboCup2D足球仿真平台上，优化足球机器人在有限信息中的快速定位问题。

1 粒子滤波定位算法

在RoboCup2D仿真平台中，每个智能体都可以从服务器端(soccer server)每隔150 ms获取一次视觉信息：

(seeObjNameDistanceDirectionDistChngDirchngBodyDirHeadDir)

其中,ObjName表示所看到物体的类别(球、球员、标志、球门、边界)。而球场上每个标志都是已知和固定的，球员可以通过视觉系统很快地获取视觉范围内的任何标志来进行自定位。

1.1 非高斯噪声的视觉模型

在RoboCup2D仿真平台中，球员通过获取球场上已知并且固定的标志点信息[6]进行自定位，为了模拟真实环境，Soccer Server会对球员获得的视觉信息加入白噪声。对于传统的定位问题，这些噪声都可以用高斯分布来建模，然后利用卡尔曼滤波或者粒子滤波算法来求解；而Soccer Server则会用更复杂的噪声模型对视觉信息进行量化处理。假设球员到某个标志点的真实距离为d，球员通过噪声量化所得到的视觉距离为d′，则该量化过程可定义为

Ns:d→d′=Q(eQ(log(d),Δ1),Δ2)

(1)

其中，量化步长Δ1和Δ2分别为0.01和0.1，量化函数Q为

(2)

通过Server的噪声量化，若标志点与球员的距离有10m时，最大的噪声误差可以达到10cm。因此球员便难以从视觉信息中获取标志点的准确位置，针对该问题，本文将采用门限滤除技术搭建视觉的误差模型。

当球员在位置pagent(x1,y1)上观察到标志点Oi时，其位置范围的误差可用截尾扇形E来表示，如图1所示。扇形E的数学表达式如下

(3)

其中，Δr和Δθ分别表示最大距离误差和最大角度误差。在SoccerServer中，球员所获得的视觉信息和距离的有很大关系的，因此本文中的Δr和Δθ参数也会随着视觉距离而变化。

图1 标志点的误差区域E

在自定位过程中，球员通过获取球场上的标志点来预估自己的位置，因此相对于标志点可能的位置，我们更感兴趣的是当获取标志点信息后，球员所预估的位置。如图2所示，本文通过一定的几何等价方法，把以标志点为中心的扇形区域转换为以球员坐标为中心的扇形区域，同样的该扇形区域表示是球员预估坐标的误差范围。而在比赛过程中，球员所获得的标志点通常不止一个，我们假设标志点有N个，从而会产生N个误差区域E1…EN，因此球员通过N个标志点所得到的误差位置区域为

(4)

图2 球员的误差区域E

1.2 粒子滤波定位算法

(5)

(6)

其中，dt-1表示机器人通过执行动作at-1后所得到的位置变化矢量。

(2)获取视觉信息并计算粒子权重：机器人通过视觉感知模块获取观察信息Ot来更新机器人可能位置的置信度bel(xt)。由于SoccerServer中对于观察信息以及动作都会带有噪声误差，对于该误差范围利用图2中的误差区域E来表示，然后根据所获得的观察信息重新计算每个粒子的权重值

(7)

在RoboCup2D仿真系统中，如果球员获得的观察信息不够理想，即所获取的特征点不够多或者特征点的误差范围比较大时，则将增加上述粒子滤波定位算法中粒子权重的计算量和采样时间，从而严重影响系统的实时性。因此，本文将引入主动定位算法来提高求解效率。

2 基于视觉噪声的机器人主动定位算法

传统的粒子滤波定位算法属于被动定位算法，尽管它可以完成移动机器人的定位任务，但却需要花费较多的动作和时间来实现，因此学者们提出了基于熵值的主动定位算法[8]，该算法通过估计不同信念状态Bel(ξk)下的不确定度，得出机器人的最优行为动作集合，使得状态的不确定性期望值最小，从而有利于机器人辨识自身的位置。熵的期望值可以通过下面公式得到

Ea[H]=-∬Bela,s(ξk)log(Bela,s(ξk))p(s)dξds

(8)

然而机器人在定位过程中可能需要移动较远的距离才能够唯一地确定机器人当前的位置，也就是需要一个较长的基本行为队列，而在不能确定机器人位姿前提下的行为决策是毫无意义的，无目地“游走”只会降低机器人的定位效率。因此本文结合上一节的粒子滤波算法，基于GDUT_TiJi2014[9]的启发式行为决策系统，提出一种鲁棒的启发式搜索定位算法，使粒子能尽快收敛到真实位姿。

在启发式搜索的求解过程中，启发式信息的选取，即奖励机制与代价机制的确定在很大程度上制约着算法的求解效率；文献[9]中启发式函数定义如下

(9)

(10)

(11)

(12)

其中，α为最优权重值，且0≤α<1，决定了误差函数对决策选择的影响。基于视觉噪声的机器人主动定位算法流程如图3所示，算法每完成一次粒子滤波算法会计算一次粒子簇的方差，当方差大于阈值ε时，表示粒子过于分散，定位算法将利用特征点信息进行全局定位，从而提高定位精度，此时α≠0，即采用式(12)对策略系统进行求解；当方差小于等于ε时，粒子已经开始趋向收敛，此时问题可表示为机器人的位置跟踪问题，可令α=0，即采用式(9)对策略系统进行求解，提高机器人的定位效率。

图3 基于视觉噪声的机器人主动定位算法流程

3 仿真实验

本文实验把基于视觉噪声的机器人主动定位算法应用到GDUT_TiJi2016球队上，并通过自定位平均误差以及球队的进球能力两个方面来验证算法的有效性及可行性。

图4给出了使用了本文算法的GDUT_TijI2016与自带定位系统的agent2D[10]所进行的一场比赛中球员自定位的平均定位误差统计，GDUT_TiJi是以agent2D为底层所开发的2D仿真球队，本文定位算法代码使用C++设计，比赛场景在i5-5200CPU(2.20GHz)和Ubuntu12.04平台下进行。由图4对比可以看出，基于视觉噪声的主动定位算法大大提高了球员的自定位误差(由1.3m提高到0.4m)，当球员获得比较准确的定位后，在启发式函数的作用下，能一直保持在一个合理的误差范围内。而在定位过程中agent2D和GDUT_TijI2016在第70s时都出现了一个峰值，这是由于GDUT_TijI2016在第70s的时候产生了一个进球，soccerserver把两队的球员都进行重置(瞬间回到初始位置)，这体现了机器人中的绑架问题，然而该问题并没有影响机器人的定位，相比agent2D队伍，本文算法能让机器人更快地确定自己的位姿。

图4 球队自定位平均误差统计

在第二个实验中，我们使用了GDUT_TiJi2014[9]作对比，因为与GDUT_TiJi2014相比，GDUT_TiJi2016唯一改动的地方就是加入了基于视觉噪声的机器人主动定位算法。该实验使用GDUT_TiJi2014和GDUT_TiJi2016分别与agent2D球队进行了200场模拟比赛，表1统计了两个版本的平均进球数和失球数，由表1可得，定位算法也对球队的整体性能有了很大的提高。

表1 比赛结果统计

4 结束语

本文基于视觉噪声的粒子滤波算法和启发式搜索，提出了一个定位算法，运用门限滤除技术，对各粒子进行筛选，再利用结合了误差函数的启发式搜索来加快球员的自定位速度。通过实验验证了该算法在特征点的帮助下能有效地提高机器人的定位速度和精度，同时也提高了GDUT_TiJi2016球队的总体性能，验证了该算法的可行性。

然而，机器人足球是一个多智能体协作的平台，每个机器人所执行的动作都会对其它机器人产生影响。因此，下一步工作将要研究如何融合多智能体的所收集回来的状态信息[11]，进一步提高机器人的定位精度和速度。

[1]Wang Y,Chen W,Wang JWH.Active global localization based on localizability for mobile robots[J].Robotica,2015,33(8):2071-2076.

[2]MahmoudII,Salama M,Tawab AAE.Particle/Kalman filter for efficient robot localization[J].International Journal of Computer Applications,2014,106(2):20-27.

[3]Burgard W,Fox D,Thrun S.Markov localization for mobile robots in dynamic environments[J].Journal of Artificial Intelligence Research,2011,11(1):391-427.

[4]Kim TG,Ko NY,Noh SW,et al.Localization on an underwater robot using Monte Carlo localization algorithm[J].The Journal of the Korea Institute of Electronic Communication Scie-nces,2011,6(2):288-295.

[5]Liu Y,Zhang H,Cui P,et al.Mobile robot active global localization based on heuristic search[J].Journal of Computational Information Systems,2012,8(2):739-746.

[6]Akiyama H.RoboCup soccer 2D simulation league:Introduction to the RoboCup soccer simulator[J].Journal of Japan Society for Fuzzy Theory & Intelligent Informatics,2011,23:714-720.

[7]Zhao ZS,Shen JH,Wang JZ,et al.Research on self-localization methods for mobile robots based on Bayes filter[J].Computer Science,2011,38(2):199-201.

[8]Seekircher A,Laue T,Röfer T.Entropy-based active vision for a humanoid soccer robot[M]//RoboCup 2010:Robot Soccer World Cup XIV.Berlin:Springer Berlin Heidelberg,2011:1-12.

[9]HUANG Haohui,YANG Wanlu,CHEN Wei.Study on the A* average reward reinforcement learning algorithm based on performance potentials[J].Computer Simulation,2014,31(7):338-341(in Chinese).[黄浩晖,杨宛璐,陈玮.基于性能势的A*平均奖赏强化学习算法研究[J].计算机仿真,2014,31(7):338-341.]

[10]Akiyama H,Nakashima T.HELIOS base:An open source package for the RoboCup soccer 2D simulation[M]//RoboCup 2013:Robot World Cup XVII,2014:528-535.

[11]Franchi A,Oriolo G,Stegagno P.Mutual localization in multi-robot systems using anonymous relative measurements[J].International Journal of Robotics Research,2013,32(11):1302-1322.