由于低精度航位推算系统带来的累积误差,自治水下航行器(AUV)在未知环境中的定位准确性会随着时间的推移变得越来越差。当环境中有非合作体运动时,AUV不但可利用自身携带的声纳传感器对其探测并利用基于模型的目标跟踪方法估计非合作体轨迹,而且可以同时利用探测到的与非合作体的相对信息来提高自身定位精度。在同时定位与制图(SLAM)方法中的FastSLAM算法框架的基础上提出了同时定位与跟踪(SLAT)算法,即设置多个估计粒子,利用每个粒子中的粒子滤波器(PF)和扩展卡尔曼滤波器(EKF)分别估计AUV和非合作体的轨迹,并能根据实际量测与粒子估计量测之间的差别赋予粒子权值,继而得到多粒子加权的最终估计。最后仿真验证了算法的有效性。
The localization accuracy of an Autonomous Underwater Vehicle(AUV) shall become increasingly worse over time in an unknown environment due to the cumulative errors brought by the low accuracy dead reckoning system.When a non-cooperative body moves in the environment,an AUV can not only use its own sonar sensors to detect the body and estimate the track of the body taking advantage of the methods of target tracking based on models,but also make use of the relative information detected to the non-cooperation body to improve their accuracy simultaneously.A Simultaneous Localization And Tracking(SLAT) algorithm is proposed based on the framework of the FastSLAM algorithm in the methods of the Simultaneous Localization And Mapping(SLAM).Namely,a number of estimate particles are set,the tracks of the AUV and the non-cooperative body can be estimated by using the Particle Filter(PF) and the Extended Kalman Filter(EKF) in each particle,the weight value of each particle is assigned according to the difference between the actual measurement and estimate measurement of each particle,then the weighted final estimates based on the multiple particles can be get.At last,the validity of the algorithm is shown by the simulation.