[發明專利]一種基于視覺慣導信息融合的定位方法及系統

有效

• 申請號: 201711077826.8
• 申請日: 2017-11-06
• 公開（公告）號: CN107869989B
• 公開（公告）日: 2020-02-07
• 發明（設計）人: 劉騰飛;張鵬
• 申請（專利權）人: 東北大學
• 主分類號: G01C21/16
• 分類號: G01C21/16;G01C21/20
• 代理公司: 11613 北京易捷勝知識產權代理事務所(普通合伙)
• 代理人: 韓國勝

1.一種基于視覺慣導信息融合的定位方法，其特征在于，包括：

步驟A1、對采集的傳感器信息進行預處理，所述傳感器信息包括：深度視覺傳感器的RGB圖像和深度圖像信息，慣性測量單元IMU的數據；

步驟A2、獲取深度視覺傳感器和慣性測量單元所屬系統的外部參數；

步驟A3、采用IMU預積分模型和深度相機模型處理所述預處理后的傳感器信息、外部參數，獲得位姿信息；

步驟A4、基于回環檢測方式對所述位姿信息進行校正，獲得校正后的位姿信息并輸出；

所述系統的外部參數為慣性測量單元和深度視覺傳感器之間的相對位姿；

所述處理后的傳感器信息包括：傳感器測量值和系統狀態向量；

所述A3具體包括：

根據IMU預積分模型計算測量值與系統狀態的殘差項和協方差；

根據深度相機模型計算特征點從前一個關鍵幀到當前關鍵幀去畸變后的重投影誤差和協方差；

初始化滑動窗口，當滑動窗口為空時，則將所述殘差項與所述重投影誤差的馬氏范數和加到滑動窗口中；

對滑動窗口中的殘差范數構造非線性最小二乘式，通過高斯牛頓法迭代求解，得到系統狀態向量的最優解；

邊緣化滑動窗口中的殘差項；

所述傳感器測量值為IMU積分得到的位置、姿態和速度以及使用LK光流跟蹤法在當前幀中跟蹤到與上一幀中的特征點匹配的特征點。

2.根據權利要求1所述的方法，其特征在于，所述步驟A1包括：

A11、采集所述深度視覺傳感器輸出的當前幀的RGB圖像和深度圖像，每一RGB圖像對應一個深度圖像，所述深度圖像中的像素值表示RGB像素點距深度視覺傳感器中相機光心的實際距離；

判斷當前幀是否是深度視覺傳感器輸出的第一幀；

若是，則從當前幀的RGB圖像中提取harris角點；

對提取的harris角點計算BRISK描述子，得到當前幀的特征點；

否則，采用LK光流跟蹤算法在當前幀的RGB圖像中查找與上一幀的RGB圖像匹配的特征點；

確定查找的匹配的特征點數量是否等于預設閾值，若等于，則將查找的匹配的特征點作為當前幀的特征點，否則，從當前幀的RGB圖像中提取新的harris角點，根據提取的harris角點，獲取新的特征點，將獲取的新的特征點和查找的匹配的特征點共同作為當前幀的特征點；

A12、采集所述慣性測量單元IMU的數據；

在當前幀為關鍵幀時，使用預積分模型計算方式，獲取相鄰關鍵幀之間IMU的狀態增量。

3.根據權利要求2所述的方法，其特征在于，所述A2包括：

查看所述深度視覺傳感器和慣性測量單元所屬系統的外部參數是否已經設定，若是，則在步驟A3中直接使用設定的外部參數；

否則，根據當前幀的RGB圖像的特征點、上一幀的RGB圖像的特征點，采用ICP算法，計算當前幀與上一幀之間的相對位姿；

根據所述位姿增量、所述相對位姿、外部參數之間的坐標變換關系、以及在所述系統啟動后采集的相關數據構建超定方程，并采用最小二乘方式求解以獲取外部參數；

所述外部參數為慣性測量單元和深度視覺傳感器之間的相對位姿；

所述系統啟動后采集的相關數據包括：深度視覺傳感器的RGB圖像和深度圖像信息和IMU輸出的加速度和角速度信息。

4.根據權利要求3所述的方法，其特征在于，所述A4包括：

從當前關鍵幀提取fast角點計算BRISK描述子作為特征點并計算對應的BoW向量；

與詞袋中的特征集進行匹配，判斷是否存在回環；

若存在回環，計算構成回環的兩個關鍵幀的相對位姿殘差以及關鍵幀庫中其他相鄰兩個關鍵幀的相對位姿殘差，使用殘差項馬氏范數構造非線性最小二乘式，對關鍵幀庫中的所有位姿進行全局優化；

若不存在回環，則將當前關鍵幀的位姿和特征點描述子存入關鍵幀庫，并將特征點描述子的BoW向量存入詞袋。

5.根據權利要求4所述的方法，其特征在于，所述邊緣化滑動窗口中的殘差項，包括：

在當前幀為關鍵幀時，邊緣化掉滑動窗口中最舊的一組殘差范數；

在當前幀為非關鍵幀時，去掉當前幀的相機觀測值，保留IMU測量值。