[發(fā)明專利]視覺慣性里程計(jì)優(yōu)化的方法及系統(tǒng)

有效

• 申請(qǐng)?zhí)枺?/td>: 201910996260.1
• 申請(qǐng)日: 2019-10-18
• 公開（公告）號(hào): CN110763251B
• 公開（公告）日: 2021-07-13
• 發(fā)明（設(shè)計(jì)）人: 姜楠;黃德斌;李進(jìn);黃偉
• 申請(qǐng)（專利權(quán)）人: 華東交通大學(xué)
• 主分類號(hào): G01C22/00
• 分類號(hào): G01C22/00;G06T7/246;G06T7/73
• 代理公司: 深圳市智圈知識(shí)產(chǎn)權(quán)代理事務(wù)所(普通合伙) 44351
• 代理人: 呂靜

1.一種視覺慣性里程計(jì)優(yōu)化的方法，其特征在于，所述方法包括：

獲取IMU數(shù)據(jù)以及視覺圖像數(shù)據(jù)；

提取所述視覺圖像數(shù)據(jù)的點(diǎn)線特征數(shù)據(jù)，并采用目標(biāo)算法對(duì)所述點(diǎn)線特征數(shù)據(jù)進(jìn)行誤匹配剔除，以得到去除點(diǎn)線特征誤差的目標(biāo)點(diǎn)線特征數(shù)據(jù)；

對(duì)所述IMU數(shù)據(jù)進(jìn)行預(yù)積分處理，得到所述視覺圖像數(shù)據(jù)所表征視覺圖像的尺度信息以及所述IMU數(shù)據(jù)的IMU狀態(tài)量變量；

基于所述尺度信息對(duì)所述目標(biāo)點(diǎn)線特征數(shù)據(jù)進(jìn)行聯(lián)合初始化，得到與所述目標(biāo)點(diǎn)線特征數(shù)據(jù)對(duì)應(yīng)的初始點(diǎn)特征像素灰度值以及初始線特征像素灰度值，所述基于所述尺度信息對(duì)所述目標(biāo)點(diǎn)線特征數(shù)據(jù)進(jìn)行聯(lián)合初始化，得到與所述目標(biāo)點(diǎn)線特征數(shù)據(jù)對(duì)應(yīng)的初始點(diǎn)特征像素灰度值以及初始線特征像素灰度值的步驟包括：構(gòu)造點(diǎn)優(yōu)化函數(shù)以及線優(yōu)化函數(shù)，通過所述點(diǎn)優(yōu)化函數(shù)以及線優(yōu)化函數(shù)對(duì)所述目標(biāo)點(diǎn)線特征數(shù)據(jù)進(jìn)行聯(lián)合初始化，得到與所述目標(biāo)點(diǎn)線特征數(shù)據(jù)對(duì)應(yīng)的初始點(diǎn)特征像素灰度值以及初始線特征像素灰度值；

計(jì)算相鄰兩幀視覺圖像之間的IMU更新量；

結(jié)合所述IMU更新量、初始點(diǎn)特征像素灰度值以及初始線特征像素灰度值獲取新的位姿；

判斷所述視覺圖像數(shù)據(jù)的不同幀圖像是否滿足目標(biāo)條件；

若滿足，獲取對(duì)應(yīng)的圖像幀作為關(guān)鍵幀；

將所述關(guān)鍵幀、所述位姿、所述目標(biāo)點(diǎn)線特征數(shù)據(jù)、預(yù)先配置的加速度零偏和陀螺儀零偏、以及IMU速度加入到滑動(dòng)窗口中，將滑動(dòng)窗口中時(shí)間最早的一幀進(jìn)行邊緣化處理，得到邊緣化先驗(yàn)；

獲取IMU殘差以及視覺殘差，并基于所述邊緣化先驗(yàn)、所述IMU殘差以及視覺殘差對(duì)視覺圖像估計(jì)進(jìn)行優(yōu)化處理。

2.根據(jù)權(quán)利要求1所述的方法，其特征在于，所述提取所述視覺圖像數(shù)據(jù)的點(diǎn)線特征數(shù)據(jù)的步驟包括：

采用FAST角點(diǎn)提取算法提取視覺圖像中的角點(diǎn)，得到點(diǎn)特征數(shù)據(jù)；

判斷所提取角點(diǎn)的數(shù)量是否達(dá)到預(yù)設(shè)閾值；

若達(dá)到，采用LSD直線檢測(cè)分割算法提取線特征數(shù)據(jù)，得到線特征數(shù)據(jù)。

3.根據(jù)權(quán)利要求1所述的方法，其特征在于，所述判斷所述視覺圖像數(shù)據(jù)的不同幀圖像是否滿足目標(biāo)條件的步驟包括：

判斷前后兩幀視覺圖像之間平移的距離或旋轉(zhuǎn)的角度是否大于對(duì)應(yīng)的預(yù)設(shè)閾值；或

判斷跟蹤點(diǎn)、線特征的數(shù)量是否小于第一閾值；或

判斷當(dāng)前幀視覺圖像的跟蹤點(diǎn)、線特征的數(shù)量是否小于上一幀視覺圖像的跟蹤點(diǎn)、線特征的數(shù)量。

4.根據(jù)權(quán)利要求1所述的方法，其特征在于，所述獲取IMU殘差以及視覺殘差的步驟包括：

獲取點(diǎn)投影誤差模型以及線投影誤差模型；

通過所述點(diǎn)投影誤差模型以及線投影誤差模型計(jì)算點(diǎn)投影誤差以及線投影誤差；

基于所述點(diǎn)投影誤差以及線投影誤差計(jì)算視覺殘差；

基于預(yù)設(shè)的IMU殘差模型計(jì)算IMU殘差。