CN106507916B - A kind of direct output intent of the quaternary number based on angular velocity and FPGA - Google Patents

Abstract

The present invention relates to a kind of direct output intent of the quaternary number based on angular velocity and FPGA, the method is by two given optimizing index and approximant, two coefficient vectors are obtained according to least square or other optimization method offline optimizations, approximant according to the two vector sum, obtain two important scalar parameters, one of scalar parameter is multiplied with three angular velocity, the state-transition matrix for obtaining quaternary number approaches value, according to quaternionic vector in kT moment values, obtain in (k+1) T moment quaternionic vector values；The present invention can by FPGA directly by obtain in angular velocity measurement quaternary number output, without DSP or other processing units, not only efficiency high, and fine in the output approximation ratio of the flight system higher to sample rate or Inertial Measurement Unit, can be widely applied in vehicle, aircraft attitude measurement and navigation.

Description

It is a kind of based on the direct output intent of the quaternary number of angular speed and FPGA

Technical field

The present invention relates to a kind of strap-down inertial control method, more particularly to a kind of quaternary number based on angular speed and FPGA is straight Connect output intent.

Background technology

Strap-down inertial control technology is one of current automatic navigation control area research focus, and attitude updating algorithm is its algorithm Core, be also to influence one of principal element of SINS precision.Therefore design and use rational attitude updating algorithm Just turn into the problem for needing to study.Mainly there are following several algorithms to Attitude Calculation from the document published：Euler's horn cupping, Direction cosine method, Rotation Vector, Quaternion Method.

1. Euler method solves attitude angle and obtained by solving Eulerian equation, but Eulerian equation has singularity, works as the angle of pitch For ± 90 ° when, roll angle and yaw angle without Par value, while close on the singular point region solve error it is excessive, cause solve distortion. In order to avoid this problem, people are using the method for limitation angle of pitch span, and this make it that equation is degenerated, it is impossible to full attitude Work, thus be difficult to be widely used in engineering practice.

2. direction cosine method avoids " unusual " phenomenon of Euler method, and calculating attitude matrix with direction cosine method does not have equation degeneration Problem, attitude can work entirely, but need to solve 9 differential equations, and amount of calculation is larger, and real-time is poor, it is impossible to meet engineering Practice calls.

3. Rotation Vector, such as list sample recursion, Shuangzi sample gyration vector, three increment gyration vectors and four increments rotation arrow Amount method and various correction algorithms and recursive algorithm on this basis etc..When studying rotating vector in document, speed is all based on Gyro is output as the algorithm of angle increment.But in Practical Project, the output of some gyros is angle rate signal, such as optical fibre gyro, Dynamic tuned gyroscope etc..When rate gyroscope is output as angle rate signal, the Algorithm Error of rotating vector law is significantly increased.

4. Quaternion Method calculates navigation attitude, can effectively make up the singularity of Euler method, as long as 4 differential equation of first order formula groups of solution , have obvious reduction than direction cosines attitude matrix differential equation amount of calculation, can meet in engineering practice to real-time It is required that.Its conventional computational methods has complete card approximatioss, second order, fourth-order Runge-Kutta method and three rank Taylor expansions etc..Bi Ka Approximatioss is substantially list sample algorithm, to caused by restricted rotational movement can not exchange error do not compensate, the attitude solution in the case of high dynamic Algorithm drift in calculation can be extremely serious.When solving quaternion differential equation using fourth-order Runge-Kutta method, with integral error Constantly accumulation, it may appear that trigonometric function value exceeds ± 1 phenomenon, dissipates so as to cause to calculate.Taylor expansion is also because calculating essence Degree deficiency and be restricted.

In addition, during carrier high maneuver, attitude orientation angular speed is larger, so the real-time calculating for attitude matrix is proposed Higher requirement.The above method all has the problem of resolving is complicated, is unfavorable for hardware and quickly resolves.

The content of the invention

In order to overcome the shortcomings of that attitude updating algorithm is complicated in existing strap-down inertial control method, the present invention provides a kind of base In the direct output intent of the quaternary number of angular speed and FPGA, this method by two given optimizing index and approximant, according to Least square or other optimization method offline optimizations obtain two coefficient vectors, approximant according to the two vector sums, obtain two Scalar parameter, one of scalar parameter is multiplied with three angular speed, and the state-transition matrix for obtaining quaternary number approaches value, according to Quaternionic vector obtains, in (k+1) T moment quaternionic vector values, making appearance in strap-down inertial control method in kT moment values State more new algorithm is simplified.

The technical scheme that the present invention solves the use of its technical problem is that a kind of quaternary number based on angular speed and FPGA is directly exported Method, is characterized in comprising the following steps：

(a) calculated with FPGA

U=[1 T²/σ T⁴/σ²…T^2j/σ^j]^T, u_T=uT

In formula, σ=p²+q²+r², T is the sampling period, and j >=2 are given to approach number of times；

(b) according to optimal index

In formula, h_i=[1 i²H² i⁴H⁴…i^2jH^2j]^T, a₀=[a₀₁ a₀₂…a_0(j+1)], d₀=[d₀₁ d₀₂…d_0(j+1)],

0 ＜ H ＜ T；A is tried to achieve according to least square or other method offline optimizations₀, d₀, then with FPGA in line computation

A=a₀U, d=d₀·u_T

(c) a=a is calculated with FPGA₀U, d=d₀·u_T；

In formula, a is vectorial a₀With vectorial u inner product, by formulaCalculate, carry out three multiplication add operations twice；Press FormulaCalculate, carry out three multiplication add operations twice；

(d) calculated with FPGA

By formulaCarry out three multiplyings；

(d) calculated with FPGA in e [(k+1) T]=Φ e (kT), formula, e (kT) is quaternionic vector in kT moment values；Quaternary Number vector is calculated by component, by formulaCalculate.

The beneficial effects of the invention are as follows：Due to by two given optimizing index and approximant, according to least square or other excellent Change method offline optimization obtains two coefficient vectors, approximant according to the two vector sums, two scalar parameters is obtained, wherein one Individual scalar parameter is multiplied with three angular speed, and the state-transition matrix for obtaining quaternary number approaches value, according to quaternionic vector in kT Quarter is worth, and obtains in (k+1) T moment quaternionic vector values, simplifies attitude updating algorithm in strap-down inertial control method.

The present invention is elaborated with reference to the accompanying drawings and examples.

Brief description of the drawings

Accompanying drawing is the flow chart of the inventive method.

Embodiment

Referring to the drawings, the present invention is described in detail.

The present invention measures tri-axis angular rate using XW-5100IMU, and FPGA is using the EP1C12 serial Cyclone of Alter companies Chip.

1) calculated with FPGA

U=[1 T²/σ T⁴/σ²]^T, u_T=uT

In formula, σ=p²+q²+r², T is the sampling period；

By actual demand, sampling period T=0.02, T are taken²、T⁴Using the numerical value precalculated, without the computing in FPGA. As shown in above formula, calculating σ needs three multipliers, two adders；Vectorial u, u_TCalculated by component, each component, which is calculated, to be used Multiplier and divider are calculated.It is actual calculate in because the IMU tri-axis angular rates exported are decimal, thus should first will be small Numerical value is amplified, and is converted into integer processing；

2) according to optimal index

In formula, h_i=[1 i²H² i⁴H⁴]^T

It can try to achieve according to least square or other method offline optimizations and try to achieve offline optimization：

d₀=[.499999970249-.208328999562D-01 .258872870899D-03]

a₀=[.999999889126-.124996242746 .258592418825D-02]

For convenience of FPGA processing, by a₀、d₀Amplify 2 respectively³¹Times, and it is sign bit to set the first, 0 represents just, and 1 represents It is negative.The binary format being converted into is as follows：

A=a is calculated with FPGA₀U, d=d₀·u_T；

Wherein a is vector a₀With vectorial u inner product, by formulaCalculate, need three multiplication add operations twice.d Using same computational methods, by formulaCalculate, need three multiplication add operations twice；

3) calculated with FPGA

By formulaNeed three multiplyings；

4) e [(k+1) T]=Φ e (kT) are calculated with FPGA, wherein e (kT) is quaternionic vector in kT moment values；Quaternary number to Amount is calculated by component, by formulaCalculate.

Claims (1)

1. it is a kind of based on the direct output intent of the quaternary number of angular speed and FPGA, it is characterised in that to comprise the following steps：

(a) calculated with FPGA

U=[1 T²/σ T⁴/σ²…T^2j/σ^j]^T, u_T=uT

In formula, σ=p²+q²+r², T is the sampling period, and j >=2 are given to approach number of times；

(b) according to optimal index

$J_1=\underset{i=0}{\overset{j}{\Σ}}{\[cos(0.5*i*H)-a_0\·h_i\]}^2,J_2=\underset{i=1}{\overset{j+1}{\Σ}}{\[sin(0.5*i*H)-d_0\·h_i\·i\·H\]}^2$

In formula, h_i=[1 i²H² i⁴H⁴…i^2jH^2j]^T, a₀=[a₀₁ a₀₂…a_0(j+1)], d₀=[d₀₁ d₀₂…d_0(j+1)],

0 ＜ H ＜ T；A is tried to achieve according to least square or other method offline optimizations₀, d₀, then with FPGA in line computation

A=a₀U, d=d₀·u_T

(c) a=a is calculated with FPGA₀U, d=d₀·u_T；

In formula, a is vectorial a₀With vectorial u inner product, by formulaCalculate, carry out three multiplication add operations twice；Press FormulaCalculate, carry out three multiplication add operations twice；

(d) calculated with FPGA

$\mathrm{\Φ}=\left\{{\mathrm{\φ}}_{jj}\right\}=\left[\begin{array}{cccc}a& -\stackrel{\‾}{p}& -\stackrel{\‾}{q}& -\stackrel{\‾}{r}\\ \stackrel{\‾}{p}& a& \stackrel{\‾}{r}& -\stackrel{\‾}{q}\\ \stackrel{\‾}{q}& -\stackrel{\‾}{r}& a& \stackrel{\‾}{p}\\ \stackrel{\‾}{r}& \stackrel{\‾}{q}& -\stackrel{\‾}{p}& a\end{array}\right]$

By formulaCarry out three multiplyings；

(d) calculated with FPGA in e [(k+1) T]=Φ e (kT), formula, e (kT) is quaternionic vector in kT moment values；Quaternary Number vector is calculated by component, by formulaCalculate.