一种量测干扰下用于飞行器姿态估计的鲁棒递推滤波方法技术

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

【技术实现步骤摘要】
一种量测干扰下用于飞行器姿态估计的鲁棒递推滤波方法
本专利技术属于利用鲁棒滤波技术进行飞行器姿态估计的
，涉及一种量测干扰下用于飞行器姿态估计的鲁棒递推滤波方法。
技术介绍
由陀螺和星敏感器组合的姿态估计系统由于定姿精度高被广泛地应用于飞行器中。欧拉角、修正罗德里格斯参数、方向余弦、四元数等是飞行器的主要姿态描述参数。四元数由于计算简单，无三角函数的运算，同时又能避免欧拉角的奇异性问题，因此，四元数常被作为姿态估计系统中的姿态描述参数。针对该系统的四元数姿态估计模型，许多基于扩展卡尔曼滤波(ExtendedKalmanFilter，EKF)的姿态估计方法被提出，如加性扩展卡尔曼滤波(AdditiveExtendedKalmanFilter，AEKF)和乘性扩展卡尔曼滤波(MultiplicativeExtendedKalmanFilter，MEKF)。然而，扩展卡尔曼滤波仅仅适合于带加性噪声的精确已知的系统模型。如果系统模型中存在模型不确定的情况，该算法的性能将会受到严重的影响。因此，许多带模型不确定的非线性滤波算法被发展，如H∞滤波，集值非线性滤波，鲁棒滤波设计等，其中，基于最小方差的鲁棒滤波设计被证明是一种用于解决系统带模型不确定情况的有效处理手段，然而大多数基于最小方差的鲁棒滤波都是针对系统只存在一种模型不确定的情况。如果系统中存在两种或是两种以上的模型不确定，上述的鲁棒滤波算法将会失效。针对姿态估计系统，系统状态方程中存在状态与高斯白噪声耦合的乘性噪声，该噪声的方差未知，因此被看成是一种状态模型不确定，一种鲁棒扩展卡尔曼滤波(RobustExtendedKalmanFilter，REKF)被提出，但是没有考虑姿态估计系统存在其他模型不确定的情况。除了乘性噪声，由于飞行器在运行过程中存在抖动或是振动的影响，未知的量测干扰将不可避免地出现在姿态估计系统中，从而导致了测量误差。虽然这种量测误差会降低姿态估计系统的性能以及导致姿态信息不准确，但是在姿态估计滤波算法中很少的研究针对这种带未知量测干扰的情况。有研究指出可以将这种未知量测干扰看成是均值与方差确定的高斯噪声，然而在实际应用中，这种未知量测干扰的先验信息往往是无法知道的，仅仅只会知道它的取值范围，因此，它应该被是看成一种有范围的量测模型不确定。为了提高姿态估计系统的估计精度和鲁棒性，有必要研究在姿态估计系统中存在乘性噪声以及未知量测干扰这两种模型不确定的情况下的鲁棒滤波设计问题。
技术实现思路
本专利技术的目的是为了提高姿态估计的精度和增强系统的鲁棒性，提出了一种量测干扰下用于飞行器姿态估计的鲁棒递推滤波方法(RobustRecursiveFilter，RRF)。本专利技术的目的是这样实现的：(1)采集飞行器运动过程中陀螺与星敏感器的输出数据；(2)建立量测干扰下基于姿态四元数的飞行器非线性状态空间模型；(2.1)建立飞行器姿态估计系统的状态方程；将姿态四元数qk和陀螺漂移βk组成维数为n的状态变量建立基于姿态四元数的飞行器状态方程为：qk为k时刻的姿态四元数；βk为k时刻的陀螺漂移值；为k时刻的陀螺输出测量值；Δt为陀螺的采样时间；||·||为求范数符号；a＝[a1a2a3]T为三维矢量，是加性噪声，方差ηu为陀螺漂移测量噪声，ηu的方差为s＝3；ηik是均值为零，方差为1的高斯白噪声；Aik为恰当维数的矩阵：σv为陀螺测量噪声；(2.2)建立量测干扰下飞行器姿态估计系统的量测方程；量测干扰下星敏感器的测量模型：为k时刻星敏感器量测值；为星敏感器的参考矢量；i为星敏感器观测到恒星的个数；为零均值的高斯白噪声，的方差为为未知的量测干扰向量；A(qk)为四元数qk的姿态描述矩阵，A(qk)为：将未知的量测干扰矩阵转化为：Δ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，求得一步状态预测值为：一步状态预测误差为：将在处进行泰勒展开有：Ak为已知的比例矩阵；βk为未知矩阵，满足Lk为已知的调节矩阵，本文档来自技高网...

【技术保护点】
一种量测干扰下用于飞行器姿态估计的鲁棒递推滤波方法，其特征在于：(1)采集飞行器运动过程中陀螺与星敏感器的输出数据；(2)建立量测干扰下基于姿态四元数的飞行器非线性状态空间模型；(2.1)建立飞行器姿态估计系统的状态方程；将姿态四元数qk和陀螺漂移βk组成维数为n的状态变量xk=qkTβkTT,]]>建立基于姿态四元数的飞行器状态方程为：xk+1=f(xk,ω~k)+Σi=1sAikηikxk+wk]]>f(xk,ω~k)=(cos(||ω~k-βk||Δt2)I4×4+sin(||ω~k-βk||Δt2)Ω(ω~k-βk)||ω~k-βk||)qkβk;]]>qk为k时刻的姿态四元数；βk为k时刻的陀螺漂移值；为k时刻的陀螺输出测量值；Δt为陀螺的采样时间；||·||为求范数符号；a＝[a1 a2 a3]T为三维矢量，Ω(a)=-[a×]a-aT0,[a×]=0-a3a2a30-a1-a2a10;wk=04×1ηu]]>是加性噪声，方差Qk=E(wkwkT)=04×404×303×4Δtσu2I3×3,]]>ηu为陀螺漂移测量噪声，ηu的方差为s＝3：ηik是均值为零，方差为1的高斯白噪声；Aik为恰当维数的矩阵：Aik=-Δtσv2Aik104×303×403×3]]>A1k1=000100100-100-1000;A2k1=00-10000110000-100;A3k1=0100-1000000100-10;]]>σv为陀螺测量噪声；(2.2)建立量测干扰下飞行器姿态估计系统的量测方程；量测干扰下星敏感器的测量模型：为k时刻星敏感器量测值；为星敏感器的参考矢量；i为星敏感器观测到恒星的个数；为零均值的高斯白噪声，的方差为为未知的量测干扰向量；A(qk)为四元数qk的姿态描述矩阵，qk=ρkTq4T,]]>A(qk)为：A(qk)=(q42-ρkTρk)I3×3+2ρkρkT-2q4[ρk×]]]>将未知的量测干扰矩阵转化为：Mi=00σiy0σiz0σix0000σiz0σix0σiy00;]]>Δi＝diag([Δix Δix Δiy Δiy Δiz Δiz])；j＝x，y，z；Ei=00010-10-1001010-1000T;]]>σij，j＝x，y，z为已知的正常数；采用3颗星的测量数据，即i＝3，飞行器姿态估计系统的量测方程被建立为：zk＝h(xk)+MΔg(xk)+vkzk=zk1zk2zk3]]>为k时刻系统的量测值，维数为m；h(xk)=A(qk)r→1A(qk)r→2A(qk)r→3]]>为量测非线性函数；g(xk)＝Eh(xk)；M=M1M2M3;E=E1E2E3;Δ=Δ1Δ2Δ3,]]>ΔΔ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+AkβkLk)T]+Σi=1sAikE(xkxkT)AikT+Qk=(Fk+AkβkLk)Σk(Fk+AkβkLk)T+Σi=1sAikE(xkxkT)AikT+Qk≤Fk(&Sig...

【技术特征摘要】
1.一种量测干扰下用于飞行器姿态估计的鲁棒递推滤波方法，其特征在于：(1)采集飞行器运动过程中陀螺与星敏感器的输出数据；(2)建立量测干扰下基于姿态四元数的飞行器非线性状态空间模型；(2.1)建立飞行器姿态估计系统的状态方程；将姿态四元数qk和陀螺漂移βk组成维数为n的状态变量建立基于姿态四元数的飞行器状态方程为：qk为k时刻的姿态四元数；βk为k时刻的陀螺漂移值；为k时刻的陀螺输出测量值；Δt为陀螺的采样时间；||·||为求范数符号；a＝[a1a2a3]T为三维矢量，是加性噪声，方差ηu为陀螺漂移测量噪声，ηu的方差为s＝3；ηik是均值为零，方差为1的高斯白噪声；Aik为恰当维数的矩阵：σv为陀螺测量噪声标准差,σu为陀螺漂移噪声标准差；(2.2)建立量测干扰下飞行器姿态估计系统的量测方程；量测干扰下星敏感器的测量模型：为k时刻星敏感器量测值；为星敏感器的参考矢量；i为星敏感器观测到恒星的个数；为零均值的高斯白噪声，的方差为为未知的量测干扰向量；A(qk)为四元数qk的姿态描述矩阵，A(qk)为：将未知的量测干扰矩阵转化为：Δ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，求得一步状态预测值为：一步状态预测误差为：将在处进行泰勒展开有：Ak为已知的比例矩阵；βk为未知矩阵，满足Lk为已知的调节矩阵，通常设为一步状态预测误差变形为：一步状态预测方差为：ε和λ都为已知的正数,λ满足一步状态预测方差的上界为：(4)进行鲁棒递推滤波量测更新，求得最优的滤波增益，进而求出k+1时刻的状态估计值和方差的上界，将k+1时刻的状态估计中四元数部分进行强制的归一化约束；一步量测预测值为：一步量测预测误差为：将h(xk+1)在处进行泰勒展开有：式中，Ck+1为已知的比例矩阵；αk+1为未知矩阵，满足一步量测预测误差能够变形为：k+1时刻的状态估计为：Kk+1为待求的滤波增益；则k+1时刻的估计...

【专利技术属性】
技术研发人员：钱华明，黄蔚，沈忱，钱林琛，孙龙，彭宇，徐祥，
申请(专利权)人：哈尔滨工程大学，
类型：发明
国别省市：黑龙江;23