一种基于边角网条件平差的主从式USV协同导航方法

本发明公开了一种基于边角网条件平差的主从式USV协同导航方法，属于自主式水下航行技术领域，包括使用捷联式惯性导航系统获得待定位USV初始时刻的导航状态，待定位USV接受USV编队中其他USV数据共享的当前位置，并将其转换为二维坐标，待定位USV根据配备的传感器获得与其他USV之间的相对距离量测信息和相对方位角建立描述待定位USV的状态方程和量测方程，建立边角网条件平差数学模型，处理量测矢量，得到量测矢量平差值及其方差阵，设置观测噪声方差阵，并结合DVL测速信息进行量测更新；本发明采用上述的一种基于边角网条件平差的主从式USV协同导航方法，可以实现待定位USV导航状态的高精度估计与协同校正。