CN105698822B - Initial Alignment Method between autonomous type inertial navigation based on reversed Attitude Tracking is advanced - Google Patents

Abstract

An autonomous inertial navigation-to-travel initial alignment method based on reverse attitude tracking, which includes the following steps: 1. Establish a transitional reference coordinate system; 2. Calculate a rough attitude matrix according to the sampling data of the sensor subsystem during the movement of the carrier ; 3. The tracking differentiator handles the speed differential of the odometer; 4. Using the stored sensor subsystem sampling data, reverse attitude tracking calculates the attitude matrix at the beginning of the initial alignment; 5. Using the stored sensor subsystem sampling data for inertial navigation solution Calculate and establish a Kalman filter for fine alignment to obtain accurate attitude matrix and carrier position information; the method of the present invention makes full use of the sensor sampling data, and can realize the travel-to-movement under the condition that only the initial position of the carrier is known. Accurate alignment of attitude array acquisition and position navigation greatly improves the maneuverability of the carrier, and is an effective method of initial alignment during travel.

Description

A Method of Initial Alignment on the Go for Autonomous Inertial Navigation Based on Reverse Attitude Tracking

1. Technical field

The invention proposes an autonomous inertial navigation on-the-go initial alignment method based on reverse attitude tracking, which relates to an inertial navigation on-the-go initial alignment method and belongs to the technical field of inertial navigation initial alignment.

2. Background technology

The initial alignment of inertial navigation during travel refers to the technology of completing the initial alignment of the inertial navigation system during the movement of the carrier. Therefore, it is a kind of initial alignment technology of the moving base. The initial alignment technology of inertial navigation has immeasurable significance and role in enhancing the carrier's maneuverability and quick response ability. Therefore, how to achieve the initial alignment during the motion of the carrier is a topic worth studying.

Different from the initial alignment environment of the traditional static base, when the carrier is in motion, the position, velocity, acceleration and angular velocity of the carrier are constantly changing, and its influence on the initial alignment is mainly manifested in: On the one hand, the line Movement will cause the ground acceleration, Gothic acceleration and other parameters in the basic equation of inertial navigation to change all the time, so the accurate information of the acceleration of gravity cannot be measured by the output data of the accelerometer in the state of motion; The angular velocity has a very wide frequency band, and the signal-to-noise ratio of the output signal of the gyroscope is low, so the useful information of the alignment of the earth's rotation angular velocity cannot be extracted from the output data of the gyroscope.

It can be seen that under the condition of carrier motion, it is not possible to rely solely on the direct measurement information of the gyroscope and accelerometer for initial alignment, but it is necessary to introduce ranging or speed measurement information to compensate for the impact of harmful acceleration on the initial alignment accuracy during motion. At present, there are mainly strapdown compass methods, inertial system alignment methods, and optimal estimation alignment methods for inertial navigation on-the-go alignment methods. The strapdown compass method uses a mature classic control theory method to realize the initial alignment during travel. The principle is simple, but the alignment time is long and it is sensitive to the low-frequency interference of the gyroscope. It is necessary to select appropriate control parameters according to the motion environment. The inertial system alignment method uses the inertial space as the intermediate transition coordinate system to isolate the interference of the angular motion of the carrier on the initial alignment. However, this method only simply handles the measurement error, so the alignment accuracy is not high and the position information of the carrier cannot be obtained. The optimal estimation alignment method establishes the inertial navigation error equation, uses the speed measurement information of the speed sensor such as the odometer as the measurement information to perform Kalman filtering, and estimates the key errors such as the platform misalignment angle to achieve the initial alignment during the journey . Most of this method is based on the inertial navigation linearization error model, and it needs to obtain a rough initial attitude matrix under static conditions before proceeding. Therefore, the maneuverability advantage of the carrier is weakened to a certain extent.

It can be seen that each method has its own characteristics and applicability. In order to not only obtain the rough attitude matrix during the movement, but also realize high-precision attitude alignment and position navigation, the present invention simply uses the odometer velocity sampling data as alignment auxiliary information to propose an autonomous inertial tracking system based on reverse attitude tracking. Navigate the alignment method between runs. This method can achieve high-precision initial alignment of inertial navigation under the condition of only knowing the initial position.

3. Contents of the invention

Aiming at the problems existing in the prior art, the present invention proposes an autonomous inertial navigation-to-travel initial alignment method based on reverse attitude tracking. Firstly, during the motion of the carrier, the speed measurement information of the odometer is used to assist the coarse alignment to obtain a rough attitude matrix, and the sampling data of the inertial measurement elements (including gyroscopes and accelerometers) and the odometer are saved at the same time. Then reverse attitude tracking is performed to obtain the attitude matrix at the beginning of the initial alignment. Finally, on this basis, the Kalman filter fine alignment is carried out by using the saved IMU sampling data and odometer speed sampling data. Finally, high-precision attitude alignment and position navigation are achieved.

The autonomous inertial navigation travel initial alignment method based on reverse attitude tracking proposed by the present invention is used in a vehicle-mounted inertial navigation system. The system includes a sensor subsystem, a data storage module, a rough alignment calculation module, and a reverse attitude tracking Calculation module and fine alignment calculation module. The relationship between them is: the sampling data of the sensor subsystem are transmitted to the data storage module and the rough alignment calculation module respectively; the rough alignment calculation module calculates the rough initial attitude matrix and transmits it to the reverse attitude tracking calculation module; The orientation tracking calculation module uses the sensor data of the data storage module to perform reverse attitude tracking, and transmits the calculation result to the fine alignment calculation module. The fine alignment calculation module uses the data of the data storage module to perform fine alignment calculation to obtain the precise alignment attitude matrix and position navigation results.

The present invention proposes an autonomous inertial navigation on-the-go initial alignment method based on reverse attitude tracking, which specifically includes the following steps:

Step 1: Establish a transitional reference coordinate system;

The four important transition reference coordinate systems established by the whole alignment algorithm are the initial moment inertial coordinate system (i ₀ system), the initial moment earth coordinate system (e ₀ system), the initial moment navigation coordinate system (n ₀ system) and the initial moment Carrier coordinate system (i _b0 system), which is defined as follows:

(1) Inertial coordinate system at the initial moment (i ₀ system)

At the beginning of the initial alignment, OX _i0 is in the local meridian plane, parallel to the equatorial plane; OZ _i0 points to the direction of the earth’s rotation axis; OY _i0 forms a right-handed spiral coordinate system with OX _i0 and OZ _i0 ; after the initial alignment begins, the i ₀ system The three axes are unchanged relative to the inertial space;

(2) The earth coordinate system at the initial moment (e ₀ system)

At the initial moment, the earth coordinate system takes the center of the earth as the origin of the coordinate system, OX _e0 points to the direction of the local meridian in the equatorial plane; OZ _i0 points to the direction of the earth’s rotation axis; OY _i0 forms a right-handed spiral coordinate system with OX _i0 and OZ _i0 ; the coordinate system is relatively stand still on the surface of the earth;

(3) Initial navigation coordinate system (n ₀ system)

The east-north-sky navigation coordinate system at the initial alignment start time is used as the initial navigation coordinate system; after the initial alignment starts, the coordinate system does not move relative to the earth's surface;

(4) Carrier coordinate system at the initial moment (i _b0 system)

The right-front-upper carrier coordinate system at the initial alignment start time is used as the initial moment carrier coordinate system; after the initial alignment starts, the coordinate system does not move relative to the inertial space;

Step 2: During the movement of the carrier, calculate the rough attitude matrix according to the sampling data of the sensor subsystem;

Taking the i ₀ system as the transition reference coordinate system, the attitude matrix of the navigation coordinate system (n system) relative to the carrier coordinate system (b system) The calculation of is divided into two parts, and its calculation process is shown in formula (1)

in, is the transformation matrix of the n series relative to the i ₀ series; is the transformation matrix of the i ₀ system relative to the b system;

a calculation matrix

Through the transition reference coordinate system n ₀ system and e ₀ system, the calculation process of the matrix can be divided into four parts, namely:

In the formula, is the transformation matrix of the n system relative to the geographic coordinate system (e system); is the transformation matrix of the e system relative to the n ₀ system;

is the transformation matrix of the n ₀ series relative to the e ₀ series; is the transformation matrix of the e ₀ system relative to the i ₀ system;

Since the position of the carrier cannot be accurately obtained in the coarse alignment module, formula (2) is approximated as:

in,

In the formula, L ₀ is the geographic latitude when the initial alignment starts; ω _ie is the angular velocity of the earth's rotation; Δt is the time difference between the current moment t and the initial alignment starting moment _tstart , that is, Δt= _ttstart ; formula (4-5 ) into formula (3), we can get:

b calculation matrix

Taking the i _b0 system as the transition reference coordinate system, the transformation matrix of the i ₀ system relative to the b system The calculation of is divided into two parts, namely:

in, is the transformation matrix of the i ₀ system relative to the i _b0 system; is the transformation matrix of the i _b0 system relative to the b system;

Using the measured angular velocity of the gyroscope, the matrix can be directly obtained through the differential equation (8) of the attitude matrix

In the formula, since both the i _b0 system and the i ₀ system are fixed relative to the inertial coordinate system (i system), the angular velocity sampling data of the inertial measurement element can be used direct replacement In addition, at the beginning of the initial alignment, the i _b0 system coincides with the b system, so the matrix The initial value of the integral calculation is the unit matrix I _3×3 ;

matrix The calculation of needs to use the nature of the earth's gravitational acceleration slowly drifting in the inertial system; its calculation method is as follows:

The i _b0 system and the i ₀ system are both fixed relative to the inertial space, so it is easy to know that the matrix is a constant matrix; for the carrier velocity Derivation on both sides gives:

in, is the component of the carrier acceleration in the n system; is the component of carrier acceleration in b system;

Substituting it into the basic equation of inertial navigation, the basic equation of inertial navigation can be rewritten as shown in formula (10);

Among them, g ⁿ is the component of the earth's gravitational acceleration in the n system; is the component of the earth's rotation angular velocity in the n system; is the rotational angular velocity of the n system relative to the i system; f ⁿ is the component of the specific force sampling data of the inertial measurement element in the n system;

Also known attitude matrix differential equation in is the rotational angular velocity of the b system relative to the n system. Substituting it into (10), we can get:

In the formula, Negligible, the above formula is further rewritten as formula (12);

Among them, g ^b is the component of the earth's gravitational acceleration in the b system; f ^b is the specific force sampling data of the inertial measurement element; v ^b is the speed sampling data of the odometer Velocity measurement vector made up of instead of Measuring Vector Differentials Using Odometer Velocity instead; it should be noted that since the speed sampling data of the odometer contains noise, direct differentiation will amplify the noise and cause the alignment accuracy to drop, so it is necessary to use a tracking differentiator to Carry out differential processing to get less noise interference

In order to get the matrix It is necessary to pass the important relationship between g ^b and g ⁿ (13);

In the formula, the component g ⁿ =[0 0 -g] ^T of the earth's gravitational acceleration in the b system is a known quantity; the matrix can be obtained by the previous calculation, so the matrix The solution of just needs to construct three vectors that are not in the same plane according to formula (13);

In order to construct the vector and attenuate the noise at the same time, integrate both sides of (13) to get:

In the formula,

get the transformation matrix Need to pass the important relationship between g ^b and g ⁿ

in, can be obtained from the previous calculation; integrating both sides of the above formula to get

Take the integral value at two time instants _t1 and _t2 and but Can be calculated by formula (17);

So far, during the movement of the carrier, the rough attitude matrix at the end time t _end of the initial alignment can be calculated by using equations (6), (8) and (17) However, the accuracy of the attitude matrix is not high, and the position of the carrier cannot be obtained at this time, so the sensor sampling data needs to be saved for further processing;

Step 3: track the differentiator to process the speed differential of the odometer;

make Then we have the following nonlinear tracking differentiator

In the formula, r is the parameter of the tracking differentiator, v(t) is the signal to be tracked which contains noise, here is the speed sampling data of the odometer; assuming that η is the sampling period, the discretization calculation method is as follows;

So far, the speed differential of the odometer that is less polluted by disturbing noise can be obtained;

Step 4: Using the stored sensor subsystem sampling data, reverse attitude tracking to calculate the attitude matrix at the beginning of the initial alignment;

The reverse attitude tracking algorithm is to convert the attitude matrix obtained by rough alignment Reverse calculation back to the initial alignment start time to obtain the attitude matrix In order to lay the foundation for the subsequent fine alignment and position determination;

Represent the attitude of the carrier with the quaternion q, then the solution of the attitude can be obtained by the quaternion differential equation (20);

in,

In the formula, is the component of the angular velocity of the b system relative to the n system in the b system; is the component of the earth's rotation angular velocity in the n system; is the component of the angular velocity of the n system relative to the earth coordinate system (e system) in the n system;

In the forward calculation of the attitude, the attitude quaternion q(k) of the previous moment k and the gyroscope sampling data of this moment k+1 are used To solve the attitude quaternion q(k+1) at the k+1 moment; this calculation process adopts the Picard solution method; The approximate calculation formula is:

In the formula, T _s is the attitude update cycle;

Δθ(k+1)=||Δθ(k+1)|| (25)

In the reverse attitude tracking calculation, the attitude quaternion q(k+1) at time k+1 and the angular velocity sampling data of the inertial measurement unit at time k are known To solve the attitude quaternion q(k) at time k; therefore, rewrite formula (23) as formula (26);

in,

Considering that after coarse alignment, the position information of the carrier is unknown, so The approximate value is At the same time, in order to improve the calculation accuracy of Δθ(k), the odometer speed sampling data is used to calculate Its calculation process is as follows:

in,

So far, the attitude matrix can be transformed into Tracking back to the initial alignment start time, we get So as to lay a solid foundation for the next fine alignment;

Step 5: Use the stored sensor subsystem sampling data to perform inertial navigation calculations, establish a Kalman filter for fine alignment, and obtain accurate attitude matrix and carrier position information;

a. Inertial navigation solution and odometer position calculation

The latitude L ₀ , longitude λ ₀ , height h ₀ and reverse attitude tracking results at the start time of the initial alignment In order to solve the initial value, the attitude, velocity and position are calculated using the stored sensor sampling data, and the inertial navigation solution position p=[L λ h] ^T and the inertial navigation solution velocity at each sampling point are obtained Inertial navigation solution attitude matrix and odometer estimated position p _D =[L _D λ _D h _D ] ^T , estimated speed

b. Build a filter state model

Taking the n system as the reference coordinate system of inertial navigation, the filtering state model is as follows:

In the formula, is the misalignment angle of the inertial navigation platform; δv ⁿ is the speed error of the inertial navigation; δ _p is the position error of the inertial navigation; ε is the zero drift of the inertial navigation gyroscope; is the zero bias of the inertial navigation accelerometer; δp _D is the estimated position error of the odometer; δK _D is the scale factor error of the odometer; the radius R _e of the earth’s equator is known, and the matrices M ₁ to M ₆ are:

M ₂ =M'+M" (32)

M ₄ =(v ⁿ ×)·(2M′+M″) (34)

The filter state model is abbreviated as

Among them, the system state transition matrix F ∈ R ^19×19 , its matrix is specifically shown in formula (38); G is the system noise transition matrix; W is the system noise;

c. Build a measurement model

The difference between the inertial navigation calculated position p and the odometer estimated position p _D is used as the measurement vector Z, and the estimated position error caused by the odometer measurement noise is established as the measurement noise of the measurement model;

Z=pp _D =δp-δp _D +V=H X+V (39)

In the formula, H is the measurement transfer matrix, H=[0 _3×6 I _3×3 0 _3×6 -I _3×3 0 _3×1 ]; V is the position measurement noise; the measurement vector Z=[LL _D λ-λ _D hh _D ] ^T ;

Finally, the inertial navigation attitude matrix calculated by correcting the inertial navigation error estimated by the filter Get the precise inertial navigation attitude matrix At the same time, the precise estimated position p _D of the odometer can also be obtained; the precise attitude alignment and position navigation of the inertial navigation are realized.

Wherein, in step 2, the "sensor subsystem" includes an inertial measurement element and an odometer. The inertial measurement element measures the angular velocity and specific force of the carrier relative to the inertial space, and obtains sampling data of the angular velocity and specific force; the odometer measures the velocity of the carrier, and obtains the sampling data of the carrier velocity.

Through the above steps, the autonomous inertial navigation on-the-fly initial alignment method based on reverse attitude tracking proposed by the present invention makes full use of the sensor sampling data, and can realize accurate on-the-fly alignment under the condition that only the initial position of the carrier is known. Aligning attitude array acquisition and position navigation greatly improves the maneuverability of the carrier, and is an effective method of initial alignment during travel.

The advantages of the present invention are:

(1) The present invention proposes an autonomous inertial navigation on-the-go initial alignment method based on reverse attitude tracking. Through reverse attitude tracking, the coarse alignment and fine alignment are organically combined, and the sampling data of the sensor subsystem is fully utilized, which can Realize the acquisition of precise alignment attitude matrix and position navigation;

(2) The present invention proposes an autonomous inertial navigation travel-based initial alignment method based on reverse attitude tracking. Kalman filtering is used for fine alignment, and the error of the inertial measurement element (accelerometer zero bias, gyroscope zero drift) and the odometer mark are corrected. Optimal estimation of degree factor error can provide compensation parameters for subsequent navigation calculations;

(3) The present invention proposes an autonomous inertial navigation initial alignment method based on reverse attitude tracking. Under the condition of only knowing the position of the initial alignment start time of the carrier, attitude alignment and position navigation can be realized, which greatly improves the The mobility of the carrier has good application prospects;

4. Description of drawings

FIG. 1 is a schematic structural diagram of an autonomous inertial navigation on-the-go initial alignment method based on reverse attitude tracking proposed by the present invention.

Fig. 2 is a flow chart of the method of the present invention.

The serial numbers, symbols and codes in the figure are explained as follows:

1—sensor subsystem 2—data storage module 3—coarse alignment calculation module

4—Reverse Attitude Tracking Calculation Module 5—Precise Alignment Calculation Module

101—Inertial Measurement Unit 102—Odometer

201—Data storage unit

301—coarse alignment calculation unit

401—Reverse attitude tracking calculation unit

501—Inertial navigation calculation unit 502—Odometer position calculation unit 503—Kalman filter unit

—Angular velocity f ^b —Specific force — Velocity of the vehicle measured by the odometer

—The attitude matrix determined by the rough alignment calculation module

—the attitude matrix determined by the reverse attitude tracking calculation module

p—Position calculated by inertial navigation p _D —Position estimated by odometer — Alignment attitude matrix determined by fine alignment

5. Specific implementation

The present invention will be further described in detail below in conjunction with the accompanying drawings.

The invention proposes an autonomous inertial navigation on-the-go initial alignment method based on reverse attitude tracking. First, during the movement of the carrier, use the speed measurement information of the odometer to assist in rough alignment to obtain a rough attitude matrix Simultaneously save the sampled data of the inertial measurement unit and the odometer. Then perform reverse attitude tracking to obtain the attitude matrix at the beginning of the initial alignment Finally, on this basis, the Kalman filter fine alignment is carried out by using the saved IMU sampling data and odometer speed sampling data. Finally, obtain a high-precision alignment attitude matrix And realize carrier position navigation.

See Fig. 1, the present invention proposes the initial alignment method between the autonomous type inertial navigation that is based on reverse attitude tracking, comprises sensor subsystem 1, data storage module 2, coarse alignment calculation module 3, reverse attitude tracking calculation module 4 and Fine alignment computing module 5.

Sampled data from the sensor subsystem system Respectively passed to the data storage module 2 and the rough alignment calculation module 3; the rough alignment calculation module 3 calculates a rough initial attitude matrix And pass it to the reverse attitude tracking calculation module 4; The reverse attitude tracking calculation module 4 utilizes the sensor data of the data storage module 2 to carry out the reverse attitude tracking, and calculates the result Pass it to the fine alignment calculation module 5. The fine alignment calculation module 5 uses the data of the data storage module 2 to perform fine alignment calculation to obtain the precise alignment attitude matrix and location navigation results.

As shown in Fig. 2, the present invention proposes an initial alignment method for autonomous inertial navigation based on reverse attitude tracking, which specifically includes the following steps:

Step 1: Establish a transitional reference coordinate system;

The four important transition reference coordinate systems established by the whole alignment algorithm are the initial moment inertial coordinate system (i ₀ system), the initial moment earth coordinate system (e ₀ system), the initial moment navigation coordinate system (n ₀ system) and the initial moment Carrier coordinate system (i _b0 system), which is defined as follows:

(1) Inertial coordinate system at the initial moment (i ₀ system)

At the beginning of the initial alignment, OX _i0 is in the local meridian plane, parallel to the equatorial plane; OZ _i0 points to the direction of the Earth’s rotation axis; OY _i0 forms a right-handed spiral coordinate system with OX _i0 and OZ _i0 ; after the initial alignment begins, the i ₀ system The three axes are unchanged relative to the inertial space;

(2) The earth coordinate system at the initial moment (e ₀ system)

At the initial moment, the earth coordinate system takes the center of the earth as the origin of the coordinate system, OX _e0 points to the direction of the local meridian in the equatorial plane; OZ _i0 points to the direction of the earth’s rotation axis; OY _i0 forms a right-handed spiral coordinate system with OX _i0 and OZ _i0 ; the coordinate system is relatively stand still on the surface of the earth;

(3) Initial navigation coordinate system (n ₀ system)

The east-north-sky navigation coordinate system at the initial alignment start time is used as the initial navigation coordinate system; after the initial alignment starts, the coordinate system does not move relative to the earth's surface;

(4) Carrier coordinate system at the initial moment (i _b0 system)

The right-front-upper carrier coordinate system at the initial alignment start time is used as the initial moment carrier coordinate system; after the initial alignment starts, the coordinate system does not move relative to the inertial space;

Step 2: During the movement of the carrier, calculate the rough attitude matrix according to the sampling data of the sensor subsystem;

Taking the i ₀ system as the transition reference coordinate system, the attitude matrix of the navigation coordinate system (n system) relative to the carrier coordinate system (b system) The calculation of is divided into two parts, and its calculation process is shown in formula (1)

in, is the transformation matrix of the n series relative to the i ₀ series; is the transformation matrix of the i ₀ system relative to the b system;

a calculation matrix

Through the transition reference coordinate system n ₀ system and e ₀ system, the calculation process of the matrix can be divided into four parts, namely:

In the formula, is the transformation matrix of the n system relative to the geographic coordinate system (e system); is the transformation matrix of the e system relative to the n ₀ system; is the transformation matrix of the n ₀ series relative to the e ₀ series; is the transformation matrix of the e ₀ system relative to the i ₀ system;

Since the position of the carrier cannot be accurately obtained in the coarse alignment module, formula (2) is approximated as:

in,

In the formula, L ₀ is the geographic latitude when the initial alignment starts; ω _ie is the angular velocity of the earth's rotation; Δt is the time difference between the current moment t and the initial alignment starting moment _tstart , that is, Δt= _ttstart ; formula (4-5 ) into formula (3), we can get:

b calculation matrix

Taking the i _b0 system as the transition reference coordinate system, the transformation matrix of the i ₀ system relative to the b system The calculation of is divided into two parts, namely:

in, is the transformation matrix of the i ₀ system relative to the i _b0 system; is the transformation matrix of the i _b0 system relative to the b system;

Using the measured angular velocity of the gyroscope, the matrix can be directly obtained through the differential equation (8) of the attitude matrix

In the formula, since both the i _b0 system and the i ₀ system are fixed relative to the inertial coordinate system (i system), the angular velocity sampling data of the inertial measurement element can be used direct replacement In addition, at the beginning of the initial alignment, the i _b0 system coincides with the b system, so the matrix The initial value of the integral calculation is the unit matrix I _3×3 ;

matrix The calculation of needs to use the nature of the earth's gravitational acceleration slowly drifting in the inertial system; its calculation method is as follows:

The i _b0 system and the i ₀ system are both fixed relative to the inertial space, so it is easy to know that the matrix is a constant matrix; for the carrier velocity Derivation on both sides gives:

in, is the component of the carrier acceleration in the n system; is the component of carrier acceleration in b system;

Substituting it into the basic equation of inertial navigation, the basic equation of inertial navigation can be rewritten as shown in formula (10);

Among them, g ⁿ is the component of the earth's gravitational acceleration in the n system; is the component of the earth's rotation angular velocity in the n system; is the rotational angular velocity of the n system relative to the i system; f ⁿ is the component of the specific force sampling data of the inertial measurement element in the n system;

Also known attitude matrix differential equation in is the rotational angular velocity of the b system relative to the n system. Substituting it into (10), we can get:

In the formula, Negligible, the above formula is further rewritten as formula (12);

Among them, g ^b is the component of the earth's gravitational acceleration in the b system; f ^b is the specific force sampling data of the inertial measurement element; v ^b is the speed sampling data of the odometer Velocity measurement vector made up of instead of Measuring Vector Differentials Using Odometer Velocity instead; it should be noted that since the speed sampling data of the odometer contains noise, direct differentiation will amplify the noise and cause the alignment accuracy to drop, so it is necessary to use a tracking differentiator to Carry out differential processing to get less noise interference

In order to get the matrix It is necessary to pass the important relationship between g ^b and g ⁿ (13);

In the formula, the component g ⁿ =[0 0 -g] ^T of the earth's gravitational acceleration in the b system is a known quantity; the matrix can be obtained by the previous calculation, so the matrix The solution of just needs to construct three vectors that are not in the same plane according to formula (13);

In order to construct the vector and attenuate the noise at the same time, integrate both sides of (13) to get:

In the formula,

get the transformation matrix Need to pass the important relationship between g ^b and g ⁿ

in, can be obtained from the previous calculation; integrating both sides of the above formula to get

Take the integral value at two time instants _t1 and _t2 and but Can be calculated by formula (17);

So far, during the movement of the carrier, the rough attitude matrix at the end time t _end of the initial alignment can be calculated by using equations (6), (8) and (17) However, the accuracy of the attitude matrix is not high, and the position of the carrier cannot be obtained at this time, so the sensor sampling data needs to be saved for further processing;

Step 3: track the differentiator to process the speed differential of the odometer;

make Then we have the following nonlinear tracking differentiator

In the formula, r is the parameter of the tracking differentiator, v(t) is the signal to be tracked which contains noise, here is the speed sampling data of the odometer; assuming that η is the sampling period, the discretization calculation method is as follows;

So far, the speed differential of the odometer that is less polluted by disturbing noise can be obtained;

Step 4: Using the stored sensor subsystem sampling data, reverse attitude tracking to calculate the attitude matrix at the beginning of the initial alignment;

The reverse attitude tracking algorithm is to convert the attitude matrix obtained by rough alignment Reverse calculation back to the initial alignment start time to obtain the attitude matrix In order to lay the foundation for the subsequent fine alignment and position determination;

Represent the attitude of the carrier with the quaternion q, then the solution of the attitude can be obtained by the quaternion differential equation (20);

in,

In the formula, is the component of the angular velocity of the b system relative to the n system in the b system; is the component of the earth's rotation angular velocity in the n system; is the component of the angular velocity of the n system relative to the earth coordinate system (e system) in the n system;

In the forward calculation of the attitude, the attitude quaternion q(k) of the previous moment k and the gyroscope sampling data of this moment k+1 are used To solve the attitude quaternion q(k+1) at the k+1 moment; this calculation process adopts the Picard solution method; The approximate calculation formula is:

In the formula, T _s is the attitude update period;

Δθ(k+1)=||Δθ(k+1)|| (25)

In the reverse attitude tracking calculation, the attitude quaternion q(k+1) at time k+1 and the angular velocity sampling data of the inertial measurement unit at time k are known To solve the attitude quaternion q(k) at time k; therefore, rewrite formula (23) as formula (26);

in,

Considering that after coarse alignment, the position information of the carrier is unknown, so The approximate value is At the same time, in order to improve the calculation accuracy of Δθ(k), the odometer speed sampling data is used to calculate Its calculation process is as follows:

in,

So far, the attitude matrix can be transformed into Tracking back to the initial alignment start time, we get So as to lay a solid foundation for the next fine alignment;

Step 5: Use the stored sensor subsystem sampling data to perform inertial navigation calculations, establish a Kalman filter for fine alignment, and obtain accurate attitude matrix and carrier position information;

a. Inertial navigation solution and odometer position calculation

The latitude L ₀ , longitude λ ₀ , height h ₀ and reverse attitude tracking results at the start time of the initial alignment In order to solve the initial value, the attitude, velocity and position are calculated using the stored sensor sampling data, and the inertial navigation solution position p=[L λ h] ^T and the inertial navigation solution velocity at each sampling point are obtained Inertial navigation solution attitude matrix and odometer estimated position p _D =[L _D λ _D h _D ] ^T , estimated speed

b. Build a filter state model

Taking the n system as the reference coordinate system of inertial navigation, the filtering state model is as follows:

In the formula, is the misalignment angle of the inertial navigation platform; δv ⁿ is the speed error of the inertial navigation; δp is the position error of the inertial navigation; ε is the zero drift of the inertial navigation gyroscope; is the zero bias of the inertial navigation accelerometer; δp _D is the estimated position error of the odometer; δK _D is the scale factor error of the odometer; the radius R _e of the earth’s equator is known, and the matrices M ₁ to M ₆ are:

M ₂ =M'+M" (32)

M ₄ =(v ⁿ ×)·(2M′+M″) (34)

The filter state model is abbreviated as

Among them, the system state transition matrix F ∈ R ^19×19 , its matrix is specifically shown in formula (38); G is the system noise transition matrix; W is the system noise;

c. Build a measurement model

The difference between the inertial navigation calculated position p and the odometer estimated position p _D is used as the measurement vector Z, and the estimated position error caused by the odometer measurement noise is established as the measurement noise of the measurement model;

Z=pp _D =δp-δp _D +V=H X+V (39)

In the formula, H is the measurement transfer matrix, H=[0 _3×6 I _3×3 0 _3×6 -I _3×3 0 _3×1 ]; V is the position measurement noise; the measurement vector Z=[LL _D λ-λ _D hh _D ] ^T ;

Finally, the inertial navigation attitude matrix calculated by correcting the inertial navigation error estimated by the filter Get the precise inertial navigation attitude matrix At the same time, the precise estimated position p _D of the odometer can also be obtained; the precise attitude alignment and position navigation of the inertial navigation are realized.

Wherein, in step 2, the "sensor subsystem" includes an inertial measurement element and an odometer. The inertial measurement element measures the angular velocity and specific force of the carrier relative to the inertial space, and obtains sampling data of the angular velocity and specific force; the odometer measures the velocity of the carrier, and obtains the sampling data of the carrier velocity.

Through the above steps, the autonomous inertial navigation on-the-fly initial alignment method based on reverse attitude tracking proposed by the present invention makes full use of the sensor sampling data, and can realize accurate on-the-fly alignment under the condition that only the initial position of the carrier is known. Aligning attitude array acquisition and position navigation greatly improves the maneuverability of the carrier, and is an effective method of initial alignment during travel.

Claims (2)

1. An autonomous inertial navigation inter-marching initial alignment method based on reverse attitude tracking is characterized in that: it comprises the following steps:

the method comprises the following steps: establishing a transition reference coordinate system;

four important transitional reference coordinate systems established by the whole alignment algorithm are an initial moment inertial coordinate system i₀Global coordinate system of system and initial time e₀Navigation coordinate system of system and initial time, i.e. n₀System and initial time carrier coordinate system i_b0It is defined as follows:

(1) initial moment inertial frame i₀Is a system

At the beginning of the initial alignment, OX_i0In the local meridian plane, parallel to the equatorial plane; OZ_i0Pointing to the direction of the earth rotation axis; OY_i0And OX_i0And OZ_i0Forming a right-handed spiral coordinate system; after the initial alignment starts, i₀The system three axes are invariant with respect to the inertial space;

(2) global coordinate system of initial time e₀Is a system

The earth coordinate system at the initial moment takes the earth center as the origin of the coordinate system, OX_e0Pointing in the local meridian direction in the equatorial plane; OZ_e0Pointing to the direction of the earth rotation axis; OY_e0And OX_e0And OZ_e0Forming a right-handed spiral coordinate system; the coordinate system is stationary relative to the earth's surface;

(3) initial time navigation coordinate system n₀Is a system

Taking an east-north-sky navigation coordinate system of the initial alignment starting time as an initial time navigation coordinate system; after the initial alignment begins, the coordinate system is stationary relative to the earth's surface;

(4) initial time carrier coordinate system i_b0Is a system

Taking a right-front-upper carrier coordinate system of the initial alignment starting moment as an initial moment carrier coordinate system; after the initial alignment begins, the coordinate system is stationary relative to the inertial space;

step two: in the motion process of the carrier, calculating a rough attitude matrix according to the sampling data of the sensor subsystem;

with i₀Is a transitional reference coordinate system, a navigation coordinate system, namely n system, is relative to a carrier coordinate system, namely b system attitude matrixThe calculation is divided into two parts, and the calculation process is shown as the formula (1)

Wherein,n is relative to i₀A transformation matrix of the system;is i₀Is a transformation matrix relative to b;

a calculation matrix

By a transitional reference coordinate system n₀System e₀The calculation process of the matrix can be divided into four parts, namely:

in the formula,a transformation matrix of n relative to the terrestrial coordinate system, i.e. e;is e is relative to n₀A transformation matrix of the system;is n₀Is relative to e₀A transformation matrix of the system;is e₀Is relative to i₀A transformation matrix of the system;

since the carrier position cannot be accurately obtained in the coarse alignment module, equation (2) is approximated as:

wherein,

in the formula, L₀The latitude of the earth at the beginning of the initial alignment; omega_ieThe rotational angular velocity of the earth; Δ t is the current time t and the initial alignment start time t_startA time difference of (a), i.e. t-t_start(ii) a Substituting the formula (4) and the formula (5) into the formula (3) to obtain:

b calculating matrix

With i_b0Is a transitional reference coordinate system, i₀Is relative to the transformation matrix of the b systemThe calculation of (a) is divided into two parts, namely:

wherein,is i₀Is relative to i_b0A transformation matrix of the system;is i_b0Is a transformation matrix relative to b;

differentiation of the measured angular velocity by the attitude matrix using inertial measurement unitsEquation (8) directly yields the matrix

In the formula, due to i_b0Is a reaction of with i₀The systems are stationary with respect to the inertial frame, i.e. the system i, so that the angular velocity of the inertial measurement unit is used to sample the dataDirect substitutionIn addition, at the initial alignment start time, i_b0Is coincident with b, so that the matrixThe integral calculation initial value is a unit matrix I_3×3；

Matrix arrayThe calculation of the method needs to use the slow drift property of the earth gravity acceleration in an inertial system; the calculation method is as follows:

i_b0a series of and i₀The systems are stationary with respect to the inertial space and thus, it is easy to know the matrixIs a constant value matrix; for carrier velocityTwo sides are derived:

wherein,is the component of the carrier acceleration in the n system;is the component of the carrier acceleration in the b system;

substituting the inertial navigation basic equation into the inertial navigation basic equation, wherein the inertial navigation basic equation can be rewritten into a form shown in a formula (10);

wherein, gⁿIs the component of the earth gravity acceleration in the n system;is the component of the rotational angular velocity of the earth in the n system;is the angular velocity of rotation of n relative to i; f. ofⁿSampling the component of the data in the n system for the specific force of the inertia measuring element;

differential equation of known attitude matrixWhereinB is a rotational angular velocity relative to n, and the following is obtained by substituting (10):

in the formula,neglect not toThe above formula is further rewritten as formula (12);

wherein, g^bIs the component of the earth gravity acceleration in the b system; f. of^bSampling data for the specific force of the inertial measurement unit; v. of^bData sampling with odometer speedConstructed velocity measurement vectorReplacing; whileVector differentiation with odometer speed measurementReplacing; it should be noted that, since the odometer speed sampling data contains noise and direct differentiation amplifies the noise to cause a decrease in alignment accuracy, it is necessary to use a tracking differentiator pairDifferential processing to obtain a signal with less noise interference

To obtain a matrixIt is necessary to pass through g^bAnd gⁿ(ii) an important relation (13);

in the formula, gⁿ＝[0 0 -g]^TIs a known amount; matrix arrayAll can be derived from previous calculations, hence the matrixThe solution of (2) is only needed to construct three vectors which are not in the same plane according to the formula (13);

in order to construct a vector and weaken noise at the same time, the two sides of equation (13) are integrated to obtain:

in the formula,

take two moments t₁And t₂Integral value ofAndthenCalculated by formula (15);

to this end, during the movement of the carrier, the initial alignment end time t is calculated by using equations (6), (8) and (15)_endCoarse attitude matrix ofHowever, the attitude matrix is not accurate and cannot be obtained at this timeCarrier position is taken, so sensor sample data needs to be saved for further processing;

step three: the tracking differentiator processes the speedometer differentiation;

order toThere is the following non-linear tracking differentiator

wherein r is a tracking differentiator parameter, v (t) is a signal to be tracked containing noise, and the signal is odometer speed sampling data;

obtaining an odometer speed differential with small degree of pollution caused by interference noise;

step four: utilizing the stored sampling data of the sensor subsystem to perform reverse attitude tracking calculation on an attitude matrix at the initial alignment starting moment;

the inverse attitude tracking algorithm is to obtain the attitude matrix by coarse alignmentReversely calculating to the initial alignment starting moment to obtain an attitude matrixTo lay the foundation for subsequent fine alignment and position determination;

the carrier attitude is expressed by a quaternion q, and the resolution of the attitude is obtained by a quaternion differential equation (18);

wherein,

in the formula,is the component of the angular velocity of b in b relative to n;is the component of the rotational angular velocity of the earth in the n system;is the component of the angular velocity of the system n relative to the terrestrial coordinate system, i.e. the system e in the system n;

in the forward solution of the attitude, the attitude quaternion q (k) at the last moment k and the inertial measurement element sampling data at the moment k +1 are usedSolving an attitude quaternion q (k +1) at the moment of k + 1; the calculation process adopts a Picard solution method; the Picard solution is a common method for solving attitude quaternion by using angle increment, and the three-order approximate calculation formula is as follows:

in the formula, T_sAn attitude update period;

Δθ(k+1)＝||Δθ(k+1)|| (23)

in addition, theIn the backward attitude tracking calculation, the attitude quaternion q (k +1) at the moment of k +1 and the angular velocity sampling data of the inertial measurement element at the moment of k are knownSolving an attitude quaternion q (k) at the moment k; therefore, formula (21) is rewritten to formula (24);

wherein,

considering that after coarse alignment the position information of the carrier is not known, thereforeIs approximately taken asMeanwhile, in order to improve the calculation precision of delta theta (k), the speed sampling data of the odometer is used for calculationThe calculation process is as follows:

wherein,

to this end, the attitude matrix is expressed by equations (24), (25) and (26)Tracking back to the initial alignment start time to obtainThereby laying a foundation for the next fine alignment;

step five: carrying out inertial navigation resolving by using the stored sampling data of the sensor subsystem, establishing a Kalman filter for fine alignment, and acquiring accurate attitude matrix and carrier position information;

a. inertial navigation solution and odometer position estimation

At the latitude L of the initial alignment start time₀Longitude λ, longitude₀Height h₀And reverse attitude tracking resultsIn order to solve the initial value, the stored sensor sampling data is used for carrying out attitude, speed and position calculation to obtain an inertial navigation calculation position p ═ L lambda h at each sampling point]^TInertial navigation resolving speedInertial navigation resolving attitude matrixAnd the estimated position p of the odometer_D＝[L_Dλ_Dh_D]^TAnd estimate the speed

b. Constructing a model of the filtering state

Taking the n system as a reference coordinate system of inertial navigation, the filtering state model is as follows:

in the formula, is the inertial navigation platform misalignment angle; delta vⁿInertial navigation speed error; δ p is the inertial navigation position error; epsilon is the null shift of the navigation inertial measurement unit;zero bias for inertial navigation accelerometers;

δp_Dcalculating a position error for the odometer; delta K_DScale factor error for odometer; known equatorial radius R of the earth_eMatrix M₁To M₆Respectively as follows:

M₂＝M′+M″ (30)

M₄＝(vⁿ×)·(2M′+M″) (32)

filter state model is abbreviated as

Wherein the system state transition matrix F∈R^19×19The matrix is specifically shown as a formula (36); g is a system noise transfer array; w is system noise;

c. building a metrology model

Calculating the inertial navigation calculated position p and the odometer calculated position p_DThe difference is used as a measurement vector Z, and the error of the calculated position caused by the noise measured by the odometer is established as the measurement noise of the measurement model;

Z＝p-p_D＝δp-δp_D+V＝H·X+V (37)

wherein H is a measurement transfer matrix, and H is [0 ]_3×6I_3×30_3×6-I_3×30_3×1](ii) a V is position measurement noise;

the measurement vector Z is [ L-L ═ L_Dλ-λ_Dh-h_D]^T；

Finally, the inertial navigation attitude matrix which is estimated by the filter and is corrected and solved by the inertial navigation errorObtaining an accurate inertial navigation attitude matrixMeanwhile, the accurate estimated position p of the odometer is obtained_D(ii) a And the accurate attitude alignment and position navigation of inertial navigation are realized.

2. The method for autonomous inertial navigation inter-travel initial alignment based on backward attitude tracking according to claim 1, wherein: the sensor subsystem recited in step two, comprising an inertial measurement unit and an odometer; the inertial measurement element measures the angular velocity and specific force of the carrier relative to an inertial space to obtain angular velocity and specific force sampling data; the odometer measures the speed of the carrier to obtain carrier speed sampling data.