一种刚体空间运动状态的切比雪夫输出方法

摘要

本发明公开了一种刚体空间运动状态的切比雪夫输出方法，该方法通过定义三元数，使得机体轴系三个速度分量和三元数构成线性微分方程组，并采用Shifted Chebyshev(变动切比雪夫)正交多项式对滚转、俯仰、偏航角速度p，q，r进行近似逼近描述，可以按照任意阶保持器的方式求解系统的状态转移矩阵，进而得到刚体运动离散状态方程的表达式，避免了姿态方程奇异问题，从而得到刚体主要运动状态；本发明通过引入三元数使得状态转移矩阵为分块上三角形式，可以降阶求解状态转移矩阵，大大简化了计算复杂度，便于工程使用。

主权项

1.一种刚体空间运动状态的切比雪夫输出方法，其特征包括以下步骤：机体轴系三个速度分量输出为：${\left[\begin{array}{c}u\left(t\right)\\ v\left(t\right)\\ w\left(t\right)\end{array}\right]}_{t=(k+1)T}={\Phi }_v[(k+1)T,\mathrm{kT}]{\left[\begin{array}{c}u\left(t\right)\\ v\left(t\right)\\ w\left(t\right)\end{array}\right]}_{t=\mathrm{kT}}+g{\Phi }_v[(k+1)T,\mathrm{kT}]{\Phi }_s[(k+1)T,\mathrm{kT}]{\left[\begin{array}{c}{s}_1\left(t\right)\\ s_2\left(t\right)\\ s_3\left(t\right)\end{array}\right]}_{t=\mathrm{kT}}$$+g{\int }_{\mathrm{kT}}^{(k+1)T}{\Phi }_v[(k+1)T,\tau ]\left[\begin{array}{c}{n}_x\\ n_y\\ n_z\end{array}\right]\mathrm{d\tau }$其中：u，v，w分别为沿刚体机体轴系x，y，z轴的速度分量，n_x，n_y，n_z分别为沿x，y，z轴的过载，g为重力加速度，s₁、s₂、s₃为定义的三元数，且${\left[\begin{array}{c}{s}_1\left(t\right)\\ s_2\left(t\right)\\ s_3\left(t\right)\end{array}\right]}_{t=(k+1)T}={\Phi }_s[(k+1)T,\mathrm{kT}]{\left[\begin{array}{c}{s}_1\left(t\right)\\ s_2\left(t\right)\\ s_3\left(t\right)\end{array}\right]}_{t=\mathrm{kT}}$${{\Phi }_v[(k+1)T,\mathrm{kT}]\approx I+{\Pi }_v\mathrm{H\xi }\left(t\right)|}_{\mathrm{kT}}^{(k+1)T}{+{\Pi }_v\{P\otimes [{\mathrm{H\xi }\left(t\right)|}_{\mathrm{kT}}^{(k+1)T}\left]\right\}{H}^T{\Pi }_v^T-{\Pi }_v\mathrm{H\xi }\left(t\right)|}_{\mathrm{kT}}^{(k+1)T}{\Pi }_v\mathrm{H\xi }\left(\mathrm{kT}\right)$${{\Phi }_s[(k+1)T,\mathrm{kT}]\approx I+{\Pi }_s\mathrm{H\xi }\left(t\right)|}_{\mathrm{kT}}^{(k+1)T}{+{\Pi }_s\{P\otimes [{\mathrm{H\xi }\left(t\right)|}_{\mathrm{kT}}^{(k+1)T}\left]\right\}{H}^T{\Pi }_s^T-{\Pi }_s\mathrm{H\xi }\left(t\right)|}_{\mathrm{kT}}^{(k+1)T}{\Pi }_s\mathrm{H\xi }\left(\mathrm{kT}\right)$p，q，r分别为滚转、俯仰、偏航角速度，T为采样周期；全文参数定义相同；$I=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 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-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=\mathrm{2,3},···,n-\mathrm{1,0}\le t\le \mathrm{NT}$为Chebyshev(切比雪夫)正交多项式的递推形式，b＝NT，滚转、俯仰、偏航角速度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 }_v=\left[\begin{array}{ccc}0& 0& 0\\ 0& 0& 1\\ 0& -1& \text{0}\end{array}\right]\left[\begin{array}{ccccc}{p}_0& p_1& ···& p_{n-1}& p_n\end{array}\right]$$+\left[\begin{array}{ccc}0& 0& -1\\ 0& 0& 0\\ 1& 0& 0\end{array}\right]\left[\begin{array}{ccccc}{q}_0& q_1& ···& q_{n-1}& q_n\end{array}\right]+\left[\begin{array}{ccc}0& 1& 0\\ -1& 0& 0\\ 0& 0& 0\end{array}\right]\left[\begin{array}{ccccc}{r}_0& r_1& ···& r_{n-1}& r_n\end{array}\right]$${\Pi }_s=\left[\begin{array}{ccc}0& 0& 0\\ 0& 0& 1\\ 0& -1& 0\end{array}\right]\left[\begin{array}{ccccc}{p}_0& p_1& ···& p_{n-1}& p_n\end{array}\right]$$+\left[\begin{array}{ccc}0& 0& 1\\ 0& 0& 0\\ -1& 0& 0\end{array}\right]\left[\begin{array}{ccccc}{q}_0& q_1& ···& q_{n-1}& q_n\end{array}\right]+\left[\begin{array}{ccc}0& -1& 0\\ 1& 0& 0\\ 0& 0& 0\end{array}\right]\left[\begin{array}{ccccc}{r}_0& r_1& ···& r_{n-1}& r_n\end{array}\right]$当p，q，r的展开式最高次项n为奇数时，m＝4，6，...，n+1，高次项n为偶数时m＝5，7，...，n+1，H_i(i＝1，2，…，n)为H相应的行向量，高度输出为：$\stackrel{·}{h}=\left[\begin{array}{ccc}u& v& w\end{array}\right]\left[\begin{array}{c}{s}_1\\ s_2\\ s_3\end{array}\right]$其中：h为高度；姿态角的输出为：$\psi \left(t\right)=\psi \left(\mathrm{kT}\right)+{\int }_{\mathrm{kT}}^t\frac{{\mathrm{qs}}_2\left(t\right)+{\mathrm{rs}}_3\left(t\right)}{s_2^2\left(t\right)+s_3^2\left(t\right)}\mathrm{dt}$其中：θ，ψ分别表示滚转、俯仰、偏航角，$\left[\begin{array}{c}{s}_1\left(t\right)\\ s_2\left(t\right)\\ s_3\left(t\right)\end{array}\right]={\Phi }_s(t,\mathrm{kT}){\left[\begin{array}{c}{s}_1\left(t\right)\\ s_2\left(t\right)\\ s_3\left(t\right)\end{array}\right]}_{t=\mathrm{kT}}.$