基于慣性/激光測距/視覺里程計組合的高空場景尺度誤差估計方法

王大元, 李 濤, 莊廣琛, 李 至

(1.北京自動化控制設(shè)備研究所，北京 100074;2.空軍裝備部駐北京地區(qū)第二軍事代表室，北京 100037)

0 引言

隨著現(xiàn)代戰(zhàn)場環(huán)境的復(fù)雜化，無人機對導(dǎo)航系統(tǒng)的自主性要求更加嚴(yán)苛，衛(wèi)星拒止條件下單純依靠慣導(dǎo)系統(tǒng)無法滿足無人機的導(dǎo)航精度需求。視覺導(dǎo)航憑借與慣性導(dǎo)航的互補性，與慣導(dǎo)融合成為了一種可靠的自主導(dǎo)航手段，在小型旋翼無人機、地面無人車等領(lǐng)域已得到廣泛應(yīng)用；但對于面向高空場景的大型無人機，仍存在尺度誤差難以精確估計的問題，即視覺里程計算出的相對位置、速度信息缺乏絕對量綱，與真值之間具有較大尺度誤差，無法為慣導(dǎo)系統(tǒng)提供精確量測信息。

對此，國內(nèi)外開展了面向無人機領(lǐng)域的慣性/視覺里程計組合導(dǎo)航方法研究。2016年，蘇黎世大學(xué)的M.Faessler等將輕量級半直接法視覺里程計(Semidirect Visual Odometry，SVO)與多傳感器融合框架(Multi-Sensor Fusion，MSF)進行組合，在具有重復(fù)和高頻紋理的場景中提高了魯棒性。2016年，Yang Z.等提出了一種不依賴人工標(biāo)記與有關(guān)機械的先驗知識的初始化速度、重力方向、視覺尺度和相機-慣性測量單元(Inertial Measurement Unit，IMU)動態(tài)外部校準(zhǔn)方法。C.Campos 等提出了一種基于最大后驗估計的尺度估計方法，先通過純視覺估計出慣性器件誤差，再利用慣性視覺組合估計出尺度誤差初值，在低空場景獲得了良好表現(xiàn)。2018年，香港科技大學(xué)沈劭劼等提出了VINS-Mono，該系統(tǒng)提出了一種在線校準(zhǔn)視覺和慣性測量之間時間偏移的方法，通過建立IMU和相機觀測數(shù)據(jù)的目標(biāo)函數(shù)，最小化聯(lián)合誤差的函數(shù)，由此獲得高精度的實時定位。2021年，翟海慶等利用超聲波傳感器通過最小二乘法完成尺度估計值的獲取，并通過擴展卡爾曼濾波(Extended Kalman Filter，EKF)實現(xiàn)了單目視覺與慣導(dǎo)的信息融合。另外，還有一部分學(xué)者采用雙目相機方案，通過基線長度為視覺導(dǎo)航系統(tǒng)提供尺度基準(zhǔn)，但是由于相機基線較短，該方法并不適用于高空場景。對于機器人、無人車等地面設(shè)備，大多采用相機前視安裝的方式，通過相機與地面間的高度為系統(tǒng)提供精確尺度基準(zhǔn)，該方法同樣無法應(yīng)用于大型無人機。

因此，針對上述情況，本文建立了適應(yīng)于高空場景的激光測距/視覺里程計誤差更新模型，通過因子圖優(yōu)化方法對三種傳感器測量值進行組合，并詳細(xì)分析了尺度誤差在無人機平飛運動狀態(tài)下的可觀測性，最后通過機載仿真試驗對本文算法在300m高空場景下進行了精度驗證。

1 慣性/激光測距/視覺里程計

本文提出的慣性/激光測距/視覺里程計導(dǎo)航方法前端采用特征點法視覺里程計，通過連續(xù)圖像間匹配的特征點計算無人機相對位置變化。系統(tǒng)框架如圖1所示，藍(lán)色矩形框為慣性、相機原始傳感器，橙色矩形框為本文引入的激光測距傳感器，為系統(tǒng)提供對地高度測量值，三種傳感器數(shù)據(jù)經(jīng)過BA(Bundle Adjustment)優(yōu)化融合，對慣性/激光測距/視覺里程計組合導(dǎo)航系統(tǒng)尺度誤差進行精確估計(紅色矩形框)。

圖1 慣性/激光測距/視覺里程計系統(tǒng)框架Fig.1 Framework of inertial/laser ranging/visual odometry system

1.1 坐標(biāo)系定義及變量表示方法

世界坐標(biāo)系(系)：坐標(biāo)系原點取為視覺里程計初始化成功第一幀(0,0,0);軸指向初始化時刻攝像機縱側(cè)方向，向右為正;指向初始化時刻攝像機豎軸方向，向下為正;指向初始化時刻攝像機縱軸方向，向前為正。

相機坐標(biāo)系(系)：坐標(biāo)系原點取為相機質(zhì)心;軸指向攝像機側(cè)軸方向，向右為正;指向攝像機豎軸方向，向下為正;指向攝像機縱軸方向，向前為正。

像素坐標(biāo)系(系)：原點位于圖像左上角;軸指向圖像水平方向，向右為正;軸指向圖像垂直方向，向下為正，為二維坐標(biāo)系。

1.2 激光測距量測更新方法

本文提出的慣性/激光測距/視覺組合狀態(tài)向量為

(1)

其中，為慣導(dǎo)系統(tǒng)狀態(tài)變量

(2)

包括位置、速度、姿態(tài)、陀螺漂移以及加速度計零偏。

為視覺里程計系統(tǒng)狀態(tài)變量

(3)

包括連續(xù)幀內(nèi)世界坐標(biāo)系下相機的位置、姿態(tài)以及特征點位置坐標(biāo)。在文獻(xiàn)[17]中，已對慣導(dǎo)狀態(tài)更新方法與視覺里程計狀態(tài)更新方法進行了詳細(xì)介紹，本節(jié)則主要分析激光測距傳感器測量值與視覺里程計狀態(tài)之間的聯(lián)系，并給出量測更新方法。

如圖2所示，相機在運動過程中采集連續(xù)地面圖像，激光測距機獲取當(dāng)前時刻無人機相對地面高度，由于面向的是高空場景，因此忽略相機光心與激光測距光心之間距離造成的誤差，默認(rèn)二者位于同一直線上。當(dāng)前地點所在的平面最少可由3個特征點確定，因此選取激光點周圍的3個特征點構(gòu)建平面，并求取其法向量

(4)

圖2 目標(biāo)點相對激光測距幾何關(guān)系示意圖Fig.2 Schematic diagram of the geometric relationship between the target point and the laser ranging

(5)

(6)

因此，激光測距測量值的誤差方程可以表示為

2δ2+3δ3+

(7)

式中，為與各狀態(tài)量相關(guān)的量測矩陣,為雅可比矩陣形式。

1.3 量測矩陣詳細(xì)推導(dǎo)

為了方便推導(dǎo)，將式(6)中的復(fù)雜項做如下標(biāo)記

(8)

已知激光測距測量值與慣導(dǎo)狀態(tài)無關(guān)聯(lián)，所以系統(tǒng)量測矩陣中非零變量只需要對視覺狀態(tài)量求偏導(dǎo)

(9)

因此，對于矩陣的計算便轉(zhuǎn)化為求解??、??與??的過程。

(10)

3×(15+6+3(--1))||3×3(-)]

(11)

3×(3+3(-))]

(12)

由此，可以得到式(7)中所有的子量測矩陣。

2 系統(tǒng)可觀測性分析

組合導(dǎo)航系統(tǒng)各狀態(tài)變量的可觀測性與無人機機動狀態(tài)密切相關(guān)，慣性/視覺里程計組合導(dǎo)航與傳統(tǒng)慣性/輪式里程計組合導(dǎo)航方法相比，主要不同點在于高空場景下視覺里程計存在大尺度誤差，影響系統(tǒng)可觀測性,而其他狀態(tài)變量在不同機動下的可觀測性在多篇文獻(xiàn)中已有明確分析結(jié)論，因此，本文只針對性討論尺度誤差對其他狀態(tài)量的影響。

2.1 傳統(tǒng)組合模型可觀測性分析

無人機飛行時垂直方向上的機動可為慣性器件提供激勵，使尺度誤差與慣性器件誤差解耦，提高可觀測性。而在大多數(shù)的巡航階段，無人機需要保持水平姿態(tài)巡航，這種平飛機動狀態(tài)下尺度誤差的可觀測性降低。本文針對這種情況對所提出的慣性/激光測距/視覺里程計系統(tǒng)可觀測性進行分析，即分析尺度誤差對本文式(1)中各狀態(tài)變量的影響。

尺度誤差的表現(xiàn)形式為無人機飛行軌跡的縮放，因此只影響到位置、速度以及加速度，對無人機的姿態(tài)變化無影響。因此,根據(jù)式(1)構(gòu)建無人機在空中平飛時由尺度誤差為主要影響因素的觀測向量

(13)

由于尺度誤差對姿態(tài)無影響，則式(13)中姿態(tài)角以及陀螺漂移所對應(yīng)項為零向量。驗證在此機動狀態(tài)下的系統(tǒng)可觀測性，即驗證是否屬于系統(tǒng)可觀測性矩陣的右非零空間，即·=0。傳統(tǒng)模型中的慣性/視覺組合模型為文獻(xiàn)[17]中式(1)定義，其狀態(tài)轉(zhuǎn)移矩陣′由文獻(xiàn)[17]中式(18)定義，系統(tǒng)量測矩陣′由文獻(xiàn)[17]中式(27)定義。則傳統(tǒng)組合模型的可觀測性矩陣′為

′=′′

(14)

則

′=(--·δ-·+)

(15)

式中，、具體形式由文獻(xiàn)[17]中式(47)、式(50)定義，其中

(16)

因此

′=(--·δ-·+)

=(-)

=′·=0

(17)

即證明，屬于系統(tǒng)可觀測性矩陣右非零空間，亦證明無人機空中平飛機動狀態(tài)下尺度誤差不可觀測。

2.2 本文組合模型可觀測性分析

本文提出的慣性/激光測距/視覺里程計系統(tǒng)可觀測性矩陣表示為

=

(18)

式中，為13節(jié)中推導(dǎo)的系統(tǒng)量測矩陣;為系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣。如1.2節(jié)中所述，由空間中3個特征點確定當(dāng)前地面，所以可將根據(jù)變量類型分解為各子觀測矩陣，由式(7)中各子量測矩陣可得

=[|||1|2|3]

=[,|,|,|,|,|,1|,2|,3]

(19)

式中，狀態(tài)轉(zhuǎn)移矩陣的詳細(xì)表達(dá)式在文獻(xiàn)[17]中已有明確定義，因此，聯(lián)立式(6)、式(8)各子觀測矩陣的詳細(xì)表達(dá)式如下

,=

(20)

,=

(21)

(22)

(23)

(24)

(25)

同樣證明式(13)由尺度誤差為主要影響因素的觀測向量不屬于的右非零空間，即證明·≠0。

·=,·+,·+,·+

,1·1+,2·2+,3·3

([(3-2)×]T2+[(1-

3)×]T2+[(2-1)×]T3)

[(3-2+1-3+2-

(26)

已知地面特征點與相機位置不在同一平面，因此·≠0，證明了本文提出的慣性/激光測距/視覺里程計導(dǎo)航系統(tǒng)在無人機平飛機動狀態(tài)下尺度誤差可觀測，對系統(tǒng)其他狀態(tài)變量的可觀測性無嚴(yán)重影響。

3 機載試驗驗證

為了驗證本文算法的有效性，對算法進行了機載試驗驗證，試驗載機、試驗設(shè)備及其安裝方式如圖3所示。

(a) 大疆M600-Pro無人機

(b) 搭載視覺導(dǎo)航樣機

(c) 樣機安裝方式

(d) 飛行試驗示意

其中，圖3(a)所示大疆M600-Pro無人機為本次試驗載機；圖3(b)所示為參試視覺導(dǎo)航樣機，圖中序號1、2、3分別為可見光相機、1km激光測距儀和sbg微慣導(dǎo)；圖3(c)與圖3(d)給出了機上安裝方式與試驗飛行過程展示。本次試驗地點為北京市六環(huán)外室外區(qū)域，地面場景為樹林及公路等特征較為豐富區(qū)域，本次試驗飛行高度300m。慣性/激光測距/視覺里程計組合導(dǎo)航系統(tǒng)在空中初始化成功后進行平飛，飛行軌跡為一段正方形軌跡。

經(jīng)過仿真驗證，在高空場景下傳統(tǒng)的慣性/視覺里程計算法在缺少縱軸方向上的運動激勵時，慣性器件偏差將與視覺里程計尺度誤差嚴(yán)重耦合，無法完成初始化。圖4給出了目前導(dǎo)航精度、魯棒性均較為優(yōu)秀的orb-slam3算法對機載數(shù)據(jù)的純視覺仿真結(jié)果。

圖4 orb-slam3算法純視覺仿真結(jié)果Fig.4 Pure vision simulation results of orb-slam3 algorithm

從圖4可以看出，紫色純視覺里程計導(dǎo)航軌跡具有明顯的尺度誤差，無法為無人機提供穩(wěn)定的導(dǎo)航信息，而通過orb-slam3中的慣性/視覺組合導(dǎo)航方法對機載數(shù)據(jù)進行仿真，結(jié)果如圖5所示。

圖5 orb-slam3慣性/視覺組合初始化位姿漂移Fig.5 Initialization pose drift of orb-slam3 inertial/visual combination

由圖5可以看出，在箭頭標(biāo)記處由于高空場景下地面特征平面化，相機無法計算相對地面深度，因此系統(tǒng)無法準(zhǔn)確對尺度誤差與加表零位解耦，導(dǎo)致系統(tǒng)初始化失敗發(fā)生位姿漂移。通過對多組不同飛行高度的機載數(shù)據(jù)進行仿真，orb-slam3算法均初始化失敗，驗證了此條件下無法完成穩(wěn)定導(dǎo)航。

而本文提出的慣性/激光測距/視覺里程計算法可以實現(xiàn)尺度誤差的精確估計，程序運行界面如圖6所示。

圖6 300m高度飛行數(shù)據(jù)程序運行界面Fig.6 Program operation interface of 300m altitude flight data

圖7給出了本組試驗飛行軌跡及誤差對比結(jié)果，從圖7(b)可以看出，通過激光測距提供尺度基準(zhǔn)，尺度誤差快速收斂。

圖7(a)中紫色軌跡為本文所提算法仿真結(jié)果，綠色軌跡為差分衛(wèi)星基準(zhǔn)，可以看出，在初始時刻系統(tǒng)即成功完成了初始化，并穩(wěn)定完成了后續(xù)階段的導(dǎo)航。本組數(shù)據(jù)全程飛行距離1.34km，最大位置誤差處為14.7m，定位精度為1.09%，本次機載共進行了4次飛行試驗，試驗結(jié)果如表1所示。

(a) 機載試驗飛行軌跡對比

(b) 本組數(shù)據(jù)尺度誤差估計結(jié)果

(c) 三方向位置誤差

(d) 徑向位置誤差圖7 機載試驗軌跡及誤差對比Fig.7 Track and error comparison of airborne experiment

表1 飛行試驗結(jié)果統(tǒng)計

4 結(jié)論

1)本文建立了慣性/激光測距/視覺里程計組合導(dǎo)航模型，并詳細(xì)推導(dǎo)了激光測距量測更新矩陣。通過激光測距儀測量的對地高度提供尺度基準(zhǔn)。

2)對本文提出的組合導(dǎo)航模型在無人機平飛機動狀態(tài)下可觀測性進行分析，驗證了通過引入激光測距實現(xiàn)慣性器件誤差與視覺里程計尺度誤差解耦的可行性。

3)通過實際機載試驗數(shù)據(jù)，驗證了在300m高度無人機平飛狀態(tài)下，組合導(dǎo)航系統(tǒng)初始化與后續(xù)導(dǎo)航精度。經(jīng)過仿真分析，傳統(tǒng)方法無法成功初始化，本文所提算法在機載試驗中最遠(yuǎn)飛行距離2.31km，最大位置誤差28.01m，定位精度1.21%，四組試驗統(tǒng)計精度優(yōu)于1.5%，達(dá)到地面慣性/視覺里程計定位精度，驗證了算法的有效性。