光流/慣導多傳感器信息融合方法

劉小明 陳萬春

邢曉嵐

殷興良

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

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

(中國航天科工集團公司，北京100830)

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

光流的測量值與光流傳感器的姿態、高度和速度耦合，同時光流傳感器具有體積小、重量輕、功耗低、可組網的特點，于是本文考慮將多個光流傳感器固聯在導彈彈體上，結合彈載速率陀螺，利用多傳感器的信息融合技術，實現對導彈姿態和光流信息的估計，并利用估計信息實現導彈的超低空突防任務.

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

卡爾曼濾波(Kalman filter)對非線性系統的處理主要有兩種方法:EKF(Extend Kalman Filter)法和UKF(Unscented Kalman Filter)法.EKF是將非線性模型用泰勒級數展開法做線性化處理，然后利用線性卡爾曼濾波算法做處理，由于EKF是對最佳估計的一階近似，存在對高階項的截斷誤差，當系統呈現強非線性時，EKF的估計精度偏低，嚴重時甚至導致濾波發散;UKF是對非線性函數的概率密度分布進行近似，而不是對非線性函數進行近似，較好地克服了 EKF算法的不足［8］.

1 多傳感器布置及其簡化模型

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

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

為了降低問題的復雜程度，簡化系統數學模型，現僅研究彈體縱向平面內的運動，并作出如下假設:

1)導彈周圍環境的質地紋理是雜亂的，光流是可測的;

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

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

4)導彈在做超低空巡航，僅在縱向平面內運動，彈道傾角非常小;

5)“瞬時平衡”假設是成立的.

基于以上假設，可得彈體縱向平面內的線化擾動運動學方程［9］為

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

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

接下來設法建立光流量測方程.

根據光流的定義和圖2中所示的幾何關系，可得出光流的表達式為

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

圖2 光流傳感器測量關系圖

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

圖3 光流傳感器在縱向平面內的布置圖

圖中，?為彈體俯仰角，h為彈體質心的高度，α為攻角，V為導彈相對于地面的速度.忽略彈體直徑，由幾何關系可以得出第i個光流傳感器的量測方程為

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

2 基于EKF和UKF的信息融合

2.1 EKF 法

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

驅動噪聲強度陣:

量測噪聲強度陣:

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

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

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

圖4 EKF對狀態變量的估計效果

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

2.2 UKF 法

根據UKF算法條件，只對量測方程(5)進行離散化處理即可.由于系統狀態維數和系統噪聲維數均為5，量測方程的維數為4，故增廣狀態向量的維數為L=5+5+4=14，Sigma點的采樣策略選用對稱采樣，其個數為2L+1=29.

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

圖6 UKF對狀態變量的估計效果

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

圖7 導彈起伏運動時的UKF融合性能

在本算例中，UKF的計算量要比EKF多得多，當仿真步長設置為0.01s時，對于12s的仿真時間，UKF法需要耗時 3.93 s，而 EKF僅需0.48 s，前者是后者的8倍多，這是因為量測方程結構形式復雜，需要占用很多CPU時間，在每一步估計循環中，EKF都只需要計算1次量測方程的導數即可，而UKF則需要計算29次量測方程，因為共有29個Sigma點要計算，這導致了UKF的計算量要遠大于EKF的計算量.

3 超低空突防算例

由上一節仿真可知，UKF在估計精度上與EKF相當，但由于量測方程的復雜性，導致UKF的實時性要遠低于EKF，所以本節將使用EKF作為信息融合算法，估計導彈的狀態信息，實現縱向平面內的高度控制，以期證明光流傳感器在超低空突防中的應用價值.

假設驅動噪聲為0，將式(1)簡寫成標準的狀態空間形式:

rank［B AB A2B A3B A4B］=4 ＜5，系統不完全可控.經計算，系統存在3個0極點，為使系統穩定或漸近穩定，需要設計狀態反饋陣，將系統的極點配置到s平面的左半平面，實現系統的鎮定.通過線性變換將系統按能控性分解為

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

經過計算，完全不能控子系統〈Anc0 Cnc〉是漸近穩定的.于是，原系統是可以通過線性狀態反饋鎮定下來的.下面，需要對完全能控子系統〈AcBcCc〉進行極點配置.

任取4個期望極點為［－7，－5，－2，－i，－2+i］，計算得狀態反饋陣增益矩陣 Kc=［0.6267 －0.831 9 6.202 9 2.293］，于是可使原系統鎮定下來的狀態反饋矩陣為［13］

以EKF對狀態的估計值作為狀態反饋，搭建導彈高度控制模型如圖8所示.

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

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

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

4 結論

本文研究了布置在彈體上的多個光流傳感器與速率陀螺的數據融合問題，建立了縱向平面內的彈體運動學模型和光流傳感器量測模型，分別使用EKF和UKF對量測信息進行了融合，對系統狀態信息進行了估計，利用估計信息進行了超低空飛行的高度控制仿真，仿真結果證明了量測模型、融合算法和高度控制器的可行性.

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

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

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］胡圣波.火箭飛行測量數據多尺度融合處理的理論及應用研究［D］.重慶大學自動化學院，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］方振平，陳萬春，張曙光.航空飛行器飛行動力學［M］.北京:北京航空航天大學出版社，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］黃顯林，周建鎖，王永富.巡航導彈最優地形跟蹤H∞控制器設計［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∞優化的地形跟蹤魯棒控制器設計［J］.哈爾濱工業大學學報，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］秦永元，張洪鉞，汪叔華.卡爾曼濾波與組合導航原理［M］西安:西北工業大學出版社，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］趙明旺，王杰，江衛華.現代控制理論［M］.武漢:華中科技大學出版社，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)