单水声信标测距辅助的水下同步定位与构图方法

摘要

本发明公开了一种单水声信标测距辅助的水下同步定位与构图方法：步骤1，确定航行器运动的状态方程，依据状态方程得到预测方程；步骤2，在实际航行过程中，航行器对环境特征进行观测，得到观测量，同时对提前布置好的单水声信标进行测距观测，得到增广的观测量；步骤3，依据步骤2得到的增广的观测量对新观测到的路标进行数据关联，不断对特征地图和状态量进行更新，从而得到实时的定位导航结果。本发明利用单水声信标测距信息对水下同步定位与构图方法进行约束，减少系统噪声的影响，提高定位精度。