Initial velocity perturbation cancellation to eliminate polar region misalignment

Abstract

The application discloses a method for eliminating initial speed disturbance and aligning a polar region dynamic base, and solves the problem that the polar region alignment is easily affected by initial speed error, and comprises the following steps: step 1, acquiring sensor data and combined navigation data; step 2, designing a data conversion method for middle and high latitudes; step 3, performing attitude updating and constructing a speed auxiliary vector observer model; step 4, designing an initial speed disturbance elimination model; step 5, realizing attitude determination by using an OBA algorithm; step 6, the iteration number of the initial alignment process is M, if k=M, the estimated error parameter is output, the initial alignment is completed, if k<M, the alignment process is not completed, and the steps 1 to 5 are repeated until the alignment process is completed. The application uses speed assistance to perform polar region alignment, realizes polar region simulation by using a data conversion principle, designs an initial speed disturbance elimination method, weakens the influence of initial speed disturbance on the initial alignment, and improves the alignment robustness.

Description

TECHNICAL FIELD

[0001] The application relates to the polar region navigation field of an inertial navigation system, and particularly relates to a method for eliminating initial speed disturbance of polar region moving base alignment. BACKGROUND

[0002] A strapdown inertial navigation system is an autonomous navigation positioning system, and an initial alignment process is a prerequisite for ensuring that the strapdown inertial navigation system can normally work. At present, the initial alignment process of the strapdown inertial navigation system is carried out under low-latitude conditions, which limits the application range of the strapdown inertial navigation system. Especially for equipment that needs to operate in the polar region, the initial alignment process usually needs to be completed under polar region conditions. However, due to the rapid convergence of longitude in the polar region, the alignment error in the polar region is prone to divergence due to the convergence of longitude. In addition, the alignment in the polar region also has the problem that the gravity vector and the earth rotation vector are collinear, and self-alignment cannot be performed.

[0003] Based on the above problems, the method for eliminating initial speed disturbance of polar region moving base alignment has important significance for equipment that needs to operate in the polar region. SUMMARY

[0004] The application aims to provide a method for eliminating initial speed disturbance of polar region moving base alignment, which realizes polar region navigation simulation by using data conversion, eliminates initial speed disturbance by using vector subtraction, improves the robustness of polar region alignment, and solves the problem that the polar region alignment is easily affected by initial speed error.

[0005] The technical solution for achieving the object of the application is as follows:

[0006] A method for eliminating initial speed disturbance of polar region moving base alignment comprises the following steps:

[0007] Step 1: Obtain sensor data and integrated navigation data in middle latitudes.

[0008] Step 2: Design a middle-high latitude data conversion method to convert the middle latitude data to high latitudes.

[0009] Step 3: Based on the converted high latitude data, update the attitude and construct a speed auxiliary vector observer model.

[0010] Step 4: Design an initial speed disturbance elimination model to remove the initial speed disturbance, determine the observation vector of the disturbance-free speed, and construct a new reference vector.

[0011] Step 5: Use the OBA algorithm to determine the attitude.

[0012] Step 6: If k=M, the initial alignment process is completed based on the determined attitude output error parameters, and if k<M, the alignment process is not completed, the above steps 1 to 5 are repeated until the alignment process is completed.

[0013] Compared with the prior art, the present application has the following beneficial effects:

[0014] (1) The present application can realize the conversion of middle latitude data to high latitude data by using data conversion principle.

[0015] (2) The initial speed disturbance elimination technology of the present application can realize the removal of initial speed disturbance, improve the alignment robustness, and solve the problem that the polar region alignment is easily affected by initial speed error. BRIEF DESCRIPTION OF DRAWINGS

[0016] Figure 1 is the alignment flowchart of the method of the present application.

[0017] Figure 2 is the motion trajectory diagram of the simulation experiment of the method of the present application.

[0018] Figure 3 is the motion attitude and speed diagram of the simulation experiment of the method of the present application.

[0019] Figure 4 is the pitch error diagram of the simulation experiment of the method of the present application.

[0020] Figure 5 is the roll error diagram of the simulation experiment of the method of the present application.

[0021] Figure 6 is the heading error diagram of the simulation experiment of the method of the present application. DETAILED DESCRIPTION

[0022] The present application will be further described in detail below in combination with the drawings and implementation examples:

[0023] The initial speed disturbance elimination polar dynamic base alignment method proposed by the present application, as shown in Figure 1 , includes the following steps:

[0024] Step 1: Obtain sensor data and integrated navigation data;

[0025] When the dynamic base alignment is performed, the inertial navigation data and satellite navigation data need to be obtained, and the inertial sensor measurement model can be expressed as:

[0026]

[0027] In the formula, where a denotes the mid-latitude accelerometer output acceleration; f b denotes the mid-latitude true acceleration; b a denotes the acceleration bias; and η a denotes the accelerometer measurement noise; where ω denotes the mid-latitude gyroscope output angular velocity; where ω denotes the mid-latitude true angular velocity; b g denotes the gyroscope bias; and η g denotes the gyroscope measurement noise;

[0028] The satellite navigation system output can be expressed as:

[0029]

[0030] where p n denotes the satellite navigation system output position; p denotes the true position; where η p denotes the position noise; where v n denotes the satellite navigation output velocity; v denotes the true velocity;

[0031] Step 2: Designing the mid-high latitude data conversion method

[0032] The mid-latitude data is converted to simulate the high-latitude carrier motion in consideration of the need for polar navigation algorithm implementation. The conversion process is performed in the grid coordinate system, and the conversion condition is:

[0033]

[0034] where C denotes the mid-latitude carrier attitude direction cosine matrix in the grid coordinate system; where C denotes the high-latitude carrier attitude direction cosine matrix in the new grid coordinate system after conversion; where v denotes the mid-latitude carrier motion velocity mapping in the grid coordinate system;

[0035] The above formula indicates that the velocity and attitude of the carrier remain unchanged during the motion, but due to the mid-high latitude data conversion, the position needs to be updated and calculated using the velocity after conversion:

[0036]

[0037] where p denotes the high-latitude position after conversion; where C denotes the high-latitude carrier attitude direction cosine matrix in the new grid coordinate system after conversion;

[0038] And the corresponding high-latitude navigation system position can be expressed as And the corresponding high-latitude navigation system position can be expressed as​​​​​

[0039]

[0040] where, represents the latitude after conversion; represents the longitude after conversion; represents the altitude after conversion; represents the x-axis information of the high latitude position on the west of the earth; represents the y-axis information of the high latitude position on the west of the earth; represents the z-axis information of the high latitude position on the west of the earth; e represents the flattening of the earth; R e represents the major axis of the earth; R p represents the minor axis of the earth; R N represents the radius of curvature of the equinoctial circle of the earth; R and θ represent the process parameters, which can be calculated as:

[0041]

[0042] where, represents the x-axis information of the high latitude position on the west of the earth; represents the y-axis information of the high latitude position on the west of the earth; represents the z-axis information of the high latitude position on the west of the earth; R e represents the major axis of the earth; R p represents the minor axis of the earth; R and θ represent the process parameters;

[0043] Based on the above analysis, the gyro output after conversion can be represented as

[0044]

[0045] where, represents the gyro output angular velocity after conversion; represents the true value angular velocity of the grid system relative to the inertial system after conversion; represents the high latitude angular velocity true value of the grid system relative to the inertial system; b g represents the gyro zero offset; η g represents the gyro measurement noise; represents the middle latitude angular velocity true value of the grid system relative to the inertial system; represents the middle latitude gyro output angular velocity; represents the middle latitude angular velocity true value of the grid system relative to the inertial system; represents the middle latitude grid system attitude direction cosine matrix under the inertial system; represents the high latitude grid system attitude direction cosine matrix under the new inertial system after conversion; represents the middle latitude angular velocity true value of the earth system relative to the inertial system; represents the true angular velocity of the middle latitude grid system relative to the earth system; represents the true angular velocity of the high latitude earth system relative to the inertial system; represents the true angular velocity of the high latitude grid system relative to the earth system;

[0046] The accelerometer conversion output is:

[0047]

[0048] In the formula, represents the accelerometer output acceleration after conversion; represents the middle latitude accelerometer output acceleration; represents the direction cosine matrix of the middle latitude grid system attitude in the carrier system; represents the direction cosine matrix of the high latitude grid system attitude in the new carrier system after conversion; represents the true angular velocity of the middle latitude earth system relative to the inertial system; represents the true angular velocity of the middle latitude grid system relative to the earth system; represents the true angular velocity of the high latitude earth system relative to the inertial system; represents the true angular velocity of the high latitude grid system relative to the earth system; represents the mapping of the middle latitude carrier motion velocity in the grid coordinate system; represents the mapping of the high latitude velocity in the new grid coordinate system after conversion; represents the direction cosine matrix of the middle latitude navigation system attitude in the grid system; represents the direction cosine matrix of the high latitude navigation system attitude in the grid system;g n represents the gravity at low latitude; represents the gravity at high latitude;

[0049] The satellite navigation output after conversion is:

[0050]

[0051] In the formula, represents the position output after conversion; represents the high latitude navigation system position; represents the satellite navigation system output position; represents the middle latitude navigation system position; represents the satellite navigation output velocity; represents the satellite navigation output after conversion;

[0052] Step 3: Perform attitude update and velocity auxiliary vector observer model construction;

[0053] According to the basic principle of dynamic base alignment, the dynamic attitude update equation can be expressed as:

[0054]

[0055] wherein, denotes the direction cosine matrix of the grid system attitude under the initial grid system; denotes the direction cosine matrix of the carrier system attitude under the initial carrier system; denotes the angular velocity of the grid system relative to the inertial system; denotes the angular velocity of the carrier system relative to the inertial system;

[0056] A vector observer can be constructed using the inertial system alignment principle:

[0057]

[0058] wherein β denotes the observation vector; and α denotes the reference vector; denotes the direction cosine matrix of the grid system attitude under the initial grid system; denotes the direction cosine matrix of the carrier system attitude under the initial carrier system; g denotes the grid system velocity; denotes the grid system velocity at the initial time; denotes the true angular velocity of the middle latitude earth system relative to the inertial system; g g denotes the mapping of the gravity in the grid system; f b denotes the middle latitude true acceleration;

[0059] Step 4: design an initial velocity disturbance elimination model;

[0060] As can be seen from the above formula, the initial velocity is contained in the observation vector, and the initial velocity error will be coupled in the observation vector, thereby affecting the alignment accuracy. Therefore, the following method can be used to eliminate the initial velocity:

[0061]

[0062] wherein, denotes the mean observation vector containing the initial velocity disturbance; denotes the observation vector containing the initial velocity disturbance; denotes the disturbance velocity error; β i denotes the observation vector without the initial velocity disturbance; and M denotes the number of alignment vectors;

[0063] Therefore, the new observation vector without the disturbance velocity can be calculated as:

[0064]

[0065] wherein, an observed vector representing the undisturbed velocity; an observed vector representing the mean value with initial velocity disturbance; an observed vector representing the mean value with initial velocity disturbance;

[0066] Similarly, the new reference vector can be constructed as:

[0067]

[0068] wherein, a new reference vector; α i a reference vector at time i; α M a reference vector at time M;

[0069] Step 5: Implementing attitude determination using OBA algorithm;

[0070] Using the above constructed vectors, the following OBA attitude matrix can be constructed:

[0071]

[0072] wherein, K k an attitude matrix constructed at time k; K k-1 an attitude matrix constructed at time k-1; a new reference vector; an observed vector representing the undisturbed velocity; and can be represented as

[0073]

[0074] wherein, a new reference vector; an observed vector representing the undisturbed velocity;

[0075] Under the following simulation conditions, MATLAB simulation experiments are conducted on the method:

[0076] The constant drift error of the gyroscope measurement is b g = [0.02 0.02 0.02] T° / h, and the random walk error of the gyroscope measurement is The output frequency is 200 Hz; the constant drift error of the accelerometer measurement is b a = [500 500 500] T μg, and the random walk error of the accelerometer measurement is The output frequency is 200 Hz; the satellite navigation system sampling period is 1 s; M=600; the simulation hardware environment is Intel(R) Core(TM) T9600 CPU 2.80GHz, 4G RAM, Windows 7 operating system. As shown in Figs. 6-8, the position information curve of the in-motion alignment process and the carrier motion curve diagram, Figure 2 , 3 , Figure 4 , 5 , 6 is an alignment error diagram, from the diagram, it can be seen that the initial speed error can be eliminated by using the initial speed disturbance elimination method, and the alignment error of the heading angle can reach the alignment precision of 1° around 200s.

[0077] The polar region alignment is assisted by the speed in the application, the polar region simulation is realized by using the data conversion principle, the initial speed disturbance elimination method is designed, the influence of the initial speed disturbance on the initial alignment is weakened, and the alignment robustness is improved.

Claims

1. A method of initial velocity perturbation cancellation polar region station keeping, characterized by, The method comprises the steps of: acquiring mid-latitude sensor data and integrated navigation data; designing a mid-high latitude data conversion method based on a data conversion principle to convert the mid-latitude data to high-latitude data; updating the attitude based on the converted high-latitude data and constructing a velocity auxiliary vector observer model; designing an initial velocity disturbance elimination model to remove the initial velocity disturbance, determining an undisturbed velocity observation vector and constructing a new reference vector; determining the attitude through an OBA algorithm; repeating the above steps, if the iteration number is equal to the set iteration number, outputting the estimated error parameters based on the determined attitude to complete the initial alignment; the mid-high latitude data conversion method based on the data conversion principle to convert the mid-latitude data to high-latitude data specifically comprises: the conversion formula for converting the mid-latitude attitude data to high-latitude attitude data in the grid coordinate system is determined as: wherein denotes the direction cosine matrix of the mid-latitude carrier pose in the grid coordinate system; denotes the direction cosine matrix of the transformed high-latitude carrier pose in the new grid coordinate system; denotes the mapping of the mid-latitude carrier motion velocity in the grid coordinate system; denotes the mapping of the transformed high-latitude velocity in the new grid coordinate system; the high-latitude position after conversion is determined as: In the formula, represents the high latitude position after conversion; the high-latitude navigation system position is determined as: wherein, represents the latitude after conversion; represents the longitude after conversion; represents the altitude after conversion; represents the x-axis information of the high-latitude position on the west side of the earth; represents the y-axis information of the high-latitude position on the west side of the earth; represents the z-axis information of the high-latitude position on the west side of the earth; e represents the flattening of the earth; R e represents the major axis of the earth; R p represents the minor axis of the earth; R N represents the radius of curvature of the equinoctial circle of the earth; R and θ represent process parameters, specifically: the high-latitude gyro output after conversion is: wherein represents the gyro output angular velocity after conversion; represents the true value of the grid system relative to the inertial system angular velocity after conversion; represents the high latitude true value of the system relative to the grid system angular velocity; represents the middle latitude true value of the system relative to the grid system angular velocity; represents the middle latitude gyro output angular velocity; represents the middle latitude true value of the grid system relative to the inertial system angular velocity; represents the middle latitude true value of the earth system relative to the inertial system angular velocity; represents the middle latitude true value of the grid system relative to the earth system angular velocity; represents the high latitude true value of the earth system relative to the inertial system angular velocity; represents the high latitude true value of the grid system relative to the earth system angular velocity; the high-latitude accelerometer output after conversion is: wherein denotes the accelerometer output acceleration after conversion; denotes the mapping of the mid-latitude carrier motion velocity in the grid coordinate system; denotes the mapping of the high-latitude velocity after conversion in the new grid coordinate system;g n denotes the gravity at low latitudes;g n* denotes the gravity at high latitudes; the high-latitude satellite navigation system output after conversion is: wherein represents a position output after conversion; represents a high-latitude navigation system position; represents a middle-latitude navigation system position; represents a satellite navigation output after conversion; the velocity auxiliary vector observer model is constructed based on the inertial system alignment principle, specifically: where β represents an observation vector; a represents a reference vector; v g represents a grid system velocity; represents an initial grid system velocity; represents a true value of the angular velocity of the middle latitude earth system relative to the inertial system; g g represents a mapping of gravity in the grid system; f b represents a true acceleration of the middle latitude.

2. The initial velocity perturbation cancellation ground station alignment method of claim 1, wherein, the sensor data is: wherein denotes the mid-latitude accelerometer output acceleration; f b denotes the mid-latitude true acceleration; b a represents the acceleration zero offset; η a represents accelerometer measurement noise; represents mid-latitude gyroscope output angular velocity; represents mid-latitude true angular velocity; b g represents the gyro zero bias; η g represents the gyroscope measurement noise.

3. The initial velocity perturbation cancellation ground station alignment method of claim 2, wherein, the integrated navigation data is: wherein represents a satellite navigation system output position; p n represents a true position; represents a position noise; represents a satellite navigation output velocity; v n represents a true velocity; represents a velocity noise.

4. The initial velocity perturbation cancellation ground station alignment method of claim 1, wherein, the attitude update equation is: wherein denotes the direction cosine matrix of the grid frame attitude in the initial grid frame; denotes the direction cosine matrix of the carrier frame attitude in the initial carrier frame; denotes the angular velocity of the grid frame relative to the inertial frame; denotes the angular velocity of the carrier frame relative to the inertial frame.

5. The initial velocity perturbation cancellation ground station alignment method of claim 1, wherein, the initial velocity disturbance elimination model is: wherein represents the mean observation vector with initial velocity perturbation; represents the observation vector with initial velocity perturbation; represents the perturbed velocity error; β i represents the observation vector without initial velocity perturbation; M represents the number of alignment vectors; 6. The initial velocity perturbation cancellation ground station alignment method of claim 1, wherein, the undisturbed velocity observation vector is: wherein represents the observed vector of undisturbed velocity; represents the mean observed vector with initial velocity disturbance; represents the observed vector with initial velocity disturbance; the new reference vector is: wherein denotes the new reference vector; a i denotes the reference vector at time i; a M denotes the reference vector at time M.

7. The initial velocity perturbation cancellation ground station alignment method of claim 1, wherein, the attitude determination through the OBA algorithm specifically comprises: the OBA attitude matrix is constructed as: where K k represents a pose matrix constructed at time k; K k-1 represents a pose matrix constructed at time k-1; and is: wherein represents a new reference vector; represents an observed vector of undisturbed velocity.

8. The initial velocity perturbation cancellation ground station alignment method of claim 3, wherein, The gyro zero bias is b g = [0.02 0.02 0.02] T ° / h, the gyro measurement noise is The measurement sensor output frequency of the gyro is 200 Hz; the acceleration zero bias is b a = [500 500 500]T μg, the accelerometer measurement noise is The measurement sensor output frequency of the acceleration is 200 Hz; the satellite navigation system sampling period is 1 s, and the set iteration number is 600.

Images