An incremental factor graph based multi-source integrated navigation method for unmanned vehicle

Abstract

The application discloses a kind of unmanned vehicle multi-source combined navigation methods based on incremental factor graph.The method is: first, obtain the various types of measurement information of unmanned vehicle by other various types of vehicle-mounted high-precision sensors, and determine the types of sensors that need to be fused according to the environment of unmanned vehicle;Then construct a multi-source navigation information fusion framework based on factor graph;Then abstract the high-precision sensor into eight factor nodes, and fuse the eight factor nodes into the multi-sensor fusion navigation framework;Under the open structure framework of factor graph, the state and measurement updating process of the system are represented, the filtering equation is established, and the effective fusion of multi-source sensor information is completed through real-time filtering estimation and correction;Finally, output the navigation information to navigate the unmanned vehicle.The application can realize the data comprehensive processing of multiple sensors, and improve the navigation real-time performance, robustness and accuracy of the navigation system.

Description

Technical Field

[0001] This invention relates to the field of unmanned vehicle navigation technology, and in particular to a multi-source integrated navigation method for unmanned vehicles based on incremental factor graphs. Background Technology

[0002] Driverless cars, also known as self-driving cars, computer-driven cars, or wheeled mobile robots, are intelligent vehicles that achieve driverless operation through computer systems. They have existed for decades in the 20th century and began to show a trend toward practical application in the early 21st century.

[0003] Autonomous vehicles rely on the collaborative efforts of artificial intelligence, computer vision, radar, monitoring devices, and global positioning systems to enable computers to operate motor vehicles automatically and safely without any human intervention. Therefore, the positioning and navigation technology of autonomous vehicles is the core of the entire autonomous driving technology.

[0004] Existing autonomous vehicle positioning and navigation technologies generally employ multi-source integrated navigation methods, which are navigation methods that achieve higher accuracy by fusing data from multiple sensors and complementing each other's strengths and weaknesses.

[0005] In multi-sensor data fusion systems, the frequencies and error characteristics of each sensor are different. In order to keep the data synchronized, traditional Kalman filtering algorithms often need to discard some measurement values, which will result in a waste of information.

[0006] Standard Kalman filters can only solve linear problems, while most sensor models incorporate nonlinearity. In complex environments, such as urban alleyways, canyons, and tunnels, satellite signals are unavailable. If the algorithm relies heavily on satellite navigation, positioning accuracy will suffer significantly in the event of satellite loss, severely impacting the normal operation of autonomous vehicles. Summary of the Invention

[0007] The purpose of this invention is to provide a multi-source integrated navigation method for unmanned vehicles based on incremental factor graphs that can effectively fuse multiple sensors under satellite rejection conditions and has high accuracy.

[0008] The technical solution to achieve the purpose of this invention is as follows: a multi-source integrated navigation method for unmanned vehicles based on incremental factor graphs, comprising the following steps:

[0009] Step 1: Obtain the angular velocity and specific force information of the unmanned vehicle through the gyroscope and accelerometer in the inertial measurement unit, and obtain various measurement information of the unmanned vehicle through other on-board sensors;

[0010] Step 2: Determine the feasibility of onboard sensors based on the environment in which the autonomous vehicle is located, and determine the types of onboard sensors that need to be fused.

[0011] Step 3: Define the navigation system state vector as the variable node of the factor graph, define the carrier measurement information obtained by the inertial measurement unit and other vehicle sensors as the factor node of the factor graph, and construct a multi-source navigation information fusion framework based on the factor graph.

[0012] Step 4: Abstract the vehicle sensors into eight factor nodes and integrate the eight factor nodes into the multi-sensor fusion navigation framework;

[0013] Step 5: Under the open structural framework of factor graph, characterize the system state and measurement update process, establish filtering equations, and complete the effective fusion of multi-source sensor information through real-time filtering estimation and correction, thereby realizing multi-source navigation information fusion based on factor graph.

[0014] Step 6: Output navigation information to navigate the unmanned vehicle.

[0015] The significant advantages of this invention compared to existing technologies are: (1) It inserts the factors consisting of MEMS inertial navigation, odometer, altimeter, magnetometer, lidar, millimeter-wave radar, ultrasonic radar and binocular camera into the global factor graph, and uses a Bayesian tree-based factor graph optimization algorithm to achieve data fusion and optimize the estimation of variable nodes. It constructs a factor graph model with navigation state as variable node and sensor model as factor node, and uses intelligent optimization algorithm to solve nonlinear problems to obtain the optimal state estimate at each moment. It can be applied under satellite denial conditions and improves the ability of unmanned vehicles to cope with complex environments; (2) It improves the real-time performance, robustness and accuracy of navigation; (3) It has high flexibility and can configure and combine sensors in a plug-and-play manner. That is, if a sensor fails, it can be removed from the factor graph framework in a timely manner. Attached Figure Description

[0016] Figure 1 This is a flowchart illustrating a multi-source integrated navigation method for unmanned vehicles based on incremental factor graphs according to the present invention.

[0017] Figure 2 This is a schematic diagram of the factor graph structure in an embodiment of the present invention.

[0018] Figure 3 This is a schematic diagram of the factor structure composed of various sensors in an embodiment of the present invention.

[0019] Figure 4 This is a trajectory diagram of the unmanned vehicle in an embodiment of the present invention.

[0020] Figure 5 This is a positioning accuracy diagram of the unmanned vehicle in an embodiment of the present invention. Detailed Implementation

[0021] The present invention will now be described in further detail with reference to the accompanying drawings and specific embodiments.

[0022] Combination Figure 1 This invention discloses a multi-source integrated navigation method for unmanned vehicles based on incremental factor graphs, comprising the following steps:

[0023] Step 1: Obtain the angular velocity and specific force information of the unmanned vehicle through the gyroscope and accelerometer in the inertial measurement unit, and obtain various measurement information of the unmanned vehicle through other on-board sensors;

[0024] Step 2: Determine the feasibility of onboard sensors based on the environment in which the autonomous vehicle is located, and determine the types of onboard sensors that need to be fused.

[0025] Step 3: Define the navigation system state vector as the variable node of the factor graph, define the carrier measurement information obtained by the inertial measurement unit and other vehicle sensors as the factor node of the factor graph, and construct a multi-source navigation information fusion framework based on the factor graph.

[0026] Step 4: Abstract the vehicle sensors into eight factor nodes and integrate the eight factor nodes into the multi-sensor fusion navigation framework;

[0027] Step 5: Under the open structural framework of factor graph, characterize the system state and measurement update process, establish filtering equations, and complete the effective fusion of multi-source sensor information through real-time filtering estimation and correction, thereby realizing multi-source navigation information fusion based on factor graph.

[0028] Step 6: Output navigation information to navigate the unmanned vehicle.

[0029] Furthermore, in step 1, the angular velocity and specific force information of the unmanned vehicle are obtained through the gyroscope and accelerometer in the inertial measurement unit, and various measurement information of the unmanned vehicle is obtained through other types of high-precision on-board sensors;

[0030] Furthermore, the other types of high-precision vehicle-mounted sensors include eight categories: MEMS inertial navigation IMU, magnetometer MAG, altimeter BAR, odometer ODO, binocular camera VIS, lidar LiDAR, millimeter-wave radar MMWR, and ultrasonic radar ULTRA.

[0031] Furthermore, in step 2, based on the environment in which the autonomous vehicle is located, the feasibility of the sensors is determined, and the types of sensors that need to be fused are identified.

[0032] Here, we take an unmanned vehicle driving in an alley as an example. At this time, the vehicle is in a satellite denial state, but other sensors are working well, and the remaining sensors can be fused.

[0033] Furthermore, in step 3, the navigation system state vector is defined as the variable node of the factor graph, and the carrier measurement information acquired by the inertial measurement unit and other types of vehicle-mounted high-precision sensors is defined as the factor node of the factor graph. A multi-source navigation information fusion framework based on the factor graph is constructed, as follows:

[0034] Step 3.1: Construct a factor graph as a bipartite graph model G = (F, X, E) for the navigation estimation problem, containing two types of nodes: one is factor nodes f i ∈F represents a local function in factorization; the other is the variable node x. j ∈X, representing a variable in a global multivariate function; marginal function e ij ∈E means if and only if the state variable node X in the factor graph j and the corresponding factor node f i When they are related, there is a connecting edge between them;

[0035] Step 3.2: Define the product of several local functions as g(x1,...,x...). n The parameters of each local function are contained in the subset {x1,...,x...}. n In, that is:

[0036]

[0037] Where J is the discrete index set, X j It is {x1,...,x} n A subset of}, f j (X j ) is a function with parameter X. j ;

[0038] Step 3.3: Factorize equation (1) into a factor graph structure, and set g(x1,x2,x3,x4,x5) as the five variables of the function. Express g as the product of five factors:

[0039] g(x1,x2,x3,x4,x5)=f A (x1)f B (x2)f C (x1,x2,x3)f D (x3,x4)f E (x3,x5) (2)

[0040] Then J = {A, B, C, D, E}, X A ={x1}, X B ={x2}, X C ={x1,x2,x3}, X D ={x3,x4}, X E={x3,x5}. The factor graph corresponding to equation (2) is as follows: Figure 2 As shown.

[0041] Furthermore, in step 4, the high-precision sensor is abstracted into eight factor nodes, and these eight factor nodes are fused into the multi-sensor fusion navigation framework, combined with... Figure 3 The details are as follows:

[0042] The IMU, MAG, BAR, ODO, VIS, LIDAR, MMWR, and ULTRA are abstracted into eight factor nodes, and the multi-sensor fusion navigation framework is represented by a factor graph, as follows: Figure 3 As shown in the diagram. Circles represent state variable nodes, black squares represent factor nodes, and X... k f represents the navigation status of the system, and f represents the measurement information from each sensor. prior This indicates previous measurement information, f IMU This represents measurement information from the IMU, and t k Time and t k+1 Related to the navigation status at any given time. MAG f BAR f ODO f VIS f LIDAR f MMWR and f ULTRA These are measurement information from MAG, BAR, ODO, VIS, LIDAR, MMWR, and ULTRA, respectively.

[0043] Step 4.1: Set prior information:

[0044] Prior information can be divided into individual prior information p(X0) for different sensors and different state variables. Each prior information can be represented by a separate prior factor. For some variables x∈X0, the prior factor is a unary factor, defined as:

[0045] f prior (x)=d(x) (3)

[0046] In the formula: d(x) is the cost function of the prior factors for x∈X0;

[0047] Step 4.2: Determine the inertial navigation factor based on the MEMS inertial navigation IMU measurement model:

[0048] The time update of the navigation state of an autonomous vehicle can be abstractly described by the following equation:

[0049]

[0050] In the formula: f b and ω bLet h(·) represent the specific force and angular velocity measured by the accelerometer and gyroscope, respectively, and h(·) be the measurement function.

[0051] The measurements of the MEMS inertial navigation IMU are expressed as It is connected to the two navigation states x k and x k+1 Relatedly, discretizing equation (4) yields:

[0052]

[0053] At this point, the factor node can be represented as:

[0054]

[0055] In the formula: d[·] represents the corresponding cost function;

[0056] Through measurement function An initial value x can be obtained through reasonable prediction. k+1 This is then added to the factor graph to form new variable nodes;

[0057] Step 4.3: Determine the odometer factor based on the odometer ODO measurement model:

[0058] The ODO (Operational Dot) measurement model can be derived from the following formula:

[0059]

[0060] in This is the state of the ODO (Operational Dot) at time t, N odo This is the measurement noise of the odometer ODO, H odo This is the measurement function of the odometer ODO, representing the relationship between the vehicle's speed and the measured value. Therefore, the factor of the odometer ODO can be defined as:

[0061]

[0062] The odometer factor is a univariate factor, and its state variable x in the factor graph and at the corresponding time point is... k Connected;

[0063] Step 4.4: Determine the altimeter factor based on the altimeter BAR measurement model:

[0064] The equation for measuring altitude can be expressed as:

[0065]

[0066] in, For t k Time of altitude measurement, h BAR Let n be the measurement function.BAR To measure noise, the altimeter factor corresponding to the above measurement equation is:

[0067]

[0068] The altimeter factor is also a univariate factor, and its relationship to the state variable x at the corresponding time point is shown in the factor diagram. k Connected;

[0069] Step 4.5: Determine the magnetometer factor based on the magnetometer's MAG measurement model:

[0070] The equation for magnetometer measurement can be expressed as:

[0071]

[0072] in, For t k Magnetic intensity measurement at time h MAG Let n be the measurement function. MAG To measure noise, the magnetometer factor corresponding to the above measurement equation is:

[0073]

[0074] The magnetometer factor is also a univariate factor, and its relationship to the state variable x at the corresponding time point is shown in the factor diagram. k Connected;

[0075] Step 4.6: Determine the LiDAR factor based on the measurement model after converting LiDAR into navigation information:

[0076] Based on the actual measurement equations and prior landmark settings, LiDAR sensor observations can be integrated at multiple levels. The construction of the factor graph model incorporating LiDAR explicitly illustrates the correlation between data. Each measurement derived from the LiDAR sensor describes the change in position and orientation on the ground plane between two time instances. It is realized as the relative attitude between the pose at time i-1 and the pose at time i, without affecting altitude, roll, and pitch. Therefore, LiDAR can be considered a binary factor, and the measurement model can be defined as follows:

[0077] z = H(x) i-1 ,x i )+n (13)

[0078] In the formula: x i-1 It is the pose state at time i-1, x iLet be the pose state at time i, and n be the difference between the two times. H(·) is the relative pose measurement function of the LiDAR, representing the relationship between the carrier's pose at two times. Therefore, the binary factor of the LiDAR can be defined as:

[0079] f LIDAR (x i-1 ,x i )=d(z i -H(x i-1 ,x i (14)

[0080] Step 4.7: Develop millimeter-wave radar factors and ultrasonic radar factors according to the LiDAR development method;

[0081] Step 4.8: Determine the binocular camera factors based on the measurement model after the VIS (Visual Identity System) of the binocular camera is converted into navigation information:

[0082] The measurement equation for a binocular camera can be expressed as:

[0083]

[0084] in, For t k Time-of-flight depth camera measurement, h VIS Let n be the measurement function. VIS For measuring noise;

[0085] The binocular camera factor corresponding to the above measurement equation is:

[0086]

[0087] The binocular camera factor is also a univariate factor, which is reflected in the factor diagram and the state variable x at the corresponding time. k Connected.

[0088] Furthermore, the process of fusing the eight factor nodes into the multi-sensor fusion navigation framework employs an incremental smoothing algorithm, which is detailed below:

[0089] When a new factor node is added to the initial factor graph framework, the affected part and its corresponding affected state, and the unaffected part and its corresponding unaffected state are determined in the Bayesian tree based on the state variables contained in the new factor node.

[0090] The affected state variables are nonlinearly fused with the state variables contained in the new factor nodes to obtain the new state variables.

[0091] Connect the newly added state variables with the unaffected state variables to obtain a new factor graph framework.

[0092] Furthermore, in step 5, within the open structural framework of the factor graph, the system state and measurement update process are characterized, a filtering equation is established, and after real-time filtering estimation and correction, the effective fusion of multi-source sensor information is completed, realizing multi-source navigation information fusion based on the factor graph, as detailed below:

[0093] Step 5.1, the state equation of the 18-dimensional navigation system is:

[0094]

[0095] In the formula: X(t) is the state vector, A(t) is the state coefficient matrix, G(t) is the error coefficient matrix, and W(t) is the white noise random error vector;

[0096] The system state vector X is:

[0097]

[0098] The system state vector X includes the basic navigation parameter errors of the 9-dimensional inertial navigation system and the error state quantities of the 9-dimensional inertial instruments, among which, For the platform error angle, δV n ,δV e ,δV u The velocity error in the northeast direction, δL, δR, δh represent the latitude, longitude, and altitude position errors, respectively, and ε bx , ε by , ε bz ε is the random constant of the gyroscope. rx , ε ry , ε rz This is a first-order Markov process for the gyroscope. First-order Markov process of accelerometer;

[0099] Step 5.2: Select the following constraint rules for the solution process of the multi-source information fusion algorithm based on factor graphs:

[0100]

[0101] In the formula: n is the number of factor nodes, N is a natural number, and f n (·) is an abstract function, and P(X) is the joint distribution function defined on X;

[0102] Step 5.3: A measurement factor node can be written as f(X) k )=L(Z kThe form L(·) represents the difference between the predicted and actual measurement information obtained by the factor node, which is used to construct the corresponding index function to obtain the estimate of the state variable. Here, L(·) is the cost function of the estimated quantity; H is the measurement function, which is related to the state variable. In the navigation framework, H can predict the sensor measurement value based on the given state estimate; Z k These are actual measurement values ​​obtained from various sensors;

[0103] Step 5.4: The MEMS inertial navigation IMU node in the factor node is different from other measurement nodes; its measurement value Z IMU The valuation X at time k k Used to predict the value X at time k+1 k+1 Measurement value Z IMU The expression is:

[0104] Z IMU ={f b ,ω b} (20)

[0105] Among them, f b ω b Given the specific force and angular velocity measured by the inertial sensor, respectively, the expression for the IMU factor node can be obtained:

[0106] f IMU =L(X) k+1 -F(X k Z IMU )) (twenty one)

[0107] Where F is the transfer function matrix of the system, X k+1 Let k+1 be the state vector of the system.

[0108] Choosing the cost function L of the factor node and minimizing its value yields:

[0109]

[0110] Where W is a positive definite weighting matrix with appropriate values. This is an estimate of the state variable X, where min is the minimum value; for equation (22) to hold, the following must be satisfied:

[0111]

[0112] Step 5.5: The estimated current state X is obtained as follows:

[0113]

[0114] Furthermore, in step 6, navigation information is output to navigate the autonomous vehicle, as detailed below:

[0115] The location information obtained by fusing information from multiple sensors is sent to the navigation software, which then drives the autonomous vehicle.

[0116] Furthermore, within the factor graph framework, the measurement value of each sensor is encoded as a factor. The data fusion and parameter estimation are accomplished simply by adding the component framework graph when the measurement value is generated and using Bayesian inference on these connected factors.

[0117] Furthermore, the MEMS inertial navigation IMU, magnetometer MAG, altimeter BAR, odometer ODO, binocular camera VIS, lidar LIDAR, millimeter-wave radar MMWR, and ultrasonic radar ULTRA can be configured and combined in a plug-and-play manner, and if a sensor fails, it can be promptly removed from the factor graph framework.

[0118] This embodiment employs the multi-source integrated navigation method for unmanned vehicles based on incremental factor graphs of the present invention to navigate the unmanned vehicle, setting the unmanned vehicle navigation trajectory according to... Figure 4 As it moves, the positioning accuracy obtained is as follows: Figure 5 As shown.

[0119] Depend on Figure 5 As can be seen, the multi-source integrated navigation method for unmanned vehicles based on incremental factor graphs of the present invention can conveniently process data from asynchronous heterogeneous sensors. After receiving the output data of the sensors, the factor graph nodes are expanded, and the system state is updated quickly and effectively according to the system's state equation and measurement equation. This realizes the comprehensive processing of multi-sensor data, effectively improving the accuracy, reliability and rapid configuration capability of the navigation system, and enhancing navigation performance.

Claims

1. A multi-source integrated navigation method for unmanned vehicles based on incremental factor graphs, characterized in that, The steps are as follows: Step 1: Obtain the angular velocity and specific force information of the unmanned vehicle through the gyroscope and accelerometer in the inertial measurement unit, and obtain various measurement information of the unmanned vehicle through other on-board sensors; Step 2: Determine the feasibility of onboard sensors based on the environment in which the autonomous vehicle is located, and determine the types of onboard sensors that need to be fused. Step 3: Define the navigation system state vector as the variable node of the factor graph, define the carrier measurement information acquired by the inertial measurement unit and other vehicle sensors as the factor node of the factor graph, and construct a multi-source navigation information fusion framework based on the factor graph, as follows: Step 3.1: Construct a factor graph as a bipartite graph model for the navigation estimation problem. It contains two types of nodes: one is factor nodes. One represents a local function in factorization; the other is a variable node. , representing variables in a global multivariate function; marginal This means if and only if the state variable nodes in the factor graph and the corresponding factor nodes When relevant, and There is a connecting edge between them; Step 3.2: Set the product of several local functions as... The parameters of each local function are contained in a subset. In, that is: （1） in It is a discrete index set. yes a subset of It is a function with parameters. ; Step 3.3: Factorize equation (1) into a factor graph structure, and set... Given five variables of the function, express g as a product of five factors: （2） but , , , , , ; Step 4: Abstract the vehicle sensors into eight factor nodes and integrate the eight factor nodes into the multi-sensor fusion navigation framework; Step 5: Under the open structural framework of factor graph, characterize the system state and measurement update process, establish filtering equations, and complete the effective fusion of multi-source sensor information through real-time filtering estimation and correction, thereby realizing multi-source navigation information fusion based on factor graph. Step 6: Output navigation information to navigate the unmanned vehicle.

2. The multi-source integrated navigation method for unmanned vehicles based on incremental factor graphs according to claim 1, characterized in that, The other vehicle-mounted sensors mentioned in step 1 include eight types of sensors: MEMS inertial navigation IMU, magnetometer MAG, altimeter BAR, odometer ODO, binocular camera VIS, lidar LiDAR, millimeter-wave radar MMWR, and ultrasonic radar ULTRA.

3. The multi-source integrated navigation method for unmanned vehicles based on incremental factor graphs according to claim 1, characterized in that, Step 2, which involves determining the feasibility of onboard sensors based on the environment in which the autonomous vehicle is located, and identifying the types of onboard sensors that need to be fused, is detailed below: Based on the environment in which the autonomous vehicle is located, the feasibility of using MEMS inertial navigation IMU, magnetometer MAG, altimeter BAR, odometry ODO, binocular camera VIS, lidar LIDAR, millimeter-wave radar MMWR, and ultrasonic radar ULTRA sensors is determined, the types of sensors that need to be fused are identified, and invalid sensor types are eliminated.

4. The multi-source integrated navigation method for unmanned vehicles based on incremental factor graphs according to claim 1, characterized in that... Step 4 involves abstracting the vehicle-mounted sensors into eight factor nodes and fusing these eight factor nodes into the multi-sensor fusion navigation framework, as detailed below: Step 4.1: Set prior information: Prior information is divided into individual prior information from different sensors and different state variables. Each piece of prior information is represented by a separate prior factor for the variable. The prior factor is a unary factor, defined as: （3） In the formula: for The cost function of the prior factors; Step 4.2: Determine the inertial navigation factor based on the MEMS inertial navigation IMU measurement model: The time update of the autonomous vehicle's navigation state is abstractly described by the following equation: （4） In the formula: and These represent the specific force and angular velocity measured by the accelerometer and gyroscope, respectively. For measurement functions; The measurement of the MEMS inertial navigation IMU is expressed as , and the two navigation states connected and Relatedly, discretizing equation (4) yields: （5） At this point, the factor node is represented as: （6） In the formula: Represent the corresponding cost function; Through measurement function Prediction is performed to obtain initial values This is then added to the factor graph to form new variable nodes; Step 4.3: Determine the odometer factor based on the odometer ODO measurement model: The ODO (Operational Dot) measurement model is derived from the following formula: （7） in This is the state of the ODO (Operational Dome) at time t. It's the measurement noise from the odometer's ODO (Operational Dot). This is the measurement function of the odometer ODO, representing the relationship between the vehicle's speed and the measured value. Therefore, the factor of the odometer ODO is defined as: （8） The odometer factor is a univariate factor, and its state variables at the corresponding time points are represented in the factor graph. Connected; Step 4.4: Determine the altimeter factor based on the altimeter BAR measurement model: The equation for measuring altitude is expressed as: （9） in, for Time-based altitude measurement value For measurement functions, To measure noise, the altimeter factor corresponding to the above measurement equation is: （10） The altimeter factor is also a univariate factor, and its state variables at the corresponding time point are represented in the factor graph. Connected; Step 4.5: Determine the magnetometer factor based on the magnetometer's MAG measurement model: The equation for magnetometer measurement is expressed as: （11） in, for Magnetic intensity measurement value at any time For measurement functions, To measure noise, the magnetometer factor corresponding to the above measurement equation is: （12） The magnetometer factor is also a univariate factor, and its state variables at the corresponding time points are shown in the factor diagram. Connected; Step 4.6: Determine the LiDAR factor based on the measurement model after converting LiDAR into navigation information: Based on the actual measurement equations and prior landmark settings, LiDAR sensor observations are integrated at multiple levels. The construction of the LiDAR factor graph model explicitly illustrates the correlation between data. Measurements derived from the LiDAR sensors, each describing the change in position and orientation on the ground plane between two time instances, contribute to... pose at time and The relative attitude between poses at any given time does not affect altitude, roll, and pitch, so LiDAR is considered a binary factor, and therefore the measurement model is defined as: （13） In the formula: It is the first The pose state at any given moment. It is the first The pose state at any given moment. This represents the difference between two consecutive moments. This is the relative pose measurement function of the LiDAR, representing the relationship between the vehicle's attitude at two different moments. Therefore, the binary factor of the LiDAR is defined as: （14） Step 4.7: Develop millimeter-wave radar factors and ultrasonic radar factors according to the LiDAR development method; Step 4.8: Determine the binocular camera factors based on the measurement model after the VIS information from the binocular camera is converted into navigation information. The measurement equation for a binocular camera is expressed as follows: （15） in, for Time-of-flight depth camera measurements For measurement functions, For measuring noise; The binocular camera factor corresponding to the above measurement equation is: （16） The binocular camera factor is also a univariate factor, which is reflected in the factor graph and the state variable at the corresponding time. Connected.

5. The multi-source integrated navigation method for unmanned vehicles based on incremental factor graphs according to claim 1, characterized in that, Step 4 involves fusing the eight factor nodes into the multi-sensor fusion navigation framework using an incremental smoothing algorithm, which is detailed below: When a new factor node is added to the initial factor graph framework, the affected part and its corresponding affected state, and the unaffected part and its corresponding unaffected state are determined in the Bayesian tree based on the state variables contained in the new factor node. The affected state variables are nonlinearly fused with the state variables contained in the new factor nodes to obtain the new state variables. Connect the newly added state variables with the unaffected state variables to obtain a new factor graph framework.

6. The multi-source integrated navigation method for unmanned vehicles based on incremental factor graphs according to claim 1, characterized in that, Step 5 describes the process of characterizing the system's state and measurement update within the open structural framework of factor graphs, establishing filtering equations, and performing real-time filtering estimation and correction to achieve effective fusion of multi-source sensor information, thus realizing multi-source navigation information fusion based on factor graphs. The details are as follows: Step 5.1, the state equation of the 18-dimensional navigation system is: （17） In the formula: For state vectors, The state coefficient matrix, The error coefficient matrix is... The random error vector is white noise. System state vector for: （18） System state vector It includes the basic navigation parameter errors of a 9-dimensional inertial navigation system and the error state quantities of 9-dimensional inertial instruments, among which, , , For the platform error angle, , , Speed ​​error in the northeast direction. , , For latitude, longitude, and altitude position errors, , , For gyroscope random constants, , , This is a first-order Markov process for the gyroscope. , , First-order Markov process of accelerometer; Step 5.2: Select the following constraint rules for the solution process of the multi-source information fusion algorithm based on factor graphs: （19） In the formula: The number of factor nodes, For natural numbers, For some abstract function, For definition in Joint distribution function on; Step 5.3: Write a measurement factor node as follows The form represents the difference between the predicted and actual measurement information obtained by the factor nodes, and constructs a corresponding index function to obtain the estimate of the state variable, where... It is the cost function of the estimated quantity; The measurement function is a function related to the state variables, and it exists within the navigation framework. Predict sensor measurements based on a given state estimate; yes The actual measurement values ​​obtained from various sensors at all times; Step 5.4: The MEMS inertial navigation IMU node in the factor node differs from other measurement nodes; the measured values... and Valuation at any moment Used to predict value of time Measurement value The expression is: （20） in, , Given the specific force and angular velocity measured by the inertial sensor, respectively, the expression for the IMU factor node is obtained: （21） Where F is the system's transfer function matrix, for The state vector of the system at any given time; Choosing the cost function L of the factor node and minimizing its value yields: （22） in, It is a positive definite weighted matrix with appropriate values. For state variables The estimate is min, which is the minimum value. These are actual measurement values ​​obtained from various sensors; For equation (22) to hold, the following must be satisfied: （23） Step 5.5: Obtain the current state. The estimate is: （24）。 7. The multi-source integrated navigation method for unmanned vehicles based on incremental factor graphs according to claim 1, characterized in that, Step 6 involves outputting navigation information to navigate the unmanned vehicle, as detailed below: The location information obtained by fusing information from multiple sensors is sent to the navigation software, which then drives the autonomous vehicle.

8. The multi-source integrated navigation method for unmanned vehicles based on incremental factor graphs according to claim 1, characterized in that, In the factor graph framework, the measurement value of each sensor is encoded as a factor. The component framework graph is added when the measurement value is generated, and data fusion and parameter estimation are completed by using Bayesian inference on these connected factors.

9. The multi-source integrated navigation method for unmanned vehicles based on incremental factor graphs according to claim 1, characterized in that, MEMS inertial navigation IMU, MAG magnetometer, BAR altimeter, ODO odometer, VIS binocular camera, LIDAR lidar, MMWR millimeter-wave radar, and ULTRA ultrasonic radar can be configured and combined in a plug-and-play manner, and if a sensor fails, it can be removed from the factor graph framework.

Images