基于容积四元数估计的航天器姿态估计方法与流程

技术特征：

1.基于容积四元数估计的航天器姿态估计方法，其特征在于：所述航天器姿态估计方法的具体过程为：

步骤一：建立航天器姿态运动学模型和观测模型；

步骤二：采用高斯滤波算法去除步骤一建立的航天器姿态运动学模型和观测模型中的噪声；

步骤三：采用容积四元数姿态估计器对航天器姿态进行估计。

2.根据权利要求1所述基于容积四元数估计的航天器姿态估计方法，其特征在于：所述步骤一中建立航天器姿态运动学模型和观测模型的具体过程为：

带有乘性噪声的非线性离散系统：

x_k+1＝(I_n×n+ζ_kΦ_k)f(x_k)+w_k (1)

y_k＝h(x_k)+v_k (2)

其中，x_k∈Rⁿ是系统k时刻的状态量，y_k∈R^m是系统k时刻的观测值；f(x_k)和h(x_k)是非线性方程；Φ_k是已知的常系数矩阵，ζ_k∈R是乘性噪声，w_k∈Rⁿ和v_k∈R^m是系统加性噪声和量测加性噪声；R为实数，Rⁿ为n维实数集，R^m为m维实数集；

ζ_k和v_k是同步相关的，且满足

$E\left\{\left[\begin{array}{c}{\mathrm{\ζ}}_t\\ v_t\end{array}\right]\left[\begin{array}{cc}{\mathrm{\ζ}}_k^T& v_k^T\end{array}\right]\right\}=\left[\begin{array}{cc}{Q}_{\mathrm{\ζ},k}& s_k\\ S_k^T& R_k\end{array}\right]{\mathrm{\δ}}_{t,k}---\left(4\right)$

其中S_k≠0是乘性噪声和加性噪声的互协方差；Q_ζ,k是ζ_k的方差，R_k是v_k的方差；t＝k时ζ_k和v_k存在相关性；

采用四元数描述的航天姿态运动学模型，f(x_k)具体形式如下：

$\stackrel{\·}{q}=\frac{1}{2}\left[\begin{array}{c}\mathrm{\ω}\\ 0\end{array}\right]\⊗q=\frac{1}{2}\mathrm{\Ω}\left(\mathrm{\ω}\right)q---\left(29\right)$

$\mathrm{\Ω}\left(\mathrm{\ω}\right)=\left[\begin{array}{cc}-\[\mathrm{\ω}\×\]& \mathrm{\ω}\\ -{\mathrm{\ω}}^T& 0\end{array}\right]---\left(30\right)$

其中，q为姿态四元数，是q的导数；ω＝[ω₁ ω₂ ω₃]^T为角速度在体坐标系下的表示，[ω×]表示由ω生成的反对称矩阵表示为：

$\[\mathrm{\ω}\×\]=\left[\begin{array}{ccc}0& -{\mathrm{\ω}}_3& {\mathrm{\ω}}_2\\ {\mathrm{\ω}}_3& 0& -{\mathrm{\ω}}_1\\ -{\mathrm{\ω}}_2& {\mathrm{\ω}}_1& 0\end{array}\right]---\left(31\right)$

观测模型的建立：

(一)陀螺输出模型

假设陀螺固连在航天器上，并且陀螺的安装方向与航天器本体坐标系重合，测量航天器的角速度；则陀螺模型表示为：

$\left\{\begin{array}{c}\widetilde{\mathrm{\ω}}=\mathrm{\ω}+\mathrm{\β}+{\mathrm{\η}}_v\\ \mathrm{\β}={\mathrm{\η}}_u\end{array}\right.---\left(32\right)$

其中，表示实际的陀螺输出值，β表示陀螺漂移值，ω表示理想情况下的陀螺输出值，η_v和η_u分别表示不相关的零均值高斯白噪声，且其协方差分别表示为和将陀螺输出和陀螺漂移方程离散化后的形式如下：

$\left\{\begin{array}{c}{\widetilde{\mathrm{\ω}}}_{k+1}={\mathrm{\ω}}_k+\frac{1}{2}{({\mathrm{\β}}_{k+1}+{\mathrm{\β}}_k)}^{1/2}+{(\frac{{\mathrm{\σ}}_v^2}{\mathrm{\Δ}t}+\frac{1}{12}{\mathrm{\σ}}_u^2\mathrm{\Δ}t)}^{1/2}{N}_v\\ {\mathrm{\β}}_{k+1}={\mathrm{\β}}_k+{\mathrm{\σ}}_u{\mathrm{\Δt}}^{1/2}{N}_u\end{array}\right.---\left(33\right)$

其中，△t表示步长，N_v和N_u分别表示离散后不相关的零均值高斯白噪声；

(二)星敏感器输出模型

假设星敏感器的安装方向与航天器本体坐标系重合，则星光矢量在航天器本体坐标系下的观测方程为：

$b=A_I^{b}r+v---\left(34\right)$

其中，r表示星光矢量在惯性系下的单位矢量方向，表示从惯性系到航天器本体坐标系的变换矩阵；v表示敏感器的观测误差，假设有m个星敏感器同时进行观测，则在第k时刻，用四元数描述的矢量观测模型为：

$z_k=\left[\begin{array}{c}{b}_1\\ b_2\\ .\\ .\\ .\\ b_m\end{array}\right]{|}_k+\left[\begin{array}{c}{v}_1\\ v_2\\ .\\ .\\ .\\ v_m\end{array}\right]{|}_k=\left[\begin{array}{c}A\left(q\right)r_1\\ A\left(q\right)r_2\\ .\\ .\\ .\\ A\left(q\right)r_m\end{array}\right]{|}_k+v_k---\left(35\right)$

其中，b_m和r_m表示第m个参考矢量分别在航天器本体坐标系和惯性坐标系下的分量；A(q)表示姿态转移矩阵，其四元数形式的描述为：

$A\left(q\right)=(q_4^2-|\left|\mathrm{\ρ}\right|{|}^2)I_{3\×3}-2q_4\[\mathrm{\ρ}\×\]+2{\mathrm{\ρ\ρ}}^T---\left(36\right)$

其中，姿态四元数q＝[q₁,q₂,q₃,q₄]^T，q分解为标量q₄和矢量ρ，ρ＝[q₁,q₂,q₃]^T；

展开形式是：

$A\left(q\right)=\left[\begin{array}{ccc}{q}_1^2-q_2^2-q_3^2+q_4^2& 2(q_1{q}_2+q_4{q}_3)& 2(q_1{q}_3-q_4{q}_2)\\ 2(q_2{q}_1-q_4{q}_3)& -q_1^2+q_2^2-q_3^2+q_4^2& 2(q_2{q}_3+q_4{q}_1)\\ 2(q_3{q}_1+q_4{q}_2)& 2(q_3{q}_2-q_4{q}_1)& -q_1^2-q_2^2+q_3^2+q_4^2\end{array}\right]---\left(37\right).$

3.根据权利要求1或2所述基于容积四元数估计的航天器姿态估计方法，其特征在于：所述步骤二中采用高斯滤波算法去除步骤一建立的航天器姿态运动学模型和观测模型中的噪声的具体过程为：

步骤二一：一步预测；

考虑非线性系统形式为式(1)和(2)，假设后验概率密度函数P_k-1|k-1和状态估计值是已知的，则x_k的一步预测均值和方差形式如下：

其中

$M_{k-1}=E\[{\mathrm{\ζ}}_{k-1}|D_{k-1}\]=S_{k-1}{P}_{\tilde{y}\tilde{y},k-1|k-1}^{-1}(y_{k-1}-h\left({\hat{x}}_{k-1|k-1}\right)\left)\right)---\left(9\right)$

$U_{k-1}=E\[{\mathrm{\ζ}}_t{\mathrm{\ζ}}_k^T|D_{k-1}\]=Q_{\mathrm{\ζ},k-1}-S_{k-1}{P}_{\tilde{y}\tilde{y},k-1|k-1}^{-1}{S}_{k-1}^T---\left(10\right)$

$D_{k-1}={\left[\begin{array}{cccc}{y}_1^T& y_2^T& \mathrm{...}& y_{k-1}^T\end{array}\right]}^T---\left(12\right)$

其中是x_k的一步预测值，I_n×n为n×n的单位矩阵，P_k|k-1为x_k的一步预测方差，为y_k-1的方差，M_k-1表示ζ_k-1的条件均值，U_k-1表示ζ_k-1的条件方差，D_k-1表示测量值的集合；

步骤二二：量测更新；

考虑非线性系统形式为式(1)和(2)，假设状态量x_k的预测均值和方差已经得到，则它的后验均值和方差P_k|k的形式如下：

${\hat{x}}_{k|k}={\hat{x}}_{k|k-1}+K_k(y_k-{\hat{y}}_{k|k-1})---\left(18\right)$

$P_{k|k}=P_{k|k-1}-K_k{P}_{\tilde{y}\tilde{y},k|k-1}{K}_k^T---\left(19\right)$

$K_k=P_{\tilde{x}\tilde{y},k|k-1}{P}_{\tilde{y}\tilde{y},k|k-1}^{-1}---\left(20\right)$

其中为y_k的一步预测值，K_k为滤波增益，为x_k和y_k的一步预测协方差，为y_k的一步预测方差。

4.根据权利要求3所述基于容积四元数估计的航天器姿态估计方法，其特征在于：所述步骤三中采用容积四元数姿态估计器对航天器姿态进行估计的具体过程为：

将状态向量选为x＝[δp^T β^T]^T，其中δp为与误差四元数对应的修正罗德里格斯参数，形式如下：

$\mathrm{\δ}p=b\frac{\mathrm{\δ}\mathrm{\ρ}}{a+{\mathrm{\δq}}_4}---\left(38\right)$

其中0≤a≤1，b为尺度参数；

(一)时间预测

已知滤波器的初值q₀，β₀，P₀，由k-1时刻的状态估计和协方差阵估计P_k-1获得k时刻的容积点为：

$X_{i,k-1}=\sqrt{P_{k-1}}{\mathrm{\ξ}}_i+{\hat{x}}_{k-1},i=1,2,...,n---\left(39\right)$

其中，δp_i,k-1和β_i,k-1分别表示姿态角误差和陀螺漂移，ξ_i表示容积点集合{ξ_i}的第i列向量，容积点集合{ξ_i}可定义为即

$\left\{{\mathrm{\ξ}}_i\right\}=\sqrt{n}\left\{\left[\begin{array}{c}1\\ 0\\ .\\ .\\ .\\ 0\end{array}\right]\left[\begin{array}{c}0\\ 1\\ .\\ .\\ .\\ 0\end{array}\right]\mathrm{...}\left[\begin{array}{c}0\\ 0\\ .\\ .\\ .\\ 1\end{array}\right]\left[\begin{array}{c}-1\\ 0\\ .\\ .\\ .\\ 0\end{array}\right]\left[\begin{array}{c}0\\ -1\\ .\\ .\\ .\\ 0\end{array}\right]\mathrm{...}\left[\begin{array}{c}0\\ 0\\ .\\ .\\ .\\ -1\end{array}\right]\right\}$

e_i表示单位向量，且该单位向量中第i个元素为1；

将误差广义罗德里格参数转换成容积误差四元数为：

$\left\{\begin{array}{c}{\mathrm{\δq}}_{4_{i,k-1}}=\frac{-a\left|\right|{\mathrm{\δp}}_{i,k-1}|{|}^2+b\sqrt{b^2+(1-a^2)\left|\right|{\mathrm{\δp}}_{i,k-1}|{|}^2}}{b^2+\left|\right|{\mathrm{\δp}}_{i,k-1}|{|}^2}\\ {\mathrm{\δ\ρ}}_{i,k-1}=b^{-1}(a+{\mathrm{\δq}}_{4_{i,k-1}}){\mathrm{\δp}}_{i,k-1}\end{array}\right.,i=1,2,\mathrm{...},n---\left(40\right)$

其中，δρ_i,k-1表示第k-1时刻误差四元数δq_i,k-1的矢量部分，表示第k-1时刻误差四元数δq_i,k-1的标量部分；

由容积误差四元数获得一步预测估计的容积四元数点集为：

${\hat{q}}_{i,k|k-1}={\mathrm{\δq}}_{i,k-1}\⊗{\hat{q}}_{k-1},i=1,2,...,n---\left(41\right)$

容积四元数由姿态运动学方程(29)获得，采用解析形式为：

${\hat{q}}_{i,k|k-1}=\left[\begin{array}{cc}cos\left(A\right)I_{3\×3}-\[{\widehat{\mathrm{\ψ}}}_{i,k-1}\×\]& {\widehat{\mathrm{\ψ}}}_{i,k-1}\\ -{\widehat{\mathrm{\ψ}}}_{i,k-1}^T& \cos \left(A\right)\end{array}\right]{\hat{q}}_{i,k-1}---\left(42\right)$

其中：

$A=\frac{1}{2}\left|\right|{\widehat{\mathrm{\ω}}}_{i,k-1}\left|\right|\mathrm{\Δ}t,{\widehat{\mathrm{\ψ}}}_{i,k-1}=\frac{sin\left(A\right){\widehat{\mathrm{\ω}}}_{i,k-1}}{\left|\right|{\widehat{\mathrm{\ω}}}_{i,k-1}\left|\right|}---\left(43\right)$

陀螺的角速度估计为：

${\widehat{\mathrm{\ω}}}_{i,k-1}={\widetilde{\mathrm{\ω}}}_{k-1}-{\mathrm{\β}}_{i,k-1},i=1,2,...,n---\left(44\right)$

其中，i表示取值为1,2,…,n的数，表示第k-1时刻陀螺输出值，β_i,k-1表示第k-1时刻陀螺漂移值；

获得容积四元数一步预测值后，再将其转换为容积误差罗德里格参数；先计算一步预测容积误差四元数得：

${\mathrm{\δq}}_{i,k|k-1}={\hat{q}}_{i,k|k-1}\⊗{\hat{q}}_{k-1}^{-1},i=1,2,...,n---\left(45\right)$

再将容积误差四元数转换为容积误差罗德里格参数为：

${\mathrm{\δp}}_{i,k|k-1}=b\frac{{\mathrm{\δ\ρ}}_{i,k|k-1}}{a+q_{4_{i,k|k-1}}},i=1,2,...,n---\left(46\right)$

在由步骤二所设计的高斯滤波算法，得到一步预测均值

$\mathrm{\δ}{\hat{p}}_{k|k-1}=\[I_{n\×n}+M_{k-1}{\mathrm{\Φ}}_{k-1}\]\frac{1}{2n}\underset{i=1}{\overset{2n}{\Σ}}{\mathrm{\δp}}_{i,k|k-1}---\left(47\right)$

容积点误差一步预测均值为和方差阵P_k|k-1为：

${\hat{x}}_{k|k-1}={\left[\begin{array}{cc}\mathrm{\δ}{\hat{p}}_{k|k-1}^T& {\widehat{\mathrm{\β}}}_{k-1}^T\end{array}\right]}^T---\left(48\right)$

$P_{k|k-1}=(I_{n\×n}+2M_{k-1}{\mathrm{\Φ}}_{k-1}+U_{k-1}{\mathrm{\Φ}}_{k-1}{\mathrm{\Φ}}_{k-1}^T)\×\frac{1}{2n}\underset{i=1}{\overset{2n}{\Σ}}{X}_{i,k}^{*}{\left(X_{i,k}^{*}\right)}^T-{\hat{x}}_{k|k-1}{\left({\hat{x}}_{k|k-1}\right)}^T+Q_{k-1}---\left(49\right)$

其中，是容积点传播值；

(二)量测更新

计算容积点

$X_{i,k}=\sqrt{P_{k|k-1}}{\mathrm{\ζ}}_i+{\hat{x}}_{k|k-1},i=1,...,2n---\left(50\right)$

星敏感器生成观测容积估计点Z_i,k为：

$Z_{i,k}=A\left({\hat{q}}_{i,k|k-1}\right)r,i=1,2,...,n---\left(51\right)$

通过以上容积点得到量测容积点均值

${\hat{z}}_{k|k-1}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\Σ}}{Z}_{i,k}---\left(52\right)$

求取容积滤波增益K_k：

$K_k=P_{xz,k}{P}_{\tilde{z}\tilde{z},k}^{-1}---\left(53\right)$

其中，协方差阵和互协方差阵P_xz,k分别表示如下：

$P_{\tilde{z}\tilde{z},k}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\Σ}}{Z}_{i,k}{Z}_{i,k}^T-{\hat{z}}_{k|k-1}{\hat{z}}_{k|k-1}^T+R_k---\left(54\right)$

$P_{xz,k}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\Σ}}{X}_{i,k}{Z}_{i,k}^T-{\hat{x}}_{k|k-1}{\hat{z}}_{k|k-1}^T---\left(55\right)$

再由式(49)得到状态量的量测更新为：

${\hat{x}}_k={\hat{x}}_{k|k-1}+K_k(z_k-{\hat{z}}_{k|k-1})---\left(56\right)$

其中，

计算更新容积误差四元数和陀螺漂移；容积误差四元数δq_k更新为：

$\left\{\begin{array}{c}{\mathrm{\δq}}_{4_k}=\frac{-a\left|\right|{\mathrm{\δp}}_k^T|{|}^2+b\sqrt{b^2+(1-a^2)\left|\right|{\mathrm{\δp}}_k^T|{|}^2}}{b^2+\left|\right|{\mathrm{\δp}}_k^T|{|}^2}\\ {\mathrm{\δ\ρ}}_k=b^{-1}(a+{\mathrm{\δq}}_{4_k}){\mathrm{\δp}}_k^T\end{array}\right.---\left(57\right)$

得到容积点四元数

${\hat{q}}_k={\mathrm{\δq}}_k\⊗{\hat{q}}_{k|k-1}---\left(58\right)$

协方差阵P_k更新为：

$P_k=P_{k|k-1}-K_k{P}_{\tilde{z}\tilde{z},k}{K}_k^T---\left(59\right).$