基于EKF和PF的多机器人协同定位技术

摘 要： 无论对于单机器人还是多机器人系统，定位能力是其完成各项任务的前提条件。针对常用定位技术难以满足移动机器人群体协同定位精度高、实时性强等要求，提出了一种基于扩展卡尔曼滤波（EKF）和粒子滤波（PF）的混合定位技术。首先，对机器人进行运动建模，然后分别讨论基于扩展卡尔曼滤波和粒子滤波的机器人协同定位的基本原理、优缺点，在此基础上提出基于扩展卡尔曼滤波和粒子滤波相结合的协同定位方法。结果表明，该算法在满足一定条件下可有效解决定位精度与运算量之间的矛盾，可实现在初始条件未知或存在较大误差情况下多机器人快速、精确地协同定位。

关键词： 多机器人系统； 协同定位； 扩展卡尔曼滤波； 粒子滤波

中图分类号： TN710?34； TP332.3 文献标识码： A 文章编号： 1004?373X（2013）23?0095?04

Multi?robots co?localization technique based on EKF and PF

TIAN Hong?bing1， FAN Guang?nan2， SONG Long2

（1. College of Computer and Communication， Lanzhou University of Technology， Lanzhou 730050， China；

2. College of Electrical and Information Engineering， Lanzhou University of Technology， Lanzhou 730050， China）

Abstract： The positioning capacity is a prerequisite to complete the tasks， no matter for single?robot or multi?robots systems. It is difficult for common location technique to meet the requirements of mobile robot groups in high co?localization accuracy and strong real?time. A hybrid location technique based on Extended Kalman Filter （EKF） and Particle Filter （PF） is proposed. The multi?robots are motion modeling， then the basic principle of robot co?localization based on EKF or PF and their advantages and disadvantages are discussed respectively. On this basis， another co?localization based on the combination of EKF and PF is proposed. The experiment result shows that this method is effectively in solving the contradiction between positioning accuracy and calculation under some condition. And when the initial condition is unknown or the errors is large， the multi?robots co?localization can also be rapidly and precisely.

Keywords： multi?robot system； co?operative localization； EKF； PF

0 引 言

随着人工智能、计算机、传感器等技术的不断发展，移动机器人的研究及应用得到了前所未有的进步。多机器人的群体协作由于具有比单一机器人系统更高的工作效率、鲁棒性、定位精度等优点而得到了广泛的关注，其中多机器人的协同定位问题成为近年来机器人研究领域的一大热点及难点。目前，机器人协同定位技术主要有：卡尔曼滤波、粒子滤波、最大似然估计、模糊逻辑、人工神经网络[1]。这些定位方法的基本思想是：如何融合内部传感器的测量信息和外部传感器的观测信息以减少过程误差和传感器的测量误差，得到更精确的机器人位置信息[2]。它们都有一定的适应范围，单一方法很难满足多机器人系统的复杂工作环境。针对初始状态未知或存在较大测量误差的情况，本文提出EKF和PF相结合的定位方法，即先用PF使初始条件收敛到EKF的初始误差允许范围，再用EKF进行迭代滤波，以达到快速、精确协同定位的目的。

1 多机器人系统协同定位技术框架

多机器人系统的协同定位是指多机器人群体利用相互之间的观测信息，在未知环境中互为路标，并通过信息交换，共享各个机器人获得的自身和环境测量信息，得到比单个机器人自身定位更精确的位姿估计[3]，其技术框架如图1所示。

单个机器人通常会配备多个内部和外部传感器以获取自身参数信息和周围环境信息。从图1可以看出，机器人的传感器信息经过滤波等预处理后，得到两种不同性质的定位信息：自身位姿信息和相对观测信息。然后运用EKF、PF等技术进行第二级的融合，最终实现协同定位。

图1 多机器人系统协同定位技术框架

2 机器人系统运动模型和观测模型

为进行机器人协同定位，首先需要建立机器人的运动模型和传感器的观测模型[3]。机器人的运动模型因其机动性设计的变化而有所不同。不失一般性，下面以二维平面内两轮机器人为例，介绍机器人运动模型和观测模型，并利用该模型进行各算法分析。

2.1 运动模型

如图2，图3所示，假设机器人在图中[A]点的位姿[（xk，yk，φk）]，运动到[B]点后的位姿变为[（xk+1，yk+1，φk+1），][φ]是机器人前进方向与[X]轴的夹角，[s1k，][s2k]是本次采样期间机器人左右轮运动的距离，[d]为两轮间距。移动机器人的运动模型建立过程如下：

图2 机器人运动的几何模型

图3 单个采样周期内机器人的运动形式

Step 1：以[A]点建立坐标系，求[B]点位姿[（Δx，Δy，Δφ）]，有：

[Δx=-0.5（s1k+s2k）sin（Δφ2）Δy=0.5（s1k+s2k）cos（Δφ2）Δφ=2arcsin[（s1k-s2k）4d]] （1）

Step 2：将上一步得到的位姿变换到图3坐标系中，有：

[xk+1yk+1=xkyk+sinφksinφk-cosφksinφkΔxΔyφk+1=φk+Δφ≈φk+（s2k-s1k）2d] （2）

Step 3：将式（1）代入式（2），可得到机器人的运动方程如下：

[xk+1=xk+0.5（s1k+s2k）cos（φk+（s2k-s1k）4d）yk+1=yk+0.5（s1k+s2k）sin（φk+（s2k-s1k）4d）φk+1=φk+（s2k-s1k）2d] （3）

Step 4：令状态变量[Xk=（xk，yk，φk）T，][Uk=（s1k，s2k）T，]则上述式（3）方程组可化为如下非线性函数形式：

[Xk+1=f（Xk，Uk）] （4）

2.2 观测模型

图4所示为某时刻机器人[i，j]进行相对观测的结果。其中[Dij]是相对距离，[α]是机器人[j]相对[i]的方向角，[β]是[i]相对[j]的方位角，统称为观测信息。相对观测信息与机器人自身位姿信息的数量关系称为观测模型，其具体关系如下：

[Zi=h（Xi，Yj）?Dij=（xj-xi）2+（yj-yi）2α=arctan（yj-yi）（xj-xi）-φi] （5）

其中观测变量[Zi]=[（Dij，α）T，]同理可得[Zj。]

图4 相对观测模型

3 扩展卡尔曼滤波

3.1 基本原理

对于线性系统而言， 卡尔曼滤波器是一个统计意义上最优的状态估计。机器人的运动和观测形式都是非线性的，所以可用扩展卡尔曼滤波[4?5]进行自身位姿信息和观测信息的融合。EKF求解过程如下：

Step 1：线性化即求解各方程的雅克比矩阵。

[Φk=?f（Xk，Uk）?Xk] （6）

[Rk=?f（Xk，Uk）?Uk] （7）

[Hik=?h（Xik，Xjk）?Xik] （8）

Step 2：预测。

状态预测：

[Xk+1=f（Xk|k，Uk）] （9）

方差预测：

[Pk+1|k=Φk?Pk|k?ΦTk+Rk?Q?ΦTk] （10）

观测量预测：

[Z=h（X（i）k+1|k，X（j）k+1|k）] （11）

Step 3：更新滤波参数。

卡尔曼增益：

[K=Pk+1|k?Hik?（HTik?Pk+1|k?Hik+ρ）-1] （12）

状态更新：

[Xk+1|k+1=Xk+1|k+K?（Z-Z）] （13）

方差更新：

[Pk+1|k+1=（1-K?HTik）?Pk+1|k] （14）

重复Step 2和Step 3直至估计值与真实值十分接近或相等。其中，[Q]和[ρ]分别为码盘误差和观测误差的方差，且初始状态和协方差已知。

3.2 算法的优缺点

扩展卡尔曼滤波是一个不断预测、修正的递推过程，其在求解时不需存储大量的观测数据，只是利用新的观测数据随时更新滤波参数，因此其便于实时处理观测结果。

由式（5）所示的观测模型方程知，当[ΔyΔx=][（yj-yi）（xj-xi）±∞]时，对应[arctan（yj-yi）（xj-xi）][±π2。]这意味着尽管两个机器人之间距离[Δx]非常小，由于定位误差的影响，当前时刻的[Δx]与前一时刻的[Δx]可能会发生符号的改变， 而[Δy]则较大或保持在一定的范围，其符号不变，从而导致了相对方向角[α]的跳变， 使相对方位的预测出现较大的误差，滤波出现发散情况，影响了定位的鲁棒性和可靠性。

4 粒子滤波

4.1 基本原理

粒子滤波器[6?8]的主要思想是利用一组带有权重的采样来表示系统状态的后验概率分布。粒子滤波器的结构示意图如图5所示。

图5 粒子滤波器PF的结构示意图

图5中的重采样是为了解决标准粒子滤波出现的粒子退化问题。重要性采样所需重要性密度函数的选择对粒子滤波至关重要。选取不同的重要性密度函数会产生不同的粒子滤波器如标准粒子滤波，高斯?斯密特、辅助、正则化等改进粒子滤波算法。

4.2 算法的优缺点

PF可以很好地表示非线性、非高斯模型，是解决及时定位和地图构建的有效手段，被认为是实现真正全自主移动机器人的关键 。

但是该算法仍然存在着一些问题，其中最主要的问题是依赖大量的样本数据才能很好地近似系统的后验概率密度，计算量大、效率不高。

5 EKF?PF混合定位

5.1 理论准备

综上可知，基于EKF和PF的协同定位方法有各自优势，但也各有不足。因此提出了基于EKF和PF相结合的协同定位方法。该混合定位方法步骤如下：

Step 1：初始化及预处理。初始化工作主要有完成PF、EKF所需的参数设置，然后作滤波去噪、放缩、限幅等预处理操作。

Step 2：使用PF算法直至初始条件收敛到满足EKF算法的初始误差范围。其中PF算法可根据情况选择一定的改进型粒子滤波算法。

Step 3：当初始误差满足EKF要求后，启用EKF滤波进行状态估计及更新，调整各机器人位姿，最终实现多机器人系统的协同定位。其中，EKF和PF的具体设计参照第3、4节。

理论上，上述混合定位方法可获得EKF和PF两者的优势，同时克服了两者的不足，在精确定位的同时使运算量减少，从而提高定位效率。

5.2 仿真结果与分析

为验证上述混合定位方法的优越性，进行以下仿真实验。选用了两个双轮机器人小车模型，其内外部传感器的数据采样频率为10 Hz和1 Hz，机器人小车宽0.7 m，以1 m/s的速度分别向东和向北运动，初始位姿分别为（5，0，0）、（0，5，[π2]）。分别采用上述三种算法进行协同定位状态估计。以单次实验数据为例，运用Matlab软件作出不同情况下两机器人小车的运动轨迹及协同定位误差[ρ]（偏离真实位置的距离）如图6，图7所示。

由仿真结果可以看出，这三种协同定位方法均能在一定程度上改善定位性能。单独EKF算法在某些情况下会产生滤波发散；单独PF算法虽最终能使估计值收敛到真实值，但所花时间较长，计算量较大；而基于EKF和PF的混合定位算法能更快、更接近机器人的真实运动，更好地实现协同定位。

图6 两个机器人的运动轨迹估计

图7 机器人定位误差对比

6 结 论

实验结果表明，基于EKF和PF的混合协同定位能得到比常用定位方法更精确的机器人定位信息，且速度更快。下一步将把上述算法运用到实体机器人上，以测试其在多机器人协同定位中的实际效果。该技术对未来的多机器人环境检测、编队协作等研究有重要参考价值。

参考文献

[1] 郭戈，胡征峰，董江辉.移动机器人导航与定位技术[J].微计算机信息，2003（8）：10?11.

[2] 邵金鑫.基于多传感器信息融合的多机器人协同定位技术研究[D].长沙：国防科技大学，2006.

[3] 刘显荣.传感器融合在多机器人合作定位中的应用[J].科技通报，2005（2）：277?281.

[4] 付梦印，邓志红，张继伟.Kalman滤波理论及其在导航系统中的应用[M].北京：科学出版社，2003.

[5] TESLI? L， ?KRJANC I， KLAN?AR G. EKF?based localization of a wheeled mobile robot in structured environment [J]. Journal of Intelligent and Robotic Systems， 2011， 62（2）： 187?203.

[6] 周翟和，刘建业，赖际舟.粒子滤波的改进优化算法在组合导航系统中的应用[J].中国惯性技术学报，2010（6）：711?715.

[7] 张淼，胡建旺，周云峰，等.改进粒子滤波算法的比较[J].电光与控制，2009（2）：30?32.

[8] 张弦.基于改进粒子滤波算法的移动机器人定位[D].北京：北京邮电大学，2010.

[9] 张诗桂，朱立新，赵义正.粒子滤波算法研究进展与展望[J].自动化技术与应用，2010（6）：1?9.

[10] 王炎庆.基于卡尔曼滤波器的神经元干细胞序列图像中活跃细胞的追踪[D].哈尔滨：哈尔滨工程大学，2010.

作者简介：田红兵 男，1976年出生，湖北孝感人，硕士，工程师。主要研究方向为嵌入式系统及计算机软件应用。

樊光南 男，1987年出生，江西吉安人，硕士研究生。主要研究方向为智能控制系统与技术、射频识别技术等。

宋 龙 男，1976年出生，甘肃静宁人，硕士，工程师。主要研究方向为计算机自动控制技术、电力电子技术等。