由于低精度航位推算系统带来的累积误差，自治水下航行器（AUV）在未知环境中的定位准确性会随着时间的推移变得越来越差。当环境中有非合作体运动时，AUV不但可利用自身携带的声纳传感器对其探测并利用基于模型的目标跟踪方法估计非合作体轨迹，而且可以同时利用探测到的与非合作体的相对信息来提高自身定位精度。在同时定位与制图（SLAM）方法中的FastSLAM算法框架的基础上提出了同时定位与跟踪（SLAT）算法，即设置多个估计粒子，利用每个粒子中的粒子滤波器（PF）和扩展卡尔曼滤波器（EKF）分别估计AUV和非合作体的轨迹，并能根据实际量测与粒子估计量测之间的差别赋予粒子权值，继而得到多粒子加权的最终估计。最后仿真验证了算法的有效性。
6 2011,47(16) Computer Engineering and Applications计算机工程与应用 (M)(A k+1 (28) (13) 则有 0=arctan(M)Y(+ r (29) 设v=[Ak△OJ服从高斯分布,则Ev]=0,E[v(v,) 归一化所有粒子的权值: R 量测模型可被简单地描述为: (30) (A)y(M) (14) WK 则k时刻AUV的位姿和目标状态估计为 5利用 FastSLAM框架的同时定位与跟踪算法 1=2(Xk+)N+1 设定滤波器中有N个粒了,并设采样时间=7M,粒 子i构成可由下式表示 k+1k+1 k+1k+ (31) Sk=((xk y wk (x)(P 1,2,…,N(15) 其中,n为k时刻粒子的权值,(PAy为k时刻粒子目标 为了保证粒子的有效性,估计过程中经常需要进行重抽 M状态估计误差协方差。 样。判断是否需进行重抽样的指标为: 粒子i对AUV在k+1时刻的状态估计值可以表示为: (32) (XkA)y-P(XIAlu/ (XRA)) 根据式(3),可得 Nc代表有效粒子数,当Na小于设定最小粒子数时,需 (XAy=f(XxA3),aA)、3) (17)要进行重采样。根据权值大小,复制权值大的粒子,并删除权 根据式(4),可以推算得到目标M在k+1时刻的状态估计值小的粒子,最终重新抽取N个粒子,并设所有粒子权重为 值为 XEMIL-f((XHL), u,o (18) 在k+1时刻目标M的状态协方差的预测为 6一致性分析 正如文献3]中所提出的理想情况下衡量算法是香满 (P)=少(P)(小):CQ(G (19) 足一致性是将其估计值与理想滤波器中获取的概率密度函数 使用kH1时刻传感器量测更新系统状态,设在粒子i在k进行比较。在实际中理想概率密度函数是无法荻取的,但当 时刻利用状态量对量测值的估计为 被估计状态真值已知时,可以使用标准化估计误差方差(Nor- ZK=K OFT=h(XIA). (X)) (20) malized Estimation Error Squared,NEES)测试滤波一致性。 Ek=(X-X P(X-XR (33) 从而k+1时刻的新息和新息协方差为: k+1-2k+1 (21)0甲,k为状态真值,X为状态估计值,P为估计误差协方差。 如果滤波过程是一致的,且近似符合线性高斯的,那么c S41=H+1(Pk+1k)(H+)2+R (22) 将服从状态维数dim(X)个自山度的x2分布。当进行N次 滤波器增益为 Monte carlo实验且N趋于无穷时有: K+1=(Pk+1)(Hx+0)(S4)2 (23) ELk=dim(x,) (34) 则对日标状态向量及其协方差矩阵进行更新,有: 若进行有限N次 Monte carlo实验,VEES的均值为 k+1|k+1 k+1k+1 E (35) N (P+″4y=[1K1H1P+m (25)则N服从自由度为MmX)的x分布。对于AUV,其位 其中 姿维数为3,则进行N-50次 Monte car实验,对于,其取值 M) (A) k+lk 0000 + (A) 0000 (x1-x1)2-(1=y)2(联-2)2+(y4-y) 权值可表示为AUV的运动分布函数与建议分布函数之比:概率为95%的置信Ⅸ间为236,372]。对于日标,其状态向量 p((Xr ikke M k+1 (27)为6维,可近似简化操作取两维位置量,则取值概率为95y%0 M 的置信区间为[1.48,2,59。如果取值高于置信区间上限为 设21=(X)x),则可得: 乐观估计,反之,低于区间下限为保守估计 卢健,徐德民,张立川,等:AUⅤ同时定位与跟踪研究 2011,47(16)7 700 450 650 SLATE SLATE 600 真实轨迹 真实轨迹 肮位推算 550 细 300 500 200 100 20040060080010001200 100200300400500600700800400 50060070080090010001100120 X轴位置/m X轴位置/m X轴位置m 图1目标和AUV的实际航行线路 图2AUV位置佔计轨迹比较 图3目标位置估计轨迹比较 3.2r SLATF 一航位推算 2.6 三40 35 2.2 10 14 020406080100120140160 020406080100120140160 020406080100120140160 时间s 时间 时间/s 图4日标位置佔计误差的方差 图5AUV位置佔计的累积误差 图6标位置NEES均值 7仿真 2007,29(2):186-192 设目标M和AUV沿各自方向运动,设定AUV开始运动4 Maczka d K, Gadre A S, Stilwell D J. Implementation of a co 位置为坐标原点,AUV可实时探测到目标M,并获取相应的距 operative navigation algorithm on a platoon of autonomous un- 4离和方位信息。为了说明算法的有效性,本文仿真中将目标 derwater vehicles[C]/2007 MTS/IEEE International Conference M的真实航迹、AUV的真实航迹和航位推算轨迹显示以比 on OCEANS.Vancouver, BC, Canada: IEEE, 2007: 1-6 较。仿真结果如图1~6所示。 [5] Bahr A, Leonard J Cooperative localization for autonomous un 图1显示了目标和AUV真实的运动轨迹关系。图2显示 derwater vehicles[J]. Autonomous Robots: Proceedings of Experi 了AUV真实运动轨迹,航位推算结果和 SLATE的估计结果。 mental Robotics, 2006, 39(5): 387-395 图3显示了目标M的真实轨迹和 SLATE的航迹估计结果。图 [6] Vaganay J, Leonard JJ, Curcio J A, et al. Experimental valida 4展示了对目标M航迹估计的方差变化。图5对AUV的航位 tion of the moving long base-line navigation concept[C]/2004 ieeE/oeS International Conference on Autonomous Underwate 推算和 SLATE的佔计误差作了比较,证明佔计航迹时 SLATE 方法明显优于航位推算。图6说明了估计过程的一致性、 Vehicles. Nagoya, Japan: IEEE, 2004: 59-65 Baker B N, Odell D L, Anderson M J, et al.A new procedure for simultaneous navigation of multiple AUVs[C]/2005 MTS 8结论 IEEE International Conference on OCEANS. Washington DC 在阐述了AUV运动模型、量测模型以及日标基本运动模 LSA: IEEE. 2005: 1-4 型的基础上,栅究了以 Fastslam为框架的同时定位与跟踪方8] Thrun s, Burgard w, Fox D Probabilistic Robotics [M]. London 法,即用粒子滤波器对AUV的航迹估计,用EKF对目标航迹 MIT Press. 2005 估计,同时完成AUV的自定位和目标的稳定跟踪。从仿真结[] Bailey T, Nieto I, Guivant JConsistency of the EKF-SLAM al- 果中可以看出,使用这种方法,不但AUV白定位的累积误差 gorithm[C]//2006 IEEE/RSJ International Conference on Intelli 相比于航位推算减小很多,而且目标的航迹估计也很准确。 gent Robots and Systems. Beijing, China: IEEE, 2006: 3562-3568 [10 L azaro M T, Castellanos J A Localization of probabilistic ro 参考文献: bot formations in SLAM[C]/2010 IEEE International Confer- [1] Curcio J, Leonard J, Vaganay J, et al. Experiments in moving ence on Robotics and Automation. Anchorage, AK, USA: IEEE baseline navigation using autonomous surface craft[C]//2005 2010:3179-3184 MTS/IEEE International Conference on OCEANS. Washington [11] Durrant-Whyte H, Bailey TSimultaneous localization and map DC,USA:IEEE,2005:730-73 ping Part I[J].IEEE Robotics and Automation Magazine, 2006 [2] Gadre A S, Stilwell D Toward underwater navigation based 13(2):99-110 on range measurements from a single location[ C]/2004 IEEE [12] Bailey T, Durrant-Whyte H Simultaneous localization and map International Conference on Robotics and Automation. New Or ping: part I.IEEE Robotics and Automation Magazine, 2006 leans. LA. USA: IEEE. 2004: 4472-4477 13(3):108-117 3」许真珍,封锡盛多UUV协作系统的研究现状与发展凵J.机器人, 下转140页)