CN-116295367-B - Unmanned vehicle multi-source integrated navigation method based on incremental factor graph

CN116295367BCN 116295367 BCN116295367 BCN 116295367BCN-116295367-B

Abstract

The invention discloses an unmanned vehicle multi-source integrated navigation method based on an incremental factor graph. The method comprises the steps of firstly obtaining various measurement information of the unmanned vehicle through other various vehicle-mounted high-precision sensors, determining the type of the sensor to be fused according to the environment where the unmanned vehicle is located, then constructing a multi-source navigation information fusion frame based on a factor graph, abstracting the high-precision sensor into eight factor nodes, fusing the eight factor nodes into the multi-sensor fusion navigation frame, under the open structure frame of the factor graph, characterizing the state and measurement updating process of the system, establishing a filtering equation, performing real-time filtering estimation and correction to complete effective fusion of the multi-source sensor information, and finally outputting navigation information to navigate the unmanned vehicle. The invention can realize the comprehensive processing of the data of multiple sensors and improve the navigation real-time performance, robustness and accuracy of the navigation system.

Inventors

CHENG YU
Zhou Niujun
CHEN SHUAI
Hou Zhikuan
NIU RENJIE
CHEN XI
Tao Yinhe
CHU FEIHUANG
LI TIANTIAN

Assignees

南京理工大学

Dates

Publication Date: 20260512
Application Date: 20220908

Claims (9)

1. The unmanned vehicle multi-source integrated navigation method based on the incremental factor graph is characterized by comprising the following steps of: Step 1, acquiring angular velocity information and specific force information of an unmanned vehicle through a gyroscope and an accelerometer in an inertial measurement unit, and acquiring various measurement information of the unmanned vehicle through other vehicle-mounted sensors; Step 2, judging the feasibility of the vehicle-mounted sensor according to the environment of the unmanned vehicle, and determining the type of the vehicle-mounted sensor to be fused; step 3, defining a navigation system state vector as a variable node of the factor graph, defining carrier measurement information acquired by an inertial measurement unit and other vehicle-mounted sensors as factor nodes of the factor graph, and constructing a multi-source navigation information fusion frame based on the factor graph, wherein the multi-source navigation information fusion frame comprises the following specific steps of: Step 3.1, constructing a factor graph as a bipartite graph model of the navigation estimation problem Comprises two types of nodes, one is a factor node Representing local functions in factorization, the other being variable nodes Representing variables in the global multi-function, edges Refers to if and only if a state variable node in a factor graph And factor node corresponding thereto In the case of the correlation, And A connecting edge exists between the two connecting edges; step 3.2, setting the product of several local functions as The parameters of each local function being contained in a subset In (3), namely: (1) Wherein the method comprises the steps of Is a set of discrete indicators that are selected, Is that Is a subset of the set of (c), Is a function of the parameters of ; Step 3.3, factorizing the formula (1) into a factor graph structure, and setting For 5 variables of the function, g is expressed as a form of 5 factor multiplications: (2) Then , , , , , ; Step 4, abstracting the vehicle-mounted sensor into eight factor nodes, and fusing the eight factor nodes into a multi-element sensor fusion navigation frame; Step 5, under the open structure framework of the factor graph, the state and measurement updating process of the system are characterized, a filtering equation is established, and the effective fusion of the information of the multi-source sensor is completed through real-time filtering estimation and correction, so that the multi-source navigation information fusion based on the factor graph is realized; And 6, outputting navigation information to navigate the unmanned vehicle.
2. The method for multi-source integrated navigation of an unmanned vehicle based on incremental factor graph according to claim 1, wherein the other vehicle-mounted sensors in step1 include eight types of sensors, such as MEMS inertial navigation IMU, magnetometer MAG, altimeter BAR, odometer ODO, binocular camera VIS, laser radar LIDAR, millimeter wave radar MMWR, and ultrasonic radar ULTRA.
3. The unmanned vehicle multi-source integrated navigation method based on the incremental factor graph according to claim 1, wherein the determining of the feasibility of the vehicle-mounted sensor according to the environment of the unmanned vehicle in step 2, determining the type of the vehicle-mounted sensor to be fused, specifically comprises the following steps: And judging the feasibility of MEMS inertial navigation IMU, magnetometer MAG, altimeter BAR, odometer ODO, binocular camera VIS, laser radar LIDAR, millimeter wave radar MMWR and ultrasonic radar ULTRA sensors according to the environment of the unmanned vehicle, determining the types of the sensors to be fused, and eliminating the types of the sensors which are invalid.
4. The unmanned vehicle multi-source integrated navigation method based on the incremental factor graph according to claim 1, wherein in step 4, the vehicle-mounted sensor is abstracted into eight factor nodes, and the eight factor nodes are fused into a multi-element sensor fusion navigation frame, specifically comprising the following steps: step 4.1, setting prior information: The prior information is divided into individual prior information of different sensors and different state variables Each a priori information is represented by a separate a priori factor for the variable Is a unary factor defined as: (3) wherein: Is that A cost function of a priori factors of (a); step 4.2, formulating an inertial navigation factor according to the MEMS inertial navigation IMU measurement model: The time update of the unmanned vehicle navigation state is abstractly described by the following equation: (4) wherein: And Representing the specific force and angular velocity measured by the accelerometer and gyroscope respectively, Is a measurement function; representing the measurement of MEMS inertial navigation IMU as And two navigational states of the connection And In relation, discretizing the formula (4) to obtain: (5) the factor node at this time is expressed as: (6) wherein: Representing a corresponding cost function; By measuring functions Predicting to obtain initial value Then adding the variable nodes into the factor graph to form new variable nodes; Step 4.3, formulating an odometer factor according to an odometer ODO measurement model: the odometer measurement model is derived by the following formula: (7) Wherein the method comprises the steps of Is the state of the odometer ODO at time t, Is the measurement noise of the odometer ODO, Is a measurement function of the odometer ODO, representing the relationship between the speed of the carrier and the measured value, and thus the factor of the odometer ODO is defined as: (8) the odometer factor is a unitary factor, and the state variables in the factor graph and at the corresponding moments Are connected; step 4.4, establishing an altimeter factor according to an altimeter BAR measurement model: The altimetric equation is expressed as: (9) Wherein, the Is that The time of day is a high-measurement value, In order to measure the function of the measurement, To measure noise, the altimeter factor corresponding to the measurement equation is: (10) the altimeter factor is also a unitary factor, and the state variables in the factor graph and at corresponding moments Are connected; and 4.5, formulating a magnetometer factor according to a magnetometer MAG measurement model: The magnetometer measurement equation is expressed as: (11) Wherein, the Is that The moment of time the magnetometer is measured, In order to measure the function of the measurement, To measure noise, the magnetometer factor corresponding to the measurement equation is: (12) Magnetometer factors are also unity factors, state variables in the factor graph and at corresponding moments Are connected; and 4.6, formulating a laser radar factor according to a measurement model after the laser radar LIDAR is converted into navigation information: Integrating the observed quantity of the laser radar LIDAR sensor at multiple levels according to the actual measurement equation and the prior landmark setting, the construction of a factor graph model incorporating the laser radar LIDAR explicitly describes the correlation between the data, the measurements derived from the laser radar LIDAR sensor, each measurement describing the changes in position and orientation at the ground plane between two time instances, these changes being at Pose and time of day The relative pose between the poses at the moment does not affect the altitude, roll and pitch, so the laser radar LIDAR is regarded as a binary factor, and hence the measurement model is defined as: (13) wherein: Is the first The position and the posture state at the moment, Is the first The position and the posture state at the moment, Is the difference between the upper and lower moments; The relative pose measurement function of the laser radar LIDAR is used for representing the relation between the poses of the carrier at two moments, so that the binary factor of the laser radar LIDAR is defined as: (14) Step 4.7, setting millimeter wave radar factors and ultrasonic radar factors according to a laser radar LIDAR setting method; Step 4.8, formulating binocular camera factors according to a measurement model after the binocular camera VIS is converted into navigation information: the binocular camera measurement equation is expressed as: (15) Wherein, the Is that The time-of-day depth camera measurement value, In order to measure the function of the measurement, To measure noise; the binocular camera factor corresponding to the measurement equation is: (16) The binocular camera factor is also a unity factor, in the factor graph and state variables at corresponding moments Are connected.
5. The unmanned vehicle multi-source integrated navigation method based on the incremental factor graph according to claim 1, wherein eight factor nodes are fused into a multi-sensor fusion navigation frame in the step 4, and an incremental smoothing algorithm is adopted, and the incremental smoothing algorithm is specifically as follows: When the initial factor graph framework is added with new factor nodes, determining an affected part and a corresponding affected state quantity, an unaffected part and a corresponding unaffected state quantity in the Bayesian tree according to the state quantity contained in the new factor nodes; nonlinear fusion is carried out on the affected state quantity and the state quantity contained in the new factor node, so as to obtain a new state quantity; and connecting the state quantities which are not affected by the newly added state quantity to obtain a new factor graph frame.
6. The unmanned vehicle multi-source integrated navigation method based on the incremental factor graph according to claim 1, wherein in the step 5, under the open structure framework of the factor graph, the state and measurement updating process of the characterization system are established, a filtering equation is established, and the effective fusion of the multi-source sensor information is completed through real-time filtering estimation and correction, so that the multi-source navigation information fusion based on the factor graph is realized, and the method is specifically as follows: The state equation of the 18-dimensional system of the navigation system in step 5.1 is: (17) wherein: As a state vector of the state vector, As a matrix of state coefficients, As a matrix of error coefficients, Is a white noise random error vector; System state vector The method comprises the following steps: (18) System state vector Comprises the basic navigation parameter error of a 9-dimensional inertial navigation system and the error state quantity of a 9-dimensional inertial instrument, wherein, , , As a function of the platform error angle, , , A speed error in the northeast direction, , , Is a function of latitude, longitude, altitude position error, , , Is a random constant of the gyroscope, , , For the first order markov process of gyroscopes, , , An accelerometer first order markov process; And 5.2, selecting constraint rules of a solving process of a multi-source information fusion algorithm based on a factor graph as follows: (19) wherein: as the number of nodes to be a factor, Is a natural number of the Chinese characters, As a function of a certain abstract function, To be defined at A joint distribution function thereon; Step 5.3, writing a measurement factor node In the form of (1) representing the difference between the factor node obtaining the predicted and actual measured information, constructing a corresponding index function to obtain an estimate of the state variable, wherein Is the cost function of the estimated quantity; For measuring functions, which are functions relating to state variables, in a navigation framework Predicting a sensor measurement value based on a given state estimate; Is that Actual measurement values obtained by various sensors at any moment; step 5.4, the MEMS inertial navigation IMU node in the factor node is different from other measuring nodes, and the measuring value is measured And (3) with Estimation of time of day Is used for prediction Value of time of day Measurement value The expression of (2) is: (20) Wherein, the 、 And respectively measuring the obtained specific force and angular velocity by an inertial sensor, and obtaining an expression of the IMU factor node: (21) where F is the transfer function matrix of the system, Is that A state vector of the time system; selecting a cost function L of the factor node, and minimizing the value of the cost function L to obtain: (22) Wherein, the Is a positive weighting matrix with proper value, Is to the state quantity Min is the minimum value; is the actual measurement value obtained by various sensors; to satisfy the expression (22): (23) Step 5.5, obtaining the current status therefrom Is estimated as: (24)。
7. the method for multi-source integrated navigation of an unmanned vehicle based on an incremental factor graph according to claim 1, wherein the outputting navigation information in step 6 is used for navigating the unmanned vehicle, and specifically comprises the following steps: And transmitting the position information fused by the multi-source sensor information to navigation software, and driving the unmanned vehicle to travel through the navigation software.
8. The unmanned vehicle multi-source integrated navigation method based on incremental factor graphs of claim 1, wherein the measured values of each sensor are encoded into a factor in the factor graph framework, and only the component framework graphs are added when the measured values are generated, and data fusion and parameter estimation are completed by using bayesian reasoning on the connected factors.
9. The unmanned vehicle multi-source integrated navigation method based on the incremental factor graph according to claim 1, wherein the MEMS inertial navigation IMU, magnetometer MAG, altimeter BAR, odometer ODO, binocular camera VIS, laser radar LIDAR, millimeter wave radar MMWR, ultrasonic radar ULTRA can be configured and combined in a plug and play manner, and if any sensor fails, the sensor is deleted from the factor graph frame.

Description

Unmanned vehicle multi-source integrated navigation method based on incremental factor graph Technical Field The invention relates to the technical field of unmanned vehicle navigation, in particular to an unmanned vehicle multi-source combined navigation method based on an incremental factor graph. Background The unmanned vehicle is also called an unmanned vehicle, a computer-driven vehicle or a wheeled mobile robot, and is an intelligent vehicle for realizing unmanned through a computer system. Decades of history have been in the 20 th century, and the 21 st century has shown a trend towards practical use. The unmanned vehicle relies on cooperation of artificial intelligence, visual computing, a radar, a monitoring device and a global positioning system, so that a computer can automatically and safely operate the motor vehicle without any active operation of human beings, and the positioning navigation technology of the unmanned vehicle is the core of the whole automatic driving technology. The existing unmanned vehicle positioning navigation technology generally adopts a multi-source combined navigation method, and the multi-source combined navigation method is a navigation method which realizes a higher-precision navigation effect by data fusion among multiple sensors and complementation of advantages and disadvantages. In a multi-sensor data fusion system, the frequencies of the sensors are different, the error characteristics are also different, and a part of measured values are always discarded by a traditional Kalman filtering algorithm in order to keep data synchronous, so that the information is wasted. Standard kalman filters can only solve the linearity problem, while most sensor models contain nonlinearities. In complex environments, such as urban roadways, canyons, tunnels, where satellite signals are unacceptable. If the algorithm is dependent on the guide, once the satellite is lost, the positioning accuracy becomes poor, and the normal operation of the unmanned vehicle is seriously affected. Disclosure of Invention The invention aims to provide an unmanned vehicle multi-source combined navigation method based on an incremental factor graph, which can effectively fuse a plurality of sensors under a satellite rejection condition and has high accuracy. The technical scheme for realizing the aim of the invention is that the unmanned vehicle multi-source integrated navigation method based on the incremental factor graph comprises the following steps: Step 1, acquiring angular velocity information and specific force information of an unmanned vehicle through a gyroscope and an accelerometer in an inertial measurement unit, and acquiring various measurement information of the unmanned vehicle through other vehicle-mounted sensors; Step 2, judging the feasibility of the vehicle-mounted sensor according to the environment of the unmanned vehicle, and determining the type of the vehicle-mounted sensor to be fused; Step 3, defining a navigation system state vector as a variable node of the factor graph, defining carrier measurement information acquired by an inertial measurement unit and other vehicle-mounted sensors as factor nodes of the factor graph, and constructing a multi-source navigation information fusion frame based on the factor graph; step 4, abstracting the vehicle-mounted sensor into eight factor nodes, and fusing the eight factor nodes into a multi-element sensor fusion navigation frame; Step 5, under the open structure framework of the factor graph, the state and measurement updating process of the system are characterized, a filtering equation is established, and the effective fusion of the information of the multi-source sensor is completed through real-time filtering estimation and correction, so that the multi-source navigation information fusion based on the factor graph is realized; And 6, outputting navigation information to navigate the unmanned vehicle. The invention has the remarkable advantages that (1) factors formed by MEMS inertial navigation, an odometer, an altimeter, a magnetometer, a laser radar, a millimeter wave radar, an ultrasonic radar and a binocular camera are inserted into a global factor graph, a Bayesian tree-based factor graph optimization algorithm is adopted to realize data fusion and optimal estimation on variable nodes, a navigation state is taken as the variable nodes, a sensor model is taken as the factor nodes to construct a factor graph model, an intelligent optimization algorithm is used for solving the nonlinear problem to obtain optimal state estimation at each moment, the method can be suitable for satellite rejection conditions, the capability of an unmanned vehicle for complex environments is improved, the navigation instantaneity, the robustness and the accuracy are improved, and (3) the method has high flexibility, can configure and combine the sensors in a plug-and-play mode, namely, if the sensors fail, the sensors can be timely deleted fr