王晨琳，劉海穎，蔣 鑫

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

0 引言

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

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

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

1 多速率數(shù)據(jù)融合的設(shè)計方案

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

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

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

2 多速率數(shù)據(jù)融合的UKF算法

2.1 濾波器的狀態(tài)預(yù)測階段

(1)

式中：R為3×3的正交單位陣；T為三維平移向量；xp1，yp1，zp1為p點(diǎn)的位置坐標(biāo)；xp2，yp2，zp2為p點(diǎn)經(jīng)坐標(biāo)系變換后的位置坐標(biāo)。

根據(jù)三焦點(diǎn)張量的點(diǎn)轉(zhuǎn)移確定狀態(tài)向量，包含當(dāng)前時刻t1載體運(yùn)動狀態(tài)和前2個時刻t2、t3載體的位姿為

(2)

結(jié)合動力學(xué)方程得到連續(xù)系統(tǒng)的狀態(tài)方程為

(3)

式中：am、ωm分別表示加速度計和陀螺儀的測量值；ba和bω是加速度計和陀螺儀的零偏；na和nω是加速度計的陀螺儀高斯噪聲；g代表當(dāng)?shù)氐闹亓铀俣?；符?代表四元數(shù)乘法。

(4)

間接法中連續(xù)系統(tǒng)的狀態(tài)方程為

(5)

(6)

(7)

最后計算一步預(yù)測均方誤差方程為

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

(8)

2.2 濾波器的測量更新階段

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

首先確定3幅圖像所在時刻t1、t2、t3，通過IMU數(shù)據(jù)預(yù)測得到3個時刻載體的絕對位姿。通過坐標(biāo)系轉(zhuǎn)化關(guān)系由IMU的絕對位姿轉(zhuǎn)化為相機(jī)坐標(biāo)系的絕對位姿?？梢缘玫桨鄼C(jī)外參數(shù)的旋轉(zhuǎn)矩陣R和平移向量T，從而得到本質(zhì)矩陣

E=[T]XR

(9)

結(jié)合事先標(biāo)定好的相機(jī)內(nèi)參數(shù)矩陣K可以得到相機(jī)的基礎(chǔ)矩陣F為

E=KTFK

(10)

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

(11)

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

第二步，計算過點(diǎn)x2并垂直于Le2的直線L2

(12)

第三步，計算轉(zhuǎn)移點(diǎn)x3=[Γ1T,Γ2T,Γ3T]L2x1。

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

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

(13)

最后計算濾波增益，更新系統(tǒng)狀態(tài)

(14)

3 實(shí)驗(yàn)結(jié)果

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

實(shí)驗(yàn)數(shù)據(jù)采用MATLAB進(jìn)行仿真，選取2組有代表性的數(shù)據(jù)，第一組場景是在市區(qū)馬路上，速度較慢。第二組場景選擇在郊區(qū)的公路上，速度較快。市區(qū)低速場景的結(jié)果如圖5和表1所示。

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

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

郊區(qū)高速場景的實(shí)驗(yàn)結(jié)果如圖6和表2所示。

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

在郊區(qū)高速場景下，單一的慣性導(dǎo)航系統(tǒng)表現(xiàn)良好。因?yàn)樵诟咚偾闆r下，測量數(shù)據(jù)是高動態(tài)的，慣性傳感器的動態(tài)性能優(yōu)越。單速率數(shù)據(jù)融合算法在位置和速度的解算結(jié)果上比單一的慣性導(dǎo)航系統(tǒng)效果略好，雖然一開始誤差比較大，但是后期行程中誤差呈下降趨勢，有效地抑制了誤差的擴(kuò)散。多速率數(shù)據(jù)融合算法在位置和速度估計上明顯優(yōu)于前面2種算法，充分利用了高速率的慣性傳感器的數(shù)據(jù)和視覺校正信息。

4 結(jié)束語

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