[发明专利]一种高速旋转弹姿态解算用四区间拉格朗日方法

摘要

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

主权项

一种高速旋转弹姿态解算用四区间拉格朗日方法，其特征是：包括下列过程：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为角速度采样时间间隔，设有载体系下测取的角速度序列：时间t₀t₀+Δtt₀+2Δtt₀+3Δtt₀+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\≠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＝t₀+hx，dt＝hdx，则有：t₀,x＝0t_m,x＝mt＝t₀+hxt_m＝t₀+hm＝t₀+mΔth＝Δ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数值积分推导过程根据四元数的增量式描述，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}{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＝[q₀,q₁,q₂,q₃]；1.3根据四元数计算欧拉角则各姿态角计算公式如下：θ＝arcsin[2(q₁q₂+q₀q₃)]    (‑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)$由此便得到了三个姿态角：俯仰角θ、偏航角ψ、滚转角γ。