[發明專利]基于稀疏直接法的視覺慣性里程計位姿估計方法

有效

• 申請號: 202010595397.9
• 申請日: 2020-06-23
• 公開（公告）號: CN111780754B
• 公開（公告）日: 2022-05-20
• 發明（設計）人: 高唱;曾慶喜;陳則王;呂查德;闞宇超
• 申請（專利權）人: 南京航空航天大學
• 主分類號: G01C21/16
• 分類號: G01C21/16;G01C22/00;G01C25/00
• 代理公司: 南京經緯專利商標代理有限公司 32200
• 代理人: 施昊

本發明公開了一種基于稀疏直接法的視覺慣性里程計位姿估計方法，首先給載體一個較大的旋轉和平移，對安裝在載體上的VO和IMU進行初始化；構建N層圖像金字塔，對整個圖像金字塔的灰度值進行調整；采用基于滑動窗口的非線性最小二乘法，維護一個具有若干個關鍵幀的滑動窗口；對于每一個圖像幀，首先根據其位姿估計判斷是否為關鍵幀，若是關鍵幀，則將當前幀加入滑動窗口進行非線性優化，同時將窗口內對窗口貢獻最小的圖像幀邊緣化，計算更新先驗信息矩陣和先驗殘差。若不是關鍵幀，則直接將當前幀移出滑動窗口，維持原先滑動窗口中的狀態變量。本發明使VIO系統對光照具有更好的魯棒性。

技術領域

本發明屬于同時定位與地圖構建(SLAM)技術領域。

背景技術

慣性測量單元(IMU)以較高的頻率測量載體的三軸角速度和加速度并解算得到姿態，可以提供短時高精度的運動估計，但是會受到零偏、溫度、振動等因素影響，存在累積誤差問題。視覺里程計(VO)通過讀取到的圖像信息可以得到較為準確的相機旋轉量和相機平移量，在靜止時不會產生漂移，但是容易受到圖像遮擋、光照環境等影響導致跟蹤丟失。由于視覺傳感器和慣性傳感器具有很好的互補性，所以將視覺和慣性傳感器融合得到視覺慣性里程計(VIO)。VIO是一種特殊的SLAM系統，在很多領域都有著廣泛的應用，如機器人、無人駕駛、智能家居等。

目前視覺傳感器和慣性傳感器的融合技術還在不斷發展和完善中。現有的經典VIO算法有MSCKF算法、OKVIS算法、ORBSLAM算法和VINS-MONO算法等，其視覺部分都是采用特征點法或光流法得到視覺觀測量，慣性部分采用流形預積分的方式得到慣性觀測量，將視覺傳感器和慣性傳感器對齊，并在后端通過卡爾曼濾波或非線性優化的方法得到最優的位姿估計值。因為IMU是一個相對獨立的傳感器，所以整個系統的魯棒性主要取決于前端VO的魯棒性。常用的VO位姿估計方法包括特征點法和光流法。特征點法需要在每一幅圖像中都提取大量的特征點，在相鄰的圖像幀之間進行特征點匹配，再根據匹配好的特征點對進行位姿估計。由于在每一幀都要提取大量的特征點并進行匹配，所以系統的計算量比較大，并且非常依賴于環境信息，在采集到的圖像沒有很好的紋理特征時幾乎無法工作。光流法先在第一幀圖像中提取一定數量的特征點，然后根據灰度不變的假設跟蹤這些特征點，得到這些特征點在下一幀圖像中的位置。相比于特征點法，光流法節省了后續特征點提取和特征點匹配的時間，具有更好的實時性，但是同樣依賴于環境信息，并且在處理邊緣上的特征點時，容易出現跟蹤失敗的問題，無法很好地利用到物體邊緣提供的信息。所以現有的VIO方法的視覺前端容易出現特征點誤匹配和圖像跟丟的問題，導致VIO位姿估計失敗，系統沒有很好的魯棒性。此外，無論是特征點法或是光流法，都沒有利用到圖像特征點之間的約束，所以會出現誤匹配的問題，影響后續的位姿估計精度。

發明內容

發明目的：為解決現有技術存在計算精度低等問題，本發明提供了一種基于稀疏直接法的視覺慣性里程計位姿估計方法。

技術方案：本發明提供了一種基于稀疏直接法的視覺慣性里程計位姿估計方法，具體包括如下步驟：

步驟一：在載體上安裝俯視單目相機VO和慣性傳感器IMU；對VO和IMU采集到的數據進行時間同步；

步驟二：對VO和IMU進行聯合初始化，并同步VO和IMU的軌跡；

步驟三：對第k+1時刻VO采集到的圖像建立N層圖像金字塔，由上往下依次對圖像金字塔的每一層進行光度補償，從而更新該第k+1時刻圖像；