基于角速度的飞行器极限飞行时四元数拉盖尔近似输出方法

摘要

本发明公开了一种基于角速度的飞行器极限飞行时四元数拉盖尔近似输出方法，用于解决现有的飞行器极限飞行时惯性设备输出四元数精度差的技术问题。技术方案是采用Shifted Laguerre正交多项式对滚转、俯仰、偏航角速度p，q，r进行近似逼近描述，直接得到了四元数状态转移矩阵，从而保证了确定四元数的迭代计算精度；本发明可以根据工程精度的要求，确定对滚转、俯仰、偏航角速度p，q，r的Shifted Laguerre正交多项式阶次，实现对四元数状态方程转移矩阵Φe[(k+1)T，kT]的超线性逼近，保证了确定四元数的迭代计算精度，从而提高了飞行器极限飞行时惯性设备输出精度。

主权项

1.一种基于角速度的飞行器极限飞行时四元数拉盖尔近似输出方法，其特征在于包括以下步骤：根据四元数连续状态方程$\stackrel{·}{e}=A_{e}e$和离散状态方程e(k+1)＝Φ_e[(k+1)T，kT]e(k)其中e＝[e₁，e₂，e₃，e₄]^T$A_e=\frac{1}{2}\left[\begin{array}{cccc}0& -p& -q& -r\\ p& 0& r& -q\\ q& -r& 0& p\\ r& q& -p& 0\end{array}\right]$Φ_e[(k+1)T，kT]为A_e的状态转移矩阵，T为采样周期，全文符号定义相同；p，q，r分别为滚转、俯仰、偏航角速度；欧拉角θ，ψ分别指滚转、俯仰、偏航角；状态转移矩阵按照逼近式${\Phi }_e[(k+1)T,\mathrm{kT}]\approx I+\mathrm{\Pi H\xi }\left(t\right){|}_{\mathrm{kT}}^{(k+1)T}+\Pi {\int }_{\mathrm{kT}}^{(k+1)T}\left[\xi \left(t\right){\xi }^T\left(t\right)\right]\mathrm{dt}{H}^T{\Pi }_1-\mathrm{\Pi H\xi }\left(t\right){|}_{\mathrm{kT}}^{(k+1)T}\mathrm{\Pi H\xi }\left(\mathrm{kT}\right)$及e(k+1)＝Φ_e[(k+1)T，kT]e(k)得到四元数的时间更新值；其中$I=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right],$ξ(t)＝[ξ₀(t)ξ₁(t)…ξ_n-1(t)ξ_n(t)]^T$\left.\begin{array}{c}{\xi }_0\left(t\right)=1\\ {\xi }_1\left(t\right)=1-t\\ {\xi }_2\left(t\right)=1-2t+0.5t^2\\ ·\\ ·\\ ·\\ (i+1){\xi }_{i+1}\left(t\right)=(i+2i-t){\xi }_i\left(t\right)-i{\xi }_{i-1}\left(t\right)\end{array}\right\}$i＝2，3，…，n-1为Laguerre正交多项式的递推形式，滚转、俯仰、偏航角速度p，q，r的展开式分别为p(t)＝[p₀ p₁…p_n-1 p_n][ξ₀(t)ξ₁(t)…ξ_n-1(t)ξ_n(t)]^Tq(t)＝[q₀ q₁…q_n-1 q_n][ξ₀(t)ξ₁(t)…ξ_n-1(t)ξ_n(t)]^Tr(t)＝[r₀ r₁…r_n-1 r_n][ξ₀(t)ξ₁(t)…ξ_n-1(t)ξ_n(t)]^T$\Pi =\frac{1}{2}\{\left[\begin{array}{cccc}0& -1& 0& 0\\ 1& 0& 0& 0\\ 0& 0& 0& 1\\ 0& 0& -1& 0\end{array}\right]\left[\begin{array}{ccccccc}{p}_0& p_1& ·& ·& ·& p_{n-1}& p_n\end{array}\right]$$+\left[\begin{array}{cccc}0& 0& -1& 0\\ 0& 0& 0& -1\\ 1& 0& 0& 0\\ 0& 1& 0& 0\end{array}\right]\left[\begin{array}{ccccccc}{q}_0& q_1& ·& ·& ·& q_{n-1}& q_n\end{array}\right]+\left[\begin{array}{cccc}0& 0& 0& -1\\ 0& 0& 1& 0\\ 0& -1& 0& 0\\ 1& 0& 0& 0\end{array}\right]\left[\begin{array}{ccccccc}{r}_0& r_1& ·& ·& ·& r_{n-1}& r_n\end{array}\right]\}$${\Pi }_1=\frac{1}{2}\{{\left[\begin{array}{ccccccc}{p}_0& p_1& ·& ·& ·& p_{n-1}& p_n\end{array}\right]}^T\left[\begin{array}{cccc}0& -1& 0& 0\\ 1& 0& 0& 0\\ 0& 0& 0& 1\\ 0& 0& -1& 0\end{array}\right]$$+{\left[\begin{array}{ccccccc}{q}_0& q_1& ·& ·& ·& q_{n-1}& q_n\end{array}\right]}^T\left[\begin{array}{cccc}0& 0& -1& 0\\ 0& 0& 0& -1\\ 1& 0& 0& 0\\ 0& 1& 0& 0\end{array}\right]+{\left[\begin{array}{ccccccc}{r}_0& r_1& ·& ·& ·& r_{n-1}& r_n\end{array}\right]}^T\left[\begin{array}{cccc}0& 0& 0& -1\\ 0& 0& 1& 0\\ 0& -1& 0& 0\\ 1& 0& 0& 0\end{array}\right]\}$当p，q，r的展开式最高次项n为奇数时，m＝4，6，...，n+1，高次项n为偶数时m＝5，7，...，n+1。