[發(fā)明專利]一種基于拓撲信息的水下航行器協(xié)同定位方法

有效

• 申請?zhí)枺?/td>: 201810143849.2
• 申請日: 2018-02-12
• 公開（公告）號: CN108332756B
• 公開（公告）日: 2021-05-07
• 發(fā)明（設計）人: 張飛虎;程晨升;王璨
• 申請（專利權）人: 西北工業(yè)大學
• 主分類號: G01C21/20
• 分類號: G01C21/20
• 代理公司: 西北工業(yè)大學專利中心 61204
• 代理人: 王鮮凱

本發(fā)明涉及一種基于拓撲信息的水下航行器協(xié)同定位方法，用matlab模擬出四個水下航行器的運行軌跡圖，用gtsam構建出因子圖，以水下航行器之間的距離作為約束條件，加入到對位姿的優(yōu)化中去，使得定位更加的精確。本發(fā)明解決水下航行器由于接收不到高精度的GPS信號而產(chǎn)生的定位誤差，經(jīng)過實驗驗證，該方法確實可以提高定位的精度，減小誤差。

技術領域

本發(fā)明屬于水下航行器的協(xié)同定位領域，涉及一種基于拓撲信息的水下航行器協(xié)同定位方法。

背景技術

水下航行器在水下作業(yè)時，由于接收不到精準的GPS信號，只能依賴內(nèi)部的傳感器進行導航，像慣性導航等，在這種傳統(tǒng)的方式下，誤差會隨著時間的延長而積累。鑒于此，本文發(fā)明了一種利用拓撲因子的協(xié)同定位方法，此方法把水下航行器間的距離作為約束因子，使得水下航行器不用浮上水面也可以獲得比之前高的定位精度。而且，在加上GPS信號的情況下，使用了拓撲因子作為約束的定位方法也比傳統(tǒng)方法的定位精度高。

發(fā)明內(nèi)容

要解決的技術問題

為了避免現(xiàn)有技術的不足之處，本發(fā)明提出一種基于拓撲信息的水下航行器協(xié)同定位方法，利用拓撲因子增加協(xié)同定位的精確度。實驗用Georgia Tech Smoothing AndMapping(gtsam)來進行。

技術方案

一種基于拓撲信息的水下航行器協(xié)同定位方法，其特征在于步驟如下：

步驟1：采用matlab模擬出四個水下航行器的運行軌跡圖，并取得四個水下航行器的運行軌跡圖的測量數(shù)據(jù)：

步驟2：以下式表示拓撲信息,計算因子圖中的水下航行器之間的距離

其中：l表示航行器的數(shù)量，其中表示水下航行器之間的距離，和分別是第i個和第j個航行器的橫坐標，和分別是第i個和第j個航行器的縱坐標，t₀表示某個具體時刻；

步驟3：根據(jù)計算的拓撲信息，采用gtsam構建因子圖；x₁,x₂,x₃,x₄分別是四個水下航行器各個時刻的位置狀態(tài)，每個水下航行器本時刻坐標與下一時刻坐標之間的聯(lián)系為內(nèi)部導航系統(tǒng)的里程計因子f_IN，每個時刻四個水下航行器之間的聯(lián)系為四個水下航行器之間的距離約束f_top；

步驟4：采用Levenberg-Marquardt法對拓撲約束的因子圖進行優(yōu)化，得到水下航行器各個時刻的估計位姿。

有益效果

本發(fā)明提出的一種基于拓撲信息的水下航行器協(xié)同定位方法，用matlab模擬出四個水下航行器的運行軌跡圖，用gtsam構建出因子圖，以水下航行器之間的距離作為約束條件，加入到對位姿的優(yōu)化中去，使得定位更加的精確。本發(fā)明解決水下航行器由于接收不到高精度的GPS信號而產(chǎn)生的定位誤差，經(jīng)過實驗驗證，該方法確實可以提高定位的精度，減小誤差。

附圖說明

圖1：水下航行器的拓撲因子圖；

圖2：水下航行器的行駛軌跡；

圖3：使用拓撲因子協(xié)同定位與傳統(tǒng)方法的實驗結(jié)果圖。

具體實施方式

現(xiàn)結(jié)合實施例、附圖對本發(fā)明作進一步描述：

在本實驗中，水面船和四個水下航行器都配備有聲吶和GPS,且水下航行器有內(nèi)部的導航系統(tǒng)。

第一步：我們用matlab模擬出四個水下航行器的運行軌跡圖(設定初始位置，方向，和增長速度的直線)，軌跡圖如附圖2，并記錄下數(shù)據(jù)：速度，位置坐標，行駛距離。