[發明專利]一種相似環境下基于IMU和視覺的二次回環檢測定位方法

有效

• 申請號: 202011206955.4
• 申請日: 2020-11-03
• 公開（公告）號: CN112461228B
• 公開（公告）日: 2023-05-09
• 發明（設計）人: 呂婧;鄒霞;岳定春;應旻;涂良輝
• 申請（專利權）人: 南昌航空大學
• 主分類號: G01C21/00
• 分類號: G01C21/00;G01C21/16;G01C21/20;G06T7/33;G06T7/80
• 代理公司: 深圳市智旭鼎浩知識產權代理事務所(普通合伙) 44746
• 代理人: 周超

1.一種相似環境下基于IMU和視覺的二次回環檢測定位方法，其特征在于，包括以下步驟：

步驟一，雙目相機與IMU參數標定與同步：

統一雙目相機與IMU的時間戳，標定所述雙目相機與IMU的參數，包括雙目相機啟動延遲時間、IMU啟動延遲時間、加速度計零偏和陀螺儀零偏，同步雙目相機與IMU時間，讀取雙目相機獲取的圖像與IMU位姿信息；

步驟二，圖像特征提取與圖像匹配：

檢測雙目相機獲取的圖像中的角點作為特征點，對這一角點周圍圖像進行描述，形成特征描述子，對連續兩幅圖像中的特征描述子使用漢明距離進行匹配，篩選特征匹配點對，以防誤匹配；

步驟三，位姿估計及移動軌跡形成：

通過匹配好的多組特征匹配點對的3D信息，采用最小二乘方法，計算使特征匹配點對誤差平方和達到極小值的旋轉向量矩陣R和平移向量t，得到雙目相機的位姿和移動地圖點軌跡；

當相機移動過快造成圖像模糊，丟失定位信息時，通過積分統一時間戳下的IMU位姿信息，獲取當前雙目相機的位姿信息，更新相關聯地圖點；

步驟四，回環檢測：

根據兩幅圖像的相似性判斷是否回到之間經過的位置以確定回環檢測關系，通過構建樹形聚類數據庫的方式度量圖像幀間相似性以判斷是否位置應一致而回環；對在圖像匹配時提取的特征點與特征描述聚類化，形成樹杈結構，以對圖像特征做快速篩選，輸出候選圖像，減少相似度判斷時間；計算與當前圖像有公共特征類的非相連但相鄰關鍵幀的類向量相似度，判斷是否回環；

步驟五，二次回環檢測機制：

在上述步驟閉環確認后，先不進行閉環，而是通過IMU位姿信息對當前圖像幀位姿進行粗比較約束，即以IMU當前位置方向與一次閉環圖像位置方向比較，進行方向一致性約束預判，以再次確認回環的正確性，形成二次檢測機制；

步驟六，重定位：

通過回環確認后的誤差結果，采用全局非線性優化，將回環誤差平均分配到所有關鍵圖像幀中，以優化和更新所有關鍵圖像幀在世界坐標系下的位姿和相關聯地圖點，獲取重定位；同時對更新后的當前地圖點微分處理，以修正當前IMU累積漂移誤差。

2.根據權利要求1所述的一種相似環境下基于IMU和視覺的二次回環檢測定位方法，其特征在于，所述步驟二中篩選特征匹配點對的條件是描述子的漢明距離小于其最小距離的兩倍。

3.根據權利要求1所述的一種相似環境下基于IMU和視覺的二次回環檢測定位方法，其特征在于，所述步驟三中最小二乘方法的計算公式為：

其中R為旋轉向量矩陣，t為平移向量矩陣，p_i為當前圖像中的特征點，為上一個圖像與p_i匹配對應的特征點。

4.根據權利要求1所述的一種，其特征在于，所述步驟四中類向量相似度的計算公式為：

其中，v₁、v₂為兩幅圖像的特征類向量。