融合静态博弈的EKF协同定位方法
华承昊，窦丽华
(北京理工大学 自动化学院，北京，100081)
摘 要：
除多机器人协同定位时的冲突相对观测信息，使协同定位中各机器人所利用的信息更具一致性、提高协同定位的实时性和有效性，提出融合完全信息静态博弈的EKF协同定位算法。该方法运用静态博弈理论对机器人间的相对观测信息冲突进行识别，依据博弈所获得的一致相对观测信息进行协同定位。针对机器人之间相对观测信息存在单向观测和双向观测2种类型，分别推导出不同的协同定位公式。仿真实验结果表明：算法使机器人团队在协同定位时能更有效率地实现信息共享，在保证协同定位精度提高的同时减少计算量。
关键词：
中图分类号：TP242.6 文献标志码：A 文章编号：1672-7207(2013)11-4534-08
A new algorithm merging static game with complete information into EKF for multi-robot cooperative localization
HUA Chenghao, DOU Lihua
(School of Automation, Beijing Institute of Technology, Beijing 100081, China)
Abstract: For recognizing and eliminating the conflicting observations of multi-robot cooperative localization, while improving its consistency and effectiveness, a new cooperative localization algorithm which merged static game with complete information into EKF (extended Kalman filter) was proposed. The proposed algorithm uses the game theory to check the relative observations. After eliminating the conflicting relative observations, cooperative localization can be more effective. Since relative observations can be classified into two types, i.e one-sided relative observations and bidirectional relative observations, two sets of EKF cooperative localization formulations were respectively deduced. The simulation results show that the proposed algorithm makes the robot team only share coherent relative observations between them. It ensures the improvement of localization accuracy of every robot and reduces the computational complexity at the same time.
Key words: multi-robot; static game with complete information; cooperative localization; EKF algorithm
自主定位技术是移动机器人在各种复杂环境中都能有效地确定自身位姿，并根据人们赋予的任务作出正确的决策和路径选择的关键[1-3]。而如何更加有效地应用机器人自身所携带的内、外部传感器感知的信息，实现高效、可靠的位姿估计是当前机器人自定位尤其是自主协同定位研究中倍受关注﹑极具挑战性的课 题[4-5]。目前，无论是单或多移动机器人自主定位功能的有效、可靠实现，均是先通过收集自身所携带的各种内、外部传感器的信息，再运用各种优化理论和数据融合算法，经过一定的处理和变换，最后让自身或机器人团队对当前所处位姿进行准确估计[6-8]。现有的移动单机器人自主定位方法较成熟，故目前多机器人协同定位方法大多是将机器人间的相对观测信息交互后结合成熟的单定位方法加以扩展应用，如：Roumeliotis等[9]将多机器人集中EKF定位算法中的交互项巧妙分布化至相关的各个机器人上进行运算；Wang等[10]将混合PF-EKF定位方法运用于协同定位；Luca等[11]扩展并改进了单机器人的Rao-Blackwellized粒子滤波定位算法，降低了原算法在协同定位时对应用条件的要求；刘利枚等[12]则通过PF结合粒子群优化算法来解决异构机器人的协同定位。上述方法能较有效地处理多移动机器人的协同定位问题，但也存在未剔除其中冲突相对观测信息、产生额外计算量的问题，这也增加了卡尔曼滤波系列算法中滤波发散等问题出现的可能性。为克服这些不足，本文提出在EKF算法[9, 13]中融合完全信息静态博弈理论[14-15]，并针对多机器人间单向观测和双向观测这2种情况进行不同的博弈分析，分别推导各自的EKF协同定位公式。经仿真实验证实，应用博弈不仅有效地解决了多机器人协同定位时各机器人感知数据关联冗余的剔除、关联计算的融合问题，而且减少了协同定位过程中大量不必要的计算量，确保了协同定位的位姿更加准确、可靠。
1 多机器人间的协同定位模型
1.1 相对观测模型
当若干个机器人组成团队在某一结构化环境中进行协同定位时，比单机器人的自定位多了明显的信息优势：机器人间会经常有对同伴的观测信息，例如当机器人j出现在机器人i的视野内时，或者2个机器人迎面相遇时。为了清晰地描述这种机器人间的观测信息[5]以及其在此信息交互基础上的协同定位模型，进行如下假设：
(1) 机器人在结构化的平面环境中运动，每个机器人的位姿都可以用一个统一的全局坐标系下的二维坐标和朝向角所构成的向量来描述，即
。
(2) 每个机器人都配有自身运动感知传感器和外部环境感知传感器，能实现环境中的自我定位；能良好地识别其他伙伴机器人并获得对其的观测信息。
(3) 机器人之间能够进行实时通讯，某个机器人会及时地将它的自身信息和它对其他伙伴机器人的观测信息发给所有机器人队友。这个假设也是机器人间能够进行协同定位的重要前提。
(4) 整队机器人是同构的，或者至少它们的外部环境传感器是相同的，因而，它们彼此间的观测信息是相同类型的，可以用同一数学模型进行描述。更进一步，假设机器人的视野都是
，即只有当其他机器人位于机器人i前方180°的视角范围内才可能被i观测到。
当机器人i观测到机器人j时，用极坐标值
来表示该观测信息。
是机器人i的观测下，机器人i与j之间的距离；
是i到j的相对角度。这里，带括号的上标(ij)表示机器人i对机器人j的观测，上标(ji)表示j对i的观测。
若某时刻，i与j彼此观测到了对方，机器人i有观测信息，而机器人j相应地有观测信息
。假如观测量不存在任何噪声，则观测的距离信息与2个机器人间的距离
有如下关系：
(1)
而2个机器人各自测量的相对角度与机器人i到机器人j的朝向角差
存在如下关系：
(2)
其中：
；
。
图1 机器人i与j之间的相互观测信息
Fig.1 Relative observations between robot i and robot j
由于机器人自身定位的误差等因素，在某些特定情况下，可能在0°附近发生跳变而影响式(2)的应用。因为
和
周围的余弦相等，且其余弦以2为周期，所以, 将三角函数公式用于式(2)得到：
(3)
传感器的观测信息是带有噪声的，假设传感器的测量噪声符合白噪声(正态分布)，分别用
，
，
和
表示机器人i与j的外部传感器的观测噪声所服从的分布。因为机器人是同构的，所以，有
，
。用
表示所有相对距离的测量噪声，
表示所有相对角度的测量噪声。除了观测噪声，2个机器人自身定位还存在不确定性，增加了机器人间相对观测信息不一致乃至冲突的情况。为此，本文在判断机器人相互间观测信息是否一致的基础上引入完全信息静态博弈理论，通过机器人间互动博弈而自主选择最优的合作时机。
1.2 协同定位的博弈模型
令G={1, 2, …, n}为所有机器人的集合，每个机器人对应1个编号，是1个独立的博弈参与者。第i个参与者的策略空间为si={0, 1} (其中，0表示独自定位，1表示进行协同定位)。
在博弈中，由所有参与人选定的策略来共同决定每个参与人的收益。当其他机器人采取某特定策略(s1, …, si-1, si+1, …, sn)，而机器人i采取策略si时，其收益函数为
ui(s1, …, s1, …, sn); i=1, 2, …, n
则n个机器人进行协同定位的博弈可用G={s1, …, sn; u1, …, un}表述。
2 融合博弈论的协同定位算法
2.1 单向观测时的协同定位
为了研究机器人间只有单向观测时如何进行协同定位的问题，引入以单向观测信息与被观测机器人自定位信息的一致程度为衡量指标的博弈机制，建立一个完全信息的静态博弈[14]。
单向观测时，只有一方机器人被观测到。依据博弈论的原理，首先定义博弈的参与者为机器人i和被i观测到的机器人j，参与者i为参与者j提供观测信息，而j的策略是根据i提供信息的效果来决定是否采用该信息。在观测时所有参与者都同时采取行动，因此，博弈是一个静态博弈。
博弈中的行动、信息、策略、收益等基本元素分别定义如下。
信息：i对j的观测量以及i和j各自的定位位姿为博弈中的信息。由于整队机器人之间都可以及时地通讯，所以，本博弈中的信息以及其他元素都是参与者的共同知识，本博弈是一个完全信息博弈。
策略：参与者每一个可能的行动就是一个策略，在单向观测的协作定位中机器人的策略分为合作/不合作。
收益：i对j的定位信息与j自身定位信息的一致程度作为合作时的收益，并将各自定位时的收益设为1。
收益函数：当只有机器人i观测到机器人j时，其对j的定位如下：
(4)
其中：上标{i}表示机器人i的观测的结果。求上式的雅克比矩阵：
(5)
其中：
。
已知机器人i的自定位误差
和i的测量误差
，机器人i的位姿误差的协方差矩阵Pi和相对观测量噪声的协方差矩阵Rij。应用误差传递公式，求得i对j的定位误差的协方差矩阵为
；Pij为2×2的矩阵。
以
和Xj之间的距离相近程度为2个机器人合作效果的评判标准，通过机器人的位姿不确定度椭圆中长轴顶点到短轴顶点的距离来衡量。
的特征值分别为位姿不确定度椭圆的半长轴和半短轴，记为aij和bij。类似地，将
的特征值求出并记为aj和bj，得到：
(6)
由式(6)得到博弈的收益函数：
(7)
为了达到协同定位后改善机器人j定位效果的目的，进一步要求Pij＜Pj，由位姿误差的协方差矩阵主对角元所构成的椭圆的面积来进行比较，即
＜
。
在单向观测时，机器人i只能独自定位。机器人独自定位时的收益为1。因此，将机器人i与机器人j的博弈表示如下：
求解此博弈的纳什均衡[15]。当f＞1时，有uj[Si, Sj=1]=f＞uj[Si, Sj=0]=1，此时，协同定位是机器人j的占优策略。 博弈可以达到唯一的纳什均衡(独自定位，协同定位)；当f＜1时，有uj[Si, Sj=1]=f＜uj[Si, Sj=0]=1，此时各自定位是双方的占优策略。博弈可以达到唯一的纳什均衡(独自定位，独自定位)；当f=1时，无论合作与否，参与者的收益都是1，而协同定位会带来额外的计算，因此，各自独立定位是占优的。
当2个机器人协同定位时，采用EKF协同定位算法[9, 13]。EKF协同定位算法分2种。
(1) 预测。当前时刻机器人i和j各自采用EKF单机器人定位算法计算的位姿作为预测步骤的结果。
预测位姿
，预测位姿误差的协方差矩阵
。
(2) 更新。此时协同定位的观测方程为：
(8)
其中：v为相对观测噪声，其协方差矩阵为Rij。
观测方程的雅克比矩阵为
(9)
其中：
；
；
。
运用EKF公式求得卡尔曼增益：
(10)
单向观测情况下协同定位的位姿为
(11)
其中：
位姿误差的协方差矩阵为
(12)
最后，将计算结果分别赋予2个参与协同定位的机器人。
2.2 双向观测时的协同定位
当出现2个机器人间相互有观测信息时，就有了双向观测，此时，需要重新设计博弈机制。应同时考察相对距离和相对角度测量的一致程度，以之作为衡量指标，并建立相应的完全信息静态博弈的算法。此时，博弈的参与者及其策略空间不变，但增加了另一参与者的观测信息。从而，将收益重新定义为：i对j的相对观测信息与j对i的相对观测信息的一致程度作为合作时的收益。并将各自定位时的收益设为1。
收益函数：首先考察
和
的一致程度，以2个测量值的差
作为判断标准。已知它们是独立同分布的，都服从
，则
~
。由正态分布的性质知：当＜
时，可以认为和是一致的。用表示和一致程度的函数
作为相对距离博弈中机器人选择合作时的收益函数。
再比较
和
的一致程度。由于和是机器人i和j以各自为中心获得的测量角度，无法进行直接比较，它们的一致程度需要通过与的比较而间接得到。
的方差为
，其中
和
分别是机器人i和j位姿角度分量的方差。按照统计学规律，取
区间的置信度，若的误差小于
，则认为两者的一致程度在可接受范围内。由(3)式知：
应该在区间
内。因此，定义：
(13)
为相对角度博弈中机器人选择合作时的收益函数。从收益函数的公式可知该收益一定为非负数。
根据以上分析，可将相对距离博弈表示如下：
求解所表示的博弈的纳什均衡。当
＞1时，对于参与者机器人i，有ui[Si=1, Sj]=＞ui[Si=0, Sj]=1；对于机器人j，有uj[Si, Sj=1]=＞uj[Si, Sj=0]=1，此时，协同定位是双方的占优策略。博弈可以达到唯一的纳什均衡(协同定位，协同定位)。当＜1时，对于机器人i，有ui[Si=1, Sj]=＜ui[Si=0, Sj]=1；对于机器人j，有uj[Si, Sj=1]=＜uj[Si, Sj=0]=1，此时，各自定位是双方的占优策略，博弈可以达到唯一的纳什均衡(独自定位，独自定位)。当=1时，合作与否参与者的收益都是1，而协同定位带来的额外计算量可视为一定的负收益，此时，选择各自独立定位是占优的。相对角度博弈表示如下：
求解所表示的博弈的纳什均衡与之前类似。当
＞1时，对于参与者机器人i，有ui[Si=1, Sj]=＞ui[Si=0, Sj]=1；对于机器人j，有uj[Si, Sj=1]=＞uj[Si, Sj=0]=1，此时，协同定位是双方的占优策略，博弈可以达到唯一的纳什均衡(协同定位、协同定位)。当＜1时，对于机器人i，有ui[Si=1, Sj]=＜ui[Si=0, Sj]=1；对于机器人j，有uj[Si, Sj=1]=＜uj[Si, Sj=0]=1，此时，各自定位是双方的占优策略，博弈可以达到唯一的纳什均衡(独自定位，独自定位)。当=1时，因为合作与否参与者的收益都是1，此时，选择各自独立定位是占优的。
双向观测时，只有当以上2个博弈的结果都是合作时，机器人i和j才会采取协同定位，并运用EKF协同定位算法。与单向观测时类似，双向观测时的EKF协同定位算法也是分2步。
(1) 预测。当前时刻机器人i和j的各自定位位姿
作为预测步骤的结果。预测位姿，预测位姿误差的协方差矩阵
。
(2) 更新。协同定位的观测方程为：
(14)
其中：v为2个相对观测噪声之和，其协方差矩阵为2Rij。观测方程的雅克比矩阵为
(15)
其中：
；；。
运用EKF公式求此协同定位的卡尔曼增益：
(16)
双向观测时协同定位的位姿为
(17)
其中：
双向观测时位姿误差的协方差矩阵为
(18)
最后，将计算协同位姿X与协同位姿误差P分别赋予2个参与协同定位的机器人。
3 仿真结果及分析
为了验证本文所提出的协同定位方法的有效性，在Open SLAM提供的SLAM(同步定位与制图)开源仿真程序的基础上[16]，创建1个多机器人协同定位的仿真平台。在1个200 m×200 m的仿真空间内，放置了若干由星号表示的路标和若干由三角形表示的机器人。每个机器人都能观测到其前方180°范围内出现物体与它的相对距离和相对角度，最大观测范围为50 m。
协同定位算法的定位轨迹如图2所示。从图2可见：3个同构机器人按照设定的各自轨迹运动，速度均为2 m/s；控制噪声为
=0.3 m/s，
=3°；模拟的激光测距仪测量噪声为=0.05 m，=3°。各机器人单独定位时采用EKF定位算法，而在出现相对观测时则采用本文提出的融合静态博弈的EKF协同定位算法。
图2 协同定位算法的定位轨迹
Fig.2 Localization trajectories of the proposed algorithm
在仿真实验中，初始位姿、运动轨迹各异的3个机器人间会出现多次相对观测数据。这些数据都可以分为单向观测数据和双向观测数据2类。将这2类观测出现时的机器人轨迹分别标识，见图3。
重复运行仿真实验10次，记录单向观测和双向观测这2类情况下剔除的冲突观测数据，见表1和表2。
从表1和表2可见：采用本文提出的协同定位算法后，多机器人协同定位更新的计算量分别减少了77.62%(单向观测时)和0.75%(双向观测时)；仅有单向观测时，观测机器人i只有对被观测机器人j提供更好的定位效果才能在博弈中“说服”机器人j与它采取合作，因此，在机器人已有较精准的EKF单机器人定位算法的前提下，本文提出的算法只保留了较少的单向相对观测数据；而有双向观测时，是对2个机器人相互观测数据的一致程度进行判断，因此，在模拟的观测数据足够精确的前提下保留了大量的双向相对观测数据。这也说明在观测信息得到相互印证的双向观测时，机器人间应该更多地采用协同定位的方式。
表1 博弈消除的单向观测冲突数
Table 1 Diminished one-sided relative observations after gaining 次
表2 博弈消除的双向观测冲突数
Table 2 Diminished bidirectional relative observations after gaining 次
图3 某机器人协同前后X向位姿误差的对比
Fig.3 Localization errors of X axis before cooperative localization and after cooperative localization
单向观测时，1个机器人参与协同定位前后，在X向分量的位姿误差如图3所示。从图3可以看出：每次协同后，该机器人在X向分量的位姿误差比协同前都有一定减小。
为了更准确地量化协同后定位误差的减少量，将参与协同定位的2个机器人协同前位姿误差的协方差矩阵减去协同后定位位姿误差的协方差矩阵，并分别标识2个机器人的数据。图4和图5所示分别为单向观测和双向观测时，每次协同定位后2个机器人位姿各分量的误差比协同前的减少量。从图4和图5可以看出：2个机器人的位姿精度通过协同定位都得到了一定提高。这验证了经过博弈所挑选的相对观测数据带来的合作效益都是正面的，本文提出的算法保证了每次协同定位后机器人位姿精度的提高。
图4 单向观测时2个机器人协同后位姿精度的提高量
Fig.4 Localization accuracy improvements using proposed algorithm when robots have only one-sided observations
图5 双向观测时2个机器人协同后位姿精度的提高量
Fig.5 Localization accuracy improvements using proposed algorithm when robots have bidirectional observations
4 结论
针对多移动机器人的协同定位问题，提出了一种可充分检测多机器人间相对观测数据一致性的融合完全信息静态博弈的EKF协同定位算法。根据相对观测可分为单向观测和双向观测的特点，算法设计了不同的博弈机制，并分别推导出不同的协同定位公式。算法的主要成果体现在：
(1) 通过互动博弈，机器人间在协同定位时能更有效率地实现信息共享，减小了计算复杂度。
(2) 博弈后所挑选的相对观测数据带来的合作效果都是正面的，保证了每次协同定位后机器人位姿精度都能有效提高。
参考文献：
[1] Thrun S, Fox D, Burgard W, et al. Robust Monte Carlo Localization for mobile robots[J]. Artificial Intelligence, 2001, 128(1/2): 99-141.
[2] Filliata D, Meyerb J A. Map-based navigation in mobile robots: Ⅰ. A review of localization strategies[J]. Cognitive Systems Research, 2003, 4(4): 243-282.
[3] Durrant-Whyte H, Bailey T. Simultaneous localization and mapping: Part Ⅰ[J]. IEEE Robotics & Automation Magazine, 2006,13(2): 99-110.
[4] Frese U. A discussion of simultaneous localization and mapping[J]. Autonomous Robots, 2006, 20(1): 25-42.
[5] Martinelli A, Pont F, Siegwart R. Multi-robot localization using relative observations[C]// Proceedings of the 2005 IEEE International Conference on Robotics and Automation. Barcelona, Spain, 2005: 2797-2802.
[6] Gutmann J S, Weigel T, Nebel B. A fast, accurate and robust method for self-localization in polygonal environments using laser range finders[J]. Advanced Robotics, 2001, 14(8): 651-667.
[7] Minguez J, Montesano L, Lamiraux F. Metric-based iterative closest point scan matching for sensor displacement estimation[J]. IEEE Transactions on Robotics, 2006, 22(5): 1047-1054.
[8] Zhou X S, Roumeliotis S I. Robot-to-robot relative pose estimation from range measurements[J]. IEEE Transactions on Robotics, 2008, 24(6): 1379-1393.
[9] Roumeliotis S I, Bekey G A. Distributed multirobot localization[J]. IEEE Transactions on Robotics and Automation, 2002, 18(5): 781-795.
[10] WANG Ling, WAN Jianwei, LIU Yunhui, et al. Cooperative localization method for multi-robot based on PF-EKF[J]. Science in China Series F: Information Science, 2008, 51(8): 1125-1137.
[11] Luca C,Miguel K N,Jing J D,et al.Rao-blackwellized particle filter multi-robot SLAM with unknown initial correspondences and limited communication[C]//2010 IEEE International Conference on Robotics and Automation. Alaska, USA, 2010: 243-250.
[12] 刘利枚, 蔡自兴. 粒子群优化的多机器人协作定位方法[J]. 中南大学学报: 自然科学版, 2011, 42(3): 682-687.
LIU Limei, CAI Zixing. Multi-robot cooperative localization based on particle swarm optimization[J]. Journal of Central South University: Science and Technology, 2011, 42(3): 682-687.
[13] Thrun S, Burgard W, Fox D. Probabilistic robotics[M]. Massachusetts: MIT Press, 2005: 48-53.
[14] 罗伯特·吉本斯. 博弈论基础[M]. 高峰, 译. 北京: 中国社会科学出版社, 1999: 2-23.
Gibbons R. A primer in game theory[M]. GAO Feng, trans. Beijing: China Social Sciences Press, 1999: 2-23.
[15] 乔尔·沃森. 策略: 博弈论导论[M]. 费方域, 赖丹馨, 译. 上海: 上海人民出版社, 2010: 77-88.
Watson J. Strategy: An introduction to game theory[M]. FEI Fangyu, LAI Danxin, trans. Shanghai: Shanghai People’s Publishing House, 2010: 77-88.
[16] Bailey T, Nieto J. SLAM package of tim bailey[EB/OL]. [2013-03-15]. http://openslam.org/bailey-slam.html.
(编辑 陈灿华)
收稿日期：2013-03-15；修回日期：2013-05-14
基金项目：北京市教育委员会共建专项资助项目(XK100070532)
通信作者：华承昊(1983-)，男，江西于都人，博士研究生，从事智能机器人自主定位研究；电话：010-68912463；E-mail: huachenghao@bit.edu.cn
摘要：为有效识别和剔除多机器人协同定位时的冲突相对观测信息，使协同定位中各机器人所利用的信息更具一致性、提高协同定位的实时性和有效性，提出融合完全信息静态博弈的EKF协同定位算法。该方法运用静态博弈理论对机器人间的相对观测信息冲突进行识别，依据博弈所获得的一致相对观测信息进行协同定位。针对机器人之间相对观测信息存在单向观测和双向观测2种类型，分别推导出不同的协同定位公式。仿真实验结果表明：算法使机器人团队在协同定位时能更有效率地实现信息共享，在保证协同定位精度提高的同时减少计算量。
