光流/慣導(dǎo)多傳感器信息融合方法

劉小明 陳萬春

邢曉嵐

殷興良

(北京航空航天大學(xué) 宇航學(xué)院，北京100191)

(空軍裝備研究院總體所，北京100076)

(中國(guó)航天科工集團(tuán)公司，北京100830)

昆蟲在移動(dòng)時(shí)，周圍環(huán)境的亮度模式在視網(wǎng)膜上形成一系列連續(xù)變化的圖像，這一系列連續(xù)變化的信息不斷“流過”視網(wǎng)膜，好像是一種光的“流”，故稱這種圖像亮度模式的表觀運(yùn)動(dòng)為光流.國(guó)外的某些實(shí)驗(yàn)室，已經(jīng)研制出了光流傳感器的物理樣機(jī)，并利用光流傳感器實(shí)現(xiàn)了UAV的自主避障［1］、等高飛行［2］、自動(dòng)著陸［3］、風(fēng)速估計(jì)［4］、目標(biāo)檢測(cè)［5］和空中懸停［6］，這些技術(shù)在軍事領(lǐng)域如空中支援、低空突防等方面將有非常重要的應(yīng)用價(jià)值.

光流的測(cè)量值與光流傳感器的姿態(tài)、高度和速度耦合，同時(shí)光流傳感器具有體積小、重量輕、功耗低、可組網(wǎng)的特點(diǎn)，于是本文考慮將多個(gè)光流傳感器固聯(lián)在導(dǎo)彈彈體上，結(jié)合彈載速率陀螺，利用多傳感器的信息融合技術(shù)，實(shí)現(xiàn)對(duì)導(dǎo)彈姿態(tài)和光流信息的估計(jì)，并利用估計(jì)信息實(shí)現(xiàn)導(dǎo)彈的超低空突防任務(wù).

信息融合是將來自多個(gè)傳感器或多源的信息進(jìn)行綜合處理，從而得到更為準(zhǔn)確、可靠的結(jié)論，達(dá)到更好地了解對(duì)象的目的［7］.由于飛行器上的信息融合必須實(shí)時(shí)進(jìn)行，并且不具備大量數(shù)據(jù)存儲(chǔ)能力，所以可以在計(jì)算機(jī)上實(shí)時(shí)運(yùn)行的離散型卡爾曼濾波算法是一個(gè)合適的選擇.

卡爾曼濾波(Kalman filter)對(duì)非線性系統(tǒng)的處理主要有兩種方法:EKF(Extend Kalman Filter)法和UKF(Unscented Kalman Filter)法.EKF是將非線性模型用泰勒級(jí)數(shù)展開法做線性化處理，然后利用線性卡爾曼濾波算法做處理，由于EKF是對(duì)最佳估計(jì)的一階近似，存在對(duì)高階項(xiàng)的截?cái)嗾`差，當(dāng)系統(tǒng)呈現(xiàn)強(qiáng)非線性時(shí)，EKF的估計(jì)精度偏低，嚴(yán)重時(shí)甚至導(dǎo)致濾波發(fā)散;UKF是對(duì)非線性函數(shù)的概率密度分布進(jìn)行近似，而不是對(duì)非線性函數(shù)進(jìn)行近似，較好地克服了 EKF算法的不足［8］.

1 多傳感器布置及其簡(jiǎn)化模型

一種光流傳感器在彈體上的全方位布置方案如圖1所示.光流傳感器可以測(cè)得彈體前方、側(cè)方、下方、上方甚至后方的全方位的光流信息，這些信息為全面估計(jì)彈體所處的周邊環(huán)境提供了依據(jù).

圖1 光流傳感器在彈體上的全方位布置

為了降低問題的復(fù)雜程度，簡(jiǎn)化系統(tǒng)數(shù)學(xué)模型，現(xiàn)僅研究彈體縱向平面內(nèi)的運(yùn)動(dòng)，并作出如下假設(shè):

1)導(dǎo)彈周圍環(huán)境的質(zhì)地紋理是雜亂的，光流是可測(cè)的;

2)每個(gè)光流傳感器都能正常工作，它們的輸出含有量測(cè)噪聲，但不存在完全錯(cuò)誤的野值;

3)光流傳感器的視場(chǎng)角很小，測(cè)得的信息為鏡頭軸線上的光流信息;

4)導(dǎo)彈在做超低空巡航，僅在縱向平面內(nèi)運(yùn)動(dòng)，彈道傾角非常小;

5)“瞬時(shí)平衡”假設(shè)是成立的.

基于以上假設(shè)，可得彈體縱向平面內(nèi)的線化擾動(dòng)運(yùn)動(dòng)學(xué)方程［9］為

式中，α為攻角;?為俯仰角;δz為舵偏角;h為彈體質(zhì)心高度;ω(t)為白噪聲過程;E［ω(t)］=0;E［ω(t)ωT(τ)］=qδ(t－τ);q為 ω(t)的方差強(qiáng)度陣.

根據(jù)文獻(xiàn)［10］和文獻(xiàn)［11］中給出的某巡航導(dǎo)彈具體參數(shù)，可以解得 a11=－2.083，a31=－6.019988，a51=83.1299，b11= －0.0175，b31=5.9245，b51=1.318.將以上數(shù)據(jù)代入式(1)并將其離散化，得到離散彈體運(yùn)動(dòng)方程:

接下來設(shè)法建立光流量測(cè)方程.

根據(jù)光流的定義和圖2中所示的幾何關(guān)系，可得出光流的表達(dá)式為

式中，v為光流傳感器的水平速度;D為光流傳感器距離地面的高度;θ為光軸與豎直方向的夾角;ω為光流傳感器的旋轉(zhuǎn)速度.

圖2 光流傳感器測(cè)量關(guān)系圖

假設(shè)縱向平面內(nèi)安裝m個(gè)光流傳感器，第i個(gè)傳感器相對(duì)于彈體質(zhì)心的安裝位置為di，相對(duì)于彈體縱軸的安裝角為φi，如圖3所示.

圖3 光流傳感器在縱向平面內(nèi)的布置圖

圖中，?為彈體俯仰角，h為彈體質(zhì)心的高度，α為攻角，V為導(dǎo)彈相對(duì)于地面的速度.忽略彈體直徑，由幾何關(guān)系可以得出第i個(gè)光流傳感器的量測(cè)方程為

式中，ν(t)為量測(cè)噪聲，假設(shè)其均值為0的白噪聲;即 E［ν(t)］=0，且 E［ν(t)νT(τ)］=rδ(tτ);r為ν(t)的方差強(qiáng)度陣.

2 基于EKF和UKF的信息融合

2.1 EKF 法

對(duì)量測(cè)方程(5)進(jìn)行線化和離散化處理，并初始條件設(shè)置如下.

驅(qū)動(dòng)噪聲強(qiáng)度陣:

量測(cè)噪聲強(qiáng)度陣:

導(dǎo)彈速度大小V=200 m/s;采樣周期Ts=0.01 s，彈體上安裝3個(gè)光流傳感器，安裝角φi分別為60°，90°和 120°，安裝位置 di分別為0.5 m，0 m 和－0.5 m.

假設(shè)控制量始終為0，即舵偏角δz≡0，得到的仿真結(jié)果如圖4所示.

圖4表明，EKF可以融合安裝在彈體的不同位置的多個(gè)光流傳感器和一個(gè)速率陀螺的信息，實(shí)現(xiàn)對(duì)導(dǎo)彈攻角、俯仰角速度、飛行高度和高度變化率的估計(jì)，對(duì)俯仰角速度的估計(jì)偏差相對(duì)比較大，但俯仰角速度是由速率陀螺直接測(cè)量給出的，之所以出現(xiàn)較大偏差，是由于速率陀螺的測(cè)量噪聲太大，可以通過提高陀螺的測(cè)量精度來提高俯仰角速度的估計(jì)精度.

圖4 EKF對(duì)狀態(tài)變量的估計(jì)效果

現(xiàn)在令控制量 δz=0.2sin(πt+π/2)，使導(dǎo)彈在豎直平面內(nèi)做起伏運(yùn)動(dòng)，測(cè)試估計(jì)值對(duì)真實(shí)值的跟蹤性能，得到的仿真結(jié)果如圖5所示.圖5表明，EKF不但可以準(zhǔn)確地估計(jì)系統(tǒng)的狀態(tài)變量，其算法的實(shí)時(shí)性能也非常好，估計(jì)值相對(duì)于真實(shí)值來講，幾乎沒有滯后，這一點(diǎn)保證了EKF可以在實(shí)際工程中得到實(shí)時(shí)的應(yīng)用.

2.2 UKF 法

根據(jù)UKF算法條件，只對(duì)量測(cè)方程(5)進(jìn)行離散化處理即可.由于系統(tǒng)狀態(tài)維數(shù)和系統(tǒng)噪聲維數(shù)均為5，量測(cè)方程的維數(shù)為4，故增廣狀態(tài)向量的維數(shù)為L(zhǎng)=5+5+4=14，Sigma點(diǎn)的采樣策略選用對(duì)稱采樣，其個(gè)數(shù)為2L+1=29.

假設(shè)控制量始終為0，即舵偏角 δz≡0，得到的仿真結(jié)果如圖6所示.

圖6 UKF對(duì)狀態(tài)變量的估計(jì)效果

圖6表明，跟EKF一樣，UKF也可以對(duì)系統(tǒng)狀態(tài)進(jìn)行準(zhǔn)確的估計(jì).在估計(jì)的初期，UKF收斂比較慢，并且有比較大的震蕩;UKF收斂之后，其估計(jì)性能與EKF相當(dāng)，對(duì)系統(tǒng)的狀態(tài)估計(jì)與EKF的估計(jì)值幾乎完全一致.現(xiàn)在令控制量 δz=0.2sin(πt+π/2)，使導(dǎo)彈在豎直平面內(nèi)做起伏運(yùn)動(dòng)，測(cè)試估計(jì)值對(duì)真實(shí)值的跟蹤性能，得到的仿真結(jié)果如圖7所示.UKF幾乎是無滯后、無偏差地、完美地跟蹤了實(shí)際狀態(tài)變量的變化.

圖7 導(dǎo)彈起伏運(yùn)動(dòng)時(shí)的UKF融合性能

在本算例中，UKF的計(jì)算量要比EKF多得多，當(dāng)仿真步長(zhǎng)設(shè)置為0.01s時(shí)，對(duì)于12s的仿真時(shí)間，UKF法需要耗時(shí) 3.93 s，而 EKF僅需0.48 s，前者是后者的8倍多，這是因?yàn)榱繙y(cè)方程結(jié)構(gòu)形式復(fù)雜，需要占用很多CPU時(shí)間，在每一步估計(jì)循環(huán)中，EKF都只需要計(jì)算1次量測(cè)方程的導(dǎo)數(shù)即可，而UKF則需要計(jì)算29次量測(cè)方程，因?yàn)楣灿?9個(gè)Sigma點(diǎn)要計(jì)算，這導(dǎo)致了UKF的計(jì)算量要遠(yuǎn)大于EKF的計(jì)算量.

3 超低空突防算例

由上一節(jié)仿真可知，UKF在估計(jì)精度上與EKF相當(dāng)，但由于量測(cè)方程的復(fù)雜性，導(dǎo)致UKF的實(shí)時(shí)性要遠(yuǎn)低于EKF，所以本節(jié)將使用EKF作為信息融合算法，估計(jì)導(dǎo)彈的狀態(tài)信息，實(shí)現(xiàn)縱向平面內(nèi)的高度控制，以期證明光流傳感器在超低空突防中的應(yīng)用價(jià)值.

假設(shè)驅(qū)動(dòng)噪聲為0，將式(1)簡(jiǎn)寫成標(biāo)準(zhǔn)的狀態(tài)空間形式:

rank［B AB A2B A3B A4B］=4 ＜5，系統(tǒng)不完全可控.經(jīng)計(jì)算，系統(tǒng)存在3個(gè)0極點(diǎn)，為使系統(tǒng)穩(wěn)定或漸近穩(wěn)定，需要設(shè)計(jì)狀態(tài)反饋陣，將系統(tǒng)的極點(diǎn)配置到s平面的左半平面，實(shí)現(xiàn)系統(tǒng)的鎮(zhèn)定.通過線性變換將系統(tǒng)按能控性分解為

式中，T為正交變換陣;下標(biāo)nc表示不可控，c表示可控.此分解可由Matlab中的ctrbf命令完成.

經(jīng)過計(jì)算，完全不能控子系統(tǒng)〈Anc0 Cnc〉是漸近穩(wěn)定的.于是，原系統(tǒng)是可以通過線性狀態(tài)反饋鎮(zhèn)定下來的.下面，需要對(duì)完全能控子系統(tǒng)〈AcBcCc〉進(jìn)行極點(diǎn)配置.

任取4個(gè)期望極點(diǎn)為［－7，－5，－2，－i，－2+i］，計(jì)算得狀態(tài)反饋陣增益矩陣 Kc=［0.6267 －0.831 9 6.202 9 2.293］，于是可使原系統(tǒng)鎮(zhèn)定下來的狀態(tài)反饋矩陣為［13］

以EKF對(duì)狀態(tài)的估計(jì)值作為狀態(tài)反饋，搭建導(dǎo)彈高度控制模型如圖8所示.

圖8 基于EKF的高度控制框圖

高度控制器可設(shè)計(jì)成一個(gè)PI控制器，其中比例系數(shù) Kp=0.8，積分系數(shù) Ki=0.4，舵偏角限制在±30°之內(nèi)，初始指令高度為10m，第5s時(shí)刻將指令高度按指數(shù)規(guī)律下調(diào)到5 m，時(shí)間常數(shù)為2 s，仿真結(jié)果如圖9所示.導(dǎo)彈可以無超調(diào)、無靜差地跟蹤高度指令，進(jìn)行安全地超低空飛行，高度最終可以控制在期望高度的±0.5 m之內(nèi)，它證明了高度控制器的有效性，也同時(shí)證明了前文中傳感器的布置、EKF算法的可行性.

圖9 基于EKF的高度控制效果圖

4 結(jié)論

本文研究了布置在彈體上的多個(gè)光流傳感器與速率陀螺的數(shù)據(jù)融合問題，建立了縱向平面內(nèi)的彈體運(yùn)動(dòng)學(xué)模型和光流傳感器量測(cè)模型，分別使用EKF和UKF對(duì)量測(cè)信息進(jìn)行了融合，對(duì)系統(tǒng)狀態(tài)信息進(jìn)行了估計(jì)，利用估計(jì)信息進(jìn)行了超低空飛行的高度控制仿真，仿真結(jié)果證明了量測(cè)模型、融合算法和高度控制器的可行性.

建立系統(tǒng)模型時(shí)的假設(shè)條件比較多，未來還有很多問題需要研究，主要包括:①當(dāng)一個(gè)傳感器失效時(shí)，如何判別、剔除錯(cuò)誤數(shù)據(jù)，重新構(gòu)建量測(cè)方程，提高數(shù)據(jù)融合的糾錯(cuò)能力;②三維情況下全方位的光流融合問題;③光流傳感器與其他彈載傳感器的數(shù)據(jù)融合問題;④地形復(fù)雜時(shí)光流信息的有效性;⑤光流信息在導(dǎo)航、導(dǎo)引、乃至自動(dòng)著陸、自動(dòng)對(duì)接中的應(yīng)用.

光流傳感器作為一種新穎的信息量測(cè)工具，在民用領(lǐng)域已經(jīng)有了很多應(yīng)用案例，它在航天、軍事領(lǐng)域的潛在應(yīng)用值得做進(jìn)一步的研究和探討.

References)

［1］Stefan H，Gaurav S S，Peter C，et al.Combined optic-flow and stereo-based navigation of urban canyons for a UAV［C］//2005 IEEE/RSJ International Conference on Intelligent Robots and Systems.Albera，Canada:［s.n.］，2005:3309 －3316

［2］Andres E O，Natasha N N.Object detection and avoidance using optical techniques in uninhabited aerial vehicles［C］//AIAA Guidance，Navigation and Control Conference and Exhibit.Hilton Head，Soth Carolina:［s.n.］，2007

［3］Barber D B，Griffiths S R，McLain T W，et al.Autonomous landing of miniature aerial vehicles［J］.Journal of Aerospace Computing，Information，and Communication，2007，4(5):770 －784

［4］Andres F R，Evan A，Justin M B，et al.Wind estimation using an optical flow sensor on a miniature air vehicle［C］//AIAA Guidance，Navigation and Control Conference and Exhibit.Hilton Head，South Carolina:［s.n.］，2007:20 －23

［5］Yoko W，Patrick F.Air-to-ground target tracking in a GPS-denied environment using optical flow estimation［C］//AIAA Guidance，navigation，and Control Conference.Chicago，llinois:［s.n.］，2009

［6］Hugo R，Sergio S，Rogelio L.Real-time stabilization of an eightrotor UAV using optical flow［J］.IEEE Transactions on Robotics，2009，25(4):809 －817

［7］胡圣波.火箭飛行測(cè)量數(shù)據(jù)多尺度融合處理的理論及應(yīng)用研究［D］.重慶大學(xué)自動(dòng)化學(xué)院，2006:1－2

Hu Shengbo.The theory and applications study for multiscale data fusion from measurement of launch vehicle［D］.College of Automation，Chongqing University，2006:1 －2(in Chinese)

［8］潘泉，楊峰，葉亮.一類非線性濾波器——UKF綜述［J］.控制與決策，2005，20(5):481 －489

Pan Quan，Yang Feng，Ye Liang.Survey of a kind of nonlinear filters—UKF［J］.Control and Decision，2005，20(5):481 － 489(in Chinese)

［9］方振平，陳萬春，張曙光.航空飛行器飛行動(dòng)力學(xué)［M］.北京:北京航空航天大學(xué)出版社，2005:390－393

Fang Zhenping，Chen Wanchun，Zhang Shuguang.Flight dynamics of aero vehicles［M］.Beijing:Beihang University Press，2005:390－393(in Chinese)

［10］黃顯林，周建鎖，王永富.巡航導(dǎo)彈最優(yōu)地形跟蹤H∞控制器設(shè)計(jì)［J］.航天控制，1999，17(1):29 －35

Huang Xianlin，Zhou Jiansuo，Wang Yongfu.Design of optimal terrain-following H∞controller for a cruise missle［J］.Aerospace Control，1999，17(1):29 －35(in Chinese)

［11］黃顯林，周建鎖，趙永闖，等.基于H∞優(yōu)化的地形跟蹤魯棒控制器設(shè)計(jì)［J］.哈爾濱工業(yè)大學(xué)學(xué)報(bào)，2000，32(2):114－117

Huang Xianlin，Zhou Jiansuo，Zhao Yongchuang，et al.Optimal design of robust controller for terrain-following based on H∞［J］.Journal of Harbin Institute of Technology，2000，32(2):114－117(in Chinese)

［12］秦永元，張洪鉞，汪叔華.卡爾曼濾波與組合導(dǎo)航原理［M］西安:西北工業(yè)大學(xué)出版社，1998:42－48

Qin Yongyuan，Zhang Hongyue，Wang Shuhua.Kalman filters and integrated navigation theory［M］.Xi'an:Northwestern Polytechnical University Press，1998:42 －48(in Chinese)

［13］趙明旺，王杰，江衛(wèi)華.現(xiàn)代控制理論［M］.武漢:華中科技大學(xué)出版社，2007:193－261

Zhao Mingwang，Wang Jie，Jiang Weihua.Modern control theory［M］.Wuhan:Huazhong University of Science and Technology University Press，2007:193 －261(in Chinese)