[發(fā)明專(zhuān)利]一種基于立體視覺(jué)慣性里程計(jì)的車(chē)輛定位導(dǎo)航方法及系統(tǒng)

在審

• 申請(qǐng)?zhí)枺?/td>: 202110862850.2
• 申請(qǐng)日: 2021-07-29
• 公開(kāi)（公告）號(hào): CN113701749A
• 公開(kāi)（公告）日: 2021-11-26
• 發(fā)明（設(shè)計(jì)）人: 徐勁松
• 申請(qǐng)（專(zhuān)利權(quán)）人: 江蘇師范大學(xué)
• 主分類(lèi)號(hào): G01C21/16
• 分類(lèi)號(hào): G01C21/16;G01S19/49
• 代理公司: 南京縱橫知識(shí)產(chǎn)權(quán)代理有限公司 32224
• 代理人: 董建林

本發(fā)明公開(kāi)了一種基于立體視覺(jué)慣性里程計(jì)的車(chē)輛定位導(dǎo)航方法及系統(tǒng)，該方法包括如下步驟：捕獲待觀測(cè)點(diǎn)的位姿信息和特征數(shù)據(jù)，進(jìn)行預(yù)校準(zhǔn)，得到位姿偏差信息和狀態(tài)信息；將位姿信息和特征數(shù)據(jù)進(jìn)行融合，得到相機(jī)位姿估計(jì)，加入到預(yù)設(shè)的INS狀態(tài)向量中，增強(qiáng)INS狀態(tài)和相機(jī)位姿的協(xié)方差矩陣；通過(guò)左側(cè)相機(jī)和右側(cè)相機(jī)觀測(cè)所有圖像中的特征，進(jìn)行相機(jī)校準(zhǔn)、特征檢測(cè)、特征匹配，據(jù)此跟蹤所有特征，得到特征信息；根據(jù)特征信息、增強(qiáng)后的INS狀態(tài)和相機(jī)位姿的協(xié)方差矩陣進(jìn)行濾波處理，校準(zhǔn)位姿偏差信息和狀態(tài)信息，更新完成跟蹤的特征、INS狀態(tài)和相機(jī)位姿，據(jù)此輸出定位結(jié)果。

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

本發(fā)明涉及一種基于立體視覺(jué)慣性里程計(jì)的車(chē)輛定位導(dǎo)航方法及系統(tǒng)，涉及衛(wèi)星導(dǎo)航、人工智能、無(wú)人駕駛以及時(shí)間頻率應(yīng)用等領(lǐng)域。

背景技術(shù)

車(chē)輛的準(zhǔn)確和連續(xù)定位是自動(dòng)駕駛汽車(chē)和移動(dòng)地圖系統(tǒng)的主要要求，全球?qū)Ш叫l(wèi)星系統(tǒng)(GNSS)是滿足戶(hù)外環(huán)境中這一需求的關(guān)鍵技術(shù)；然而，在建筑密集的地區(qū)、城市峽谷和隧道中，GNSS無(wú)法很好利用，準(zhǔn)確和可靠的車(chē)輛定位成為一個(gè)挑戰(zhàn)。

慣性傳感器通常與GNSS集成，可以在沒(méi)有GNSS觀測(cè)的情況下提供位置估計(jì)，但慣性導(dǎo)航系統(tǒng)(INS,Inertial Navigation System)定位會(huì)隨著時(shí)間的推移定位誤差會(huì)大量地積累和增長(zhǎng)，因此INS不適合在GNSS信號(hào)丟失時(shí)長(zhǎng)時(shí)間的定位。

慣性傳感器和視覺(jué)傳感器具有互補(bǔ)的特性，INS可以在每次圖像采集時(shí)提供相機(jī)姿態(tài)的初始估計(jì)，視覺(jué)特征可以限制INS漂移；此外，無(wú)法保證純粹基于視覺(jué)的導(dǎo)航系統(tǒng)可以提供連續(xù)定位，因?yàn)樗鼈兏叨纫蕾?lài)視覺(jué)特征，因此在無(wú)特征的環(huán)境中表現(xiàn)不佳。

基于相機(jī)與慣性傳感器集成的一般導(dǎo)航方法被稱(chēng)為視覺(jué)慣性里程計(jì)(VIO,Visual-Inertial Odometry)。在機(jī)器人和計(jì)算機(jī)視覺(jué)方面對(duì)VIO進(jìn)行了大量研究；用于將慣性測(cè)量與圖像觀測(cè)相結(jié)合的算法主要分為兩類(lèi)：基于優(yōu)化的方法和濾波方法；優(yōu)化算法通過(guò)對(duì)最小二乘誤差函數(shù)的迭代最小化來(lái)估計(jì)未知參數(shù)，盡管這些算法提供了更好的估計(jì)，但它們必須限制可用的測(cè)量值，以實(shí)現(xiàn)實(shí)時(shí)姿態(tài)估計(jì)；與優(yōu)化方法相反，濾波算法一有圖像觀測(cè)結(jié)果就以遞歸方式估計(jì)參數(shù)；在過(guò)去的幾十年里，人們提出了許多濾波方法，用來(lái)解決實(shí)時(shí)問(wèn)題；這些方法要么計(jì)算成本高，因此不適合實(shí)時(shí)定位，要么沒(méi)有充分利用跟蹤特性為提供的約束。

多態(tài)約束卡爾曼濾波(MSCKF,Multi-State Constraint Kalman Filter)使用了一個(gè)特征從多個(gè)相機(jī)姿勢(shì)提供的最大幾何約束，很好地解決了實(shí)時(shí)性和跟蹤特征的約束問(wèn)題，但該方法的一個(gè)挑戰(zhàn)是單圖像序列跟蹤特征可靠性低，影響姿態(tài)估計(jì)的準(zhǔn)確性。

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

本發(fā)明的目的是提供一種基于立體視覺(jué)慣性里程計(jì)的車(chē)輛定位導(dǎo)航方法及系統(tǒng)，解決現(xiàn)有技術(shù)中單圖像序列跟蹤特征可靠性低，姿態(tài)估計(jì)的準(zhǔn)確性低等問(wèn)題。

本發(fā)明是通過(guò)下述技術(shù)方案實(shí)現(xiàn)的：

第一方面，提供了一種基于立體視覺(jué)慣性里程計(jì)的車(chē)輛定位導(dǎo)航方法，包括如下步驟：

捕獲待觀測(cè)點(diǎn)的位姿信息和特征數(shù)據(jù)，進(jìn)行預(yù)校準(zhǔn)，得到位姿偏差信息和狀態(tài)信息，將所述位姿信息、特征數(shù)據(jù)、位姿偏差信息和狀態(tài)信息進(jìn)行傳播；

將位姿信息和特征數(shù)據(jù)進(jìn)行融合，得到相機(jī)位姿估計(jì)，加入到預(yù)設(shè)的INS狀態(tài)向量中，增強(qiáng)INS狀態(tài)和相機(jī)位姿的協(xié)方差矩陣；

通過(guò)左側(cè)相機(jī)和右側(cè)相機(jī)觀測(cè)所有圖像中的特征，進(jìn)行相機(jī)校準(zhǔn)、特征檢測(cè)、特征匹配，據(jù)此跟蹤所有特征，得到特征信息；

根據(jù)特征信息、增強(qiáng)后的INS狀態(tài)和相機(jī)位姿的協(xié)方差矩陣進(jìn)行濾波處理，校準(zhǔn)位姿偏差信息和狀態(tài)信息，更新完成跟蹤的特征、INS狀態(tài)和相機(jī)位姿，據(jù)此輸出定位結(jié)果。

結(jié)合第一方面，進(jìn)一步的，通過(guò)INS捕獲待觀測(cè)點(diǎn)的位姿信息。