[發(fā)明專利]一種視覺慣性里程計(jì)系統(tǒng)

在審

• 申請(qǐng)?zhí)枺?/td>: 202110433639.9
• 申請(qǐng)日: 2021-04-20
• 公開（公告）號(hào): CN113358134A
• 公開（公告）日: 2021-09-07
• 發(fā)明（設(shè)計(jì)）人: 董子樂;余崇圣;李夢(mèng);陳方圓;王建波
• 申請(qǐng)（專利權(quán)）人: 重慶知至科技有限公司
• 主分類號(hào): G01C22/00
• 分類號(hào): G01C22/00;G01C21/16;G06K9/46
• 代理公司: 重慶智慧之源知識(shí)產(chǎn)權(quán)代理事務(wù)所(普通合伙) 50234
• 代理人: 余洪

本發(fā)明提供一種視覺慣性里程計(jì)系統(tǒng)，包括數(shù)據(jù)采集模塊和數(shù)據(jù)處理模塊，數(shù)據(jù)采集模塊用關(guān)于采集圖像信息、角速度和加速度；數(shù)據(jù)處理模塊用于執(zhí)行以下預(yù)處理和后端優(yōu)化算法；本發(fā)明算法使用視覺/慣性緊組合導(dǎo)航技術(shù)，對(duì)比傳統(tǒng)視覺里程計(jì)，該視覺慣性里程計(jì)系統(tǒng)可以在高動(dòng)態(tài)情況下穩(wěn)定運(yùn)行且有較好的性能水平；相比于基于松組合的視覺慣性里程計(jì)，本發(fā)明能夠充分傳感器的信息，在使用相同水平的傳感器時(shí)發(fā)揮出更好的性能；定位精度可以達(dá)到分米級(jí)，無(wú)需依賴衛(wèi)星信號(hào)。

技術(shù)領(lǐng)域

本發(fā)明涉及視覺慣性導(dǎo)航技術(shù)領(lǐng)域，尤其涉及一種視覺慣性里程計(jì)系統(tǒng)。

背景技術(shù)

隨著無(wú)人機(jī)、無(wú)人車以及移動(dòng)機(jī)器人的井噴式發(fā)展，導(dǎo)航技術(shù)成為了制約無(wú)人平臺(tái)廣泛應(yīng)用的瓶頸技術(shù)之一。SLAM技術(shù)一直被廣泛用于無(wú)人機(jī)、無(wú)人車以及移動(dòng)機(jī)器人的導(dǎo)航，即自主導(dǎo)航領(lǐng)域，SLAM是“Simultaneous Localization And Mapping”的縮寫，譯為同步定位與地圖構(gòu)建。經(jīng)過了三十多年的發(fā)展，視覺SLAM——即使用視覺傳感器作為主要信息輸入的SLAM技術(shù)，由于其成本的低廉且圖像信息更為豐富，逐漸成為了SLAM研究熱點(diǎn)之一。

視覺SLAM算法框架主要分為視覺里程計(jì)和地圖構(gòu)建兩大模塊，視覺里程計(jì)輸出系統(tǒng)的位置姿態(tài)信息，地圖構(gòu)建模塊通過結(jié)合視覺里程計(jì)的信息和圖像信息完成地圖構(gòu)建。其中，單目視覺SLAM是使用單目相機(jī)作為主要傳感器的SLAM技術(shù)，擁有系統(tǒng)結(jié)構(gòu)簡(jiǎn)單、成本較低的優(yōu)勢(shì)，但是由于單目相機(jī)不能得到絕對(duì)深度信息，因而實(shí)現(xiàn)難度也是最大的；且只使用相機(jī)的視覺里程計(jì)對(duì)于高動(dòng)態(tài)運(yùn)動(dòng)情況(即有較大的旋轉(zhuǎn)運(yùn)動(dòng))，容易丟失特征點(diǎn)而導(dǎo)致里程計(jì)失效。

故而提高定位精度，特別是高動(dòng)態(tài)情況下的定位精度，成為了視覺里程計(jì)的重要研究方向。在應(yīng)用需求的牽引下，使用視覺/慣性組合導(dǎo)航技術(shù)的視覺慣性里程計(jì)，特別是視覺與微慣性傳感器的組合，逐漸發(fā)展成為當(dāng)前自主導(dǎo)航及機(jī)器人領(lǐng)域的研究熱點(diǎn)。視覺/慣性組合導(dǎo)航具有顯著的優(yōu)點(diǎn)：(1)微慣性器件和視覺傳感器具有體積小、成本低的優(yōu)點(diǎn)，隨著制造技術(shù)的不斷進(jìn)步，器件越來(lái)越小，且成本越來(lái)越低。(2)不同于衛(wèi)星和無(wú)線電導(dǎo)航，視覺和慣性導(dǎo)航均不依賴外部設(shè)施支撐，可以實(shí)現(xiàn)自主導(dǎo)航。(3)慣性器件和視覺器件具有很好的互補(bǔ)性，慣性導(dǎo)航誤差隨時(shí)間累積，但是在短時(shí)間內(nèi)可以很好地跟蹤載體快速運(yùn)動(dòng)，保證短時(shí)間的導(dǎo)航精度；而視覺導(dǎo)航在低動(dòng)態(tài)運(yùn)動(dòng)中具有很高的估計(jì)精度，且引入了視覺閉環(huán)矯正可以極大地抑制組合導(dǎo)航誤差，兩者的組合可以更好地估計(jì)導(dǎo)航參數(shù)。

視覺/慣性組合導(dǎo)航技術(shù)以組合方式可以分為松組合和緊組合：松組合把視覺傳感器和慣性傳感器分別作為獨(dú)立的模塊進(jìn)行組合，兩個(gè)模塊分別使用視覺導(dǎo)航算法和慣性導(dǎo)航算法來(lái)獲得系統(tǒng)的導(dǎo)航信息，然后再對(duì)兩個(gè)信息進(jìn)行融合濾波，來(lái)達(dá)到更好的導(dǎo)航結(jié)果；緊組合則是使用兩個(gè)傳感器的原始數(shù)據(jù)，然后構(gòu)建一個(gè)基于最小二乘法的優(yōu)化框架，把所有數(shù)據(jù)都放入框架內(nèi)進(jìn)行優(yōu)化，最后得到導(dǎo)航結(jié)果。松組合的優(yōu)點(diǎn)是結(jié)構(gòu)簡(jiǎn)單，實(shí)現(xiàn)難度較低，但缺點(diǎn)是性能較低，達(dá)不到很好的精度；緊組合可以達(dá)到很好的性能水平，但是結(jié)構(gòu)復(fù)雜，實(shí)現(xiàn)難度大。

發(fā)明內(nèi)容

為了在保證導(dǎo)航精度的同時(shí)，降低算法復(fù)雜度，提高處理效率，本發(fā)明提供一種視覺慣性里程計(jì)系統(tǒng)，包括數(shù)據(jù)采集模塊和數(shù)據(jù)處理模塊，所述數(shù)據(jù)采集模塊用關(guān)于采集圖像信息、角速度和加速度；所述數(shù)據(jù)處理模塊用于執(zhí)行以下預(yù)處理和后端優(yōu)化算法：

預(yù)處理：

對(duì)采集的所述圖像信息、角速度和加速度進(jìn)行時(shí)間同步對(duì)齊；

針對(duì)當(dāng)前圖像信息，提取圖像Harris角點(diǎn)，跟蹤相鄰幀與所述當(dāng)前圖像信息的圖像Harris角點(diǎn)匹配的特征點(diǎn)；

利用所述采集的角速度和加速度進(jìn)行積分，得到當(dāng)前時(shí)刻的位置速度旋轉(zhuǎn)矩陣PVQ；

計(jì)算所述相鄰幀的預(yù)積分增量，以及誤差傳遞矩陣；

后端優(yōu)化：