CN114459474B - Inertial/polarization/radar/optical-fluidic combined navigation method based on factor graph - Google Patents
Inertial/polarization/radar/optical-fluidic combined navigation method based on factor graph Download PDFInfo
- Publication number
- CN114459474B CN114459474B CN202210143142.8A CN202210143142A CN114459474B CN 114459474 B CN114459474 B CN 114459474B CN 202210143142 A CN202210143142 A CN 202210143142A CN 114459474 B CN114459474 B CN 114459474B
- Authority
- CN
- China
- Prior art keywords
- navigation system
- carrier
- radar
- measurement
- coordinate system
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 41
- 230000010287 polarization Effects 0.000 title claims abstract description 41
- 238000005259 measurement Methods 0.000 claims abstract description 162
- 230000010354 integration Effects 0.000 claims abstract description 53
- 230000009466 transformation Effects 0.000 claims abstract description 21
- 230000003287 optical effect Effects 0.000 claims description 97
- 239000011159 matrix material Substances 0.000 claims description 35
- 238000005457 optimization Methods 0.000 claims description 22
- 238000000605 extraction Methods 0.000 claims description 5
- 230000001133 acceleration Effects 0.000 claims description 4
- 238000006073 displacement reaction Methods 0.000 claims description 4
- 239000013598 vector Substances 0.000 claims description 4
- 239000000969 carrier Substances 0.000 claims description 3
- 230000017105 transposition Effects 0.000 claims description 3
- 238000004364 calculation method Methods 0.000 claims description 2
- 238000006243 chemical reaction Methods 0.000 claims description 2
- 238000009434 installation Methods 0.000 claims description 2
- 230000000452 restraining effect Effects 0.000 claims description 2
- 230000001131 transforming effect Effects 0.000 claims description 2
- 230000003416 augmentation Effects 0.000 abstract 1
- 230000004927 fusion Effects 0.000 description 13
- 238000005516 engineering process Methods 0.000 description 5
- 230000000295 complement effect Effects 0.000 description 3
- 238000010276 construction Methods 0.000 description 3
- 238000007500 overflow downdraw method Methods 0.000 description 3
- 230000008901 benefit Effects 0.000 description 2
- 238000001514 detection method Methods 0.000 description 2
- 238000001914 filtration Methods 0.000 description 2
- 238000003384 imaging method Methods 0.000 description 2
- 230000008569 process Effects 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 238000000149 argon plasma sintering Methods 0.000 description 1
- 238000013473 artificial intelligence Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000005315 distribution function Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 230000010365 information processing Effects 0.000 description 1
- 230000007246 mechanism Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 239000011664 nicotinic acid Substances 0.000 description 1
- 238000000059 patterning Methods 0.000 description 1
- 230000005855 radiation Effects 0.000 description 1
- 230000001360 synchronised effect Effects 0.000 description 1
- 230000035899 viability Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
- G01C21/1652—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with ranging devices, e.g. LIDAR or RADAR
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
- G01C21/1656—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with passive imaging devices, e.g. cameras
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/183—Compensation of inertial measurements, e.g. for temperature effects
- G01C21/188—Compensation of inertial measurements, e.g. for temperature effects for accumulated errors, e.g. by coupling inertial systems with absolute positioning systems
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
- G01S13/86—Combinations of radar systems with non-radar systems, e.g. sonar, direction finder
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
- G01S13/88—Radar or analogous systems specially adapted for specific applications
- G01S13/93—Radar or analogous systems specially adapted for specific applications for anti-collision purposes
- G01S13/931—Radar or analogous systems specially adapted for specific applications for anti-collision purposes of land vehicles
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Computer Networks & Wireless Communication (AREA)
- Electromagnetism (AREA)
- Navigation (AREA)
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
Technical Field
The invention belongs to the category of autonomous navigation technology, and relates to a multi-sensor fusion navigation method based on the combination of inertia/polarization/radar/optical optics of factor graph.
Background
Multisensor Fusion (MSF) is an information processing process that utilizes computer technology to automatically analyze and integrate information and data from multiple sensors or sources with certain criteria to accomplish the required decisions and estimations. The multi-sensor data fusion technology is a research hot spot direction in recent years, and has wide application prospects in the fields of military, manned aerospace, automatic driving and the like.
Compared with a single sensor, the multi-sensor information fusion technology is applied to solve the problems of detection, tracking, target identification and the like, the system viability can be enhanced, the reliability and the robustness of the whole system are improved, the reliability of data is enhanced, the precision is improved, the time and the space coverage rate of the system are expanded, the instantaneity and the information utilization rate of the system are increased and the like.
Common methods of multisensor data fusion can be largely divided into two main categories: a random class and an artificial intelligence class. The current method of multi-sensor data fusion focuses on the random class. The Kalman filtering algorithm is a fusion method commonly used for a multi-sensor combination system, and has the defects that the real-time performance is poor when sensor information is redundant, fusion history information cannot be fully utilized, so that the fault probability is improved when subsystems are increased, and the robustness of the system is reduced. The multi-sensor data fusion method based on the factor graph is characterized in that a graph model in a certain time period of the system is constructed, the system state is related to navigation information, data fusion is realized based on a posterior estimation theory, and after all available measurement values are given, the maximum posterior probability estimation of a joint probability distribution function of all states is calculated, so that the plug-and-play of the navigation sensor can be effectively realized, the problem of multi-sensor observation data asynchronism is solved, and the calculated quantity is reduced while the accuracy is ensured. However, the navigation accuracy of the multi-sensor data fusion method based on the factor graph also depends on the accuracy of each sensor, and when the sensors are subjected to external interference (such as when the geomagnetic navigation system is subjected to strong magnetic interference), the accuracy of the multi-sensor fusion system is reduced, and the accumulated error is increased.
Researches show that polarized light has good autonomy, can be applied to a multi-sensor fusion system, a well-known physical expert in the United kingdom puts forward Rayleigh scattering law in 1871, and light scattering characteristics are revealed, so that a sky atmosphere polarization distribution mode is found, the atmosphere polarization mode is symmetrically and stably distributed about a solar meridian and a reverse solar meridian, important navigation information is contained, and the sky atmosphere polarization distribution mode is difficult to be subjected to artificial interference in a large range and errors are not accumulated along with time, so that the multi-sensor fusion system has the advantage of being hopeful to become an important auxiliary means in multi-sensor combination navigation.
The invention is different from the patent 'a bionic polarization synchronous positioning and composition method based on graph optimization' (hereinafter referred to as patent 1) in the following points:
distinction 1: in the patent 1, a loose combination factor graph method is used for estimating the optimal value of the combined navigation system state, and the sensor data is insufficiently utilized.
Distinction 2: compared with the patent 1, the optical flow sensor is additionally added to provide speed constraint, the factor graph is added with new constraint in the optimization process, the optimized optimal estimated value result is more accurate, and the whole estimated result is not destroyed even if the measurement error of a single sensor is larger.
Distinction 3: the method comprises the steps of 1, establishing a state node position constraint of a carrier based on a radar odometer measurement model, establishing a state node course constraint based on a polarized light camera measurement model, and establishing an adjacent state node pose transformation constraint by an inertial navigation system motion model; the method comprises the steps of establishing a tightly combined navigation system adjacent node pre-integration constraint based on an inertial navigation system pre-integration model, establishing a tightly combined navigation system adjacent node heading constraint based on a polarized light intensity sensor measurement model, establishing an adjacent node pose transformation constraint based on a radar odometer measurement model, and establishing a state node speed constraint based on an optical flow sensor measurement model; constructing different adjacent node state constraints results in higher positioning and patterning accuracy.
In practical navigation applications, navigation angle information can be output by extracting polarization information from the atmospheric polarization mode. And the polarization imaging technology can acquire additional polarization multidimensional information of target radiation or reflection, and has the capability of improving the target detection precision and the recognition probability. Optical flow is an image-based method that obtains the speed of movement of a carrier by acquiring the displacement of pixels in an image. The optical flow sensor and the polarization sensor complement each other in navigation, and the navigation positioning accuracy is improved.
Disclosure of Invention
The invention solves the technical problems that: the method comprises the steps of combining polarized light information with inertial navigation system information and radar odometer information, introducing optical flow information, realizing complementary advantages of the inertial navigation system information, the radar odometer information, polarized light angle information and optical flow speed information in navigation, and applying a graph optimization method to a multi-sensor combined navigation system to solve the problems of poor robustness, non-ideal navigation precision and difficulty in self-position determination in the multi-sensor combined navigation system.
The invention aims to solve the technical scheme that: a method of factor graph based inertial/polarization/radar/optofluidic integrated navigation comprising the steps of:
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: and 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 the carrier with the minimum error function under the 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 under 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;
The course angle and the speed of adjacent state nodes of the integrated navigation system are respectively obtained in the form of polarization and optical-optical combination, so that the integrated navigation system is an innovative improvement in the integrated navigation field, and the robustness of the navigation system is greatly enhanced by utilizing the strong autonomy of polarization.
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 representPosition and velocity of the carrier in world coordinate system at time 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 A Gaussian noise measured by a polarized light intensity sensor is represented;
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 [ k, k+1 ] under radar coordinate system]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 middle reference point;
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;
the method comprises the steps of adopting a pre-integration form of an inertial navigation system to obtain position, speed and rotation matrix pre-integration measurement of adjacent state nodes of the tightly combined navigation system, carrying out tightly combined fusion on data of the inertial navigation system, adopting a polarized light intensity sensor to obtain measurement of an included angle between a carrier longitudinal axis and a solar meridian of the adjacent state nodes of the tightly combined navigation system, and creatively adding the measurement of the polarized light intensity sensor into the tightly combined navigation system in a tightly combined form; acquiring carrier attitude 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 in an innovative feature extraction mode; acquiring carrier speed measurement of a current state node of the tightly-integrated navigation system by adopting an optical flow sensor, and creatively adding the optical flow sensor speed measurement into the polarized tightly-integrated navigation system; by constructing a novel inertial/polarization/radar/optical-flow combined navigation method, autonomy, robustness and accuracy of the combined navigation system are improved, and navigation effect of polarization in a shielding environment is enhanced.
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 systemConverting the radar scanning point cloud under the carrier coordinate system into a global coordinate system, and establishing a global map taking the initial position as the origin, thereby realizing the design of the inventionAnd initializing a new state equation of the tightly combined navigation system.
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.
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 a 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 For measuring errors of optical flow sensor, sigma 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.
The invention has the beneficial effects that:
(1) The invention relates to a navigation method based on the combination of inertia/polarization/radar/optical flow and tightly combining factor graph, which aims at solving the problems of how to determine the position of the navigation system and sense the external environment, a multi-sensor combined navigation system aims at completing the navigation positioning and the drawing of surrounding maps by combining a system model with a corresponding filtering method or optimizing method. The characteristics that polarization information, optical flow information, inertial navigation information, radar odometer information and multisource information are fused, matched and complemented and are not easy to be interfered by other outside are effectively utilized, the characteristics of measurement and historical information of states are fully considered in factor graph optimization, the historical information of the states is utilized in a sliding window mode, and the calculated amount is reduced while the accuracy is ensured. The plug and play of the navigation sensor can be effectively realized, and the problem of asynchronism of the observation data of multiple sensors is solved.
(2) The invention uses the polarized light flow navigation mechanism, is a very effective navigation means, has the characteristics of strong robustness, high precision, good concealment and the like, and can provide a new solution for navigation tasks in complex environments. The polarization azimuth information and the optical flow speed information are introduced, and the accuracy of the self-position and the surrounding map construction can be improved on the multi-sensor combination navigation by using the graph optimization method, so that the anti-interference performance, the accuracy and the autonomy of the system are improved.
(3) The invention is based on multi-sensor fusion of inertia, polarization, radar and optical flow, has the characteristics of strong robustness, high precision, wide application range and the like, and is complementary in information; the tight combination of the sensors can realize the functions of high navigation precision, strong anti-interference performance and the like; the characteristics of the historical information of the optimal measurement and state of the graph lead to good environmental adaptability and high positioning and composition precision.
Drawings
FIG. 1 is a flow chart of an inertial/polarization/radar/optical-fluidic integrated navigation method based on graph optimization in accordance with the present invention;
FIG. 2 is a block diagram of the factor graph of the present invention.
Detailed Description
The accompanying drawings, which are incorporated in and constitute a part of this specification, illustrate the principles of the invention and, together with the description, serve to explain the principles of the invention.
The invention relates to a map optimization-based inertial/polarization/radar/optical-flow combined navigation method, which is characterized in that the attitude, the position and the speed of a carrier under a world coordinate system are selected as the state vector of an inertial/polarization/radar/optical-flow combined navigation system, and a state equation of the inertial navigation system pre-integration/polarization light intensity sensor/radar odometer/optical-flow sensor combined navigation system is established; based on basic acceleration and angular velocity information of an inertial navigation system, establishing a pre-integration model based on the inertial navigation system, and constructing a node of the pre-integration model of the inertial navigation system; based on the polarized light intensity information output by the polarized light intensity sensor, a polarized measurement equation is established, and a polarized light intensity sensor node is constructed; based on the point cloud characteristic information output by the radar odometer, a radar odometer measurement equation is established, and a radar odometer node is constructed; based on the output speed information of the optical flow sensor, an optical flow measurement equation is established, an optical flow sensor node is constructed, and pose determination and surrounding environment map construction are realized by using a factor graph optimization algorithm.
As shown in fig. 1, the inertial/polarization/radar/optical-fluidic integrated navigation method based on graph optimization provided by the invention comprises the following steps:
Step 1: the carrier posture, position and speed under the world coordinate system are selected as the state of an inertial/polarization/radar/optical compact integrated navigation system; respectively establishing an inertial navigation system pre-integration model, a polarized light intensity measurement model, a radar mileage measurement model and an optical flow sensor measurement model;
step 2: initializing an inertial navigation system, a polarized light intensity sensor, a radar odometer, an optical flow sensor and a combined inertial/polarized/radar/optical-compact navigation system to obtain an initial combined navigation system state, a map and a radar point cloud reference value;
step 3: respectively establishing a current moment integrated navigation system adjacent node pre-integration constraint based on an inertial navigation system pre-integration model, establishing an integrated navigation system adjacent node heading constraint based on a polarized light intensity sensor measurement model, establishing an adjacent node pose transformation constraint based on a radar odometer measurement model, and establishing a state node speed constraint based on an optical flow sensor measurement model; designing an inertial/polarization/radar/optical-compact combined navigation system state as an error function of a state variable node and a sensor measurement as a factor node according to the carrier adjacent state node pre-integral constraint, the carrier adjacent state node heading constraint, the carrier adjacent state node pose transformation constraint and the carrier state node speed constraint at the current moment to obtain a factor graph to be optimized;
Step 4: optimizing an error function of the factor graph based on a nonlinear solver to obtain optimal estimated values of parameters such as the attitude, the position, the speed and the like of a carrier with the minimum error function under a world coordinate system; updating the optimal estimated values and the map of the carrier posture, the carrier position and the carrier speed under the world coordinate system with the minimum error function, and synchronously realizing the positioning and the composition of the carrier;
step 5: judging whether a new optimal estimated value and map data are input or not, and executing the step 6 if the new optimal estimated value and the map data are input; when no new optimal estimated value and map data are input, executing the step 7;
step 6: inputting the new optimal estimated value and map data to execute step 4;
step 7: no new optimal estimate and no map data input ends.
In a preferred embodiment, the method of the invention constructs the state node of the tightly-integrated navigation system at the k moment through the tightly-integrated navigation system state in the step 1 as follows:
(1) Selecting the posture, the position and the speed of the carrier as a state construction state node of the tightly combined navigation system;
(2) A world coordinate system (world system) taking a starting position as an origin, namely a w system, taking a geographic north direction as a positive direction of an x axis of the world coordinate system, taking a positive west direction as a positive direction of a y axis of the world coordinate system, and determining a z axis positive direction of the world coordinate system according to a right-hand rule;
(3) Establishing a carrier coordinate system taking the center of the carrier as an origin, namely a b system, taking the direction parallel to the longitudinal axis of the carrier and pointing to the machine head as the positive direction of the x axis of the carrier coordinate system, taking the direction parallel to the transverse axis of the carrier and pointing to the left as the positive direction of the y axis of the carrier coordinate system, and determining the positive direction of the z axis of the carrier coordinate system according to the right-hand rule;
(4) And selecting the posture, the position and the speed under the world coordinate system as the state of the tightly-integrated navigation system, and constructing the state of the tightly-integrated navigation system at the moment k 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 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 have been calibrated and compensated, so that the sensor coordinate systems are supposed to coincide with the carrier coordinate system.
In a preferred embodiment, the invention selects the inertial navigation system pre-integration measurement information as a measurement value, and establishes the inertial navigation system pre-integration measurement model in step 1 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 The position, the speed and the rotation matrix pre-integral measurement errors of the inertial navigation system are represented.
In a preferred embodiment, the measurement model of the polarized light intensity sensor in step 1 is established 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; />
Too much for the carrier to be polarized at kAzimuth angle alpha s The solar azimuth angle can be calculated by looking up a tableWherein->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 The gaussian noise measured by the polarized camera is shown.
In a preferred embodiment, the method selects the corresponding relation of the feature point clouds of adjacent moments acquired by the radar odometer as a measured value, and obtains [ k, k+1 ] through a curvature-based feature extraction method]Intra-moment radar feature point cloudFeature point setComprises 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>Matching is carried out, and a measurement model of the radar odometer in the step 1 is established 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 [ k, k+1 ] under radar coordinate system]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 of day 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.
In a preferred embodiment, the speed information measured by the optical flow sensor is selected as a measured 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
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 ) Measuring function for optical flow sensorNumber, x k Is the status node of the tightly-packed navigation system 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; v o,k Representing gaussian noise measured by the optical flow sensor.
In the preferred embodiment, the initialization of the tightly-integrated navigation system in the step 2 of the invention is to convert the radar scanning point cloud under the carrier coordinate system into the global coordinate system according to the rotation matrix from the carrier coordinate system to the world coordinate system, and establish the global map with the initial position as the origin, thereby realizing the initialization of the state equation of the novel tightly-integrated navigation system designed by the invention.
In a preferred embodiment, the step of obtaining the carrier neighboring state node pre-integration constraint in step 3 is as follows: integrating all inertial navigation measurement data between the current moment and the previous moment through an inertial navigation system motion model to obtain pre-integration measurement of the inertial navigation system, and establishing pre-integration constraint of the inertial navigation system of the adjacent state nodes of the carrier by utilizing the attitude, speed and position data of the carrier obtained by integration.
In a preferred embodiment, the step of obtaining the heading constraint of the carrier state adjacent state node in the step 3 is as follows: and 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 a preferred embodiment, the steps of obtaining the carrier adjacent state node pose transformation constraint in step 3 are as follows: and matching the point cloud in the radar mileage measurement data with the global map according to all the radar mileage measurement data between the current moment and the previous moment. And performing feature matching on the mileage measurement data at the current moment and the reference data at the previous moment, and establishing node pose transformation constraint of the carrier adjacent state by using the radar mileage measurement model and the radar mileage measurement data.
In a preferred embodiment, the step of obtaining the carrier state node speed constraint at the current time in step 3 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.
In a preferred embodiment, the specific steps for obtaining the optimal estimated values of the posture, the position and the speed of the carrier in the world coordinate system with the minimum error function in the step 4 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 a 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 For measuring errors of optical flow sensor, sigma o,k+1 Covariance matrix of nodes of the optical flow sensor;
step (5 f): the factor graph optimization problem is established 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 problem 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 a minimum error function;
preferably, updating the optimal estimated values of the carrier posture, the position and the speed under the world coordinate system with the minimum error function is updating of the state node, matching the corresponding updated point cloud of the radar odometer into the global map, updating the global map, and removing the repeated radar odometer point cloud in the global map.
The factor graph structure of the inertial/polarization/radar/optical-fluidic integrated navigation method based on factor graph of the present invention as shown in fig. 2 is further described below with reference to fig. 2: in the figure, the optical flow represents an optical flow sensor node, the inertia represents an inertial navigation system node, the radar represents a radar odometer node, the polarization represents an optical intensity sensor node, and Xk represents a state node.
Inertial navigation system node and x at time 0 0 ,x 1 By inertia ofMeasurement data between 0 and 1 time of navigation pre-integration and inertial navigation system pre-integration measurement model, and establishing carrier x 0 ,x 1 Pre-integration constraints between state nodes to establish state node x 0 ,x 1 The connection relation with the inertial navigation system pre-integration node; polarized light intensity sensor node and x at 0 th moment 0 ,x 1 The carrier x is established by measuring data between 0 and 1 time of the polarized light intensity sensor and a polarized light intensity sensor measuring model 0 ,x 1 Heading constraint between state nodes to establish state node x 0 ,x 1 The connection relation with the polarized light intensity sensor node; radar odometer node and x at time 0 0 ,x 1 The carrier x is established through the measurement data between the radar odometer 0 and the radar odometer 1 and the radar odometer measurement model 0 ,x 1 Pose transformation constraints between state nodes to establish state node x 0 ,x 1 The connection relation with the radar odometer node; the state node at time 0 is x 0 ，x 0 The optical flow sensor node at the 0 th moment establishes the current carrier state node speed constraint through the measurement data of the optical flow sensor at the 0 th moment and the optical flow sensor measurement model, thereby establishing a state node x 0 Connection relation with the optical flow sensor node;
inertial navigation system node and x at time 1 1 ,x 2 The carrier x is built through measurement data between 1 and 2 moments of inertial navigation pre-integration and an inertial navigation system pre-integration measurement model 1 ,x 2 Pre-integration constraints between state nodes to establish state node x 1 ,x 2 The connection relation with the inertial navigation system pre-integration node; polarized light intensity sensor node and x at 1 st moment 1 ,x 2 The carrier x is established by measuring data between the moments 1 and 2 of the polarized light intensity sensor and a polarized light intensity sensor measuring model 1 ,x 2 Heading constraint between state nodes to establish state node x 1 ,x 2 The connection relation with the polarized light intensity sensor node; radar odometer node and x at time 1 1 ,x 2 The carrier x is established by measuring data between 1 and 2 moments of the radar odometer and a radar odometer measuring model 1 ,x 2 Pose transformation constraints between state nodes to establish state node x 1 ,x 2 The connection relation with the radar odometer node; the state node at time 1 is x 1 ，x 1 The optical flow sensor node at the 1 st moment establishes the current carrier state node speed constraint through the measurement data of the optical flow sensor at the 1 st moment and the optical flow sensor measurement model, thereby establishing a state node x 1 Connection relation with the optical flow sensor node;
inertial navigation system node and x at time 2 2 ,x 3 The carrier x is established through measurement data between 2 and 3 moments of inertial navigation pre-integration and an inertial navigation system pre-integration measurement model 2 ,x 3 Pre-integration constraints between state nodes to establish state node x 2 ,x 3 The connection relation with the inertial navigation system pre-integration node; polarized light intensity sensor node and x at time 2 2 ,x 3 The carrier x is established by measuring data between the moments of the polarized light intensity sensor 2 and 3 and a polarized light intensity sensor measuring model 2 ,x 3 Heading constraint between state nodes to establish state node x 2 ,x 3 The connection relation with the polarized light intensity sensor node; radar odometer node and x at time 2 2 ,x 3 The carrier x is established by measuring data between the radar milestones 2 and 3 time and a radar milestone measuring model 2 ,x 3 Pose transformation constraints between state nodes to establish state node x 2 ,x 3 The connection relation with the radar odometer node; the state node at time 2 is x 2 ，x 2 The optical flow sensor node at the 2 nd moment establishes the current carrier state node speed constraint through the measurement data of the optical flow sensor at the 2 nd moment and the optical flow sensor measurement model, thereby establishing a state node x 2 Connection relation with the optical flow sensor node;
inertial navigation system node and x at kth moment k ,x k+1 The carrier x is established through measurement data between the moment k and the moment k+1 of inertial navigation pre-integration and an inertial navigation system pre-integration measurement model k ,x k+1 Pre-integration constraints between state nodes to establish state node x k ,x k+1 The connection relation with the inertial navigation system pre-integration node; polarized light intensity sensor node and x at kth moment k ,x k+1 The carrier x is established by measuring data between the k time and the k+1 time of the polarized light intensity sensor and a polarized light intensity sensor measuring model k ,x k+1 Heading constraint between state nodes to establish state node x k ,x k+1 The connection relation with the polarized light intensity sensor node; radar odometer node and x at the kth time k ,x k+1 The carrier x is established through the measurement data between the radar odometer k and the time k+1 and the radar odometer measurement model k ,x k+1 Pose transformation constraints between state nodes to establish state node x k ,x k+1 The connection relation with the radar odometer node; the state node at the kth time is x k ，x k The optical flow sensor node at the kth moment and the optical flow sensor node establish the current carrier state node speed constraint through the measurement data of the optical flow sensor at the kth moment and the optical flow sensor measurement model, thereby establishing a state node x k Connection relation with the optical flow sensor node;
the previous description of the disclosed embodiments is provided to enable any person skilled in the art to make or use the present invention. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the generic principles defined herein may be applied to other embodiments without departing from the spirit or scope of the invention. Thus, the present invention is not intended to be limited to the embodiments shown herein but is to be accorded 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.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210143142.8A CN114459474B (en) | 2022-02-16 | 2022-02-16 | Inertial/polarization/radar/optical-fluidic combined navigation method based on factor graph |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210143142.8A CN114459474B (en) | 2022-02-16 | 2022-02-16 | Inertial/polarization/radar/optical-fluidic combined navigation method based on factor graph |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114459474A CN114459474A (en) | 2022-05-10 |
| CN114459474B true CN114459474B (en) | 2023-11-24 |
Family
ID=81413436
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210143142.8A Active CN114459474B (en) | 2022-02-16 | 2022-02-16 | Inertial/polarization/radar/optical-fluidic combined navigation method based on factor graph |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114459474B (en) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115574816B (en) * | 2022-11-24 | 2023-03-14 | 东南大学 | Bionic vision multi-source information intelligent perception unmanned platform |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106197408A (en) * | 2016-06-23 | 2016-12-07 | 南京航空航天大学 | A kind of multi-source navigation data fusion method based on factor graph |
| CN111045454A (en) * | 2019-12-30 | 2020-04-21 | 北京航空航天大学 | Unmanned aerial vehicle self-driving instrument based on bionic autonomous navigation |
| CN111504312A (en) * | 2020-07-02 | 2020-08-07 | 中国人民解放军国防科技大学 | Unmanned aerial vehicle pose estimation method based on visual inertial polarized light fusion |
| CN111623771A (en) * | 2020-06-08 | 2020-09-04 | 青岛智融领航科技有限公司 | Polarization inertial navigation tight combination navigation method based on light intensity |
| CN112697138A (en) * | 2020-12-07 | 2021-04-23 | 北方工业大学 | Factor graph optimization-based bionic polarization synchronous positioning and composition method |
| CN113066105A (en) * | 2021-04-02 | 2021-07-02 | 北京理工大学 | Positioning and mapping method and system based on fusion of laser radar and inertial measurement unit |
| CN113781582A (en) * | 2021-09-18 | 2021-12-10 | 四川大学 | Synchronous positioning and map creating method based on laser radar and inertial navigation combined calibration |
-
2022
- 2022-02-16 CN CN202210143142.8A patent/CN114459474B/en active Active
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106197408A (en) * | 2016-06-23 | 2016-12-07 | 南京航空航天大学 | A kind of multi-source navigation data fusion method based on factor graph |
| CN111045454A (en) * | 2019-12-30 | 2020-04-21 | 北京航空航天大学 | Unmanned aerial vehicle self-driving instrument based on bionic autonomous navigation |
| CN111623771A (en) * | 2020-06-08 | 2020-09-04 | 青岛智融领航科技有限公司 | Polarization inertial navigation tight combination navigation method based on light intensity |
| CN111504312A (en) * | 2020-07-02 | 2020-08-07 | 中国人民解放军国防科技大学 | Unmanned aerial vehicle pose estimation method based on visual inertial polarized light fusion |
| CN112697138A (en) * | 2020-12-07 | 2021-04-23 | 北方工业大学 | Factor graph optimization-based bionic polarization synchronous positioning and composition method |
| CN113066105A (en) * | 2021-04-02 | 2021-07-02 | 北京理工大学 | Positioning and mapping method and system based on fusion of laser radar and inertial measurement unit |
| CN113781582A (en) * | 2021-09-18 | 2021-12-10 | 四川大学 | Synchronous positioning and map creating method based on laser radar and inertial navigation combined calibration |
Non-Patent Citations (5)
| Title |
|---|
| LiDAR/IMU紧耦合的实时定位方法;李帅鑫等;《自动化学报》;第47卷(第6期);第1377-1389页 * |
| 一种偏振光/双目视觉仿生组合导航方法;褚金奎等;《一种偏振光/双目视觉仿生组合导航方法》;第50卷(第5期);第052800-1-10 * |
| 一种基于偏振度加权的车载仿生偏振/惯性紧组合导航方法;张青云等;《2020中国自动化大会(CAC2020)论文集》;第924-929页 * |
| 仿生偏振光/GPS/地磁组合导航方法设计及实现;崔帅;《中国优秀硕士学位论文全文数据库 信息科技辑》;第I136-1023 * |
| 基于图优化的仿生偏振光组合导航*** SLAM 方法研究;曾云豪;《中国优秀硕士学位论文全文数据库 工程科技Ⅱ辑》(第01期);第C031-240页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114459474A (en) | 2022-05-10 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110243358B (en) | Multi-source fusion unmanned vehicle indoor and outdoor positioning method and system | |
| CN108731670B (en) | Inertial/visual odometer integrated navigation positioning method based on measurement model optimization | |
| CN112697138B (en) | Bionic polarization synchronous positioning and composition method based on factor graph optimization | |
| CN102435188B (en) | Monocular vision/inertia autonomous navigation method for indoor environment | |
| CN114526745B (en) | Drawing construction method and system for tightly coupled laser radar and inertial odometer | |
| CN108362288B (en) | Polarized light SLAM method based on unscented Kalman filtering | |
| CN108387236B (en) | Polarized light SLAM method based on extended Kalman filtering | |
| CN103674021A (en) | Integrated navigation system and method based on SINS (Strapdown Inertial Navigation System) and star sensor | |
| CN101344391A (en) | Lunar vehicle pose self-confirming method based on full-function sun-compass | |
| CN111426320B (en) | Vehicle autonomous navigation method based on image matching/inertial navigation/milemeter | |
| CN107144278B (en) | Lander visual navigation method based on multi-source characteristics | |
| Cai et al. | Mobile robot localization using gps, imu and visual odometry | |
| CN114719848B (en) | Unmanned aerial vehicle height estimation method based on vision and inertial navigation information fusion neural network | |
| CN115272596A (en) | Multi-sensor fusion SLAM method oriented to monotonous texture-free large scene | |
| CN112577493A (en) | Unmanned aerial vehicle autonomous positioning method and system based on remote sensing map assistance | |
| CN114608568B (en) | Multi-sensor information based instant fusion positioning method | |
| CN114459474B (en) | Inertial/polarization/radar/optical-fluidic combined navigation method based on factor graph | |
| CN109387198A (en) | A kind of inertia based on sequential detection/visual odometry Combinated navigation method | |
| CN114690229A (en) | GPS-fused mobile robot visual inertial navigation method | |
| CN115574816B (en) | Bionic vision multi-source information intelligent perception unmanned platform | |
| Xu et al. | A novel heading angle estimation methodology for land vehicles based on deep learning and enhanced digital map | |
| Hoshizaki et al. | Performance of Integrated Electro‐Optical Navigation Systems | |
| Wang et al. | Low-speed unmanned vehicle localization based on sensor fusion of low-cost stereo camera and IMU | |
| Luo et al. | An imu/visual odometry integrated navigation method based on measurement model optimization | |
| Zhou et al. | Localization for unmanned vehicle |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |