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

摘要

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

说明书

技术领域

本发明属于利用鲁棒滤波技术进行飞行器姿态估计的技术领域，涉及一种量测干扰下用 于飞行器姿态估计的鲁棒递推滤波方法。

背景技术

由陀螺和星敏感器组合的姿态估计系统由于定姿精度高被广泛地应用于飞行器中。欧拉 角、修正罗德里格斯参数、方向余弦、四元数等是飞行器的主要姿态描述参数。四元数由于 计算简单，无三角函数的运算，同时又能避免欧拉角的奇异性问题，因此，四元数常被作为 姿态估计系统中的姿态描述参数。针对该系统的四元数姿态估计模型，许多基于扩展卡尔曼 滤波(Extended Kalman Filter，EKF)的姿态估计方法被提出，如加性扩展卡尔曼滤波 (Additive Extended Kalman Filter，AEKF)和乘性扩展卡尔曼滤波(Multiplicative  Extended Kalman Filter，MEKF)。然而，扩展卡尔曼滤波仅仅适合于带加性噪声的精确已知 的系统模型。如果系统模型中存在模型不确定的情况，该算法的性能将会受到严重的影响。 因此，许多带模型不确定的非线性滤波算法被发展，如H_∞滤波，集值非线性滤波，鲁棒滤波 设计等，其中，基于最小方差的鲁棒滤波设计被证明是一种用于解决系统带模型不确定情况 的有效处理手段，然而大多数基于最小方差的鲁棒滤波都是针对系统只存在一种模型不确定 的情况。如果系统中存在两种或是两种以上的模型不确定，上述的鲁棒滤波算法将会失效。

针对姿态估计系统，系统状态方程中存在状态与高斯白噪声耦合的乘性噪声，该噪声的 方差未知，因此被看成是一种状态模型不确定，一种鲁棒扩展卡尔曼滤波(Robust Extended  Kalman Filter，REKF)被提出，但是没有考虑姿态估计系统存在其他模型不确定的情况。除 了乘性噪声，由于飞行器在运行过程中存在抖动或是振动的影响，未知的量测干扰将不可避 免地出现在姿态估计系统中，从而导致了测量误差。虽然这种量测误差会降低姿态估计系统 的性能以及导致姿态信息不准确，但是在姿态估计滤波算法中很少的研究针对这种带未知量 测干扰的情况。有研究指出可以将这种未知量测干扰看成是均值与方差确定的高斯噪声，然 而在实际应用中，这种未知量测干扰的先验信息往往是无法知道的，仅仅只会知道它的取值 范围，因此，它应该被是看成一种有范围的量测模型不确定。为了提高姿态估计系统的估计 精度和鲁棒性，有必要研究在姿态估计系统中存在乘性噪声以及未知量测干扰这两种模型不 确定的情况下的鲁棒滤波设计问题。

发明内容

本发明的目的是为了提高姿态估计的精度和增强系统的鲁棒性，提出了一种量测干扰下 用于飞行器姿态估计的鲁棒递推滤波方法(Robust Recursive Filter，RRF)。

本发明的目的是这样实现的：

(1)采集飞行器运动过程中陀螺与星敏感器的输出数据；

(2)建立量测干扰下基于姿态四元数的飞行器非线性状态空间模型；

(2.1)建立飞行器姿态估计系统的状态方程；

将姿态四元数q_k和陀螺漂移β_k组成维数为n的状态变量$x_k={\left(\begin{array}{cc}{q}_k^T& {\beta }_k^T\end{array}\right)}^T,$建立基于姿态四 元数的飞行器状态方程为：

$x_{k+1}=f(x_k,{\tilde{\omega }}_k)+\underset{i=1}{\overset{s}{\Sigma }}{A}_{\mathrm{ik}}{\eta }_{\mathrm{ik}}{x}_k+w_k$

$f(x_k,{\tilde{\omega }}_k)=\left(\begin{array}{c}(\cos \left(\frac{\left|\right|{\tilde{\omega }}_k-{\beta }_k\left|\right|\mathrm{\Delta t}}{2}\right)I_{4\times 4}+\sin \left(\frac{\left|\right|{\tilde{\omega }}_k-{\beta }_k\left|\right|\mathrm{\Delta t}}{2}\right)\frac{\Omega ({\tilde{\omega }}_k-{\beta }_k)}{\left|\right|{\tilde{\omega }}_k-{\beta }_k\left|\right|})q_k\\ {\beta }_k\end{array}\right);$q_k为k时刻的姿态四元数； β_k为k时刻的陀螺漂移值；为k时刻的陀螺输出测量值；Δt为陀螺的采样时间；||·||为求范 数符号；a＝[a₁ a₂ a₃]^T为三维矢量，$\Omega \left(a\right)=\left(\begin{array}{cc}-[a\times ]& a\\ -a^T& 0\end{array}\right),[a\times ]=\left(\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right);w_k=\left(\begin{array}{c}{0}_{4\times 1}\\ {\eta }_u\end{array}\right)$是加 性噪声，方差$Q_k=E\left(w_k{w}_k^T\right)=\left(\begin{array}{cc}{0}_{4\times 4}& 0_{4\times 3}\\ 0_{3\times 4}& \mathrm{\Delta t}{\sigma }_u^2{I}_{3\times 3}\end{array}\right),$η_u为陀螺漂移测量噪声，η_u的方差为s＝3； η_ik是均值为零，方差为1的高斯白噪声；A_ik为恰当维数的矩阵：

$A_{\mathrm{ik}}=-\frac{\mathrm{\Delta t}{\sigma }_v}{2}\left(\begin{array}{cc}{A}_{\mathrm{ik}}^1& 0_{4\times 3}\\ 0_{3\times 4}& 0_{3\times 3}\end{array}\right)$

$A_{1k}^1=\left(\begin{array}{cccc}0& 0& 0& 1\\ 0& 0& 1& 0\\ 0& -1& 0& 0\\ -1& 0& 0& 0\end{array}\right);A_{2k}^1=\left(\begin{array}{cccc}0& 0& -1& 0\\ 0& 0& 0& 1\\ 1& 0& 0& 0\\ 0& -1& 0& 0\end{array}\right);A_{3k}^1=\left(\begin{array}{cccc}0& 1& 0& 0\\ -1& 0& 0& 0\\ 0& 0& 0& 1\\ 0& 0& -1& 0\end{array}\right);$σ_v为陀螺测量噪声；

(2.2)建立量测干扰下飞行器姿态估计系统的量测方程；

量测干扰下星敏感器的测量模型：

为k时刻星敏感器量测值；为星敏感器的参考矢量；i为星敏感器观测到恒星的个数； 为零均值的高斯白噪声，的方差为为未知的量测干扰向量；A(q_k) 为四元数q_k的姿态描述矩阵，$q_k={\left(\begin{array}{cc}{\rho }_k^T& q_4\end{array}\right)}^T,$A(q_k)为：

$A\left(q_k\right)=(q_4^2-{{\rho }_k}^T{\rho }_k)I_{3\times 3}+2{\rho }_k{{\rho }_k}^T-2q_4[{\rho }_k\times ]$

将未知的量测干扰矩阵转化为：

$M_i=\left(\begin{array}{cccccc}0& 0& {\sigma }_{\mathrm{iy}}& 0& {\sigma }_{\mathrm{iz}}& 0\\ {\sigma }_{\mathrm{ix}}& 0& 0& 0& 0& {\sigma }_{\mathrm{iz}}\\ 0& {\sigma }_{\mathrm{ix}}& 0& {\sigma }_{\mathrm{iy}}& 0& 0\end{array}\right);$Δ_i＝diag([Δ_ix Δ_ix Δ_iy Δ_iy Δ_iz Δ_iz])；$E_i={\left(\begin{array}{cccccc}0& 0& 0& 1& 0& -1\\ 0& -1& 0& 0& 1& 0\\ 1& 0& -1& 0& 0& 0\end{array}\right)}^T;$σ_ij，j＝x，y，z为已知的正常数；

采用3颗星的测量数据，即i＝3，飞行器姿态估计系统的量测方程被建立为：

z_k＝h(x_k)+MΔg(x_k)+v_k

$z_k=\left(\begin{array}{c}{z}_k^1\\ z_k^2\\ z_k^3\end{array}\right)$为k时刻系统的量测值，维数为m；$h\left(x_k\right)=\left(\begin{array}{c}A\left(q_k\right){\overrightarrow{r}}^1\\ A\left(q_k\right){\overrightarrow{r}}^2\\ A\left(q_k\right){\overrightarrow{r}}^3\end{array}\right)$为量测非线性函数； g(x_k)＝Eh(x_k)；$M=\left(\begin{array}{ccc}{M}_1& & \\ & M_2& \\ & & M_3\end{array}\right);E=\left(\begin{array}{ccc}{E}_1& & \\ & E_2& \\ & & E_3\end{array}\right);\Delta =\left(\begin{array}{ccc}{\Delta }_1& & \\ & {\Delta }_2& \\ & & {\Delta }_3\end{array}\right),$ΔΔ^T≤I_18×18；v_k是零均 值高斯白噪声，v_k的方差为

(3)进行时间更新，求得一步状态预测和预测方差的上界；

k时刻的状态估计值为方差上界为Σ_k，求得一步状态预测值为：

${\hat{x}}_{k+1|k}=f({\hat{x}}_k,{\tilde{\omega }}_k)$

一步状态预测误差为：

${\tilde{x}}_{k+1|k}=x_{k+1}-{\hat{x}}_{k+1|k}=f(x_k,{\tilde{\omega }}_k)-f({\hat{x}}_k,{\tilde{\omega }}_k)+\underset{i=1}{\overset{s}{\Sigma }}{A}_{\mathrm{ik}}{\eta }_{\mathrm{ik}}{x}_k+w_k$

将在处进行泰勒展开有：

$f(x_k,{\tilde{\omega }}_k)=f({\hat{x}}_k,{\tilde{\omega }}_k)+F_k{\tilde{x}}_k+A_k{\beta }_k{L}_k{\tilde{x}}_k$

A_k为已知的比例矩阵；β_k为未知矩阵，满足L_k为已知 的调节矩阵，通常设为

一步状态预测误差变形为：

${\tilde{x}}_{k+1|k}=(F_k+A_k{\beta }_k{L}_k){\tilde{x}}_k+\underset{i=1}{\overset{s}{\Sigma }}{A}_{\mathrm{ik}}{\eta }_{\mathrm{ik}}{x}_k+w_k$

一步状态预测方差为：

$\left(\begin{array}{c}{P}_{k+1|k}=E\left[(F_k+A_k{\beta }_k{L}_k){\tilde{x}}_k{\tilde{x}}_k^T{(F_k+A_k{\beta }_k{L}_k)}^T\right]+\underset{i=1}{\overset{s}{\Sigma }}{A}_{\mathrm{ik}}E\left(x_k{x}_k^T\right)A_{\mathrm{ik}}^T+Q_k\\ =(F_k+A_k{\beta }_k{L}_k){\Sigma }_k{(F_k+A_k{\beta }_k{L}_k)}^T+\underset{i=1}{\overset{s}{\Sigma }}{A}_{\mathrm{ik}}E\left(x_k{x}_k^T\right)A_{\mathrm{ik}}^T+Q_k\\ \le F_k{({\Sigma }_k^{-1}-{\mathrm{\lambda L}}_k^T{L}_k)}^{-1}{F}_k^T+{\lambda }^{-1}{A}_k{A}_k^T+\underset{i=1}{\overset{s}{\Sigma }}{A}_{\mathrm{ik}}[(1+ϵ){\Sigma }_k+(1+{ϵ}^{-1}){\hat{x}}_k{\hat{x}}_k^T]A_{\mathrm{ik}}^T+Q_k\end{array}\right)$

ε和λ都为已知的正数，λ满足

${\lambda }^{-1}{I}_{n\times n}-L_k{\Sigma }_k{L}_k^T>0;$

一步状态预测方差的上界为：

${\Sigma }_{k+1|k}=F_k{({\Sigma }_k^{-1}-\lambda L_k^T{L}_k)}^{-1}{F}_k^T+{\lambda }^{-1}{A}_k{A}_k^T+\underset{i=1}{\overset{s}{\Sigma }}{A}_{\mathrm{ik}}[(1+ϵ){\Sigma }_k+(1+{ϵ}^{-1}){\hat{x}}_k{\hat{x}}_k^T]A_{\mathrm{ik}}^T+Q_k$

(4)进行鲁棒递推滤波量测更新，求得最优的滤波增益，进而求出k+1时刻的状态估计 值和方差的上界，将k+1时刻的状态估计中四元数部分进行强制的归一化约束；

一步量测预测值为：

${\hat{z}}_{k+1}=h\left({\hat{x}}_{k+1|k}\right)$

一步量测预测误差为：

${\tilde{z}}_{k+1}=z_{k+1}-h\left({\hat{x}}_{k+1|k}\right)=h\left(x_{k+1}\right)-h\left({\hat{x}}_{k+1|k}\right)+\mathrm{M\Delta g}\left(x_{k+1}\right)+v_{k+1}$

将h(x_k+1)在处进行泰勒展开有：

$h\left(x_{k+1}\right)=h\left({\hat{x}}_{k+1|k}\right)+H_{k+1}{\tilde{x}}_{k+1|k}+C_{k+1}{\alpha }_{k+1}{L}_{k+1}{\tilde{x}}_{k+1|k}$

式中，$H_{k+1}=\partial h\left(x_{k+1}\right)/\partial x_{k+1}{|}_{x_{k+1}={\hat{x}}_{k+1|k}};$C_k+1为已知的比例矩阵；α_k+1为未知矩阵，满足

一步量测预测误差能够变形为：

${\tilde{z}}_{k+1}=(H_{k+1}+C_{k+1}{\alpha }_{k+1}{L}_{k+1}){\tilde{x}}_{k+1|k}+\mathrm{M\Delta g}\left(x_{k+1}\right)+v_{k+1}$

k+1时刻的状态估计为：

${\hat{x}}_{k+1}={\hat{x}}_{k+1|k}+K_{k+1}{\tilde{z}}_{k+1}$

K_k+1为待求的滤波增益；

则k+1时刻的估计误差为：

${\tilde{x}}_{k+1}=x_{k+1}-{\hat{x}}_{k+1}=(I_{n\times n}-K_{k+1}{H}_{k+1}-K_{k+1}{C}_{k+1}{\alpha }_{k+1}{L}_{k+1}){\tilde{x}}_{k+1|k}-K_{k+1}[\mathrm{M\Delta g}\left(x_k\right)+v_{k+1}]$

k+1时刻的估计误差协方差矩阵为：

$\left(\begin{array}{c}{P}_{k+1}=E\left[{\tilde{x}}_{k+1}{\tilde{x}}_{k+1}^T\right]=(I_{n\times n}-K_{k+1}{H}_{k+1}-K_{k+1}{C}_{k+1}{\alpha }_{k+1}{L}_{k+1}){\Sigma }_{k+1|k}{(I_{n\times n}-K_{k+1}{H}_{k+1}-K_{k+1}{C}_{k+1}{\alpha }_{k+1}{L}_{k+1})}^T\\ +E\left[K_{k+1}\mathrm{M\Delta g}\left(x_{k+1}\right)g^T\left(x_{k+1}\right){\Delta }^T{M}^T{K}_{k+1}^T\right]+K_{k+1}{R}_{k+1}{K}_{k+1}^T\\ -E\left[(I_{n\times n}-K_{k+1}{H}_{k+1}-K_{k+1}{C}_{k+1}{\alpha }_{k+1}{L}_{k+1}){\tilde{x}}_{k+1|k}{g}^T\left(x_{k+1}\right){\Delta }^T{M}^T{K}_{k+1}^T\right]\\ -E\left[K_{k+1}\mathrm{M\Delta g}\left(x_{k+1}\right){\tilde{x}}_{k+1|k}^T{(I_{n\times n}-K_{k+1}{H}_{k+1}-K_{k+1}{C}_{k+1}{\alpha }_{k+1}{L}_{k+1})}^T\right]\end{array}\right)$

$\left(\begin{array}{c}-(I_{n\times n}-K_{k+1}{H}_{k+1}-k_{k+1}{C}_{k+1}{\alpha }_{k+1}{L}_{k+1}){\tilde{x}}_{k+1|k}{g}^T\left(x_{k+1}\right){\Delta }^T{M}^T{K}_{k+1}^T\\ -K_{k+1}\mathrm{M\Delta g}\left(x_{k+1}\right){\tilde{x}}_{k+1|k}^T{(I_{n\times n}-K_{k+1}{H}_{k+1}-K_{k+1}{C}_{k+1}{\alpha }_{k+1}{L}_{k+1})}^T\\ \le {ϵ}_1(I_{n\times n}-K_{k+1}{H}_{k+1}-K_{k+1}{C}_{k+1}{\alpha }_{k+1}{L}_{k+1}){\tilde{x}}_{k+1|k}{\tilde{x}}_{k+1|k}^T{(I_{n\times n}-K_{k+1}{H}_{k+1}-K_{k+1}{C}_{k+1}{\alpha }_{k+1}{L}_{k+1})}^T\\ +{{ϵ}_1}^{-1}{K}_{k+1}\mathrm{M\Delta g}\left(x_{k+1}\right)g^T\left(x_{k+1}\right){\Delta }^T{M}^T{K}_{k+1}^T\end{array}\right)$

估计误差协方差矩阵变形为：

$\left(\begin{array}{c}{P}_{k+1}\le (1+{ϵ}_1)(I_{n\times n}-K_{k+1}{H}_{k+1}-K_{k+1}{C}_{k+1}{\alpha }_{k+1}{L}_{k+1}){\Sigma }_{k+1|k}{(I_{n\times n}-K_{k+1}{H}_{k+1}-K_{k+1}{C}_{k+1}{\alpha }_{k+1}{L}_{k+1})}^T\\ +(1+{{ϵ}_1}^{-1})K_{k+1}\mathrm{ME}\left[\mathrm{\Delta g}\left(x_{k+1}\right)g^T\left(x_{k+1}\right){\Delta }^T\right]M^T{K}_{k+1}^T+K_{k+1}{R}_{k+1}{K}_{k+1}^T\\ \le (1+{ϵ}_1)[(I_{n\times n}-K_{k+1}{H}_{k+1}){({\Sigma }_{k+1|k}^{-1}-{\mathrm{\mu L}}_{k+1}^T{L}_{k+1})}^{-1}{(I_{n\times n}-K_{k+1}{H}_{k+1})}^T+{\mu }^{-1}{K}_{k+1}{C}_{k+1}{C}_{k+1}^T{K}_{k+1}^T]\\ +(1+{{ϵ}_1}^{-1})K_{k+1}{\mathrm{MM}}^T{K}_{k+1}^T+K_{k+1}{R}_{k+1}{K}_{k+1}^T\end{array}\right)$

μ为已知的正数，满足${\mu }^{-1}{I}_{n\times n}-L_{k+1}{P}_{k+1|k}{L}_{k+1}^T>0;$

k+1时刻的估计误差的上界为：

$\left(\begin{array}{c}{\Sigma }_{k+1}=(1+{ϵ}_1)[(I_{n\times n}-K_{k+1}{H}_{k+1}){({\Sigma }_{k+1|k}^{-1}-{\mathrm{\mu L}}_{k+1}^T{L}_{k+1})}^{-1}{(I_{n\times n}-K_{k+1}{H}_{k+1})}^T+{\mu }^{-1}{K}_{k+1}{C}_{k+1}{C}_{k+1}^T{K}_{k+1}^T]\\ +(1+{{ϵ}_1}^{-1})K_{k+1}{\mathrm{MM}}^T{k}_{k+1}^T+K_{k+1}{R}_{k+1}{K}_{k+1}^T\end{array}\right)$

令求得最优滤波增益为：

$\left(\begin{array}{c}{K}_{k+1}=(1+{ϵ}_1){({\Sigma }_{k+1|k}^{-1}-{\mathrm{\mu L}}_{k+1}^T{L}_{k+1})}^{-1}{H}_{k+1}^T\\ \times \{(1+{ϵ}_1)H_{k+1}{({\Sigma }_{k+1|k}^{-1}-{\mathrm{\mu L}}_{k+1}^T{L}_{k+1})}^{-1}{H}_{k+1}^T+{\mu }^{-1}{C}_{k+1}{C}_{k+1}^T+(1+{{ϵ}_1}^{-1}){\mathrm{MM}}^T+R_{k+1}{\}}^{-1}\end{array}\right)$

求出k+1时刻的状态估计值和方差的上界Σ_k+1；

||q_k||₂＝1，再将k+1时刻的状态估计值中的四元数部分进行归一化约束；

(5)姿态估计系统的运行时间为M，若k＝M，则输出姿态四元数及陀螺漂移的结果， 完成姿态估计；若k＜M，表示滤波过程未完成，则重复步骤(3)至步骤(4)，直至姿态估 计系统运行结束。

步骤(2)中陀螺测量噪声标准差为σ_v＝2.6875×10^-7rad/s^1/2，陀螺漂移噪声标准差为 σ_u＝8.9289×10^-10rad/s^3/2，陀螺的采样周期为Δt＝0.25s；星敏感器测量噪声标准差为σ_s＝18″，输 出频率为1Hz；星敏感器的参考矢量设为${\overrightarrow{r}}^1={\left(\begin{array}{ccc}1& 0& 0\end{array}\right)}^T,$${\overrightarrow{r}}^2={\left(\begin{array}{ccc}0& 1& 0\end{array}\right)}^T$and${\overrightarrow{r}}^3={\left(\begin{array}{ccc}0& 0& 1\end{array}\right)}^T;$初始陀 螺漂移β＝[0.1 0.1 0.1]^T°/h；σ_ij＝10″，j＝x，y，z；

步骤(3)中初始状态估计值为${\hat{x}}_{0|0}={\left(\begin{array}{ccccccc}0& 0& 0& 1& 0& 0& 0\end{array}\right)}^T;$初始方差阵设为 P_0|0＝diag([(0.1°)² (0.1°)² (0.1°)² (0.1°)² (0.2°/h)² (0.2°/h)² (0.2°/h)²]；A_k＝0_n×n；为 λ＝0.0001，ε＝0.1。

步骤(4)中C_k+1＝0_n×n；μ＝0.0001；ε₁＝0.1。

步骤(5)中M＝1000。

本发明的有益效果在于：

(1)本发明同时考虑到非线性姿态估计系统状态模型存在乘性噪声以及量测模型存在未 知干扰的情况，有利于提高系统的鲁棒性；

(2)本发明采用基于最小方差的鲁棒滤波设计，能够实现在最小方差意义下最优滤波增 益设计，有利用提高系统的姿态估计精度。

附图说明

图1是现有的AEKF算法的一次独立实验姿态四元数误差结果图；

图2是现有的REKF算法的一次独立实验姿态四元数误差结果图；

图3是发明的RRF算法的一次独立实验姿态四元数误差结果图；

图4是发明的RRF算法与AEKF，REKF算法的姿态角均方根误差结果对比图；

图5是本发明的方法流程图。

具体实施方式

下面将结合附图和实施例对本发明作进一步的详细说明。

本发明提出的一种量测干扰下用于飞行器姿态估计的鲁棒递推滤波方法是通过Matlab 仿真软件进行仿真实验，与现有滤波算法的估计性能进行比较，如加性扩展卡尔曼滤波(AEKF) 以及鲁棒扩展卡尔曼滤波(REKF)。仿真硬件环境均为Intel(R)Core(TM)i5-2410M CPU2. 30GHz，4G RAM，Windows7操作系统。如图1到图3所示，实线代表四元数估计误差，虚 线代表误差协方差阵中对应元素的均方根，明显的可以看出，AEKF和REKF算法的四元数向 量部分的误差超出计算方差的范围，本发明的RRF算法的四元数估计误差均在计算方差的范 围内，说明RRF算法具有很好的鲁棒性。这是因为AEKF算法和REKF算法都没有考虑未知量 测干扰，而RRF算法在设计过程中补偿了乘性噪声和未知量测干扰这两种模型不确定的影响。 从图4所示的均方根误差比较中可以看出，在量测干扰下，RRF算法比AEKF算法和REKF算 法具有更高的估计精度。

。本发明考虑到非线性姿态估计系统状态模型存在乘性噪声以及量测模型存在未知干扰 的情况，建立了两种模型不确定存在下的飞行器姿态估计模型，同时基于扩展卡尔曼滤波的 结构，利用鲁棒递推设计找到预测方差及估计方差的上界范围，再利用最小方差理论设计最 优的滤波增益。

一种量测干扰下用于飞行器姿态估计的鲁棒递推滤波方法，流程如图5所示，包括以下 几个步骤：

步骤一：采集飞行器运动过程中陀螺与星敏感器的输出数据；

步骤二：建立量测干扰下基于姿态四元数的飞行器非线性状态空间模型；

步骤2.1建立飞行器姿态估计系统的状态方程；

将姿态四元数q_k和陀螺漂移β_k组成维数为n的状态变量$x_k={\left(\begin{array}{cc}{q}_k^T& {\beta }_k^T\end{array}\right)}^T,$根据飞行器的运 动学方程，建立基于姿态四元数的飞行器状态方程为：

$x_{k+1}=f(x_k,{\tilde{\omega }}_k)+\underset{i=1}{\overset{s}{\Sigma }}{A}_{\mathrm{ik}}{\eta }_{\mathrm{ik}}{x}_k+w_k---\left(1\right)$

式中：$f(x_k,{\tilde{\omega }}_k)=\left(\begin{array}{c}(\cos \left(\frac{\left|\right|{\tilde{\omega }}_k-{\beta }_k\left|\right|\mathrm{\Delta t}}{2}\right)I_{4\times 4}+\sin \left(\frac{\left|\right|{\tilde{\omega }}_k-{\beta }_k\left|\right|\mathrm{\Delta t}}{2}\right)\frac{\Omega ({\tilde{\omega }}_k-{\beta }_k)}{\left|\right|{\tilde{\omega }}_k-{\beta }_k\left|\right|})q_k\\ {\beta }_k\end{array}\right);$q_k为k时刻的姿态四 元数；β_k为k时刻的陀螺漂移值；为k时刻的陀螺输出测量值；Δt为陀螺的采样时间；||·||为 求范数符号；假设a＝[a₁ a₂ a₃]^T为三维矢量，则$\Omega \left(a\right)=\left(\begin{array}{cc}-[a\times ]& a\\ -a^T& 0\end{array}\right),[a\times ]=\left(\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right);$$w_k=\left(\begin{array}{c}{0}_{4\times 1}\\ {\eta }_u\end{array}\right)$是加性噪声，方差$Q_k=E\left(w_k{w}_k^T\right)=\left(\begin{array}{cc}{0}_{4\times 4}& 0_{4\times 3}\\ 0_{3\times 4}& \mathrm{\Delta t}{\sigma }_u^2{I}_{3\times 3}\end{array}\right),$η_u为陀螺漂移测量噪声，其方差为 s＝3；η_ik是均值为零，方差为1的高斯白噪声；A_ik为恰当维数的矩阵，能够被表示 为：

$A_{\mathrm{ik}}=-\frac{\mathrm{\Delta t}{\sigma }_v}{2}\left(\begin{array}{cc}{A}_{\mathrm{ik}}^1& 0_{4\times 3}\\ 0_{3\times 4}& 0_{3\times 3}\end{array}\right)---\left(2\right)$

式中：$A_{1k}^1=\left(\begin{array}{cccc}0& 0& 0& 1\\ 0& 0& 1& 0\\ 0& -1& 0& 0\\ -1& 0& 0& 0\end{array}\right);A_{2k}^1=\left(\begin{array}{cccc}0& 0& -1& 0\\ 0& 0& 0& 1\\ 1& 0& 0& 0\\ 0& -1& 0& 0\end{array}\right);A_{3k}^1=\left(\begin{array}{cccc}0& 1& 0& 0\\ -1& 0& 0& 0\\ 0& 0& 0& 1\\ 0& 0& -1& 0\end{array}\right);$σ_v为陀螺测量噪声；

步骤2.2建立量测干扰下飞行器姿态估计系统的量测方程；

量测干扰下星敏感器的测量模型为：

式中，为k时刻星敏感器量测值；为星敏感器的参考矢量；i为星敏感器观测到恒星 的个数；为零均值的高斯白噪声，其方差为为未知的量测干扰向量； A(q_k)为四元数q_k的姿态描述矩阵，若$q_k={\left(\begin{array}{cc}{\rho }_k^T& q_4\end{array}\right)}^T,$A(q_k)能被表示为：

$A\left(q_k\right)=(q_4^2-{{\rho }_k}^T{\rho }_k)I_{3\times 3}+2{\rho }_k{{\rho }_k}^T-2q_4[{\rho }_k\times ]---\left(4\right)$

将未知的量测干扰矩阵转化为：

式中，$M_i=\left(\begin{array}{cccccc}0& 0& {\sigma }_{\mathrm{iy}}& 0& {\sigma }_{\mathrm{iz}}& 0\\ {\sigma }_{\mathrm{ix}}& 0& 0& 0& 0& {\sigma }_{\mathrm{iz}}\\ 0& {\sigma }_{\mathrm{ix}}& 0& {\sigma }_{\mathrm{iy}}& 0& 0\end{array}\right);$Δ_i＝diag([Δ_ix Δ_ix Δ_iy Δ_iy Δ_iz Δ_iz])；$E_i={\left(\begin{array}{cccccc}0& 0& 0& 1& 0& -1\\ 0& -1& 0& 0& 1& 0\\ 1& 0& -1& 0& 0& 0\end{array}\right)}^T;$σ_ij，j＝x，y，z为已知的正常数；

由于工程上常采用3颗星的测量数据，即i＝3，飞行器姿态估计系统的量测方程被建立 为：

z_k＝h(x_k)+MΔg(x_k)+v_k    (6)

式中，$z_k=\left(\begin{array}{c}{z}_k^1\\ z_k^2\\ z_k^3\end{array}\right)$为k时刻系统的量测值，其维数为m；$h\left(x_k\right)=\left(\begin{array}{c}A\left(q_k\right){\overrightarrow{r}}^1\\ A\left(q_k\right){\overrightarrow{r}}^2\\ A\left(q_k\right){\overrightarrow{r}}^3\end{array}\right)$为量测非线性函数； g(x_k)＝Eh(x_k)；$M=\left(\begin{array}{ccc}{M}_1& & \\ & M_2& \\ & & M_3\end{array}\right);E=\left(\begin{array}{ccc}{E}_1& & \\ & E_2& \\ & & E_3\end{array}\right);\Delta =\left(\begin{array}{ccc}{\Delta }_1& & \\ & {\Delta }_2& \\ & & {\Delta }_3\end{array}\right),$ΔΔ^T≤I_188×8；v_k是零均 值高斯白噪声，其方差为

由式(1)所示的状态方程和式(6)所示的量测方程就构成了量测干扰下基于姿态四元数的 飞行器非线性状态空间模型；

步骤三：针对上述量测干扰下的飞行器状态空间模型，已知k时刻的状态估计值和方差 的上界，以扩展卡尔曼滤波为结构框架，利用鲁棒递推滤波设计，进行时间更新，求得一步 状态预测和预测方差的上界；

假设k时刻的状态估计值为方差上界为∑_k，根据扩展卡尔曼滤波，求得一步状态预 测值为：

${\hat{x}}_{k+1|k}=f({\hat{x}}_k,{\tilde{\omega }}_k)---\left(7\right)$

一步状态预测误差为：

${\tilde{x}}_{k+1|k}=x_{k+1}-{\hat{x}}_{k+1|k}=f(x_k,{\tilde{\omega }}_k)-f({\hat{x}}_k,{\tilde{\omega }}_k)+\underset{i=1}{\overset{s}{\Sigma }}{A}_{\mathrm{ik}}{\eta }_{\mathrm{ik}}{x}_k+w_k---\left(8\right)$

将在处进行泰勒展开有：

$f(x_k,{\tilde{\omega }}_k)=f({\hat{x}}_k,{\tilde{\omega }}_k)+F_k{\tilde{x}}_k+A_k{\beta }_k{L}_k{\tilde{x}}_k---\left(9\right)$

式中，A_k为已知的比例矩阵；β_k为未知矩阵，满足L_k为已知的调节矩阵，通常设为

将式(9)代入到式(8)中，一步状态预测误差变形为：

${\tilde{x}}_{k+1|k}=(F_k+A_k{\beta }_k{L}_k){\tilde{x}}_k+\underset{i=1}{\overset{s}{\Sigma }}{A}_{\mathrm{ik}}{\eta }_{\mathrm{ik}}{x}_k+w_k---\left(10\right)$

一步状态预测方差为：

$\left(\begin{array}{c}{P}_{k+1|k}=E\left[(F_k+A_k{\beta }_k{L}_k){\tilde{x}}_k{\tilde{x}}_k^T{(F_k+A_k{\beta }_k{L}_k)}^T\right]+\underset{i=1}{\overset{s}{\Sigma }}{A}_{\mathrm{ik}}E\left(x_k{x}_k^T\right)A_{\mathrm{ik}}^T+Q_k\\ =(F_k+A_k{\beta }_k{L}_k){\Sigma }_k{(F_k+A_k{\beta }_k{L}_k)}^T+\underset{i=1}{\overset{s}{\Sigma }}{A}_{\mathrm{ik}}+E\left(x_k{x}_k^T\right)A_{\mathrm{ik}}^T{+Q}_k\\ \le F_k{({\Sigma }_k^{-1}-\lambda L_k^T{L}_k)}^{-1}{F}_k^T+{\lambda }^{-1}{A}_k{A}_k^T+\underset{i=1}{\overset{s}{\Sigma }}{A}_{\mathrm{ik}}[(1+ϵ){\Sigma }_k+(1+{ϵ}^{-1}){\hat{x}}_k{\hat{x}}_k^T]A_{\mathrm{ik}}^T+Q_k\end{array}\right)---\left(11\right)$

式中，ε和λ都为已知的正数，λ满足

根据式(11)可以得到，一步状态预测方差的上界为：

${\Sigma }_{k+1|k}=F_k{({\Sigma }_k^{-1}-\lambda L_k^T{L}_k)}^{-1}{F}_k^T+{\lambda }^{-1}{A}_k{A}_k^T+\underset{i=1}{\overset{s}{\Sigma }}{A}_{\mathrm{ik}}[(1+ϵ){\Sigma }_k+(1+{ϵ}^{-1}){\hat{x}}_k{\hat{x}}_k^T]A_{\mathrm{ik}}^T+Q_k---\left(12\right)$

步骤四：利用上述求得的一步状态预测和预测方差的上界，进行鲁棒递推滤波量测更新， 求得最优的滤波增益，进而求出k+1时刻的状态估计值和方差的上界，同时由于四元数归一 化约束，将k+1时刻的状态估计中四元数部分进行强制的归一化约束；

根据式(6)可知，一步量测预测值为：

${\hat{z}}_{k+1}=h\left({\hat{x}}_{k+1|k}\right)---\left(13\right)$

一步量测预测误差为：

${\tilde{z}}_{k+1}=z_{k+1}-h\left({\hat{x}}_{k+1|k}\right)=h\left(x_{k+1}\right)-h\left({\hat{x}}_{k+1|k}\right)+\mathrm{M\Delta g}\left(x_{k+1}\right)+v_{k+1}---\left(14\right)$

将h(x_k+1)在处进行泰勒展开有：

$h\left(x_{k+1}\right)=h\left({\hat{x}}_{k+1|k}\right)+H_{k+1}{\tilde{x}}_{k+1|k}+C_{k+1}{\alpha }_{k+1}{L}_{k+1}{\tilde{x}}_{k+1|k}---\left(15\right)$

式中，C_k+1为已知的比例矩阵；α_k+1为未知矩阵，满足

将式(15)代入到式(14)中，一步量测预测误差能够变形为：

${\tilde{z}}_{k+1}=(H_{k+1}+C_{k+1}{\alpha }_{k+1}{L}_{k+1}){\tilde{x}}_{k+1|k}+\mathrm{M\Delta g}\left(x_{k+1}\right)+v_{k+1}---\left(16\right)$

根据扩展卡尔曼滤波理论，k+1时刻的状态估计为：

${\hat{x}}_{k+1}={\hat{x}}_{k+1|k}+K_{k+1}{\tilde{z}}_{k+1}---\left(17\right)$

式中，k_k+1为待求的滤波增益；

则k+1时刻的估计误差能表示为：

${\tilde{x}}_{k+1}=x_{k+1}-{\hat{x}}_{k+1}=(I_{n\times n}-K_{k+1}{H}_{k+1}-K_{k+1}{C}_{k+1}{\alpha }_{k+1}{L}_{k+1}){\tilde{x}}_{k+1|k}-K_{k+1}[\mathrm{M\Delta g}\left(x_k\right)+v_{k+1}]---\left(18\right)$

k+1时刻的估计误差协方差矩阵为：

$P_{k+1}=E\left[{\tilde{x}}_{k+1}{\tilde{x}}_{k+1}^T\right]=\left(\begin{array}{c}(I_{n\times n}-K_{k+1}{H}_{k+1}-K_{k+1}{C}_{k+1}{\alpha }_{k+1}{L}_{k+1}){\Sigma }_{k+1|k}{(I_{n\times n}-K_{k+1}{H}_{k+1}-K_{k+1}{C}_{k+1}{\alpha }_{k+1}{L}_{k+1})}^T\\ +E\left[K_{k+1}\mathrm{M\Delta g}{\left(x_{k+1}\right)}_{}{g}^T\left(x_{k+1}\right){\Delta }^T{M}^T{K}_{k+1}^T\right]+K_{k+1}{R}_{k+1}{K}_{k+1}^T\\ -E\left[(I_{n\times n}-K_{k+1}{H}_{k+1}-K_{k+1}{C}_{k+1}{\alpha }_{k+1}{L}_{k+1}){\tilde{x}}_{k+1|k}{g}^T\left(x_{k+1}\right){\Delta }^T{M}^T{K}_{k+1}^T\right]\\ -E\left[K_{k+1}\mathrm{M\Delta g}\left(x_{k+1}\right){\tilde{x}}_{k+1|k}^T{(I_{n\times n}-K_{k+1}{H}_{k+1}-K_{k+1}{C}_{k+1}{\alpha }_{k+1}{L}_{k+1})}^T\right]\end{array}\right)---\left(19\right)$

若ε₁为小的正数，下述不等式恒成立：

$\left(\begin{array}{c}-(I_{n\times n}-K_{k+1}{H}_{k+1}-K_{k+1}{C}_{k+1}{\alpha }_{k+1}{L}_{k+1}){\tilde{x}}_{k+1|k}{g}^T\left(x_{k+1}\right){\Delta }^T{M}^T{K}_{k+1}^T\\ -K_{k+1}\mathrm{M\Delta g}\left(x_{k+1}\right){\tilde{x}}_{k+1|k}^T{(I_{n\times n}-K_{k+1}{H}_{k+1}-K_{k+1}{C}_{k+1}{\alpha }_{k+1}{L}_{k+1})}^T\\ \le {ϵ}_1(I_{n\times n}-K_{k+1}{H}_{k+1}-K_{k+1}{C}_{k+1}{\alpha }_{k+1}{L}_{k+1}){\tilde{x}}_{k+1|k}{{\tilde{x}}_{k+1|k}^T(I_{n\times n}-K_{k+1}{H}_{k+1}-K_{k+1}{C}_{k+1}{\alpha }_{k+1}{L}_{k+1})}^T\\ +{{ϵ}_1}^{-1}{K}_{k+1}\mathrm{M\Delta g}\left(x_{k+1}\right){g^T\left(x_{k+1}\right)\Delta }^T{M}^T{K}_{k+1}^T\end{array}\right)---\left(20\right)$

将式(20)代入式(19)中，估计误差协方差矩阵变形为：

$\left(\begin{array}{c}{P}_{k+1}\le (1+{ϵ}_1)(I_{n\times n}-K_{k+1}{H}_{k+1}-K_{k+1}{C}_{k+1}{\alpha }_{k+1}{L}_{k+1}){\Sigma }_{k+1|k}{(I_{n\times n}-k_{k+1}{H}_{k+1}-K_{k+1}{C}_{k+1}{\alpha }_{k+1}{L}_{k+1})}^T\\ +(1+{{ϵ}_1}^{-1})K_{k+1}\mathrm{ME}\left[\mathrm{\Delta g}\left(x_{k+1}\right)g^T\left(x_{k+1}\right){\Delta }^T\right]M^T{K}_{k+1}^T+K_{k+1}{R}_{k+1}{K}_{k+1}^T\\ \le (1+{ϵ}_1)[(I_{n\times n}-K_{k+1}{H}_{k+1}){({\Sigma }_{k+1|k}^{-1}-\mu L_{k+1}^T{L}_{k+1})}^{-1}{(I_{n\times n}-K_{k+1}{H}_{k+1})}^T+{\mu }^{-1}{K}_{k+1}{C}_{k+1}{C}_{k+1}^T{K}_{k+1}^T]\\ +(1+{{ϵ}_1}^{-1})K_{k+1}{\mathrm{MM}}^T{K}_{k+1}^T+K_{k+1}{R}_{k+1}{K}_{k+1}^T\end{array}\right)---\left(21\right)$

式中，μ为已知的正数，满足

因此，k+1时刻的估计误差的上界为：

$\left(\begin{array}{c}{\Sigma }_{k+1}=(1+{ϵ}_1)[(I_{n\times n}-K_{k+1}{H}_{k+1}){({\Sigma }_{k+1|k}^{-1}-\mu L_{k+1}^T{L}_{k+1})}^{-1}{(I_{n\times n}-K_{k+1}{H}_{k+1})}^T+{\mu }^{-1}{K}_{k+1}{C}_{k+1}{C}_{k+1}^T{K}_{k+1}^T]\\ +(1+{{ϵ}_1}^{-1})K_{k+1}{\mathrm{MM}}^T{K}_{k+1}^T+K_{k+1}{R}_{k+1}{K}_{k+1}^T\end{array}\right)---\left(22\right)$

利用最小方差理论，令求得最优滤波增益为：

$\left(\begin{array}{c}{K}_{k+1}=(1+{ϵ}_1){({\Sigma }_{k+1|k}^{-1}-\mu L_{k+1}^T{L}_{k+1})}^{-1}{H}_{k+1}^T\\ \times {\{(1+{ϵ}_1)H_{k+1}{({\Sigma }_{k+1|k}^{-1}-\mu L_{k+1}^T{L}_{k+1})}^{-1}{H}_{k+1}^T+{\mu }^{-1}{C}_{k+1}{C}_{k+1}^T+(1+{{ϵ}_1}^{-1}){\mathrm{MM}}^T+R_{k+1}\}}^{-1}\end{array}\right)---\left(23\right)$

最后将上述求得的最优滤波增益代入到式(17)和式(22)中，求出k+1时刻的状态估计值 和方差的上界

由于状态变量四元数部分满足四元数归一化约束||q_k||₂＝1，因此再将k+1时刻的状态估计 值中的四元数部分进行归一化约束；

步骤五：姿态估计系统的运行时间为M，若k＝M，则输出姿态四元数及陀螺漂移的结 果，完成姿态估计；若k＜M，表示滤波过程未完成，则重复步骤三至步骤四，直至姿态估计 系统运行结束。

对本发明的有益效果说明如下：

MATLAB仿真实验，在以下的仿真条件下，对该方法进行仿真实验：

陀螺测量噪声标准差为σ_v＝2.6875×10^-7rad/s^1/2，陀螺漂移噪声标准差为 σ_u＝8.9289×10^-10rad/s^3/2，陀螺的采样周期为Δt＝0.25s；星敏感器测量噪声标准差为σ_s＝18″，输 出频率为1Hz；星敏感器的参考矢量设为${\overrightarrow{r}}^1\text{=}{\left(\begin{array}{ccc}1& 0& 0\end{array}\right)}^T,{\overrightarrow{r}}^2={\left(\begin{array}{ccc}0& 1& 0\end{array}\right)}^T\mathrm{and}{\overrightarrow{r}}^3={\left(\begin{array}{ccc}0& 0& 1\end{array}\right)}^T;$初始 陀螺漂移β＝[0.1 0.1 0.1]^T°/h；初始状态估计值为${\hat{x}}_{0|0}={\left(\begin{array}{ccccccc}0& 0& 0& 1& 0& 0& 0\end{array}\right)}^T;$初始方差阵 设为P_0|0＝diag([(0.1°)² (0.1°)² (0.1°)² (0.1°)² (0.2°/h)² (0.2°/h)² (0.2°/h)²]；忽略非线 性函数线性化的高阶项，设置A_k＝0_n×n，C_k+1＝0_m×n；为了确保估计精度，设置λ＝μ＝0.0001， ε＝ε₁＝0.1；σ_ij＝10″，j＝x，y，z；仿真时间设为1000s。