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

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

實(shí)現(xiàn)高精度、強(qiáng)穩(wěn)健性定位是完成移動(dòng)智能體自主導(dǎo)航、探索未知區(qū)域等既定任務(wù)的重要前提[1- 2]。單目視覺傳感器可獲得形狀、顏色及紋理等豐富的環(huán)境信息，但光照變化、無紋理對(duì)象、運(yùn)動(dòng)模糊和未知尺度都嚴(yán)重影響單目視覺定位結(jié)果。慣性測量單元(IMU)提供了移動(dòng)體高采樣率的測量數(shù)據(jù)，動(dòng)態(tài)性能好，但存在漂移現(xiàn)象。利用這兩類傳感器的互補(bǔ)性，可將視覺定位與慣性導(dǎo)航技術(shù)融合即VIO(visual inertial odometry)[3- 4]。當(dāng)單目視覺傳感器無法獲得有效視覺信息時(shí)，IMU繼續(xù)進(jìn)行運(yùn)動(dòng)估計(jì)并恢復(fù)尺度，而視覺過程解決了IMU的漂移問題，兩者結(jié)合可構(gòu)建出精度更高、穩(wěn)健性更強(qiáng)的定位系統(tǒng)。因此，VIO在微型飛行器、自動(dòng)駕駛、虛擬現(xiàn)實(shí)及增強(qiáng)現(xiàn)實(shí)等領(lǐng)域有著巨大潛力和廣泛的應(yīng)用前景[5- 8]。

文獻(xiàn)[9]在OKVIS框架中提出了融合視覺慣性的前端模型，該模型實(shí)現(xiàn)了視覺和慣性數(shù)據(jù)的緊耦合，但未恢復(fù)出系統(tǒng)尺度和重力加速度等數(shù)據(jù)，同時(shí)不含預(yù)積分框架，因此其定位精度和穩(wěn)健性較差。文獻(xiàn)[10]提出了基于ORB_SLAM2的單目視覺慣性定位前端框架，其預(yù)積分模型采用流型預(yù)積分算法，視覺慣性初始化模型需約15 s，在該階段無法實(shí)現(xiàn)實(shí)時(shí)穩(wěn)健性定位，同時(shí)其跟蹤模型采用簡單的勻速模型，使相機(jī)劇烈運(yùn)動(dòng)時(shí)無法正確初始化，甚至跟蹤失敗，最終導(dǎo)致定位效果不佳。同年，文獻(xiàn)[11]提出了VINS(visual- inertial navigation system)，其初始化模型相對(duì)較快，但并未對(duì)加速度計(jì)偏置進(jìn)行標(biāo)定，同時(shí)初始化精度略低，采用傳統(tǒng)的參考幀跟蹤模型限制了其定位精度。

綜上，針對(duì)目前國內(nèi)外學(xué)者提出的單目視覺慣性融合定位模型存在的定位精度略低、初始化時(shí)間較長及穩(wěn)健性不高的問題，本文提出一種包含IMU加權(quán)預(yù)積分、快速聯(lián)合初始化及視覺IMU輔助跟蹤的面向單目視覺慣性緊耦合定位的改進(jìn)前端模型Improved_VIO，模型框架如圖1所示。

1 IMU加權(quán)預(yù)積分算法

IMU預(yù)積分旨在將相鄰兩個(gè)關(guān)鍵幀間IMU測量值進(jìn)行統(tǒng)一積分得到運(yùn)動(dòng)狀態(tài)的相對(duì)增量，為視覺慣性聯(lián)合初始化提供初始值，并為后端優(yōu)化提供IMU約束。IMU預(yù)積分精度直接影響初始化結(jié)果和系統(tǒng)穩(wěn)健性。傳統(tǒng)預(yù)積分模型均采用流型預(yù)積分和四元數(shù)預(yù)積分模型[12-13]，積分效果基本滿足定位要求。本節(jié)針對(duì)視覺慣性融合定位強(qiáng)穩(wěn)健性、高精度的需求，重點(diǎn)探討視覺慣性數(shù)據(jù)融合同步問題，建立面向VIO的IMU高精度加權(quán)預(yù)積分模型。

1.1 加權(quán)IMU測量值

IMU的測量值包含由陀螺儀和加速度計(jì)分別提供的角速度和加速度，t時(shí)刻典型的測量值方程如下

(1)

定義t到t+Δt時(shí)刻之間真實(shí)的加權(quán)IMU測量值為這兩個(gè)時(shí)刻測量值加權(quán)描述的常數(shù)，即

(2)

式中，加權(quán)系數(shù)c1(t)、c2(t)、c3(t)、c4(t)具體表示如下

c1(t)=wB(t)/(wB(t)+wB(t+Δt))

c2(t)=wB(t+Δt)/(wB(t)+wB(t+Δt))

c3(t)=aB(t)/(aB(t)+aB(t+Δt))

c4(t)=aB(t+Δt)/(aB(t)+aB(t+Δt))

1.2 加權(quán)預(yù)積分模型

當(dāng)前時(shí)刻t+Δt的狀態(tài)可由t時(shí)刻的狀態(tài)積分得到，即

(3)

(4)

(5)

與經(jīng)典的歐拉積分相比，加權(quán)歐拉積分更充分地利用了角速度和加速度測量值，可更真實(shí)地反映速度和角度的變化趨勢。

2 視覺慣性松耦合的快速聯(lián)合初始化方法

在視覺慣性耦合系統(tǒng)中，初始化是在緊耦合處理前計(jì)算絕對(duì)尺度、重力加速度、速度等信息的過程，此外初始化計(jì)算的加速度計(jì)偏置和陀螺儀偏置能使重新計(jì)算的預(yù)積分值更精確。初始化精度和時(shí)間直接影響視覺慣性緊耦合系統(tǒng)的穩(wěn)健性、定位精度及實(shí)時(shí)性。為縮短初始化時(shí)間，本節(jié)重點(diǎn)研究尺度恢復(fù)問題，提出了一種視覺慣性松耦合的快速聯(lián)合初始化方法。

2.1 偏置及重力加速度估計(jì)

2.1.1 陀螺儀偏置估計(jì)

由于噪聲的存在，分別從陀螺儀測量值和視覺測量值得到的旋轉(zhuǎn)不相等，故通過最小化相對(duì)旋轉(zhuǎn)的差異來優(yōu)化陀螺儀偏置，優(yōu)化模型如下

(6)

2.1.2 重力加速度和加速度計(jì)偏置估計(jì)

估計(jì)從IMU坐標(biāo)系到相機(jī)坐標(biāo)系的變換時(shí)包含了尺度s

(7)

結(jié)合式(7)和描述了兩個(gè)連續(xù)關(guān)鍵幀的式(3)，忽略加速度計(jì)及陀螺儀的偏置可得

(8)

估計(jì)重力加速度時(shí)，同時(shí)計(jì)算出不準(zhǔn)確的尺度s，考慮連續(xù)3個(gè)關(guān)鍵幀間的式(7)、 式 (8)關(guān)系，速度項(xiàng)由相對(duì)速度表示，分離的變量如下

(9)

(10)

(11)

式中

結(jié)合一階近似值,gw可表示為

(12)

結(jié)合式(8)和式 (12)并包含加速度偏置，得到

(13)

式(9)進(jìn)一步轉(zhuǎn)化為含有尺度、重力加速度方向修正及加速度計(jì)偏置的公式

(14)

2.2 快速尺度恢復(fù)算法

將速度、重力向量及尺度合為一個(gè)狀態(tài)向量

(15)

(16)

式中

通過求解此線性最小二乘問題，可得到相機(jī)幀的速度v和優(yōu)化后尺度s

(17)

3 視覺IMU輔助跟蹤方法

IMU輔助跟蹤模型結(jié)合了單目勻速模型和IMU先驗(yàn)數(shù)據(jù)，為初始化階段及初始化完成后提供一個(gè)穩(wěn)健性的視覺前端，能有效提高視覺慣性系統(tǒng)的整體定位性能。IMU輔助跟蹤模型主要包括IMU先驗(yàn)數(shù)據(jù)解算和系統(tǒng)初始化估計(jì)兩個(gè)部分，IMU先驗(yàn)數(shù)據(jù)解算包含在IMU預(yù)積分中，本節(jié)將主要介紹系統(tǒng)初始化估計(jì)的跟蹤部分。

系統(tǒng)的初始化階段由于無法獲取速度、重力加速度及尺度，故只能使用IMU旋轉(zhuǎn)數(shù)據(jù)。勻速跟蹤模型視相鄰關(guān)鍵幀間的相對(duì)變換不變，即對(duì)于3個(gè)連續(xù)幀F(xiàn)1、F2及F3，相互間的相對(duì)旋轉(zhuǎn)分別為ΔR12和ΔR23，且有

ΔT12=ΔT23

(18)

F1和F2之間的相對(duì)變換及F3的變換矩陣分別為

(19)

(20)

本文將IMU初始的先驗(yàn)旋轉(zhuǎn)數(shù)據(jù)由旋轉(zhuǎn)矩陣間轉(zhuǎn)換關(guān)系得到

(21)

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

(22)

再將改進(jìn)的T3作為初始值代入優(yōu)化模型。當(dāng)系統(tǒng)完成初始化后，可獲得準(zhǔn)確的重力加速度和IMU速度，因此在視覺優(yōu)化前可通過IMU獲得整個(gè)系統(tǒng)的速度信息

(23)

最終將改進(jìn)的T3和v3聯(lián)合作為初始狀態(tài)代入優(yōu)化模型。

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

為驗(yàn)證提出的單目視覺慣性改進(jìn)前端模型的可行性和穩(wěn)定性，本文算法基于視覺慣性O(shè)RB_SLAM2(后文記為Origin_VIO)，并采用公開的EuRoC MAV數(shù)據(jù)集中的9個(gè)測試序列[15]。該數(shù)據(jù)集包含了不同的房間和工業(yè)環(huán)境中，配備有VI- Sensor雙目慣性相機(jī)的無人機(jī)動(dòng)態(tài)運(yùn)動(dòng)。其圖像采集頻率為20 Hz，IMU采樣頻率為200 Hz，數(shù)據(jù)集提供真實(shí)軌跡。由于是單目視覺慣性平臺(tái)，故僅使用數(shù)據(jù)集中左目相機(jī)測量值。為表述簡便，將該數(shù)據(jù)集的數(shù)據(jù)序列簡記，如MH_03_medium記為MH03，其他序列類似。本文試驗(yàn)使用內(nèi)存為8 GB的Intel Core i7- 7000筆記本，將Improved_VIO試驗(yàn)結(jié)果與Origin_VIO結(jié)果作比較，并圖示了V101和MH03兩個(gè)數(shù)據(jù)序列以展示試驗(yàn)效果。

圖2與圖3分別為MH03和V101序列在Origin_VIO及Improved_VIO 2種系統(tǒng)中的定位軌跡平面圖。由于系統(tǒng)基于關(guān)鍵幀進(jìn)行定位跟蹤，故定位軌跡會(huì)存在部分不連續(xù)的情況。由圖中可知，2個(gè)系統(tǒng)估計(jì)的結(jié)果與實(shí)際軌跡基本重合，但I(xiàn)mproved_VIO的軌跡較于Origin_VIO的軌跡整體更連續(xù)、穩(wěn)定且偏離更小。

圖4—圖6為MH03序列在2種系統(tǒng)中定位的軌跡誤差分析圖。其中，圖4對(duì)比了Origin_VIO和Improved_VIO的絕對(duì)定位誤差，顯示出Improved_VIO的定位結(jié)果整體更優(yōu)。圖5具體表示了2種系統(tǒng)在3個(gè)不同方向上的位置坐標(biāo)變化情況。由圖中可知，Improved_VIO及Origin_VIO的軌跡在X、Y方向上均較好地符合實(shí)際軌跡，Z方向上Improved_VIO表現(xiàn)更優(yōu)。圖6為定位誤差的小提琴圖，主要統(tǒng)計(jì)分析數(shù)據(jù)的離散程度和分布密度。Improved_VIO的軌跡誤差分布主要集中在0.095 m左右，而Origin_VIO的軌跡誤差主要集中在0.194 m附近，前者誤差的中位數(shù)和四分位數(shù)都較小，且下側(cè)的離散值表現(xiàn)也更優(yōu)。

為驗(yàn)證本文提出的聯(lián)合初始化方法的快速性，以V101序列為例做初始化狀態(tài)圖。圖7—圖10分別為初始化尺度、重力加速度、陀螺儀偏置及加速度計(jì)偏置的變化情況。由圖中可知，這4類初始化量在Improved_VIO中10 s內(nèi)穩(wěn)定收斂，快于Origin_VIO。

表1給出9種EuRoC數(shù)據(jù)序列在Origin_VIO及Improved_VIO 2種系統(tǒng)中的定位結(jié)果。由表1可知，本文定位結(jié)果優(yōu)于Origin_VIO的定位結(jié)果，定位精度平均提高了約30%。

表1 2種系統(tǒng)在EuRoC數(shù)據(jù)集上定位結(jié)果對(duì)比

5 結(jié) 語

本文提出了一種改進(jìn)的單目視覺慣性定位前端模型，結(jié)合IMU加權(quán)預(yù)積分模型、快速聯(lián)合初始化方法及視覺IMU輔助跟蹤模型，提升了單目視覺慣性定位的精度和實(shí)時(shí)性。通過對(duì)EuRoC數(shù)據(jù)集的試驗(yàn)驗(yàn)證，本文提出的Improved_VIO相比于視覺慣性O(shè)RB_SLAM2在小尺度范圍內(nèi)的定位精度有較大改善。其中IMU加權(quán)預(yù)積分方法表現(xiàn)出良好性能，同時(shí)實(shí)現(xiàn)了10 s內(nèi)的準(zhǔn)確初始化，系統(tǒng)整體定位精度提高了約30%。本文研究模型仍有改進(jìn)空間，初始位姿估計(jì)中未利用IMU預(yù)積分提供的位置數(shù)據(jù)，對(duì)傳感器偏置和噪聲處理較簡單，未來將重點(diǎn)研究位置數(shù)據(jù)有效地融入初始位姿估計(jì)中，并建立偏置和噪聲分析處理模型，以進(jìn)一步提升單目視覺慣性緊耦合定位的精度和穩(wěn)健性。