基于角速度的欧拉角切比雪夫近似输出方法

摘要

本发明公开了一种基于角速度的欧拉角切比雪夫近似输出方法，用于解决现有的飞行器机动飞行时欧拉角输出精度差的技术问题。技术方案是通过引入多个参数并将滚转、俯仰、偏航角速度按照变动区间的切比雪夫正交多项式展开，按照依次求解俯仰角、滚转角、偏航角，直接对欧拉角的表达式进行高阶逼近积分，使得欧拉角的求解按照超线性逼近，保证了确定欧拉角的时间更新迭代计算精度，从而提高了惯性设备输出飞行姿态的准确性。

主权项

一种基于角速度的飞行器欧拉角切比雪夫近似输出方法，其特征在于包括以下步骤：步骤1、(a)根据欧拉方程:式中：θ,ψ分别指滚转、俯仰、偏航角；p,q,r分别为滚转、俯仰、偏航角速度；这三个欧拉角的计算按照依次求解俯仰角、滚转角、偏航角的步骤进行；滚转、俯仰、偏航角速度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其中$\left.\begin{array}{c}{\xi }_0\left(t\right)=1\\ {\xi }_1\left(t\right)=1-2t/b\\ {\xi }_2\left(t\right)=8{(t/b)}^2-8(t/b)+1\\ ·\\ ·\\ ·\\ {\xi }_{i+1}\left(t\right)=2{\xi }_1\left(t\right){\xi }_i\left(t\right)-{\xi }_{i-1}\left(t\right)\end{array}\right\}$i＝2,3,…,n‑1,0≤t≤NT,b＝NT为Chebyshev正交多项式的递推形式，T为采样周期；(b)俯仰角的时间更新求解式为：式中：$\begin{array}{c}{a}_1=1+\left[\begin{array}{ccccc}{p}_0& p_1& ...& p_{n-1}& p_n\end{array}\right]\{P\otimes [\mathrm{H\xi }\left(t\right){|}_{\mathrm{kT}}^{(k+1)T}\left]\right\}{H}^T{\left[\begin{array}{ccccc}{p}_0& p_1& ...& p_{n-1}& p_n\end{array}\right]}^T\\ +\left[\begin{array}{ccccc}{q}_0& q_1& ...& q_{n-1}& q_n\end{array}\right]\{P\otimes [\mathrm{H\xi }\left(t\right){|}_{\mathrm{kT}}^{(k+1)T}\left]\right\}{\left[\begin{array}{ccccc}{q}_0& q_1& ...& q_{n-1}& q_n\end{array}\right]}^T\\ +\left[\begin{array}{ccccc}{r}_0& r_1& ...& r_{n-1}& r_n\end{array}\right]\{P\otimes [\mathrm{H\xi }\left(t\right){|}_{\mathrm{kT}}^{(k+1)T}\left]\right\}{H}^T{\left[\begin{array}{ccccc}{r}_0& r_1& ...& r_{n-1}& r_n\end{array}\right]}^T\\ -\left[\begin{array}{ccccc}{p}_0& p_1& ...& p_{n-1}& p_n\end{array}\right]H{\left[\begin{array}{ccccc}{\xi }_0\left(t\right)& {\xi }_1\left(t\right)& ...& {\xi }_n\left(t\right)& {\xi }_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}·\\ {\left[\begin{array}{ccccc}{\xi }_0\left(t\right)& {\xi }_1\left(t\right)& ...& {\xi }_n\left(t\right)& {\xi }_{n+1}\left(t\right)\end{array}\right]}_{\mathrm{kT}}{H}^T{\left[\begin{array}{ccccc}{p}_0& p_1& ...& p_{n-1}& p_n\end{array}\right]}^T\\ -\left[\begin{array}{ccccc}{q}_0& q_1& ...& q_{n-1}& q_n\end{array}\right]H{\left[\begin{array}{ccccc}{\xi }_0\left(t\right)& {\xi }_1\left(t\right)& ...& {\xi }_n\left(t\right)& {\xi }_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}·\\ {\left[\begin{array}{ccccc}{\xi }_0\left(t\right)& {\xi }_1\left(t\right)& ...& {\xi }_n\left(t\right)& {\xi }_{n+1}\end{array}\left(t\right)\right]}_{\mathrm{kT}}{H}^T{\left[\begin{array}{ccccc}{q}_0& q_1& ...& q_{n-1}& q_n\end{array}\right]}^T\\ -\left[\begin{array}{ccccc}{r}_0& r_1& ...& r_{n-1}& r_n\end{array}\right]H{\left[\begin{array}{ccccc}{\xi }_0\left(t\right)& {\xi }_1\left(t\right)& ...& {\xi }_n\left(t\right)& {\xi }_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}·\\ {\left[\begin{array}{ccccc}{\xi }_0\left(t\right)& {\xi }_1\left(t\right)& ...& {\xi }_n\left(t\right)& {\xi }_{n+1}\left(t\right)\end{array}\right]}_{\mathrm{kT}}{H}^T{\left[\begin{array}{ccccc}{r}_0& r_1& ...& r_{n-1}& r_n\end{array}\right]}^T\\ +0.25{\left\{\left[\begin{array}{ccccc}{p}_0& p_1& ...& p_{n-1}& p_n\end{array}\right]H{\left[\begin{array}{ccccc}{\xi }_0\left(t\right)& {\xi }_1\left(t\right)& ...& {\xi }_n\left(t\right)& {\xi }_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}\right\}}^2\\ +0.25{\left\{\left[\begin{array}{ccccc}{q}_0& q_1& ...& q_{n-1}& q_n\end{array}\right]H{\left[\begin{array}{ccccc}{\xi }_0\left(t\right)& {\xi }_1\left(t\right)& ...& {\xi }_n\left(t\right)& {\xi }_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}\right\}}^2\\ -0.25{\left\{\left[\begin{array}{ccccc}{r}_0& r_1& ...& r_{n-1}& r_n\end{array}\right]H{\left[\begin{array}{ccccc}{\xi }_0\left(t\right)& {\xi }_1\left(t\right)& ...& {\xi }_n\left(t\right)& {\xi }_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}\right\}}^2\end{array}$$\begin{array}{c}{a}_2=\left[\begin{array}{ccccc}{q}_0& q_1& ...& q_{n-1}& q_n\end{array}\right]H{\left[\begin{array}{ccccc}{\xi }_0\left(t\right)& {\xi }_1\left(t\right)& ...& {\xi }_n\left(t\right)& {\xi }_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}\\ -0.5\left[\begin{array}{ccccc}{r}_0& r_1& ...& r_{n-1}& r_n\end{array}\right]\{p\otimes [\mathrm{H\xi }\left(t\right){|}_{\mathrm{kT}}^{(k+1)T}\left]\right\}{H}^T{\left[\begin{array}{ccccc}{p}_0& p_1& ...& p_{n-1}& p_n\end{array}\right]}^T\\ +0.5\left[\begin{array}{ccccc}{r}_0& r_1& ...& r_{n-1}& r_n\end{array}\right]H{\left[\begin{array}{ccccc}{\xi }_0\left(t\right)& {\xi }_1\left(t\right)& ...& {\xi }_n\left(t\right)& {\xi }_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}\\ ·{\left[\begin{array}{ccccc}{\xi }_0\left(t\right)& {\xi }_1\left(t\right)& ...& {\xi }_n\left(t\right)& {\xi }_{n+1}\left(t\right)\end{array}\right]}_{\mathrm{kT}}{H}^T{\left[\begin{array}{ccccc}{p}_0& p_1& ...& p_{n-1}& p_n\end{array}\right]}^T\end{array}$$\begin{array}{c}{a}_3=\left[\begin{array}{ccccc}{r}_0& r_1& ...& r_{n-1}& r_n\end{array}\right]H{\left[\begin{array}{ccccc}{\xi }_0\left(t\right)& {\xi }_1\left(t\right)& ...& {\xi }_n\left(t\right)& {\xi }_{n+1}\left(t\right)\end{array}\right]}^T\\ -0.5\left[\begin{array}{ccccc}{q}_0& q_1& ...& q_{n-1}& q_n\end{array}\right]\{p\otimes [\mathrm{H\xi }\left(t\right){|}_{\mathrm{kT}}^{(k+1)T}\left]\right\}{H}^T{\left[\begin{array}{ccccc}{p}_0& p_1& ...& p_{n-1}& p_n\end{array}\right]}^T\\ +0.5\left[\begin{array}{ccccc}{q}_0& q_1& ...& q_{n-1}& q_n\end{array}\right]H{\left[\begin{array}{ccccc}{\xi }_0\left(t\right)& {\xi }_1\left(t\right)& ...& {\xi }_n\left(t\right)& {\xi }_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}\\ ·{\left[\begin{array}{ccccc}{\xi }_0\left(t\right)& {\xi }_1\left(t\right)& ...& {\xi }_n\left(t\right)& {\xi }_{n+1}\left(t\right)\end{array}\right]}_{\mathrm{kT}}{H}^T{\left[\begin{array}{ccccc}{p}_0& p_1& ...& p_{n-1}& p_n\end{array}\right]}^T\end{array}$当p,q,r的展开式最高次项n为奇数时，m＝4,6,…,n+1，高次项n为偶数时m＝5,7,...,n+1，H_i(i＝1,2,...,n)为H相应的行向量；步骤2、(a)在已知俯仰角的情况下，滚转角的时间更新求解式为：其中$\begin{array}{c}{a}_4=1+\left[\begin{array}{ccccc}{p}_0& p_1& ...& p_{n-1}& p_n\end{array}\right]\{P\otimes [\mathrm{H\xi }\left(t\right){|}_{\mathrm{kT}}^{(k+1)T}\left]\right\}{H}^T{\left[\begin{array}{ccccc}{p}_0& p_1& ...& p_{n-1}& p_n\end{array}\right]}^T\\ +\left[\begin{array}{ccccc}{q}_0& q_1& ...& q_{n-1}& q_n\end{array}\right]\{P\otimes [\mathrm{H\xi }\left(t\right){|}_{\mathrm{kT}}^{(k+1)T}\left]\right\}{\left[\begin{array}{ccccc}{q}_0& q_1& ...& q_{n-1}& q_n\end{array}\right]}^T\\ +\left[\begin{array}{ccccc}{r}_0& r_1& ...& r_{n-1}& r_n\end{array}\right]\{P\otimes [\mathrm{H\xi }\left(t\right){|}_{\mathrm{kT}}^{(k+1)T}\left]\right\}{H}^T{\left[\begin{array}{ccccc}{r}_0& r_1& ...& r_{n-1}& r_n\end{array}\right]}^T\\ -\left[\begin{array}{ccccc}{p}_0& p_1& ...& p_{n-1}& p_n\end{array}\right]H{\left[\begin{array}{ccccc}{\xi }_0\left(t\right)& {\xi }_1\left(t\right)& ...& {\xi }_n\left(t\right)& {\xi }_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}·\\ {\left[\begin{array}{ccccc}{\xi }_0\left(t\right)& {\xi }_1\left(t\right)& ...& {\xi }_n\left(t\right)& {\xi }_{n+1}\left(t\right)\end{array}\right]}_{\mathrm{kT}}{H}^T{\left[\begin{array}{ccccc}{p}_0& p_1& ...& p_{n-1}& p_n\end{array}\right]}^T\\ -\left[\begin{array}{ccccc}{q}_0& q_1& ...& q_{n-1}& q_n\end{array}\right]H{\left[\begin{array}{ccccc}{\xi }_0\left(t\right)& {\xi }_1\left(t\right)& ...& {\xi }_n\left(t\right)& {\xi }_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}·\\ {\left[\begin{array}{ccccc}{\xi }_0\left(t\right)& {\xi }_1\left(t\right)& ...& {\xi }_n\left(t\right)& {\xi }_{n+1}\end{array}\left(t\right)\right]}_{\mathrm{kT}}{H}^T{\left[\begin{array}{ccccc}{q}_0& q_1& ...& q_{n-1}& q_n\end{array}\right]}^T\\ -\left[\begin{array}{ccccc}{r}_0& r_1& ...& r_{n-1}& r_n\end{array}\right]H{\left[\begin{array}{ccccc}{\xi }_0\left(t\right)& {\xi }_1\left(t\right)& ...& {\xi }_n\left(t\right)& {\xi }_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}·\\ {\left[\begin{array}{ccccc}{\xi }_0\left(t\right)& {\xi }_1\left(t\right)& ...& {\xi }_n\left(t\right)& {\xi }_{n+1}\left(t\right)\end{array}\right]}_{\mathrm{kT}}{H}^T{\left[\begin{array}{ccccc}{r}_0& r_1& ...& r_{n-1}& r_n\end{array}\right]}^T\\ +0.25{\left\{\left[\begin{array}{ccccc}{p}_0& p_1& ...& p_{n-1}& p_n\end{array}\right]H{\left[\begin{array}{ccccc}{\xi }_0\left(t\right)& {\xi }_1\left(t\right)& ...& {\xi }_n\left(t\right)& {\xi }_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}\right\}}^2\\ -0.25{\left\{\left[\begin{array}{ccccc}{r}_0& r_1& ...& r_{n-1}& r_n\end{array}\right]H{\left[\begin{array}{ccccc}{\xi }_0\left(t\right)& {\xi }_1\left(t\right)& ...& {\xi }_n\left(t\right)& {\xi }_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}\right\}}^2\\ -0.25{\left\{\left[\begin{array}{ccccc}{q}_0& q_1& ...& q_{n-1}& q_n\end{array}\right]H{\left[\begin{array}{ccccc}{\xi }_0\left(t\right)& {\xi }_1\left(t\right)& ...& {\xi }_n\left(t\right)& {\xi }_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}\right\}}^2\end{array}$$\begin{array}{c}{a}_5=\left[\begin{array}{ccccc}{p}_0& p_1& ...& p_{n-1}& p_n\end{array}\right]H{\left[\begin{array}{ccccc}{\xi }_0\left(t\right)& {\xi }_1\left(t\right)& ...& {\xi }_n\left(t\right)& {\xi }_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}\\ +0.5\left[\begin{array}{ccccc}{r}_0& r_1& ...& r_{n-1}& r_n\end{array}\right]\{p\otimes [\mathrm{H\xi }\left(t\right){|}_{\mathrm{kT}}^{(k+1)T}\left]\right\}{H}^T{\left[\begin{array}{ccccc}{q}_0& q_1& ...& q_{n-1}& q_n\end{array}\right]}^T\\ -0.5\left[\begin{array}{ccccc}{r}_0& r_1& ...& r_{n-1}& r_n\end{array}\right]H{\left[\begin{array}{ccccc}{\xi }_0\left(t\right)& {\xi }_1\left(t\right)& ...& {\xi }_n\left(t\right)& {\xi }_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}\\ ·{\left[\begin{array}{ccccc}{\xi }_0\left(t\right)& {\xi }_1\left(t\right)& ...& {\xi }_n\left(t\right)& {\xi }_{n+1}\left(t\right)\end{array}\right]}_{\mathrm{kT}}{H}^T{\left[\begin{array}{ccccc}{q}_0& q_1& ...& q_{n-1}& q_n\end{array}\right]}^T\end{array}$$\begin{array}{c}{a}_6=\left[\begin{array}{ccccc}{r}_0& r_1& ...& r_{n-1}& r_n\end{array}\right]H{\left[\begin{array}{ccccc}{\xi }_0\left(t\right)& {\xi }_1\left(t\right)& ...& {\xi }_n\left(t\right)& {\xi }_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}\\ -0.5\left[\begin{array}{ccccc}{p}_0& p_1& ...& p_{n-1}& p_n\end{array}\right]\{p\otimes [\mathrm{H\xi }\left(t\right){|}_{\mathrm{kT}}^{(k+1)T}\left]\right\}{H}^T{\left[\begin{array}{ccccc}{q}_0& q_1& ...& q_{n-1}& q_n\end{array}\right]}^T\\ +0.5\left[\begin{array}{ccccc}{p}_0& p_1& ...& p_{n-1}& p_n\end{array}\right]H{\left[\begin{array}{ccccc}{\xi }_0\left(t\right)& {\xi }_1\left(t\right)& ...& {\xi }_n\left(t\right)& {\xi }_{n+1}\left(t\right)\end{array}\right]}^T{|}_{\mathrm{kT}}^{(k+1)T}\\ ·{\left[\begin{array}{ccccc}{\xi }_0\left(t\right)& {\xi }_1\left(t\right)& ...& {\xi }_n\left(t\right)& {\xi }_{n+1}\left(t\right)\end{array}\right]}_{\mathrm{kT}}{H}^T{\left[\begin{array}{ccccc}{q}_0& q_1& ...& q_{n-1}& q_n\end{array}\right]}^T\end{array}$(b)在俯仰角、滚转角已知情况下，偏航角的求解式为：$\psi \left(t\right)=\psi \left(\mathrm{KT}\right)+{\int }_{\mathrm{kT}}^t[b_1\left(t\right)+b_2\left(t\right)]\mathrm{dt}$式中：