基于VINS-Mono的室內機器人定位系統*

曾 超， 鄭茜穎， 程樹英

(福州大學 物理與信息工程學院，福建 福州 350108)

0 引 言

在全球定位系統(GPS)失效的情況下，室內移動機器人通過融合多傳感器的數據獲取精確的位置是一項富有挑戰性的工作[1]。由于單目相機和慣性測量單元(inertial measurement unit，IMU)兩種傳感器有互補的特點，并且成本低廉，基于單目相機和IMU融合的視覺—慣性里程計(visual-inertial odometry,VIO)在近些年取得了飛快的發展[2～5]， 其中，VINS-Mono[5]就是比較經典的開源框架之一。然而，VIO在移動機器人場景中由于定位精度發散較快，經常導致系統崩潰。Wu K J等人分析并通過實驗證明了在移動機器人場景中，由于加速度計得不到充分的激勵，僅使用單目相機和IMU計算出的軌跡誤差很大，并通過加入輪式里程計提升了機器人的定位精度[6]。

本文針對室內移動機器人場景中VINS-Mono定位精度發散較快的問題，融合單目相機、陀螺儀和輪式里程計的數據，并加入平面約束，最終獲得精確的軌跡。

1 系統框架

本文在VINS-Mono的基礎上，用輪式里程計的數據代替加速度計進行位移的積分，提出如圖1所示的機器人室內定位框架。系統總體框架主要包括4部分：1)在測量預處理中，系統分別對單目圖像、陀螺儀數據以及輪式里程計數據進行處理，采用KLT稀疏光流追蹤算法[7]對現有特征點進行追蹤，得到連續2幀圖像間的特征點關系，并同時對陀螺儀數據和輪式里程計數據進行預積分，得到2幀關鍵幀之間的相對位姿，然后通過誤差傳遞方程量化預積分的誤差以及計算預積分結果對各個變量的雅可比；2)在第二部分初始化階段，首先,通過運動恢復結構(structure from motion，SFM)算法計算出無尺度的相機位姿和特征點深度，然后，通過視覺—陀螺儀—輪式里程計聯合校準算法，計算出陀螺儀偏差和單目相機尺度；3)第三部分基于滑動窗口的非線性優化中，對滑動窗口內的關鍵幀數據以及對應的預積分數據通過非線性最小二乘優化求得最優解，從而對軌跡進行更精確的估計；4)第四部分是一個單獨的模塊，當該模塊開啟時，系統利用DBOW2[8]進行回環檢測，對相似度高于閾值的回環候選幀進行特征點檢索，并去除誤匹配點，隨后通過重定位算法和全局位姿圖優化算法對軌跡進行修正，保證軌跡的全局一致性。

圖1 系統的整體框架

本文在VINS-Mono的基礎上主要修改了預積分部分、初始化部分和基于滑動窗口的非線性優化部分。

2 改進的預積分算法

2.1 陀螺儀和輪式里程計的數學模型

(1)

(2)

(3)

(4)

2.2 陀螺儀—輪式里程計預積分

對于時間上連續的兩個輪式里程計坐標系Ok和Ok+1，在時間段[k,k+1]中存在多個陀螺儀和輪式里程計的測量值。假設陀螺儀的偏差值bω已經被估計出來，則在Ok坐標系上對這些數據進行積分可以得到

(5)

(6)

3 改進的初始化算法

初始化算法的步驟如下：1)通過SFM算法估計滑動窗口內的所有幀的位姿和特征點的深度[5]；2)結合單目相機SFM的計算結果以及預積分的值，建立聯立方程，求得陀螺儀的偏差以及單目相機的尺度。

3.1 陀螺儀偏差校準

(7)

根據旋轉之間的幾何關系，可以得到式(8)

(8)

式中B為滑動窗口內的所有圖像，結合式(7)和式(8)即可解出陀螺儀偏差bw。

3.2 尺度校準

(9)

(10)

(11)

結合式(9)～式(11)，便可求得尺度s。

4 改進的非線性優化算法

VINS-Mono非線性優化的核心思想是將視覺約束、預積分約束、回環檢測約束和邊緣化的成本函數放在一個大的目標函數中，通過求目標函數的最小值來獲得滑動窗口內所有幀的姿態、陀螺儀偏差、特征點深度的最優值。在本文中，主要修改了預積分約束，并且添加了平面約束。

4.1 預積分約束

相比于VINS-Mono，本系統不需要優化加速度計的偏差以及相機的速度，陀螺儀—輪式里程計殘差方程只有3項，設滑動窗口內的2個連續幀Ok和Ok+1，則陀螺儀—輪式里程計預積分的殘差可以表示為

(12)

4.2 平面約束

由于室內機器人運動的軌跡都在平面上，系統加入平面約束[6,9]，以獲得更精確的軌跡

(13)

5 實 驗

本文在數據集DS1[6]上進行仿真實驗，該數據集由Pioneer 3 DX機器人和Project Tango平板電腦進行收集，提供了頻率為30 Hz、分辨率為640×480的灰度圖像，頻率為10 Hz的輪式編碼器數據以及頻率為100 Hz的IMU數據，此外該數據集還提供了軌跡真值。數據集在一棟大樓內采集，機器人運行的軌跡長度約1 km。本文通過該數據集測試了VINS-Mono以及本文改進后的系統，圖2為本文改進后的系統在DS1數據上運行的效果。

圖2 改進后系統的運行效果

其中，圖2左邊的3幅圖從上到下分別代表機器人上攝像頭拍攝的原圖、圖中跟蹤的特征點，以及回環檢測中匹配的特征點，右邊的圖像則代表機器人運行的軌跡以及計算出的3維路標點。對VINS-Mono以及本文改進后系統的運行結果進行分析，圖3中左圖和右圖分別代表VINS-Mono和改進后系統運行的軌跡與真實軌跡的對比，其中，虛線代表的是真實的軌跡，VINS-Mono_loop和VINS-Mono_no_loop分別代表VINS-Mono運行DS1數據集時有回環檢測和無回環檢測的軌跡。由于VINS-mono運行到700 s時尺度開始漂移并且之后系統崩潰，因此只顯示了前700 s的軌跡。Our_method_loop和Our_method_no_loop分別代表改進后的系統在運行DS1數據集時有回環檢測和無回環檢測的軌跡。圖4是兩個系統對應的絕對軌跡誤差的箱型圖，改進系統的絕對軌跡誤差在各方面的屬性都優于原系統。測得的VINS-Mono無回環檢測和有回環檢測的軌跡均方根誤差分別為13.39 m和12.73 m；本文改進后的系統無回環檢測和有回環檢測的軌跡均方根誤差分別為1.59 m和1.54 m。綜上可見，在室內移動機器人場景中，本系統解決了VINS-Mono定位精度發散較快的問題，軌跡的精度也有了大幅的提升。

圖3 VINS-Mono計算的軌跡與真實軌跡對比(左圖) 改進后系統計算的軌跡與真實軌跡對比(右圖)

圖4 改進前后2個系統對應的絕對軌跡誤差箱型圖

6 結 論

基于改進VINS-Mono的室內移動機器人定位系統，主要改進了系統中的預積分、初始化以及非線性優化部分。通過輪式里程計數據替代加速度數據進行預積分，可以得到精確的運動約束；結合之前的預積分數據及單目SFM算法的運算結果，通過松耦合方法，能夠計算出陀螺儀偏差以及單目相機的尺度；用非線性最小二乘優化算法融合預積分數據和相機數據，能夠對機器人軌跡、路標點深度以及陀螺儀偏差進行優化。仿真實驗證明了改進的室內移動機器人定位系統相比于原系統的定位精度有較大的提升。