王 帥，潘樹國(guó)，黃礪梟，曾 攀

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

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

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

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

1 IMU輔助跟蹤模型設(shè)計(jì)

前文提到的勻速跟蹤模型和傳統(tǒng)的參考幀跟蹤模型原理簡(jiǎn)單，但無(wú)法克服劇烈運(yùn)動(dòng)問題，因此本文重點(diǎn)提出一種IMU輔助跟蹤模型。IMU輔助跟蹤模型是結(jié)合單目勻速模型和IMU先驗(yàn)數(shù)據(jù)，為初始化階段及初始化完成后提供一個(gè)穩(wěn)健性的視覺前端，從而有效提高視覺慣性系統(tǒng)的整體性能。

1.1 IMU先驗(yàn)數(shù)據(jù)解算

IMU可以輸出加速度和角速度，對(duì)角速度進(jìn)行積分可以獲得IMU單元的相對(duì)旋轉(zhuǎn)。

(1)

同時(shí)IMU的數(shù)據(jù)幀率一般遠(yuǎn)高于視覺，在視覺的k和k+1幀之間通常會(huì)有多組IMU數(shù)據(jù)，IMU預(yù)積分模型[14]即是計(jì)算視覺k和k+1幀之間的一組IMU積分值。式(2)為IMU運(yùn)動(dòng)模型

(2)

t+Δt時(shí)刻的運(yùn)動(dòng)狀態(tài)可通過積分獲得

(3)

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

(4)

上式可以看出每次對(duì)t+Δt時(shí)刻的旋轉(zhuǎn)計(jì)算都需要知道t時(shí)刻的旋轉(zhuǎn)，因此當(dāng)RWB(t)隨時(shí)間變化后需要重新計(jì)算積分，這將導(dǎo)致計(jì)算量的增大。為了避免重復(fù)的狀態(tài)傳播計(jì)算，采用預(yù)積分的方式

(5)

(6)

(7)

1.2 系統(tǒng)初始狀態(tài)估計(jì)

當(dāng)系統(tǒng)處于初始化階段時(shí)，由于系統(tǒng)速度、重力加速度和尺度無(wú)法獲取，因此只能使用IMU旋轉(zhuǎn)數(shù)據(jù)。

圖1 勻速跟蹤模型

圖1為一般的勻速跟蹤模型，假設(shè)有3個(gè)連續(xù)幀，F(xiàn)1、F2和F3之間的相對(duì)旋轉(zhuǎn)分別為ΔR12和ΔR23，勻速跟蹤模型是假設(shè)相鄰幀之間的相對(duì)變換不變，即

ΔT12=ΔT23

(8)

F1與F2之間的相對(duì)變換

(9)

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

因此F3的變換矩陣

TW3=ΔT23TW3=ΔT12TW3=

(10)

由旋轉(zhuǎn)矩陣間的轉(zhuǎn)換關(guān)系可知

(11)

式中，RBC為相機(jī)和IMU之間的旋轉(zhuǎn)外參；ΔR23為相機(jī)幀F(xiàn)2與相機(jī)幀F(xiàn)3之間的相對(duì)旋轉(zhuǎn)，是由IMU先驗(yàn)數(shù)據(jù)提供；RW3為F3的旋轉(zhuǎn)。

將旋轉(zhuǎn)矩陣代入到變換矩陣模型中

(12)

最終將改進(jìn)的TW3作為初始值代入到優(yōu)化模型中。

當(dāng)系統(tǒng)完成初始化后，重力加速度和IMU速度可以準(zhǔn)確地獲得，因此在視覺優(yōu)化前可以通過IMU獲得整個(gè)系統(tǒng)的速度信息

(13)

2 試驗(yàn)驗(yàn)證與結(jié)果分析

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

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

表1輔助模型與ORB_SLAM2雙目在V1_01_easy數(shù)據(jù)集下誤差狀態(tài)對(duì)比m

模型stdrmsemaxminORB_SLAM2雙目0.0250.0920.1520.022VIO_IMU0.0230.0490.1380.006

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

圖2

圖3

圖4

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

模型V1_01V1_02V1_03MH_03OKVIS0.0880.1730.1970.269VIO_IMU0.0650.1210.1560.099

3 結(jié) 語(yǔ)

本文針對(duì)快速運(yùn)動(dòng)下單目視覺慣性定位跟蹤失效的問題，提出了一種面向單目視覺慣性緊耦合定位的IMU輔助跟蹤模型，主要利用IMU獲得先驗(yàn)的運(yùn)動(dòng)狀態(tài)作為整個(gè)系統(tǒng)的定位初值，代入到優(yōu)化算法中，提升單目視覺慣性緊耦合定位下的穩(wěn)健性。由于整個(gè)視覺慣性系統(tǒng)分為初始化狀態(tài)和初始化完成狀態(tài)，因此分別對(duì)不同階段實(shí)現(xiàn)相應(yīng)的跟蹤方法，建立一個(gè)整體的IMU輔助跟蹤模型。

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