一种基于多测距圆交汇的AUV协同定位方法

本发明是一种基于多测距圆交汇的AUV协同定位方法。本发明涉及水下AUV协同定位技术领域，本发明建立AUV的运动学模型和量测模型；获取当前时刻的测距圆方程以及当前控制输入，联合解算历史测距圆构建的方程组；剔除错误候选点；确定确定所有候选解的误差协方差矩阵和候选解之间的互协方差矩阵，得到所有位置估计的联合协方差矩阵，使用线性最小二乘对候选解进行最优加权融合，得出最优的位置估计以及相应的估计误差协方差。本发明创造性地提出了通过历史测距方程的联合求解生成多个潜在位置估计值，并根据不确定性区域的形状，系统地去除模糊解，获得准确的内点集，在保证估计一致性的同时具备较高的状态估计精度和较低的时间复杂度。