基于UKF自主定姿方法研究

宋金来高延浩张 军弥 曼(北京航天微系统研究所北京100094)

基于UKF自主定姿方法研究

宋金来，高延浩，张 军，弥 曼
(北京航天微系统研究所，北京100094)

轻小型飞行器在飞行中卫星导航失效时，余度控制回路要求导航系统具有自主确定姿态的能力。提出了基于IMU的输出确定水平姿态的方法，并采用UKF实现飞行中的实时滤波估计。对某无人机实际飞行的MEMS型IMU数据进行了仿真，结果表明该方法给出的姿态角信息满足控制精度需求。将UKF与EKF滤波估计结果进行比较，UKF更具有优越性。

导航；自主定姿；MEMS；UKF

0 引言

在轻小型无人机、巡航弹等飞行器的控制系统研制中，MINS(MEMS型INS)＋BD2／GPS组合导航系统以体积小、功耗低、质量轻、精度高等特点，成为导航系统的最佳选择方案。在飞行控制中，由MINS＋BD2／GPS组合导航系统实时输出速度、位置、姿态、加速度、角速度等信息，飞控系统利用这些信息通过控制回路，实现对飞行器的飞行稳定与控制。

MINS＋BD2／GPS组合导航［1⁃2］方案，利用了二者的互补性，保证了飞行器长航时导航系统输出的精度。然而，在卫星导航失效状态下，MINS输出将无法长时间保持飞行控制系统所需要的信息精度。这样，随着速度、位置、姿态误差的增加，UAV飞行将偏离规划的航迹，也将导致失稳情况的发生。为了确保飞行器飞行安全，当此情况发生时，常用的飞行模式是：控制飞行器稳定飞行，按指令返航或在目标上空盘旋等待卫星导航有效恢复，这是飞行故障自动诊断与处理的常用策略。对于中型无人机等飞行器而言，利用上垂直陀螺输出的俯仰角、滚转角姿态信息，可以通过俯仰、滚转回路的控制实现飞行器的稳定飞行。

本文研究了在卫星导航失效时利用MEMS惯性仪表的输出(加速度、角速度)实现自主定姿的方法。利用此方法实现垂直陀螺的功能，即实时获得飞行器的两个水平姿态信息，为飞行器的稳定飞行提供保障。自主定姿问题的解决使轻小型无人机导航系统实现了自身有效的余度设计，增强了飞行器故障自动检测与隔离及控制律重构的功能。理论上，在飞行器处于巡航飞行时，利用加速度计的输出，可以实时确定两个水平姿态。但是，由于飞行中受发动机等因素的影响，MIMU (MEMS型IMU)的输出信息被噪声污染，直接解算姿态的结果将无法满足飞行控制的使用要求。本文给出的自主定姿方法是借助飞行器姿态运动学方程，利用加速度计调平原理，通过滤波估计器，实时确定出满足稳定控制精度要求的两个水平姿态角。由于姿态运动学方程以及量测方程均呈非线性，因此，采用了UKF滤波［3⁃4］估计方法。和常用的EKF滤波［5］方法相比较，UKF不需要求导计算Jacobian矩阵，稳定性较强，二者整体运算量相差不多。这种定姿方法不存在误差积累，其精度主要取决于加速度计的精度，模拟飞行仿真结果表明了此方法的有效性。

1 自主定姿模型

在卫星导航失效状态下，飞行器将按指令保持当前高度或返航飞行，并保持姿态平稳、避免机动。

一般地，小型飞行器巡航飞行速度不超过200km／h，飞行高度不超过5km，稳定控制回路对姿态角要求为精度优于0.5°。

设飞行器的俯仰角为θ，滚转角为γ，偏航角为ψ。

1.1 姿态运动学方程

机体系(xb，yb，zb)与航迹系 (xg，yg，zg)的位置关系如图1所示［6］。用表示机体系相对航迹系的角速度在机体系的分量构成的列向量，有：

其中，、分别为地球的转速、航迹系相对地面转速在机体系的分量构成的列向量，是机体系相对惯性系转动角速度在机体系的分量构成的列向量。

由图1可知，和姿态角速率·θ、·γ、ψ·的关系为：

1.2 加速度与水平姿态的关系

飞行中，与加速度计输出相关的比力方程［7］为：

将式(4)在机体系下展开并整理得到加速度与姿态的关系如下：

不难得知，在飞行器平稳飞行时，3个加速度均为小量。假设水平飞行速度为200km／h，当地纬度为39°，由式(6)可计算得到3个加速度为0.005m／s2左右。

1.3 自主定姿模型

(1)状态方程由式(1)可得：

由式(3)和式(8)，略去小量，得状态方程：

写成向量形式：

其中，

(2)量测方程

由式(5)和式(6)，略去小量，得量测方程为：

写成向量的形式如下：

2 UKF滤波器设计

2.1 UT变换

假设一个非线性变换y＝f(x)。状态变量x为n维随机变量，并已知其均值x和方差Px，则可以通过下面的UT变换得到2n＋1个列向量χi(Sigma 点)和相应的权值Wi来计算y的统计特性［8］：

这里λ＝α2（n＋κ）－n是一个比例因子，α决定x周围Sigma点的分布状态，通常选择10－4≤α≤1。κ为常数，设置为0或3－n。参数β≥0，调节β可以提高方差的精度，β＝2最优。i表示矩阵（n＋λ）Px的平方根的第i列或第i行。各个Sigma点的权值分别为：

其中，为均值的加权值，为方差的加权值。

为了得到输出的均值和方差，将这些Sigma点分别通过非线性传递函数进行非线性变换，得到变换后的各个Sigma点为：

则可以通过计算得到y的均值和方差为：

2.2 UKF算法实现

对于非线性动态系统，有：

式中，xk为nx维的系统状态向量，yk为ny维的系统观测向量。wk为系统噪声，其协方差矩阵为Qk。νk为观测噪声，其协方差阵为Rk。假设wk、νk都为Gauss白噪声，并且互不相关。

UKF滤波算法如下［9⁃10］：

(1)初始化

(2)UT变换

采用修正比例对称分布采样，得到k时刻状态估计的Sigma点集｛χi，k｝，i＝1，2，…，L。其中，L＝2n＋1为采样点数量。

(3)时间更新

(4)量测更新

3 仿真结果

利用某无人机上搭载飞行得到的MIMU数据进行数值仿真。无人机共飞行了两个多小时，飞行40min后给IMU上电，对IMU的输出及组合导航结果进行全过程记录。选取IMU加电后130s～800s的数据进行离线UKF自主确定姿态仿真。相应的无人机飞行轨迹及飞行高度如图2和图3所示。

MIMU惯性仪表的输出如图4和图5所示。

已知惯性仪表的精度为：陀螺零位漂移80(°)／h，加速度计零位偏置3×10－3g。

为了应用UKF滤波估计方法，将式(10)进行1阶离散化处理，离散化步长及采样间隔均为5ms。UKF滤波估计器初值的选取：状态变量x的初值由无人机直接装定，130s时无人机导航姿态为：

滤波器的其他参数为：α＝10－4，β＝2，κ＝0；Q＝diag ｛(20rad／h)2， (20rad／h)2｝，R＝ diag ｛(1.5m／s2)2，(1.5m／s2)2，(1.5m／s2)2｝P＝diag ｛(0.1rad)2，(0.1rad)2｝。

利用上述条件进行UKF滤波估计仿真。以GPS／INS组合导航结果作为参照，将自主定姿结果与其比较，可知：UKF估计姿态的精度优于0.5°，满足余度稳定控制回路的要求。俯仰角估计比较如图6所示，滚转角比较如图7所示。

由比较曲线可见，全过程中UKF自主确定水平姿态优于EKF滤波的精度。综合考虑，UKF在状态估计应用中，有比EKF方法更大的优越性。

4 结论

本文研究了飞行器自主确定姿态的方法。针对定姿数学模型的非线性，采用了UKF滤波估计方法。仿真结果表明，UKF方法实用、有效，比EKF方法具有更大的优势。该自主定姿方法为长航时飞行器自主故障诊断与处理提供了一种有效途径，具有一定的实际应用价值。

[1] 秦永元，张洪钺，汪叔华.卡尔曼滤波与组合导航原理[M].西安:西北工业大学出版社，1998.QIN Yong⁃yuan，ZHANG Hong⁃yue，WANG Shu⁃hua. Kalman filter and integrated navigation principle[M]. Xi'an:Northwest Industry University Press，1998.

[2] 曲蕴杰，孙尧，莫宏伟.小型无人机组合导航系统的平方根UKF技术研究[J].弹箭与制导学报，2009，29 (1):91⁃94. QU Yun⁃jie，SUN Yao，MO Hong⁃wei.Squre⁃root un⁃scented Kalman filter of small UAV integrated navigation system[J].Journal of Projectiles，Rockets，Missiles and Guidance，2009，29(1):91⁃94.

[3] 徐慧娟，吴美平.EKF和UKF在INS／GPS组合导航中的应用分析[J].航天控制，2006，24(6):7⁃16. XU Hui⁃juan，WU Mei⁃ping.Analysis of extended and unscented Kalman filtering for INS／GPS integration[J]. Aerospace Control，2006，24(6):7⁃16.

[4] 赵鹤，王喆垚.基于UKF的MEMS传感器姿态测量系统[J].传感技术学报，2011，24(5):642⁃646. ZHAO He，WANG Zhe⁃yao.MEMS sensors based attitude measurement system using UKF[J].Chinese Journal of Sensors and Actuators，2011，24(5):642⁃646.

[5] 范晓蓉，张海，范耀祖，等.微捷联姿态系统的一种扩展卡尔曼滤波方法[J].北京航空航天大学学报，2007，33(8):933⁃935. FAN Xiao⁃rong，ZHANG Hai，FAN Yao⁃zu，et al.Ex⁃tended Kalman filter method for micro⁃inertial strapdown attitude determination system[J].Journal of Beijing Uni⁃versity of Aeronautics and Astronautics，2007，33(8): 933⁃935.

[6] 赵汉元.飞行器再入动力学和制导[M].长沙:国防科技大学出版社，1997. ZHAO Han⁃yuan.Reentry dynamics and guidance[M]. Changsha:National University of Defense Technology Press，1997.

[7] 以光衢.惯性导航原理[M].北京:航空工业出版社，1987. Yi Guang⁃qu.Inertial navigation principle[M].Beijing: Aviation Industry Press，1987.

[8] Julier S J，Uhlmann J K，Durrant⁃Whyte H F.A new ap⁃proach for filtering nonlinear systems[C].Proceedings of the American Control Conference，Washington，1995: 1628⁃1632.

[9] De Marina H G，Pereda F J，Giron⁃Sierra J M，et al. UAV attitude estimation using unscented Kalman filter and TRIAD[J].IEEE Transactions on Industrial Electronics，2012，59(11):4465⁃4474.

[10] Gustafsson F，Hendeby G.Some relations between ex⁃tended and unscented Kalman filters [J].IEEE Transactions on SignalProcessing，2012，60 (2): 545⁃555.

Autonomous Attitude Determination for Small Aircraft Based on UKF

SONG Jin⁃lai，GAO Yan⁃hao，ZHANG Jun，MI Man
(Beijing Aerospace Institute of Microsystems，Beijing 100094)

To satisfy the requirement of redundance control loop，navigation system needs to independently determi⁃nate the attitude of small craft in case of GPS loss.A method is proposed to determinate attitude based on the accelerometer and gyroscope output，and UKF is used to realize filter estimation in real time during the flight.MEMS IMU data of UAV (unmanned aerial vehicle)flight is simulated and the simulation results show that the attitude information calculated in this method meets the requirement of control system.Supposing that attitude given by SINS／GPS integrated navigation system is of high accuracy，simulation results comparisons which are made between UKF and EKF show UKF is of better perform⁃ance.

navigation;autonomous attitude determination;micro electro mechanical system(MEMS);unscented Kalman filter(UKF)

V249.3

A

1674⁃5558(2017)01⁃01287

10.3969／j.issn.1674⁃5558.2017.02.004

宋金来，男，博士，研究员，硕士生导师，研究方向为飞行器导航、控制技术应用。

2016⁃06⁃28