CN107741228A - A Strapdown Inertial Navigation Attitude Calculation Method Based on Center of Gravity Lagrangian Interpolation Method - Google Patents

Abstract

The invention discloses a strapdown inertial navigation attitude solution method based on the center of gravity Lagrangian interpolation method. This method can increase the number of sampling points in the update period under the premise that the device bandwidth is limited and the update frequency is not reduced, and the multi-sample equivalent rotation vector algorithm is used to calculate the attitude and improve the calculation accuracy. Specifically, this method first calculates the interpolated angular rate in the current update period by using the barycentric Lagrangian interpolation method based on the angular rate information sampled in the previous few periods, and then calculates the rotation vector of this period in combination with the real sampling points. The quaternion update equation solves the attitude matrix, and solves the attitude information (attitude angle) within the update period. The invention can effectively reduce the calculation amount in the interpolation process, and at the same time improve the solution precision of the algorithm. Simulation experiments show that: under the same sensor accuracy, update frequency and sampling rate, this method can improve the solution accuracy by 75%.

Description

A Strapdown Inertial Navigation Attitude Calculation Method Based on Center of Gravity Lagrangian Interpolation Method

technical field

The invention belongs to the field of attitude calculation methods of strapdown inertial navigation systems. The attitude calculation method of strapdown inertial navigation is to use the angular rate or angular increment measured by the gyroscope to calculate the attitude of the carrier, and obtain the attitude angle of the carrier and the direction cosine transformed from the carrier coordinate system to the navigation coordinate system matrix (strapdown matrix). The velocity and position of the carrier can be calculated by using the direction cosine matrix and the acceleration measured by the accelerometer. Therefore, the attitude calculation method of the strapdown inertial navigation system is the core method in the strapdown inertial navigation system, and the design of a high-precision attitude calculation method can improve the calculation accuracy of the strapdown inertial navigation system.

Background technique

In the navigation system, the strapdown inertial navigation system is an inertial navigation system that connects the inertial measurement device to the carrier. It is characterized by not relying on external information, simple structure, and high reliability. Although the strapdown inertial navigation system has many advantages compared with other navigation systems, since the inertial device is directly fixed on the carrier, there are high requirements for the accuracy of the inertial device and the strapdown inertial navigation method. In the actual carrier movement, angular vibration and linear vibration will appear on the carrier due to factors such as aerodynamics and engine vibration. These angular vibrations and linear vibrations are sensed by inertial devices, which requires high bandwidth of inertial devices, and at the same time requires the solution method to compensate for non-exchangeable errors caused by angular vibrations and linear vibrations, so as to ensure The accuracy of the SINS in the environment.

Usually, improving the calculation accuracy of the strapdown inertial navigation system can be started from two aspects: one is to improve the accuracy of the device, using high-precision inertial sensors, such as fiber optic gyroscopes, and quartz accelerometers; the other is to design high-precision strapdown inertial sensors. guide method. Since the high-precision inertial devices are very expensive and subject to the technical blockade of other countries, the research on improving the precision and performance of the strapdown inertial navigation method is of great significance.

Usually, in order to compensate the error caused by high dynamic motion, such as coning error, in the attitude calculation of strapdown inertial navigation, the multi-sample equivalent rotation vector method is generally used. The multi-sampling equivalent rotation vector method refers to taking multiple sampling points in one update period. In the case that the update rate is expected to remain unchanged, if you want to improve the accuracy of the method, you can only increase the sampling rate. However, due to the limitation of device bandwidth, sometimes there is no way to increase the sampling rate. In this case, the strapdown inertial navigation attitude solution method based on the center of gravity Lagrangian interpolation method of the present invention can be used to increase the update cycle through mathematical calculations. The number of sampling points in the model, so that the multi-sample equivalent rotation vector method can be used. The simulation experiment proves that this method can improve the accuracy of attitude calculation.

Contents of the invention

1. Lagrangian interpolation method

(a) Definition of Lagrangian interpolation method

Lagrangian interpolation is a method that can interpolate discrete sampling points. For several different sampling points, it can find a polynomial, so that it can just take the corresponding sampling value at each sampling point. This polynomial is called a Lagrange polynomial, so Lagrangian interpolation is a polynomial interpolation method.

The general definition of Lagrange interpolation method is to give k+1 points: (x ₀ ,y ₀ ),...,(x _j ,y _j ),...,(x _k ,y _k ) sampling Points x _j vary, the Lagrangian polynomial is:

in:

From the definition, it can be concluded that:

at the same time Therefore, for all x=x _i except l _i (x) is 1, other l _j (x), i≠j is equal to 0. That is, y _i l _i ( _xi )=y _i , and then L( _xi )=y _i , so the Lagrange polynomial can correctly fit the existing sampling points.

(b) Proof of the uniqueness of Lagrange polynomials

The uniqueness of the Lagrangian polynomial means that there is at most one polynomial whose fitting times do not exceed k for k+1 sampling points. Assuming that there are two Lagrange polynomials L ₁ and L ₂ , their difference L ₁ -L ₂ must be 0 at these k+1 sampling points, so L ₁ -L ₂ must be (xx ₀ )(xx ₁ )...(xx _k ). We assume that L ₁ ≠ L ₂ , then L ₁ -L ₂ ≠ 0, and the degree of L ₁ -L ₂ must be ≥ k+1, but this is inconsistent with the premise that L ₁ and L ₂ are polynomials whose degree does not exceed k . Therefore, L ₁ and L ₂ must be equal, and the Lagrangian polynomial fitted to k+1 sampling points is unique.

(c) Centroid Lagrangian interpolation method

The biggest problem with the application of Lagrange interpolation method in the strapdown inertial navigation solution system is the increase in the amount of calculation. For DSP-based SINS, computing resources are very valuable. On the one hand, the multi-sample solution method will consume more computing resources, the more the number of samples, the more the number of multiplications, the longer the calculation time of a single solution; on the other hand, a faster attitude is required under high dynamic conditions The update frequency, while the attitude update frequency is restricted by the single solution time. Therefore, it is necessary to accelerate the execution efficiency of the method as much as possible in the Lagrangian interpolation method, and the barycentric Lagrange interpolation method or the improved Lagrange interpolation method is to achieve this purpose.

Assuming that the number of sampling points involved in each calculation of the interpolation is n, the time complexity of each calculation of the interpolation polynomial is O(n ² ), because the sampling window is shifted one bit to the right in each update cycle. Each basic polynomial l _j (x) of the Lagrange polynomial needs to be recalculated once, and the time complexity is O(n), so The time complexity of is O(n ² ). However, when each sampling point is changed, not all are updated, but only two points are increased or decreased, so it is not necessary to recalculate all the basic polynomials l _j (x). The expression method of weight can be introduced to calculate.

Let l(x)=(xx ₀ )(xx ₁ )...(xx _k ) define the weight as:

The Lagrange polynomial becomes:

When the sampling window slides a sampling point, w′ _j ＝w _j ×(x _j -x ₀ )÷(x _j -x _k+1 ), so the time complexity of updating the entire Lagrangian polynomial is reduced to O( n).

2. Fitting of the attitude angular rate curve by the center of gravity Lagrangian interpolation method

The purpose of using the interpolation method is to increase the number of sampling points equivalently in an attitude update cycle, so that the multi-sample method of the equivalent rotation vector can be used to further improve the attitude calculation without reducing the update frequency. accuracy. Therefore, the fitting degree of the newly inserted "sampling point" to the original angular rate curve is directly related to the solution effect.

In the attitude calculation, the coning error has the greatest influence, and its error is the largest during the coning motion. Therefore, when studying the attitude calculation, take the conical motion as an example. General coning motion is angular vibration in the direction of two perpendicular axes. Due to the angular vibrations in the directions of these two perpendicular axes, there is a drift in the angular rate in the direction of the third perpendicular axis. The angular rate curve of angular vibration is sinusoidal.

3. Attitude solution method - equivalent rotation vector method

The equivalent rotation vector method compensates to a certain extent the typical error (non-commutability error) in the attitude calculation, so the equivalent rotation vector method has a relatively high accuracy. However, since the calculation of the differential equation of the equivalent rotation vector is more complicated, the amount of calculation required will be larger than that of the quaternion method.

The rotation vector (rotation vector) is a three-element vector, which is often represented by Φ=αn. The direction n of the rotation vector represents the direction of the rotation axis, and the amplitude α of the rotation vector represents the magnitude of the rotation angle. Combined with the triangular representation of the quaternion, the quaternion update process after the equivalent rotation vector is obtained:

The differential equation for the equivalent rotation vector is:

Where ω is the projection of the rotational angular velocity from the carrier to the navigation coordinate system in the carrier coordinate system, that is Generally, we use the simplified form of the above differential equation for easy calculation:

It can even be further streamlined in cases where the calculation frequency needs to be fast:

According to an update period of the equivalent rotation vector and the number of samples of the angular increment, the equivalent rotation vector method can be divided into: single sub-sampling method, double-sub-sampling method, three-sub-sampling method and four-sub-sampling method. The accuracy of the front-to-back method is getting higher and higher, and the computational complexity is also increasing.

4. Two sample method of equivalent rotation vector

In the two-sample method, we generally take two data at equal intervals within the sampling period, and we assume that the change of the angular rate is linear, so there are:

ω(t _k-1 +τ)=a+bτ0≤τ≤h (10)

Expand ω(t _k-1 +τ)Taylor:

It can be obtained from the above two formulas:

From (3.25) we know:

Expand Φ(t _k-1 +h)Taylor:

Wherein, Φ(t _k-1 )=Φ(t _k-1 +0)=0. By (3.25), (3.34), (3.35) can get:

The two-sample method performs two samplings in the [t _k-1 ,t _k ] period to obtain the angle increments θ ₁ , θ ₂ , so

Then it can be solved that a=3θ ₁ -θ ₂ So have:

In the formula, θ=θ ₁ +θ ₂ . The update equation of the quaternion is in the following form, the quaternion represented by the rotation vector can be brought in, and the update quaternion can be calculated by solving. Let the attitude quaternion be Q(t), By The antisymmetric matrix of Indicates the quaternion multiplication, then the differential equation of the attitude quaternion is:

The matrix representation is:

in Each component can be measured and combined with position and velocity information to calculate.

After solving the updated quaternion, it can be used to represent the direction cosine matrix:

The attitude angle information can be calculated from the direction cosine matrix. The attitude angle is defined as the three included angles between the navigation coordinate system and the carrier coordinate system as follows:

Course angle (ψ)—the angle between the longitudinal axis of the vehicle and the north axis of the navigation coordinate system, clockwise is positive.

Pitch angle (θ)—the angle between the longitudinal axis of the vehicle and the horizontal plane measured in the vertical plane, and the upward direction is positive.

Roll angle (γ)——The angle between the longitudinal axis of the vehicle and the horizontal plane measured in the cross section, positive to the left.

Suppose Ψ is the heading angle of the carrier, θ is the pitch angle of the carrier, and γ is the roll angle of the carrier. The navigation coordinate system first rotates Ψ around the Y axis, then rotates θ around the z′ axis, and finally rotates γ around the x” axis, that is, transforms from the navigation system to the carrier system.

We can calculate the three attitude angles directly from the direction cosine matrix: Let We can get the attitude angle information as:

5. Attitude calculation method based on center of gravity Lagrangian interpolation method

Using Lagrangian interpolation can increase the number of sampling points while keeping the attitude update frequency unchanged without increasing the sampling frequency, and then the multi-sample optimization method can be used to improve the solution accuracy. First, make statistics on the angular rate of the carrier motion in the past period measured by the gyroscope, and use the improved Lagrangian interpolation method to calculate the angular rate that needs to be inserted in the current update period:

In the above formula, we use the first n sampling angular rates at the current sampling moment to establish the Lagrangian equation and calculate m interpolation points.

Use the obtained equivalent angular rate of m+2 sampling periods to construct the i angular increments of the sampling period:

Θ=(θ ₁ ,θ ₂ ,...,θ _i )=f(Ω) (22)

f can be a simple integral function, or a high-precision angular increment fitting function can be constructed by using multiple angular rate values. After obtaining the angle increment, the equivalent rotation vector can be constructed, and then the equivalent rotation vector can be used to calculate the quaternion update equation, and the strapdown matrix can be calculated, and finally the attitude information of the carrier can be solved by using the strapdown matrix (attitude angle: horizontal roll, pitch and azimuth).

The abscissa in Figure 2.a is the simulation time, and the vertical axis is the platform misalignment angle in the z-axis direction, which represents the deviation between the calculated strapdown matrix and the real strapdown matrix, which can reflect the calculation error. It can be seen from the figure that the more points inserted, the smaller the deviation of attitude calculation, but the increase in the number of inserted points will increase the amount of calculation, which can be considered as a compromise.

From Figure 2.b, it can be seen that the three high-order interpolation methods of 'spline', 'cubic' and Lagrangian have the smallest solution errors, while using the barycentric Lagrangian interpolation method (improved Lagrangian Daily interpolation method) has the smallest amount of calculation for each update cycle, and is suitable for solving with the equivalent rotation vector method.

Description of drawings

Fig. 1 shows the flow chart of the attitude calculation method based on the center of gravity Lagrangian interpolation method.

Figure 2 shows the simulation diagram of Lagrange interpolation method, where (a) shows the influence of different insertion points on the final solution error, and (b) shows the influence of different interpolation methods on the solution error.

detailed description

Shown in the flow chart according to Fig. 1, introduce the attitude solution method based on the center of gravity Lagrangian interpolation method that the number of insertion points is 1, the specific embodiment of the present invention can be divided into the following steps to complete:

1. System startup

This step includes the initial alignment process of the strapdown inertial navigation system, which provides initial values for the update iteration variables of the entire strapdown inertial navigation system (including: attitude, velocity, position, strapdown matrix, and quaternion, etc.).

2. Linear interpolation method to insert angular rate information

In the first few cycles of the strapdown solution, since the available sampling angular rate information is relatively small, it is not enough to support the calculation of the Langrange interpolation method, so a simple linear interpolation method can be used. Each interpolation point is the average value of the front and rear sampling angular rates: ω _M =(ω _k-1 +ω _k )/2.

3. Calculation of interpolation points by center of gravity Lagrangian interpolation method

Calculate the Lagrangian polynomial L(x) according to the angular velocity of the previous L periods and formula (5) and calculate the inserted value ω _L . Simultaneously update the weights in formula (4) and the Lagrangian polynomial. Since this update uses a sliding window, the update calculation of the weight requires a method complexity of O(1), and the update of the Lagrange polynomial only needs Requires O(L) method complexity. Among them, L is the number of sampling points for calculating the Lagrange polynomial.

4. Calculate the rotation vector, quaternion update equation

After the interpolated angular rate is obtained, the sampling angular rate of the kth period is (ω _k-1 , ω _L , ω _k ) and integrated to obtain two angular increments (Δθ ₁ , Δθ ₂ ). The rotation vector can be obtained by using the equivalent rotation vector method of two samples: Φ(t _k-1 +T)=Δθ ₁ + Then calculate the quaternion update equation and solve the updated strapdown matrix

5. Calculate attitude information

Using the updated strapdown matrix, according to (20), three attitude angle information can be obtained: roll angle, pitch angle and heading angle: set The attitude angle information can be obtained as:

Claims (5)

1. a kind of SINS Attitude calculation method based on center of gravity Lagrange's interpolation comprises the following steps：

Step S1：Using the sampled point of current update cycle and preceding several cycles, interpolation point angular speed is calculated；

Step S2：With reference to interpolation point angular speed, the angle increment in the current update cycle is calculated；

Step S3：Equivalent rotating vector is calculated using angle increment, and substitutes into quaternary number renewal equation, calculates strapdown attitude matrix；

Step S4：Attitude angle information is solved using strapdown attitude matrix, completes to resolve.

2. the attitude algorithm method according to claim 1 based on center of gravity Lagrange's interpolation, it is characterised in that：It is described In step S1, the method for interpolation point is calculated using center of gravity Lagrange's interpolation, while calculating interpolation point, updates center of gravity Lagrange polynomial, next update cycle is facilitated to calculate interpolation point；If Ω is the angular speed vector of current update cycle,For the angular speed value of insertion, (ω_k-1,ω_k-2,·…,ω_k-n) it is preceding n periodic sampling angular speed Value；

<mrow> <mi>&amp;Omega;</mi> <mo>=</mo> <msub> <mi>&amp;omega;</mi> <mi>k</mi> </msub> <mo>,</mo> <mrow> <mo>(</mo> <msub> <mi>&amp;omega;</mi> <msub> <mi>k</mi> <mn>1</mn> </msub> </msub> <mo>,</mo> <msub> <mi>&amp;omega;</mi> <msub> <mi>k</mi> <mn>2</mn> </msub> </msub> <mo>,</mo> <mo>...</mo> <mo>,</mo> <msub> <mi>&amp;omega;</mi> <msub> <mi>k</mi> <mi>m</mi> </msub> </msub> <mo>)</mo> </mrow> <mo>,</mo> <msub> <mi>&amp;omega;</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> </mrow>

<mrow> <mo>(</mo> <msub> <mi>&amp;omega;</mi> <msub> <mi>k</mi> <mn>1</mn> </msub> </msub> <mo>,</mo> <msub> <mi>&amp;omega;</mi> <msub> <mi>k</mi> <mn>2</mn> </msub> </msub> <mo>,</mo> <mo>...</mo> <mo>,</mo> <msub> <mi>&amp;omega;</mi> <msub> <mi>k</mi> <mi>m</mi> </msub> </msub> <mo>)</mo> <mo>=</mo> <mi>L</mi> <mo>(</mo> <msub> <mi>&amp;omega;</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>,</mo> <msub> <mi>&amp;omega;</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>2</mn> </mrow> </msub> <mo>,</mo> <mo>...</mo> <mo>,</mo> <msub> <mi>&amp;omega;</mi> <mrow> <mi>k</mi> <mo>-</mo> <mi>n</mi> </mrow> </msub> <mo>)</mo> </mrow>

WhereinFundamental polynomials l (x)=(x-x₀)(x-x₁)...(x-x_k), weight

3. the attitude algorithm method according to claim 1 based on center of gravity Lagrange's interpolation, it is characterised in that：It is described In step S2, the method that angular speed is transformed into angle increment uses integration method, and the angle increment number of conversion is by the update cycle The angular speed number of sampling and the angular speed number of insertion determine.

4. the attitude algorithm method according to claim 1 based on center of gravity Lagrange's interpolation, it is characterised in that：It is described In step S3, rotating vector is calculated using the equivalent rotating vector method of corresponding sample number according to the number of angle increment, with two increments Exemplified by：

<mrow> <mi>&amp;Phi;</mi> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>+</mo> <mi>T</mi> <mo>)</mo> </mrow> <mo>=</mo> <msub> <mi>&amp;Delta;&amp;theta;</mi> <mn>1</mn> </msub> <mo>+</mo> <msub> <mi>&amp;Delta;&amp;theta;</mi> <mn>2</mn> </msub> <mo>+</mo> <mfrac> <mn>2</mn> <mn>3</mn> </mfrac> <mrow> <mo>(</mo> <msub> <mi>&amp;Delta;&amp;theta;</mi> <mn>1</mn> </msub> <mo>&amp;times;</mo> <msub> <mi>&amp;Delta;&amp;theta;</mi> <mn>2</mn> </msub> <mo>)</mo> </mrow> </mrow>

Wherein, (Δ θ₁,Δθ₂) it is two angle increments, T is the update cycle；And attitude quaternion is updated, solve strapdown posture square Battle array.

5. the attitude algorithm method according to claim 1 based on center of gravity Lagrange's interpolation, it is characterised in that：It is described In step S4, when solving attitude angle information using attitude matrix, if strapdown attitude matrixCan Using obtain the information of attitude angle as：Wherein θ, γ, Ψ are respectively the roll angle in attitude angle, pitching Angle and attitude angle.