一種單目視覺信息輔助慣性導航算法

王晨琳，劉海穎，蔣 鑫

(南京航空航天大學 航天學院，南京 210016)

0 引言

自主導航有著突出的環境適應能力，在各個領域有著廣闊的應用需求[1-2]。單一的慣性導航系統由于誤差積累的特性不利于長時間的自主導航，解決這一問題的根本辦法是采用高精度的慣性傳感器，但是昂貴的成本有限制了其實用性。視覺導航方法由于其自主性、廉價性成為導航策略領域的研究熱點，用視覺輔助慣性導航是一種很好的解決方案。

近年來，視覺輔助導航算法被應用于不同的場景。文獻[3]將圖像輔助導航用于飛行器著陸的情況做了可行性分析。文獻[4]針對航天器交會對接的情境，利用雙目視覺的技術，運用對偶四元數建立了2個航天器之間描述相對位姿參數的模型。文獻[5]提出了一種基于空間關系幾何約束的無人機景象匹配導航方法。在視覺信息和慣性傳感器信息融合的算法中，現階段普遍采用擴展卡爾曼濾波(extended Kalman filter，EKF)。文獻[6]基于EKF算法，提出了一種視覺輔助慣性測量單元(inertial measurement unit，IMU)的無人機導航方法。文獻[7]在飛行器在大海拔變化的場景下，研究了基于單目EKF的同步定位和制圖(simultaneous location and mapping，SLAM)性能。文獻[8]設計了一種多模態數據融合的EKF算法，用于結合視覺、激光、慣性導航和里程計的信息。根據視覺信息和慣性傳感器信息的耦合程度可以分為松耦合和緊耦合。文獻[9]針對融合視覺里程計和慣性導航系統的方案，設計了一種松耦合的間接反饋型卡爾曼濾波器。文獻[10]提出了一種緊耦合的EKF算法用于視覺慣性導航組合的相對位姿測量，并與松耦合的EKF算法進行了比較。

為了充分利用所有導航子系統的信息資源，提高組合導航的濾波效率，選取慣性傳感器的高采樣率作為濾波器的采樣周期，低采樣率的相機傳感器數據作為量測信息，在此基礎上提出了一種多信息多速率融合的無損卡爾曼濾波(unscented Kalman filter,UKF)算法。

1 多速率數據融合的設計方案

一般情況下，卡爾曼濾波器的狀態更新和量測更新在同步進行，這就需要在多傳感器的情況下進行數據同步，舍去高采樣率傳感器的大部分數據。這個過程中人為地不可避免地降低了高采樣率傳感器的性能，濾波周期變長，見圖1所示。

多傳感器數據采樣率不一致的情況如圖2所示，濾波器首先判斷相機傳感器是否有采集圖像，是否輸入了圖像數據并通過算法驗證圖像的質量。如果沒有圖像數據或者圖像質量差，濾波器只做狀態一步預測，將狀態估計值作為輸出。如果圖像數據滿足條件，則濾波器在空間上進行數據融合，當視覺數據的采樣周期不固定以及視覺數據的采樣點出現異常丟失時，濾波器也可以繼續運行，這樣提高了整個系統的冗余性。這種方案解決了組合導航系統中某一子系統測量失敗導致的濾波性能下降的問題。

多速率數據融合算法流程如圖3所示：首先，濾波器根據慣性傳感器輸入的加速度和角速度積分，獲得系統狀態的估計值；第二步是判定階段，濾波器判斷是否有圖像數據輸入，并且檢驗圖像質量。由尺度不變特征變換(scale-invariant feature transform，SIFT)算法獲取的特征點通過隨機抽樣一致算法(random sample consensus，RANSAC)[11]來進行匹配，通過匹配點的數量衡量圖像質量，一方面有效地剔除了誤匹配點，另一方面能夠控制特征點的數量；第三步，根據系統狀態的估計值獲得載體運動信息，構建三焦點張量，將匹配特征點通過三焦點張量進行轉移得到特征點位置的估計值；第四步，根據圖像中的特征點位置的實際測量值和估計值校正系統狀態量。

2 多速率數據融合的UKF算法

2.1 濾波器的狀態預測階段

(1)

式中：R為3×3的正交單位陣；T為三維平移向量；xp1，yp1，zp1為p點的位置坐標；xp2，yp2，zp2為p點經坐標系變換后的位置坐標。

根據三焦點張量的點轉移確定狀態向量，包含當前時刻t1載體運動狀態和前2個時刻t2、t3載體的位姿為

(2)

結合動力學方程得到連續系統的狀態方程為

(3)

式中：am、ωm分別表示加速度計和陀螺儀的測量值；ba和bω是加速度計和陀螺儀的零偏；na和nω是加速度計的陀螺儀高斯噪聲；g代表當地的重力加速度；符號?代表四元數乘法。

(4)

間接法中連續系統的狀態方程為

(5)

(6)

(7)

最后計算一步預測均方誤差方程為

Pk|k-1=FdPk-1FdT+Qd

(8)

2.2 濾波器的測量更新階段

單目相機提供的是以時間為標準的一連串圖像序列信息，提取同一特征點在不同的圖像中像素坐標作為觀測量。在2個視圖之間，相同的特征點通過基礎矩陣聯系起來，是一種極線約束。在三視圖的情況下，相同的特征點通過三焦點張量聯系起來，這是幾何約束從二視圖到三視圖的擴展。三焦點張量可以由慣性導航系統的位姿解算得到，將得到的三焦點張量來預測第三幅圖像中的特征點的像平面坐標,特征點轉移的過程見圖4所示。

首先確定3幅圖像所在時刻t1、t2、t3，通過IMU數據預測得到3個時刻載體的絕對位姿。通過坐標系轉化關系由IMU的絕對位姿轉化為相機坐標系的絕對位姿。可以得到包含相機外參數的旋轉矩陣R和平移向量T，從而得到本質矩陣

E=[T]XR

(9)

結合事先標定好的相機內參數矩陣K可以得到相機的基礎矩陣F為

E=KTFK

(10)

令兩兩視圖之間的基礎矩陣記為F12和F23，二視圖之間有如下對極幾何的約束關系為

(11)

第一步，計算垂直于x1的對極線Le2=F12x；

第二步，計算過點x2并垂直于Le2的直線L2

(12)

第三步，計算轉移點x3=[Γ1T,Γ2T,Γ3T]L2x1。

根據多視圖幾何約束得到的量測方程為

z(k)=[x1F12x2,x2F23x3,
[Γ1T,Γ2T,Γ3T]L2x1]T

(13)

最后計算濾波增益，更新系統狀態

(14)

3 實驗結果

本次實驗采用自駕車釆集的實測數據驗證算法的有效性。自駕車配備有全球定位系統(global positioning system，GPS)及IMU組合導航系統。IMU的型號為OXTS RT 3003,采樣率是100 Hz。其中，陀螺儀的量程為±300(°)/s，角速度偏差為2(°)/h；加速度計的量程為±300 m/s2，加速度偏差為0.02 m/s2。灰色攝像機的數據經過校正后的圖像分辨率為1 226個像素乘370個像素，圖像采樣率大致為10 Hz。

實驗數據采用MATLAB進行仿真，選取2組有代表性的數據，第一組場景是在市區馬路上，速度較慢。第二組場景選擇在郊區的公路上，速度較快。市區低速場景的結果如圖5和表1所示。

在市區低速場景下，單一的慣性導航系統誤差積累速度很快，35 s以后位置誤差達到了25 m。在加入視覺輔助信息后，將相機數據和慣性傳感器數據同步采樣率，位置誤差積累速度明顯降低。在35 s以后位置誤差僅有5.3 m，明顯優于單一的慣性導航系統，大大提高了定位精度。在單速率的情況下，由于現實場景中車輛顛簸，光照等客觀因素的影響，相機采樣率具有局限性和不確定性，無法達到慣性導航器件的高穩定和高采樣率的水平。所以，在2種數據的同步中必須舍去大部分的慣性傳感器的數據。并且如果相機出現故障，濾波器就無法工作，不利于系統的魯棒性。多速率數據融合的算法融合了所有傳感器的數據，將慣性傳感器采集的高速率數據全部用于定位解算，提高了數據的利用率。多速率數據融合算法在全程的位置誤差最大值為3.1 m，位置均方根誤差相比于單速率算法更低，位置估計的穩定性也更高。在速度估計方面，由于濾波器估計速度來源于慣性傳感器的信息，多速率算法的采樣率遠遠高于單速率，得到的速度精度也更高，穩定性也更好。

數據處理方法位置誤差最大值/m位置均方根誤差/m速度誤差最大值/(m·s-1)速度均方根誤差/(m·s-1)單一IMU數據24.879 611.180 2單速率數據融合5.329 73.934 20.776 30.342 6多速率數據融合3.126 71.481 90.376 20.180 2

郊區高速場景的實驗結果如圖6和表2所示。

數據處理方法位置誤差最大值/m位置均方根誤差/m速度誤差最大值/(m·s-1)速度均方根誤差/(m·s-1)單一IMU數據32.225 016.832 0單速率數據融合29.588 113.849 32.002 00.945 8多速率數據融合14.569 87.493 61.207 60.487 2

在郊區高速場景下，單一的慣性導航系統表現良好。因為在高速情況下，測量數據是高動態的，慣性傳感器的動態性能優越。單速率數據融合算法在位置和速度的解算結果上比單一的慣性導航系統效果略好，雖然一開始誤差比較大，但是后期行程中誤差呈下降趨勢，有效地抑制了誤差的擴散。多速率數據融合算法在位置和速度估計上明顯優于前面2種算法，充分利用了高速率的慣性傳感器的數據和視覺校正信息。

4 結束語

本文利用相機提供的視覺信息，提出了一種基于UKF的多速率數據融合算法，用于輔助慣性導航。該算法不需要同步所有傳感器的采樣率，一方面充分利用了所有傳感器數據，提高了信息利用率，另一方面在視覺輔助信息不足時濾波器能夠正常工作，提高了系統冗余性。通過采集的實測數據驗證，這種視覺信息輔助慣性導航方法有效地抑制了慣性器件帶來的定位誤差，提高了導航的定位精度。