CN102359790B - 一种刚体空间运动状态的傅里埃输出方法 - Google Patents

Abstract

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

Description

一种刚体空间运动状态的傅里埃输出方法

技术领域

本发明涉及空间运动刚体模型，特别涉及飞行器大机动飞行状态输出问题。

背景技术

机体轴系刚体运动微分方程是描述飞行器、鱼雷、航天器等空间运动的基本方程。通常，在数据处理等应用中，体轴系的状态变量主要包含3个速度分量、三个欧拉角、以及地面坐标系的X_E，Y_E，Z_E等，由于Z_E定义为垂直地面指向地球中心，因此Z_E实际为负的飞行高度；X_E，Y_E通常主要依赖GPS、GNSS、北斗等直接给出；欧拉角表示刚体空间运动姿态，而刻画刚体姿态的微分方程又是其中的核心，通常以三个欧拉角即俯仰、滚转和偏航角来描述。当刚体的俯仰角为±90°时，滚转角和偏航角无法定值，同时临近该奇点的区域求解误差过大，导致工程上不可容忍的误差而不能使用；为了避免这一问题，人们首先采用限制俯仰角取值范围的方法，这使得方程式退化，不能全姿态工作，因而难以广泛用于工程实践。随着对飞行器极限飞行的研究，人们又相继采用了方向余弦法、等效转动矢量法、四元数法等推算刚体运动姿态。

方向余弦法避免了欧拉角描述方法的“奇异”现象，用方向余弦法计算姿态矩阵没有方程退化问题，可以全姿态工作，但需要求解9个微分方程，计算量较大，实时性较差，无法满足工程实践要求。等效转动矢量法如单子样递推、双子样转动矢量、三子样转动矢量和四子样旋转矢量法以及在此基础上的各种修正算法和递推算法等。文献中研究旋转矢量时，都是基于速率陀螺输出为角增量的算法。然而在实际工程中，一些陀螺的输出是角速率信号，如光纤陀螺、动力调谐陀螺等。当速率陀螺输出为角速率信号时，旋转矢量法的算法误差明显增大。四元数法是定义4个欧拉角的函数来计算航姿，能够有效弥补欧拉角描述方法的奇异性，只要解4个一阶微分方程式组即可，比方向余弦姿态矩阵微分方程式计算量有明显的减少，能满足工程实践中对实时性的要求。其常用的计算方法有毕卡逼近法、二阶、四阶龙格-库塔法和三阶泰勒展开法等。毕卡逼近法实质是单子样算法，对有限转动引起的不可交换误差没有补偿，在高动态情况下姿态解算中的算法漂移会十分严重。采用四阶龙格-库塔法求解四元数微分方程时，随着积分误差的不断积累，会出现三角函数取值超出±1的现象，从而导致计算发散；泰勒展开法也因计算精度的不足而受到制约。当刚体大机动时，角速率较大导致上述方法的误差更大；不仅如此，姿态估计的误差常常会导致速度4个分量、高度输出的误差急剧增大。

发明内容

为了克服现有刚体运动模型输出误差大的问题，本发明提供一种刚体空间运动状态的傅里埃输出方法，该方法通过定义三元数，使得机体轴系三个速度分量和三元数构成线性微分方程组，并采用傅里埃级数对滚转、俯仰、偏航角速度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}={\mathrm{\Φ}}_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{\mathrm{\Φ}}_v[(k+1)T,\mathrm{kT}]{\mathrm{\Φ}}_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{\∫}_{\mathrm{kT}}^{(k+1)T}{\mathrm{\Φ}}_v[(k+1)T,\mathrm{\τ}]\left[\begin{array}{c}{n}_x\\ n_y\\ n_z\end{array}\right]\mathrm{d\τ}$

其中：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}={\mathrm{\Φ}}_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}}$

${{\mathrm{\Φ}}_v[(k+1)T,\mathrm{kT}]\≈I+{\mathrm{\Π}}_{\mathrm{vA}}{H}_A{\mathrm{\ξ}}_{\mathrm{AT}}\left(t\right)|}_{\mathrm{kT}}^{(k+1)T}+{{\mathrm{\Π}}_{\mathrm{vB}}{H}_B{\mathrm{\ξ}}_{\mathrm{BT}}\left(t\right)|}_{\mathrm{kT}}^{(k+1)T}{+{\mathrm{\Π}}_{\mathrm{vA}}{P}_{\mathrm{AAT}}|}_{\mathrm{kT}}^{(k+1(T}{H_A^T{\mathrm{\Π}}_{\mathrm{vA}}^T+{\mathrm{\Π}}_{\mathrm{vB}}{P}_{\mathrm{BAT}}|}_{\mathrm{kT}}^{(k+1(T}{H}_A^T{\mathrm{\Π}}_{\mathrm{vA}}^T$

${+{\mathrm{\Π}}_{\mathrm{vA}}{P}_{\mathrm{ABT}}|}_{\mathrm{kT}}^{(k+1(T}{H_B^T{\mathrm{\Π}}_{\mathrm{vB}}^T+{\mathrm{\Π}}_{\mathrm{vB}}{P}_{\mathrm{BBT}}|}_{\mathrm{kT}}^{(k+1(T}{H}_B^T{\mathrm{\Π}}_{\mathrm{vB}}^T$

${-[{\mathrm{\Π}}_{\mathrm{vA}}{H}_A{\mathrm{\ξ}}_{\mathrm{AT}}\left(t\right)+{\mathrm{\Π}}_{\mathrm{vB}}{H}_B{\mathrm{\ξ}}_{\mathrm{BT}}\left(t\right)]|}_{\mathrm{kT}}^{(k+1)T}{[{\mathrm{\ξ}}_{\mathrm{AT}}^T\left(t\right)H_A^T{\mathrm{\Π}}_{\mathrm{vA}}^T+{\mathrm{\ξ}}_{\mathrm{BT}}^T\left(t\right)H_B^T{\mathrm{\Π}}_{\mathrm{vB}}^T]|}_{\mathrm{kT}}$

${{\mathrm{\Φ}}_s[(k+1)T,\mathrm{kT}]\≈I+{\mathrm{\Π}}_{\mathrm{sA}}{H}_A{\mathrm{\ξ}}_{\mathrm{AT}}\left(t\right)|}_{\mathrm{kT}}^{(k+1)T}+{{\mathrm{\Π}}_{\mathrm{sB}}{H}_B{\mathrm{\ξ}}_{\mathrm{BT}}\left(t\right)|}_{\mathrm{kT}}^{(k+1)T}{+{\mathrm{\Π}}_{\mathrm{sA}}{P}_{\mathrm{AAT}}|}_{\mathrm{kT}}^{(k+1(T}{H_A^T{\mathrm{\Π}}_{\mathrm{sA}}^T+{\mathrm{\Π}}_{\mathrm{sB}}{P}_{\mathrm{BAT}}|}_{\mathrm{kT}}^{(k+1(T}{H}_A^T{\mathrm{\Π}}_{\mathrm{sA}}^T$

${+{\mathrm{\Π}}_{\mathrm{sA}}{P}_{\mathrm{ABT}}|}_{\mathrm{kT}}^{(k+1(T}{H_B^T{\mathrm{\Π}}_{\mathrm{sB}}^T+{\mathrm{\Π}}_{\mathrm{vB}}{P}_{\mathrm{BBT}}|}_{\mathrm{kT}}^{(k+1(T}{H}_B^T{\mathrm{\Π}}_{\mathrm{sB}}^T$

${-[{\mathrm{\Π}}_{\mathrm{sA}}{H}_A{\mathrm{\ξ}}_{\mathrm{AT}}\left(t\right)+{\mathrm{\Π}}_{\mathrm{sB}}{H}_B{\mathrm{\ξ}}_{\mathrm{BT}}\left(t\right)]|}_{\mathrm{kT}}^{(k+1)T}{[{\mathrm{\ξ}}_{\mathrm{AT}}^T\left(t\right)H_A^T{\mathrm{\Π}}_{\mathrm{sA}}^T+{\mathrm{\ξ}}_{\mathrm{BT}}^T\left(t\right)H_B^T{\mathrm{\Π}}_{\mathrm{sB}}^T]|}_{\mathrm{kT}}$

p，q，r分别为滚转、俯仰、偏航角速度，T为采样周期；全文参数定义相同；

$I=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right],$

ξ_A1(t)＝[t sin(ωt)…sin[(n-1)ωt]sin(nωt)]^T

ξ_B1(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][1cos(ω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)]^T

q(t)＝[q_a0 q_a1…q_a(n-1) q_an][1cos(ω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)]^T

r(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

${\mathrm{\Π}}_{\mathrm{vA}}=\left[\begin{array}{ccc}0& 0& 0\\ 0& 0& 1\\ 0& -1& \text{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}{ccc}0& 0& -1\\ 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}{ccc}0& 1& 0\\ -1& 0& 0\\ 0& 0& 0\end{array}\right]\left[\begin{array}{ccccc}{r}_{a0}& r_{a1}& \·\·\·& r_{a(n-1)}& r_{\mathrm{an}}\end{array}\right]$

${\mathrm{\Π}}_{\mathrm{sA}}=\left[\begin{array}{ccc}0& 0& 0\\ 0& 0& 1\\ 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}{ccc}0& 0& 1\\ 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}{ccc}0& -1& 0\\ 1& 0& 0\\ 0& 0& 0\end{array}\right]\left[\begin{array}{ccccc}{r}_{a0}& r_{a1}& \·\·\·& r_{a(n-1)}& r_{\mathrm{an}}\end{array}\right]$

${\mathrm{\Π}}_{\mathrm{vB}}=\left[\begin{array}{ccc}0& 0& 0\\ 0& 0& 1\\ 0& -1& \text{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}{ccc}0& 0& -1\\ 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}{ccc}0& 1& 0\\ -1& 0& 0\\ 0& 0& 0\end{array}\right]\left[\begin{array}{ccccc}{r}_{b1}& r_{b2}& \·\·\·& r_{b(n-1)}& r_{\mathrm{bn}}\end{array}\right]$

${\mathrm{\Π}}_{\mathrm{sB}}=\left[\begin{array}{ccc}0& 0& 0\\ 0& 0& 1\\ 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}{ccc}0& 0& 1\\ 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}{ccc}0& -1& 0\\ 1& 0& 0\\ 0& 0& 0\end{array}\right]\left[\begin{array}{ccccc}{r}_{b1}& r_{b2}& \·\·\·& r_{b(n-1)}& r_{\mathrm{bn}}\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}{\mathrm{\ω}},\frac{1}{2\mathrm{\ω}},\·\·\·,\frac{1}{\mathrm{n\ω}}\},$ H_B＝-H_A

2、高度输出为：

$\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为高度；

3、姿态角的输出为：

$\mathrm{\ψ}\left(t\right)=\mathrm{\ψ}\left(\mathrm{kT}\right)+{\∫}_{\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]={\mathrm{\Φ}}_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}}.$

本发明的有益效果是：通过引入三元数使得状态转移矩阵为分块上三角形式，可以降阶求解状态转移矩阵，大大简化了计算复杂度，便于工程使用。

下面结合实施例对本发明作详细说明。

具体实施方式

1、机体轴系三个速度分量输出为：

${\left[\begin{array}{c}u\left(t\right)\\ v\left(t\right)\\ w\left(t\right)\end{array}\right]}_{t=(k+1)T}={\mathrm{\Φ}}_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{\mathrm{\Φ}}_v[(k+1)T,\mathrm{kT}]{\mathrm{\Φ}}_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{\∫}_{\mathrm{kT}}^{(k+1)T}{\mathrm{\Φ}}_v[(k+1)T,\mathrm{\τ}]\left[\begin{array}{c}{n}_x\\ n_y\\ n_z\end{array}\right]\mathrm{d\τ}$

其中：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}={\mathrm{\Φ}}_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}}$

${{\mathrm{\Φ}}_v[(k+1)T,\mathrm{kT}]\≈I+{\mathrm{\Π}}_{\mathrm{vA}}{H}_A{\mathrm{\ξ}}_{\mathrm{AT}}\left(t\right)|}_{\mathrm{kT}}^{(k+1)T}+{{\mathrm{\Π}}_{\mathrm{vB}}{H}_B{\mathrm{\ξ}}_{\mathrm{BT}}\left(t\right)|}_{\mathrm{kT}}^{(k+1)T}{+{\mathrm{\Π}}_{\mathrm{vA}}{P}_{\mathrm{AAT}}|}_{\mathrm{kT}}^{(k+1(T}{H_A^T{\mathrm{\Π}}_{\mathrm{vA}}^T+{\mathrm{\Π}}_{\mathrm{vB}}{P}_{\mathrm{BAT}}|}_{\mathrm{kT}}^{(k+1(T}{H}_A^T{\mathrm{\Π}}_{\mathrm{vA}}^T$

${+{\mathrm{\Π}}_{\mathrm{vA}}{P}_{\mathrm{ABT}}|}_{\mathrm{kT}}^{(k+1(T}{H_B^T{\mathrm{\Π}}_{\mathrm{vB}}^T+{\mathrm{\Π}}_{\mathrm{vB}}{P}_{\mathrm{BBT}}|}_{\mathrm{kT}}^{(k+1(T}{H}_B^T{\mathrm{\Π}}_{\mathrm{vB}}^T$

${-[{\mathrm{\Π}}_{\mathrm{vA}}{H}_A{\mathrm{\ξ}}_{\mathrm{AT}}\left(t\right)+{\mathrm{\Π}}_{\mathrm{vB}}{H}_B{\mathrm{\ξ}}_{\mathrm{BT}}\left(t\right)]|}_{\mathrm{kT}}^{(k+1)T}{[{\mathrm{\ξ}}_{\mathrm{AT}}^T\left(t\right)H_A^T{\mathrm{\Π}}_{\mathrm{vA}}^T+{\mathrm{\ξ}}_{\mathrm{BT}}^T\left(t\right)H_B^T{\mathrm{\Π}}_{\mathrm{vB}}^T]|}_{\mathrm{kT}}$

${{\mathrm{\Φ}}_s[(k+1)T,\mathrm{kT}]\≈I+{\mathrm{\Π}}_{\mathrm{sA}}{H}_A{\mathrm{\ξ}}_{\mathrm{AT}}\left(t\right)|}_{\mathrm{kT}}^{(k+1)T}+{{\mathrm{\Π}}_{\mathrm{sB}}{H}_B{\mathrm{\ξ}}_{\mathrm{BT}}\left(t\right)|}_{\mathrm{kT}}^{(k+1)T}{+{\mathrm{\Π}}_{\mathrm{sA}}{P}_{\mathrm{AAT}}|}_{\mathrm{kT}}^{(k+1(T}{H_A^T{\mathrm{\Π}}_{\mathrm{sA}}^T+{\mathrm{\Π}}_{\mathrm{sB}}{P}_{\mathrm{BAT}}|}_{\mathrm{kT}}^{(k+1(T}{H}_A^T{\mathrm{\Π}}_{\mathrm{sA}}^T$

${+{\mathrm{\Π}}_{\mathrm{sA}}{P}_{\mathrm{ABT}}|}_{\mathrm{kT}}^{(k+1(T}{H_B^T{\mathrm{\Π}}_{\mathrm{sB}}^T+{\mathrm{\Π}}_{\mathrm{vB}}{P}_{\mathrm{BBT}}|}_{\mathrm{kT}}^{(k+1(T}{H}_B^T{\mathrm{\Π}}_{\mathrm{sB}}^T$

${-[{\mathrm{\Π}}_{\mathrm{sA}}{H}_A{\mathrm{\ξ}}_{\mathrm{AT}}\left(t\right)+{\mathrm{\Π}}_{\mathrm{sB}}{H}_B{\mathrm{\ξ}}_{\mathrm{BT}}\left(t\right)]|}_{\mathrm{kT}}^{(k+1)T}{[{\mathrm{\ξ}}_{\mathrm{AT}}^T\left(t\right)H_A^T{\mathrm{\Π}}_{\mathrm{sA}}^T+{\mathrm{\ξ}}_{\mathrm{BT}}^T\left(t\right)H_B^T{\mathrm{\Π}}_{\mathrm{sB}}^T]|}_{\mathrm{kT}}$

p，q，r分别为滚转、俯仰、偏航角速度，T为采样周期；全文参数定义相同；

$I=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right],$

ξ_A1(t)＝[t sin(ωt)…sin[(n-1)ωt]sin(nωt)]^T

ξ_B1(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][1cos(ω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)]^T

q(t)＝[q_a0 q_a1 … q_a(n-1) q_an][1cos(ω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)]^T

r(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

${\mathrm{\Π}}_{\mathrm{vA}}=\left[\begin{array}{ccc}0& 0& 0\\ 0& 0& 1\\ 0& -1& \text{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}{ccc}0& 0& -1\\ 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}{ccc}0& 1& 0\\ -1& 0& 0\\ 0& 0& 0\end{array}\right]\left[\begin{array}{ccccc}{r}_{a0}& r_{a1}& \·\·\·& r_{a(n-1)}& r_{\mathrm{an}}\end{array}\right]$

${\mathrm{\Π}}_{\mathrm{sA}}=\left[\begin{array}{ccc}0& 0& 0\\ 0& 0& 1\\ 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}{ccc}0& 0& 1\\ 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}{ccc}0& -1& 0\\ 1& 0& 0\\ 0& 0& 0\end{array}\right]\left[\begin{array}{ccccc}{r}_{a0}& r_{a1}& \·\·\·& r_{a(n-1)}& r_{\mathrm{an}}\end{array}\right]$

${\mathrm{\Π}}_{\mathrm{vB}}=\left[\begin{array}{ccc}0& 0& 0\\ 0& 0& 1\\ 0& -1& \text{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}{ccc}0& 0& -1\\ 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}{ccc}0& 1& 0\\ -1& 0& 0\\ 0& 0& 0\end{array}\right]\left[\begin{array}{ccccc}{r}_{b1}& r_{b2}& \·\·\·& r_{b(n-1)}& r_{\mathrm{bn}}\end{array}\right]$

${\mathrm{\Π}}_{\mathrm{sB}}=\left[\begin{array}{ccc}0& 0& 0\\ 0& 0& 1\\ 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}{ccc}0& 0& 1\\ 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}{ccc}0& -1& 0\\ 1& 0& 0\\ 0& 0& 0\end{array}\right]\left[\begin{array}{ccccc}{r}_{b1}& r_{b2}& \·\·\·& r_{b(n-1)}& r_{\mathrm{bn}}\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}{\mathrm{\ω}},\frac{1}{2\mathrm{\ω}},\·\·\·,\frac{1}{\mathrm{n\ω}}\},$ H_B＝-H_A

2、高度输出为：

$\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为高度；

3、姿态角的输出为：。

$\mathrm{\ψ}\left[(k+1)T\right]=\mathrm{\ψ}\left(\mathrm{kT}\right)+{\∫}_{\mathrm{kT}}^{(k+1)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]}_{t=(k+1)T}={\mathrm{\Φ}}_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}}.$

Claims (1)

1.一种刚体空间运动状态的傅里埃输出方法，其特征包括以下步骤：

机体轴系三个速度分量输出为：

其中：u，v，w分别为沿刚体机体轴系x，y，z轴的速度分量，n_x，n_y，n_z分别为沿x，y，z轴的过载，g为重力加速度，s₁、s₂、s₃为定义的三元数，且

p，q，r分别为滚转、俯仰、偏航角速度，T为采样周期；全文参数定义相同；

ξ_A1(t)＝[t sin(ωt)…sin[(n-1)ωt]sin(nωt)]^T

ξ_B1(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)]^T

q(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)]^T

r(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

H_B＝-H_A

高度输出为：

其中：h为高度；

姿态角的输出为：

其中： 

，ψ分别表示滚转、俯仰、偏航角，