[发明专利]一种基于拓扑信息的水下航行器协同定位方法

摘要

本发明涉及一种基于拓扑信息的水下航行器协同定位方法，用matlab模拟出四个水下航行器的运行轨迹图，用gtsam构建出因子图，以水下航行器之间的距离作为约束条件，加入到对位姿的优化中去，使得定位更加的精确。本发明解决水下航行器由于接收不到高精度的GPS信号而产生的定位误差，经过实验验证，该方法确实可以提高定位的精度，减小误差。

主权项

1.一种基于拓扑信息的水下航行器协同定位方法，其特征在于步骤如下：步骤1：采用matlab模拟出四个水下航行器的运行轨迹图，并取得四个水下航行器的运行轨迹图的测量数据：步骤2：以下式表示拓扑信息,计算因子图中的水下航行器之间的距离其中：l表示航行器的数量，其中表示水下航行器之间的距离，和分别是第i个和第j个航行器的横坐标，和分别是第i个和第j个航行器的纵坐标，t₀表示某个具体时刻；步骤3：根据计算的拓扑信息，采用gtsam构建因子图；x1,x2,x3,x4分别是四个水下航行器各个时刻的位置状态，每个水下航行器本时刻坐标与下一时刻坐标之间的联系为内部导航系统的里程计因子fIN，每个时刻四个水下航行器之间的联系为四个水下航行器之间的距离约束ftop；步骤4：采用Levenberg‑Marquardt法对拓扑约束的因子图进行优化，得到水下航行器各个时刻的估计位姿。