李群生， 趙剡， 王進達

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

0 引言

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

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

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

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

1 系統(tǒng)設計

1.1 視覺導航原理和時空對準

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

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

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

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

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

(1)

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

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

1.3.1 偽距觀測方程

偽距觀測方程為

Zρ=HρX+Vρ，

(2)

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

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

1.3.2 偽距率觀測方程

偽距率觀測方程為

(3)

1.3.3 平臺失準角觀測方程

(4)

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

1)平臺失準角誤差的觀測方程為

Za=HaX+Va，

(5)

2)位置觀測方程為

(6)

3)速度觀測方程為

(7)

1.4 信息融合方案

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

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

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

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

(8)

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

Z1=H1X+V1，

(9)

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

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

Z2=H2X+V2，

(10)

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

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

(11)

(12)

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

(13)

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

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

2 仿真實驗與分析

2.1 仿真條件

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

2.2 性能分析

仿真時間為200 s，圖2、圖3、圖4分別為SINS、無視覺組合與視覺輔助組合的姿態(tài)估計誤差對比結(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可以看出：有視覺輔助的組合導航系統(tǒng)，其姿態(tài)角誤差有明顯下降；而無視覺組合系統(tǒng)姿態(tài)誤差與SINS姿態(tài)誤差基本相同。這是因為視覺姿態(tài)信息的引入，增加了平臺誤差角可觀測性，提高了系統(tǒng)姿態(tài)估計精度。因此，視覺輔助GPS/MEMS-SINS組合系統(tǒng)改善了無視覺組合系統(tǒng)姿態(tài)誤差估計精度不高的缺點。

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

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

圖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信號受到強干擾影響，信號載噪比降低到4 dB-Hz，150 s后噪聲消失，載噪比恢復正常。

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

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

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

圖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é)論

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

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

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

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

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

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