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

摘要

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

主权项

一种基于角速度的飞行器极限飞行时四元数Hartley近似输出方法，其特征在于包括以下步骤：根据四元数连续状态方程$\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}+\mathrm{\Pi \Omega }{\left(t\right)|}_{\mathrm{kT}}^{(k+1)T}{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]$$\xi \left(t\right)={\left[\begin{array}{ccccccc}{\xi }_{-n}\left(t\right)& ···& {\xi }_{-1}\left(t\right)& {\xi }_0\left(t\right)& {\xi }_1\left(t\right)& ···& {\xi }_n\left(t\right)\end{array}\right]}_{(2n+1)\times 1}^T$ξ_i(t)＝cas(iωt)＝cos(iωt)+sin(iωt)，(f＝‑n，‑n+1，…，‑1，0，1，2，…，n)ω为角频率；滚转、俯仰、偏航角速度p，q，r的Hartley展开式分别为p(t)＝[p_‑n … p_‑1 p₀ p₁ … p_n][ξ_‑n(t) … ξ_‑1(t) ξ₀(t) ξ₁(t) … ξ_n(t)]^Tq(t)＝[q_‑n … q_‑1 q₀ q₁ … q_n][ξ_‑n(t) … ξ_‑1(t) ξ₀(t) ξ₁(t) … ξ_n(t)]^Tr(t)＝[r_‑n … r_‑1 r₀ r₁ … r_n][ξ_‑n(t) … ξ_‑1(t) ξ₀(t) ξ₁(t) … ξ_n(t)]^T$\Pi =\begin{array}{c}\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}_{-n}& ···& p_{-1}& p_0& p_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}_{-n}& ···& q_{-1}& q_0& q_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}_{-n}& ···& r_{-1}& r_0& r_1& ···& r_n\end{array}\right]\}\end{array}$${\Pi }_1=\begin{array}{c}\frac{1}{2}\{{\left[\begin{array}{ccccccc}{p}_{-n}& ···& p_{-1}& p_0& p_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}_{-n}& ···& q_{-1}& q_0& q_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}_{-n}& ···& r_{-1}& r_0& r_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]\}\end{array}$${\int }_0^t{\xi }_0\left(t\right)\mathrm{dt}=t=\frac{\xi \left(t\right)}{2\pi }\left[\begin{array}{ccccccccc}\frac{1}{n}& ···& \frac{1}{2}& 1& \pi & -1& \frac{1}{2}& ···& \frac{1}{n}\end{array}\right]$${\int }_0^t{\xi }_{-n}\left(t\right)\mathrm{dt}=\frac{\xi \left(t\right)}{2\pi }\left[\begin{array}{ccccccccc}0& ···& 0& 0& -\frac{1}{n}& 0& 0& ···& \frac{1}{n}\end{array}\right]$${\int }_0^t{\xi }_{-n}\left(t\right)\mathrm{dt}=\frac{\xi \left(t\right)}{2\pi }\left[\begin{array}{ccccccccc}-\frac{1}{n}& 0& ···& 0& \frac{1}{n}& 0& ···\in & 0& 0\end{array}\right]$