[发明专利]一种量测干扰下用于飞行器姿态估计的鲁棒递推滤波方法

摘要

本发明属于利用鲁棒滤波技术进行飞行器姿态估计的技术领域，涉及一种量测干扰下用于飞行器姿态估计的鲁棒递推滤波方法。本发明包括：采集飞行器运动过程中陀螺与星敏感器的输出数据；建立量测干扰下基于姿态四元数的飞行器非线性状态空间模型；进行时间更新，求得一步状态预测和预测方差的上界；进行鲁棒递推滤波量测更新，求得最优的滤波增益，进而求出k+1时刻的状态估计值和方差的上界，将k+1时刻的状态估计中四元数部分进行强制的归一化约束；输出姿态四元数及陀螺漂移的结果，完成姿态估计。本发明采用基于最小方差的鲁棒滤波设计，能够实现在最小方差意义下最优滤波增益设计，有利用提高系统的姿态估计精度。

主权项

一种量测干扰下用于飞行器姿态估计的鲁棒递推滤波方法，其特征在于：(1)采集飞行器运动过程中陀螺与星敏感器的输出数据；(2)建立量测干扰下基于姿态四元数的飞行器非线性状态空间模型；(2.1)建立飞行器姿态估计系统的状态方程；将姿态四元数qk和陀螺漂移βk组成维数为n的状态变量建立基于姿态四元数的飞行器状态方程为：xk+1=f(xk,ω~k)+Σi=1sAikηikxk+wk]]>qk为k时刻的姿态四元数；βk为k时刻的陀螺漂移值；为k时刻的陀螺输出测量值；Δt为陀螺的采样时间；||·||为求范数符号；a＝[a1 a2 a3]T为三维矢量，是加性噪声，方差ηu为陀螺漂移测量噪声，ηu的方差为s＝3；ηik是均值为零，方差为1的高斯白噪声；Aik为恰当维数的矩阵：Aik=-Δtσv2Aik104×303×403×3]]>σv为陀螺测量噪声标准差,σu为陀螺漂移噪声标准差；(2.2)建立量测干扰下飞行器姿态估计系统的量测方程；量测干扰下星敏感器的测量模型：为k时刻星敏感器量测值；为星敏感器的参考矢量；i为星敏感器观测到恒星的个数；为零均值的高斯白噪声，的方差为为未知的量测干扰向量；A(qk)为四元数qk的姿态描述矩阵，A(qk)为：A(qk)=(q42-ρkTρk)I3×3+2ρkρkT-2q4[ρk×]]]>将未知的量测干扰矩阵转化为：Δi＝diag([Δix Δix Δiy Δiy Δiz Δiz])；σij,j＝x,y,z为已知的正常数；采用3颗星的测量数据，即i＝3，飞行器姿态估计系统的量测方程被建立为：zk＝h(xk)+MΔg(xk)+vk为k时刻系统的量测值，维数为m；为量测非线性函数；g(xk)＝Eh(xk)；ΔΔT≤I18×18；vk是零均值高斯白噪声，vk的方差为(3)进行时间更新，求得一步状态预测和预测方差的上界；k时刻的状态估计值为方差上界为Σk，求得一步状态预测值为：x^k+1|k=f(x^k,ω~k)]]>一步状态预测误差为：x~k+1|k=xk+1-x^k+1|k=f(xk,ω~k)-f(x^k,ω~k)+Σi=1sAikηikxk+wk]]>将在处进行泰勒展开有：f(xk,ω~k)=f(x^k,ω~k)+Fkx~k+AkβkLkx~k]]>Ak为已知的比例矩阵；βk为未知矩阵，满足Lk为已知的调节矩阵，通常设为一步状态预测误差变形为：x~k+1|k=(Fk+AkβkLk)x~k+Σi=1sAikηikxk+wk]]>一步状态预测方差为：Pk+1|k=E[(Fk+AkβkLk)x~kx~kT(FkβkLk)T]+Σi=1sAikE(xkxkT)AikT+Qk=(Fk+AkβkLk)Σk(Fk+AkβkLk)T+Σi=1sAikE(xkxkT)AikT+Qk≤Fk(Σk-1-λLkTLk)-1FkT+λ-1AkAkT+Σi=1sAik[(1+ϵ)Σk+(1+ϵ-1)x^kx~kT]AikT+Qk]]>ε和λ都为已知的正数,λ满足λ-1In×n-LkΣkLkT>0;]]>一步状态预测方差的上界为：Σk+1|k=Fk(Σk-1-λLkTLk)-1FkT+λ-1AkAkT+Σi=1sAik[(1+ϵ)Σk+(1+ϵ-1)x^xx^kT]AikT+Qk]]>(4)进行鲁棒递推滤波量测更新，求得最优的滤波增益，进而求出k+1时刻的状态估计值和方差的上界，将k+1时刻的状态估计中四元数部分进行强制的归一化约束；一步量测预测值为：z^k+1=h(x^k+1|k)]]>一步量测预测误差为：z~k+1=zk+1-h(x^k+1|k)=h(xk+1)-h(x^k+1|k)+MΔg(xk+1)+vk+1]]>将h(xk+1)在处进行泰勒展开有：h(xk+1)=h(x^k+1|k)+Hk+1x~k+1|k+Ck+1αk+1Lk+1x~k+1|k]]>式中，Ck+1为已知的比例矩阵；αk+1为未知矩阵，满足一步量测预测误差能够变形为：z~k+1=(Hk+1+Ck+1αk+1Lk+1)x~k+1|k+MΔg(xk+1)+vk+1]]>k+1时刻的状态估计为：x^k+1=x^k+1|k+Kk+1z~k+1]]>Kk+1为待求的滤波增益；则k+1时刻的估计误差为：x~k+1=xk+1-x^k+1=(In×n-Kk+1Hk+1-Kk+1Ck+1αk+1Lk+1)x~k+1|k-Kk+1[MΔg(xk)+vk+1]]]>k+1时刻的估计误差协方差矩阵为：Pk+1=E[x~k+1x~k+1T]=(In×n-Kk+1Hk+1-Kk+1Ck+1αk+1Lk+1)Σk+1|k(In×n-Kk+1Hk+1-Kk+1Ck+1αk+1Lk+1)T+E[Kk+1MΔg(xk+1)gT(xk+1)ΔTMTKk+1T]+Kk+1Rk+1Kk+1T-E[(In×n-Kk+1Hk+1-Kk+1Ck+1αk+1Lk+1)x~k+1|kgT(xk+1)ΔTMTKk+1T]-E[Kk+1MΔg(xk+1)x~k+1|kT(In×n-Kk+1Hk+1-Kk+1Ck+1αk+1Lk+1)T]]]>-(In×n-Kk+1Hk+1-Kk+1Ck+1αk+1Lk+1)x~k+1|kgT(xk+1)ΔTMTKk+1T-Kk+1MΔg(xk+1)x~k+1|kT(In×n-Kk+1Hk+1-Kk+1Ck+1αk+1Lk+1)T≤ϵ1(In×n-Kk+1Hk+1-Kk+1Ck+1αk+1Lk+1)x~k+1|kx~k+1|kT(In×n-Kk+1Hk+1-Kk+1Ck+1αk+1Lk+1)T+ϵ1-1Kk+1MΔg(xk+1)gT(xk+1)ΔTMTKk+1T]]>估计误差协方差矩阵变形为：Pk+1≤(1+ϵ1)(In×n-Kk+1Hk+1-Kk+1Ck+1αk+1Lk+1)Σk+1|k(In×n-Kk+1Hk+1-Kk+1Ck+1αk+1Lk+1)T+(1+ϵ1-1)Kk+1ME[Δg(xk+1)gT(xk+1)ΔT]MTKk+1T+Kk+1Rk+1Kk+1T≤(1+ϵ1)[(In×n-Kk+1Hk+1)(Σk+1|k-1-μLk+1TLk+1)-1(In×n-Kk+1Hk+1)T+μ-1Kk+1Ck+1Ck+1TKk+1T]+(1+ϵ1-1)Kk+1MMTKk+1T+Kk+1Rk+1Kk+1T]]>μ为已知的正数，满足k+1时刻的估计误差的上界为：Σk+1=(1+ϵ1)[(In×n-Kk+1Hk+1)(Σk+1|k-1-μLk+1TLk+1)-1(In×n-Kk+1Hk+1)T+μ-1Kk+1Ck+1Ck+1TKk+1T]+(1+ϵ1-1)Kk+1MMTKk+1T+Kk+1Rk+1Kk+1T]]>令求得最优滤波增益为：Kk+1=(1+ϵ1)(Σk+1|k-1-μLk+1TLk+1)-1Hk+1T×{(1+ϵ1)Hk+1(Σk+1|k-1-μLk+1TLk+1)-1Hk+1T+μ-1Ck+1Ck+1T+(1+ϵ1-1)MMT+Rk+1}-1]]>求出k+1时刻的状态估计值和方差的上界Σk+1；||qk||2＝1，再将k+1时刻的状态估计值中的四元数部分进行归一化约束；(5)姿态估计系统的运行时间为M，若k＝M，则输出姿态四元数及陀螺漂移的结果，完成姿态估计；若k＜M，表示滤波过程未完成，则重复步骤(3)至步骤(4)，直至姿态估计系统运行结束。