單目視覺慣性定位的IMU輔助跟蹤模型

王 帥，潘樹國，黃礪梟，曾 攀

(東南大學儀器科學與工程學院，江蘇 南京 210096)

定位是機器人根據任務做出正確決策和路徑選擇的基礎[1-2]，如何設計基于多傳感器的高精度和穩健性的跟蹤定位模型顯得尤為重要。傳統的室外定位主要是衛星導航定位[3-5]，室內定位算法主要有慣性導航法和視覺定位方法[6]。利用慣性傳感器與圖像傳感器的互補性，可將視覺定位與慣性導航技術相融合，得到精度更高、穩健性更好的緊耦合定位技術。

單目視覺慣性緊耦合定位技術近年來發展迅速，由最初的基于卡爾曼濾波[7]的緊耦合方法逐漸發展到基于光束平差法(bundle adjustment，BA[8])優化和圖優化的方法。Anastasios I.Mourikis于2007年提出了一個典型的基于擴展卡爾曼濾波的緊耦合模型MSCKF[9]，從此開啟了視覺慣性緊耦合的定位方法。MSCKF的跟蹤模型是基本的視覺參考幀模型，在跟蹤情況下沒有使用IMU的先驗數據，因此系統極易不收斂，導致定位精度較差。國外ORB_SLAM2[10]的作者Raul Mur-Artal于2017年提出了具有地圖重用的視覺慣性單目SLAM技術[11]，提出了一種簡便的視覺慣性聯合初始化方法，但該方法需要15 s左右的初始化時間，因此在初始化成功之前需要穩定的視覺前端基礎，而ORB_SLAM在初始化期間采用的是簡單的勻速跟蹤模型，在相機劇烈運動時無法正確初始化，甚至跟蹤失敗。香港科大機器人研究所于2017年提出了VINS(visual-inertial system)[13]，VINS視覺方面采用的是稀疏光流法進行跟蹤定位，文中提出了一種穩健性的視覺慣性聯合初始化過程和恢復過程，以及基于滑動窗口的緊耦合非線性優化方法，但是整個系統采用的是最基本的參考幀跟蹤模型，無法克服復雜的運動環境。

綜上所述，由于傳統的單目視覺慣性定位跟蹤模型都采用簡單的參考幀模型和勻速模型，系統的定位結果易受劇烈運動和復雜環境影響，從而降低定位精度；而IMU先驗數據可以有效克服劇烈運動等影響，因此設計一種面向單目視覺慣性緊耦合定位的IMU輔助跟蹤模型具有重要的意義。

1 IMU輔助跟蹤模型設計

前文提到的勻速跟蹤模型和傳統的參考幀跟蹤模型原理簡單，但無法克服劇烈運動問題，因此本文重點提出一種IMU輔助跟蹤模型。IMU輔助跟蹤模型是結合單目勻速模型和IMU先驗數據，為初始化階段及初始化完成后提供一個穩健性的視覺前端，從而有效提高視覺慣性系統的整體性能。

1.1 IMU先驗數據解算

IMU可以輸出加速度和角速度，對角速度進行積分可以獲得IMU單元的相對旋轉。

(1)

同時IMU的數據幀率一般遠高于視覺，在視覺的k和k+1幀之間通常會有多組IMU數據，IMU預積分模型[14]即是計算視覺k和k+1幀之間的一組IMU積分值。式(2)為IMU運動模型

(2)

t+Δt時刻的運動狀態可通過積分獲得

(3)

利用中值積分法，聯立式(1)和式(3)可得

(4)

上式可以看出每次對t+Δt時刻的旋轉計算都需要知道t時刻的旋轉，因此當RWB(t)隨時間變化后需要重新計算積分，這將導致計算量的增大。為了避免重復的狀態傳播計算，采用預積分的方式

(5)

(6)

(7)

1.2 系統初始狀態估計

當系統處于初始化階段時，由于系統速度、重力加速度和尺度無法獲取，因此只能使用IMU旋轉數據。

圖1 勻速跟蹤模型

圖1為一般的勻速跟蹤模型，假設有3個連續幀，F1、F2和F3之間的相對旋轉分別為ΔR12和ΔR23，勻速跟蹤模型是假設相鄰幀之間的相對變換不變，即

ΔT12=ΔT23

(8)

F1與F2之間的相對變換

(9)

式中，RW1和RW2分別為F1和F2的旋轉；pW1和pW2分別為F1和F2的位移；TW1和TW2分別為F1和F2的位姿。

因此F3的變換矩陣

TW3=ΔT23TW3=ΔT12TW3=

(10)

由旋轉矩陣間的轉換關系可知

(11)

式中，RBC為相機和IMU之間的旋轉外參；ΔR23為相機幀F2與相機幀F3之間的相對旋轉，是由IMU先驗數據提供；RW3為F3的旋轉。

將旋轉矩陣代入到變換矩陣模型中

(12)

最終將改進的TW3作為初始值代入到優化模型中。

當系統完成初始化后，重力加速度和IMU速度可以準確地獲得，因此在視覺優化前可以通過IMU獲得整個系統的速度信息

(13)

2 試驗驗證與結果分析

本文中的算法驗證基于視覺慣性ORB_SLAM2框架，數據采用的是EuRoC數據集，這些數據集都是由迷你飛行器對兩個不同的房間和工業環境采集得到。數據集分為簡單的、中等的和復雜的情況，所使用的視覺慣性器件為VI-Sensor雙目慣性相機，其中相機的采樣頻率為20 Hz，IMU的采樣頻率為200 Hz，真實軌跡可由數據提供，試驗只使用其左目相機，處理器使用的是內存為8 GB的Intel Core i7-7000筆記本。在整個試驗中，將本方案分別和OKVIS雙目[15]及ORB_SLAM2的雙目作比較，本試驗使用了兩個數據集畫圖以驗證試驗效果，分別為V1_01_easy、MH_03_median。

試驗1采用V1_01_easy數據集，將本方案與ORB_SLAM2雙目作比較，圖2(a)為ORB_SLAM2的V1_01_easy軌跡平面圖，圖2(b)為VIO_IMU的V1_01_easy軌跡平面圖，VIO_IMU為采用IMU輔助跟蹤模型的定位結果圖，其中ORB_SLAM2雙目的均方根誤差為0.087 m，VIO_IMU的均方根誤差為0.065 m。由圖2(a)可以看出，VIO_IMU的定位軌跡與真實軌跡更為符合。圖3(a)給出了與ORB_SLAM2雙目比較的V1_01_easy軌跡誤差，圖中可以明顯看出VIO_IMU的定位誤差在整體上要優于ORB_SLAM2雙目的定位誤差；表1列出了定位誤差的狀態對比，可以看出，VIO_IMU定位誤差的中誤差、均方根誤差、最小值、中位數、均小于ORB_SLAM2雙目定位誤差的相應結果。圖3右是定位誤差的小提琴圖，主要用來統計和分析數據的離散程度和分布密度，圖中可以看出VIO_IMU誤差的分布主要集中在0.025 m左右，而ORB_SLAM2雙目主要集中在0.095 m附近，VIO_IMU誤差的中位數和四分位數都優于ORB_SLAM2雙目，上側的離散情況二者基本相同，VIO_IMU誤差的下側離散值要優于ORB_SLAM2雙目的誤差。

表1輔助模型與ORB_SLAM2雙目在V1_01_easy數據集下誤差狀態對比m

模型stdrmsemaxminORB_SLAM2雙目0.0250.0920.1520.022VIO_IMU0.0230.0490.1380.006

試驗2采用MH_03_media數據集，將本方案與OKVIS雙目作比較。由于本文的定位方案及OKVIS是由慣性器件來恢復整個系統的尺度信息，存在一定的尺度誤差，而ORB_SLAM2可以直接由雙目獲得，對于中等尺度的MAV-MH數據集，ORB_SLAM2占絕對優勢，因此MH_03_media數據集不考慮與ORB_SLAM2雙目的比較。圖4(a)為OKVIS雙目的MH_03_media軌跡平面圖，圖4(b)為VIO_IMU的MH_03_media軌跡平面圖，由圖4可知VIO_IMU和真實軌跡更為接近。表2為視覺慣性輔助模型與OKVIS在EuRoC數據集下的定位均方根誤差，由表1可以看到本文提出的定位方案優于OKVIS的定位結果。

圖2

圖3

圖4

表2輔助模型與OKVIS在EuRoC數據集下的均方根誤差m

模型V1_01V1_02V1_03MH_03OKVIS0.0880.1730.1970.269VIO_IMU0.0650.1210.1560.099

3 結 語

本文針對快速運動下單目視覺慣性定位跟蹤失效的問題，提出了一種面向單目視覺慣性緊耦合定位的IMU輔助跟蹤模型，主要利用IMU獲得先驗的運動狀態作為整個系統的定位初值，代入到優化算法中，提升單目視覺慣性緊耦合定位下的穩健性。由于整個視覺慣性系統分為初始化狀態和初始化完成狀態，因此分別對不同階段實現相應的跟蹤方法，建立一個整體的IMU輔助跟蹤模型。

通過對不同SLAM公開數據集進行試驗驗證，IMU輔助跟蹤模型優于OKVIS雙目定位模型，相比ORB_SLAM2雙目的定位精度在小尺度范圍內有較大的改善，說明本文的模型在單目視覺慣性定位中具有一定的研究意義。但本文的研究模型仍然存在不足之處，在初始位姿估計上只利用了IMU預積分提供的先驗旋轉和速度信息，當同時使用IMU預積分解算的位置數據時，定位結果出現較大誤差。下一步將重點研究如何有效地將位置數據融合到初始位姿估計中，以提升單目視覺慣性緊耦合定位的精度和穩健性。