Multi-source information fusion combined navigation method and system independent of satellite navigation

Abstract

The invention discloses a multi-source information fusion integrated navigation method and system independent of satellite navigation, and belongs to the field of navigation and positioning of autonomous mobile platforms. Establishing a vision-laser-inertial odometer and polarized light sensor measurement model, establishing an inertial navigation system motion model, initializing the integrated navigation system, and determining the attitude of the integrated navigation system according to a carrier state node position constraint, a course constraint and an adjacent state node pose transformation constraint at the current moment; establishing an integrated navigation system state vector and a measurement error function to obtain a to-be-optimized factor graph, and optimizing the to-be-optimized factor graph by using a Gaussian-Newton method to obtain optimal estimated values of the carrier position, speed and attitude under a world coordinate system with the minimum error function. According to the method, multi-source information is fused, information contributions are balanced through optimization of the factor graph, the robustness and reliability of the system in a satellite navigation signal rejection environment are remarkably improved, and the method has good practical engineering significance.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of navigation and positioning of autonomous mobile platforms, and in particular to a multi-source information fusion combined navigation method and system independent of satellite navigation. BACKGROUND

[0002] Navigation and positioning is the core technology for autonomous mobile platforms to achieve autonomy. Currently, the commonly used navigation devices in this field include inertial navigation system (INS) based on micro-electromechanical system inertial measurement unit (MEMS-IMU), global navigation satellite system (GNSS), odometer, and magnetometer. INS calculates the pose change of the platform by integrating the angular velocity and acceleration measured by IMU, and is completely autonomous and does not rely on external signals. However, the gyroscopes and accelerometers in INS have inherent bias and noise, which causes the navigation error to accumulate over time, and therefore cannot be used alone for long-term high-precision navigation. GNSS can provide absolute position information and is the basis for most outdoor autonomous mobile platforms. However, GNSS signals are extremely weak and are easily affected by shielding, reflection, and multipath interference, and in environments such as tunnels, underground, mountainous areas, or densely populated cities where GNSS signals are blocked or attenuated, the positioning accuracy will decrease sharply or even fail completely.

[0003] In addition, the heading angle is the direction reference of the navigation system and directly determines the long-term accuracy and reliability of position calculation. However, INS cannot directly observe the heading, and the magnetic heading angle measured by the magnetometer is not the true north heading angle. The difference between the two, the magnetic declination angle, is also a quantity that varies with geographic location, and the magnetometer is extremely susceptible to external interference, with extremely poor measurement accuracy and reliability.

[0004] From the above analysis, single navigation mode always has different defects. Multi-source information fusion combined navigation has become the current mainstream. The most common one is GNSS / INS combined navigation. However, once GNSS / INS combined navigation enters a GNSS blocked environment, the system error will accumulate rapidly, leading to navigation failure. Therefore, finding a reliable satellite navigation alternative for platforms in GNSS blocked environments has very important practical engineering significance. SUMMARY

[0005] The present application aims to overcome the problem of navigation and positioning of autonomous mobile platforms in GNSS blocked environments and provides a multi-source information fusion combined navigation method and system independent of satellite navigation.

[0006] The purpose of the present application is achieved by the following technical solutions: In a first aspect, a multi-source information fusion combined navigation method independent of satellite navigation is provided, comprising the following steps: (1) selecting the position, velocity, and attitude of the carrier as the state of the combined navigation system; (2) establishing a visual-laser-inertial odometry measurement model, a polarized light sensor measurement model, and an inertial navigation system motion model; (3) initializing a combined navigation system composed of a visual-laser-inertial odometry, a polarized light sensor, and an inertial navigation system; (4) obtaining a carrier state node position constraint, a carrier adjacent state node pose transformation constraint, and a carrier state node heading constraint at a current time; (5) using the carrier state node position constraint, the carrier adjacent state node pose transformation constraint, and the carrier state node heading constraint to establish an error function of a state and a measurement node of the combined navigation system, obtaining an optimized factor graph, and optimizing the optimized factor graph to obtain optimal estimation values of a carrier position, velocity, and attitude in a world coordinate system with a minimum error function, thereby obtaining combined navigation parameters.

[0007] In some embodiments, an error term of the inertial odometry is included in the state for synchronous estimation in the visual-laser-inertial odometry measurement model.

[0008] In some embodiments, the polarized light sensor measurement model is used to provide an absolute heading reference, to calculate a carrier heading angle by detecting an atmospheric polarization pattern, and to achieve periodic heading error correction.

[0009] In some embodiments, the combined navigation system initialization in step (3) adopts a hierarchical strategy: First, the initial bias and the gravity direction are estimated by IMU static data to determine the roll and pitch angles; then the laser-inertial odometry is started, and the velocity and pose are solved by using the scaled observation of the laser radar; finally, the visual information is introduced to construct a map with the starting position as the origin.

[0010] In some embodiments, the carrier adjacent state node pose transformation constraint in step (4) is obtained by the following steps: All inertial navigation measurement data between the current time stamp and the last time stamp are integrated, and the carrier position, velocity, and attitude data obtained by the integration are used to establish the carrier adjacent state node pose transformation constraint.

[0011] In some embodiments, the carrier state node heading constraint in step (4) is obtained by the following steps: The polarized light sensor measurement data at the current time stamp is searched, and the carrier state node heading constraint is established by the polarized light sensor measurement model and the polarized light sensor measurement data.

[0012] In some embodiments, the specific steps of obtaining the optimal estimation values of the carrier position, velocity, and attitude in the world coordinate system with the minimum error function include: establishing an error function of a state node of the carrier; According to the carrier state node position constraint, an error function of the odometer measurement is established; According to the carrier state node heading constraint, an error function of the polarization light sensor measurement is established; According to the carrier adjacent state node pose transformation constraint, an error function of the inertial navigation system measurement is established; The error function of the factor graph is obtained, and a Gauss-Newton method is used to solve the factor graph optimization problem to obtain the optimal estimation value of the carrier position, velocity and attitude in the world coordinate system with the minimum error function.

[0013] In the second aspect, a multi-source information fusion integrated navigation system independent of satellite navigation is provided for executing the navigation method of the first aspect, comprising: A vision-laser-inertial odometer module is configured to obtain the three-dimensional position of the autonomous mobile platform; A polarization light sensor module is configured to obtain the heading angle of the autonomous mobile platform; An inertial navigation module is configured to calculate the three-dimensional position, velocity and attitude; A factor graph optimization module is configured to establish the error functions of the integrated navigation system state and measurement nodes using the carrier state node position constraint, the carrier adjacent state node pose transformation constraint and the carrier state node heading constraint, to obtain the factor graph to be optimized; and to optimize the factor graph to be optimized to obtain the optimal estimation value of the carrier position, velocity and attitude in the world coordinate system with the minimum error function, thereby obtaining the integrated navigation parameters.

[0014] It should be further explained that the technical features of the above-mentioned various option embodiments can be combined or replaced with each other to form new technical solutions without conflict.

[0015] Compared with the prior art, the present application has the following advantages: 1. Multi-source information deep fusion, improving system robustness: through the collaborative fusion of vision-laser-inertial odometer, polarization light sensor and inertial navigation system, a complementary observation system is constructed, which can provide stable, reliable and slowly decaying precision navigation output in most GNSS denial environments.

[0016] 2. Polarization light heading reference, solving the heading divergence problem: the polarization light sensor provides absolute heading information that is not affected by electromagnetic interference, effectively correcting the heading cumulative error of the inertial navigation and odometer system, and improving the long-time navigation precision.

[0017] 3. Factor graph optimization framework, improving state estimation precision: using the Gauss-Newton method to optimize the factor graph can directly optimize the nonlinear function without linearization approximation, thereby more accurately estimating the state of the system and improving the navigation precision.

[0018] 4. Layered initialization strategy to improve system startup stability: Through a layered process of static initialization, laser inertial odometry startup and visual map construction, the system is ensured to quickly and stably enter a high-precision navigation state under various initial conditions.

[0019] 5. Strong environmental adaptability: It is suitable for various complex environments such as indoors, tunnels, densely populated urban areas, forests, underground, and electromagnetic interference, and has good engineering practical value and application prospects. Attached Figure Description

[0020] Figure 1 This is a flowchart of a multi-source information fusion and integrated navigation method that does not rely on satellite navigation according to the present invention; Figure 2 This is a schematic diagram of a multi-source information fusion integrated navigation system that does not rely on satellite navigation according to the present invention; Figure 3 This is a schematic diagram of factor graph optimization according to the present invention. Detailed Implementation

[0021] The technical solution of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, not all embodiments. The components of the embodiments of this application described and shown in the accompanying drawings can generally be arranged and designed in various different configurations. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0022] It should be noted that the defects in the solutions in the prior art are all the results of the inventors' practice and careful research. Therefore, the discovery process of the above problems and the solutions proposed by the embodiments of this application in the following text should be the inventors' contributions to this application in the process of invention and creation, and should not be understood as technical content known to those skilled in the art.

[0023] In view of the technical problems pointed out in the background art, the present invention provides the following embodiments: like Figure 1 As shown in an exemplary embodiment, a multi-source information fusion navigation method that does not rely on satellite navigation includes the following steps: (1) Select the carrier's position, velocity, and attitude as the states of the integrated navigation system; (2) Establish a visual-laser-inertial odometry measurement model, a polarized light sensor measurement model, and an inertial navigation system motion model; (3) Initialize the integrated navigation system consisting of vision-laser-inertial odometry, polarization light sensor and inertial navigation; (4) Obtain the current position constraints of the carrier state node, the pose transformation constraints of adjacent carrier state nodes, and the heading constraints of the carrier state node; (5) Establish the error function of the state and measurement nodes of the integrated navigation system using the position constraints of the carrier state nodes, the pose transformation constraints of the adjacent state nodes of the carrier, and the heading constraints of the carrier state nodes, and obtain the factor map to be optimized; and optimize the factor map to obtain the optimal estimated values ​​of the position, velocity and attitude of the world coordinate system download body with the minimum error function, and obtain the integrated navigation parameters.

[0024] In step (1), the position, velocity, and attitude of the carrier are selected as the state variables of the integrated navigation system. This step is the foundation for constructing the entire filtering estimation process, enabling the system to directly fuse observation information from multiple sensors, including visual-laser-inertial odometry, polarization light sensors, and the inertial navigation system, thereby achieving optimal correction of the state vector. For example, this invention constructs the state of the integrated navigation system as described in step (1). The status nodes of the integrated navigation system at any given time are shown below: in, for The status nodes of the navigation system are constantly combined. express The three-dimensional position of the carrier in the world coordinate system at any time. express The three-dimensional velocity of the carrier in the world coordinate system at all times express The attitude of the carrier in the world coordinate system at any given time is represented by Euler angles. These represent the roll angle, pitch angle, and yaw angle, respectively.

[0025] In step (2), a deep fusion architecture of heterogeneous models is constructed, enabling continuous, reliable, and high-precision autonomous navigation in complex environments. This heterogeneous model incorporates the error terms of the inertial sensor (inertial odometry) into the state for synchronous estimation, providing a time reference and prediction basis for the entire fusion system. Simultaneously, using the models on both sides of the polarization sensor as absolute heading references, the absolute heading is calculated by detecting atmospheric polarization patterns, effectively providing the system with a "natural compass" free from electromagnetic interference. This solves the inherent heading divergence problem of inertial navigation and odometry systems, enabling periodic heading error correction.

[0026] For example, the present invention selects the position information measured by the visual-laser-inertial odometry as the measurement value, and the established measurement model of the visual-laser-inertial odometry is as follows: in, express Three-dimensional position measurements from a real-time vision-laser-inertial odometry system. Represents the measurement function. for The status nodes of the navigation system are constantly combined. Gaussian noise for odometer measurement.

[0027] For example, the established polarization light sensor measurement model is as follows: in, It is a measurement value from a polarization light sensor, defined as the angle between the longitudinal axis of the carrier and the solar meridian. This indicates the measurement noise of the polarization light sensor. Download the heading angle for the world coordinate system. The solar azimuth angle in the geographic coordinate system. pass Calculated It is the sine function of the solar altitude angle. The solar declination angle, The latitude of the observation point It is the solar hour angle.

[0028] For example, the motion model of the established inertial navigation system is as follows: in, Measurements for inertial navigation systems Coordinate values ​​in three axes, for The position, velocity, and attitude of the carrier in the world coordinate system at all times; for Time-based control input, , In the world coordinate system The inertial navigation system compensates for angular velocity and acceleration after zero bias. In the carrier coordinate system The angular velocity and acceleration measured by the inertial navigation system at all times. The zero bias values ​​are the calibrated gyroscope and accelerometer zero bias values. The sampling interval for the inertial navigation system. Let be the rotation matrix from the carrier coordinate system to the world coordinate system, and its value is: .

[0029] For example, in step (3), the integrated navigation system is initialized using a layered strategy. First, the system remains briefly stationary, and the initial offset and gravity direction are estimated using IMU data to determine the roll and pitch angles. Then, the laser-inertial odometry module is activated, and the velocity and pose are solved using the scaled observations of the lidar. Subsequently, visual information is introduced into the map with the starting position as the origin.

[0030] For example, the steps for obtaining the pose transformation constraints of adjacent state nodes of the carrier in step (4) are as follows: Integrate all inertial navigation measurement data between the current time stamp and the previous time stamp, and use the integrated carrier position, velocity, and attitude data to establish pose transformation constraints for adjacent state nodes of the carrier.

[0031] For example, the steps for obtaining the heading constraints of the carrier state node in step (4) are as follows: Find the polarization sensor measurement data with the current time stamp, and establish the carrier state node heading constraint by using the polarization sensor measurement model and the polarization sensor measurement data.

[0032] For example, step (5) specifically includes: Step (5a): Establishing a carrier The state node error function at time t; Step (5b): Based on the vehicle state node position constraints, establish the error function for odometer measurement: in, Due to odometer measurement error, For the odometer node covariance, for Odometer position measurement value at time. The measurement model of the odometer relative to the system state quantities; Step (5c): Based on the heading constraints of the carrier state node, establish the error function for the polarization sensor measurement: in, For the measurement error of the polarization light sensor, For the node covariance of the polarization light sensor, for The time-of-flight polarization sensor measurement value is defined as the angle between the longitudinal axis of the carrier measured by the polarization sensor and the solar meridian. This is a measurement model for a polarized light sensor. Step (5d): Based on the pose transformation constraints of adjacent state nodes of the carrier, establish the error function for the inertial navigation system measurement: in, For measurement errors in inertial navigation systems, For the nodal covariance of the motion model of the inertial navigation system, for Measurements from the inertial navigation system at all times. For measurement models of inertial navigation systems; Step (5e): Obtain the error function of the factor plot, expressed as follows: in, For state vectors, for The optimal estimate; Step (5f): Use the Gauss-Newton method to solve the factor graph optimization problem and obtain the optimal estimates of the position, velocity and attitude of the world coordinate system downloaded body with the minimum error function.

[0033] Furthermore, such as Figure 3 As shown, the factor graph optimization structure of the present invention will be further described: The state node at time 0 is , With the visual-laser-inertial odometry node at time 0 Using the measurement data and measurement model at time 0 of the odometer, the position constraints of the carrier state nodes are established, namely... and The connection relationship between them; With the polarization sensor node at time 0 Using the measurement data and measurement model from the polarization sensor at time 0, the heading constraint of the carrier state node is established, i.e. and The connection relationships between them; the inertial navigation system node at time 0. With state nodes Using measurement data from inertial navigation between time 0 and 1 and the inertial navigation system model, a carrier state node is established. pose transformation constraints between them, i.e. and The connection relationship between them; The state node at time 1 is , With the visual-laser-inertial odometry node at time 1 Using the measurement data and measurement model from the odometer at time step 1, positional constraints for the carrier's state nodes are established. and The connection relationship between them; With the polarization sensor node at time 1 Using the measurement data and measurement model from the polarization light sensor at time 1, the heading constraint of the carrier state node is established, i.e. and The connection relationships between them; the inertial navigation system node at time 1. With state nodes Using measurement data from inertial navigation time points 1 to 2 and the inertial navigation system model, a carrier state node is established. pose transformation constraints between them, i.e. and The connection relationship between them; The state node at time 2 is , With the visual-laser-inertial odometry node at time 2 Using the measurement data and measurement model from the odometer at time 2, positional constraints for the carrier's state nodes are established. and The connection relationship between them; With the polarization sensor node at time 2 Using the measurement data and measurement model from the polarization sensor at time 2, the heading constraint of the carrier state node is established, i.e. and The connection relationships between them; the inertial navigation system nodes at time 2. With state nodes Using measurement data from inertial navigation between times 2 and 3 and the inertial navigation system model, the carrier state nodes are established. pose transformation constraints between them, i.e. and The connection relationship between them; No. The state node at time is , With the Moment-time visual-laser-inertial odometry node Through the odometer Based on the measurement data and measurement model at each moment, the position constraints of the carrier state nodes are established, i.e. and The connection relationship between them; With the Polarization sensor node at time Through the polarization light sensor Based on the measurement data and measurement model at each moment, the heading constraints of the carrier state nodes are established, i.e. and The connection between them; the first Inertial navigation system node at any moment With state nodes via inertial navigation arrive Using measurement data and inertial navigation system models between different time points, establish the carrier state nodes. pose transformation constraints between them, i.e. and The connection relationship between them.

[0034] like Figure 2 As shown, in another exemplary embodiment, based on the same inventive concept as the method embodiment, a multi-source information fusion combined navigation system that does not rely on satellite navigation is provided for performing the navigation method, including: A vision-laser-inertial odometry module is used to obtain the three-dimensional position of the autonomous mobile platform; A polarized light sensor module is used to obtain the heading angle of the autonomous mobile platform; Inertial navigation module, used to calculate three-dimensional position, velocity and attitude; The factor graph optimization module uses the position constraints of the carrier state nodes, the pose transformation constraints of adjacent carrier state nodes, and the heading constraints of the carrier state nodes to establish the error function of the state and measurement nodes of the integrated navigation system, thereby obtaining the factor graph to be optimized. The module then optimizes the factor graph to obtain the optimal estimates of the position, velocity, and attitude of the downloaded body in the world coordinate system with the minimum error function, thus obtaining the integrated navigation parameters.

[0035] The above detailed embodiments are a description of the present invention. It should not be considered that the specific embodiments of the present invention are limited to these descriptions. For those skilled in the art, several simple deductions and substitutions can be made without departing from the concept of the present invention, and all of these should be considered to fall within the protection scope of the present invention.

Claims

1. A multi-source information fusion and integrated navigation method that does not rely on satellite navigation, characterized in that, Includes the following steps: (1) Select the carrier's position, velocity, and attitude as the states of the integrated navigation system; (2) Establish a visual-laser-inertial odometry measurement model, a polarized light sensor measurement model, and an inertial navigation system motion model; (3) Initialize the integrated navigation system consisting of vision-laser-inertial odometry, polarization light sensor and inertial navigation; (4) Obtain the current position constraints of the carrier state node, the pose transformation constraints of adjacent carrier state nodes, and the heading constraints of the carrier state node; (5) Establish the error function of the state and measurement nodes of the integrated navigation system using the position constraints of the carrier state nodes, the pose transformation constraints of the adjacent state nodes of the carrier, and the heading constraints of the carrier state nodes, and obtain the factor map to be optimized; and optimize the factor map to obtain the optimal estimated values ​​of the position, velocity and attitude of the world coordinate system download body with the minimum error function, and obtain the integrated navigation parameters.

2. The multi-source information fusion and integrated navigation method that does not rely on satellite navigation according to claim 1, characterized in that, The vision-laser-inertial odometry measurement model incorporates the error term of the inertial odometry into the state for synchronous estimation.

3. The multi-source information fusion and integrated navigation method that does not rely on satellite navigation according to claim 1, characterized in that, The polarization sensor measurement model is used to provide an absolute heading reference. By detecting the atmospheric polarization mode, the heading angle of the carrier is calculated, and periodic heading error correction is achieved.

4. The multi-source information fusion and integrated navigation method that does not rely on satellite navigation according to claim 1, characterized in that, In step (3), the initialization of the integrated navigation system adopts a layered strategy: First, the initial bias and gravity direction are estimated using IMU static data to determine the roll and pitch angles. Then, the laser-inertial odometry is activated, and the velocity and pose are solved using scaled observations from the lidar. Finally, visual information is introduced to construct a map with the starting position as the origin.

5. The multi-source information fusion and integrated navigation method that does not rely on satellite navigation according to claim 1, characterized in that, The steps for obtaining the pose transformation constraints of adjacent state nodes of the carrier in step (4) are as follows: Integrate all inertial navigation measurement data between the current time stamp and the previous time stamp, and use the integrated carrier position, velocity, and attitude data to establish pose transformation constraints for adjacent state nodes of the carrier.

6. The multi-source information fusion and integrated navigation method that does not rely on satellite navigation according to claim 1, characterized in that, The steps for obtaining the heading constraints of the carrier state node in step (4) are as follows: Find the polarization sensor measurement data with the current time stamp, and establish the carrier state node heading constraint by using the polarization sensor measurement model and the polarization sensor measurement data.

7. The multi-source information fusion and integrated navigation method that does not rely on satellite navigation according to claim 1, characterized in that, The specific steps for obtaining the optimal estimates of the position, velocity, and attitude of the downloaded volume in the world coordinate system with the minimum error function include: Establish the state node error function of the carrier; Based on the constraints of the carrier state node position, an error function for mileage measurement is established; Based on the heading constraints of the carrier state nodes, an error function for the polarization light sensor measurements is established. Based on the pose transformation constraints of adjacent state nodes of the carrier, an error function for inertial navigation system measurements is established; The error function of the factor graph is obtained, and the Gauss-Newton method is used to solve the factor graph optimization problem to obtain the optimal estimates of the position, velocity and attitude of the world coordinate system downloaded body with the minimum error function.

8. A multi-source information fusion integrated navigation system that does not rely on satellite navigation, used to execute the navigation method according to any one of claims 1-7, characterized in that, include: A vision-laser-inertial odometry module is used to obtain the three-dimensional position of the autonomous mobile platform; A polarized light sensor module is used to obtain the heading angle of the autonomous mobile platform; Inertial navigation module, used to calculate three-dimensional position, velocity and attitude; The factor graph optimization module uses the position constraints, pose change constraints, and heading constraints of the carrier state nodes to establish the error function of the state and measurement nodes of the integrated navigation system, obtains the factor graph to be optimized, and optimizes the factor graph to obtain the optimal estimates of the position, velocity, and attitude of the downloaded body in the world coordinate system with the minimum error function, thus obtaining the integrated navigation parameters.

Images