Inertial navigation / mileage integrated navigation method based on truncation error compensation

Abstract

The present invention relates to an inertial navigation / mileage combined navigation method based on truncation error compensation. The method is characterized in that: the inertial navigation speed output is converted into a pulse output, and the error equation is established by subtracting the inertial navigation speed output from the mileage meter pulse output; the historical difference is used to predict the true value of the truncation error of the mileage meter at the next moment through a Gaussian regression process, and the truncation error is compensated, and then Kalman filter navigation is performed, including five steps: constructing a vehicle-mounted inertial navigation and mileage meter combined navigation system, establishing a relationship between the inertial navigation pulse output and the mileage meter pulse output, converting the inertial navigation measurement speed into a pulse output, using a Gaussian regression process to predict the mileage meter truncation error, and completing navigation through a Kalman filter using the observed amount after compensating the truncation error. Compared with the prior art, it does not rely on the mileage meter resolution, can correct the mileage meter pulse output to a large extent, the adaptive estimation method improves the navigation positioning accuracy, the model is simple, the amount of calculation is small, and it can effectively improve the positioning accuracy of the vehicle-mounted inertial navigation, which has important practical significance.

Description

Technical Field

[0001] The invention belongs to the technical field of vehicle-mounted strapdown inertial navigation autonomous navigation, and relates to an inertial navigation / mileage combined navigation method based on truncation error compensation. Background Art

[0002] The strapdown inertial navigation system can measure the angular velocity and acceleration information of the carrier. As long as the initial position of the carrier is known, the specific position of the carrier can be presented to achieve autonomous navigation. The current vehicle-mounted integrated navigation system often uses the global satellite positioning system GPS or the Beidou satellite navigation system BDS to assist in achieving high-precision positioning of the vehicle, but the satellite signal is easily interfered with and there is a risk of exposing its own position, which cannot meet the requirements of military vehicles on the battlefield for autonomy. The odometer can be seen everywhere in the vehicle and is also very convenient to use as a measuring tool. The research on the inertial navigation / odometer combined navigation has become a focus.

[0003] In the prior art before the present invention, most of the research focuses on three aspects: compensation of odometer scale coefficient error, calibration of inertial navigation installation deviation angle, and compensation of inertial navigation and odometer lever arm effect. However, none of them considers the output truncation of the odometer, which is defective. The odometer measures the vehicle displacement in the form of pulse output, which can only be an integer. It is inevitable that the measurement value less than one pulse will be discarded, thus causing measurement error. As the driving distance increases, the truncation error generated by each output of the odometer will accumulate, which can easily cause a large cumulative positioning error. Therefore, the impact of the truncation of the odometer output cannot be ignored and needs to be compensated. Wu Meng and others from the Armored Forces Engineering Academy published a paper titled "Vehicle-mounted Strapdown Inertial Navigation / Odometer Combined Navigation Algorithm Based on Fixed Mileage Measurement", in which they proposed a method for correcting the truncation error of the odometer pulse count. The difference between the inertial navigation position and the dead reckoning position within a fixed mileage range was used as the system observation quantity, effectively separating the errors of each sensor in the vehicle-mounted combined navigation system. This method relies on the resolution of the odometer and can only correct the odometer pulse output to a certain extent. Wei Ouyang and others from Shanghai Jiao Tong University published a paper titled "INS / Odometer Land Navigation by Accurate Measurement Modelingand Multiple-Model Adaptive Estimation", in which they proposed a new pulse accumulation observation model, which improved the navigation positioning accuracy through a multi-model adaptive estimation method. However, the model is too complex and computationally intensive, and the pulse is derived from the integral of the pulse velocity, which is prone to large errors. Summary of the invention

[0004] In view of the above-mentioned status of the prior art, the purpose of the present invention is to provide an inertial navigation / mileage combined navigation method based on truncation error compensation, which adopts a Gaussian regression process to estimate the truncation error of the odometer in real time and compensate it in the system observation quantity, thereby significantly improving the positioning accuracy of the vehicle-mounted inertial navigation / mileage combined navigation.

[0005] The technical solution of the present invention is described as follows:

[0006] The present invention proposes an inertial navigation / mileage integrated navigation method based on truncation error compensation. The basic concept is to convert the inertial navigation velocity output into a pulse output, make a difference with the mileage pulse output, and establish an error equation based on pulse observation; predict the true value of the truncation error of the mileage at the next moment through a series of historical difference values ​​through a Gaussian regression process, and compensate the truncation error of the system observation quantity, and then perform Kalman filter navigation, thereby improving the positioning accuracy of the integrated navigation. The method specifically includes the following steps:

[0007] Step 1: Build a vehicle-mounted inertial navigation and odometer combined navigation system

[0008] Step 1.1: Establish the coordinate system of the integrated navigation system, select the northeast sky as the spatial coordinate reference system of the vehicle-borne integrated navigation, denoted as System; The odometer is installed at the center of the front axle of the vehicle and is connected to the vehicle system ( Department) Center coincide, The axis points to the right side of the vehicle body. The axis points to the front in the longitudinal direction of the vehicle body. The axis points to the sky, satisfying the right-hand rectangular coordinate system; the inertial navigation coordinate system is recorded as The coordinate system is consistent with the vehicle body coordinate system;

[0009] Step 1.2: Mathematical navigation coordinate system established by inertial navigation The real Northeast Celestial Coordinate System The systems are not overlapping. Department and There is a misalignment angle between the , then the mathematical coordinate system To the inertial navigation coordinate system The transformation matrix is:

[0010] (1)

[0011] In the formula, represents the unit matrix; is an antisymmetric matrix.

[0012] Step 1.3: Due to the limited accuracy of the inertial navigation installation, Department and The systems cannot be strictly aligned, and there is an installation error angle between the two systems, which are the pitch error angle , heading error angle (Roll error angle Does not affect navigation positioning), remember to install the vector ,but Tie to Installation deviation matrix of the system It can be expressed as:

[0013] (2)

[0014] Step 2: Establish the relationship between the inertial navigation pulse output and the odometer pulse output

[0015] Step 2.1: Set the sampling period , then at any sampling time Let the truncation error of the odometer measurement be , then the actual pulse value of the odometer is Output value of odometer measurement There are the following relationships:

[0016] (3)

[0017] Step 2.2: Convert the actual measured speed of the inertial navigation system into a pulse output value , the measurement error converted to pulse output is The error-free output true value is , then there is the following relationship:

[0018] (4)

[0019] Step 2.3: Combining equations (4) and (5), we can get the following relationship:

[0020] (5)

[0021] Step 3: Convert the inertial navigation speed measurement into pulse output

[0022] Step 3.1: Assume that the vehicle is running normally, the wheels do not slip or jump, and the speed is output only in the direction of the vehicle's forward movement. Assume that the pulse outputs in the lateral, longitudinal and azimuth directions of the vehicle are , , , then in the sampling period Inertial navigation output speed It means that, since the sampling time is very short, the attitude matrix is a constant matrix, so in the sampling period The increment of internal inertial navigation pulse output is:

[0023] (6)

[0024] In the formula, For the The number of unit column vectors is 1, , It is called virtual observation.

[0025] Step 3.2: When the vehicle is driving, the road is bumpy and the odometer scale factor It is not a fixed value and will change with the influence of the environment. Let the error of the odometer scale coefficient be , and considering the deviation angle of the inertial navigation installation, rewrite equation (6), and the real pulse output by the inertial navigation should be:

[0026] (7)

[0027] Step 3.3: Subtract equation (6) from equation (7), so the incremental error of the inertial navigation pulse is:

[0028] (8)

[0029] Step 4: Use Gaussian regression process to predict the truncation error of the odometry

[0030] Step 4.1: Subtract the odometer pulse from the inertial navigation pulse in equation (5) use Indicates that it is the truncation error observation contaminated by noise; the inertial navigation measurement error Use noise Express, obey Gaussian distribution of ; truncation error use express, Indicates the pulse output of the odometer , so there is the following Gaussian regression model with noise:

[0031] (9)

[0032] Step 4.2: When the odometer historical pulse input set is known And the difference output of inertial navigation pulse and odometer pulse , for the new odometer pulse input , in order to obtain the truncation error without noise , then the prior distribution of historical observations and the joint prior distribution of new input points and historical observations are:

[0033] (10)

[0034] (11)

[0035] In the formula is the covariance matrix; Is a new input point and history input The covariance matrix between ; is the covariance of the new input point itself;

[0036] Step 4.3: By analyzing the pulse error model, the SE covariance kernel function is selected, and its covariance expression is:

[0037] (12)

[0038] In the formula, is the Dirac function;

[0039] Step 4.3: Set initial parameter values , , , historical data , And new input Substituting into formula (11) for data learning, the true value of the truncation error at the next moment can be predicted: , then the system observation measurement based on the Gaussian regression model prediction is:

[0040] (13)

[0041] Step 5: Compensate the system observations after truncation error Substitute it into the Kalman filter and complete navigation through filtering.

[0042] Compared with the prior art, the present invention does not rely on the resolution of the odometer, can correct the odometer pulse output to a greater extent, and the adaptive estimation method improves the navigation positioning accuracy. The model is simple, the calculation amount is small, and it can effectively improve the positioning accuracy of the vehicle-mounted inertial navigation, which has important practical significance. BRIEF DESCRIPTION OF THE DRAWINGS

[0043] Figure 1 : Navigation coordinate system Department and Department Schematic

[0044] Figure 2 : Inertial navigation coordinate system Coordinate system and vehicle body coordinate system Department Schematic

[0045] Figure 3 :Relationship diagram between inertial navigation and odometer pulse output

[0046] Figure 4:The positioning error effect diagram of the method of the present invention in vehicle-mounted integrated navigation DETAILED DESCRIPTION

[0047] The embodiments of the present invention will now be further described with reference to the accompanying drawings.

[0048] The basic concept of the present invention is to The speed calculated by the strapdown inertial navigation system is converted into The pulse output of the system is subtracted from the pulse output of the odometer, and all the difference data within one second before the vehicle's travel are selected. The Gaussian regression process learning of the pulse difference data of the two is used to predict the truncation error value of the odometer at the next moment, which is compensated in the system pulse observation quantity, and then filtered navigation is performed.

[0049] Step 1: Figure 1 As shown, define the navigation coordinate system Department and Tie. The origin of the coordinate system is the geometric center of the vehicle. Axis and The axes are in the tangent plane of the ellipsoid at the vehicle's location, pointing to geographic east and geographic north respectively. The axis is collinear with the ellipsoid normal at the vehicle's location, pointing to the sky, and the three axes satisfy the right-hand rectangular coordinate system. The ellipsoid refers to the WGS-84 model; The origin of the coordinate system is the geometric center of the vehicle. Axis and The axes are in the local horizontal plane, pointing to the East and North Pole of the Earth respectively. The axes are collinear with the vertical line of the horizontal plane where the vehicle is located, and the directions of the three axes satisfy the right-hand rectangular coordinate system.

[0050] Step 2: Mathematical navigation coordinate system established by inertial navigation The real Northeast Celestial Coordinate System The systems are not overlapping. Department and There is a misalignment angle between the , then the mathematical coordinate system To the real navigation coordinate system The transformation matrix is:

[0051] (1)

[0052] Step 3: If Figure 2 As shown, define the vehicle body coordinate system Inertial navigation coordinate system The origin of the vehicle coordinate system is the center of the vehicle's front axle. , The axis points to the right side of the vehicle body. The axis points to the front in the longitudinal direction of the vehicle body. The axis points to the sky, satisfying the right-hand rectangular coordinate system; the origin of the inertial navigation coordinate system is the inertial navigation geometric center , The axis points to the right side of the vehicle body. The axis points to the front in the longitudinal direction of the vehicle body. The axis points toward the sky, which also satisfies the right-hand rectangular coordinate system.

[0053] Step 4: Since the inertial navigation system installed on the vehicle has limited accuracy, Department and The coordinates of the two coordinate systems do not coincide with each other. There is an installation deviation angle between the two coordinate systems: pitch deviation angle , Roll deviation angle , heading deviation angle , are small angles, then there is an installation error matrix :

[0054] (2)

[0055] Step 5: Figure 3 As shown, the relationship between the odometer pulse output and the inertial navigation pulse output is constructed. The black nodes are the pulse output points of the odometer, and the odometer measurement output value is ,and is the cutoff value of the odometer, so the odometer pulse outputs the true value for:

[0056] (3)

[0057] Due to the measurement error of the inertial navigation, the vehicle travel distance during the sampling time is larger than the actual value, and there will be measurement errors after being converted into pulses. , then the inertial navigation is converted into pulse output The actual pulse value of the odometer The relationship is:

[0058] (4)

[0059] Combining equation (3) and equation (4), we can get:

[0060] (5)

[0061] Step 6: Assume that the vehicle is running normally, the wheels do not slip or jump, and the speed is output only in the direction of the vehicle's forward movement. Assume that the pulse outputs in the lateral, longitudinal and azimuth directions of the vehicle are , , , then in the sampling period Inertial navigation output speed It means that, since the sampling time is very short, the attitude matrix is a constant matrix, so in the sampling period The increment of internal inertial navigation pulse output is:

[0062] (6)

[0063] In the formula, For the The number of unit column vectors is 1, , It is called virtual observation.

[0064] Step 7: When the vehicle is driving, the road is rough and the odometer scale factor It is not a fixed value and will change with the influence of the environment. Let the error of the odometer scale coefficient be , and considering the deviation angle of the inertial navigation installation, rewrite equation (6), and the real pulse output by the inertial navigation should be:

[0065] (7)

[0066] Step 8: Subtract equation (6) from equation (7), so the increment of the inertial guidance pulse error is:

[0067] (8)

[0068] Step 9: Subtract the odometer pulse from the inertial navigation pulse in equation (5) use Indicates that it is the truncation error observation contaminated by noise; the inertial navigation measurement error Use noise Express, obey Gaussian distribution of ; truncation error use express, Indicates the pulse output of the odometer , so there is the following Gaussian regression model with noise:

[0069] (9)

[0070] Step 10: Store the pulse set within the previous second when the vehicle is driving and the difference between the inertial navigation pulse and the odometer pulse , for the new odometer pulse input , in order to obtain the truncation error without noise , then the prior distribution of historical observations and the joint distribution of new input points and historical observations are:

[0071] (10)

[0072] (11)

[0073] In the formula is the covariance matrix; Is a new input point and history input The covariance matrix between ; is the covariance of the new input point itself.

[0074] Step 11: By analyzing the pulse error model, the SE covariance kernel function is selected, and its covariance expression is:

[0075] (12)

[0076] In the formula, is the Dirac function.

[0077] Step 12: Set initial parameter values , , 1, 0.1, 0.1 respectively, and the historical data , And new input Substituting into formula (11) for data learning, the true value of the truncation error at the next moment can be predicted: .

[0078] Step 13: Establish an 18-dimensional state Kalman filter to transform the attitude error of the entire integrated navigation system , speed error , position error , gyro zero drift , accelerometer bias , odometer scale factor error And the inertial navigation installation deviation angle , As the system state variable, the system state variable It can be expressed as:

[0079] (13)

[0080] The Kalman filter state equation is:

[0081] (14)

[0082] The Kalman filter state matrix As shown below:

[0083] (15-1)

[0084] (15-2)

[0085] (15-3)

[0086] (15-4)

[0087] (15-5)

[0088] In the formula, L , h are the latitude and altitude of the vehicle’s location, R N and R M They are the curvature radius of the meridian circle and the curvature radius of the meridian circle where the vehicle is located.

[0089] The system noise matrix and system measurement noise They are as follows:

[0090] (16)

[0091] (17)

[0092] In the formula, and represent the measurement noise of the gyroscope and accelerometer, respectively.

[0093] Step 14: Select the incremental output of the inertial navigation pulse within the sampling period With odometer output The difference is used as the system observation and compensates for the predicted odometer truncation error , then the system observation is:

[0094] (18)

[0095] The Kalman filter measurement equation is:

[0096] (19)

[0097] The measurement matrix as follows:

[0098] (20)

[0099] In the formula, Represents the measurement noise, which is white noise.

[0100] Step 15: Carry out vehicle experiments. The vehicle is equipped with an electric level to power the navigation equipment; a computer to display and save various real-time data; a GPS module to provide navigation positioning reference; a high-precision laser inertial navigation to provide real-time attitude angle and acceleration information of the vehicle; and a wheel odometer installed at the center of the front axle of the vehicle, which can provide information on the vehicle's forward speed as long as the wheels are working properly.

[0101] The compensation method of the present invention is used for vehicle-mounted integrated navigation. Taking the SINS / GPS integrated navigation result as the true value, the horizontal positioning error result is as follows: Figure 4 The black line is the traditional combined navigation algorithm that uses the difference between the odometer and the inertial navigation speed as the observation quantity, and the positioning error is 168.5m in a mileage of more than 140 kilometers; the black dotted line is the invented method that uses the difference between the odometer and the inertial navigation pulse as the observation and compensates for the truncation error at the same time, and the positioning error is 37.2m. It can be seen that the method of the present invention can effectively improve the positioning accuracy of the vehicle-mounted inertial navigation and has important practical significance.

Claims

1. An inertial navigation / mileage integrated navigation method based on truncation error compensation is characterized by: The inertial navigation velocity output is converted into pulse output, and the difference is made with the odometer pulse output, and the error equation based on pulse observation is established; A series of historical differences are used to predict the true value of the truncation error of the odometer at the next moment through the Gaussian regression process, and the truncation error is compensated for the system observation quantity, and then Kalman filter navigation is performed to improve the positioning accuracy of the integrated navigation, including the following steps: Step 1: Build a vehicle-mounted inertial navigation and odometer combined navigation system; Step 2: Establish the relationship between the inertial navigation pulse output and the odometer pulse output; The establishment of the relationship between the inertial navigation pulse output and the odometer pulse output described in step 2 specifically includes the following steps: Step 2.1: Set the sampling period , then at any sampling time Let the truncation error of the odometer measurement be , then the actual pulse value of the odometer Output value of odometer measurement There are the following relationships: (3) Step 2.2: Convert the actual measured speed of the inertial navigation system into a pulse output value , the measurement error converted to pulse output is The error-free output true value is , then there is the following relationship: (4) Step 2.3: Combining equations (3) and (4), we can get the following relationship: (5) Step 3: Convert the inertial navigation measurement speed into pulse output; Step 4: Use Gaussian regression process to predict the odometer truncation error; Step 5: Compensate the system observations after truncation error Substitute it into the Kalman filter and complete navigation through filtering.

2. The inertial navigation / mileage integrated navigation method based on truncation error compensation according to claim 1, characterized in that: The construction of the vehicle-mounted inertial navigation and odometer combined navigation system described in step 1 specifically includes the following steps: Step 1.1: Establish the coordinate system of the integrated navigation system, select the northeast sky as the spatial coordinate reference system of the vehicle-borne integrated navigation, denoted as The odometer is installed at the center of the front axle of the vehicle and is connected to the vehicle system. Department Center coincide, The axis points to the right side of the vehicle body. The axis points to the front in the longitudinal direction of the vehicle body. The axis points to the sky, satisfying the right-hand rectangular coordinate system; the inertial navigation coordinate system is recorded as The coordinate system is consistent with the vehicle body coordinate system; Step 1.2: Mathematical navigation coordinate system established by inertial navigation The real Northeast Celestial Coordinate System The systems are not overlapping. Department and There is a misalignment angle between the , then the mathematical coordinate system To the inertial navigation coordinate system The transformation matrix is: (1) In the formula, represents the unit matrix; is an antisymmetric matrix; Step 1.3: Due to the limited accuracy of the inertial navigation installation, Department and The systems cannot be strictly aligned, and there is an installation error angle between the two systems, which are the pitch error angle , heading error angle , rolling error angle Does not affect navigation positioning, remember to install the vector ,but Tie to Installation deviation matrix of the system It can be expressed as: (2) 3. The inertial navigation / mileage integrated navigation method based on truncation error compensation according to claim 1, characterized in that: The step 3 of converting the inertial navigation measurement speed into pulse output specifically includes the following steps: Step 3.1: Assume that the vehicle is running normally, the wheels do not slip or jump, and the speed is output only in the direction of the vehicle's forward movement. Assume that the pulse outputs in the lateral, longitudinal and azimuth directions of the vehicle are , , , then in the sampling period Inertial navigation output speed It means that, since the sampling time is very short, the attitude matrix is a constant matrix, so in the sampling period The increment of internal inertial navigation pulse output is: (6) In the formula, For the The number of unit column vectors is 1, , It is called virtual observation; Step 3.2: When the vehicle is driving, the road is bumpy and the odometer scale factor It is not a fixed value and will change with the influence of the environment. Let the error of the odometer scale coefficient be , and considering the deviation angle of the inertial navigation installation, rewrite equation (6), and the real pulse output by the inertial navigation should be: (7) In the formula, for Tie to The installation deviation matrix of the system, For mathematical coordinate system To the inertial navigation coordinate system The transformation matrix of Step 3.3: Subtract equation (6) from equation (7), so the incremental error of the inertial navigation pulse is: (8) In the formula, .

4. The inertial navigation / mileage integrated navigation method based on truncation error compensation according to claim 1, characterized in that: The use of the Gaussian regression process to predict the odometer truncation error described in step 4 specifically includes the following steps: Step 4.1: Subtract the odometer pulse from the inertial navigation pulse in equation (5) use Indicates that it is the truncation error observation contaminated by noise; the inertial navigation measurement error Use noise Express, obey Gaussian distribution of; truncation error use express, Indicates the pulse output of the odometer , so there is the following Gaussian regression model with noise: (9) Step 4.2: When the odometer historical pulse input set is known And the difference output of inertial navigation pulse and odometer pulse , for the new odometer pulse input , in order to obtain the truncation error without noise , then the prior distribution of historical observations and the joint prior distribution of new input points and historical observations are: (10) (11) In the formula is the covariance matrix; Is a new input point and history input The covariance matrix between ; is the covariance of the new input point itself; Step 4.3: By analyzing the pulse error model, the SE covariance kernel function is selected, and its covariance expression is: (12) In the formula, is the Dirac function; Step 4.3: Set initial parameter values , , , historical data , And new input Substituting into formula (11) for data learning, the true value of the truncation error at the next moment can be predicted: , then the system observation measurement based on the Gaussian regression model prediction is: (13)。

Images