基于視覺(jué)的慣性導(dǎo)航誤差在線修正

， ，

(航天科工智能機(jī)器人有限責(zé)任公司，北京 100074)

0 引言

基于視覺(jué)的同步定位與建圖(Simultaneous Localization and Mapping,SLAM)，已經(jīng)被越來(lái)越多地應(yīng)用在機(jī)器人導(dǎo)航、無(wú)人機(jī)導(dǎo)航等領(lǐng)域。但是單純的視覺(jué)SLAM很難勝任復(fù)雜的任務(wù)，往往需要多傳感器融合才能滿足任務(wù)要求。其中慣性測(cè)量單元(Inertial Measurement Unit,IMU)被認(rèn)為與相機(jī)傳感器有明顯的互補(bǔ)優(yōu)勢(shì)，而且十分有潛力在融合之后得到更加完善的SLAM系統(tǒng)[1]。

視覺(jué)通過(guò)外部特征點(diǎn)的匹配獲取自身位姿，所以視覺(jué)不需要對(duì)時(shí)間進(jìn)行積分，沒(méi)有像IMU一樣的零漂，但是視覺(jué)不能直接得到自身的運(yùn)動(dòng)狀態(tài)，所以在大加速度和大角速度的運(yùn)動(dòng)下無(wú)法給出較為準(zhǔn)確的姿態(tài)估計(jì)，容易導(dǎo)致跟蹤失敗，同時(shí)也無(wú)法辨別環(huán)境中的動(dòng)態(tài)物體。IMU雖然可以測(cè)得角速度和加速度，但這些測(cè)量量都存在明顯的漂移，尤其對(duì)于低成本的IMU來(lái)說(shuō)，長(zhǎng)時(shí)間積分得到的導(dǎo)航信息非常不可靠，但對(duì)于短時(shí)間內(nèi)的快速運(yùn)動(dòng)可以提供較好的估計(jì)。正是由于這些特點(diǎn)，視覺(jué)和IMU可以?xún)?yōu)缺點(diǎn)互補(bǔ)。視覺(jué)SLAM不會(huì)隨著時(shí)間漂移，可以有效地修正IMU數(shù)據(jù)中的漂移，使其在慢速運(yùn)動(dòng)中的位姿估計(jì)依然可用。IMU對(duì)自身加速度和角速度的測(cè)量可以提高系統(tǒng)的動(dòng)態(tài)性能，同時(shí)幫助系統(tǒng)判別環(huán)境中的動(dòng)態(tài)障礙物。

因此多年來(lái)出現(xiàn)了很多視覺(jué)和IMU組合導(dǎo)航的算法。其中一大類(lèi)是基于濾波的組合算法，在文獻(xiàn)[2]、文獻(xiàn)[3]的視覺(jué)慣性導(dǎo)航(Visual Inertial Navigation,VIN)算法中有比較充分的介紹，盡管這類(lèi)算法在實(shí)時(shí)性上有不錯(cuò)的表現(xiàn)，但是由于線性化等誤差以及誤差積累，導(dǎo)致長(zhǎng)時(shí)間工作時(shí)狀態(tài)估計(jì)誤差增大，甚至導(dǎo)致濾波發(fā)散；另一類(lèi)算法是基于非線性?xún)?yōu)化的組合算法，例如文獻(xiàn)[4]、文獻(xiàn)[5]，但是隨著應(yīng)用環(huán)境逐步增大，數(shù)據(jù)越來(lái)越多，實(shí)時(shí)性變得越來(lái)越差。因此發(fā)展出基于關(guān)鍵幀的非線性的優(yōu)化算法，通過(guò)優(yōu)化與跟蹤并行的非同步優(yōu)化方法來(lái)提高實(shí)時(shí)性，如文獻(xiàn)[6]、文獻(xiàn)[7]。

本文針對(duì)廉價(jià)的慣性測(cè)量單元和視覺(jué)融合的算法，提出一種基于Kalman濾波的在線估計(jì)陀螺加速度計(jì)零偏的算法。因?yàn)榱畠r(jià)的慣性器件精度較低，整個(gè)SLAM系統(tǒng)并不將其用于提高視覺(jué)SLAM算法的精度，而是用于提高系統(tǒng)在大加速度、大角速度下的適應(yīng)性，因此采用濾波與文獻(xiàn)[8]中的非線性?xún)?yōu)化結(jié)合的方法。通過(guò)優(yōu)化算法提高視覺(jué)SLAM的精度，將視覺(jué)信息作為觀測(cè)量，利用濾波進(jìn)行零偏估計(jì)，為視覺(jué)SLAM提供運(yùn)動(dòng)預(yù)測(cè)，使整個(gè)SLAM系統(tǒng)具有很好的動(dòng)態(tài)適應(yīng)性，并保持很好的實(shí)時(shí)性。

1 系統(tǒng)模型

1.1 IMU積分模型

在此約定，用W表示世界坐標(biāo)系。W坐標(biāo)系是在建圖時(shí)隨機(jī)確定的，但一旦確定，在建圖過(guò)程中和建圖完成后的導(dǎo)航任務(wù)中，都是固定不變的。B表示IMU坐標(biāo)系，由于IMU與車(chē)體固聯(lián)，B系亦即載體坐標(biāo)系。

(1)

(2)

由于世界坐標(biāo)系是隨機(jī)確定的，重力加速度已知，但在世界坐標(biāo)系各軸分量未知，同時(shí)重力加速度和加速度計(jì)零偏區(qū)分比較困難，因此在算法中需要對(duì)重力加速度的分量和IMU的零偏同時(shí)進(jìn)行估計(jì)。有了加速度和角速度的測(cè)量以后，可以積分得到IMU相對(duì)世界坐標(biāo)系的姿態(tài)RBW，以下簡(jiǎn)稱(chēng)R。本文姿態(tài)均用旋轉(zhuǎn)矩陣表示。IMU相對(duì)世界坐標(biāo)系的速度和位置分別用V和P表示。有如下關(guān)系：

(3)

(4)

(5)

其中,(·)∧表示一種運(yùn)算。

(6)

積分式(3)、式(4)、式(5)并代入式(1)、式(2)，可得

(7)

其中，Exp(φ)=exp(φ∧)。

V(t+Δt)=V(t)+gΔt+

(8)

(9)

式(7)、式(8)、式(9)就是本文使用的IMU數(shù)據(jù)的積分模型，其中陀螺零偏bg、加速度零偏ba就是本文所要估計(jì)的量。

1.2 相機(jī)模型

相機(jī)模型為針孔模型如圖1所示，用I表示圖像坐標(biāo)系o′x′y′。P=[X,Y,Z]T表示空間一點(diǎn)相對(duì)相機(jī)坐標(biāo)系C的位置在C中的表示。p=[u,v,1]T為點(diǎn)P在I投影的齊次坐標(biāo)，已知相機(jī)的內(nèi)參矩陣K有

(10)

圖1 針孔相機(jī)模型示意圖Fig.1 Diagrammatic sketch of pinhole camera model

系統(tǒng)中，IMU和相機(jī)是固連在一起的，其中IMU與相機(jī)的位置關(guān)系已知。用C表示相機(jī)坐標(biāo)系，用RCB、PCB分別表示由相機(jī)坐標(biāo)系到IMU坐標(biāo)系的位姿變化。RWC、PWC表示相機(jī)相對(duì)世界坐標(biāo)系的位姿，有：

RWB=RWCRCB

(11)

PWB=RWCPCB+PWC

(12)

1.3 IMU和相機(jī)的組合導(dǎo)航算法

IMU數(shù)據(jù)采集的頻率往往比視覺(jué)數(shù)據(jù)采集的頻率高很多。如圖2所示，視覺(jué)中，相鄰的圖像幀Ci、Cj通過(guò)共同觀察到的路標(biāo)集合M建立起相對(duì)位姿的關(guān)系。具體算法在文獻(xiàn)[9]中的ORB_SLAM算法中有比較詳細(xì)的描述。每一個(gè)路標(biāo)為空間中的一點(diǎn)mi，對(duì)應(yīng)一個(gè)觀測(cè)zi，zi為mi在I中投影的齊次坐標(biāo)，由式(10)有

圖2 視覺(jué)和IMU組合模型Fig.2 Model of visual-inertial navigation

(13)

(14)

關(guān)于PCi、RCi的殘差rc可以表示為

(15)

(16)

根據(jù)式(11)、式(12)、式(16)可以得到RWB和PWB。通過(guò)非線性?xún)?yōu)化方法可以很好地消除圖像采集和特征提取的噪聲，同時(shí)可以減小錯(cuò)誤匹配帶來(lái)的影響，得到較為準(zhǔn)確的位姿，但式(15)是非凸優(yōu)化，優(yōu)化的精度和速度對(duì)優(yōu)化初值很敏感。當(dāng)初值和真值相差較遠(yuǎn)，甚至?xí)玫藉e(cuò)誤的位姿，并且還可能造成特征匹配失敗。慣性導(dǎo)航可以為特征匹配和優(yōu)化算法提供一個(gè)更好的初值，提升算法的速度、精度和動(dòng)態(tài)的適應(yīng)性。

2 模型的線性化

2.1 相關(guān)知識(shí)

由于旋轉(zhuǎn)矩陣R對(duì)加法不封閉，而是屬于李群空間的元素，所以引用李群和李代數(shù)的知識(shí)。這些理論都基于文獻(xiàn)[10-11]。記(·)∨為(·)∧的逆運(yùn)算，若S=ω∧記S∨=ω。對(duì)于(·)∧有如下規(guī)則：

a∧b=-b∧a?a,b∈R3

(17)

(18)

exp(φ∧)≈I+φ∧

(19)

對(duì)于屬于李群空間的R記log(·)為：

(20)

(21)

Log(R)=log(R)∨

(22)

Exp(φ+δφ)≈Exp(φ)Exp(Jrδφ)

(23)

Jr是李群空間的右乘雅克比矩陣

(24)

對(duì)Log運(yùn)算有如下近似

Log[Exp(φ)Exp(δφ)]≈φ+Jr-1δφ

(25)

在IMU積分線性化的過(guò)程中還會(huì)用到下邊一個(gè)很重要的公式

RExp(φ)RT=Exp(Rφ)

(26)

2.2 線性化

本文借鑒文獻(xiàn)[12]的預(yù)積分方法對(duì)模型進(jìn)行線性化。如圖3所示，因?yàn)閳D像和IMU的采集頻率不同，本文把每隔一定時(shí)間的圖像當(dāng)作關(guān)鍵位置，輸出其位姿信息。

圖3 視覺(jué)慣性導(dǎo)航模型的線性化Fig.3 Linearization of the visual-inertial navigation model

(27)

(28)

Pi表示IMU在圖像Ci幀時(shí)的位置，結(jié)合式(9)有

(29)

(30)

結(jié)合式(23)、式(24)有

(31)

結(jié)合式(26)有

(32)

其中

(33)

將式(32)代入式(27)并結(jié)合式(18)、式(6)有

(34)

對(duì)式(34)兩側(cè)做Log()運(yùn)算并結(jié)合式(25)有

Log(ER)=Jr(ER)Jbgbg

(35)

(36)

式(36)對(duì)bg、ba進(jìn)行線性化，結(jié)合式(17)、式(19)有

(37)

式(37)即為式(36)線性化的結(jié)果。

(38)

同理有

(39)

式(39)即為式(38)的線性化結(jié)果。

3 Kalman濾波估計(jì)

3.1 陀螺零偏的估計(jì)

(40)

(41)

圖4 Kalman濾波流程圖Fig.4 Flow chart of Kalman filter

3.2 加速度計(jì)零偏的估計(jì)

通過(guò)3.1節(jié)的估計(jì)，在ba的估計(jì)中，bg是已知的。代入式(37)中有

(42)

(43)

采用相鄰的三幀圖像數(shù)據(jù)，將絕對(duì)速度V消去。因?yàn)橐曈X(jué)無(wú)法提供準(zhǔn)確的速度信息，時(shí)刻k的絕對(duì)速度僅能通過(guò)加速度計(jì)積分得到，誤差會(huì)不斷的積累，消去絕對(duì)速度可以很好地提高估計(jì)精度。同時(shí)，在得到了IMU的零偏以后，根據(jù)式(29)可以求得Ci幀的速度Vi，結(jié)合式(28)得到Cj幀的速度Vi，從而通過(guò)短時(shí)間的慣性導(dǎo)航為視覺(jué)SLMA提供高精度的位姿預(yù)測(cè)。為了表達(dá)方便，用(·)ij表示Ci幀到Cj的相關(guān)數(shù)據(jù)，比如式(30)中的ΔR可以記為ΔRij。根據(jù)式(29)、式(39)有：

(44)

(45)

聯(lián)合式(44)、式(45)、式(36)、式(42)、式(43)消去Vi、Vj有

λi=βig+γiba

(46)

其中

由于重力加速度和加速度計(jì)零偏很難被區(qū)分出來(lái)，所以首先假設(shè)ba=0，有

λi=βig

(47)

這方程可以比較穩(wěn)定的解出g，將這個(gè)解作為一個(gè)初值記為G。并記G方向的單位向量Gn，世界坐標(biāo)系W的z軸方向單位向量Zn=(0，0，1)T。有

(48)

Rzg=Exp(θzgεn)

(49)

世界坐標(biāo)系中的重力加速度可以表示為

(50)

g=RzgExp(δθ)εnGR

(51)

式(46)可表示為

λi=βiRzgExp(δθ)εnGE+γiba

(52)

結(jié)合式(12)、式(14)有

λi=βiRzgεnGE-βiRzgεn∧GEδθ+γiba

(53)

記(αi1，αi2，αi3)=-βiRzgεn∧GE，有：

狀態(tài)觀測(cè)

(54)

測(cè)量矩陣

(55)

估計(jì)量

(56)

觀測(cè)方程為

Zk=HkXk+Vk

(57)

狀態(tài)轉(zhuǎn)移矩陣為I，觀測(cè)噪聲協(xié)方差為R。如圖4進(jìn)行Kalman濾波得到ba。

4 實(shí)驗(yàn)驗(yàn)證

基于ROS系統(tǒng)開(kāi)發(fā)環(huán)境[13]，實(shí)驗(yàn)數(shù)據(jù)采用Euroc Dataset發(fā)布的公開(kāi)數(shù)據(jù)集V2_02_mdium進(jìn)行驗(yàn)證。該數(shù)據(jù)集包含視頻錄像和同步采集到的IMU數(shù)據(jù)，同時(shí)給出了IMU與相機(jī)位置關(guān)系，滿足論文驗(yàn)證的前提條件。圖5、圖6所示為該數(shù)據(jù)集在該算法下的運(yùn)行結(jié)果。

圖5 視覺(jué)慣性SLAM生成的點(diǎn)云地圖Fig.5 Point cloud map of visual-inertial SLAM

圖6 視覺(jué)慣性SLAM提取的視覺(jué)特征Fig.6 Visual feature points of visual-inertial SLAM

圖7所示為陀螺零偏估計(jì)的結(jié)果，X軸為時(shí)間，Y軸為陀螺零偏，圖中3條曲線分別為零偏bg在IMU坐標(biāo)系下三軸的分量。其中，z軸的分量估計(jì)相對(duì)穩(wěn)定，x軸和y軸的跳動(dòng)較大，但最終收斂，這是因?yàn)樵撘曈X(jué)SLAM系統(tǒng)估計(jì)姿態(tài)時(shí)，姿態(tài)在x軸和y軸的誤差較大，從而引起零偏估計(jì)收斂較慢。這也正是用IMU陀螺修正視覺(jué)SLAM姿態(tài)的優(yōu)勢(shì)之一。

圖7 陀螺零偏估計(jì)結(jié)果Fig.7 Estimation results of gyro biases

圖8所示為加速度計(jì)零偏估計(jì)的結(jié)果，X軸為時(shí)間，Y軸為加速度計(jì)零偏，圖中3條曲線為零偏ba在IMU坐標(biāo)系下的分量。

圖8 加速度計(jì)零偏估計(jì)Fig.8 Estimation results of accelerometer biases

圖9所示為世界坐標(biāo)系中重力加速度在各軸的分量。X軸為時(shí)間，Y軸為重力加速度。

圖9 重力加速度分量Fig.9 Components of the gravity acceleration

圖10所示為估計(jì)IMU零偏后的誤差err1和未估計(jì)IMU零偏的誤差err0。該誤差表示在該時(shí)刻到前20幀圖像的時(shí)間內(nèi)IMU導(dǎo)航與視覺(jué)導(dǎo)航之間的誤差。X軸為時(shí)間，Y軸為積分誤差，估計(jì)零偏后的誤差是未估計(jì)零偏誤差的10.9%，說(shuō)明本文方法對(duì)IMU的積分精度有明顯的提高。

圖10 慣性導(dǎo)航誤差Fig.10 Error of inertial navigation

5 結(jié)論

本文通過(guò)合理地應(yīng)用IMU零偏模型，利用李代數(shù)和李群的知識(shí)將IMU零偏非線性模型線性化，與用視覺(jué)SLAM得到的位姿相結(jié)合，消除了速度積分的積累誤差，通過(guò)Kalman濾波得到陀螺零偏、加速度計(jì)零偏，結(jié)果穩(wěn)定可靠，對(duì)慣性導(dǎo)航精度有明顯的提高。

參考文獻(xiàn)

[1] Gui J, Gu D, Wang S, et al. A review of visual inertial odometry from filtering and optimisation perspectives[J]. Advanced Robotics, 2015, 29(20): 1289-1301.

[2] 逯建軍，任曉軍，孫偉，等.慣性/雙目視覺(jué)里程計(jì)深組合導(dǎo)航方法[J].導(dǎo)航定位與授時(shí)，2016，3(3)：6-12.

[3] Jones E S, Soatto S. Visual-inertial navigation, map-ping and localization: A scalable real-time causal approach[J]. The International Journal of Robotics Research, 2011, 30(4): 407-430.

[4] Patron-Perez A, Lovegrove S, Sibley G. A spline-based trajectory representation for sensor fusion and rolling shutter cameras[J]. International Journal of Computer Vision, 2015, 113(3): 208-219.

[5] Indelman V, Williams S, Kaess M, et al. Infor-mation fusion in navigation systems via factor graph based incremental smoothing[J]. Robotics and Autonomous Systems, 2013, 61(8): 721-738.

[6] Strasdat H, Montiel J M M, Davison A J. Real-time monocular SLAM: Why filter?[C]// 2010 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2010: 2657-2664.

[7] Nerurkar E D, Wu K J, Roumeliotis S I. C-KLAM: Constrained keyframe-based localization and mapping[C]// 2014 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2014: 3638-3643.

[8] Kümmerle R, Grisetti G, Strasdat H, et al. G2o: A general framework for graph optimization[C]// 2011 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2011: 3607-3613.

[9] Mur-Artal R, Tardos J D.ORB-SLAM2:An open-source SLAM system for monocular, stereo, and RGB-D cameras[J]. IEEE Transactions on Robotics, 2017, 33(5): 1255-1262.

[10] Chirikjian G S. Stochastic models, information theo-ry, and lie groups, Volume 2: Analytic methods and modern applications[M]. Springer Science & Busi-ness Media, 2011.

[11] Wang Y, Chirikjian G S. Nonparametric second-order theory of error propagation on motion groups[J]. The International Journal of Robotics Research, 2008, 27(11-12): 1258-1273.

[12] Forster C, Carlone L, Dellaert F, et al. IMU preintegration on manifold for efficient visual-inertial maximum-a-posteriori estimation[C]// Georgia Institute of Technology, 2015.

[13] 劉汝佳，王芳，張強(qiáng)，等.基于ROS的機(jī)械臂軌跡規(guī)劃研究[J].導(dǎo)航定位與授時(shí)，2016，3(6)：76-81.