[發明專利]一種視覺慣性里程計系統

在審

• 申請號: 202110433639.9
• 申請日: 2021-04-20
• 公開（公告）號: CN113358134A
• 公開（公告）日: 2021-09-07
• 發明（設計）人: 董子樂;余崇圣;李夢;陳方圓;王建波
• 申請（專利權）人: 重慶知至科技有限公司
• 主分類號: G01C22/00
• 分類號: G01C22/00;G01C21/16;G06K9/46
• 代理公司: 重慶智慧之源知識產權代理事務所(普通合伙) 50234
• 代理人: 余洪

1.一種視覺慣性里程計系統，其特征在于，包括數據采集模塊和數據處理模塊，所述數據采集模塊用關于采集圖像信息、角速度和加速度；所述數據處理模塊用于執行以下預處理和后端優化算法：

預處理：

對采集的所述圖像信息、角速度和加速度進行時間同步對齊；

針對當前圖像信息，提取圖像Harris角點，跟蹤相鄰幀與所述當前圖像信息的圖像Harris角點匹配的特征點；

利用所述采集的角速度和加速度進行積分，得到當前時刻的位置速度旋轉矩陣PVQ；

計算所述相鄰幀的預積分增量，以及誤差傳遞矩陣；

后端優化：

判斷所述當前圖像信息的特征點，與前一幀圖像匹配的特征點之間的相對位移是否超過閾值，如是，判定所述當前圖像信息為關鍵幀，邊緣化滑動窗口內最老的圖像幀，并將該關鍵幀存入所述滑動窗的最新位置；若否，則丟棄前一圖像幀，并將所述當前圖像信息和前一圖像幀之間的預積分增量，轉換為所述當前圖像信息與前二圖像幀之間的預積分增量；

構建由邊緣化的先驗信息、測量殘差、視覺重投影誤差構成的目標函數，通過優化目標函數來得到系統狀態向量的最優解，所述系統狀態向量包括系統的位置、姿態、速度、加速度計誤差和、陀螺儀誤差；基于最優解對應的系統狀態向量得到導航信息。

2.如權利要求1所述的視覺慣性里程計系統，其特征在于，所述數據采集模塊包括相機和慣性傳感器，所述相機用于所述采集圖像信息，所述慣性傳感器用于所述采集角速度和加速度。

3.如權利要求1所述的視覺慣性里程計系統，其特征在于，所述利用所述角速度和加速度進行積分，得到當前時刻的位置速度旋轉矩陣，包括通過如下積分運算得到：

所述為k+1時刻世界坐標系下的的位置，所述為k時刻的世界坐標系下的位置，所述為k時刻世界坐標系下的的速度，所述為k+1時刻世界坐標系下的的速度，所述Δt_k為k時刻到k+1時刻的時間差，所述為Body自身坐標系到Word世界坐標系的旋轉矩陣，所述和分別為所述采集的加速度和角速度，所述g^w為重力向量，所述和所述分別為加速度偏差和角速度偏差。

4.如權利要求1所述的視覺慣性里程計系統，其特征在于，所述計算所述相鄰幀的預積分增量，通過如下預積分增量函數得到：

式中，所述所述所述分別為式(1)中位置速度旋轉矩陣的預積分增量真值；所述所述所述分別為所述數據采集模塊測量值得到的預積分增量；所述所述為位置P矩陣的誤差預積分增量；所述為速度V矩陣的誤差預積分增量；所述為轉移Q矩陣的誤差預積分增量。

5.如權利要求1-4任一項所述的視覺慣性里程計系統，其特征在于，所述目標函數包括：

所述‖r_p-J_pX‖²為邊緣化的先驗信息，所述測量殘差，所述為視覺重投影誤差。

6.如權利要求5所述的視覺慣性里程計系統，其特征在于，所述通過優化目標函數來得到系統狀態向量的最優解包括：

當目標函數中的系統狀態向量X有一個增量ΔX后，目標函數有最小值，構造一個關于增量ΔX的增量公式：

上式中，為預積分誤差項的協方差，為誤差項r_B關于所有狀態向量X的Jacobian矩陣，為視覺觀測的誤差協方差矩陣；

對上述增量公式進行簡化：

(Λ_p+Λ_B+Λ_C)ΔX＝b_p+b_B+b_C (5)

其中，Λ_p，Λ_B，Λ_C為Hessian矩陣；

通過最小二乘法求解上述簡化后的增量公式，可得當前時刻系統狀態向量X的最優解。