[發(fā)明專利]一種基于視覺慣導(dǎo)信息融合的定位方法及系統(tǒng)

有效

• 申請?zhí)枺?/td>: 201711077826.8
• 申請日: 2017-11-06
• 公開（公告）號: CN107869989B
• 公開（公告）日: 2020-02-07
• 發(fā)明（設(shè)計）人: 劉騰飛;張鵬
• 申請（專利權(quán)）人: 東北大學(xué)
• 主分類號: G01C21/16
• 分類號: G01C21/16;G01C21/20
• 代理公司: 11613 北京易捷勝知識產(chǎn)權(quán)代理事務(wù)所(普通合伙)
• 代理人: 韓國勝

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

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

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

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

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

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

所述處理后的傳感器信息包括：傳感器測量值和系統(tǒng)狀態(tài)向量；

所述A3具體包括：

根據(jù)IMU預(yù)積分模型計算測量值與系統(tǒng)狀態(tài)的殘差項和協(xié)方差；

根據(jù)深度相機模型計算特征點從前一個關(guān)鍵幀到當(dāng)前關(guān)鍵幀去畸變后的重投影誤差和協(xié)方差；

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

對滑動窗口中的殘差范數(shù)構(gòu)造非線性最小二乘式，通過高斯牛頓法迭代求解，得到系統(tǒng)狀態(tài)向量的最優(yōu)解；

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

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

2.根據(jù)權(quán)利要求1所述的方法，其特征在于，所述步驟A1包括：

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

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

若是，則從當(dāng)前幀的RGB圖像中提取harris角點；

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

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

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

A12、采集所述慣性測量單元IMU的數(shù)據(jù)；

在當(dāng)前幀為關(guān)鍵幀時，使用預(yù)積分模型計算方式，獲取相鄰關(guān)鍵幀之間IMU的狀態(tài)增量。

3.根據(jù)權(quán)利要求2所述的方法，其特征在于，所述A2包括：

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

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

根據(jù)所述位姿增量、所述相對位姿、外部參數(shù)之間的坐標(biāo)變換關(guān)系、以及在所述系統(tǒng)啟動后采集的相關(guān)數(shù)據(jù)構(gòu)建超定方程，并采用最小二乘方式求解以獲取外部參數(shù)；

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

所述系統(tǒng)啟動后采集的相關(guān)數(shù)據(jù)包括：深度視覺傳感器的RGB圖像和深度圖像信息和IMU輸出的加速度和角速度信息。

4.根據(jù)權(quán)利要求3所述的方法，其特征在于，所述A4包括：

從當(dāng)前關(guān)鍵幀提取fast角點計算BRISK描述子作為特征點并計算對應(yīng)的BoW向量；

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

若存在回環(huán)，計算構(gòu)成回環(huán)的兩個關(guān)鍵幀的相對位姿殘差以及關(guān)鍵幀庫中其他相鄰兩個關(guān)鍵幀的相對位姿殘差，使用殘差項馬氏范數(shù)構(gòu)造非線性最小二乘式，對關(guān)鍵幀庫中的所有位姿進行全局優(yōu)化；

若不存在回環(huán)，則將當(dāng)前關(guān)鍵幀的位姿和特征點描述子存入關(guān)鍵幀庫，并將特征點描述子的BoW向量存入詞袋。

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

在當(dāng)前幀為關(guān)鍵幀時，邊緣化掉滑動窗口中最舊的一組殘差范數(shù)；

在當(dāng)前幀為非關(guān)鍵幀時，去掉當(dāng)前幀的相機觀測值，保留IMU測量值。