小型尾坐式飞行器自适应SRUKF姿态算法

武斌，陈鹏，胡永江，王长龙(军械工程学院，石家庄050003)

小型尾坐式飞行器自适应SRUKF姿态算法

武斌，陈鹏*，胡永江，王长龙
(军械工程学院，石家庄050003)

针对小型尾坐式飞行器姿态估计问题，设计了由陀螺、加速度计、磁强计组成的姿态测量系统。为了抑制MEMS陀螺漂移导致的姿态误差，以四元数为状态变量，以加速度计和磁强计的输出作为观测变量，建立了滤波模型。采用平方根无迹卡尔曼滤波(SRUKF)对传感器信息进行融合，保证了滤波算法的数值稳定性。由于小型尾坐式飞行器抗干扰能力弱，引入自适应算法，解决了量测信息受到干扰时滤波精度下降的问题，提高了系统的鲁棒性和可靠性。仿真结果表明，存在外界磁场干扰时，姿态误差小于1°。通过实际飞行实验，验证了算法的可行性。

姿态估计;尾坐式飞行器;平方根UKF;自适应

小型尾坐式飞行器具有垂直起降、结构简单等优点，精确的姿态测量是实现其稳定控制的基础。小型尾坐式飞行器倾转过程中需根据俯仰角调整拉重比［1］，这就要求俯仰角有高的精度;小型尾坐式飞行器飞行器导航系统抗干扰能力弱、系统模型误差和噪声的不确定性会导致滤波精度的降低甚至发散。因此，高精度和具有抗干扰能力的姿态测量算法对飞行器的稳定飞行显得尤为重要。

张荣辉［2］等从数值解算方法的角度对四元数法姿态解算方法进行了研究，但没有考虑传感器自身误差对姿态估计的影响。文献［3］采用卡尔曼滤波对陀螺和加速度计信号进行融合，修正了陀螺仪的随机漂移。这种方法要求系统模型是线性的，噪声为高斯白噪声，而实际复杂的环境很难满足这种条件。粒子滤波［4-5］从理论上来说适用于任何非线性非高斯随机系统，然而大量的粒子导致计算效率极大地降低，无法满足系统实时性要求。

本文针对由MEMS加速度计、陀螺仪和磁强计构成的惯导系统，采用自适应SRUKF对姿态信息进行融合，减小了陀螺漂移带来的误差，增强了滤波算法的稳定性，提高了系统对模型和噪声的自适应的能力。仿真和飞行实验验证了自适应SRUKF提高了姿态解算性能。

1 系统模型

1.1 系统硬件组成

本文采用三轴加速度计、三轴陀螺、三轴磁强计各一个分别测量飞行器的线加速度、角速度和磁场强度。采用Atmega 2560单片机完成数据的采集、处理和飞行器的控制。系统中机体坐标系定义为“前左上”，导航坐标系定义为“东北天”。传感器的安装轴与机体坐标系重合。

1.2 运动模型

以四元数为状态变量，考虑到陀螺仪存在漂移，为了防止误差积累，将状态变量扩增为X=［q0q1q2q3ωxωyωz］T，其中q=［q0q1q2q3］T为姿态四元数，ω=［ωxωyωz］T为陀螺漂移，估计出陀螺漂移并对其进行抑制。四元数微分方程为［6］:

应用龙格-库塔法将式(1)离散化，得:

其中，

其中，T为陀螺采样周期。

机体系下陀螺漂移误差方程为［7-8］:

由式(3)和式(5)可得，系统状态方程为:

w(k)为系统噪声，假定为零均值高斯白噪声，其协方差为Q。

以加速度计和磁强计的输出作为观测值，则观测方程为:

式中，［abmb］、［anmn］分别为机体系和导航系下加速度和磁场强度的值，Cbn［9］为四元数表示的姿态矩阵，v(k)为观测噪声，假定为零均值高斯白噪声，其协方差为R。

2 自适应平方根UKF算法

(2)计算Sigma点

2.1 平方根UKF

无迹卡尔曼滤波(UKF)用一系列确定的采样点对状态的概率密度进行近似，经过UT变换，得到估计的均值和方差，对于高斯系统可以达到三阶泰勒精度［10］。实际应用中，由于数值计算存在舍入误差等因素，导致协方差矩阵负定，影响滤波算法的稳定性。平方根UKF(SRUKF)用协方差平方根代替协方差，可以保证滤波的数值稳定性。

根据本文建立的运动模型，SRUKF的具体步骤为［7，11］:

(1)初始状态

其中，λ=α2(n+κ)-n，n为状态维数，κ通常为0，α决定了Sigma点离散程度，本文取为0.01。

(3)时间更新

式中，qr{·}表示矩阵的QR分解，cholupdate{·}表示矩阵Cholesky分解的修正。Wmi，Wci分别为均值和协方差的权系数，β用来描述状态变量的先验分布信息，对于高斯分布，最佳值为β=2［12］。

(4)量测更新

χk|k-1是由Xk|k-1和PXk|k-1计算出的Sigma点。

(5)滤波更新

其中，εk=Zk-Zk|k-1为新息序列。

2.2 自适应SRUKF(ASRUKF)

为了获得状态向量的最优估计值，必须准确获取噪声的统计特性。实际应用中，由于模型误差、量测信息受到干扰等因素使得噪声不确定，导致滤波精度降低、不稳定。因此，将自适应算法引入SRUKF中，减小运动加速度和磁干扰等造成的姿态误差。

根据极大似然准则，自适应量测噪声方差阵满足［13］:

式中，N为窗口数量，表示所用新息数量的多少。令N=1，得到量测噪声测量值:

式中，b为可调节的参数，b越小，新息序列对量测噪声估计值贡献越大。当量测误差较大时，调节b的大小，将式(27)中^Rk代入式(18)中，就可以实时估计量测噪声方差，进而通过式(21)调整滤波增益，降低量测值的权重，使滤波算法对噪声变化具有适应性，提高滤波精度。

3 仿真实验

仿真实验所用传感器性能参数如表1所示。将系统安装在转台上，模拟小型尾坐式飞行器水平与垂直状态转换的过渡飞行模式，初始姿态角为［φ，θ，ψ］=［2°，1°，45°］。采用JAVA语言编制了姿态显示界面，如图1所示，对传感器数据和姿态信息进行实时显示和存储。

表1 传感器性能参数

图2为只使用陀螺测量值解算的四元数法误差(图中虚线)和采用SRUKF融合算法分别解算出的姿态角误差(图中实线)。

图1 姿态显示界面

图2 纯陀螺与融合算法解算的姿态角误差

对图2分析可知，只采用陀螺解算的姿态角误差较大，这是由于陀螺的漂移造成的。融合算法抑制了陀螺漂移造成的姿态误差，姿态角误差小于0.5°，滚转、俯仰、偏航角误差标准差分别为0.13°、0.06°、0.09°。

为了验证自适应算法在量测信息受到干扰时的有效性，在转台附近放置磁铁模拟磁场干扰，初始姿态［φ，θ，ψ］=［0°，0°，-150°］。图3和图4分别为SRUKF(图中虚线)和自适应SRUKF算法(图中实线)求解的偏航角和误差曲线。

图3 偏航角变化曲线

图4 偏航角误差曲线

由图3和图4可见，当存在磁场干扰时，SRUKF解算误差明显增大，而自适应算法仍能保证姿态误差小于1°。由此可见，自适应SRUKF算法能有效抑制干扰，提高了系统了鲁棒性。

将系统安装到飞行器样机上，采用遥控方式对样机进行控制，在-8℃、微风(风速小于3 m/s)条件下实现了样机的起飞、倾转和降落，图5为飞行过程中某个时刻样机的实际状态。

图5 飞行过程截图

4 结论

本文以加速度计和磁强计的测量值补偿陀螺漂移带来的姿态误差，推导了采用自适应平方根UKF对传感器数据进行融合的姿态测量算法，有效的抑制了陀螺漂移导致的姿态累积误差，提高了姿态解算精度。采用自适应算法，有效的改进了小型尾坐式飞行器易受干扰的不足，提高了系统的鲁棒性。由于样机处于初步研制阶段，机体本身抗风能力较弱。因此，需要对机体进行改进，进而对本文提出的算法在更加恶劣环境下的可行性作进一步研究。

［1］Hugh Stone R，Peter Anderson，Colin Hutchison，et al.Flight Testing of the T-Wing Tail-Sitter Unmanned Air Vehicle［J］.Journal of Aircraft，2008，45(2):673-685.

［2］张荣辉，贾宏光，陈涛，等.基于四元数法的捷联式惯性导航系统的姿态解算［J］.光学精密工程，2008，16(10):1963-1970.

［3］叶锃锋，冯恩信.基于四元数和卡尔曼滤波的两轮车姿态稳定方法［J］.传感技术学报，2012，25(4):524-528.

［4］乔相伟，周卫东，吉宇人.基于四元数粒子滤波的飞行器姿态估计算法研究［J］.兵工学报，2012，33(9):1070-1075.

［5］张共愿，赵忠.粒子滤波及其在导航系统中的应用综述［J］.中国惯性技术学报，2006，14(6):91-94.

［6］罗秋凤，高振，牛妍.基于DSC与MEMS的微型测姿系统研究［J］.传感技术学报，2013，26(6):861-864.

［7］曲藴杰，孙尧，莫宏伟.小型无人机组合导航系统的平方根UKF技术研究［J］.弹箭与制导学报，2009，29(1):91-94.

［8］Zhou Zhanxin，Gao Yanan，Chen Jiabin.Unscented Kalman filter for SINS Alignment［J］.Journal of Systems Engineering and Electronics，2007，18(2):327-333.

［9］Mohamed Okasha，Brett Newman.Switching Algorithm to Avoid Attitude Representation Singularity［C］//AIAA Atmospheric Flight Mechanics Conference.Chicago，2009:1-17.

［10］Crassidis J L.Survey of Nonlinear Attitude Estimation Methods［J］. Journal of Guidance，Control，and Dynamics，2007，30(1):12-28.

［11］赵鹤，王喆垚.基于UKF的MEMS传感器姿态测量系统［J］.传感技术学报，2011，24(5):642-646.

［12］赵琳.非线性系统滤波理论［M］.北京:国防工业出版社，2012.

［13］秦伟，苑伟政，常洪龙，等.基于自适应UKF算法的MEMS陀螺空中在线标定技术［J］.中国惯性技术学报，2011，19(2): 170-174

武斌(1990-)，男，江苏南京人，硕士研究生在读，研究方向为传感器数据融合，wubin435@163.com;

陈鹏(1968-)，男，吉林通化人，博士，教授，硕士生导师，研究方向为设备智能检测技术。

EEACC:7230 doi:10.3969/j.issn.1004－1699.2014.05.013

Attitude Algorithm of Small Tail-Sitter Aircraft Based on Adaptive SRUKF

WU Bin，CHEN Peng*，HU Yongjiang，WANG Changlong

(Ordnance Engineering College，Shijiazhuang 050003，China)

For attitude estimation of small tail-sitter aircraft，the attitude measurement system composed of gyroscope，accelerometer and magnetometer is designed.In order to prevent the attitude error caused by drift of MEMS gyroscope，the filter modelis established with quaternion as state variables and the output ofaccelerometer and magnetometer as measurement information.Square root kalman filter(SRUKF)is carried out on the sensor information fusion to guarantee the numerical stability of filtering algorithm.Due to the weak anti-interference ability of small tail-sitter aircraft，the adaptive algorithm is introduced，which solves the problem of the decline of the filtering precision when measurement information is interferenced and improves the robustness and reliability of the system.Simulation results show that attitude error is less than 1°under the condition of magnetic disturbance.Actual flight test verifies the feasibility of the algorithm.

attitude estimation;tail-sitter aircraft;SRUKF;adaptive

V249

A

1004－1699(2014)05－0633－04

10.3969/j.issn.1004－1699.2014.05.012

2014-03-21

2014-04-22