基于加權預積分和快速初始化的慣性輔助單目前端模型

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

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

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

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

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

1 IMU加權預積分算法

IMU預積分旨在將相鄰兩個關鍵幀間IMU測量值進行統一積分得到運動狀態的相對增量，為視覺慣性聯合初始化提供初始值，并為后端優化提供IMU約束。IMU預積分精度直接影響初始化結果和系統穩健性。傳統預積分模型均采用流型預積分和四元數預積分模型[12-13]，積分效果基本滿足定位要求。本節針對視覺慣性融合定位強穩健性、高精度的需求，重點探討視覺慣性數據融合同步問題，建立面向VIO的IMU高精度加權預積分模型。

1.1 加權IMU測量值

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

(1)

定義t到t+Δt時刻之間真實的加權IMU測量值為這兩個時刻測量值加權描述的常數，即

(2)

式中，加權系數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 加權預積分模型

當前時刻t+Δt的狀態可由t時刻的狀態積分得到，即

(3)

(4)

(5)

與經典的歐拉積分相比，加權歐拉積分更充分地利用了角速度和加速度測量值，可更真實地反映速度和角度的變化趨勢。

2 視覺慣性松耦合的快速聯合初始化方法

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

2.1 偏置及重力加速度估計

2.1.1 陀螺儀偏置估計

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

(6)

2.1.2 重力加速度和加速度計偏置估計

估計從IMU坐標系到相機坐標系的變換時包含了尺度s

(7)

結合式(7)和描述了兩個連續關鍵幀的式(3)，忽略加速度計及陀螺儀的偏置可得

(8)

估計重力加速度時，同時計算出不準確的尺度s，考慮連續3個關鍵幀間的式(7)、 式 (8)關系，速度項由相對速度表示，分離的變量如下

(9)

(10)

(11)

式中

結合一階近似值,gw可表示為

(12)

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

(13)

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

(14)

2.2 快速尺度恢復算法

將速度、重力向量及尺度合為一個狀態向量

(15)

(16)

式中

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

(17)

3 視覺IMU輔助跟蹤方法

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

系統的初始化階段由于無法獲取速度、重力加速度及尺度，故只能使用IMU旋轉數據。勻速跟蹤模型視相鄰關鍵幀間的相對變換不變，即對于3個連續幀F1、F2及F3，相互間的相對旋轉分別為ΔR12和ΔR23，且有

ΔT12=ΔT23

(18)

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

(19)

(20)

本文將IMU初始的先驗旋轉數據由旋轉矩陣間轉換關系得到

(21)

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

(22)

再將改進的T3作為初始值代入優化模型。當系統完成初始化后，可獲得準確的重力加速度和IMU速度，因此在視覺優化前可通過IMU獲得整個系統的速度信息

(23)

最終將改進的T3和v3聯合作為初始狀態代入優化模型。

4 試驗驗證與結果分析

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

圖2與圖3分別為MH03和V101序列在Origin_VIO及Improved_VIO 2種系統中的定位軌跡平面圖。由于系統基于關鍵幀進行定位跟蹤，故定位軌跡會存在部分不連續的情況。由圖中可知，2個系統估計的結果與實際軌跡基本重合，但Improved_VIO的軌跡較于Origin_VIO的軌跡整體更連續、穩定且偏離更小。

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

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

表1給出9種EuRoC數據序列在Origin_VIO及Improved_VIO 2種系統中的定位結果。由表1可知，本文定位結果優于Origin_VIO的定位結果，定位精度平均提高了約30%。

表1 2種系統在EuRoC數據集上定位結果對比

5 結 語

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