一种量测干扰下用于飞行器姿态估计的鲁棒递推滤波方法
【专利摘要】本发明属于利用鲁棒滤波技术进行飞行器姿态估计的【技术领域】，涉及一种量测干扰下用于飞行器姿态估计的鲁棒递推滤波方法。本发明包括：采集飞行器运动过程中陀螺与星敏感器的输出数据；建立量测干扰下基于姿态四元数的飞行器非线性状态空间模型；进行时间更新，求得一步状态预测和预测方差的上界；进行鲁棒递推滤波量测更新，求得最优的滤波增益，进而求出k+1时刻的状态估计值和方差的上界，将k+1时刻的状态估计中四元数部分进行强制的归一化约束；输出姿态四元数及陀螺漂移的结果，完成姿态估计。本发明采用基于最小方差的鲁棒滤波设计，能够实现在最小方差意义下最优滤波增益设计，有利用提高系统的姿态估计精度。
【专利说明】—种量测干扰下用于飞行器姿态估计的鲁棒递推滤波方法
【技术领域】
[0001]本发明属于利用鲁棒滤波技术进行飞行器姿态估计的【技术领域】，涉及一种量测干扰下用于飞行器姿态估计的鲁棒递推滤波方法。
【背景技术】
[0002]由陀螺和星敏感器组合的姿态估计系统由于定姿精度高被广泛地应用于飞行器中。欧拉角、修正罗德里格斯参数、方向余弦、四元数等是飞行器的主要姿态描述参数。四元数由于计算简单，无三角函数的运算，同时又能避免欧拉角的奇异性问题，因此，四元数常被作为姿态估计系统中的姿态描述参数。针对该系统的四元数姿态估计模型，许多基于扩展卡尔曼滤波(ExtendedKalmanFilter, EKF)的姿态估计方法被提出，如加性扩展卡尔曼滤波(Additive Extended Kalman Filter, AEKF)和乘性扩展卡尔曼滤波(MultiplicativeExtended Kalman Filter7MEKF)。然而,扩展卡尔曼滤波仅仅适合于带加性噪声的精确已知的系统模型。如果系统模型中存在模型不确定的情况，该算法的性能将会受到严重的影响。因此，许多带模型不确定的非线性滤波算法被发展，如H00滤波，集值非线性滤波，鲁棒滤波设计等，其中，基于最小方差的鲁棒滤波设计被证明是一种用于解决系统带模型不确定情况的有效处理手段，然而大多数基于最小方差的鲁棒滤波都是针对系统只存在一种模型不确定的情况。如果系统中存在两种或是两种以上的模型不确定，上述的鲁棒滤波算法将会失效。
[0003]针对姿态估计系统，系统状态方程中存在状态与高斯白噪声耦合的乘性噪声，该噪声的方差未知，因此被看成是一种状态模型不确定，一种鲁棒扩展卡尔曼滤波(RobustExtended Kalman Filter, REKF)被提出，但是没有考虑姿态估计系统存在其他模型不确定的情况。除了乘性噪声，由于飞行器在运行过程中存在抖动或是振动的影响，未知的量测干扰将不可避免地出现在姿态估计系统中，从而导致了测量误差。虽然这种量测误差会降低姿态估计系统的性能以及导致姿态信息不准确，但是在姿态估计滤波算法中很少的研究针对这种带未知量测干扰的情况。有研究指出可以将这种未知量测干扰看成是均值与方差确定的高斯噪声，然而在实际应用中，这种未知量测干扰的先验信息往往是无法知道的，仅仅只会知道它的取值范围，因此，它应该被是看成一种有范围的量测模型不确定。为了提高姿态估计系统的估计精度和鲁棒性，有必要研究在姿态估计系统中存在乘性噪声以及未知量测干扰这两种模型不确定的情况下的鲁棒滤波设计问题。
【发明内容】
[0004]本发明的目的是为了提高姿态估计的精度和增强系统的鲁棒性，提出了一种量测干扰下用于飞行器姿态估计的鲁棒递推滤波方法(Robust Recursive Filter, RRF)。
[0005]本发明的目的是这样实现的:
[0006](I)采集飞行器运动过程中陀螺与星敏感器的输出数据；
[0007](2)建立量测干扰下基于姿态四元数的飞行器非线性状态空间模型；[0008](2.1)建立飞行器姿态估计系统的状态方程；
[0009]将姿态四元数qk和陀螺漂移β k组成维数为η的状态变量&: ?/，建立基于姿态四元数的飞行器状态方程为:
【权利要求】
1.一种量测干扰下用于飞行器姿态估计的鲁棒递推滤波方法，其特征在于: (1)采集飞行器运动过程中陀螺与星敏感器的输出数据； (2)建立量测干扰下基于姿态四元数的飞行器非线性状态空间模型； (2.1)建立飞行器姿态估计系统的状态方程； 将姿态四元数qk和陀螺漂移β k组成维数为η的状态变量^ = I^/ P 建立基于姿态四元数的飞行器状态方程为:
2.根据权利要求1所述的一种量测干扰下用于飞行器姿态估计的鲁棒递推滤波方法，其特征在于:步骤(2)中陀螺测量噪声标准差为Ov = 26875X10_7rad/s1/2，陀螺漂移噪声标准差为σ u = 89289Χ 10-lclrad/s3/2，陀螺的采样周期为At = 025s ;星敏感器测量噪声标准差为Os= 18"，输出频率为IHz ;星敏感器的参考矢量设为？'=[1 O Of, ?2=[0 I Ofand ?3=[0 O ij 初始陀螺漂移 β = [0.1 0.1 0.1]T° /h ； σ = 10" , j = χ, y, ζ。
3.根据权利要求1或2所述的一种量测干扰下用于飞行器姿态估计的鲁棒递推滤波方法，其特征在于:步骤(3)中初始状态估计值为元?=[0 0 0 1 0 0 Of;初始方差阵设为 P。。= diag([(0.1。)2 (0.1。)2 (0.1。)2 (0.1。)2 (0.2° /h)2 (0.2° /h)2 (0.2。/h) ] ；Ak = 0nXn ;为 λ = 0.0001，ε = 01。
4.根据权利要求3所述的一种量测干扰下用于飞行器姿态估计的鲁棒递推滤波方法，其特征在于:步骤⑷中Ck+1 = Omxn ；μ = 0.0001 ; ε i = 0.1。
5.根据权利要求4所述的一种量测干扰下用于飞行器姿态估计的鲁棒递推滤波方法，其特征在于:步骤(5)中M = 1000。
【文档编号】G05B13/04GK104020671SQ201410234813
【公开日】2014年9月3日 申请日期:2014年5月30日 优先权日:2014年5月30日
【发明者】钱华明, 黄蔚, 沈忱, 钱林琛, 孙龙, 彭宇, 徐祥 申请人:哈尔滨工程大学