一種適用于高動態(tài)強(qiáng)干擾環(huán)境的視覺輔助微機(jī)械捷聯(lián)慣性導(dǎo)航系統(tǒng)/全球定位系統(tǒng)超緊組合導(dǎo)航系統(tǒng)

李群生， 趙剡， 王進(jìn)達(dá)

(1.北京航空航天大學(xué) 儀器科學(xué)與光電工程學(xué)院， 北京 100191； 2.中國空空導(dǎo)彈研究院， 河南 洛陽 471009)

0 引言

使用低成本微機(jī)械(MEMS)傳感器的捷聯(lián)慣性導(dǎo)航系統(tǒng)(SINS)/與全球定位系統(tǒng)(GPS)超緊組合導(dǎo)航系統(tǒng)并不能解決在惡劣GPS環(huán)境下的信號跟蹤問題。而視覺傳感器輸出的誤差不會隨時(shí)間積累，基于互補(bǔ)金屬氧化物半導(dǎo)體(CMOS)的數(shù)字成像技術(shù)作為一種被動觀測方法具有良好的隱蔽性，且景物視覺特征不易受到人為干擾。因此，視覺成為輔助MEMS-SINS/GPS超緊組合系統(tǒng)的理想選擇。

國外對視覺輔助的SINS/GPS組合導(dǎo)航技術(shù)已經(jīng)開展了一些研究。早期視覺導(dǎo)航解決方案是面向自主地面機(jī)器人研發(fā)的。近年來，視覺導(dǎo)航系統(tǒng)在無人飛行器(UAV)、深空探測器和水下機(jī)器人領(lǐng)域得到廣泛應(yīng)用，并進(jìn)一步刺激了基于視覺的組合導(dǎo)航算法研究[1-2]。歐美等發(fā)達(dá)國家較早地開展了視覺提取導(dǎo)航信息的技術(shù)研究。美國加州大學(xué)伯克力分校[3]將視覺導(dǎo)航應(yīng)用到無人直升機(jī)自主著陸中，并給出了實(shí)際的飛行實(shí)驗(yàn)結(jié)果；飛行實(shí)驗(yàn)結(jié)果表明該視覺導(dǎo)航系統(tǒng)可以使無人直升機(jī)著陸的軸向定位精度達(dá)到5 cm. 佛羅里達(dá)大學(xué)[4]研究了一套通過圖像獲得飛行器滾轉(zhuǎn)角和俯仰角的方法。上述研究都是以視覺導(dǎo)航信息為主，并未涉及多傳感器的信息融合，然而這些圖像處理、視覺信息提取方面的研究是值得借鑒的。在使用視覺輔助SINS/GPS組合導(dǎo)航方面，Wu等[5]采用標(biāo)準(zhǔn)擴(kuò)展卡爾曼濾波算法將圖像中得到的無人機(jī)相對于目標(biāo)距離和方向與GPS和慣性導(dǎo)航系統(tǒng)(INS)融合，得到了較高精度的導(dǎo)航結(jié)果。Barreau等[6]對視覺輔助下的INS狀態(tài)模型和量測模型進(jìn)行了細(xì)致分析，研究了視覺/INS輔助的基于卡爾曼濾波器的全球衛(wèi)星導(dǎo)航系統(tǒng)(GNSS)信號跟蹤方法。Benoit等[7]在之前的基礎(chǔ)上進(jìn)一步開展了視覺輔助的GNSS/INS超緊組合系統(tǒng)的性能評價(jià)工作，使用一個(gè)單目相機(jī)，通過匹配地標(biāo)點(diǎn)獲取載體位置信息，對慣性測量單元(IMU)的誤差進(jìn)行標(biāo)定。仿真結(jié)果表明，使用視覺輔助可以有效地抑制慣性元件參數(shù)的漂移，將定位精度從幾百米降低到10 m左右。

國內(nèi)對這方面的研究相對較少。李榮冰等[8]提出利用視覺/GPS/MEMS-SINS組合導(dǎo)航系統(tǒng)確定微型飛行器姿態(tài)的方案，增加視覺姿態(tài)后，濾波器中INS平臺誤差角的可觀測性得到了增強(qiáng)，姿態(tài)估計(jì)精度也得到了提高。李旭等[9]提出INS/GPS/視覺/電子地圖融合組合方式，該組合導(dǎo)航系統(tǒng)能為智能車輛提供其空間位置、速度、加速度與姿態(tài)角等眾多導(dǎo)航信息，并且當(dāng)GPS較長時(shí)間中斷時(shí)，通過SINS/視覺/數(shù)字地圖的組合仍然能夠?yàn)橹悄苘囕v提供可靠的導(dǎo)航數(shù)據(jù)。姬張建等[10]研究了差分全球定位系統(tǒng)(DGPS)/INS/視覺組合導(dǎo)航算法，采用INS精度相對較高，GPS采用DGPS，并使用了實(shí)現(xiàn)預(yù)知的電子地圖加以輔助，取得了良好的導(dǎo)航效果。邵瑋等[11]提出了基于視覺/GPS/MEMS-SINS的微小型無人機(jī)(MUAV)數(shù)據(jù)融合導(dǎo)航方案，采用位置與速度組合方式，仿真驗(yàn)證結(jié)果表明該組合導(dǎo)航方案具有較高的導(dǎo)航精度，能夠保證導(dǎo)航系統(tǒng)的穩(wěn)定性。宋琳娜等[12]研究了利用計(jì)算機(jī)視覺和SINS進(jìn)行組合導(dǎo)航的算法，實(shí)現(xiàn)了視覺導(dǎo)航圖像信息與SINS的信息融合。文獻(xiàn)[13] 提出了一種基于對偶4元數(shù)的慣性/衛(wèi)星/天文組合導(dǎo)航系統(tǒng)改進(jìn)聯(lián)邦濾波方法,通過可觀測性分析對子濾波器進(jìn)行降維,能在保證導(dǎo)航精度的同時(shí)提高系統(tǒng)實(shí)時(shí)性。文獻(xiàn)[14]針對接收機(jī)在強(qiáng)干擾高動態(tài)環(huán)境難以定位導(dǎo)航的問題,提出基于慣性導(dǎo)航速度輔助衛(wèi)星跟蹤環(huán)路算法。文獻(xiàn)[15]提出一種高動態(tài)情況下INS/視覺組合導(dǎo)航中視覺系統(tǒng)的快速定位方法。文獻(xiàn)[16]提出高動態(tài)和強(qiáng)干擾環(huán)境下新型超緊組合GPS/INS導(dǎo)航算法。文獻(xiàn)[17]系統(tǒng)研究了基于矢量跟蹤的高動態(tài)載體超緊組合導(dǎo)航技術(shù)。

綜合以上研究可以看出，目前視覺輔助下的導(dǎo)航技術(shù)較為單一[18-20]，并且沒有涉及SINS/GPS超緊組合，針對高動態(tài)、強(qiáng)干擾以及惡劣GPS環(huán)境的容錯性并不強(qiáng)。因此，本文提出一種適用于高動態(tài)強(qiáng)干擾環(huán)境的視覺輔助MEMS-SINS/GPS超緊組合導(dǎo)航系統(tǒng)，通過將雙目視覺提供的姿態(tài)信息引入MEMS-SINS/GPS超緊組合系統(tǒng)中，提高了平臺失準(zhǔn)角的可觀測性。推導(dǎo)了系統(tǒng)狀態(tài)方程和量測方程，提高了系統(tǒng)在高動態(tài)強(qiáng)干擾環(huán)境中的可靠性，有效解決了低空飛行器在GPS信號被遮擋或干擾情況下的導(dǎo)航問題。

1 系統(tǒng)設(shè)計(jì)

1.1 視覺導(dǎo)航原理和時(shí)空對準(zhǔn)

視覺導(dǎo)航和空間對準(zhǔn)方法詳見文獻(xiàn)[21]，文獻(xiàn)[22]給出了低動態(tài)、中動態(tài)和高動態(tài)情況下的硬件或軟件同步方法。本文采用的時(shí)間對準(zhǔn)采用文獻(xiàn)[23]的方法。

1.2 系統(tǒng)狀態(tài)方程

由于雙目視覺導(dǎo)航信息以位置和姿態(tài)的形式輸出到信息融合模塊，為了便于系統(tǒng)建模和提高濾波效率，采用傳統(tǒng)超緊組合系統(tǒng)的狀態(tài)方程作為視覺輔助GPS/SINS超緊組合系統(tǒng)的狀態(tài)方程。

坐標(biāo)系定義如下：i為地心慣性系，t為當(dāng)?shù)氐乩碜鴺?biāo)系，e為地心地固坐標(biāo)系，p為慣性導(dǎo)航平臺坐標(biāo)系，b為載體坐標(biāo)系，c為大地坐標(biāo)系。

視覺輔助GPS/SINS超緊組合系統(tǒng)的狀態(tài)方程為

(1)

1.3 系統(tǒng)觀測方程

將SINS和GPS的偽距、偽距率之差、SINS的平臺失準(zhǔn)角、SINS與視覺的位置和速度誤差作為視覺輔助GPS/SINS超緊組合系統(tǒng)的觀測量。

1.3.1 偽距觀測方程

偽距觀測方程為

Zρ=HρX+Vρ，

(2)

式中：Zρ為偽距觀測量；Hρ為觀測矩陣；X為系統(tǒng)狀態(tài)變量；Vρ為觀測噪聲；

ρI為慣性導(dǎo)航計(jì)算的偽距，ρG為接收機(jī)測量的偽距，Li(i=1,2,3,4)為第i顆衛(wèi)星的方向余弦矢量，R為地球半徑，h為海拔高，λ為緯度，L為經(jīng)度。

1.3.2 偽距率觀測方程

偽距率觀測方程為

(3)

1.3.3 平臺失準(zhǔn)角觀測方程

(4)

式中：φx、φy、φz分別為平臺失準(zhǔn)角的3個(gè)分量。利用(4)式將姿態(tài)誤差角轉(zhuǎn)換為平臺失準(zhǔn)角誤差，并將平臺失準(zhǔn)角誤差作為觀測值，可以得到：

1)平臺失準(zhǔn)角誤差的觀測方程為

Za=HaX+Va，

(5)

2)位置觀測方程為

(6)

3)速度觀測方程為

(7)

1.4 信息融合方案

如圖1所示為組合導(dǎo)航系統(tǒng)的信息融合方式，系統(tǒng)采用級聯(lián)濾波結(jié)構(gòu)，其中aV為視覺系統(tǒng)得到的加速度。圖1中：S1為控制開關(guān)；子濾波器1為雙目視覺輔助GPS/SINS超緊組合卡爾曼濾波器；子濾波器2為雙目視覺與SINS的組合導(dǎo)航濾波器；主導(dǎo)航估計(jì)器采用模糊控制算法，與傳統(tǒng)聯(lián)邦濾波相比，不需要進(jìn)行大量矩陣求逆計(jì)算，計(jì)算量大大降低，更適合在彈載計(jì)算機(jī)上實(shí)時(shí)計(jì)算。

圖1 組合導(dǎo)航系統(tǒng)信息融合結(jié)構(gòu)圖Fig.1 Information fusion architecture of integrated navigation system

由于各個(gè)子系統(tǒng)的輸出頻率不同，在進(jìn)行信息融合時(shí)需要進(jìn)行時(shí)序?qū)?zhǔn)。MEMS-SINS系統(tǒng)的更新頻率為1 kHz，GPS信息輸出頻率為100 Hz，而視覺模塊的量測信息輸出頻率為10 Hz. 子濾波器1和子濾波器2的狀態(tài)方程相同，而觀測方程不同，因此其觀測信息的更新周期不同。

對于子濾波器1，既有來自GPS的觀測信息，也有來自慣性導(dǎo)航的信息。對于整10 ms且非整100 ms的時(shí)刻，系統(tǒng)觀測方程與超緊組合系統(tǒng)相同，即

(8)

而對于整10 ms且整100 ms的時(shí)刻，系統(tǒng)量測量既有GPS信息也有視覺信息，觀測方程為

Z1=H1X+V1，

(9)

式中：ZG為來自GPS的觀測量。

當(dāng)GPS信號中斷時(shí)，系統(tǒng)轉(zhuǎn)換為以子濾波器2進(jìn)行濾波，其觀測方程為

Z2=H2X+V2，

(10)

主導(dǎo)航估計(jì)器利用模糊控制方法融合兩個(gè)子濾波器的導(dǎo)航結(jié)果，并根據(jù)觀測信息判斷能否正常跟蹤GPS，進(jìn)而通過控制開關(guān)S1選擇使用哪個(gè)子濾波器進(jìn)行濾波。

設(shè)置的鎖相環(huán)(PLL)鎖定檢測器用來檢測載波相位的跟蹤狀態(tài)，該檢測器輸出值LD是載波相位跟蹤誤差的函數(shù)，可表示為

(11)

(12)

X=μX1+(1-μ)X2.

(13)

式中：X1和X2分別為子濾波器1和子濾波器2的輸出；根據(jù)經(jīng)驗(yàn)，Tdl取0.6，Tdu取0.7.由于系統(tǒng)選擇的狀態(tài)量為系統(tǒng)的各項(xiàng)誤差，不能直接輸出。因此，估計(jì)出狀態(tài)量后需要對導(dǎo)航信息進(jìn)行校正。通常的校正方式有輸出校正和反饋校正。輸出校正利用誤差估計(jì)值修正系統(tǒng)導(dǎo)航輸出，易于實(shí)現(xiàn)但精度不高；反饋校正在慣性導(dǎo)航系統(tǒng)解算過程中進(jìn)行，能夠獲得較高的精度，但當(dāng)估計(jì)結(jié)果不準(zhǔn)確時(shí)會導(dǎo)致濾波發(fā)散，系統(tǒng)可靠性不足。

對于狀態(tài)量采用哪種方式進(jìn)行反饋，取決于此狀態(tài)的可觀測性。對于可觀測狀態(tài)量，采用反饋校正可以獲得更高的精度；對于不可觀測狀態(tài)量，采用反饋校正會導(dǎo)致系統(tǒng)的發(fā)散。對于無視覺輔助的GPS/SINS超緊組合系統(tǒng)，由于其狀態(tài)量中的平臺失準(zhǔn)角是無法觀測的，不能利用反饋校正修正姿態(tài)誤差。本文提出的基于視覺輔助的超緊組合系統(tǒng)方案，在觀測量中引入了姿態(tài)信息，使平臺失準(zhǔn)角可觀測，此時(shí)采用反饋校正方法可以使姿態(tài)估計(jì)精度大大提高。

2 仿真實(shí)驗(yàn)與分析

2.1 仿真條件

以地理坐標(biāo)系作為導(dǎo)航坐標(biāo)系，采用MEMS慣性器件作為慣性導(dǎo)航元件。慣性器件誤差如下：陀螺漂移為15°/h，其標(biāo)度因子誤差為3 000×10-6；加速度計(jì)漂移為1×10-3g，其標(biāo)度因子誤差為1 000×10-6；陀螺和加速度計(jì)的白噪聲均方差分別為0.04°/h和0.001×10-3g；GPS導(dǎo)航定位在GPS軟件接收機(jī)的基礎(chǔ)上實(shí)現(xiàn)。在視覺輔助SINS/GPS超緊組合系統(tǒng)中，導(dǎo)航系統(tǒng)切換到組合導(dǎo)航模式之前，各個(gè)子系統(tǒng)各自獨(dú)立工作，GPS的捕獲和初始跟蹤都在此階段完成。GPS接收機(jī)模塊中，采樣頻率為38.162 MHz，預(yù)檢測時(shí)間為1 ms，采樣后信號中頻為9.548 MHz，跟蹤環(huán)路中包括載波跟蹤環(huán)路和碼跟蹤環(huán)路兩部分。當(dāng)GPS單獨(dú)工作時(shí)，載波環(huán)為2階環(huán)路，其噪聲帶寬為20 Hz，阻尼因子為0.707，碼環(huán)為2階延遲鎖定環(huán)，噪聲帶寬為3 Hz，載波環(huán)和碼環(huán)的增益分別為0.25和1. 當(dāng)處于組合導(dǎo)航模式時(shí)，載波環(huán)路帶寬降為8 Hz，碼環(huán)帶寬降為0.5 Hz. 視覺系統(tǒng)采用雙目立體攝像機(jī)，安裝在載體的正下方，雙目攝像機(jī)的基線距為4 m，兩攝像機(jī)光軸方向平行且垂直向下；攝像機(jī)分辨率為1 024像素×1 024像素；圖像中特征點(diǎn)提取存在0.001像素的誤差。INS的信號輸出頻率為1 000 Hz，視覺導(dǎo)航系統(tǒng)的信號輸出頻率為10 Hz，組合導(dǎo)航系統(tǒng)數(shù)據(jù)的輸出頻率為100 Hz. 仿真運(yùn)動的最大速度馬赫數(shù)為3，加速度設(shè)置為40g.

2.2 性能分析

仿真時(shí)間為200 s，圖2、圖3、圖4分別為SINS、無視覺組合與視覺輔助組合的姿態(tài)估計(jì)誤差對比結(jié)果。

圖2 偏航角誤差對比Fig.2 Comparison of yaw angle errors

圖3 俯仰角誤差對比Fig.3 Comparison of pitch angle errors

圖4 滾轉(zhuǎn)角誤差對比Fig.4 Comparison of roll angle errors

從圖2～圖4可以看出：有視覺輔助的組合導(dǎo)航系統(tǒng)，其姿態(tài)角誤差有明顯下降；而無視覺組合系統(tǒng)姿態(tài)誤差與SINS姿態(tài)誤差基本相同。這是因?yàn)橐曈X姿態(tài)信息的引入，增加了平臺誤差角可觀測性，提高了系統(tǒng)姿態(tài)估計(jì)精度。因此，視覺輔助GPS/MEMS-SINS組合系統(tǒng)改善了無視覺組合系統(tǒng)姿態(tài)誤差估計(jì)精度不高的缺點(diǎn)。

為了驗(yàn)證視覺輔助的效果，在整個(gè)200 s仿真過程中，50～150 s期間GPS信號中斷，當(dāng)GPS信號存在時(shí)，系統(tǒng)處于無視覺組合模式下。當(dāng)GPS信號中斷時(shí)，一種方式是引入視覺信息，校正MEMS-SINS的誤差，系統(tǒng)處于視覺/SINS組合模式，而另一種方式是只利用MEMS-SINS進(jìn)行導(dǎo)航。圖7～圖10分別為超緊組合系統(tǒng)在引入視覺輔助與沒有視覺輔助情況下的導(dǎo)航結(jié)果對比圖。

由圖5～圖8可以看出，無視覺組合系統(tǒng)可以很好地估計(jì)導(dǎo)航結(jié)果，位置誤差在1 m以內(nèi)，速度誤差在0.1 m/s以內(nèi)。當(dāng)GPS信號中斷時(shí)，如果沒有引入視覺信息，則系統(tǒng)進(jìn)入純慣性導(dǎo)航模式，由于本文中采用性能比較差的MEMS慣性導(dǎo)航系統(tǒng)，誤差發(fā)散速度很快，在100 s時(shí)間內(nèi)，北向位置誤差達(dá)到350 m，北向速度誤差達(dá)到8.5 m/s. 當(dāng)引入雙目視覺導(dǎo)航系統(tǒng)對超緊組合系統(tǒng)進(jìn)行校正后，位置速度誤差保持在5 m以內(nèi)，導(dǎo)航精度大大提高。而且在GPS信號恢復(fù)后，無視覺輔助的超緊組合系統(tǒng)導(dǎo)航誤差明顯比有視覺輔助時(shí)誤差大，收斂速度慢。

圖6 北向位置誤差Fig.6 Position errors in the north direction

圖7 天向位置誤差Fig.7 Position errors in the zenith direction

圖8 速度誤差Fig.8 Velocity errors

在仿真過程中，在50～150 s期間，GPS信號受到強(qiáng)干擾影響，信號載噪比降低到4 dB-Hz，150 s后噪聲消失，載噪比恢復(fù)正常。

當(dāng)GPS接收機(jī)信號受到強(qiáng)干擾時(shí)，GPS與SINS超緊組合系統(tǒng)的導(dǎo)航性能會受到影響，利用視覺輔助GPS/SINS超緊組合系統(tǒng)，可以提高系統(tǒng)的可靠性和導(dǎo)航精度。

圖9 東向位置誤差Fig.9 Position errors in the east direction

圖9～圖12為受干擾仿真條件下，分別采用無視覺組合導(dǎo)航與視覺輔助組合導(dǎo)航方式下的位置、速度導(dǎo)航誤差結(jié)果。由圖9～圖12可以看出，經(jīng)視覺輔助后的GPS/SINS超緊組合系統(tǒng)在受到強(qiáng)干擾時(shí)，位置和速度誤差比無圖像輔助的小，并且在干擾消失后導(dǎo)航誤差收斂更快。

圖10 北向位置誤差Fig.10 Position errors in the north direction

圖11 天向位置誤差Fig.11 Position errors in the zenith direction

圖12 速度誤差Fig.12 Velocity errors

3 結(jié)論

本文設(shè)計(jì)并驗(yàn)證了一種雙目視覺輔助MEMS-SINS/GPS超緊組合系統(tǒng)方案，對系統(tǒng)仿真結(jié)果進(jìn)行了分析。得到主要如下結(jié)論：

1)通過將雙目視覺提供的姿態(tài)信息引入MEMS-SINS/GPS超緊組合系統(tǒng)中，提高了平臺失準(zhǔn)角的可觀測性，大大降低了超緊組合系統(tǒng)的姿態(tài)誤差，進(jìn)而提高了系統(tǒng)導(dǎo)航精度。

2)當(dāng)GPS信號中斷時(shí)，利用雙目視覺導(dǎo)航信息校正MEMS-SINS的導(dǎo)航誤差，很大程度上抑制了INS的發(fā)散，提高了位置、速度的導(dǎo)航精度。

3)當(dāng)GPS信號受到強(qiáng)噪聲或多徑干擾造成跟蹤精度下降時(shí)，雙目視覺輔助GPS/MEMS-SINS超緊組合系統(tǒng)可以有效降低導(dǎo)航誤差，提高組合系統(tǒng)的導(dǎo)航精度，從而提高系統(tǒng)在高動態(tài)強(qiáng)干擾環(huán)境中的可靠性。

4)設(shè)計(jì)了一種模糊控制方法融合兩個(gè)子濾波器的導(dǎo)航結(jié)果，計(jì)算量大大降低，適合在彈載計(jì)算機(jī)上實(shí)時(shí)計(jì)算。

這種新型超緊組合結(jié)構(gòu)通過引入雙目視覺信息輔助超緊組合導(dǎo)航系統(tǒng)，有效地解決了低空飛行器在GPS信號被遮擋或干擾情況下的導(dǎo)航問題。