CN114459474B - A method of inertial/polarization/radar/optical flow tight combined navigation based on factor graph - Google Patents

Abstract

The invention discloses an inertial/polarization/radar/optical-flow combined navigation method based on factor graph, which comprises the steps of selecting the posture, the position and the speed of a carrier under a world coordinate system as the state of a tightly combined navigation system, respectively establishing an inertial navigation system pre-integration model, establishing a polarized light intensity sensor measurement model, establishing a radar odometer measurement model and an optical-flow sensor measurement model, establishing a unified tightly combined navigation system measurement model after augmentation, initializing the tightly combined navigation system and acquiring an initial value. Selecting the state of the tightly-combined navigation system at the current moment as a factor node, establishing pre-integration, heading and pose transformation constraint of the system adjacent node at the current moment, constraint of the state node speed, designing an error function of the tightly-combined navigation system, and optimizing by using a nonlinear solver to obtain the optimal estimated value of parameters such as the pose, the position and the speed of the carrier under the world coordinate system. The method has the characteristics of strong robustness, high precision and the like.

Description

A method of inertial/polarization/radar/optical flow tight integration navigation based on factor graph

Technical Field

The present invention belongs to the category of autonomous navigation technology, and relates to a multi-sensor fusion navigation method based on a tight combination of inertia/polarization/radar/optical flow of factor graphs.

Background Art

Multi-sensor Fusion (MSF) is an information processing process that uses computer technology to automatically analyze and synthesize information and data from multiple sensors or multiple sources according to certain criteria to complete the required decision-making and estimation. Multi-sensor data fusion technology has been a hot research direction in recent years and has broad application prospects in the fields of military, manned spaceflight, and autonomous driving.

Compared with a single sensor, the use of multi-sensor information fusion technology can enhance the system's survivability, improve the reliability and robustness of the entire system, enhance the credibility of data, improve accuracy, expand the system's time and space coverage, increase the system's real-time performance and information utilization, etc. in solving problems such as detection, tracking and target identification.

The commonly used methods of multi-sensor data fusion can basically be divided into two categories: random and artificial intelligence. At present, the methods of multi-sensor data fusion focus on random. The Kalman filter algorithm is a commonly used fusion method for multi-sensor combination systems. Its disadvantages are poor real-time performance when sensor information is redundant, and failure to fully utilize fusion history information, resulting in an increase in the probability of failure when the subsystem is increased, reducing the robustness of the system. The multi-sensor data fusion method based on factor graphs constructs a graph model of the system within a certain period of time, associates the system state with navigation information, and realizes data fusion based on the posterior estimation theory. After all available measurement values are given, the maximum posterior probability estimate of the joint probability distribution function of all states is calculated, which can effectively realize the plug-and-play of navigation sensors, solve the asynchronous problem of multi-sensor observation data, and reduce the amount of calculation while ensuring accuracy. However, the navigation accuracy of the multi-sensor data fusion method based on factor graphs also depends on the accuracy of each sensor. When the sensor is subject to external interference (such as when the geomagnetic navigation system is subject to strong magnetic interference), the accuracy of the multi-sensor fusion system decreases and the cumulative error increases.

Research has found that polarized light has good autonomy and can be used in multi-sensor fusion systems. In 1871, the famous British physicist Rayleigh proposed the Rayleigh scattering law, which revealed the scattering characteristics of light and thus discovered the polarization distribution pattern of the sky atmosphere. The atmospheric polarization pattern is symmetrically distributed about the solar meridian and anti-solar meridian, and contains important navigation information. Using the sky atmospheric polarization distribution pattern for navigation has the advantages of being difficult to be affected by human interference on a large scale and the error does not accumulate over time. It is expected to become an important auxiliary means in multi-sensor combined navigation.

The differences between the present invention and the patent "A method for bionic polarization synchronous positioning and composition based on graph optimization" (hereinafter referred to as Patent 1) are as follows:

Difference 1: Patent 1 uses a loose combination factor graph method to estimate the optimal value of the combined navigation system state, and does not fully utilize the sensor data. The present invention uses a tight combination factor graph method to estimate the optimal value of the combined navigation system state, uses more original sensor data, and associates the states of two adjacent combined navigation systems to obtain higher navigation accuracy.

Difference 2: Compared with Patent 1, the present invention additionally adds an optical flow sensor to provide speed constraints, and the factor graph adds new constraints during the optimization process. The optimized optimal estimate result is more accurate, and even if the measurement error of a single sensor is large, it will not destroy the overall estimation result.

Difference 3: Patent 1 establishes the state node position constraints of the carrier based on the radar odometer measurement model, establishes the state node heading constraints based on the polarized light camera measurement model, and establishes the adjacent state node pose transformation constraints based on the inertial navigation system motion model; the present invention establishes the adjacent node pre-integration constraints of the tightly combined navigation system at the current moment based on the inertial navigation system pre-integration model, establishes the adjacent node heading constraints of the tightly combined navigation system based on the polarized light intensity sensor measurement model, establishes the adjacent node pose transformation constraints based on the radar odometer measurement model, and establishes the state node speed constraints based on the optical flow sensor measurement model; constructing different adjacent node state constraints can obtain higher positioning and composition accuracy.

In actual navigation applications, by extracting polarization information from the atmospheric polarization pattern, navigation angle information can be output. Polarization imaging technology can obtain additional polarization multi-dimensional information of target radiation or reflection, and has the ability to improve target detection accuracy and recognition probability. Optical flow is an image-based method that obtains the moving speed of the carrier by obtaining the displacement of pixels in the image. Optical flow sensors and polarization sensors complement each other in navigation to improve navigation positioning accuracy.

Summary of the invention

The technical problem solved by the present invention is as follows: the deficiencies of the prior art are overcome, and a method for inertial/polarization/radar/optical flow tightly integrated navigation based on a factor graph is provided, polarization light information is tightly combined with inertial navigation system information and radar odometer, and optical flow information is introduced, so as to realize the complementary advantages of inertial navigation system information, radar odometer information, polarization light angle information and optical flow speed information in navigation, and a graph optimization method is applied to a multi-sensor integrated navigation system to solve the problems of poor robustness, unsatisfactory navigation accuracy and difficulty in determining the own position in the multi-sensor integrated navigation system.

The technical solution to be solved by the present invention is: a method for inertial/polarization/radar/optical flow tight integrated navigation based on factor graph, comprising the following steps:

Step 1: Select the posture, position and speed of the carrier in the world coordinate system as the state of the inertial/polarization/radar/optical flow tightly integrated navigation system composed of the inertial navigation system, polarization light intensity sensor, radar odometer and optical flow sensor at time k, and establish the inertial navigation system pre-integration model, polarization light intensity measurement model and radar odometer measurement model respectively; select the speed information measured by the optical flow sensor as the measurement value to establish the optical flow sensor measurement model;

Step 2: Initialize the inertial/polarization/radar/optical flow tightly integrated navigation system composed of the inertial navigation system, polarization light intensity sensor, radar odometer and optical flow sensor, and obtain the initial state of the tightly integrated navigation system, map and radar point cloud reference value;

Step 3: Establish pre-integration constraints of adjacent state nodes of the integrated navigation system at the current moment based on the inertial navigation system pre-integration model, establish heading constraints of adjacent state nodes of the integrated navigation system at the current moment based on the polarization light intensity sensor measurement model, establish pose transformation constraints of adjacent state nodes of the integrated navigation system at the current moment with the radar point cloud reference value based on the radar odometer measurement model, establish speed constraints of state nodes of the integrated navigation system at the current moment based on the optical flow sensor measurement model, and design an error function with the inertial/polarization/radar/optical flow tight integrated navigation system state as the state variable node and the sensor measurement as the factor node according to the pre-integration constraints of adjacent state nodes of the integrated navigation system at the current moment, the heading constraints of adjacent state nodes of the integrated navigation system, the pose transformation constraints of adjacent state nodes of the integrated navigation system and the speed constraints of the state nodes of the integrated navigation system, and obtain the error function of the factor graph to be optimized;

Step 4: Optimize the error function of the factor graph based on the nonlinear solver to obtain the optimal estimated values of the attitude, position and velocity parameters of the carrier with the smallest error function in the world coordinate system. Update the optimal estimated values and map of the attitude, position and velocity of the carrier with the smallest error function in the world coordinate system to simultaneously realize the positioning and composition of the carrier.

In step 1, the state node of the tightly integrated navigation system at time k is constructed as follows:

in:

x _k is the state node of the tightly integrated navigation system at time k, Indicates the carrier posture, which is expressed in the form of Euler angles. They are roll angle, pitch angle and yaw angle respectively; represents the rotation matrix from the carrier coordinate system to the world coordinate system at time k, and SO(3) represents the 3D special orthogonal group

Represents the three-dimensional position information of the carrier in the world coordinate system at time k, They represent the coordinate values of the x, y, and z axes in the world coordinate system of the carrier at time k, respectively, and T represents the transpose of the vector;

represents the three-dimensional velocity of the carrier in the world coordinate system at time k, They represent the velocities of the carrier pointing to the x, y, and z axes in the world coordinate system at time k respectively;

The installation errors between the inertial navigation system coordinate system, polarization light intensity sensor coordinate system, radar odometer coordinate system, optical flow sensor coordinate system and carrier coordinate system of the tightly integrated navigation system have been calibrated and compensated. Therefore, the conversion relationship between each sensor coordinate system and the carrier coordinate system is known. Therefore, there is no need to add additional error terms when constructing the measurement model of each sensor.

Through the tight combination of polarization and optical flow, the heading angle and speed of adjacent state nodes of the integrated navigation system are obtained respectively. This is an innovative improvement in the field of integrated navigation. The strong autonomy of polarization is used to greatly enhance the robustness of the navigation system.

In step 1, the measurement model of the pre-integration of the inertial navigation system is established as follows:

The pre-integrated measurement information of the inertial navigation system is selected as the measurement value, and the pre-integrated measurement model of the inertial navigation system is established as follows:

z _i,k+1 =h _i (x _k ,x _k+1 )+δv _i,k+1

in:

represents the pre-integrated measurement value of the inertial navigation system at time k+1, Pre-integrated measurement of position, velocity, and rotation matrix for the inertial navigation system;

h _i (x _k ,x _k+1 ) is the pre-integrated measurement function of the inertial navigation system, x _k ,x _k+1 are the state nodes of the tightly integrated navigation system at time k, k+1 respectively;

Respectively represent the pre-integrated measurement functions of the position, velocity, and rotation matrix of the carrier in the world coordinate system at time [k, k+1]; wherein R _k , R _k+1 represent the rotation matrix from the carrier coordinate system to the world coordinate system at time k, k+1 respectively, p _k+1 , v _k+1 represent the position and velocity of the carrier in the world coordinate system at time k+1 respectively, p _k , v _k represent the position and velocity of the carrier in the world coordinate system at time k respectively, g＝[0 0 g] ^T represents the gravitational acceleration in the world coordinate system, Δt _k ＝t _k+1 -t _k , δv _i,k+1 represents the pre-integrated measurement errors of the position, velocity, and rotation matrices of the inertial navigation system;

In step 1, the polarization light intensity sensor measurement model is established as follows:

The angle between the longitudinal axis of the carrier and the solar meridian measured by the polarization intensity sensor is selected as the measurement value data, which is called the polarization intensity sensor information as the measurement value. The polarization intensity sensor measurement model is established as follows:

z _p,k+1 =h _p (x _k ,x _k+1 )+v _p,k+1

in:

The angle between the longitudinal axis of the carrier and the solar meridian measured by the polarization light intensity sensor is taken as the measurement value data, which is called the polarization light intensity sensor measurement value; l _1.k , l _2.k and l _1.k+1 , l _2.k+1 are the light intensity ratios of the opposite channels measured by the polarization light intensity sensor at time k and k+1 respectively;

is the polarization angle of the carrier at time k, and the solar azimuth angle _αs is calculated by looking up the table. in is the solar altitude angle, is the solar declination, is the latitude of the observation point, is the solar hour angle, when When β＝1, When , β = -1; v _p,k+1 represents the Gaussian noise measured by the polarization light intensity sensor;

In step 1, the measurement model of the radar odometer is established as follows:

The corresponding relationship of feature point clouds at adjacent moments obtained by the radar odometer is selected as the measurement value, and the radar feature point cloud set at time [k, k+1] is obtained by the curvature-based feature extraction method. Feature point set It contains the edge feature point set E _k+1 , the plane feature point set H _k+1 , and the point cloud set for The projection at time k contains a set of edge feature points Plane feature point set Will Reference feature point cloud at time k For matching, the measurement model of the radar odometer is established as follows:

in:

Under the uniform motion model, we have

is the pose transformation matrix of the laser point cloud data at time [k, k+1] are the roll angle, pitch angle and yaw angle between the moments [k, k+1] in the radar coordinate system, are the relative displacements in the three axes between the moments [k, k+1] in the radar coordinate system, t _k , t _k+1 are the times on the time axis corresponding to the moments k, k+1, respectively, a _s is a moment between [k, k+1], s∈N ^* , is the time corresponding to the moment a _s on the time axis, is the pose transformation matrix between [k,a _s ];

is the feature point set The coordinates of the feature points in represents the feature point n in the set at time a _s , which is the known measurement point of the radar odometer. Point cloud collection The coordinates of the feature points in is the rotation matrix between [a _s ,k];

is the reference feature point cloud at time k is the known reference value at time k, express The edge feature points and plane feature points are The characteristic distance of the reference point in the

In step 1, the measurement model of the optical flow sensor is established as follows:

The speed information measured by the optical flow sensor is selected as the measurement value, and the measurement model of the optical flow sensor is established as follows:

z _o,k =h _o (x _k )+v _o,k

in:

represents the optical flow sensor measurement value at time k, The speed in the directions of x, y, and z axes measured by the optical flow sensor;

h _o (x _k ) is the measurement function of the optical flow sensor, x _k is the state node of the tightly integrated navigation system at time k, They represent the velocity of the carrier pointing to the x, y, and z axes in the world coordinate system at time k; v _o,k represents the Gaussian noise measured by the optical flow sensor;

The pre-integration form of the inertial navigation system is adopted to obtain the pre-integration measurement of the position, speed and rotation matrix of the adjacent state nodes of the tightly combined navigation system, and the data of the inertial navigation system are tightly combined and fused. The polarization light intensity sensor is adopted to obtain the angle measurement between the longitudinal axis of the carrier and the solar meridian of the adjacent state nodes of the tightly combined navigation system, and the polarization light intensity sensor measurement is innovatively added to the tightly combined navigation system in the form of tight combination; the corresponding relationship of the feature point cloud at adjacent moments obtained by the radar odometer is adopted to obtain the carrier attitude and position measurement of the adjacent state nodes of the tightly combined navigation system, and the radar point cloud data is innovatively added to the tightly combined navigation system by using the feature extraction method; the optical flow sensor is adopted to obtain the carrier speed measurement of the current state node of the tightly combined navigation system, and the optical flow sensor speed measurement is innovatively added to the polarization tightly combined navigation system; by constructing a new inertial/polarization/radar/optical flow tightly combined navigation method, the autonomy, robustness and accuracy of the combined navigation system are improved, and the navigation effect of polarization in an obstructed environment is enhanced.

In step 2, the compact integrated navigation system is initialized according to the rotation matrix from the carrier coordinate system to the world coordinate system The radar scanning point cloud in the carrier coordinate system is converted to the global coordinate system, and a global map with the starting position as the origin is established, thereby initializing the state equation of the new tightly integrated navigation system designed by the present invention.

In step 3, the steps of obtaining the pre-integration constraints of the adjacent state nodes of the integrated navigation system are as follows: integrating all the inertial navigation system measurement data between the current moment and the previous moment through the inertial navigation system motion model to obtain the pre-integration measurement of the inertial navigation system, and using the attitude, speed, and position data of the carrier obtained by the integration to establish the pre-integration constraints of the inertial navigation system of the adjacent state nodes of the carrier;

In step 3, the steps of the adjacent state node heading constraint of the integrated navigation system are as follows: finding the polarization light intensity sensor measurement data at the current moment and the previous moment, and establishing the carrier adjacent state node heading constraint through the polarization light intensity sensor measurement model and the polarization light intensity sensor measurement data;

In step 3, the steps of constraining the pose transformation of adjacent state nodes of the integrated navigation system are as follows: matching the point cloud in the radar odometer measurement data to the global map according to all radar odometer measurement data between the current moment and the previous moment; performing feature matching on the odometer measurement data at the current moment and the reference data at the previous moment, and establishing the pose transformation constraints of the adjacent state nodes of the carrier using the radar odometer measurement model and the radar odometer measurement data;

In step 3, the steps of constraining the speed of the node of the state of the combined navigation system at the current moment are as follows: obtaining the current time stamp and the optical flow sensor measurement data, and establishing the current time carrier state node speed constraint through the optical flow sensor measurement model and the optical flow sensor measurement data.

In step 4, the specific steps of obtaining the optimal estimated values of the attitude, position and velocity parameters of the carrier with the smallest error function in the world coordinate system are as follows:

Step (5a): Construct the state node error function of carrier (k-m+1)~k+1 time m is a positive integer:

Step (5b): According to the motion state constraints of adjacent state nodes, the error function of the pre-integration measurement of the inertial navigation system is established as:

Where e _i,k+1 is the pre-integrated measurement error of the inertial navigation system, ∑ _i,k+1 is the pre-integrated node covariance matrix of the inertial navigation system;

Step (5c): According to the heading constraints of the adjacent state nodes, the error function of the polarization intensity sensor measurement is established as:

Where e _p,k+1 is the measurement error of the polarization light intensity sensor, ∑ _p,k+1 is the node covariance matrix of the polarization light intensity sensor;

Step (5d): According to the matching constraints of the feature point cloud of adjacent state nodes, the optimization function of radar odometer measurement is established as:

Where e _l,k+1 is the characteristic distance of the radar odometer;

Step (5e): According to the state node velocity constraint, the error function of the optical flow sensor measurement is established as:

Where e _o,k+1 is the measurement error of the optical flow sensor, ∑ _o,k+1 is the covariance matrix of the optical flow sensor node;

Step (5f): Based on the error function of step (5e), a factor graph optimization function is established, which is expressed as follows:

Where X=[x _k-m+1 …x _k+1 ] ^T , is the optimal estimate of X;

Step (5g): Use the Gauss-Newton method to solve the factor graph optimization function to obtain the optimal estimated values of the body's attitude, position, and velocity in the world coordinate system with the minimum error function;

The optimal estimate of the body's attitude, position, and speed in the world coordinate system with the smallest error function is updated by updating the state node, and the corresponding updated radar odometer point cloud is matched to the global map, and then the global map is updated, and the repeated radar odometer point clouds in the global map are removed. The optimal estimate value updated at the current moment and the global map are used as the initial value and radar point cloud reference value at the next moment to continue factor graph optimization.

Beneficial effects of the present invention:

(1) The present invention is a navigation method based on the tight combination of inertial/polarization/radar/optical flow of factor graph. The multi-sensor combined navigation system aims to complete the navigation positioning and the drawing of the surrounding map through the system model, combined with the corresponding filtering method or optimization method. The present invention establishes the pre-integration model of the inertial navigation system and the polarization light intensity measurement model, the radar odometer measurement model, and the measurement model of the optical flow sensor, and realizes the determination of the current position of the carrier and the construction of the surrounding map through the tight combination factor graph optimization algorithm, and fuses the polarization azimuth information with the optical flow velocity information, which makes up for the shortcomings of the polarization sensor being easily affected by the atmospheric environment and can only output angle information, and the optical flow sensor data accuracy decreases under low illumination conditions. The characteristics of the fusion matching complementarity of polarization information, optical flow information, inertial navigation information, radar odometer information and multi-source information and not being easily affected by other external interference are effectively utilized. The factor graph optimization fully considers the characteristics of the historical information of measurement and state, and utilizes the historical information of the state through the sliding window method, while ensuring the accuracy, reducing the amount of calculation. It can effectively realize the plug-and-play of navigation sensors and solve the asynchronous problem of multi-sensor observation data.

(2) The polarized optical flow navigation mechanism used in the present invention is a very effective navigation method with the characteristics of strong robustness, high accuracy, good concealment, etc., and can provide a new solution for navigation tasks in complex environments. The introduction of polarization azimuth information and optical flow velocity information, and the use of graph optimization methods in multi-sensor combined navigation can improve the accuracy of its own position and the construction of surrounding maps, and enhance the system's anti-interference, accuracy and autonomy.

(3) The present invention is based on the fusion of inertial, polarization, radar and optical flow sensors, with complementary information, and has the characteristics of strong robustness, high precision and wide application range; the tight combination of sensors can achieve functions such as high navigation accuracy and strong anti-interference; the historical information characteristics of the map optimization measurement and status make it have good environmental adaptability and high positioning and composition accuracy.

BRIEF DESCRIPTION OF THE DRAWINGS

FIG1 is a flow chart of an inertial/polarization/radar/optical flow tightly integrated navigation method based on graph optimization of the present invention;

FIG. 2 is a structural diagram of a factor graph of the present invention.

DETAILED DESCRIPTION

The present invention will be described in detail below in conjunction with the accompanying drawings, which serve as a part of this specification and illustrate the principles of the present invention through implementation. Other aspects, features and advantages of the present invention will become apparent from the detailed description.

The invention discloses an inertial/polarization/radar/optical flow tightly combined navigation method based on graph optimization. The method comprises the following steps: selecting the posture, position and speed of a carrier in a world coordinate system as the state vector of an inertial/polarization/radar/optical flow tightly combined navigation system, establishing a state equation of an inertial navigation system pre-integration/polarization light intensity sensor/radar odometer/optical flow sensor tightly combined navigation system; establishing a pre-integration model based on an inertial navigation system based on basic acceleration and angular velocity information of the inertial navigation system, and constructing a pre-integration model node of the inertial navigation system; establishing a polarization measurement equation based on polarization light intensity information output by a polarization light intensity sensor, and constructing a polarization light intensity sensor node; establishing a radar odometer measurement equation based on point cloud feature information output by a radar odometer, and constructing a radar odometer node; establishing an optical flow measurement equation based on speed information output by an optical flow sensor, and constructing an optical flow sensor node, and using a factor graph optimization algorithm to realize posture determination and surrounding environment map construction.

As shown in FIG1 , the present invention is a graph-optimized inertial/polarization/radar/optical flow tightly integrated navigation method, and the specific implementation of the technical solution includes the following steps:

Step 1: Select the carrier's attitude, position, and velocity in the world coordinate system as the state of the inertial/polarization/radar/optical flow tightly integrated navigation system; establish an inertial navigation system pre-integration model, a polarization light intensity measurement model, a radar odometer measurement model, and an optical flow sensor measurement model respectively;

Step 2: Initialize the inertial/polarization/radar/optical flow integrated navigation system composed of the inertial navigation system, polarization light intensity sensor, radar odometer, and optical flow sensor, and obtain the initial integrated navigation system state, map, and radar point cloud reference values;

Step 3: Establish the pre-integration constraints of the adjacent nodes of the integrated navigation system at the current moment based on the pre-integration model of the inertial navigation system, establish the heading constraints of the adjacent nodes of the integrated navigation system based on the measurement model of the polarization light intensity sensor, establish the posture transformation constraints of the adjacent nodes based on the radar odometer measurement model, and establish the speed constraints of the state nodes based on the optical flow sensor measurement model; design the error function of the inertial/polarization/radar/optical flow tight integrated navigation system state as the state variable node and the sensor measurement as the factor node according to the pre-integration constraints of the adjacent state nodes of the carrier at the current moment, the heading constraints of the adjacent state nodes of the carrier, the posture transformation constraints of the adjacent state nodes of the carrier, and the speed constraints of the carrier state nodes, and obtain the factor graph to be optimized;

Step 4: Optimize the error function of the factor graph based on the nonlinear solver to obtain the optimal estimated values of the parameters such as the attitude, position and speed of the carrier with the minimum error function in the world coordinate system; update the optimal estimated values and map of the attitude, position and speed of the carrier in the world coordinate system with the minimum error function, and synchronously realize the positioning and composition of the carrier;

Step 5: Determine whether there is a new optimal estimate and map data input, if there is a new optimal estimate and map data input, execute step 6; if there is no new optimal estimate and map data input, execute step 7;

Step 6: Enter the new optimal estimate and map data and execute step 4;

Step 7: No new best estimate and map data input ends.

In a preferred embodiment, the present invention uses the state of the tightly integrated navigation system described in step 1 to construct the state node of the tightly integrated navigation system at time k, and the specific steps are as follows:

(1) Select the carrier attitude, position, and speed as the state node for building the tightly integrated navigation system state;

(2) The world coordinate system (world system) with the starting position as the origin, that is, the w system, with the geographic north direction as the positive direction of the x-axis of the world coordinate system, and the west direction as the positive direction of the y-axis of the world coordinate system. The positive direction of the z-axis of the world coordinate system is determined according to the right-hand rule;

(3) Establish a carrier coordinate system with the center of the carrier as the origin, i.e., the b system. The positive direction of the x-axis of the carrier coordinate system is parallel to the longitudinal axis of the carrier and points to the direction of the machine head. The positive direction of the y-axis of the carrier coordinate system is parallel to the horizontal axis of the carrier and points to the left. The positive direction of the z-axis of the carrier coordinate system is determined according to the right-hand rule.

(4) Select the attitude, position and speed in the world coordinate system as the state of the tightly integrated navigation system, and then construct the state of the tightly integrated navigation system at time k as follows:

in:

x _k is the state node of the tightly integrated navigation system at time k, Indicates the carrier posture, which is expressed in the form of Euler angles. They are roll angle, pitch angle and yaw angle respectively; represents the rotation matrix from the carrier coordinate system to the world coordinate system at time k, and SO(3) represents the 3D special orthogonal group

Represents the three-dimensional position information of the carrier in the world coordinate system at time k, They represent the coordinate values of the x, y, and z axes in the world coordinate system of the carrier at time k, respectively, and T represents the transpose of the vector;

represents the three-dimensional velocity of the carrier in the world coordinate system at time k, They represent the velocities of the carrier pointing to the x, y, and z axes in the world coordinate system at time k respectively;

The installation errors between the inertial navigation system coordinate system, polarization light intensity sensor coordinate system, radar odometer coordinate system, optical flow sensor coordinate system and the carrier coordinate system of the tightly integrated navigation system have been calibrated and compensated, so the coordinate systems of each sensor are assumed to coincide with the carrier coordinate system.

In a preferred embodiment, the present invention selects the pre-integrated measurement information of the inertial navigation system as the measurement value, and establishes the measurement model of the pre-integrated measurement of the inertial navigation system in step 1 as follows:

z _i,k+1 =h _i (x _k ,x _k+1 )+δv _i,k+1

in:

represents the pre-integrated measurement value of the inertial navigation system at time k+1, Pre-integrated measurement of position, velocity, and rotation matrix for the inertial navigation system;

h _i (x _k ,x _k+1 ) is the pre-integrated measurement function of the inertial navigation system, x _k ,x _k+1 are the state nodes of the tightly integrated navigation system at time k and k+1 respectively;

Respectively represent the pre-integrated measurement functions of the position, velocity and rotation matrix of the carrier in the world coordinate system at time [k, k+1]; wherein R _k , R _k+1 represent the rotation matrix from the carrier coordinate system to the world coordinate system at time k, k+1 respectively, p _k+1 , v _k+1 represent the position and velocity of the carrier in the world coordinate system at time k+1 respectively, p _k , v _k represent the position and velocity of the carrier in the world coordinate system at time k respectively, g＝[0 0 g] ^T represents the gravitational acceleration in the world coordinate system, Δt _k ＝t _k+1 -t _k ;

δvi _,k+1 represents the pre-integrated measurement error of the position, velocity, and rotation matrix of the inertial navigation system.

In a preferred embodiment, the polarized light intensity sensor measurement model described in step 1 is established as follows:

z _p,k+1 =h _p (x _k ,x _k+1 )+v _p,k+1

in:

The angle between the longitudinal axis of the carrier and the solar meridian measured by the polarization light intensity sensor is taken as the measurement value data, which is called the polarization light intensity sensor measurement value; l _1.k , l _2.k and l _1.k+1 , l _2.k+1 are the light intensity ratios of the opposite channels measured by the polarization light intensity sensor at time k and k+1 respectively;

is the polarization angle of the carrier at time k, and the solar azimuth angle _αs can be calculated by looking up the table. in is the solar altitude angle, is the solar declination, is the latitude of the observation point, is the solar hour angle, when When β＝1, When , β = -1; v _p,k+1 represents the Gaussian noise measured by the polarization camera.

In a preferred embodiment, the present invention selects the corresponding relationship of the feature point clouds at adjacent moments obtained by the radar odometer as the measurement value, and obtains the radar feature point cloud set at the moment [k, k+1] by a feature extraction method based on curvature. Feature point set Contains the edge feature point set E _k+1 and the plane feature point set H _k+1 . Point cloud set for The projection at time k contains a set of edge feature points Plane feature point set Will Reference feature point cloud at time k Matching is performed to establish the measurement model of the radar odometer described in step 1 as follows:

in:

Under the uniform motion model, we have

is the pose transformation matrix of the laser point cloud data at time [k, k+1] are the roll angle, pitch angle and yaw angle between the moments [k, k+1] in the radar coordinate system, are the relative displacements in the three axes between the moments [k, k+1] in the radar coordinate system, t _k , t _k+1 are the times on the time axis corresponding to the moments k, k+1, respectively, a _s is a moment between [k, k+1], s∈N ^* , is the time on the timeline corresponding to moment a _s . is the pose transformation matrix between [k,a _s ].

is the feature point set The coordinates of the feature points in represents the feature point n in the set at time a _s , which is the known measurement point of the radar odometer. Point cloud collection The coordinates of the feature points in is the rotation matrix between [a _s ,k].

is the reference feature point cloud at time k is the known reference value at time k, express The edge feature points and plane feature points are The characteristic distance of the reference point in .

In a preferred embodiment, the speed information measured by the optical flow sensor is selected as the measurement value, and the measurement model of the optical flow sensor in step 1 is established as follows:

z _o,k =h _o (x _k )+v _o,k

in:

represents the optical flow sensor measurement value at time k, The speed in the directions of x, y, and z axes measured by the optical flow sensor;

h _o (x _k ) is the measurement function of the optical flow sensor, x _k is the state node of the tightly integrated navigation system at time k, They represent the velocities of the carrier in the x, y, and z directions in the world coordinate system at time k respectively; v _o,k represents the Gaussian noise measured by the optical flow sensor.

In a preferred embodiment, the initialization of the tightly integrated navigation system in step 2 of the present invention is to convert the radar scanning point cloud in the carrier coordinate system to the global coordinate system according to the rotation matrix from the carrier coordinate system to the world coordinate system, and establish a global map with the starting position as the origin, thereby initializing the state equation of the new tightly integrated navigation system designed by the present invention.

In a preferred embodiment, the steps of obtaining the pre-integration constraints of the adjacent state nodes of the carrier at the current moment described in step 3 of the present invention are as follows: all inertial navigation measurement data between the current moment and the previous moment are integrated through the inertial navigation system motion model to obtain the pre-integration measurement of the inertial navigation system, and the attitude, speed, and position data of the carrier obtained by the integration are used to establish the pre-integration constraints of the inertial navigation system of the adjacent state nodes of the carrier.

In a preferred embodiment, the steps for obtaining the carrier state adjacent state node heading constraints in step 3 of the present invention are as follows: finding the polarization light intensity sensor measurement data at the current moment and the previous moment, and establishing the carrier adjacent state node heading constraints through the polarization light intensity sensor measurement model and the polarization light intensity sensor measurement data.

In a preferred embodiment, the steps of obtaining the pose transformation constraints of the adjacent state nodes of the carrier in step 3 are as follows: according to all the radar odometer measurement data between the current moment and the previous moment, the point cloud in the radar odometer measurement data is matched to the global map. The odometer measurement data at the current moment is feature matched with the reference data at the previous moment, and the pose transformation constraints of the adjacent state nodes of the carrier are established using the radar odometer measurement model and the radar odometer measurement data.

In a preferred embodiment, the steps of obtaining the node speed constraint of the carrier state at the current moment described in step 3 of the present invention are as follows: obtaining the current time stamp and the optical flow sensor measurement data, and establishing the node speed constraint of the carrier state at the current moment through the optical flow sensor measurement model and the optical flow sensor measurement data.

In a preferred embodiment, the specific steps of obtaining the optimal estimated values of the posture, position and velocity of the object in the world coordinate system with the minimum error function in step 4 are as follows:

Step (5a): Construct the state node error function of carrier (k-m+1)~k+1 time m is a positive integer:

Step (5b): According to the motion state constraints of adjacent state nodes, the error function of the pre-integration measurement of the inertial navigation system is established as:

Where e _i,k+1 is the pre-integrated measurement error of the inertial navigation system, ∑ _i,k+1 is the pre-integrated node covariance matrix of the inertial navigation system;

Step (5c): According to the heading constraints of the adjacent state nodes, the error function of the polarization intensity sensor measurement is established as:

Where e _p,k+1 is the measurement error of the polarization light intensity sensor, ∑ _p,k+1 is the node covariance matrix of the polarization light intensity sensor;

Step (5d): According to the matching constraints of the feature point cloud of adjacent state nodes, the optimization function of radar odometer measurement is established as:

Where e _l,k+1 is the characteristic distance of the radar odometer;

Step (5e): According to the state node velocity constraint, the error function of the optical flow sensor measurement is established as:

Where e _o,k+1 is the measurement error of the optical flow sensor, ∑ _o,k+1 is the covariance matrix of the optical flow sensor node;

Step (5f): Establish the factor graph optimization problem, which is expressed as follows:

Where X=[x _k-m+1 …x _k+1 ] ^T , is the optimal estimate of X;

Step (5g): Use the Gauss-Newton method to solve the factor graph optimization problem and obtain the optimal estimated values of the body's attitude, position, and velocity in the world coordinate system with the minimum error function;

Preferably, the optimal estimate of the body's attitude, position, and velocity in the world coordinate system where the error function is minimized is updated by updating the state node, and the corresponding updated radar odometer point cloud is matched to the global map, and then the global map is updated to remove duplicate radar odometer point clouds in the global map.

As shown in Figure 2, the factor graph structure of the factor graph of the present invention is further described using Figure 2: the optical flow in the figure represents the optical flow sensor node, inertia represents the inertial navigation system node, radar represents the radar odometer node, polarization represents the light intensity sensor node, and Xk represents the state node.

The inertial navigation system node at the 0th moment establishes the pre-integration constraint between the state nodes of the carrier x ₀ , x ₁ through the measurement data between the inertial navigation pre-integration time ₀ and ₁ and the inertial navigation system pre-integration measurement model, thereby establishing the connection relationship between the state node x ₀ , x ₁ and the inertial navigation system pre-integration node; the polarization light intensity sensor node at the 0th moment establishes the heading constraint between the state nodes of the carrier x ₀ , x ₁ through the measurement data between the polarization light intensity sensor time ₀ and ₁ and the polarization light intensity sensor measurement model, thereby establishing the connection relationship between the state node x ₀ , x ₁ and the polarization light intensity sensor node; the radar odometer node at the 0th moment establishes the posture transformation constraint between the state nodes of the carrier x ₀ , x ₁ through the measurement data between the radar odometer time ₀ and ₁ and the radar odometer measurement model, thereby establishing the connection relationship between the state node x ₀ , x ₁ and the radar odometer node; the state node at the 0th moment is x ₀ , x ₀ and the optical flow sensor node at the 0th moment establish the connection relationship between the state node x ₀ and the optical flow sensor node through the measurement data of the optical flow sensor at the 0th moment and the optical flow sensor measurement model, and establish the current carrier state node speed constraint;

At the first moment, the inertial navigation system node and x ₁ , x ₂ establish pre-integration constraints between the state nodes of the carriers x ₁ , x 2 through the measurement data between the inertial navigation pre-integration 1 and ₂ moments and the inertial navigation system pre-integration measurement model, thereby establishing a connection relationship between the state nodes x ₁ , x ₂ and the inertial navigation system pre-integration node; at the first moment, the polarization light intensity sensor node and x ₁ , x ₂ establish heading constraints between the state nodes of the carriers x ₁ , x ₂ through the measurement data between the polarization light intensity sensor 1 and 2 moments and the polarization light intensity sensor measurement model, thereby establishing a connection relationship between the state nodes x ₁ , x ₂ and the polarization light intensity sensor node; at the first moment, the radar odometer node and x ₁ , x ₂ establish posture transformation constraints between the state nodes of the carriers x ₁ , x ₂ through the measurement data between the radar odometer 1 and 2 moments and the radar odometer measurement model, thereby establishing a connection relationship between the state nodes x ₁ , x ₂ and the radar odometer node; the state node at the first moment is x ₁ , x ₁ and the optical flow sensor node at the first moment establish the connection relationship between the state node x ₁ and the optical flow sensor node through the measurement data of the optical flow sensor at the first moment and the optical flow sensor measurement model, and establish the current carrier state node speed constraint;

At the second moment, the inertial navigation system node and x ₂ , x ₃ establish pre-integration constraints between the state nodes of the carrier x ₂ , x 3 through the measurement data between the inertial navigation pre-integration time 2 and ₃ and the inertial navigation system pre-integration measurement model, thereby establishing a connection relationship between the state nodes x ₂ , x ₃ and the inertial navigation system pre-integration node; at the second moment, the polarization light intensity sensor node and x ₂ , x ₃ establish heading constraints between the state nodes of the carrier x ₂ , x ₃ through the measurement data between the polarization light intensity sensor time 2 and 3 and the polarization light intensity sensor measurement model, thereby establishing a connection relationship between the state nodes x ₂ , x ₃ and the polarization light intensity sensor node; at the second moment, the radar odometer node and x ₂ , x ₃ establish posture transformation constraints between the state nodes of the carrier x ₂ , x ₃ through the measurement data between the radar odometer time 2 and 3 and the radar odometer measurement model, thereby establishing a connection relationship between the state nodes x ₂ , x ₃ and the radar odometer node; the state node at the second moment is x ₂ , x ₂ and the optical flow sensor node at the second moment establish the connection relationship between the state node x ₂ and the optical flow sensor node through the measurement data of the optical flow sensor at the second moment and the optical flow sensor measurement model, and establish the current carrier state node speed constraint;

The inertial navigation system node at the kth moment and x _k , x _k+1 establish the pre-integration constraint between the state nodes of the carrier x _k , x k ₊₁ through the measurement data of the inertial navigation pre-integration between the k and k+1 moments and the pre-integration measurement model of the inertial navigation system, thereby establishing the connection relationship between the state nodes x _k , x _k+1 and the pre-integration node of the inertial navigation system; the polarization light intensity sensor node at the kth moment and x _k , x _k+1 establish the heading constraint between the state nodes of the carrier x _k , x k+1 through the measurement data of the polarization light intensity sensor between the k and k+1 moments and the measurement model of the polarization light intensity sensor, thereby establishing the connection relationship between the state nodes x _k , x _k+1 and the polarization light intensity sensor node; the radar odometer node at the kth moment and x _k , x _{k +1} establish the posture transformation constraint between the state nodes of the carrier x _k , x k+1 through the measurement data of the radar odometer between the k and k+1 moments and the radar odometer measurement model, thereby establishing the state nodes x k , x _{k +1} . _k+1 is connected to the radar odometer node; the state node at the kth moment is x _k , and x _k is connected to the optical flow sensor node at the kth moment by using the optical flow sensor's measurement data at the kth moment and the optical flow sensor measurement model to establish the current carrier state node speed constraint, thereby establishing a connection relationship between the state node x _k and the optical flow sensor node;

The above description of the disclosed embodiments enables one skilled in the art to implement or use the present invention. Various modifications to these embodiments will be apparent to one skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the present invention. Therefore, the present invention will not be limited to the embodiments shown herein, but rather to the widest scope consistent with the principles and novel features disclosed herein.

Claims (6)

1. An inertial/polarization/radar/optical-fluidic integrated navigation method based on factor graph, characterized in that it comprises the following steps:

step 1: the method comprises the steps of selecting the posture, the position and the speed of a carrier under a world coordinate system as the state of an inertial navigation system, a polarized light intensity sensor, a radar odometer and an optical flow sensor combined navigation system at the moment k, respectively establishing an inertial navigation system pre-integration model, a polarized light intensity measurement model and a radar odometer measurement model; selecting speed information measured by an optical flow sensor as a measured value to establish an optical flow sensor measuring model;

Step 2: initializing an inertial/polarized/radar/optical compact integrated navigation system composed of an inertial navigation system, a polarized light intensity sensor, a radar odometer and an optical flow sensor, and acquiring an initial state, a map and a radar point cloud reference value of the compact integrated navigation system;

step 3: establishing a current moment combined navigation system adjacent state node pre-integration constraint based on an inertial navigation system pre-integration model, establishing a current moment combined navigation system adjacent state node heading constraint based on a polarized light intensity sensor measurement model, establishing a radar point cloud reference value current moment combined navigation system adjacent state node position and attitude transformation constraint based on a radar mileage meter measurement model, establishing a current moment combined navigation system state node speed constraint based on an optical flow sensor measurement model, and designing an error function taking an inertial/polarization/radar/optical flow compact combined navigation system state as a state variable node and a sensor measurement as a factor node according to the current moment combined navigation system adjacent state node pre-integration constraint, the combined navigation system adjacent state node heading constraint, the combined navigation system adjacent state node position and the combined navigation system state node speed constraint to obtain a factor graph error function to be optimized;

Step 4: optimizing an error function of the factor graph based on a nonlinear solver to obtain optimal estimated values of the posture, the position and the speed parameters of a carrier with the minimum error function in a world coordinate system, updating the optimal estimated values and the map of the posture, the position and the speed of the carrier with the minimum error function in the world coordinate system, and synchronously realizing the positioning and the composition of the carrier;

in the step 1, the state nodes of the tightly-integrated navigation system at the moment k are constructed as follows:

wherein:

x _k is the status node of the tightly-packed navigation system at time k,representing the carrier pose, the carrier pose being expressed in terms of Euler angles,>respectively a roll angle, a pitch angle and a yaw angle;Rotation matrix representing carrier coordinate system to world coordinate system at k moment, SO (3) represents special orthogonal group of 3 dimension

Representing three-dimensional position information under the world coordinate system of the carrier at time k +.>Respectively representing coordinate values of three axial directions of x, y and z under a carrier world coordinate system at the moment k, wherein T represents the transposition of vectors;

representing three-dimensional velocity in the world coordinate system of the carrier at time k,/->Respectively representing speeds of three axial directions of x, y and z of the carrier world coordinate system at the moment k;

the installation errors existing between the inertial navigation system coordinate system, the polarized light intensity sensor coordinate system, the radar odometer coordinate system, the optical flow sensor coordinate system and the carrier coordinate system of the tightly combined navigation system are calibrated and compensated, so that the conversion relation between each sensor coordinate system and the carrier coordinate system is known, and additional error items are not needed to be added again when each sensor measurement model is constructed;

In the step 1, the establishment of the measurement model of the inertial navigation system pre-integration is realized as follows:

the measurement information of the inertial navigation system pre-integration is selected as a measured value, and a measurement model of the inertial navigation system pre-integration is established as follows:

z _i,k+1 ＝h _i (x _k ,x _k+1 )+δv _i,k+1

wherein:

representing a pre-integral measurement of the inertial navigation system at time k+1, which is +.>Pre-integral measurement of the inertial navigation system on the position, the speed and the rotation matrix;

h _i (x _k ,x _k+1 ) Pre-integration measurement function, x, for inertial navigation system _k ,x _k+1 The state nodes are the state nodes of the tightly-integrated navigation system in the time k and the time k+1 respectively;

respectively represent [ k, k+1 ]]A pre-integral measurement function of a position, a speed and a rotation matrix under a time carrier world coordinate system; wherein R is _k ,R _k+1 Rotation matrix from carrier coordinate system to world coordinate system at k, k+1 time, p _k+1 ,v _k+1 Respectively representing the position and the speed of the carrier under the world coordinate system at the moment k+1, p _k ,v _k Respectively representing the position and the speed of the carrier under the world coordinate system at the moment k, and g= [0 g ]] ^T Representing gravitational acceleration, Δt, in world coordinate system _k ＝t _k+1 -t _k ,δv _i,k+1 Representing the position, speed and rotation matrix pre-integral measurement error of the inertial navigation system;

in the step 1, the establishment of the measurement model of the polarized light intensity sensor is realized as follows:

the method comprises the steps of selecting an included angle between a longitudinal axis of a carrier measured by a polarized light intensity sensor and a solar meridian as measurement value data, namely information of the polarized light intensity sensor as a measurement value, and establishing a polarized light intensity sensor measurement model as follows:

z _p,k+1 ＝h _p (x _k ,x _k+1 )+v _p,k+1

Wherein:

the included angle between the longitudinal axis of the carrier and the solar meridian measured by the polarized light intensity sensor is used as measured value data, namely the measured value of the polarized light intensity sensor; l (L) _1,k ,l _2,k And l _1,k+1 ,l _2,k+1 The light intensity ratios of opposite channels measured by the polarized light intensity sensors at the moments k and k+1 are respectively;

for the carrier polarization angle at time k, sun azimuth angle alpha _s By table look-up calculation, the solar azimuth angleWherein->For the solar altitude, +.>For declination of the sun, & lt + & gt>Latitude of observation point->Is the sun hour angle, when->Beta=1 when +.>When, beta= -1; v _p,k+1 Representing gaussian noise measured by a polarized light intensity sensor.

2. The factor graph-based inertial/polarization/radar/optical-compact integrated navigation method of claim 1, wherein: in the step 1, the establishment of a measurement model of the radar odometer is realized as follows:

selecting the corresponding relation of feature point clouds of adjacent moments acquired by a radar odometer as a measured value, and acquiring [ k, k+1 ] through a curvature-based feature extraction method]Intra-moment radar feature point cloudFeature Point set->Comprises an edge characteristic point set E _k+1 Plane feature point set H _k+1 Point cloud set->Is->Projection at time k, comprising edge feature point set +.>Plane feature point set- >Will->Reference feature point cloud at time k>And (3) matching, and establishing a measuring model of the radar odometer as follows:

wherein:

under the uniform motion model, there are

For laser point cloud data in [ k, k+1 ]]Moment pose transformation matrix->Respectively [ k, k+1 ] under radar coordinate system]Roll angle, pitch angle and yaw angle between moments, < >>Respectively under radar coordinate system [k,k+1]Relative displacement in three axial directions between moments, t _k ,t _k+1 Respectively, the k and the k+1 moments correspond to the time on the time axis, a _s Is [ k, k+1 ]]At some point in between, s.epsilon.N ^* ，Is a as _s The time corresponds to the time on the time axis, +.>Is [ k, a ] _s ]A pose transformation matrix between the two;

is a feature point set->Coordinates of feature points in ∈ ->Representation a _s The characteristic point n in the time set is a measuring point known by the radar odometer, +.>Is a dot cloud set->Coordinates of feature points in ∈ ->Is [ a ] _s ,k]A rotation matrix therebetween;

reference feature point cloud for time k +.>Is a reference value known at the moment k,representation->Middle edge feature point and plane feature point and +.>Characteristic distance of the reference point.

3. The factor graph-based inertial/polarization/radar/optical-compact integrated navigation method of claim 1, wherein: in the step 1, the establishment of the measurement model of the optical flow sensor is realized as follows:

The speed information measured by the optical flow sensor is selected as a measured value, and a measurement model of the optical flow sensor is established as follows:

z _o,k ＝h _o (x _k )+v _o,k

wherein:

representing the optical flow sensor measurement at time k, which +.>The speed of the optical flow sensor in the directions of x, y and z axes is measured;

h _o (x _k ) For measuring function of optical flow sensor, x _k Is the status node of the tightly-packed navigation system at time k,respectively representing the speeds of the carrier pointing to the directions of the x, y and z axes in the world coordinate system at the moment k; v _o,k Gaussian noise representing optical flow sensor measurements;

acquiring the position, the speed and the rotation matrix pre-integration measurement of adjacent state nodes of the tightly combined navigation system by adopting a pre-integration form of the inertial navigation system, tightly combining and fusing the data of the inertial navigation system, acquiring the measurement of the included angle between the longitudinal axis of a carrier of the adjacent state nodes of the tightly combined navigation system and the solar meridian by adopting a polarized light intensity sensor, and adding the measurement of the polarized light intensity sensor into the tightly combined navigation system in a tightly combined form; acquiring carrier posture and position measurement of adjacent state nodes of the tightly-integrated navigation system by adopting a corresponding relation of feature point clouds of adjacent moments acquired by a radar odometer, and adding radar point cloud data into the tightly-integrated navigation system by using a feature extraction mode; and acquiring carrier speed measurement of the current state node of the tightly-integrated navigation system by adopting an optical flow sensor, and adding the optical flow sensor speed measurement into the polarized tightly-integrated navigation system.

4. The factor graph-based inertial/polarization/radar/optical-compact integrated navigation method of claim 1, wherein: in the step 2, the tightly-integrated navigation system is initialized according to a rotation matrix from a carrier coordinate system to a world coordinate systemThe radar scanning point cloud under the carrier coordinate system is converted into the global coordinate system, and a global map taking the initial position as the origin is established, so that the state equation of the novel tightly-combined navigation system designed by the invention is initialized.

5. The factor graph-based inertial/polarization/radar/optical-compact integrated navigation method of claim 1, wherein: in the step 3, the step of obtaining the pre-integration constraint of the adjacent state nodes of the integrated navigation system is as follows: integrating all inertial navigation system measurement data between the current moment and the previous moment through an inertial navigation system motion model to obtain pre-integral measurement of the inertial navigation system, and establishing pre-integral constraint of the inertial navigation system of the adjacent state node of the carrier by utilizing the attitude, speed and position data of the carrier obtained by integration;

in the step 3, the step of course constraint of adjacent state nodes of the integrated navigation system is as follows: searching polarized light intensity sensor measurement data at the current moment and the last moment, and establishing carrier adjacent state node heading constraint through a polarized light intensity sensor measurement model and the polarized light intensity sensor measurement data;

In the step 3, the step of transforming and restraining the node position of the adjacent state of the integrated navigation system is as follows: matching point clouds in the radar mileage measurement data with a global map according to all the radar mileage measurement data between the current moment and the previous moment; feature matching is carried out on the mileage measurement data at the current moment and the reference data at the previous moment, and the radar mileage measurement model and the radar mileage measurement data are utilized to establish node pose transformation constraint of the carrier adjacent state;

in the step 3, the step of restricting the speed of the state node of the integrated navigation system at the current moment is as follows: and acquiring a current time stamp and optical flow sensor measurement data, and establishing carrier state node speed constraint at the current time through an optical flow sensor measurement model and the optical flow sensor measurement data.

6. The factor graph-based inertial/polarization/radar/optical-compact integrated navigation method of claim 1, wherein: in the step 4, the specific steps of the optimal estimated values of the attitude, the position and the speed parameters of the carrier with the minimum error function in the world coordinate system are as follows:

step (5 a): constructing a state node error function with positive integers from moment m of carriers (k-m+1) to moment m of k+1:

Step (5 b): according to the motion state constraint of the adjacent state nodes, an error function of inertial navigation system pre-integration measurement is established as follows:

wherein e _i,k+1 Pre-integrating the measurement error, Σ, for the inertial navigation system _i,k+1 Pre-integrating the node covariance matrix for the inertial navigation system;

step (5 c): according to the heading constraint of the adjacent state nodes, an error function measured by a polarized light intensity sensor is established as follows:

wherein e _p,k+1 For measuring errors of polarized light intensity sensor, sigma _p,k+1 A node covariance matrix of the polarized light intensity sensor;

step (5 d): according to the adjacent state node characteristic point cloud matching constraint, an optimization function of radar odometer measurement is established as follows:

wherein e _l,k+1 The characteristic distance of the radar odometer;

step (5 e): according to the speed constraint of the state node, an error function measured by an optical flow sensor is established as follows:

wherein e _o,k+1 Measuring errors for optical flow sensors, Σ _o,k+1 Covariance matrix of nodes of the optical flow sensor;

step (5 f): establishing a factor graph optimization function according to the error function in the step (5 e), wherein the factor graph optimization function is expressed as follows:

wherein x= [ X ] _k-m+1 … x _k+1 ] ^T ，Is the optimal estimate of X;

step (5 g): solving a factor graph optimization function by using a Gaussian-Newton method to obtain an optimal estimated value of the carrier posture, position and speed under a world coordinate system with the minimum error function;

And updating the optimal estimated value of the carrier attitude, the position and the speed under the world coordinate system with the minimum error function, namely updating a state node, matching the corresponding updated point cloud of the radar odometer into a global map, updating the global map, removing repeated radar odometer point cloud in the global map, and continuously performing factor graph optimization by taking the updated optimal estimated value and the global map at the current moment as initial values and Lei Dadian cloud reference values at the next moment respectively.