[發明專利]一種RGI-Lidar/SINS緊耦合AUV水下導航定位方法與系統

有效

• 申請號: 202011373126.5
• 申請日: 2020-11-30
• 公開（公告）號: CN112611376B
• 公開（公告）日: 2023-08-01
• 發明（設計）人: 劉曉泉;于寶成;盧濤;徐文霞;劉威;徐國平;栗娟;侯偉
• 申請（專利權）人: 武漢工程大學
• 主分類號: G01C21/16
• 分類號: G01C21/16;G01C21/14;G01S7/48;G01S7/4861;G01S17/89;G01S7/487
• 代理公司: 湖北武漢永嘉專利代理有限公司 42102
• 代理人: 唐萬榮

1.一種RGI-Lidar/SINS緊耦合AUV水下導航定位方法，其特征在于，包括以下步驟：

通過捷聯解算得到相應時刻包含AUV位置信息、速度、姿態信息的導航信息；

獲取水下遠距離、高清晰度的選通圖像；

對IMU輸出信息進行預積分，獲取AUV在不同時刻的IMU位姿約束；

對當前幀的選通圖像進行特征提取，并將上一幀的選通圖像作為參考幀與之進行特征匹配，獲取相鄰圖像之間的位姿關系，即視覺里程計約束；

對當前幀的選通圖像進行回環檢測，若檢測到符合條件的回環幀圖像，則將當前幀的選通圖像與相應的回環幀圖像進行特征匹配，獲取回環檢測約束；

將IMU位姿約束、視覺里程計約束以及回環檢測約束作為約束關系加入到位姿圖中進行聯合優化，獲取優化后的AUV位姿；

輸出AUV位姿；

將AUV位姿作為慣性數據輸入；

對IMU進行預積分的具體方法為先捷聯解算得到AUV在不同時刻速度、位置和姿態，再根據以下公式進行預積分處理，獲取AUV在不同時刻之間的位姿約束因子，獲取AUV在不同時刻的IMU位姿約束，

式中，表示i時刻旋轉矩陣的轉置，R_j表示j時刻的旋轉矩陣，v_i和v_j分別表示i時刻和j時刻的速度，g是重力加速度，p_i和p_j分別表示i時刻和j時刻的位置，Δt_ij表示i時刻和j時刻的時間差；

特征提取的方法為ORB特征提取方法，即利用FAST角點檢測子提取關鍵點的位置，并為FAST角點增加尺度和旋轉的描述，采用BRIEF描述子來描述關鍵點的屬性；

特征匹配的方法為在相鄰幀圖像提取的特征點中，找到相對應的特征點，即特征點的描述子特征向量之間的距離越小，表示兩個特征點越匹配；

回環檢測為基于歐氏距離的回環檢測方法，即根據捷聯慣性導航系統推算出當前時刻的位姿X_i，并對之前時刻的位姿進行搜索，找到歐氏距離最近的位姿X_j，用d來表示最近歐氏距離；若d大于閾值d₀，則認為當前幀的選通圖像無相應的回環幀圖像；若d不大于閾值d₀，則認為當前幀的選通圖像有相應的回環幀圖像，求出兩者的位姿關系，即回環約束；其中閾值d₀是根據實際應用中AUV的移動速度和成像系統的幀頻來設置的；

位姿圖根據下式表示：

G＝(X，F)

式中，X是位姿圖中的節點，表示的是與選通圖像相對應時刻AUV的位姿；F是位姿圖中的邊，表示的是節點之間或者單個節點的約束條件。

2.一種用于實現權利要求1所述的RGI-Lidar/SINS緊耦合AUV水下導航定位方法的系統，其特征在于，包括RGI-Lidar，SINS和數據處理單元；其中，

RGI-Lidar，用于獲取水下遠距離、高清晰度的選通圖像，并將選通圖像發送至數據處理單元；

SINS，用于通過捷聯慣導算法計算得到相應時刻的AUV位置信息、姿態信息的導航信息并發送至數據處理單元；

數據處理單元，用于根據AUV位置信息，對IMU進行預積分，獲取AUV在不同時刻的IMU位姿約束；對當前幀的選通圖像進行特征提取，并將上一幀的選通圖像作為參考幀與之進行特征匹配，獲取相鄰圖像之間的位姿關系，即視覺里程計約束；對當前幀的選通圖像進行回環檢測，若檢測到符合條件的回環幀圖像，則將當前幀的選通圖像與相應的回環幀圖像進行特征匹配，獲取回環檢測約束；將IMU位姿約束、視覺里程計約束以及回環檢測約束作為約束關系加入到位姿圖中進行聯合優化，獲取并輸出優化后的AUV位姿；將AUV位姿發送至SINS作為慣性數據輸入。