第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研究的