CN102506865B

CN102506865B - Four-ary number polynomial approximate output method during extreme aerobat flight based on angular velocity

Info

Publication number: CN102506865B
Application number: CN201110366736.7A
Authority: CN
Inventors: 史忠科
Original assignee: Northwestern Polytechnical University
Current assignee: Northwestern Polytechnical University
Priority date: 2011-11-17
Filing date: 2011-11-17
Publication date: 2014-08-20
Anticipated expiration: 2031-11-17
Also published as: CN102506865A

Abstract

本发明公开了一种基于角速度的飞行器极限飞行时四元数多项式类近似输出方法，用于解决现有的飞行器极限飞行时惯性设备输出四元数精度差的技术问题。技术方案是采用可以描述任意有限的统一多项式对滚转、俯仰、偏航角速度p，q，r进行近似逼近描述，直接得到四元数状态转移矩阵；可以根据工程精度的要求，事先定义常数矩阵M，减少对滚转、俯仰、偏航角速度p，q，r多项式描述的阶次，实现对四元数状态方程转移矩阵Φe[(k+1)T，kT]的超线性或更高阶逼近，保证了确定四元数的迭代计算精度，从而提高了飞行器极限飞行时惯性设备输出精度。The invention discloses an angular velocity-based quaternion polynomial approximate output method during extreme flight of an aircraft, which is used to solve the existing technical problem of poor precision of quaternion output by inertial equipment during extreme flight of an aircraft. The technical solution is to use an arbitrary finite unified polynomial to approximate and describe the roll, pitch, and yaw angular velocities p, q, and r, and directly obtain the quaternion state transition matrix; the constant matrix can be defined in advance according to the requirements of engineering precision M, reduce the order of the roll, pitch, yaw angular velocity p, q, r polynomial description, and realize the superlinear or higher order of the quaternion state equation transition matrix Φe[(k+1)T, kT] Approximation ensures the iterative calculation accuracy of determining the quaternion, thereby improving the output accuracy of the inertial equipment during the extreme flight of the aircraft.

Description

Quaternion Polynomial-like Approximate Output Method Based on Angular Velocity for Aircraft Limit Flight

技术领域 technical field

本发明涉及一种飞行器机载惯性设备的姿态输出方法，特别涉及一种基于角速度的飞行器极限飞行时四元数多项式类近似输出方法。The invention relates to an attitude output method of an airborne inertial device of an aircraft, in particular to a quaternion polynomial approximate output method based on an angular velocity when the aircraft is in flight.

背景技术 Background technique

通常，刚体运动的加速度、角速度和姿态等都依赖于惯性设备输出，因此提高惯性设备的输出精度具有明确的实际意义。飞行器、鱼雷、航天器等空间运动在大多数情况下都采用刚体运动微分方程；而刻画刚体姿态的微分方程又是其中的核心，通常以三个欧拉角即俯仰、滚转和偏航角来描述，通常都由机载惯性设备中俯仰、滚转和偏航角速度解算后输出。当刚体当俯仰角为±90°时，滚转角和偏航角无法定值，同时临近该奇点的区域求解误差过大，导致工程上不可容忍的误差而不能使用；为了避免这一问题，人们采用限制俯仰角取值范围的方法，这使得方程式退化，不能全姿态工作，因而难以广泛用于工程实践。为此，人们基于机载惯性设备中的俯仰、滚转和偏航角速度直接测量值，并采用了方向余弦法、等效转动矢量法、四元数法等输出飞行姿态。Usually, the acceleration, angular velocity, and attitude of rigid body motion depend on the output of inertial equipment, so improving the output accuracy of inertial equipment has clear practical significance. Space motions such as aircraft, torpedoes, and spacecraft use differential equations of rigid body motion in most cases; and the differential equations describing the attitude of rigid bodies are the core of them, usually with three Euler angles, namely pitch, roll and yaw angles To describe, usually the pitch, roll and yaw angular velocities in the airborne inertial equipment are calculated and output. When the pitch angle of the rigid body is ±90°, the roll angle and yaw angle cannot be fixed. At the same time, the solution error of the area near the singularity is too large, resulting in an intolerable error in engineering and cannot be used; in order to avoid this problem, People use the method of limiting the value range of the pitch angle, which degenerates the equation and cannot work at all attitudes, so it is difficult to be widely used in engineering practice. For this reason, based on the direct measurements of the pitch, roll and yaw angular velocities in the airborne inertial equipment, the direction cosine method, the equivalent rotation vector method, and the quaternion method are used to output the flight attitude.

方向余弦法避免了欧拉法的“奇异”现象，用方向余弦法计算姿态矩阵没有方程退化问题，可以全姿态工作，但需要求解九个微分方程，计算量较大，实时性较差，无法满足工程实践要求。等效转动矢量法如单子样递推、双子样转动矢量、三子样转动矢量和四子样旋转矢量法以及在此基础上的各种修正算法和递推算法等。文献中研究旋转矢量时，都是基于速率陀螺输出为角增量的算法。然而在实际工程中，一些陀螺的输出是角速率信号，如光纤陀螺、动力调谐陀螺等。当速率陀螺输出为角速率信号时，旋转矢量法的算法误差明显增大。四元数方法是最为广泛使用的方法，该方法是定义四个欧拉角的函数来计算航姿，能够有效弥补欧拉法的奇异性，只要解四个一阶微分方程式组即可，比方向余弦姿态矩阵微分方程式计算量有明显的减少，能满足工程实践中对实时性的要求。其常用的计算方法有毕卡逼近法、二阶、四阶龙格-库塔法和三阶泰勒展开法等(Paul G.Savage.A Unified MathematicalFramework for Strapdown Algorithm Design[J].Journal of guidance，control，anddynamics，2006，29(2)：237-248)。毕卡逼近法实质是单子样算法，对有限转动引起的不可交换误差没有补偿，在高动态情况下姿态解算中的算法漂移会十分严重。采用四阶龙格-库塔法求解四元数微分方程时，随着积分误差的不断积累，会出现三角函数取值超出±1的现象，从而导致计算发散。泰勒展开法也因计算精度的不足而受到制约，特别是对于飞行器机动飞行，姿态方位角速率通常都较大，而且对姿态的估计精度提出了更高要求，而四元数等参数确定带来的误差使得上述方法大多数情况下不能满足工程精度。The direction cosine method avoids the "singularity" phenomenon of the Euler method. The calculation of the attitude matrix by the direction cosine method has no problem of equation degradation, and it can work in all attitudes, but it needs to solve nine differential equations, which requires a large amount of calculation and poor real-time performance. Meet the requirements of engineering practice. Equivalent rotation vector methods, such as single-sample recursion, double-sample rotation vector, three-sample rotation vector, and four-sample rotation vector methods, as well as various correction algorithms and recursive algorithms based on this. When studying the rotation vector in the literature, it is based on the algorithm that the output of the rate gyro is an angular increment. However, in actual engineering, the output of some gyroscopes is an angular rate signal, such as fiber optic gyroscopes, dynamic tuning gyroscopes, etc. When the output of the rate gyro is an angular rate signal, the algorithm error of the rotation vector method increases obviously. The quaternion method is the most widely used method. This method is to define the function of four Euler angles to calculate the attitude, which can effectively compensate for the singularity of the Euler method. It only needs to solve four first-order differential equations. The amount of calculation of the direction cosine attitude matrix differential equation is significantly reduced, which can meet the real-time requirements in engineering practice. The commonly used calculation methods include Picard approximation method, second-order, fourth-order Runge-Kutta method and third-order Taylor expansion method (Paul G.Savage.A Unified Mathematical Framework for Strapdown Algorithm Design[J].Journal of guidance, control, and dynamics, 2006, 29(2): 237-248). The essence of the Picasat approximation method is a single sample algorithm, which does not compensate for the non-exchangeable error caused by the finite rotation, and the algorithm drift in the attitude calculation will be very serious under high dynamic conditions. When using the fourth-order Runge-Kutta method to solve quaternion differential equations, with the continuous accumulation of integral errors, the value of trigonometric functions will exceed ±1, which will lead to calculation divergence. The Taylor expansion method is also restricted by the lack of calculation accuracy, especially for aircraft maneuvering, the attitude and azimuth rate are usually relatively large, and higher requirements are placed on the estimation accuracy of the attitude, and the determination of parameters such as quaternions brings The error makes the above method can not meet the engineering accuracy in most cases.

发明内容 Contents of the invention

为了克服现有四元数输出误差大的问题，本发明提供一种基于角速度的飞行器极限飞行时四元数超线性输出方法，该方法采用可以描述任意有限的统一多项式对滚转、俯仰、偏航角速度p，q，r进行近似逼近描述，直接得到四元数状态转移矩阵，可以保证确定四元数的迭代计算精度，从而提高飞行器极限飞行时惯性设备输出四元数精度。In order to overcome the problem of large error in the existing quaternion output, the present invention provides a method for superlinear output of quaternion based on angular velocity when the aircraft is in flight. Angular velocities p, q, r are approximated and described, and the quaternion state transition matrix is directly obtained, which can ensure the iterative calculation accuracy of the quaternion, thereby improving the precision of the quaternion output by the inertial equipment during the extreme flight of the aircraft.

本发明解决其技术问题采用的技术方案是，一种基于角速度的飞行器极限飞行时四元数多项式类近似输出方法，其特点是包括以下步骤：The technical scheme that the present invention solves its technical problem adopts is, a kind of quaternion number polynomial class approximate output method when the aircraft limit flight based on angular velocity, it is characterized in that comprising the following steps:

根据四元数连续状态方程According to the quaternion continuous state equation

$\overset{\cdot &Center Dot;}{e e} = = {A A}_{e e} e e$

和离散状态方程and the discrete equation of state

e(k+1)＝Φ_e[(k+1)T，kT]e(k)e(k+1)=Φ _e [(k+1)T, kT]e(k)

其中e＝[e₁，e₂，e₃，e₄]^T $A_{e} = \frac{1}{2} [\begin{matrix} 0 & - p & - q & - r \\ p & 0 & r & - q \\ q & - r & 0 & p \\ r & q & - p & 0 \end{matrix}]$ where e=[e ₁ , e ₂ , e ₃ , e ₄ ] ^T $A_{e} = \frac{1}{2} [\begin{matrix} 0 & - p & - q & - r \\ p & 0 & r & - q \\ q & - r & 0 & p \\ r & q & - p & 0 \end{matrix}]$

Φ_e[(k+1)T，kT]为A_e的状态转移矩阵，T为采样周期，全文符号定义相同；Φ _e [(k+1)T, kT] is the state transition matrix of A _e , T is the sampling period, and the full-text symbols are defined the same;

p，q，r分别为滚转、俯仰、偏航角速度；欧拉角θ，ψ分别指滚转、俯仰、偏航角；p, q, r are roll, pitch, yaw angular velocity respectively; Euler angle θ, ψ refer to roll, pitch and yaw angle respectively;

状态转移矩阵按照逼近式The state transition matrix according to the approximation

${Φ Φ}_{e e} [[((k k + + 11)) T T,, kT kT]] \approx \approx I I + + ΠMHξ ΠMHξ ((t t)) {| |}_{kT kT}^{((k k + + 11)) T T} + + ΠMP ΠMP ((t t)) {| |}_{kT kT}^{((k k + + 11)) T T} {H h}^{T T} {M m}^{T T} {Π Π}_{11} - - ΠMHξ ΠMHξ ((t t)) {| |}_{kT kT}^{((k k + + 11)) T T} ΠMHξ ΠMHξ ((kT kT))$

及e(k+1)＝Φ_e[(k+1)T，kT]e(k)得到四元数的时间更新值；And e(k+1)= _Φe [(k+1)T, kT]e(k) obtains the time update value of the quaternion;

其中 $I = [\begin{matrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{matrix}],$ ξ(y)＝[t t²…tⁿ tⁿ⁺¹]^T in $I = [\begin{matrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{matrix}],$ ξ(y)=[t t ² …t ⁿ t ⁿ⁺¹ ] ^T

滚转、俯仰、偏航角速度p，q，r的展开式分别为The roll, pitch, and yaw angular velocities p, q, and r are expanded as

p(t)＝[p₀ p₁…p_n-1 p_n]M[1 t … t^n-1 tⁿ]^T p(t)＝[p ₀ p ₁ …p _n-1 p _n ]M[1 t … t ^n-1 t ⁿ ] ^T

q(t)＝[q₀ q₁…q_n-1 q_n]M[1 t … t^n-1 tⁿ]^T q(t)＝[q ₀ q ₁ ...q _n-1 q _n ]M[1 t ... t ^n-1 t ⁿ ] ^T

r(t)＝[r₀ r₁…r_n-1 r_n]M[1 t … t^n-1 tⁿ]^T r(t)＝[r ₀ r ₁ …r _n-1 r _n ]M[1 t … t ^n-1 t ⁿ ] ^T

M为事先定义的常数矩阵；M is a constant matrix defined in advance;

$Π Π = = \frac{11}{22} {{[\begin{matrix} 00 & - - 11 & 00 & 00 \\ 11 & 00 & 00 & 00 \\ 00 & 00 & 00 & 11 \\ 00 & 00 & - - 11 & 00 \end{matrix}] [\begin{matrix} {p p}_{00} & {p p}_{11} & \cdot \cdot & \cdot \cdot & \cdot &Center Dot; & {p p}_{n no - - 11} & {p p}_{n no} \end{matrix}]$

$+ + [\begin{matrix} 00 & 00 & - - 11 & 00 \\ 00 & 00 & 00 & - - 11 \\ 11 & 00 & 00 & 00 \\ 00 & 11 & 00 & 00 \end{matrix}] [\begin{matrix} {q q}_{00} & {q q}_{11} & \cdot &Center Dot; & \cdot &Center Dot; & \cdot &Center Dot; & {q q}_{n no - - 11} & {q q}_{n no} \end{matrix}] + + [\begin{matrix} 00 & 00 & 00 & - - 11 \\ 00 & 00 & 11 & 00 \\ 00 & - - 11 & 00 & 00 \\ 11 & 00 & 00 & 00 \end{matrix}] [\begin{matrix} {r r}_{00} & {r r}_{11} & \cdot &Center Dot; & \cdot &Center Dot; & \cdot &Center Dot; & {r r}_{n no - - 11} & {r r}_{n no} \end{matrix}]}}$

${Π Π}_{11} = = \frac{11}{22} {{{[\begin{matrix} {p p}_{00} & {p p}_{11} & \cdot &Center Dot; & \cdot &Center Dot; & \cdot &Center Dot; & {p p}_{n no - - 11} & {p p}_{n no} \end{matrix}]}^{T T} [\begin{matrix} 00 & - - 11 & 00 & 00 \\ 11 & 00 & 00 & 00 \\ 00 & 00 & 00 & 11 \\ 00 & 00 & - - 11 & 00 \end{matrix}]$

$+ + {[\begin{matrix} {q q}_{00} & {q q}_{11} & \cdot &Center Dot; & \cdot &Center Dot; & \cdot &Center Dot; & {q q}_{n no - - 11} & {q q}_{n no} \end{matrix}]}^{T T} [\begin{matrix} 00 & 00 & - - 11 & 00 \\ 00 & 00 & 00 & - - 11 \\ 11 & 00 & 00 & 00 \\ 00 & 11 & 00 & 00 \end{matrix}] + + {[\begin{matrix} {r r}_{00} & {r r}_{11} & \cdot &Center Dot; & \cdot &Center Dot; & \cdot &Center Dot; & {r r}_{n no - - 11} & {r r}_{n no} \end{matrix}]}^{T T} [\begin{matrix} 00 & 00 & 00 & - - 11 \\ 00 & 00 & 11 & 00 \\ 00 & - - 11 & 00 & 00 \\ 11 & 00 & 00 & 00 \end{matrix}]}}$

$H h = = diag diag {{11,, \frac{11}{22},, \frac{11}{33},, \cdot &Center Dot; \cdot &Center Dot; \cdot &Center Dot;,, \frac{11}{n no},, \frac{11}{n no + + 11}}};;$

本发明的有益效果是：由于采用可以描述任意有限的统一多项式对滚转、俯仰、偏航角速度p，q，r进行近似逼近描述，直接得到四元数状态转移矩阵，保证了确定四元数的迭代计算精度，从而提高了飞行器极限飞行时惯性设备输出四元数精度The beneficial effects of the present invention are: due to the use of any finite unified polynomial that can describe the roll, pitch, and yaw angular velocities p, q, and r for approximate approximation, the quaternion state transition matrix is directly obtained, ensuring the determination of the quaternion Iterative calculation accuracy, which improves the quaternion accuracy of the inertial device output during the extreme flight of the aircraft

下面结合实施例对本发明作详细说明。The present invention is described in detail below in conjunction with embodiment.

具体实施方式 Detailed ways

$\overset{\cdot &Center Dot;}{e e} = = {A A}_{e e} e e$

和离散状态方程and the discrete equation of state

e(k+1)＝Φ_e[(k+1)T，kT]e(k)e(k+1)=Φ _e [(k+1)T, kT]e(k)

Φ_e[(k+1)T，kT]为A_e的状态转移矩阵，T为采样周期，Φ _e [(k+1)T, kT] is the state transition matrix of A _e , T is the sampling period,

${Φ Φ}_{e e} = = [[((k k + + 11)) T T,, kT kT]] \approx \approx I I + + ΠMHξ ΠMHξ ((t t)) {| |}_{kT kT}^{((k k + + 11)) T T} + + ΠMP ΠMP ((t t)) {| |}_{kT kT}^{((k k + + 11)) T T} {H h}^{T T} {M m}^{T T} {Π Π}_{11} - - ΠMHξ ΠMHξ ((t t)) {| |}_{kT kT}^{((k k + + 11)) T T} ΠMHξ ΠMHξ ((kT kT))$

其中 $I = [\begin{matrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{matrix}],$ ξ(t)＝[t t²…tⁿ tⁿ⁺¹]^T in $I = [\begin{matrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{matrix}],$ ξ(t)=[t t ² …t ⁿ t ⁿ⁺¹ ] ^T

M为事先定义的常数矩阵，对于Chebyshev(切比雪夫)正交多项式：M is a constant matrix defined in advance, for Chebyshev (Chebyshev) orthogonal polynomial:

$\begin{matrix} ξ_{0} (t) = 1 \\ ξ_{1} (t) = 1 - 2 t / b \\ ξ_{2} (t) = 8 {(t / b)}^{2} - 8 (t / b) + 1 \\ \cdot \\ \cdot \\ \cdot \\ ξ_{i + 1} (t) = 2 ξ_{1} (t) ξ_{i} (t) - ξ_{i - 1} (t) \end{matrix}\}$ i＝2，3，…，n-1，0≤t≤NT，b＝NT $\begin{matrix} ξ_{0} (t) = 1 \\ ξ_{1} (t) = 1 - 2 t / b \\ ξ_{2} (t) = 8 {(t / b)}^{2} - 8 (t / b) + 1 \\ &Center Dot; \\ &Center Dot; \\ &Center Dot; \\ ξ_{i + 1} (t) = 2 ξ_{1} (t) ξ_{i} (t) - ξ_{i - 1} (t) \end{matrix}\}$ i=2, 3,..., n-1, 0≤t≤NT, b=NT

则常数矩阵Then the constant matrix

$m (i, j) = 2 m (i - 1, j) - m (i - 2, j) - \frac{4}{b} m (i - 1, j - 1),$ (i＝3，4，…，N；j＝1，2，…，i) $m (i, j) = 2 m (i - 1, j) - m (i - 2, j) - \frac{4}{b} m (i - 1, j - 1),$ (i=3, 4, ..., N; j = 1, 2, ..., i)

m(i，j)＝0，(j＞i)m(i,j)=0, (j>i)

m(i，0)＝0，(j＝1，2，…，N)m(i,0)=0, (j=1,2,...,N)

${Π Π}_{11} = = \frac{11}{22} {{{[\begin{matrix} {p p}_{00} & {p p}_{11} & \cdot \cdot & \cdot &Center Dot; & \cdot \cdot & {p p}_{n no - - 11} & {p p}_{n no} \end{matrix}]}^{T T} [\begin{matrix} 00 & - - 11 & 00 & 00 \\ 11 & 00 & 00 & 00 \\ 00 & 00 & 00 & 11 \\ 00 & 00 & - - 11 & 00 \end{matrix}]$

$+ + {[\begin{matrix} {q q}_{00} & {q q}_{11} & \cdot &Center Dot; & \cdot \cdot & \cdot \cdot & {q q}_{n no - - 11} & {q q}_{n no} \end{matrix}]}^{T T} [\begin{matrix} 00 & 00 & - - 11 & 00 \\ 00 & 00 & 00 & - - 11 \\ 11 & 00 & 00 & 00 \\ 00 & 11 & 00 & 00 \end{matrix}] + + {[\begin{matrix} {r r}_{00} & {r r}_{11} & \cdot \cdot & \cdot &Center Dot; & \cdot \cdot & {r r}_{n no - - 11} & {r r}_{n no} \end{matrix}]}^{T T} [\begin{matrix} 00 & 00 & 00 & - - 11 \\ 00 & 00 & 11 & 00 \\ 00 & - - 11 & 00 & 00 \\ 11 & 00 & 00 & 00 \end{matrix}]}}$

当对惯性设备直接输出滚转、俯仰、偏航角速度p，q，r采用三阶逼近描述时，所得结果也接近O(T³)，相比毕卡逼近等方法的O(T²)精度要高。When the inertial equipment directly outputs the roll, pitch, and yaw angular velocities p, q, and r using the third-order approximation description, the obtained result is also close to O(T ³ ), compared with the O(T ² ) accuracy of the Picard approximation and other methods to be tall.

Claims

1. A quaternion polynomial class approximation output method based on angular velocity aircraft limit flight, it is characterized in that comprising the following steps:

According to the quaternion continuous state equation

and the discrete equation of state

e(k+1)=Φ _e [(k+1)T, kT]e(k)

where e=[e ₁ , e ₂ , e ₃ , e ₄ ] ^T

Φ _e [(k+1)T, kT] is the state transition matrix of A _e , and T is the sampling period;

p, q, r are roll, pitch, yaw angular velocity respectively; Euler angle Respectively refer to roll, pitch, yaw angle;

The state transition matrix according to the approximation formula

And e(k+1)=Φ _e [(k+1)T, kT]e(k) to get the time update value of the quaternion;

in

The roll, pitch, and yaw angular velocities p, q, and r are expanded as

p(t)=[p ₀ p ₁ … p _n-1 p _n ]M[1 t … t ^n-1 t ⁿ ] ^T

q(t)＝[q ₀ q ₁ ... q _n-1 q _n ]M[1 t ... t ^n-1 t ⁿ ] ^T

r(t)=[r ₀ r ₁ … r _n-1 r _n ]M[1 t … t ^n-1 t ⁿ ] ^T

M is a constant matrix defined in advance;