第5卷第2期 智能系统学报 Vol.56.2 2010年4月 CAAI Transactions on Intelligent Systems Apr.2010 doi:10.3969/i.issn.1673-4785.2010.02.006 一种机器人未知环境下动态目标跟踪 交互多模滤波算法 伍明，孙继银 (中国人民解放军第二炮兵工程学院计算机应用系，陕西西安710025) 摘要：为了解决机器人同时定位、地图构建和目标跟踪问题，提出了一种基于交互多模滤波(interacting multiple model filter,IMM)的方法.该方法将机器人状态、目标状态和环境特征状态作为整体来构成系统状态向量并利用全 关联扩展式卡尔曼滤波算法对系统状态进行估计，由此随着迭代估计的进行，系统各对象状态之间将产生足够的相 关性，这种相关性能够正确反映各对象状态估计间的依赖关系，因此提高了目标跟踪的准确性.该方法进一步和传 统的IMM滤波算法相结合，从而解决了目标运动模式未知性问题，IMM方法的采用使系统在完成目标追踪的同时 还能对其运动模态进行估计，进而提高了该算法对于机动目标的跟踪能力.仿真实验验证了该方法对机器人和目标 的运动轨迹以及目标运动模态进行估计的准确性和有效性. 关键词：IMM滤波；EKF滤波；同时定位；地图构建；目标跟踪；移动机器人 中图分类号：TP242.6文献标识码：A文章编号：16734785(2010)020127-12 An interacting multiple model filtering algorithm for mobile robots to improve tracking of moving objects in unknown environments WU Ming,SUN Ji-yin (Department of Computer,The Second Artillery Engineering College,Xi'an 710025,China) Abstract:A novel method was developed for synchronous localization and mapping (SLAM)and object tracking (OT)to provide simultaneous estimation of a robots and any objects trajectories in an unknown environment.The system was based on interacting multiple model (IMM)filtering.In this approach,the states of robots,objects and landmarks were used to form an integrated system state.A full covariance extended Kalman filter (EKF)was then employed to estimate system state.As the iterative estimation progressed,sufficient correlations between the differ- ent objects in the system could be establish to reflect the interdependent relationships of estimations between differ- ent system objects.In this way the precision of object state estimation was improved.Moreover,when combined with a traditional IMM filter algorithm,this method solved the uncertainty problem for modes of object motion.With the application of IMM,the method helped robots to track objects and estimate their modes of motion,improving the accuracy of object localization.Simulation results validated the effectiveness of the proposed method in the esti- mation of the trajectories of robots and objects and the modes of motion of tracked targets. Keywords:interacting multiple model filter;extended Kalman filter;simultaneous localization and mapping;object tracking;mobile robot 机器人同时定位与地图构建(SLAM)是指机器 程.SLAM的难度在于准确的地图构建依靠于准确 人在未知环境下，根据传感器信息迭代地完成环境 的机器人位姿估计，而准确的机器人位姿估计反过 地图构建，并同时对机器人位姿状态进行估计的过 来又依靠于准确的环境地图.对于这个“鸡生蛋，蛋 生鸡”的问题主要存在2类解决方法，其一是基于 收稿日期：200908-30. 通信作者：伍明.E-mail:hyacinth531@163.com 扫描点匹配的方法13]，其二是基于Bayes估计的方
·128· 智能系统学报 第5卷 法，而基于Bayes估计的方法又主要分成基于扩展 x-1+4-△tcos(θ-+ys-1) 式卡尔曼滤波(EKF)的方法[4和基于粒子滤波的 yR-1+k-1△t8in(0瞻-+Yk-i) (1) 方法9).基于Bayes估计的SLAM解决方法的难点 0-l+(k-△t/B)·sin(yk-i） 在于对估计误差的正确处理，文献[10]已经对 SLAM误差产生的原因进行了分析 式中：△：为状态更新间隔，B为机器人轮毂长度，该 对于某些实际任务单凭传统的SLAM方法是无 轮式机器人模型符合非完整约束条件， 法解决的，例如：未知环境下目标跟踪任务，未知环境 1.2目标运动模型 下机器人围捕任务.这些任务要求将SLAM方法和目 目标在运动过程中一般并非保持惟一运动模 标跟踪方法相结合，该问题的难度集中于：如何在完 式，这里假设有2种可能的目标运动模型，即定速度 成未知环境下机器人定位任务的同时对未知运动模 模型(the constant velocity model,CVM)以及定加速 态的目标进行跟踪，并同时保证机器人和目标定位的 度模型(the constant acceleration model,CAM) 准确性.Wang等人21首先对该问题进行了研究并 首先定义CVM,假设目标在时刻k的状态由平 提出一种基于扫描点匹配的解决方法，由于该方法采 面笛卡尔坐标xCM和y以及在X、Y轴方向上 用的是最近点迭代(ICP)匹配算法，因此无法表示机 的速度M和M组成，即 器人和目标的不确定性.赵璇等3]提出了一种基于 XIVMCVM yICVMM yCVM] 粒子滤波解决方法，但该方法存在计算量大，以及地 则根据文献[2]介绍的方法可知目标CVM运动模 图长期一致性难以保证的问题4]， 式预测函数为 本文提出了一种基于MM的机器人同时定位、 MM(M,AM)= 地图构建和目标跟踪(simultaneous localization and mapping witl汕object tracking,SLAMOT)方法.该方法 AX四Y+9. (2) 将传统的IMM目标追踪算法和基于EKF的SLAM 式中：41为状态转移矩阵，9为目标运动随机 算法相结合，完成了在未知环境下对机器人轨迹和 参量，其符合方差阵为Q的高斯白噪声， 目标轨迹的同时估计.仿真实验表明该方法对机器 对于CAM来说，目标的状态为xCW= 人轨迹、目标轨迹以及目标运动模态估计均较为准 [x4y“],其中和 确，并且由于系统所有状态向量均存在相关性，因此 A分别为目标在X、Y轴方向上的加速度，类似于 能够保证较好的地图长期一致性. CVM可以将CAM目标状态转移函数表示为 1机器人和目标的运动模型以及观测 MA(MAM) 模型 Ax4+9. (3) 式中：4为状态转移矩阵，q为目标运动随机 本节主要介绍系统所涉及到的相关运动模型以 参量，其符合方差阵为Q的高斯白噪声，限于篇 及观测模型，在机器人同时定位、地图构建和目标跟 幅，这里对这些参数的具体取值不予介绍，读者可参 踪过程中首先需要建立机器人和目标的运动模型， 见文献[2]，图1显示了利用式(2)、(3)得到的目标 分别介绍如下 运行轨迹。 1.1机器人运动模型 18 CVM 设机器人状态为X=[xy],其中x、、 分别为k时刻机器人在全局坐标系下的平面笛卡尔坐 10 标以及相对于X轴的转角（标号上标表示描述对象，下 6 标表示时间).k-1=[k-1%-1]为k-1时刻机器人 2 的速度、角度控制量，“k-1存在均值为0、方差为Q的 0 -6 - 68 X/m 高斯噪声，则机器人运动模型如下： X=(X1,8-1,△t)= (a)CVM模态运行轨迹
第2期 伍明，等，一种机器人未知环境下动态目标跟踪交互多模滤波算法 ·129· 80 CAM 目标位置和 难点在于对于机器人状态和环境特征不确定性的刻 60 不确定范围 画上.因此对于机器人同时定位、地图构建和目标跟 目40 踪来说，其研究难点至少将是2个子问题难点之和. 20 下面给出基于IMM滤波的解决方法.算法总体流程 o 如图2所示. -20 -80 -60-40-20010 X/m 该算法主要包含2部分内容，其分别是基于 EKF的单模SLAMOT算法（图中对应Mode i EKF (b)CAM模态运行轨迹 方框)，以及基于MM滤波的多模目标跟踪算法.需 图1目标CAM和CVM模态运行轨迹图 Fig.1 The tracks of object in CVM and CAM model 要说明的是，整个算法是在MM框架下进行的，而 图1中2幅子图分别显示了目标以CVM和 基于EKF的单模SLAMOT算法是为了完成不同系 CAM运行时的轨迹，从该图可见，当目标以CAM运 统状态的预测和更新而设计的.具体来说，每当系 动时，无论其实际运动范围，还是位置不确定范围， 统获得环境规测值，将利用这些观测值对不同目标 均大于以CVM运动的结果.由此说明，CAM适合描 运动模态对应的系统状态分别进行更新，进而得到 述运动不确定性较大的目标，而CVM则适合描述运 不同的系统状态向量和协方差阵更新值和P, 动不确定性小的目标. 在此基础上，运用MM方法得到本次迭代最终的系 1.3特征、目标观测模型 统状态和协方差阵X。和Pk,以下对整个过程进行 假设特征lm的状态为其笛卡尔坐标，即X= 详细介绍。 [xy]T.观测量为距离和方向信息，即z= 状态估计 4,i 协方差组 [dy]I,则系统对目标T和特征lm:的观测函数分 合产生 别为 XiPv zi=[diye]=h(XK,Xi,R)= Mode 1 EKR 「√(-)2+(y-) ,P…i 模式概 +R (4) 更新 环境 Mode 2 EKF 观测值 和 XiPiVi 数据交互 i=[dim yim ]=hM (XR,X,Rim)= Mode s EKF X √(0-)2+(y-y) Xupe.Vi +Rm.(5) 图2 SLAMOTIMM滤波算法流程图 Fig.2 The flow chart of SLAMOTIMM algorithm 式中：X、X分别为机器人和目标的状态量，R是 2.1状态向量的表示 均值为0，协方差为diag(q,q)的观测噪声. 首先介绍系统状态的设置，这里将系统状态表 示成机器人状态、目标状态和环境特征状态组合的 2 基于IMM的机器人同时定位、地图 形式，即 构建和运动目标跟踪方法 X= (6) 目标跟踪和机器人SLAM研究的重点各有不 LLM: 同，例如，对于目标跟踪来讲研究的难点一般在于对 式中：X=[y]为机器人k时刻的状态值， 目标运动模型的未知性上，而机器人SLAM研究的
130. 智能系统学报 第5卷 指出的是系统最初的向量只包括机器人状态，而 为目标k时刻的状态值；LM= : 为当前已发现 目标状态X和环境特征状态LM是在机器人取得 特定观测值后逐步扩充进来的， 环境特征状态的组合（其中X:=[y]).需要 相应地，该系统的协方差阵具有如下形式： C(Ri,R:) C(R:,T) C(Rk,m1）… C(R:,Im)7 C(T,R) C(T,T) C(Tk,lm1)…C(Tk,lmn) P= C(m1,R）C(lm1,T) C(lm1,lm1)…C(lm1,lmn） (7) -C(Im,R)C(Im,Tx) C(Im ,Im)...C(Im,Im 为了形象起见，这里用Rk表示，Tk表示X, Xo=converse(X,z） 1m:表示X,C(A,B)代表A和B的协方差阵，由 x+dcos(y+)1 (9) 式(7)可知，该系统主要存在6种相互依赖关系，其 y+sin( 分别是：机器人自身之间的依赖关系C(R,R),目 这里假设对于目标和新环境特征的观测值分别 标自身之间的依赖关系C(Tk,T),环境特征自身 为z和z,由于该逆推函数只能提供对象的笛卡 之间的依赖关系C(m:,m:),机器人和目标之间的 尔坐标信息，因此式(8)中只得到目标状态X的前 依赖关系C(R,T),机器人和环境特征之间的依赖 2项.下面分别说明对Xt和X的扩展方法. 关系C(R,m),目标和环境特征之间的依赖关系 1)针对目标状态扩展.假设系统状态向量已经 C(T,m:),不同环境特征之间的依赖关系 包括n个特征向量（不包括特征的情况类似），则当 C(m,m),i≠i 得到目标X时扩展方法如下： 2.2EKF形式的SLAMOT解决 X 本节介绍如何用EKF来解决SLAMOT问题，其 X 主要分为：扩充阶段(augment)、预测阶段(predict)、 augment(,X): (10) 更新阶段(update). 2.2.1扩充阶段 初始阶段系统的状态向量只包括，在运行过 度前 扩恶后 程中，当第一次得到对于目标的观测值或新环境特 从上式可见目标状态子向量紧跟着机器人状态 征的观测值后系统将对状态向量和协方差阵进行扩 子向量。 展，本节介绍具体方法 2)针对新特征状态扩展假设系统状态向量已经 首先介绍对于状态向量的扩展.式(4)、(5)给 包括n个特征向量和目标状态向量（不包括特征的情 出了对于目标和环境特征的观测函数，这里可以得 况类似)，则当得到新特征Xm时扩展方法如下： 到其逆推形式如下，即已知X和z:条件下如何得 X 到X和Xe, g X=converse()= XI X 「x+dcos(y+) augment(Xk,Xao）: y y+di sin(y+o) Xie 0 0 (8) 守度前 .· 扩展后 0 (11)
第2期 伍明，等，一种机器人未知环境下动态目标跟踪交互多模滤波算法 ·131· 此时系统的已知特征数量为n+1. 1)针对目标的系统协方差阵扩展.假设系统状 在对系统状态向量完成扩展后还须对系统协方 态向量已经包括n个特征向量（不包括特征的情况 差阵P.进行扩展，同样分别对目标和新特征分别进 类似)，则针对目标T系统协方差阵扩展方法如下. 行说明. C(R,R:) C(Rk,m1） C(Rx,Im,) C(m1,R)C(lm1,lm1）…C(lm1,lmn) augment(P:,CE.g,CE:): C(mn,Rs)C(mn,m1）…C(mn,lmn） 扩展前 C(Rx,R) C(Rk,T)C(Rk,lm1)… C(Rk,lmn） C(T:,R) C(Tk,T)C(Tk,lm)… C(T,Im,) C(m1,R)C(lm1T)C(lm1,lm1)…C(lm1,lmn)） (12) C(Im,R)C(Im,T)C(Im,Im)..C(Im,Im)- 扩展后 式中 CE= converse() C(T,R)=(C(R,T.))=CExgC(Rx,R), z (13) cos(0+y)-di sin(+) (17) C(T:,T:)=CExC(R:,R:)(CExs)T sin(0+) d话cos(0瞻+y) CE:R"(CE:t)", (14) 式(16)、(17)分别为式(8)对机器人状态X和目 C(T:,Im;)=(C(Imi,T:))= 标规测值z的雅可比阵，式(13)~(15)分别由误 CExgC(Rx,Im;),i=1,2,...n (15) 差传播公式得到. a converse() 2)针对新发现目标的系统协方差阵扩展.这里 OXR 假设此时系统状态向量已经包括目标状态和n个特 [1 0 -d sin(0+) 征状态（不包含的情况可以类似得到），以下用m+ Lo 1 (16) di cos() 代表新发现的特征，那么系统协方差阵扩展如下， C(R:,R:)C(R:,T:)C(R:,Im).C(R:,Im,) C(Tx,R) C(Tk,Tk）C(Tk,lm1)…C(Tk,lmn) augment(P:,CExg,CE): C(Im,R:)C(Im,Tx)C(im,Im)..C(Im,Im) -C(mn,Rg)C(lmn,Tk)C(lmn,lm）…C(mn,lmn） 扩展前 C(R,R) C(Rx,Tx) C(R,1m1) C(RIm C(Rk,lma+i） C(T,Rx) C(Ti,T) C(Tk,1m1）… C(T&,Im,) C(Tx,Im) C(Im,Rx) C(m1,T)C(lm1,lm1)… C(lm1,lmn） C(lm1,lm+l） (18) C(Im,Rx) C(Im,,T:)C(Im,,Im) C(Im.Im. C(Im,Im) C(lml,Rs)C(m+1,T）C(m+1,m1）…C(m+1,m,）C(ml,mi) 扩展后