·132· 智能系统学报 第5卷 式中： X山k=产（X发，△t,4是+). (25) C(m+1,R)=（C(R,lmn+1))T= 同样将根据完成对目标状态子向量的预测，即 CExgC(R:,R:), (19) X=f(X,ARu,g). (26) C(Im,T)=(C(T,Im))= 因此经过系统状态预测后，系统的状态向量变化如下： CExgC(Rx,Tx), (20) X-=StatePredict(X:,,At,Aiu,)= C(Im,Im)=(C(Im,,Im))= Xk 「严（X装，△t,4数+k) CExgC(Rx,Im),i=1,2,.n, (21) Xi-uk f(X,A4k,9） (27) C(Im,Im)CExgC(R:,R.)(CExg)T+ LMk-uk LMLM CER(CE)T, (22) 2)系统协方差预测.协方差预测实际上可以理 converse() 解为一种不确定性传播过程，机器人和目标在状态 OXR 预测后不确定性也应相应传播扩大.这种传播扩大 [10-dsin(眼+yo) 不仅体现在机器人和目标本身的协方差子阵上 (23) L0 1 cos(08 +) (C(Rk,R)和C(Tk,T),也应体现在机器人和目 dconverse(X,zo） 标之间(C(R,T)、机器人和特征之间(C(R, CEme= mw 1m))以及目标和特征之间(C(Tk,1m))的协方差 cos(狼+y）-dsin(眼+ym)1 子阵上，以下分别介绍. sin(+y）dcos(6g+y) 对于机器人自身协方差子阵预测利用误差传播 公式可得 (24) C(Rk+k,R+I）= 式(23)、(24)分别为式(9)对机器人状态X FC(R:,R:)(F)+FF), 和特征观测值的雅可比阵，式(19)~(21)同 (28) 样由误差传播公式得到. 2.2.2状态预测阶段 r10-△tsin(0+Ys) 在SLAM0T过程中主要存在2个运动物体，其 F路= ax=01 v△tcos(瞻+Y) 分别是机器人和目标，状态预测阶段的任务就是利 L00 1 用两者的运动模型对其状态和系统协方差阵进行预 (29) 测.一般来说，作为观测主体的机器人其运动模型是 确定的，而作为观测对象的目标其运动模型并不确 定，本节假设已知机器人和目标的运动状态，后面章 △tcos(瞻+ya)-x△tsin(+a)T 节将考虑多模态情况下的应用. △tsin(0+yk) v△tsin(0+Ye) (30) 这里采用式(1)作为机器人的运动模型，记 △t&in(yz)/B △tcos(Ya)/B 为，目标的运动模型为式(2)和式(3)之一，记为 式中：C(Rk+k,Rk+)代表机器人预测协方差子阵 ∫.预测阶段包括：系统状态预测和系统协方差预 的值，Q代表机器人控制输入的误差阵，Fg、F 测2部分，分别介绍如下. 分别为式(1)对机器人状态X和机器人控制向量 1)系统状态预测.由于特征分布是固定不变 的雅可比阵。 的，因此系统状态预测操作的对象是式(6)表示的 类似地，对于目标自身协方差子阵预测值为 系统状态X,的前2项，即机器人状态子向量X和 C(TT)=AIC(T,T)(AI)+k 目标状态子向量X.这里将根据“得到预测后的 (31) 机器人状态子向量+为 式中：C(Tk+k,T+1k)代表预测后目标协方差子阵