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

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

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

0 引言

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

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

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

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

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

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

1.1 坐標系定義及變量表示方法

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

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

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

1.2 激光測距量測更新方法

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

(1)

其中，為慣導系統狀態變量

(2)

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

為視覺里程計系統狀態變量

(3)

包括連續幀內世界坐標系下相機的位置、姿態以及特征點位置坐標。在文獻[17]中，已對慣導狀態更新方法與視覺里程計狀態更新方法進行了詳細介紹，本節則主要分析激光測距傳感器測量值與視覺里程計狀態之間的聯系，并給出量測更新方法。

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

(4)

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

(5)

(6)

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

2δ2+3δ3+

(7)

式中，為與各狀態量相關的量測矩陣,為雅可比矩陣形式。

1.3 量測矩陣詳細推導

為了方便推導，將式(6)中的復雜項做如下標記

(8)

已知激光測距測量值與慣導狀態無關聯，所以系統量測矩陣中非零變量只需要對視覺狀態量求偏導

(9)

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

(10)

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

(11)

3×(3+3(-))]

(12)

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

2 系統可觀測性分析

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

2.1 傳統組合模型可觀測性分析

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

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

(13)

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

′=′′

(14)

則

′=(--·δ-·+)

(15)

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

(16)

因此

′=(--·δ-·+)

=(-)

=′·=0

(17)

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

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

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

=

(18)

式中，為13節中推導的系統量測矩陣;為系統狀態轉移矩陣。如1.2節中所述，由空間中3個特征點確定當前地面，所以可將根據變量類型分解為各子觀測矩陣，由式(7)中各子量測矩陣可得

=[|||1|2|3]

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

(19)

式中，狀態轉移矩陣的詳細表達式在文獻[17]中已有明確定義，因此，聯立式(6)、式(8)各子觀測矩陣的詳細表達式如下

,=

(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，證明了本文提出的慣性/激光測距/視覺里程計導航系統在無人機平飛機動狀態下尺度誤差可觀測，對系統其他狀態變量的可觀測性無嚴重影響。

3 機載試驗驗證

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

(a) 大疆M600-Pro無人機

(b) 搭載視覺導航樣機

(c) 樣機安裝方式

(d) 飛行試驗示意

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

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

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

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

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

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

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

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

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

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

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

(b) 本組數據尺度誤差估計結果

(c) 三方向位置誤差

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

表1 飛行試驗結果統計

4 結論

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

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

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