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

摘要

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

主权项

一种基于角速度的飞行器极限飞行时四元数傅里埃近似输出方法，其特征在于包括以下步骤：根据四元数连续状态方程$\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分别为滚转、俯仰、偏航角速度；欧拉角θ，ψ分别指滚转、俯仰、偏航角；状态转移矩阵按照逼近式$\begin{array}{c}{\Phi }_e[(k+1)T,\mathrm{kT}]\approx I+{\Pi }_A{H}_A{\xi }_{\mathrm{AI}}\left(t\right){|}_{\mathrm{kT}}^{(k+1)T}+{\Pi }_B{H}_B{\xi }_{\mathrm{BI}}\left(t\right){|}_{\mathrm{kT}}^{(k+1)T}+{\Pi }_A{P}_{\mathrm{AAI}}{|}_{\mathrm{kT}}^{(k+1(T}{H}_A^T{\Pi }_{A1}+{\Pi }_B{P}_{\mathrm{BAI}}{|}_{\mathrm{kT}}^{(k+1(T}{H}_A^T{\Pi }_{A1}\\ +{\Pi }_A{P}_{\mathrm{ABI}}{|}_{\mathrm{kT}}^{(k+1(T}{H}_B^T{\Pi }_{B1}+{\Pi }_B{P}_{\mathrm{BBI}}{|}_{\mathrm{kT}}^{(k+1(T}{H}_B^T{\Pi }_{B1}\\ -[{\Pi }_A{H}_A{\xi }_{\mathrm{AI}}\left(t\right)+{\Pi }_B{H}_B{\xi }_{\mathrm{BI}}\left(t\right)]{|}_{\mathrm{kT}}^{(k+1)T}[{\xi }_{\mathrm{AI}}^T\left(t\right)H_A^T{\Pi }_{A1}+{\xi }_{\mathrm{BI}}^T\left(t\right)H_B^T{\Pi }_{B1}]{|}_{\mathrm{kT}}\end{array}$及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],$ξ_AI(t)＝[t sin(ωt) … sin[(n‑1)ωt] sin(nωt)]^Tξ_BI(t)＝[cos(ωt) cos(2ωt) … cos[(n‑1)ωt] cos(nωt)]^T滚转、俯仰、偏航角速度p，q，r的n阶展开式分别为p(t)＝[p_a0 p_a1 … p_a(n‑1) p_an][1 cos(ωt) … cos[(n‑1)ωt] cos(nωt)]^T+[p_b1 p_b2 … p_b(n‑1) p_bn][sin(ωt) sin(2ωt) … sin[(n‑1)ωt] sin(nωt)]^Tq(t)＝[q_a0 q_a1 … q_a(n‑1) q_an][1 cos(ωt) … cos[(n‑1)ωt] cos(nωt)]^T+[q_b1 q_b2 … q_b(n‑1) q_bn][sin(ωt) sin(2ωt) … sin[(n‑1)ωt] sin(nωt)]^Tr(t)＝[r_a0 r_a1 … r_a(n‑1) r_an][cos(ωt) cos(2ωt) … cos[(n‑1)ωt] cos(nωt)]^T+[r_b1 r_b2 … r_b(n‑1) r_bn][sin(ωt) sin(2ωt) … sin[(n‑1)ωt] sin(nωt)]^T${\Pi }_A=\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}{ccccc}{p}_{a0}& p_{a1}& ···& p_{a(n-1)}& p_{\mathrm{an}}\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}{ccccc}{q}_{a0}& q_{a1}& ···& q_{a(n-1)}& q_{\mathrm{an}}\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}{ccccc}{r}_{a0}& r_{a1}& ···& r_{a(n-1)}& r_{\mathrm{an}}\end{array}\right]\}$${\Pi }_B=\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}{ccccc}{p}_{b1}& p_{b2}& ···& p_{b(n-1)}& p_{\mathrm{bn}}\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}{ccccc}{q}_{b1}& q_{b2}& ···& q_{b(n-1)}& q_{\mathrm{bn}}\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}{ccccc}{r}_{b1}& r_{b2}& ···& r_{b(n-1)}& r_{\mathrm{bn}}\end{array}\right]\}$${\Pi }_{A1}=\frac{1}{2}\{{\left[\begin{array}{ccccc}{p}_{a0}& p_{a1}& ···& p_{a(n-1)}& p_{\mathrm{an}}\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}{ccccc}{q}_{a0}& q_{a1}& ···& q_{a(n-1)}& q_{\mathrm{an}}\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}{ccccc}{r}_{a0}& r_{a1}& ···& r_{a(n-1)}& r_{\mathrm{an}}\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]\}$${\Pi }_{B1}=\frac{1}{2}\{{\left[\begin{array}{ccccc}{p}_{b1}& p_{b2}& ···& p_{b(n-1)}& p_{\mathrm{bn}}\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}{ccccc}{q}_{b1}& q_{b2}& ···& q_{b(n-1)}& q_{\mathrm{bn}}\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}{ccccc}{r}_{b1}& r_{b2}& ···& r_{b(n-1)}& r_{\mathrm{bn}}\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]\}$$H_A={\left[\begin{array}{cccc}{H}_0^T& H_1^T& ···& H_n^T\end{array}\right]}^T=\mathrm{diag}\{1,\frac{1}{\omega },\frac{1}{2\omega },···,\frac{1}{\mathrm{n\omega }}\},$H_B＝‑H_A