光流/惯导多传感器信息融合方法

刘小明 陈万春

邢晓岚

殷兴良

(北京航空航天大学 宇航学院，北京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)