CN118463982A - A compact integrated navigation method based on mutual correction of kinematic model/polarization/inertial navigation - Google Patents

Abstract

The invention relates to a tightly combined navigation method for mutual correction of a kinematic model/polarization/inertial navigation, which belongs to the field of motion navigation, and is characterized in that a two-stage filter is applied, and the position, heading and front wheel steering angle error output by a two-degree-of-freedom kinematic model of a vehicle are used as state quantities to establish a front-stage filter state equation; based on the course information calculated by polarization measurement, constructing a measurement equation of a front-stage filter for filtering calculation; taking a three-dimensional attitude misalignment angle, a three-dimensional speed error, a three-dimensional position error, a three-dimensional gyroscope random drift and a three-dimensional accelerometer random zero offset of inertial navigation as state quantities, and establishing a state equation of a later-stage filter; and constructing a measurement equation based on the position, the heading information and the vehicle speed information output by the front-stage filter, and estimating the inertial navigation error by adopting a Kalman filtering method. The invention can improve the position and course resolving precision of the vehicle kinematic model, realize the mutual correction of inertial navigation and kinematic model errors, and further effectively improve the navigation precision of the vehicle.

Description

A compact integrated navigation method based on mutual correction of kinematic model/polarization/inertial navigation

Technical Field

The invention belongs to the field of motion navigation, and in particular relates to a tightly combined navigation method of kinematic model/polarization/inertial navigation mutual correction.

Background Art

The autonomous navigation technology of vehicles has important research value in both civil and military fields. At present, satellite-based vehicle-mounted integrated navigation technology is widely used, but satellite signals are easily interfered with and there is a risk of signal denial, which seriously affects the navigation performance of vehicles. Studies have found that many organisms in nature, such as sand ants, bees, and monarch butterflies, can achieve autonomous navigation by sensing polarized light in the sky. Inspired by biology, people have found that bionic polarization navigation has the advantages of strong autonomy, not susceptible to electromagnetic interference, and no error accumulation. At present, people have carried out a series of studies on vehicle-mounted polarization/inertial navigation. Chinese patent application CN201911250920.8 (a bionic polarization autonomous integrated navigation method based on polarization degree weighting) proposes to establish a polarization/inertial navigation combined navigation model for polarization degree information in different directions. Chinese patent application CN202010512168.6 (a polarization-inertial navigation tight integrated navigation method based on light intensity) establishes the measurement equation of the polarization/inertial navigation tight integrated navigation system based on the light intensity of the opposing channels, further improving the accuracy of polarization measurement.

However, the existing polarization/inertial combined navigation methods often have poor positioning effects due to the lack of position and speed constraints, which in turn affects the accuracy of the navigation system. In order to improve the positioning accuracy of vehicle-mounted polarization combined navigation technology, other navigation technologies are often needed to assist in positioning. The navigation method based on the kinematic model has attracted widespread attention and research due to its advantages of strong autonomy, simple structure and low cost. The paper "Research on Vehicle Autonomous Navigation Method Based on Polarization-Assisted Orientation" (Wang Jixu, Xiong Jian, Guo Hang, et al. Computer Engineering and Applications, 2016, 52(7): 242-247.) The heading angle information obtained by measuring and solving the vehicle-mounted polarization sensor system and the position information provided by the four-wheel motion model are fused and filtered with the output of the inertial navigation system. However, the above paper did not consider the parameter errors of the kinematic model itself and did not implement the correction of the kinematic model.

Summary of the invention

In order to overcome the deficiencies of the prior art, the present invention proposes a tightly integrated navigation method of kinematic model/polarization/inertial navigation mutual correction, wherein the position and heading of the two-degree-of-freedom kinematic model are corrected by a polarization/kinematic pre-filter, and the inertial navigation error is corrected by a polarization/inertial navigation/kinematic post-filter, and the mutual correction of the inertial navigation and the two-degree-of-freedom kinematic model is realized by a two-stage filter. The present invention estimates the parameter error of the two-degree-of-freedom kinematic model by polarization heading, corrects the position and heading of the model, and corrects the attitude, speed, and position errors of the inertial navigation by a kinematic/inertial navigation pre-filter; the mutual correction of the two-degree-of-freedom kinematic model and the inertial navigation is realized by the design of a two-stage filter, thereby improving the navigation accuracy of the vehicle-mounted integrated navigation system.

To achieve the above object, the present invention adopts the following technical scheme:

A tightly integrated navigation method based on kinematic model/polarization/inertial navigation mutual correction comprises the following steps:

Step 1: The eastward position of the vehicle output by the two-degree-of-freedom kinematic model , North position ,course And the vehicle front wheel angle error As a state quantity ,Right now , establish the state equation of the polarization/kinematic pre-filter , Representation and state quantity Related mapping functions; Indicates the state The first derivative of ; the superscript T indicates the transpose of the matrix;

Step 2: Construct the heading measurement equation of the polarization/kinematics pre-filter based on the heading calculated by polarization measurement, and estimate the state quantity using the nonlinear filtering method. , realize the vehicle position and heading output of the two-degree-of-freedom kinematic model And the vehicle front wheel angle error Correction to obtain the vehicle position and heading output by the polarization/kinematics pre-filter ;

Step 3: 3D posture misalignment angle , 3D velocity error , 3D position error , 3D gyroscope random drift , 3D accelerometer random bias As a state quantity ,Right now , establish polarization/inertial navigation/kinematics post-filter state equation , Representation and state quantity Related mapping functions; subscript Indicates the three directions of east, north and sky; Indicates the state The first derivative of ;

Step 4: Vehicle position and heading according to the output of the polarization/kinematics pre-filter in step 2 As well as the vehicle speed calculated by the odometer, the vehicle position and heading of the polarization/inertial navigation/kinematic post-filter are constructed and speed measurement equation; the Kalman filter method is used to estimate the state quantity The 3D posture misalignment angle , 3D velocity error , 3D position error , to realize the correction of inertial navigation error.

The beneficial effects of the present invention compared with the prior art are:

(1) The front wheel steering angle error of the two-degree-of-freedom kinematic model is taken as one of the state quantities, and a polarization/kinematic pre-filter is constructed to estimate the two-degree-of-freedom model parameter error, correct the vehicle position and heading output by the model, and improve the positioning and orientation accuracy of the model.

(2) Based on the polarization/kinematics front-stage filter, a polarization/inertial navigation/kinematics post-stage filter is constructed to correct the inertial navigation heading error, velocity error, and position error. Through the design of a two-stage filter, the mutual correction of the inertial navigation error and the kinematic model error is realized, thereby improving the navigation accuracy of the vehicle-mounted bionic polarization integrated navigation system.

BRIEF DESCRIPTION OF THE DRAWINGS

FIG1 is a flow chart of a tightly integrated navigation method of kinematic model/polarization/inertial navigation mutual correction according to the present invention;

FIG2 is a diagram showing a vehicle simulation trajectory setting according to an embodiment of the present invention;

FIG3 is a front wheel steering angle error estimation curve diagram of an embodiment of the present invention;

FIG. 4 is a position error estimation curve diagram according to an embodiment of the present invention.

DETAILED DESCRIPTION

In order to make the purpose, technical solutions and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention. In addition, the technical features involved in the various embodiments of the present invention described below can be combined with each other as long as they do not conflict with each other.

The present invention discloses a tightly integrated navigation method of kinematic model/polarization/inertial navigation mutual correction. First, the position, heading and front wheel turning angle error of the vehicle's two-degree-of-freedom kinematic model output are used as state quantities to establish a polarization/kinematics pre-filter state equation; based on the heading information calculated by polarization measurement, the measurement equation of the pre-filter is constructed, and the pre-filter is filtered and solved; the three-dimensional attitude misalignment angle, three-dimensional velocity error, three-dimensional position error, three-dimensional gyroscope random drift and three-dimensional accelerometer random zero bias of the inertial navigation are used as state quantities to establish a polarization/inertial navigation/kinematics post-filter state equation; based on the position, heading information and vehicle speed information output by the pre-filter, the measurement equation of the post-filter is constructed, and the estimation of the inertial navigation error is realized by using the Kalman filtering method. The two-degree-of-freedom kinematic model error is estimated by a two-stage filter, the inertial navigation position, velocity and heading errors are corrected, and the positioning and orientation accuracy of the vehicle as a whole is effectively improved.

Specifically, as shown in FIG1 , a tightly integrated navigation method of kinematic model/polarization/inertial navigation mutual correction of the present invention comprises the following steps:

Step 1: Using the longitude, latitude, heading, and front wheel steering angle error of the vehicle output by the kinematic model as state quantities, and based on the two-degree-of-freedom kinematic model of the vehicle, construct the state equation of the polarization/kinematic pre-filter, including:

The east position of the vehicle output by the two-degree-of-freedom kinematic model , North position ,course And the vehicle front wheel angle error As a state quantity ,Right now , according to the two-degree-of-freedom kinematic model of the vehicle, the state equation of the polarization/kinematic pre-filter is constructed as:

(1)

The superscript T represents the transpose of the matrix. represents the forward speed of the vehicle, represents the distance from the front of the vehicle to the center of mass of the vehicle, represents the distance from the rear end to the center of mass of the vehicle, represents the front wheel turning angle of the vehicle, for The first-order derivative of for The first-order derivative of for The first-order derivative of express The first derivative of . The angle between the vehicle's center of mass velocity and the direction of the vehicle body is called the center of mass sideslip angle and is expressed as:

(2)

Step 2: Calculate the vehicle's heading information based on the polarization measurement, construct the heading vector measurement equation of the polarization/kinematics pre-filter, use the nonlinear filtering method to filter and solve the pre-filter, and realize the correction of the vehicle position, heading and front wheel steering angle error output by the two-degree-of-freedom kinematic model, including:

The vehicle's heading information is calculated based on the polarization measurement and is recorded as , the heading vector measurement equation for constructing the polarization/kinematics pre-filter is:

(3)

in, represents the heading observation, The observation matrix representing the heading, . To measure noise.

The state equation of the polarization/kinematics pre-filter in equation (1) is a nonlinear equation. The nonlinear filtering method is used to perform filtering and solving, and the front wheel steering angle error of the vehicle is estimated, so as to achieve effective estimation of the vehicle position and heading output by the two-degree-of-freedom kinematic model of the vehicle.

Step 3: Using the 3D attitude misalignment angle, 3D velocity error, 3D position error, 3D gyroscope random drift, and 3D accelerometer random zero bias as state quantities, establish the state equation of the polarization/inertial navigation/kinematics post-filter, including:

Misalignment angle in 3D posture , 3D velocity error , 3D position error , 3D gyroscope random drift , 3D accelerometer random bias As a state quantity ,Right now , the state equation of the polarization/inertial navigation/kinematics post-filter is established as:

(4)

(5)

(6)

Among them, the subscript It means east, north, and sky; is the state quantity The first-order derivative of is the state transfer equation, is the system state noise, Representation and state quantity The mapping function is is the three-dimensional relationship matrix of attitude, velocity and position errors obtained from the inertial navigation error equation, It is an intermediate matrix and has no specific meaning; is the attitude transfer matrix, The coordinate system representing the vehicle itself is called the carrier system. The coordinate system representing the solution of vehicle navigation information is called the navigation system.

Step 4: Based on the vehicle position and heading output by the polarization/kinematics pre-filter and the vehicle speed information calculated by the odometer, the measurement equation of the polarization/inertial navigation/kinematics post-filter is constructed, and the three-dimensional attitude misalignment angle, three-dimensional velocity error and three-dimensional position error in the state quantity are estimated by the Kalman filtering method to correct the inertial navigation position error, velocity error and heading error, including:

The measurement equation for the vehicle position based on the vehicle position output by the polarization/kinematic pre-filter is:

(7)

in, represents the position observation, The observation matrix representing the position, . is the longitude of the vehicle output by the integrated navigation system, is the latitude of the vehicle output by the integrated navigation system, is the longitude of the vehicle output by the polarization/kinematics pre-filter, is the latitude of the vehicle output by the front filter, To measure the noise, represents a diagonal matrix, Indicates the main curvature radius of the Maoyou circle at the location, Indicates the principal radius of curvature of the meridian at the location.

The heading measurement equation based on the heading output of the polarization/kinematic pre-filter is:

(8)

in, represents the heading observation, The observation matrix representing the heading, . is the heading output by the integrated navigation system, is the pitch angle output by the integrated navigation system, is the vehicle heading output from the polarization/kinematics pre-filter, To measure noise.

The speed measurement equation based on the vehicle speed output by the vehicle odometer is:

(9)

in, represents the velocity observation, The observation matrix representing the velocity, . is the third-order identity matrix, is the navigation system speed output by the integrated navigation system, To measure the noise, is the navigation system speed calculated based on the vehicle odometer, expressed as , is the carrier speed output by the odometer, and Their mean is 0 and their variance is and Gaussian white noise.

According to the state equations of the polarization/inertial navigation/kinematics post-filter in equations (4) to (6) and the measurement equations in equations (7) to (9), the Kalman filtering method is used to estimate the three-dimensional attitude misalignment angle, three-dimensional velocity error, and three-dimensional position error in the state quantities, thereby correcting the inertial navigation position error, velocity error, and heading error.

Example:

The simulation trajectory of the vehicle is: straight section (acceleration 2s/constant speed 100s), left turn 300s, right turn 300s, left turn 200s, right turn 200s, total mileage 22101m. The noise of the inertial sensor is set to gyro zero bias 20°/h, angle random walk , accelerometer zero bias 10ug, speed random walk FIG. 2 is a diagram showing the setting of the vehicle simulation trajectory of this embodiment.

A constant deviation of 11.5° (0.2 rad) and uniformly distributed noise with an amplitude of 0.57° (0.01 rad) are added to the front wheel steering angle of the vehicle. Using the method of the present invention, the front wheel steering angle error of the vehicle is estimated and corrected by using a polarization/kinematics pre-filter, and the inertial navigation error is further corrected by using a polarization/inertial navigation/kinematics post-filter. The obtained front wheel steering angle error curve and position error curve are shown in Figures 3 and 4, respectively.

As can be seen from FIG3, the method of the present invention can quickly and accurately estimate the front wheel steering angle error. On this basis, the root mean square error (RMSE) of the longitude position error (corresponding to the upper curve in FIG4) estimated is 3.14m, and the root mean square error (RMSE) of the latitude position error (corresponding to the lower curve in FIG4) is 0.97m. At the end of the simulation, the longitude error is 9.45m, and the latitude error is -1.03m. In the case of serious errors in the front wheel steering angle, the method described in the present invention can be used to correct it and obtain a better position estimation effect.

It will be easily understood by those skilled in the art that the above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions and improvements made within the spirit and principles of the present invention should be included in the protection scope of the present invention.

Claims (5)

1. A tightly integrated navigation method based on kinematic model/polarization/inertial navigation mutual correction, characterized in that it comprises the following steps: Step 1: The eastward position of the vehicle output by the two-degree-of-freedom kinematic model , North position ,course And the vehicle front wheel angle error As a state quantity ,Right now , establish the state equation of the polarization/kinematic pre-filter , Representation and state quantity Related mapping functions; Indicates the state The first derivative of ; the superscript T indicates the transpose of the matrix; Step 2: Construct the heading measurement equation of the polarization/kinematics pre-filter based on the heading calculated by polarization measurement, and estimate the state quantity using the nonlinear filtering method. , realize the vehicle position and heading output of the two-degree-of-freedom kinematic model And the vehicle front wheel angle error Correction to obtain the vehicle position and heading output by the polarization/kinematics pre-filter ; Step 3: 3D posture misalignment angle , 3D velocity error , 3D position error , 3D gyroscope random drift , 3D accelerometer random bias As a state quantity ,Right now , establish the state equation of polarization/inertial navigation/kinematics post-filter , Representation and state quantity Related mapping functions; subscript Indicates the three directions of east, north and sky; Indicates the state The first derivative of ; Step 4: Vehicle position and heading according to the output of the polarization/kinematics pre-filter in step 2 As well as the vehicle speed calculated by the odometer, the vehicle position and heading of the polarization/inertial navigation/kinematic post-filter are constructed and speed measurement equation; the Kalman filter method is used to estimate the state quantity The 3D posture misalignment angle , 3D velocity error , 3D position error , to realize the correction of inertial navigation error. 2. A tightly integrated navigation method based on kinematic model/polarization/inertial navigation mutual correction according to claim 1, characterized in that the state equation of the polarization/kinematic pre-stage filter in step 1 is The specific manifestations are: (1) in, represents the forward speed of the vehicle, represents the distance from the front of the vehicle to the center of mass of the vehicle, represents the distance from the rear end to the center of mass of the vehicle, represents the front wheel turning angle of the vehicle, for The first-order derivative of for The first-order derivative of For the course The first-order derivative of Indicates the vehicle's front wheel steering angle error The first derivative of ; The angle between the vehicle's center of mass velocity and the direction of the vehicle body is called the center of mass sideslip angle and is expressed as: (2). 3. A tightly integrated navigation method based on kinematic model/polarization/inertial navigation mutual correction according to claim 2, characterized in that, in step 2, the heading measurement equation for constructing the heading of the polarization/kinematic pre-filter is: (3) in, is the polarization measurement noise, is the vehicle heading information calculated based on polarization measurement, The observation matrix representing the heading, ; The state equation of the polarization/kinematics pre-filter of equation (1) is filtered and solved using a nonlinear filtering method to estimate the vehicle front wheel steering angle error. , realize the vehicle position and heading output of the vehicle's two-degree-of-freedom kinematic model effective estimate of . 4. A tightly integrated navigation method based on kinematic model/polarization/inertial navigation mutual correction according to claim 3, characterized in that in step 3, the polarization/inertial navigation/kinematic post-stage filter state equation The specific manifestations are: (4) (5) (6) in, is the state quantity The first-order derivative of is the state transfer equation, is the system state noise, is the three-dimensional relationship matrix of attitude, velocity and position errors obtained from the inertial navigation error equation, It is an intermediate matrix and has no specific meaning; is the attitude transfer matrix, The coordinate system representing the vehicle itself is called the carrier system. The coordinate system representing the solution of vehicle navigation information is called the navigation system. 5. The tightly integrated navigation method based on kinematic model/polarization/inertial navigation mutual correction according to claim 4, characterized in that step 4 comprises: Based on the vehicle position output by the polarization/kinematics pre-filter, the vehicle position measurement equation is constructed as follows: (7) in, represents the position observation, The observation matrix representing the position, , is the longitude of the vehicle output by the integrated navigation system, is the latitude of the vehicle output by the integrated navigation system, is the longitude of the vehicle output by the polarization/kinematics pre-filter, is the latitude of the vehicle output by the front filter, To measure the noise, represents a diagonal matrix, Indicates the main curvature radius of the Maoyou circle at the location, Indicates the principal radius of curvature of the meridian at the location; Based on the vehicle's heading output by the polarization/kinematics pre-filter, the heading measurement equation is constructed as follows: (8) in, represents the heading observation, The observation matrix representing the heading, ; is the heading output by the integrated navigation system, is the pitch angle output by the integrated navigation system, is the vehicle heading output from the polarization/kinematics pre-filter, To measure noise; Based on the vehicle speed output by the vehicle odometer, the speed measurement equation constructed is: (9) in, represents the velocity observation, The observation matrix representing the velocity, , is the third-order identity matrix, is the navigation system speed output by the integrated navigation system, To measure the noise, is the navigation system speed calculated based on the vehicle odometer, expressed as , is the carrier speed output by the odometer, and Their mean is 0 and their variance is and Gaussian white noise; According to the state equations of the polarization/inertial navigation/kinematics post-filter of equations (4) to (6) and the measurement equations of equations (7) to (9), the Kalman filter method is used to estimate the 3D attitude misalignment angle , 3D velocity error , 3D position error , to realize the correction of inertial navigation position error, velocity error and heading error.