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

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

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＝[k0 k1 k2 k3]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]$
设已知在t0时刻，有姿态四元数q0。Δ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以及坐标变化四元数序列q0～4均采用Lagrange插
值函数拟合，则：
ω(t)＝Lωn(t)+Rωn(t)
(1.3)
q(t)＝Lqn(t)+Rqn(t)
其中，Lωn(t)、Lqn(t)分别为角速度ω、四元数q的插值基函数，Rqn(t)、Rωn(t)为对应
的余项，t＝[t0,t4],ti＝t0+iΔt
则(5.2-9)式可写为：

其中：Li(t)——ω对应的Lagrange插值基函数；
Lj(t)——q对应的Lagrange插值基函数。
$\begin{array}{cc}{L}_i\left(t\right)=\underset{i\≠k}{\overset{n}{\underset{k=0}{\mathrm{\Π}}}}\frac{(t-t_k)}{(t_i-t_k)}& i=0,...,n\\ L_j\left(t\right)=\underset{j\≠k}{\overset{n}{\underset{k=0}{\mathrm{\Π}}}}\frac{(t-t_k)}{(t_j-t_k)}& j=0,...,n\end{array}---\left(1.5\right)$
作变换：t＝t0+hx，dt＝hdx，则有：
t0,x＝0
tm,x＝m
t＝t0+hx
tm＝t0+hm＝t0+mΔt
h＝Δt
$\begin{array}{cc}{L}_i\left(x\right)=\underset{i\≠k}{\overset{n}{\underset{k=0}{\mathrm{\Π}}}}\frac{(t_0+\mathrm{hx}-t_0-h.k)}{(t_0+\mathrm{hi}-t_0-h.k)}=\underset{i\≠k}{\overset{n}{\underset{i=0}{\mathrm{\Π}}}}\frac{x-k}{i-k}& i=0,...,n\\ L_j\left(x\right)=\underset{j\≠k}{\overset{n}{\underset{k=0}{\mathrm{\Π}}}}\frac{(t_0+\mathrm{hx}-t_0-h.k)}{(t_0+\mathrm{hj}-t_0-h.k)}=\underset{j\≠k}{\overset{n}{\underset{j=0}{\mathrm{\Π}}}}\frac{x-k}{j-k}& j=0,...,n\end{array}---\left(1.6\right)$
1.2数值积分推导过程
根据四元数的增量式描述，tm时刻的四元数可表示为：

其中：q0——t0时刻的坐标变换四元数；
qm——tm时刻的坐标变换四元数；
$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时，有q0＝q0；m＝1,...,4时，有：
$\left\{\begin{array}{cc}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}{\widetilde{\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＝[q0,q1,q2,q3]；
1.3根据四元数计算欧拉角
则各姿态角计算公式如下：
θ＝arcsin[2(q1q2+q0q3)] (-90°,90°) (1.17)
当$\left|\mathrm{\θ}\right|\<\frac{\mathrm{\π}}{2}$时：
$\mathrm{\ψ}=\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{\π}+\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\end{array}\right.---\left(1.18\right)$
$\mathrm{\γ}=\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{\π}+\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\end{array}\right.---\left(1.19\right)$
当$\left|\mathrm{\θ}\right|=\frac{\mathrm{\π}}{2}$时
ψ=0
$\mathrm{\γ}=\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{\π}+\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\end{array}\right.---(1.20)$
由此便得到了三个姿态角：俯仰角θ、偏航角ψ、滚转角γ。

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

技术领域

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

背景技术

在传统的飞行力学中，因为已假定弹体具备飞行稳定性，弹体攻角的幅值很小，所以，
飞行器相对于参考系的姿态可通过欧拉(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＝[k0 k1 k2 k3]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]$

设已知在t0时刻，有姿态四元数q0。Δ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以及坐标变化四元数序列q0～4均采用Lagrange插
值函数拟合，则：

ω(t)＝Lωn(t)+Rωn(t)

(1.3)

q(t)＝Lqn(t)+Rqn(t)

其中，Lωn(t)、Lqn(t)分别为角速度ω、四元数q的插值基函数，Rqn(t)、Rωn(t)为对应
的余项，t＝[t0,t4],ti＝t0+iΔt

则(5.2-9)式可写为：

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

Lj(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＝t0+hx，dt＝hdx，则有：

t0,x＝0

tm,x＝m

t＝t0+hx

tm＝t0+hm＝t0+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数值积分推导过程

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

其中：q0——t0时刻的坐标变换四元数；

qm——tm时刻的坐标变换四元数；

$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时，有q0＝q0；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＝[q0,q1,q2,q3]；

1.3根据四元数计算欧拉角

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

θ＝arcsin[2(q1q2+q0q3)] (-90°,90°) (1.17)

当$\left|\mathrm{\θ}\right|\<\frac{\mathrm{\π}}{2}$时：

$\mathrm{\ψ}=\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{\π}+\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\end{array}\right.---\left(1.18\right)$

$\mathrm{\γ}=\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{\π}+\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\end{array}\right.---\left(1.19\right)$

当$\left|\mathrm{\θ}\right|=\frac{\mathrm{\π}}{2}$时

ψ＝0

$\mathrm{\γ}=\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{\π}+\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\end{array}\right.---(1.20)$

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

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

具体实施方式

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

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＝[k0 k1 k2 k3]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]$

设已知在t0时刻，有姿态四元数q0。Δ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以及坐标变化四元数序列q0～4均采用Lagrange插
值函数拟合，则：

ω(t)＝Lωn(t)+Rωn(t)

(1.3)

q(t)＝Lqn(t)+Rqn(t)

其中，Lωn(t)、Lqn(t)分别为角速度ω、四元数q的插值基函数，Rqn(t)、Rωn(t)为对应
的余项，t＝[t0,t4],ti＝t0+iΔt

则(5.2-9)式可写为：

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

Lj(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＝t0+hx，dt＝hdx，则有：

t0,x＝0

tm,x＝m

t＝t0+hx

tm＝t0+hm＝t0+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数值积分推导过程

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

其中：q0——t0时刻的坐标变换四元数；

qm——tm时刻的坐标变换四元数；

$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时，有q0＝q0；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＝[q0,q1,q2,q3]；

1.3根据四元数计算欧拉角

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

θ＝arcsin[2(q1q2+q0q3)] (-90°,90°) (1.17)

当$\left|\mathrm{\θ}\right|\<\frac{\mathrm{\π}}{2}$时：

$\mathrm{\ψ}=\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{\π}+\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\end{array}\right.---\left(1.18\right)$

$\mathrm{\γ}=\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{\π}+\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\end{array}\right.---\left(1.19\right)$

当$\left|\mathrm{\θ}\right|=\frac{\mathrm{\π}}{2}$时

ψ＝0

$\mathrm{\γ}=\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{\π}+\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\end{array}\right.---(1.20)$

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