CN105987695A - 一种高速旋转弹姿态解算用四区间拉格朗日方法 - Google Patents

Abstract

本发明具体涉及一种高速旋转弹姿态解算用四区间拉格朗日方法。本发明主要解决计算复杂的问题，具体方法包括下列过程：角速度与四元数的插值基函数表示、数值积分推导过程。本发明方法采用四元数法描述飞行器的旋转运动，解决了参数退化问题，同时还可减少三角函数的计算量，有利于提高运算速度和精度。因此，与现有技术相比，本发明具有计算简单方便等优点。

Description

一种高速旋转弹姿态解算用四区间拉格朗日方法

技术领域

本发明具体涉及一种高速旋转弹姿态解算用四区间拉格朗日方法。

背景技术

在传统的飞行力学中，因为已假定弹体具备飞行稳定性，弹体攻角的幅值很小，所以，飞行器相对于参考系的姿态可通过欧拉(Euler)角来描述的。三个欧拉角分别为俯仰角θ、偏航角ψ、滚转角γ，则这3个欧拉角的变化率与弹体角速度的关系可表示为如下微分方程形式：

$\left\{\begin{array}{c}\stackrel{\·}{\mathrm{\γ}}={\mathrm{\ω}}_x+(-{\mathrm{\ω}}_y\cos \mathrm{\γ}+{\mathrm{\ω}}_z\sin \mathrm{\γ})\mathrm{tg\θ}\\ \stackrel{\·}{\mathrm{\θ}}={\mathrm{\ω}}_y\sin \mathrm{\γ}+{\mathrm{\ω}}_z\cos \mathrm{\γ}\\ \stackrel{\·}{\mathrm{\ψ}}=({\mathrm{\ω}}_y\cos \mathrm{\γ}-{\mathrm{\ω}}_z\sin \mathrm{\γ})/\cos \mathrm{\θ}\end{array}\right.---\left(1.1\right)$

但是，这种表达方式不适用于大幅度的姿态运动。因为在某些特殊情况下，个别姿态角不确定，运动学方程出现奇异性。例如当俯仰角θ＝90°时，偏航角ψ为不确定，且方程dψ/dt有奇异性。在控制弹道中，因为弹体的攻角可能很大，所以，传统上利用欧拉角的建模方法在精度、稳定性方面已不能满足要求，已不再适用于有控弹道，需要采用新的建模方法^[103]。

六参数法、九参数法和四元数法都是解姿态角变化的有效方法。其中，六参数法和九参数法是求解坐标变换的方向余弦，当然，用方向余弦描述飞行器的旋转运动，在任何情况下都不会退化。但无论是六参数法还是九参数法，其参数都不是全部独立的，它们之间要满足以下6个非线性约束方程：

$\begin{array}{cc}\left.\begin{array}{c}{\mathrm{\α}}_{11}^2+{\mathrm{\α}}_{12}^2+{\mathrm{\α}}_{13}^3=1\\ {\mathrm{\α}}_{21}^2+{\mathrm{\α}}_{22}^2+{\mathrm{\α}}_{23}^2=1\\ {\mathrm{\α}}_{31}^2+{\mathrm{\α}}_{32}^2+{\mathrm{\α}}_{33}^2=1\end{array}\right\}& \left.\begin{array}{c}{\mathrm{\α}}_{11}{\mathrm{\α}}_{21}+{\mathrm{\α}}_{12}{\mathrm{\α}}_{22}+{\mathrm{\α}}_{13}{\mathrm{\α}}_{23}=0\\ {\mathrm{\α}}_{21}{\mathrm{\α}}_{31}+{\mathrm{\α}}_{22}{\mathrm{\α}}_{32}+{\mathrm{\α}}_{23}{\mathrm{\α}}_{33}=0\\ {\mathrm{\α}}_{31}{\mathrm{\α}}_{11}+{\mathrm{\α}}_{32}{\mathrm{\α}}_{12}+{\mathrm{\α}}_{33}{\mathrm{\α}}_{13}=0\end{array}\right\}\end{array}---\left(1.2\right)$

所以，以方向余弦确定动系对定系的位置，只能选定9个参数中的3个为独立参数，而其余6个参数都表示为这3个独立参数的函数。因此，用方向余弦表示刚体旋转运动的方程虽不退化，但参数和联系方程数目较多，计算复杂。

若采用四元数法描述飞行器的旋转运动，不存在参数退化问题，而且，四元数法只有4个参数，1个联系方程，比较简单方便，同时还可减少三角函数的计算量，有利于提高运算速度和精度。

发明内容

本发明的目的是为解决上述现有技术中存在计算复杂的问题，而提供一种高速旋转弹姿态解算用四区间拉格朗日方法。

为解决上述技术问题，本发明所采用的技术方案是：一种高速旋转弹姿态解算用四区间拉格朗日方法，其中：包括下列过程：

1.1角速度与四元数的插值基函数表示

四元数运动微分方程为

或

式中，

q为惯性系至载体系的坐标变换四元数；

ω＝[ω_x ω_y ω_z]^T，表示刚体角速度在在体系中的分量；

$\widehat{\mathrm{\ω}}={\left[\begin{array}{cccc}0& {\mathrm{\ω}}_x& {\mathrm{\ω}}_y& {\mathrm{\ω}}_z\end{array}\right]}^T$

写为矩阵式，有

$\stackrel{\·}{q}=\frac{1}{2}\widetilde{\mathrm{\ω}}q$

其中： $\stackrel{\·}{q}={\left[\begin{array}{cccc}{\stackrel{\·}{k}}_0& {\stackrel{\·}{k}}_1& {\stackrel{\·}{k}}_2& {\stackrel{\·}{k}}_3\end{array}\right]}^T,$ q＝[k₀ k₁ k₂ k₃]^T

$\widetilde{\mathrm{\ω}}=\left[\begin{array}{cccc}0& -{\mathrm{\ω}}_x& -{\mathrm{\ω}}_y& -{\mathrm{\ω}}_z\\ {\mathrm{\ω}}_x& 0& {\mathrm{\ω}}_z& -{\mathrm{\ω}}_y\\ {\mathrm{\ω}}_y& -{\mathrm{\ω}}_z& 0& {\mathrm{\ω}}_x\\ {\mathrm{\ω}}_z& {\mathrm{\ω}}_y& -{\mathrm{\ω}}_x& 0\end{array}\right]$

设已知在t₀时刻，有姿态四元数q₀。Δt为角速度采样时间间隔，设有载体系下测取的角速度序列：

时间	t0	t0+Δt	t0+2Δt	t0+3Δt	t0+4Δt
						角速度	ω0	ω1	ω2	ω3	ω4
姿态四元数	q0	q1	q2	q3	q4

对载体系下测取的角速度序列ω_0～4以及坐标变化四元数序列q_0～4均采用Lagrange插值函数拟合，则：

ω(t)＝L_ωn(t)+R_ωn(t)

(1.3)

q(t)＝L_qn(t)+R_qn(t)

其中，L_ωn(t)、L_qn(t)分别为角速度ω、四元数q的插值基函数，R_qn(t)、R_ωn(t)为对应的余项，t＝[t₀,t₄],t_i＝t₀+iΔt

则(5.2-9)式可写为：

其中：L_i(t)——ω对应的Lagrange插值基函数；

L_j(t)——q对应的Lagrange插值基函数。

$\begin{array}{cc}{L}_i\left(t\right)=\underset{\underset{i\≠l}{k=0}}{\overset{n}{\mathrm{\Π}}}\frac{\left({t-t}_k\right)}{(t_i-t_k)}& i=0,...,n\\ L_j\left(t\right)=\underset{\underset{j\≠k}{k=0}}{\overset{n}{\mathrm{\Π}}}\frac{(t-t_k)}{(t_j-t_k)}& j=0,...,n\end{array}---\left(1.5\right)$

作变换：t＝t₀+hx，dt＝hdx，则有：

t₀,x＝0

t_m,x＝m

t＝t₀+hx

t_m＝t₀+hm＝t₀+mΔt

h＝Δt

$\begin{array}{cc}{L}_i\left(x\right)=\underset{\underset{i\≠k}{k=0}}{\overset{n}{\mathrm{\Π}}}\frac{(t_0+\mathrm{hx}-t_0-h.k)}{(t_0+\mathrm{hi}-t_0-h.k)}=\underset{\underset{i\≠k}{i=0}}{\overset{n}{\mathrm{\Π}}}\frac{x-k}{i-k}& i=0,...,n\\ L_j\left(x\right)=\underset{\underset{j\≠k}{k=0}}{\overset{n}{\mathrm{\Π}}}\frac{(t_0+\mathrm{hx}-t_0-h.k)}{(t_0+\mathrm{hj}-t_0-h.k)}=\underset{\underset{j\≠k}{j=0}}{\overset{n}{\mathrm{\Π}}}\frac{x-k}{j-k}& j=0,...,n\end{array}---\left(1.6\right)$

1.2数值积分推导过程

根据四元数的增量式描述，t_m时刻的四元数可表示为：

其中：q₀——t₀时刻的坐标变换四元数；

q_m——t_m时刻的坐标变换四元数；

$U_{i,j}^m--{\∫}_0^m{L}_i\left(x\right)L_j\left(x\right)\mathrm{dx};$

对于四区间数值积分，令：

${\mathrm{\Ω}}_j^m=\underset{i=0}{\overset{n}{\mathrm{\Σ}}}{U}_{i,j}^m{\mathrm{\ω}}_i,m=0,...,4,j=0,...,4---\left(1.8\right)$

其中： ${\mathrm{\Ω}}_j^0=0,j=0,...,4$

则(1.7)式可进一步描述为：

即：

式(1.10)的矩阵式为

${\hat{q}}_m-\frac{h}{2}\underset{j=0}{\overset{n}{\mathrm{\Σ}}}{\widetilde{\mathrm{\Ω}}}_j^m{\hat{q}}_j={\hat{q}}_0---\left(1.11\right)$

其中，Ω＝[Ω_x Ω_y Ω_z]^T, $\hat{q}={\left[\begin{array}{cccc}{k}_0& k_1& k_2& k_3\end{array}\right]}^T;$

$\widetilde{\mathrm{\Ω}}=\left[\begin{array}{cccc}0& -{\mathrm{\Ω}}_x& -{\mathrm{\Ω}}_y& -{\mathrm{\Ω}}_z\\ {\mathrm{\Ω}}_x& 0& {\mathrm{\Ω}}_z& -{\mathrm{\Ω}}_y\\ {\mathrm{\Ω}}_y& -{\mathrm{\Ω}}_z& 0& {\mathrm{\Ω}}_x\\ {\mathrm{\Ω}}_z& {\mathrm{\Ω}}_y& -{\mathrm{\Ω}}_x& 0\end{array}\right],\tilde{q}=\left[\begin{array}{cccc}{k}_0& {-k}_1& -k_2& -k_3\\ k_1& k_0& -k_3& k_2\\ k_2& k_3& k_0& -k_1\\ k_3& -k_2& k_1& k_0\end{array}\right]$

对于(6.2-19)式，m＝0时，有q₀＝q₀；m＝1,…,4时，有：

$\left\{\begin{array}{c}m=1(E-\frac{h}{2}{\widetilde{\mathrm{\Ω}}}_1^1){\hat{q}}_1-\frac{h}{2}{\widetilde{\mathrm{\Ω}}}_2^1{\hat{q}}_2-\frac{h}{2}{\widetilde{\mathrm{\Ω}}}_3^1{\hat{q}}_3-\frac{h}{2}{\widetilde{\mathrm{\Ω}}}_4^1{\hat{q}}_4=(\frac{h}{2}{\widetilde{\mathrm{\Ω}}}_0^1+E){\hat{q}}_0\\ m=2-\frac{h}{2}{\widetilde{\mathrm{\Ω}}}_1^2{\hat{q}}_1+(E-\frac{h}{2}{\widehat{\mathrm{\Ω}}}_2^2){\hat{q}}_2-\frac{h}{2}{\widetilde{\mathrm{\Ω}}}_3^2{\hat{q}}_3-\frac{h}{2}{\widetilde{\mathrm{\Ω}}}_4^2{\hat{q}}_4=(\frac{h}{2}{\widetilde{\mathrm{\Ω}}}_0^2+E){\hat{q}}_0\\ m=3-\frac{h}{2}{\widetilde{\mathrm{\Ω}}}_1^3{\hat{q}}_1-\frac{h}{2}{\widetilde{\mathrm{\Ω}}}_2^3{\hat{q}}_2+(E-\frac{h}{2}{\widetilde{\mathrm{\Ω}}}_3^3){\hat{q}}_3-\frac{h}{2}{\widetilde{\mathrm{\Ω}}}_4^3{\hat{q}}_4=(\frac{h}{2}{\widetilde{\mathrm{\Ω}}}_0^3+E){\hat{q}}_0\\ m=4-\frac{h}{2}{\widetilde{\mathrm{\Ω}}}_1^4{\hat{q}}_1-\frac{h}{2}{\widetilde{\mathrm{\Ω}}}_2^4{\hat{q}}_2-\frac{h}{2}{\widetilde{\mathrm{\Ω}}}_3^4{\hat{q}}_3+(E-\frac{h}{2}{\widetilde{\mathrm{\Ω}}}_4^4){\hat{q}}_4=(\frac{h}{2}{\widetilde{\mathrm{\Ω}}}_0^4+E){\hat{q}}_0\end{array}\right.---\left(1.12\right)$

将(1.12)写成四元数矩阵形式：

[A][Q]＝[B] (1.13)

其中： $B_m=(\frac{h}{2}{\widetilde{\mathrm{\Ω}}}_0^m+E)q_0,m=1,...,4$

$A_{m,j}=\left\{\begin{array}{cc}-\frac{h}{2}{\widetilde{\mathrm{\Ω}}}_j^m& j\≠m\\ E-\frac{h}{2}{\widetilde{\mathrm{\Ω}}}_m^m& j=m\end{array}\right.$

$Q={\left[\begin{array}{cccc}{\hat{q}}_1& {\hat{q}}_2& {\hat{q}}_3& {\hat{q}}_4\end{array}\right]}^T$

展开(1.14)式，有：

$\left\{\begin{array}{c}{A}_{\mathrm{1,1}}{\hat{q}}_1+A_{\mathrm{1,2}}{\hat{q}}_2+A_{\mathrm{1,3}}{\hat{q}}_3+A_{\mathrm{1,4}}{\hat{q}}_4=B_1\\ A_{2.1}{\hat{q}}_1+A_{2.2}{\hat{q}}_2+A_{\mathrm{2,3}}{\hat{q}}_3+A_{\mathrm{2,4}}{\hat{q}}_4=B_2\\ .\\ .\\ .\\ A_{\mathrm{4,1}}{\hat{q}}_1+A_{\mathrm{4,2}}{\hat{q}}_2+A_{\mathrm{4,3}}{\hat{q}}_3+A_{\mathrm{4,4}}{\hat{q}}_4=B_4\end{array}\right.---\left(1.14\right)$

用消元法求解四元数方程组，完成消元后：

$\left[\begin{array}{cccc}{A}_{11}& & & \\ 0& A_{22}& & \\ 0& 0& A_{33}& \\ 0& 0& 0& A_{44}\end{array}\right]\left[\begin{array}{c}{\hat{q}}_1\\ {\hat{q}}_2\\ {\hat{q}}_3\\ {\hat{q}}_4\end{array}\right]=\left[\begin{array}{c}{C}_1\\ C_2\\ C_3\\ C_4\end{array}\right]---\left(1.15\right)$

回代过程：

$\begin{array}{cc}{A}_{\mathrm{4,4}}{\hat{q}}_4=C_4& {\hat{q}}_4=A_{\mathrm{4,4}}^{-1}{C}_4\\ A_{\mathrm{3,3}}{\hat{q}}_3+A_{\mathrm{3,4}}{\hat{q}}_4=C_3& {\hat{q}}_3=A_{\mathrm{3,3}}^{-1}(C_3-A_{\mathrm{3,4}}{\hat{q}}_4)\\ {\hat{q}}_2=A_{\mathrm{2,2}}^{-1}(C_2-A_{\mathrm{2,4}}{\hat{q}}_4-A_{\mathrm{2,3}}{\hat{q}}_3)& \\ {\hat{q}}_1=A_{\mathrm{1,1}}^{-1}(C_1-A_{\mathrm{1,4}}{\hat{q}}_4-A_{\mathrm{1,3}}{\hat{q}}_3-A_{\mathrm{1,2}}{\hat{q}}_2)& \end{array}---\left(1.16\right)$

由此求得了四元数q，q＝[q₀,q₁,q₂,q₃]；

1.3根据四元数计算欧拉角

则各姿态角计算公式如下：

θ＝arcsin[2(q₁q₂+q₀q₃)] (-90°,90°) (1.17)

当 $\left|\mathrm{\&theta;}\right|<\frac{\mathrm{\&pi;}}{2}$ 时：

$\mathrm{\&psi;}=\left\{\begin{array}{cc}\arctan \left[\frac{-2(q_1{q}_3-q_0{q}_2)}{q_0^2+q_1^2-q_2^0-q_3^2}\right]& a_{11}=q_0^2+q_1^2-q_2^2-q_3^2>0\\ \mathrm{\&pi;}+\arctan \left[\frac{-2(q_1{q}_3-q_0{q}_2)}{q_0^2+q_1^2-q_2^2-q_3^2}\right]& a_{11}=q_0^2+q_1^2-q_2^2-q_3^2\&le;0\end{array}\right.---\left(1.18\right)$

$\mathrm{\&gamma;}=\left\{\begin{array}{cc}\arctan \left[\frac{-2(q_2{q}_3-q_0{q}_1)}{q_0^2-q_1^2+q_2^2-q_3^2}\right]& a_{22}=q_0^2-q_1^2+q_2^2-q_3^2>0\\ \mathrm{\&pi;}+\arctan \left[\frac{-2(q_2{q}_3-q_0{q}_1)}{q_0^2-q_1^2+q_2^2-q_3^2}\right]& a_{22}=q_0^2-q_1^2+q_2^2-q_3^2\&le;0\end{array}\right.---\left(1.19\right)$

当 $\left|\mathrm{\&theta;}\right|=\frac{\mathrm{\&pi;}}{2}$ 时

ψ＝0

$\mathrm{\&gamma;}=\left\{\begin{array}{cc}\arctan \frac{2(q_2{q}_3-q_0{q}_1)}{q_0^2-q_1^2+q_2^2-q_3^2}& a_{33}=q_0^2-q_1^2-q_2^2+q_3^2>0\\ \mathrm{\&pi;}+\arctan \frac{2(q_2{q}_3-q_0{q}_1)}{q_0^2-q_1^2+q_2^2-q_3^2}& a_{33}=q_0^2-q_1^2-q_2^2+q_3^2\&le;0\end{array}\right.---(1.20)$

由此便得到了三个姿态角：俯仰角θ、偏航角ψ、滚转角γ。

由于本发明方法采用四元数法描述飞行器的旋转运动，解决了参数退化问题，同时还可减少三角函数的计算量，有利于提高运算速度和精度。因此，与现有技术相比，本发明具有计算简单方便等优点。

具体实施方式

本实施例一种高速旋转弹姿态解算用四区间拉格朗日方法，其特征是：包括下列过程：

1.1角速度与四元数的插值基函数表示

四元数运动微分方程为

或

式中，

q为惯性系至载体系的坐标变换四元数；

ω＝[ω_x ω_y ω_z]^T，表示刚体角速度在在体系中的分量；

$\widehat{\mathrm{\&omega;}}={\left[\begin{array}{cccc}0& {\mathrm{\&omega;}}_x& {\mathrm{\&omega;}}_y& {\mathrm{\&omega;}}_z\end{array}\right]}^T$

写为矩阵式，有

$\stackrel{\&CenterDot;}{q}=\frac{1}{2}\widetilde{\mathrm{\&omega;}}q$

其中： $\stackrel{\&CenterDot;}{q}={\left[\begin{array}{cccc}{\stackrel{\&CenterDot;}{k}}_0& {\stackrel{\&CenterDot;}{k}}_1& {\stackrel{\&CenterDot;}{k}}_2& {\stackrel{\&CenterDot;}{k}}_3\end{array}\right]}^T,$ q＝[k₀ k₁ k₂ k₃]^T

$\widetilde{\mathrm{\&omega;}}=\left[\begin{array}{cccc}0& -{\mathrm{\&omega;}}_x& -{\mathrm{\&omega;}}_y& -{\mathrm{\&omega;}}_z\\ {\mathrm{\&omega;}}_x& 0& {\mathrm{\&omega;}}_z& -{\mathrm{\&omega;}}_y\\ {\mathrm{\&omega;}}_y& -{\mathrm{\&omega;}}_z& 0& {\mathrm{\&omega;}}_x\\ {\mathrm{\&omega;}}_z& {\mathrm{\&omega;}}_y& -{\mathrm{\&omega;}}_x& 0\end{array}\right]$

设已知在t₀时刻，有姿态四元数q₀。Δt为角速度采样时间间隔，设有载体系下测取的角速度序列：

时间	t0	t0+Δt	t0+2Δt	t0+3Δt	t0+4Δt
						角速度	ω0	ω1	ω2	ω3	ω4
姿态四元数	q0	q1	q2	q3	q4

对载体系下测取的角速度序列ω_0～4以及坐标变化四元数序列q_0～4均采用Lagrange插值函数拟合，则：

ω(t)＝L_ωn(t)+R_ωn(t)

(1.3)

q(t)＝L_qn(t)+R_qn(t)

其中，L_ωn(t)、L_qn(t)分别为角速度ω、四元数q的插值基函数，R_qn(t)、R_ωn(t)为对应的余项，t＝[t₀,t₄],t_i＝t₀+iΔt

则(5.2-9)式可写为：

其中：L_i(t)——ω对应的Lagrange插值基函数；

L_j(t)——q对应的Lagrange插值基函数。

$\begin{array}{cc}{L}_i\left(t\right)=\underset{\underset{i\&NotEqual;l}{k=0}}{\overset{n}{\mathrm{\&Pi;}}}\frac{\left({t-t}_k\right)}{(t_i-t_k)}& i=0,...,n\\ L_j\left(t\right)=\underset{\underset{j\&NotEqual;k}{k=0}}{\overset{n}{\mathrm{\&Pi;}}}\frac{(t-t_k)}{(t_j-t_k)}& j=0,...,n\end{array}---\left(1.5\right)$

作变换：t＝t₀+hx，dt＝hdx，则有：

t₀,x＝0

t_m,x＝m

t＝t₀+hx

t_m＝t₀+hm＝t₀+mΔt

h＝Δt

$\begin{array}{cc}{L}_i\left(x\right)=\underset{\underset{i\&NotEqual;k}{k=0}}{\overset{n}{\mathrm{\&Pi;}}}\frac{(t_0+\mathrm{hx}-t_0-h.k)}{(t_0+\mathrm{hi}-t_0-h.k)}=\underset{\underset{i\&NotEqual;k}{i=0}}{\overset{n}{\mathrm{\&Pi;}}}\frac{x-k}{i-k}& i=0,...,n\\ L_j\left(x\right)=\underset{\underset{j\&NotEqual;k}{k=0}}{\overset{n}{\mathrm{\&Pi;}}}\frac{(t_0+\mathrm{hx}-t_0-h.k)}{(t_0+\mathrm{hj}-t_0-h.k)}=\underset{\underset{j\&NotEqual;k}{j=0}}{\overset{n}{\mathrm{\&Pi;}}}\frac{x-k}{j-k}& j=0,...,n\end{array}---\left(1.6\right)$

1.2数值积分推导过程

根据四元数的增量式描述，t_m时刻的四元数可表示为：

其中：q₀——t₀时刻的坐标变换四元数；

q_m——t_m时刻的坐标变换四元数；

$U_{i,j}^m--{\&Integral;}_0^m{L}_i\left(x\right)L_j\left(x\right)\mathrm{dx};$

对于四区间数值积分，令：

${\mathrm{\&Omega;}}_j^m=\underset{i=0}{\overset{n}{\mathrm{\&Sigma;}}}{U}_{i,j}^m{\mathrm{\&omega;}}_i,m=0,...,4,j=0,...,4---\left(1.8\right)$

其中： ${\mathrm{\&Omega;}}_j^0=0,j=0,...,4$

则(1.7)式可进一步描述为：

即：

式(1.10)的矩阵式为

${\hat{q}}_m-\frac{h}{2}\underset{j=0}{\overset{n}{\mathrm{\&Sigma;}}}{\widetilde{\mathrm{\&Omega;}}}_j^m{\hat{q}}_j={\hat{q}}_0---\left(1.11\right)$

其中，Ω＝[Ω_x Ω_y Ω_z]^T, $\hat{q}={\left[\begin{array}{cccc}{k}_0& k_1& k_2& k_3\end{array}\right]}^T;$

$\widetilde{\mathrm{\&Omega;}}=\left[\begin{array}{cccc}0& -{\mathrm{\&Omega;}}_x& -{\mathrm{\&Omega;}}_y& -{\mathrm{\&Omega;}}_z\\ {\mathrm{\&Omega;}}_x& 0& {\mathrm{\&Omega;}}_z& -{\mathrm{\&Omega;}}_y\\ {\mathrm{\&Omega;}}_y& -{\mathrm{\&Omega;}}_z& 0& {\mathrm{\&Omega;}}_x\\ {\mathrm{\&Omega;}}_z& {\mathrm{\&Omega;}}_y& -{\mathrm{\&Omega;}}_x& 0\end{array}\right],\tilde{q}=\left[\begin{array}{cccc}{k}_0& {-k}_1& -k_2& -k_3\\ k_1& k_0& -k_3& k_2\\ k_2& k_3& k_0& -k_1\\ k_3& -k_2& k_1& k_0\end{array}\right]$

对于(6.2-19)式，m＝0时，有q₀＝q₀；m＝1,…,4时，有：

$\left\{\begin{array}{c}m=1(E-\frac{h}{2}{\widetilde{\mathrm{\&Omega;}}}_1^1){\hat{q}}_1-\frac{h}{2}{\widetilde{\mathrm{\&Omega;}}}_2^1{\hat{q}}_2-\frac{h}{2}{\widetilde{\mathrm{\&Omega;}}}_3^1{\hat{q}}_3-\frac{h}{2}{\widetilde{\mathrm{\&Omega;}}}_4^1{\hat{q}}_4=(\frac{h}{2}{\widetilde{\mathrm{\&Omega;}}}_0^1+E){\hat{q}}_0\\ m=2-\frac{h}{2}{\widetilde{\mathrm{\&Omega;}}}_1^2{\hat{q}}_1+(E-\frac{h}{2}{\widehat{\mathrm{\&Omega;}}}_2^2){\hat{q}}_2-\frac{h}{2}{\widetilde{\mathrm{\&Omega;}}}_3^2{\hat{q}}_3-\frac{h}{2}{\widetilde{\mathrm{\&Omega;}}}_4^2{\hat{q}}_4=(\frac{h}{2}{\widetilde{\mathrm{\&Omega;}}}_0^2+E){\hat{q}}_0\\ m=3-\frac{h}{2}{\widetilde{\mathrm{\&Omega;}}}_1^3{\hat{q}}_1-\frac{h}{2}{\widetilde{\mathrm{\&Omega;}}}_2^3{\hat{q}}_2+(E-\frac{h}{2}{\widetilde{\mathrm{\&Omega;}}}_3^3){\hat{q}}_3-\frac{h}{2}{\widetilde{\mathrm{\&Omega;}}}_4^3{\hat{q}}_4=(\frac{h}{2}{\widetilde{\mathrm{\&Omega;}}}_0^3+E){\hat{q}}_0\\ m=4-\frac{h}{2}{\widetilde{\mathrm{\&Omega;}}}_1^4{\hat{q}}_1-\frac{h}{2}{\widetilde{\mathrm{\&Omega;}}}_2^4{\hat{q}}_2-\frac{h}{2}{\widetilde{\mathrm{\&Omega;}}}_3^4{\hat{q}}_3+(E-\frac{h}{2}{\widetilde{\mathrm{\&Omega;}}}_4^4){\hat{q}}_4=(\frac{h}{2}{\widetilde{\mathrm{\&Omega;}}}_0^4+E){\hat{q}}_0\end{array}\right.---\left(1.12\right)$

将(1.12)写成四元数矩阵形式：

[A][Q]＝[B] (1.13)

其中： $B_m=(\frac{h}{2}{\widetilde{\mathrm{\&Omega;}}}_0^m+E)q_0,m=1,...,4$

$A_{m,j}=\left\{\begin{array}{cc}-\frac{h}{2}{\widetilde{\mathrm{\&Omega;}}}_j^m& j\&NotEqual;m\\ E-\frac{h}{2}{\widetilde{\mathrm{\&Omega;}}}_m^m& j=m\end{array}\right.$

$Q={\left[\begin{array}{cccc}{\hat{q}}_1& {\hat{q}}_2& {\hat{q}}_3& {\hat{q}}_4\end{array}\right]}^T$

展开(1.14)式，有：

$\left\{\begin{array}{c}{A}_{\mathrm{1,1}}{\hat{q}}_1+A_{\mathrm{1,2}}{\hat{q}}_2+A_{\mathrm{1,3}}{\hat{q}}_3+A_{\mathrm{1,4}}{\hat{q}}_4=B_1\\ A_{2.1}{\hat{q}}_1+A_{2.2}{\hat{q}}_2+A_{\mathrm{2,3}}{\hat{q}}_3+A_{\mathrm{2,4}}{\hat{q}}_4=B_2\\ .\\ .\\ .\\ A_{\mathrm{4,1}}{\hat{q}}_1+A_{\mathrm{4,2}}{\hat{q}}_2+A_{\mathrm{4,3}}{\hat{q}}_3+A_{\mathrm{4,4}}{\hat{q}}_4=B_4\end{array}\right.---\left(1.14\right)$

用消元法求解四元数方程组，完成消元后：

$\left[\begin{array}{cccc}{A}_{11}& & & \\ 0& A_{22}& & \\ 0& 0& A_{33}& \\ 0& 0& 0& A_{44}\end{array}\right]\left[\begin{array}{c}{\hat{q}}_1\\ {\hat{q}}_2\\ {\hat{q}}_3\\ {\hat{q}}_4\end{array}\right]=\left[\begin{array}{c}{C}_1\\ C_2\\ C_3\\ C_4\end{array}\right]---\left(1.15\right)$

回代过程：

$\begin{array}{cc}{A}_{\mathrm{4,4}}{\hat{q}}_4=C_4& {\hat{q}}_4=A_{\mathrm{4,4}}^{-1}{C}_4\\ A_{\mathrm{3,3}}{\hat{q}}_3+A_{\mathrm{3,4}}{\hat{q}}_4=C_3& {\hat{q}}_3=A_{\mathrm{3,3}}^{-1}(C_3-A_{\mathrm{3,4}}{\hat{q}}_4)\\ {\hat{q}}_2=A_{\mathrm{2,2}}^{-1}(C_2-A_{\mathrm{2,4}}{\hat{q}}_4-A_{\mathrm{2,3}}{\hat{q}}_3)& \\ {\hat{q}}_1=A_{\mathrm{1,1}}^{-1}(C_1-A_{\mathrm{1,4}}{\hat{q}}_4-A_{\mathrm{1,3}}{\hat{q}}_3-A_{\mathrm{1,2}}{\hat{q}}_2)& \end{array}---\left(1.16\right)$

由此求得了四元数q，q＝[q₀,q₁,q₂,q₃]；

1.3根据四元数计算欧拉角

则各姿态角计算公式如下：

θ＝arcsin[2(q₁q₂+q₀q₃)] (-90°,90°) (1.17)

当 $\left|\mathrm{\&theta;}\right|<\frac{\mathrm{\&pi;}}{2}$ 时：

$\mathrm{\&psi;}=\left\{\begin{array}{cc}\arctan \left[\frac{-2(q_1{q}_3-q_0{q}_2)}{q_0^2+q_1^2-q_2^0-q_3^2}\right]& a_{11}=q_0^2+q_1^2-q_2^2-q_3^2>0\\ \mathrm{\&pi;}+\arctan \left[\frac{-2(q_1{q}_3-q_0{q}_2)}{q_0^2+q_1^2-q_2^2-q_3^2}\right]& a_{11}=q_0^2+q_1^2-q_2^2-q_3^2\&le;0\end{array}\right.---\left(1.18\right)$

$\mathrm{\&gamma;}=\left\{\begin{array}{cc}\arctan \left[\frac{-2(q_2{q}_3-q_0{q}_1)}{q_0^2-q_1^2+q_2^2-q_3^2}\right]& a_{22}=q_0^2-q_1^2+q_2^2-q_3^2>0\\ \mathrm{\&pi;}+\arctan \left[\frac{-2(q_2{q}_3-q_0{q}_1)}{q_0^2-q_1^2+q_2^2-q_3^2}\right]& a_{22}=q_0^2-q_1^2+q_2^2-q_3^2\&le;0\end{array}\right.---\left(1.19\right)$

当 $\left|\mathrm{\&theta;}\right|=\frac{\mathrm{\&pi;}}{2}$ 时

ψ＝0

$\mathrm{\&gamma;}=\left\{\begin{array}{cc}\arctan \frac{2(q_2{q}_3-q_0{q}_1)}{q_0^2-q_1^2+q_2^2-q_3^2}& a_{33}=q_0^2-q_1^2-q_2^2+q_3^2>0\\ \mathrm{\&pi;}+\arctan \frac{2(q_2{q}_3-q_0{q}_1)}{q_0^2-q_1^2+q_2^2-q_3^2}& a_{33}=q_0^2-q_1^2-q_2^2+q_3^2\&le;0\end{array}\right.---(1.20)$

由此便得到了三个姿态角：俯仰角θ、偏航角ψ、滚转角γ。

Claims (1)

1.一种高速旋转弹姿态解算用四区间拉格朗日方法，其特征是：包括下列过程：

1.1角速度与四元数的插值基函数表示

四元数运动微分方程为

或

式中，

q为惯性系至载体系的坐标变换四元数；

ω＝[ω_x ω_y ω_z]^T，表示刚体角速度在在体系中的分量；

$\widehat{\mathrm{\&omega;}}={\left[\begin{array}{cccc}0& {\mathrm{\&omega;}}_x& {\mathrm{\&omega;}}_y& {\mathrm{\&omega;}}_z\end{array}\right]}^T$

写为矩阵式，有

$\stackrel{\&CenterDot;}{q}=\frac{1}{2}\widetilde{\mathrm{\&omega;}}q$

其中： $\stackrel{\&CenterDot;}{q}={\left[\begin{array}{cccc}{\stackrel{\&CenterDot;}{k}}_0& {\stackrel{\&CenterDot;}{k}}_1& {\stackrel{\&CenterDot;}{k}}_2& {\stackrel{\&CenterDot;}{k}}_3\end{array}\right]}^T,$ q＝[k₀ k₁ k₂ k₃]^T

$\widetilde{\mathrm{\&omega;}}=\left[\begin{array}{cccc}0& -{\mathrm{\&omega;}}_x& -{\mathrm{\&omega;}}_y& -{\mathrm{\&omega;}}_z\\ {\mathrm{\&omega;}}_x& 0& {\mathrm{\&omega;}}_z& -{\mathrm{\&omega;}}_y\\ {\mathrm{\&omega;}}_y& -{\mathrm{\&omega;}}_z& 0& {\mathrm{\&omega;}}_x\\ {\mathrm{\&omega;}}_z& {\mathrm{\&omega;}}_y& -{\mathrm{\&omega;}}_x& 0\end{array}\right]$

设已知在t₀时刻，有姿态四元数q₀。Δt为角速度采样时间间隔，设有载体系下测取的角速度序列：

时间 t₀ t₀+Δt t₀+2Δt t₀+3Δt t₀+4Δt 角速度 ω₀ ω₁ ω₂ ω₃ ω₄ 姿态四元数 q₀ q₁ q₂ q₃ q₄

对载体系下测取的角速度序列ω_0～4以及坐标变化四元数序列q_0～4均采用Lagrange插值函数拟合，则：

ω(t)＝L_ωn(t)+R_ωn(t)

(1.3)

q(t)＝L_qn(t)+R_qn(t)

其中，L_ωn(t)、L_qn(t)分别为角速度ω、四元数q的插值基函数，R_qn(t)、R_ωn(t)为对应的余项，t＝[t₀,t₄],t_i＝t₀+iΔt

则(5.2-9)式可写为：

其中：L_i(t)——ω对应的Lagrange插值基函数；

L_j(t)——q对应的Lagrange插值基函数。

$\begin{array}{cc}{L}_i\left(t\right)=\underset{i\&NotEqual;k}{\overset{n}{\underset{k=0}{\mathrm{\&Pi;}}}}\frac{(t-t_k)}{(t_i-t_k)}& i=0,...,n\\ L_j\left(t\right)=\underset{j\&NotEqual;k}{\overset{n}{\underset{k=0}{\mathrm{\&Pi;}}}}\frac{(t-t_k)}{(t_j-t_k)}& j=0,...,n\end{array}---\left(1.5\right)$

作变换：t＝t₀+hx，dt＝hdx，则有：

t₀,x＝0

t_m,x＝m

t＝t₀+hx

t_m＝t₀+hm＝t₀+mΔt

h＝Δt

$\begin{array}{cc}{L}_i\left(x\right)=\underset{i\&NotEqual;k}{\overset{n}{\underset{k=0}{\mathrm{\&Pi;}}}}\frac{(t_0+\mathrm{hx}-t_0-h.k)}{(t_0+\mathrm{hi}-t_0-h.k)}=\underset{i\&NotEqual;k}{\overset{n}{\underset{i=0}{\mathrm{\&Pi;}}}}\frac{x-k}{i-k}& i=0,...,n\\ L_j\left(x\right)=\underset{j\&NotEqual;k}{\overset{n}{\underset{k=0}{\mathrm{\&Pi;}}}}\frac{(t_0+\mathrm{hx}-t_0-h.k)}{(t_0+\mathrm{hj}-t_0-h.k)}=\underset{j\&NotEqual;k}{\overset{n}{\underset{j=0}{\mathrm{\&Pi;}}}}\frac{x-k}{j-k}& j=0,...,n\end{array}---\left(1.6\right)$

1.2数值积分推导过程

根据四元数的增量式描述，t_m时刻的四元数可表示为：

其中：q₀——t₀时刻的坐标变换四元数；

q_m——t_m时刻的坐标变换四元数；

$U_{i,j}^m--{\&Integral;}_0^m{L}_i\left(x\right)L_j\left(x\right)\mathrm{dx};$

对于四区间数值积分，令：

${\mathrm{\&Omega;}}_j^m=\underset{i=0}{\overset{n}{\mathrm{\&Sigma;}}}{U}_{i,j}^m{\mathrm{\&omega;}}_i,m=0,...,4,j=0,...,4---\left(1.8\right)$

其中： ${\mathrm{\&Omega;}}_j^0=0,j=0,...,4$

则(1.7)式可进一步描述为：

即：

式(1.10)的矩阵式为

${\hat{q}}_m-\frac{h}{2}\underset{j=0}{\overset{n}{\mathrm{\&Sigma;}}}{\widetilde{\mathrm{\&Omega;}}}_j^m{\hat{q}}_j={\hat{q}}_0---\left(1.11\right)$

其中，Ω＝[Ω_x Ω_y Ω_z]^T, $\hat{q}={\left[\begin{array}{cccc}{k}_0& k_1& k_2& k_3\end{array}\right]}^T;$

$\widetilde{\mathrm{\&Omega;}}=\left[\begin{array}{cccc}0& -{\mathrm{\&Omega;}}_x& -{\mathrm{\&Omega;}}_y& -{\mathrm{\&Omega;}}_z\\ {\mathrm{\&Omega;}}_x& 0& {\mathrm{\&Omega;}}_z& -{\mathrm{\&Omega;}}_y\\ {\mathrm{\&Omega;}}_y& -{\mathrm{\&Omega;}}_z& 0& {\mathrm{\&Omega;}}_x\\ {\mathrm{\&Omega;}}_z& {\mathrm{\&Omega;}}_y& -{\mathrm{\&Omega;}}_x& 0\end{array}\right],\tilde{q}=\left[\begin{array}{cccc}{k}_0& -k_1& -k_2& -k_3\\ k_1& k_0& -k_3& k_2\\ k_2& k_3& k_0& -k_1\\ k_3& -k_2& k_1& k_0\end{array}\right]$

对于(6.2-19)式，m＝0时，有q₀＝q₀；m＝1,...,4时，有：

$\left\{\begin{array}{cc}m=1& (E-\frac{h}{2}{\widetilde{\mathrm{\&Omega;}}}_1^1){\hat{q}}_1-\frac{h}{2}{\widetilde{\mathrm{\&Omega;}}}_2^1{\hat{q}}_2-\frac{h}{2}{\widetilde{\mathrm{\&Omega;}}}_3^1{\hat{q}}_3-\frac{h}{2}{\widetilde{\mathrm{\&Omega;}}}_4^1{\hat{q}}_4=(\frac{h}{2}{\widetilde{\mathrm{\&Omega;}}}_0^1+E){\hat{q}}_0\\ m=2& -\frac{h}{2}{\widetilde{\mathrm{\&Omega;}}}_1^2{\hat{q}}_1+(E-\frac{h}{2}{\widetilde{\mathrm{\&Omega;}}}_2^2){\hat{q}}_2-\frac{h}{2}{\widetilde{\mathrm{\&Omega;}}}_3^2{\hat{q}}_3-\frac{h}{2}{\widetilde{\mathrm{\&Omega;}}}_4^2{\hat{q}}_4=(\frac{h}{2}{\widetilde{\mathrm{\&Omega;}}}_0^2+E){\hat{q}}_0\\ m=3& -\frac{h}{2}{\widetilde{\mathrm{\&Omega;}}}_1^3{\hat{q}}_1-\frac{h}{2}{\widetilde{\mathrm{\&Omega;}}}_2^3{\hat{q}}_2+(E-\frac{h}{2}{\widetilde{\mathrm{\&Omega;}}}_3^3){\hat{q}}_3-\frac{h}{2}{\widetilde{\mathrm{\&Omega;}}}_4^3{\hat{q}}_4=(\frac{h}{2}{\widetilde{\mathrm{\&Omega;}}}_0^3+E){\hat{q}}_0\\ m=4& -\frac{h}{2}{\widetilde{\mathrm{\&Omega;}}}_1^4{\hat{q}}_1-\frac{h}{2}{\widetilde{\mathrm{\&Omega;}}}_2^4{\hat{q}}_2-\frac{h}{2}{\widetilde{\mathrm{\&Omega;}}}_3^4{\hat{q}}_3+(E-\frac{h}{2}{\widetilde{\mathrm{\&Omega;}}}_4^4){\hat{q}}_4=(\frac{h}{2}{\widetilde{\mathrm{\&Omega;}}}_0^4+E){\hat{q}}_0\end{array}\right.---\left(1.12\right)$

将(1.12)写成四元数矩阵形式：

[A] [Q]＝[B] (1.13)

其中： $B_m=(\frac{h}{2}{\widetilde{\mathrm{\&Omega;}}}_0^m+E)q_0,m=1,...,4$

$A_{m,j}=\left\{\begin{array}{cc}-\frac{h}{2}{\widetilde{\mathrm{\&Omega;}}}_j^m& j\&NotEqual;m\\ E-\frac{h}{2}{\widetilde{\mathrm{\&Omega;}}}_m^m& j=m\end{array}\right.$

$Q={\left[\begin{array}{cccc}{\hat{q}}_1& {\hat{q}}_2& {\hat{q}}_3& {\hat{q}}_4\end{array}\right]}^T$

展开(1.14)式，有：

$\left\{\begin{array}{c}{A}_{\mathrm{1,1}}{\hat{q}}_1+A_{\mathrm{1,2}}{\hat{q}}_2+A_{\mathrm{1,3}}{\hat{q}}_3+A_{\mathrm{1,4}}{\hat{q}}_4=B_1\\ A_{2.1}{\hat{q}}_1+A_{2.2}{\hat{q}}_2+A_{\mathrm{2,3}}{\hat{q}}_3+A_{\mathrm{2,4}}{\hat{q}}_4=B_2\\ .\\ .\\ .\\ A_{\mathrm{4,1}}{\hat{q}}_1+A_{\mathrm{4,2}}{\hat{q}}_2+A_{\mathrm{4,3}}{\hat{q}}_3+A_{\mathrm{4,4}}{\hat{q}}_4=B_4\end{array}\right.---\left(1.14\right)$

用消元法求解四元数方程组，完成消元后：

$\left[\begin{array}{cccc}{A}_{11}& & & \\ 0& A_{22}& & \\ 0& 0& A_{33}& \\ 0& 0& 0& A_{44}\end{array}\right]\left[\begin{array}{c}{\hat{q}}_1\\ {\hat{q}}_2\\ {\hat{q}}_3\\ {\hat{q}}_4\end{array}\right]=\left[\begin{array}{c}{C}_1\\ C_2\\ C_3\\ C_4\end{array}\right]---\left(1.15\right)$

回代过程：

$\begin{array}{cc}{A}_{\mathrm{4,4}}{\hat{q}}_4=C_4& {\hat{q}}_4=A_{\mathrm{4,4}}^{-1}{C}_4\\ A_{\mathrm{3,3}}{\hat{q}}_3+A_{\mathrm{3,4}}{\hat{q}}_4=C_3& {\hat{q}}_3=A_{\mathrm{3,3}}^{-1}(C_3-A_{\mathrm{3,4}}{\hat{q}}_4)\\ {\hat{q}}_2=A_{\mathrm{2,2}}^{-1}(C_2-A_{\mathrm{2,4}}{\hat{q}}_4-A_{\mathrm{2,3}}{\hat{q}}_3)& \\ {\hat{q}}_1=A_{\mathrm{1,1}}^{-1}(C_1-A_{\mathrm{1,4}}{\hat{q}}_4-A_{\mathrm{1,3}}{\hat{q}}_3-A_{\mathrm{1,2}}{\hat{q}}_2)& \end{array}---\left(1.16\right)$

由此求得了四元数q，q＝[q₀,q₁,q₂,q₃]；

1.3根据四元数计算欧拉角

则各姿态角计算公式如下：

θ＝arcsin[2(q₁q₂+q₀q₃)] (-90°,90°) (1.17)

当 $\left|\mathrm{\&theta;}\right|<\frac{\mathrm{\&pi;}}{2}$ 时：

$\mathrm{\&psi;}=\left\{\begin{array}{cc}\arctan \left[\frac{-2(q_1{q}_3-q_0{q}_2)}{q_0^2+q_1^2-q_2^2-q_3^2}\right]& a_{11}=q_0^2+q_1^2-q_2^2-q_3^2>0\\ \mathrm{\&pi;}+\arctan \left[\frac{-2(q_1{q}_3-q_0{q}_2)}{q_0^2+q_1^2-q_2^2-q_3^2}\right]& a_{11}=q_0^2+q_1^2-q_2^2-q_3^2\&le;0\end{array}\right.---\left(1.18\right)$

$\mathrm{\&gamma;}=\left\{\begin{array}{cc}\arctan \left[\frac{-2(q_2{q}_3-q_0{q}_1)}{q_0^2-q_1^2+q_2^2-q_3^2}\right]& a_{22}=q_0^2-q_1^2+q_2^2-q_3^2>0\\ \mathrm{\&pi;}+\arctan \left[\frac{-2(q_2{q}_3-q_0{q}_1)}{q_0^2-q_1^2+q_2^2-q_3^2}\right]& a_{22}=q_0^2-q_1^2+q_2^2-q_3^2\&le;0\end{array}\right.---\left(1.19\right)$

当 $\left|\mathrm{\&theta;}\right|=\frac{\mathrm{\&pi;}}{2}$ 时

ψ=0

$\mathrm{\&gamma;}=\left\{\begin{array}{cc}\arctan \frac{2(q_2{q}_3-q_0{q}_1)}{q_0^2-q_1^2+q_2^2-q_3^2}& a_{33}=q_0^2-q_1^2-q_2^2+q_3^2>0\\ \mathrm{\&pi;}+\arctan \frac{2(q_2{q}_3-q_0{q}_1)}{q_0^2-q_1^2+q_2^2-q_3^2}& a_{33}=q_0^2-q_1^2-q_2^2+q_3^2\&le;0\end{array}\right.---(1.20)$

由此便得到了三个姿态角：俯仰角θ、偏航角ψ、滚转角γ。