CN102495825B - 基于角速度的飞行器极限飞行时四元数超线性输出方法 - Google Patents

Abstract

本发明公开了一种基于角速度的飞行器极限飞行时四元数超线性输出方法，用于解决现有的飞行器极限飞行时惯性设备输出四元数精度差的技术问题。技术方案是通过分析线性时变系统状态转移矩阵的展开式，重新定义有关参数，使得近似表达式的前三项与线性时变系统状态转移矩阵的展开式前三项完全相同，且对后面高阶项有一定逼近程度，通过定义新参数及|λ|，实现了对四元数状态方程转移矩阵Φ_e[(k+1)T，kT]的超线性逼近，直接得到了四元数状态转移矩阵二阶逼近式，保证了确定四元数的计算精度，从而提高了飞行器极限飞行时惯性设备输出精度。

Description

基于角速度的飞行器极限飞行时四元数超线性输出方法

技术领域

本发明涉及一种飞行器机载惯性设备的姿态输出方法，特别涉及一种基于角速度的飞行器极限飞行时四元数超线性输出方法。

背景技术

通常，刚体运动的加速度、角速度和姿态等都依赖于惯性设备输出，因此提高惯性设备的输出精度具有明确的实际意义。飞行器、鱼雷、航天器等空间运动在大多数情况下都采用刚体运动微分方程；而刻画刚体姿态的微分方程又是其中的核心，通常以三个欧拉角即俯仰、滚转和偏航角来描述，通常都由机载惯性设备中俯仰、滚转和偏航角速度解算后输出。当刚体当俯仰角为±90°时，滚转角和偏航角无法定值，同时临近该奇点的区域求解误差过大，导致工程上不可容忍的误差而不能使用；为了避免这一问题，人们采用限制俯仰角取值范围的方法，这使得方程式退化，不能全姿态工作，因而难以广泛用于工程实践。为此，人们基于机载惯性设备中的俯仰、滚转和偏航角速度直接测量值，并采用了方向余弦法、等效转动矢量法、四元数法等输出飞行姿态。

方向余弦法避免了欧拉法的“奇异”现象，用方向余弦法计算姿态矩阵没有方程退化问题，可以全姿态工作，但需要求解九个微分方程，计算量较大，实时性较差，无法满足工程实践要求。等效转动矢量法如单子样递推、双子样转动矢量、三子样转动矢量和四子样旋转矢量法以及在此基础上的各种修正算法和递推算法等。文献中研究旋转矢量时，都是基于速率陀螺输出为角增量的算法。然而在实际工程中，一些陀螺的输出是角速率信号，如光纤陀螺、动力调谐陀螺等。当速率陀螺输出为角速率信号时，旋转矢量法的算法误差明显增大。四元数方法是最为广泛使用的方法，该方法是定义四个欧拉角的函数来计算航姿，能够有效弥补欧拉法的奇异性，只要解四个一阶微分方程式组即可，比方向余弦姿态矩阵微分方程式计算量有明显的减少，能满足工程实践中对实时性的要求。其常用的计算方法有毕卡逼近法、二阶、四阶龙格-库塔法和三阶泰勒展开法等(Paul G.Savage.A Unified MathematicalFramework for Strapdown Algorithm Design[J].Journal of guidance，control，anddynamics，2006，29(2)：237-248)。毕卡逼近法实质是单子样算法，对有限转动引起的不可交换误差没有补偿，在高动态情况下姿态解算中的算法漂移会十分严重。采用四阶龙格-库塔法求解四元数微分方程时，随着积分误差的不断积累，会出现三角函数取值超出±1的现象，从而导致计算发散。泰勒展开法也因计算精度的不足而受到制约，特别是对于飞行器机动飞行，姿态方位角速率通常都较大，而且对姿态的估计精度提出了更高要求，而四元数等参数确定带来的误差使得上述方法大多数情况下不能满足工程精度。

发明内容

为了克服现有四元数输出误差大的问题，本发明提供一种基于角速度的飞行器极限飞行时四元数超线性输出方法，该方法通过分析线性时变系统状态转移矩阵的展开式，重新定义有关参数，使得近似表达式的前三项与线性时变系统状态转移矩阵的展开式前三项完全相同，且对后面高阶项有一定逼近程度，保证确定四元数的计算精度，从而提高飞行器极限飞行时惯性设备输出四元数精度。

本发明解决其技术问题采用的技术方案是，一种基于角速度的飞行器极限飞行时四元数超线性输出方法，其特点是包括以下步骤：

根据四元数连续状态方程

$\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分别为滚转、俯仰、偏航角速度；欧拉角 Ψ分别指滚转、俯仰、偏航角；

状态转移矩阵按照逼近式

${\mathrm{\Φ}}_e[(k+1)T,\mathrm{kT}]\≈\cos \left(\right|\mathrm{\λ}\left|\right)I+\frac{\sin \left(\right|\mathrm{\λ}\left|\right)}{\left|\mathrm{\λ}\right|}\mathrm{\Π}$

及e(k+1)＝Φ_e[(k+1)T，kT]e(k)得到四元数的时间更新值；

其中

$\mathrm{\Π}=\left[\begin{array}{cccc}0& -\stackrel{\‾}{p}& -\stackrel{\‾}{q}& -\stackrel{\‾}{r}\\ \stackrel{\‾}{p}& 0& \stackrel{\‾}{r}& -\stackrel{\‾}{q}\\ \stackrel{\‾}{q}& -\stackrel{\‾}{r}& 0& \stackrel{\‾}{p}\\ \stackrel{\‾}{r}& \stackrel{\‾}{q}& -\stackrel{\‾}{p}& 0\end{array}\right]$

$\stackrel{\‾}{p}=0.5{\∫}_{\mathrm{kT}}^t\mathrm{pdt}+0.25({\∫}_{\mathrm{kT}}^{t}r{\∫}_{\mathrm{kT}}^t\mathrm{qd\τdt}-{\∫}_{\mathrm{kT}}^{t}q{\∫}_{\mathrm{kT}}^t\mathrm{rd\τdt})$

$\stackrel{\‾}{q}=0.5{\∫}_{\mathrm{kT}}^t\mathrm{qdt}+0.25({\∫}_{\mathrm{kT}}^{t}p{\∫}_{\mathrm{kT}}^t\mathrm{rd\τdt}-{\∫}_{\mathrm{kT}}^{t}r{\∫}_{\mathrm{kT}}^t\mathrm{pd\τdt}).$

$\stackrel{\‾}{r}=0.5{\∫}_{\mathrm{kT}}^t\mathrm{rdt}+0.25({\∫}_{\mathrm{kT}}^{t}q{\∫}_{\mathrm{kT}}^t\mathrm{pd\τdt}-{\∫}_{\mathrm{kT}}^{t}p{\∫}_{\mathrm{kT}}^t\mathrm{qd\τdt})$

$\left|\mathrm{\λ}\right|=0.5\sqrt{{\∫}_{\mathrm{kT}}^{t}p{\∫}_{\mathrm{kT}}^t\mathrm{pd\τdt}+{\∫}_{\mathrm{kT}}^{t}q{\∫}_{\mathrm{kT}}^t\mathrm{qd\τdt}+{\∫}_{\mathrm{kT}}^{t}r{\∫}_{\mathrm{kT}}^t\mathrm{rd\τdt}}$

本发明的有益效果是：由于通过分析线性时变系统状态转移矩阵的展开式，重新定义有关参数，使得近似表达式的前三项与线性时变系统状态转移矩阵的展开式前三项完全相同，且对后面高阶项有一定逼近程度，保证了确定四元数的计算精度，从而提高了飞行器极限飞行时惯性设备输出四元数精度。

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

具体实施方式

根据四元数连续状态方程

$\stackrel{\·}{e}=A_{e}e$

和离散状态方程

 e(k+1)＝Φ_e[ek+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分别为滚转、俯仰、偏航角速度；欧拉角 Ψ分别指滚转、俯仰、偏航角；

状态转移矩阵按照逼近式

${\mathrm{\Φ}}_e[(k+1)T,\mathrm{kT}]\≈\cos \left(\right|\mathrm{\λ}\left|\right)I+\frac{\sin \left(\right|\mathrm{\λ}\left|\right)}{\left|\mathrm{\λ}\right|}\mathrm{\Π}$

进行计算

其中

$\mathrm{\Π}=\left[\begin{array}{cccc}0& -\stackrel{\‾}{p}& -\stackrel{\‾}{q}& -\stackrel{\‾}{r}\\ \stackrel{\‾}{p}& 0& \stackrel{\‾}{r}& -\stackrel{\‾}{q}\\ \stackrel{\‾}{q}& -\stackrel{\‾}{r}& 0& \stackrel{\‾}{p}\\ \stackrel{\‾}{r}& \stackrel{\‾}{q}& -\stackrel{\‾}{p}& 0\end{array}\right]$

$\stackrel{\‾}{p}=0.5{\∫}_{\mathrm{kT}}^t\mathrm{pdt}+0.25({\∫}_{\mathrm{kT}}^{t}r{\∫}_{\mathrm{kT}}^t\mathrm{qd\τdt}-{\∫}_{\mathrm{kT}}^{t}q{\∫}_{\mathrm{kT}}^t\mathrm{rd\τdt})$

$\stackrel{\‾}{q}=0.5{\∫}_{\mathrm{kT}}^t\mathrm{qdt}+0.25({\∫}_{\mathrm{kT}}^{t}p{\∫}_{\mathrm{kT}}^t\mathrm{rd\τdt}-{\∫}_{\mathrm{kT}}^{t}r{\∫}_{\mathrm{kT}}^t\mathrm{pd\τdt})$

$\stackrel{\‾}{r}=0.5{\∫}_{\mathrm{kT}}^t\mathrm{rdt}+0.25({\∫}_{\mathrm{kT}}^{t}q{\∫}_{\mathrm{kT}}^t\mathrm{pd\τdt}-{\∫}_{\mathrm{kT}}^{t}p{\∫}_{\mathrm{kT}}^t\mathrm{qd\τdt})$

$\left|\mathrm{\λ}\right|=0.5\sqrt{{\∫}_{\mathrm{kT}}^{t}p{\∫}_{\mathrm{kT}}^t\mathrm{pd\τdt}+{\∫}_{\mathrm{kT}}^{t}q{\∫}_{\mathrm{kT}}^t\mathrm{qd\τdt}+{\∫}_{\mathrm{kT}}^{t}r{\∫}_{\mathrm{kT}}^t\mathrm{rd\τdt}}$

假定在每个采样周期之间p，q，r按照线性变化，p(k)，q(k)，r(k)及p(k+1)，q(k+1)，r(k+1)已知，

${\∫}_{\mathrm{kT}}^{(k+1)T}q{\∫}_{\mathrm{kT}}^{t}q{\mathrm{dt}}_1\mathrm{dt}=\frac{T^2}{8}{[q(k+1)+q\left(k\right)]}^2$

${\∫}_{\mathrm{kT}}^{(k+1)T}q{\∫}_{\mathrm{kT}}^{t}r{\mathrm{dt}}_1\mathrm{dt}-{\∫}_{\mathrm{kT}}^{(k+1)T}r{\∫}_{\mathrm{kT}}^{t}q{\mathrm{dt}}_1\mathrm{dt}=\frac{1}{6}[r\left(k\right)q(k+1)-q\left(k\right)r(k+1)]T^2$

同理可以求得其它有关积分值，可以得到有关计算参数的具体表达式：

$\stackrel{\‾}{p}=\frac{1}{4}[p\left(k\right)+p(k+1)]T+\frac{1}{24}[r(k+1)q\left(k\right)-q(k+1)r\left(k\right)]T^2$

$\stackrel{\‾}{q}=\frac{1}{4}[q\left(k\right)+q(k+1)]T+\frac{1}{24}[p(k+1)r\left(k\right)-r(k+1)p\left(k\right)]T^2$

$\stackrel{\‾}{r}=\frac{1}{4}[r\left(k\right)+r(k+1)]T+\frac{1}{24}[q(k+1)p\left(k\right)-p(k+1)q\left(k\right)]T^2$

${\left|\mathrm{\λ}\right|}^2=0.25{\∫}_{\mathrm{kT}}^{t}p{\∫}_{\mathrm{kT}}^t\mathrm{pd\τdt}+{\∫}_{\mathrm{kT}}^{t}q{\∫}_{\mathrm{kT}}^t\mathrm{qd\τdt}+{\∫}_{\mathrm{kT}}^{t}r{\∫}_{\mathrm{kT}}^t\mathrm{rd\τdt}$

$=\frac{T^2}{32}\{{[p(k+1)+p\left(k\right)]}^2+{[q(k+1)+q\left(k\right)]}^2+{[r(k+1)+r\left(k\right)]}^2\}$

带入 $\mathrm{\Π}=\left[\begin{array}{cccc}0& -\stackrel{\‾}{p}& -\stackrel{\‾}{q}& -\stackrel{\‾}{r}\\ \stackrel{\‾}{p}& 0& \stackrel{\‾}{r}& -\stackrel{\‾}{q}\\ \stackrel{\‾}{q}& -\stackrel{\‾}{r}& 0& \stackrel{\‾}{p}\\ \stackrel{\‾}{r}& \stackrel{\‾}{q}& -\stackrel{\‾}{p}& 0\end{array}\right]$ 中，通过 ${\mathrm{\Φ}}_e[(k+1)T,\mathrm{kT}]\≈\cos \left(\right|\mathrm{\λ}\left|\right)I+\frac{\sin \left(\right|\mathrm{\λ}\left|\right)}{\left|\mathrm{\λ}\right|}\mathrm{\Π}$

及e(k+1)＝Φ_e[(k+1)T，kT]e(k)得到四元数的时间更新值；

当对惯性设备直接输出滚转、俯仰、偏航角速度p，q，r采用三阶逼近描述时，所得结果也接近O(T³)，相比毕卡逼近等方法的O(T²)精度要高。

Claims (1)

1.一种基于角速度的飞行器极限飞行时四元数超线性输出方法，其特征在于包括以下步骤：

根据四元数连续状态方程

$\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分别为滚转、俯仰、偏航角速度；欧拉角ψ分别指滚转、俯仰、偏航角；

建立状态转移矩阵逼近式

${\mathrm{\Φ}}_e[(k+1)T,\mathrm{kT}]\≈\cos \left(\right|\mathrm{\λ}\left|\right)I+\frac{\sin \left(\right|\mathrm{\λ}\left|\right)}{\left|\mathrm{\λ}\right|}\mathrm{\Π}$

及e(k+1)＝Φ_e[(k+1)T，kT]e(k)得到四元数的时间更新输出模型；

其中

$\mathrm{\Π}=\left[\begin{array}{cccc}0& -\stackrel{\‾}{p}& -\stackrel{\‾}{q}& -\stackrel{\‾}{r}\\ \stackrel{\‾}{p}& 0& \stackrel{\‾}{r}& -\stackrel{\‾}{q}\\ \stackrel{\‾}{q}& -\stackrel{\‾}{r}& 0& \stackrel{\‾}{p}\\ \stackrel{\‾}{r}& \stackrel{\‾}{q}& -\stackrel{\‾}{p}& 0\end{array}\right]$

$\stackrel{\‾}{p}=0.5{\∫}_{\mathrm{kT}}^t\mathrm{pdt}+0.25({\∫}_{\mathrm{kT}}^{t}r{\∫}_{\mathrm{kT}}^t\mathrm{qd\τdt}-{\∫}_{\mathrm{kT}}^{t}q{\∫}_{\mathrm{kT}}^t\mathrm{rd\τdt})$

$\stackrel{\‾}{q}=0.5{\∫}_{\mathrm{kT}}^t\mathrm{qdt}+0.25({\∫}_{\mathrm{kT}}^{t}p{\∫}_{\mathrm{kT}}^t\mathrm{rd\τdt}-{\∫}_{\mathrm{kT}}^{t}r{\∫}_{\mathrm{kT}}^t\mathrm{pd\τdt}).$

$\stackrel{\‾}{r}=0.5{\∫}_{\mathrm{kT}}^t\mathrm{rdt}+0.25({\∫}_{\mathrm{kT}}^{t}q{\∫}_{\mathrm{kT}}^t\mathrm{pd\τdt}-{\∫}_{\mathrm{kT}}^{t}p{\∫}_{\mathrm{kT}}^t\mathrm{qd\τdt})$

$\left|\mathrm{\λ}\right|=0.5\sqrt{{\∫}_{\mathrm{kT}}^{t}p{\∫}_{\mathrm{kT}}^t\mathrm{pd\τdt}+{\∫}_{\mathrm{kT}}^{t}q{\∫}_{\mathrm{kT}}^t\mathrm{qd\τdt}+{\∫}_{\mathrm{kT}}^{t}r{\∫}_{\mathrm{kT}}^t\mathrm{rd\τdt}}$