Factor graph multi-source information fusion method based on incremental learning

Abstract

The application discloses a kind of factor graph multi-source information fusion methods based on incremental learning.The method is: firstly, the sensor and program-controlled power supply carried by unmanned vehicle are self-checked, and the corresponding factor graph framework is matched;Then, the sensor measurement information is collected, the navigation parameter solution is synchronously carried out, and is transmitted to central processing unit;Then, prior information is set, the factor graph model of sensor and the corresponding cost function are established;When there is new observation information, the part affected by new added factor is converted back to factor graph, and the factor associated with new measurement is added, incremental learning is carried out in training set, and by selecting the elimination order of variable, the node is eliminated from factor graph and the conditional probability table is constructed, to obtain the bayesian network representing variable conditional dependence relationship;If there is new observation information, repeat factor adding;Otherwise, output navigation information.The application has the advantages of strong applicability, high calculation efficiency, strong anti-interference ability, strong maintainability and strong expansibility.

Description

Technical Field

[0001] This invention belongs to the field of integrated navigation and multi-source information fusion technology, and in particular, it is a factor graph multi-source information fusion method based on incremental learning. Background Technology

[0002] In the modern military environment, satellite denial poses a threat and interferes with satellite communication and navigation systems, severely impacting the continuity and reliability of related applications. With the development and widespread application of information technology, multi-source information fusion has become an important means of solving real-world problems. Multi-source information fusion refers to the integration and analysis of multiple information sources from different sensors, platforms, or technologies to obtain more comprehensive, accurate, and reliable information.

[0003] Typically, integrated navigation filters employ Kalman filtering. Traditional Kalman filtering updates based on current measurements and previous state estimates. The filter's response to new measurement information has a certain lag, making it unable to reflect system changes in real time. For rapidly changing systems or applications requiring timely responses, traditional Kalman filtering cannot meet real-time requirements.

[0004] Unlike Kalman filter-based multi-source fusion methods, the factor graph framework can combine numerous sensors with different frequencies and errors for application. For example, IMU sensors update rapidly, while cameras and radars update more slowly. Based on the influence of measurement information on navigation state variables, the factor graph encodes the relationship between unknown variable nodes and known measurements. The problem of fusing observations from different, asynchronous sensors becomes connecting factors defined by measurements to corresponding nodes in the factor graph. The factor graph framework can also be extended to include more newly added sensors. The advantage of factor graph optimization is its plug-and-play capability for multi-source navigation information. Multi-source fusion methods based on factor graph optimization retain historical information and improve estimation accuracy by jointly estimating states at multiple times. However, this method suffers from high computational cost and latency. Summary of the Invention

[0005] The purpose of this invention is to provide a multi-source information fusion method that has low computational cost, good real-time performance, can meet the requirements of real-time estimation, has high classification accuracy, and high incremental learning efficiency.

[0006] The technical solution to achieve the purpose of this invention is: a factor graph multi-source information fusion method based on incremental learning, comprising the following steps:

[0007] Step 1: Perform self-test on the sensors and programmable power supply of the unmanned vehicle, identify the current environment, and match the corresponding factor graph framework;

[0008] Step 2: Collect sensor measurement information in real time, calculate navigation parameters synchronously, and transmit the data to the central processing unit;

[0009] Step 3: Establish factor graph models and corresponding cost functions for different navigation sensors;

[0010] Step 4: If there is new observation information, convert the part affected by the newly added factor back to the factor graph, add the factor associated with the new measurement, add it to the training set for incremental learning, select feature terms and set initial weights, dynamically adjust the confidence factor and correct the cost function.

[0011] Step 5: Select the variable with the smallest uneliminated dimension as the next variable to be eliminated. By selecting the elimination order of the variables, construct the network structure, eliminate nodes from the factor graph and construct a conditional probability table to obtain a Bayesian network representing the conditional dependencies of the variables, which is used for probability modeling and inference.

[0012] Step 6: If there is new observation information, return to step 4; otherwise, obtain the optimal state estimate and output navigation information.

[0013] Furthermore, the sensors described in step 1 include an inertial navigation IMU, a lidar, a visual sensor, a magnetic sensor (Magnet), an odometer (Odo), an altimeter (Alt), and an ultrasonic radar (Ultra).

[0014] Furthermore, step 1 involves performing a self-test on the sensors and programmable power supply mounted on the unmanned vehicle, identifying the current environment, and matching the corresponding factor graph framework, as detailed below:

[0015] Step 1.1: Install each sensor on the vehicle body using tooling, connect the power supply for preheating, initialize navigation parameters, display the positioning information in real time on the monitoring terminal, and perform self-tests on the sensors and programmable power supply of the unmanned vehicle.

[0016] Step 1.2: Based on the mission requirements and environment of the unmanned vehicle, determine the sensor type and its feasibility, and decide on the corresponding factor graph framework.

[0017] Furthermore, in step 3, factor graph models for different navigation sensors are established, as follows:

[0018] The factor graph is represented as follows: ,in For the set of factor nodes, local functions in factorization ; For the set of variable nodes, the variables in the global multivariate function. ; Edge lines represent factor nodes. With variable nodes The set of connecting edges, ;

[0019] The factor graph G is defined as a function Factorization:

[0020]

[0021] in It is related to factor nodes The set of all relevant variable nodes. ;

[0022] In the factor graph model, the interaction between variable nodes and factor nodes, or between factor nodes and variable nodes, is achieved through information exchange. Changes and additions or deletions of nodes are reflected through information transmission.

[0023] Factor graph probabilistic graphical models are used to describe the joint probability distribution of navigation state variables, transforming the solution of the maximum a posteriori estimate of navigation state into factor graph inference. The calculation method is as follows:

[0024]

[0025] In the formula, Indicates the requirement to satisfy Maximum The value; Navigation system Time's up All state quantities between time points, express Time's up All measurements received within this time period; Indicates that all known observations Lower estimated state The probability function;

[0026] The navigation state of the autonomous vehicle is abstracted as variable nodes, and the asynchronous and heterogeneous measurement information from different navigation sensors is abstracted as factor nodes. Represents the navigation status of the system. Represents the measurement information from each sensor;

[0027] Represents prior information, This indicates measurement information from the IMU, and Time and Related to the navigation status at any given time; , , , , and These represent measurement information from Mag, Alt, Odo, Visual, Lidar, and Ultra, respectively.

[0028] Furthermore, in step 3, the cost functions corresponding to different navigation sensors are established, as follows:

[0029] Step 3.1: Set prior information;

[0030] Step 3.2: Perform pre-integration processing on the observation information of the inertial navigation IMU within the pre-integration interval, and add inertial navigation IMU factor nodes. Sum of deviation factor nodes ;

[0031] Step 3.3: Add the corresponding factor nodes directly to the observation information from other sensors.

[0032] Furthermore, the setting of prior information in step 3.1 is as follows:

[0033] The prior information for the autonomous vehicle localization factor graph model includes the vehicle's initial position and initial velocity, for certain variables... The prior information is that the mean is variance is Gaussian distribution model, prior factors Represented as:

[0034]

[0035] In the formula, for The cost function of the prior factors.

[0036] Furthermore, in step 3.2, the observation information from the inertial navigation IMU is pre-integrated within the pre-integration interval, and inertial navigation IMU factor nodes are added. Sum of deviation factor nodes The details are as follows:

[0037] Predicting navigation state This is described by the following continuous nonlinear differential equation:

[0038]

[0039] In the formula, The current navigation status. and These represent the specific force and angular velocity measured by the accelerometer and gyroscope, respectively. For the measurement function, predict the initial value. ; For IMU calibration parameters, The time propagation is described by its nonlinear model:

[0040]

[0041] For a given IMU measurement The standard factor for IMU is:

[0042]

[0043] In the formula, Represent the corresponding cost function;

[0044] IMU measurement and current estimated value Used to predict states The predicted value and the current estimate The difference between them is the error function expressed by factors. ;

[0045] Bias factor associated with IMU error calculation model for:

[0046]

[0047] in, In a factor graph, this is represented as a variable node;

[0048] The new IMU-measured motion state update formula is:

[0049]

[0050] in, , , They are time points At the time Updated by attitude, velocity, and position, index and Representing time respectively and time subscript Indicates fixed at Historical framework in (inertial coordinate system) (World coordinate system), subscript Indicates fixed at Historical framework in (inertial coordinate system) (Carrier coordinate system), indicating a transformation from subscript to exponent coordinate system. Indicates at time Quaternions relative to w-frame rotation Or rotation matrix It is caused by the Earth's rotation. Represents the identity matrix. Represents gravitational acceleration. It is the angular velocity mapped from the geocentric inertial coordinate system to the world coordinate system. Indicates the IMU sampling time interval;

[0051] To eliminate the dependence on the initial motion state, at time... Will Fixed to (Earth coordinate system), i.e. Derivation Reference framework for time-refined pre-integral models:

[0052]

[0053] in, , and These are attitude pre-integration, velocity pre-integration, and position pre-integration, respectively. The superscript indicates mapping to the target coordinate system, and the subscript indicates mapping to the initial coordinate system. The attitude term is used in fine pre-integration. This is because it requires an absolute attitude to account for the Earth's rotation. and These are the Coriolis correction terms for velocity pre-integration and position pre-integration, respectively. It is a rotation matrix.

[0054] Furthermore, the observation information from other sensors described in step 3.3 is directly supplemented with corresponding factor nodes, as follows:

[0055] Step 3.3.1, the factor graph model of the LiDAR is as follows:

[0056]

[0057] In the formula, This is the current measurement value from the lidar. It is a lidar measurement function. It is the first The pose state at any given moment. It is the first The pose state at any given moment. To measure noise for lidar;

[0058] The cost function for LiDAR is:

[0059]

[0060] in, It is a cost function;

[0061] Step 3.3.2, the factor graph model of Ultra ultrasonic radar is as follows:

[0062]

[0063] In the formula, This is the current measurement value from the ultrasonic radar. It is an ultrasonic radar measurement function. It is the first The pose state at any given moment. It is the first The pose state at any given moment. For measuring noise using ultrasonic radar;

[0064] The cost function for Ultrasonic radar is:

[0065]

[0066] Step 3.3.3, the factor graph model of the stereo camera Visual factor is as follows:

[0067]

[0068] in, for Time-of-flight depth camera measurements For measurement functions, For measuring noise;

[0069] The cost function for the stereo camera Visual is:

[0070]

[0071] Step 3.3.4, the factor graph model of the Mag factor of the magnetic sensor is as follows:

[0072]

[0073] in, for The magnetic sensor measurement value at any time. For measurement functions, For measuring noise;

[0074] The cost function of the magnetic sensor Mag is:

[0075]

[0076] Step 3.3.5, the factor graph model of the odometer Odo factor is as follows:

[0077]

[0078] in, for Time-mileage measurement value For measurement functions, For measuring noise;

[0079] The cost function for the Odo odometer is:

[0080]

[0081] Step 3.3.6, the factor graph model of the altimeter Alt factor is as follows:

[0082]

[0083] in, for Time-based altitude measurement value For measurement functions, For measuring noise;

[0084] The cost function for the altimeter Alt is:

[0085]

[0086] Furthermore, in step 4, if new observation information is found, the portion affected by the newly added factor is converted back into a factor graph, and the factor associated with the new measurement is added to the graph. This graph is then added to the training set for incremental learning. Feature terms are selected and initial weights are set. The confidence factor is dynamically adjusted, and the cost function is corrected, as detailed below:

[0087] Step 4.1, the definition of domain incremental learning is:

[0088]

[0089]

[0090] in, , Tasks Data sample set and category label set, superscript , Indicates the task category, and Representing tasks Data probability distribution and label probability distribution;

[0091] Step 4.2, Sensor Factor Information Category The probability is, :

[0092]

[0093] in, For class Conditional probability, whose value belongs to the class The ratio of the number of factor information items to the total number of training factor information items. For characteristic terms In category The probability of it appearing in;

[0094] Step 4.3: In the incremental Bayesian model, the prior probability of the event occurring is set as... , Let represent its probability density function, where This represents prior information. According to Bayes' principle, when there are new samples... Enter the training set, based on prior information The posterior information was calculated. The formula is:

[0095]

[0096] in, Let f(x) represent the probability density function of a sample occurring given prior information and prior probabilities. This represents the probability density function that indicates the occurrence of a prior probability given prior information. The probability density function representing the occurrence of a sample given prior information;

[0097] in:

[0098]

[0099] in, This represents the probability density function of a sample occurring given that the prior probability has occurred.

[0100] The Bayesian incremental learning model integrates prior information with information from newly added samples. Obtain posterior information ,Right now:

[0101]

[0102] Furthermore, step 5 involves selecting the variable with the smallest uneliminated dimension as the next variable to be eliminated. By selecting the elimination order of the variables, a network structure is constructed. Nodes are eliminated from the factor graph, and a conditional probability table is built to obtain a Bayesian network representing the conditional dependencies of the variables, which is used for probability modeling and inference, as detailed below:

[0103] Step 5.1: Using the minimum dimension method in heuristic sorting, select a variable with the smallest uneliminated dimension as the next variable to be eliminated;

[0104] The heuristic sorting method is as follows:

[0105] ① Calculate the number of comparisons between each element in the original sequence and other elements. The number of comparisons is defined based on the Euclidean distance.

[0106] ②The initial sorting order is the same as the original sequence order;

[0107] ③ For each element, calculate the average number of comparisons between that element and other elements, that is, count the number of comparisons between that element and other elements in the current sorting order, and calculate the average value;

[0108] ④ Select the element with the smallest average number of comparisons and place it at the beginning of the current sort order;

[0109] ⑤ Repeat the above steps until all elements are sorted;

[0110] Step 5.2: Temporary factor graph. By selecting a variable with the smallest uneliminated dimension as the elimination order for the next variable to be eliminated, a network structure is constructed. Nodes are eliminated from the factor graph, and a conditional probability table is constructed to obtain a Bayesian network representing the conditional dependencies of variables, which is used for probability modeling and inference.

[0111] Compared with the prior art, the significant advantages of the present invention are: (1) When the complex battlefield environment changes dynamically, the plug-and-play function can be achieved by adding or removing factors, which has strong applicability; (2) The iSAM2 optimization toolbox is used to add a sliding window in the incremental factor graph, which restricts the factor information involved in the optimization and only uses the historical information in the most recent window to predict the measurement information at the next moment. When new data arrives, only the affected features and factors are updated, instead of the entire model is updated globally, which reduces the amount of computation and improves the computational efficiency; (3) The priority strategy method of dynamic confidence factor is adopted to measure the contribution to the fusion task according to the characteristics of the data source. By maximizing the classification confidence, the micro-average is used. The mean is monitored and evaluated in real time to evaluate the classifier performance and dynamically adjusted as the data source confidence factor to improve the classifier learning efficiency. It can effectively balance the completeness of incremental learning sample data and the efficiency of incremental learning; (4) The absolute value or square of the model parameters is introduced into the loss function of the model as a penalty term to control the size of the parameters and avoid the overfitting of the model by data noise; (5) An autonomous inertial navigation system with good concealment that does not rely on external information is used as the core and is combined with different sensors for information fusion, which improves the anti-interference ability; (6) The Ubuntu operating system is used, which is suitable for various general computing needs. ROS adopts a modular architecture, which improves the maintainability and scalability of the system. Attached Figure Description

[0112] Figure 1 This is a flowchart illustrating a factor graph multi-source information fusion method based on incremental learning according to the present invention.

[0113] Figure 2 This is a flowchart illustrating the factor graph multi-source information fusion software based on incremental learning in an embodiment of the present invention.

[0114] Figure 3 This is a schematic diagram of the general model of the factor graph in the embodiments of the present invention.

[0115] Figure 4 This is a schematic diagram of the structure of each sensor factor node and variable node in the embodiments of the present invention.

[0116] Figure 5 This is a schematic diagram of the incremental Bayesian model in an embodiment of the present invention. Detailed Implementation

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

[0118] The purpose of this invention is to provide a factor graph embedded multi-source information fusion method that combines incremental learning, which can achieve multi-source information fusion in an Ubuntu environment with inertial navigation as the core, including sensors such as lidar, vision, magnetic sensors, odometers, altimeters, and ultrasonic radar.

[0119] Combination Figure 1 This invention discloses a factor graph multi-source information fusion method based on incremental learning, comprising the following steps:

[0120] Step 1: Perform a self-test on the sensors and programmable power supply of the autonomous vehicle to ensure normal operation and safety, identify the current environment, and match the corresponding factor graph framework, as follows:

[0121] Step 1.1: Install each sensor on the vehicle body using tooling, including the inertial navigation IMU, LiDAR, Visual, Mag, Odo, Alt, and Ultra ultrasonic radar. Connect the power supply for preheating, initialize navigation parameters, display the positioning information in real time on the monitoring terminal, and perform self-tests on the sensors and programmable power supply of the unmanned vehicle to ensure normal operation.

[0122] Step 1.2: Based on the mission requirements and environment of the autonomous vehicle, determine the sensor types and their feasibility, and decide on the appropriate factor graph framework.

[0123] Step 2: Real-time acquisition of sensor measurement information, simultaneous calculation of navigation parameters, and transmission to the central processing unit, as detailed below:

[0124] Step 3: Set prior information and establish factor graph models and corresponding cost functions f(X) for different navigation sensors, as detailed below:

[0125] Step 3.1: Set prior information, as follows:

[0126] Factor diagram of multi-source navigation information processing system for:

[0127]

[0128] in, For the set of factor nodes, local functions in factorization ; For the set of variable nodes, the variables in the global multivariate function. ; For the set of edge connections, factor nodes With variable nodes Connecting edge ;

[0129] Define the factor graph G as a function Factorization:

[0130] (1)

[0131] in It is related to factor nodes The set of all relevant variable nodes. ;

[0132] The general model of a factor graph is as follows: Figure 3 As shown, in the factor graph model, variable nodes to factor nodes or factor nodes to variable nodes are connected through information exchange. Therefore, changes and additions or deletions of nodes can be reflected through information transmission.

[0133] A factor graph model is used to describe the joint probability distribution of navigation state variables. Solving for the maximum a posteriori estimate of the navigation state is transformed into factor graph inference. The calculation method is as follows:

[0134] (2)

[0135] In the formula, Indicates the requirement to satisfy Maximum The value; Navigation system Time's up All state quantities between time points, This represents all measurements received within the corresponding time period; Indicates that all known observations Lower estimated state The probability function.

[0136] The autonomous vehicle localization factor graphical model uses prior information to represent prior factors, including the vehicle's initial position and initial velocity. Prior information Defined as mean variance is The Gaussian distribution model is expressed as:

[0137] (3)

[0138] In the formula for The cost function of the prior factors.

[0139] Step 3.2: Perform pre-integration processing on the IMU observation information within the pre-integration interval, and add IMU factor nodes. Sum of deviation factor nodes The details are as follows:

[0140] Predicting navigation state This can be conceptually described by the following continuous nonlinear differential equation:

[0141] (4)

[0142] In the formula, and These represent the specific force and angular velocity measured by the accelerometer and gyroscope, respectively. For measurement functions; by The indicated IMU calibration parameters are used to correct IMU measurements based on an assumed IMU error model. and This IMU error model is usually estimated in combination with the estimation of the navigation state; the linearization of Equation (4) uses a state-space representation containing an appropriate Jacobian matrix and process noise, which is set to zero-mean Gaussian noise.

[0143] Under normal circumstances, The time propagation of time can be described using its own nonlinear model, such as random walk:

[0144] (5)

[0145] For a given IMU measurement The standard factor for IMU is:

[0146] (6)

[0147] In the formula: This represents the corresponding cost function. For measurement functions;

[0148] IMU measurement and current estimated value Used to predict states The predicted value and the current estimate The difference between them is an error function represented by factors;

[0149] When adding new variable nodes to the graph A reasonable initial value needs to be set for it. The bias factor related to the IMU error calculation model is given by the following formula:

[0150] (7)

[0151] in In the factor plot, these are represented as variable nodes. (Due to IMU calibration parameters) Typical slow dynamics, factors Bias and actual calibration nodes can be added to the factor plot at a frequency significantly lower than the IMU rate.

[0152] For new IMU measurements, use Equation (8) to update the motion state, including attitude, velocity, and position:

[0153] (8)

[0154] in, , , They are time points At the time Updated by attitude, velocity, and position, index and Representing time respectively and time subscript Indicates fixed at Historical framework in (inertial coordinate system) (World coordinate system), subscript Indicates fixed at Historical framework in (inertial coordinate system) (Carrier coordinate system), indicating a transformation from subscript to exponent coordinate system. Indicates at time Compared to rotation, quaternion Or rotation matrix It is caused by the Earth's rotation. Represents the identity matrix. Represents gravitational acceleration. It is the angular rate of transformation from the geocentric inertial coordinate system to the world coordinate system. Indicates time At the time .

[0155] Because IMU motion integration depends on the initial motion state, it cannot be effectively applied to the FGO framework. When the linearized point or bias estimates change, pre-integration theory is used to reformulate the model to avoid repeated integration. To eliminate the dependence on the initial motion state, at time... Will Fixed to ,Right now Defined as a reference framework for refined pre-integral models:

[0156] (9)

[0157] in, , and These are attitude pre-integration, velocity pre-integration, and position pre-integration, respectively. The superscript indicates mapping to the target coordinate system, and the subscript indicates mapping to the initial coordinate system. The attitude term is used in fine pre-integration. This is because it requires an absolute attitude to account for the Earth's rotation. and These are the Coriolis correction terms for velocity pre-integration and position pre-integration, respectively. It is a rotation matrix.

[0158] Step 3.3: For observation information from other sensors, directly add the corresponding factor nodes, as follows:

[0159] Step 3.3.1, Factor graph model and cost function of LiDAR:

[0160] Based on the actual measurement equations and prior landmark settings, lidar sensor observations can be integrated at multiple levels. Each measurement describes the change in position and orientation on the ground plane between two time instances. Its implementation is the relative attitude between the pose at time t and time t, without affecting altitude, roll, and pitch. Therefore, lidar can be considered a binary factor, and the measurement model is as follows:

[0161] (10)

[0162] In the formula, This is the current measurement value from the lidar. 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. It is a lidar measurement function that represents the relationship between the attitudes of the carrier at two different moments. Therefore, lidar... The cost function is:

[0163] (11)

[0164] in, It is the cost function.

[0165] Step 3.3.2, Ultrasonic Radar The factor graph model is as follows:

[0166] (10)

[0167] In the formula, This is the current measurement value from the lidar. It is an ultrasonic radar measurement function. It is the first The pose state at any given moment. It is the first The pose state at any given moment. For measuring noise;

[0168] The cost function for Ultrasonic radar is:

[0169] (11)

[0170] in, It is the cost function.

[0171] Step 3.3.3: Factor graph model and cost function for the stereo camera Visual:

[0172] The measurement equation for the Visual binocular camera is:

[0173] (12)

[0174] in, for Time-of-flight depth camera measurements For measurement functions, For measuring noise;

[0175] The cost function for the stereo camera Visual is:

[0176] (13)

[0177] in, It is the cost function, and the stereo camera factor is also a univariate factor, which is reflected in the factor graph and the state variables at the corresponding time points. Connected;

[0178] Step 3.3.4: Factor graph model and cost function of the magnetic sensor Mag:

[0179] The measurement equation for the Mag magnetic sensor is:

[0180] (14)

[0181] in, for The magnetic sensor measurement value at any time. For measurement functions, For measuring noise;

[0182] The cost function of the magnetic sensor Mag is:

[0183] (15)

[0184] in, It is a cost function, and the magnetic sensor factor is also a univariate factor, which is reflected in the factor graph and the state variables at the corresponding time points. Connected;

[0185] Step 3.3.5, Factor graph model and cost function of odometer Odo:

[0186] The measurement equation for the Odo odometer is:

[0187] (16)

[0188] in, for Time-mileage measurement value For measurement functions, For measuring noise;

[0189] The cost function for the Odo odometer is:

[0190] 17)

[0191] in, It is a cost function, and the odometry factor is also a univariate factor, which is the state variable in the factor graph and at the corresponding time. Connected;

[0192] Step 3.3.6, Factor graph model and cost function of altimeter Alt:

[0193] The measurement equation for altimeter Alt is:

[0194] (18)

[0195] in, for Time-based altitude measurement value For measurement functions, For measuring noise;

[0196] The cost function for the altimeter Alt is:

[0197] (19)

[0198] in, It is a cost function, and the altimeter factor is also a univariate factor, which is represented in the factor graph and the state variables at the corresponding time points. Connected.

[0199] The factor graph framework has abstracted the navigation state of the autonomous vehicle into variable nodes, and the asynchronous heterogeneous measurement information from different navigation sensors into factor nodes. The software flowchart is as follows: Figure 2 As shown:

[0200] 1) Create and initialize the IMU measurement object: Create the IMU measurement object using the one provided by GTSAM. This class passes the IMU's measurement data to this object and sets the IMU's noise parameters, including the noise covariance matrix and bias; then it creates the IMU pre-integrator. An object for pre-integrating IMU measurement data and calculating the pre-integration factor; initializing the state vector and noise covariance matrix.

[0201] 2) Create an optimization parameter object: Whether to use the optimization toolbox Make a judgment; otherwise, use the least squares method. If used, create an optimization parameter object, set the frame of the corresponding factor graph and the type of sensor connected, and set the optimization parameters. Add prior factors Within the factor graph framework, construct the IMU factor noise model.

[0202] 3) Real-time publishing and fusion: Based on the creation of ROS nodes and callback functions, subscribe to inertial navigation topics. Add IMU pre-integration factor Construct IMU factors, and the callback function loops and waits for other sensors. , Add the corresponding sensor factors to the graph.

[0203] Step 4: If new observation information is available, convert the portion affected by the newly added factors back into a factor graph, add the factors associated with the new measurements, add them to the training set for incremental learning, select feature terms and set initial weights, dynamically adjust the confidence factor and correct the cost function, as follows:

[0204] Step 4.1: To address the catastrophic forgetting problem in incremental learning—that is, when the model learns new tasks, it forgets knowledge learned from previous tasks—a dynamic architecture is chosen to solve the catastrophic forgetting problem, leveraging the flexibility of the incremental factor graph framework. Its advantage lies in the ability to dynamically allocate structures or parameters for different tasks, further improving the model's scalability and adaptability. Based on the characteristics of the learning scenario and task status, a domain-based incremental learning approach is adopted. Data arriving at different times belongs to the same category of the same task, but the distribution of the input data changes, with data in different time slices belonging to different domains. In this scenario, task identifiers are provided during the training phase, but not during the inference phase; instead, a shared identifier is provided.

[0205] The basic settings for domain incremental learning are:

[0206] (20)

[0207] (twenty one)

[0208] in, , Tasks Data sample set and category label set, superscript , Indicates the task category, and Representing tasks Data probability distribution and label probability distribution.

[0209] Step 4.2: The Naive Bayes classifier is a probabilistic classification model based on Bayes' theorem and the assumption of conditional independence of features. In the traditional Naive Bayes model, each feature is considered independent, and each feature has the same weight influencing the classification result. However, in some cases, different features may have different importance or weights in the classification result. By introducing sample weights, the weighted Naive Bayes model can better adapt to imbalanced datasets or give certain samples higher importance. It can be used for various classification tasks and can be combined with other techniques, such as feature selection and regularization, to improve the performance of the classifier. Sensor factor information Category The probability of is, where The probability can be calculated using the following formula:

[0210] (twenty two)

[0211] in, For class Conditional probability, whose value belongs to the class The ratio of the number of factor information items to the total number of training factor information items. For characteristic terms In category The probability of it appearing in the middle.

[0212] Step 4.3: When the training sample data is incomplete or when faced with a large influx of information, the Naive Bayes classification algorithm may predict the sample type inaccurately. To address this issue, an incremental learning method can be introduced. By gradually updating the prior probability and conditional probability of the category, the improved Naive Bayes algorithm can better handle new data and achieve more accurate classification results.

[0213] In the incremental Bayesian model, let the prior probability of an event occurring be denoted by . express, Let represent its probability density function, where This represents prior information. According to Bayes' principle, when there are new samples... Enter the training set, based on prior information The posterior information was calculated. The formula is:

[0214] (twenty three)

[0215] in, Let f(x) represent the probability density function of a sample occurring given prior information and prior probabilities. This represents the probability density function that indicates the occurrence of a prior probability given prior information. It represents the probability density function of a sample occurring given prior information.

[0216] in:

[0217] (twenty four)

[0218] in, It represents the probability density function of a sample occurring given that the prior probability has occurred.

[0219] Depend on Figure 5 It can be seen that the Bayesian incremental learning model integrates prior information and information from newly added samples. Finally, we obtain the posterior information, namely:

[0220] (25)

[0221] Bayesian incremental learning is essentially a dynamic process that adapts to new sample information by continuously refining and updating prior information. When a new sample arrives, the previously accumulated posterior information is immediately transformed into the current prior information. A crucial prerequisite is that the posterior information must have the same distribution as the prior information. This means that the distribution of new samples must be consistent with the distribution of existing samples. Only when the posterior and prior information distributions are identical can the current posterior information be used as the prior information for the next iteration. If the distributions are inconsistent, the current posterior information cannot be directly used as the prior information for the next learning iteration, and specific methods may be needed to address the distribution inconsistency, such as domain adaptation, feature selection, or feature transformation techniques.

[0222] The performance of incremental learning can be evaluated from the following aspects:

[0223] (a) Accuracy: The accuracy of the model is determined by considering the matrix. The diagonal elements are used to consider the average precision of the training and test sets. Accuracy It can be calculated using the following formula:

[0224] (26)

[0225] In the formula, given the accuracy matrix of the training and test samples Each element The model obtained for observing the last sample of task i in task i The accuracy of the test classification.

[0226] (b) Forgetting rate: To assess the degree of forgetting in the model, This indicates that after training the model on task i, the model is applied to task i. The degree of forgetting is defined as follows:

[0227] (27)

[0228] in For the model in the task After training, in the mission Classification accuracy on the task Average forgetting rate The metric is defined as:

[0229] (28)

[0230] (c) Computational efficiency: The average computational efficiency across tasks is :

[0231] (29)

[0232] in, It refers to The total number of multiplication and addition operations required during training; and refer to The number of operations required to perform one forward and backward propagation. To avoid If it's too small, multiply it uniformly by a scaling factor greater than 1. , and learning It depends on the number of epochs required.

[0233] Step 5: Select the variable with the smallest uneliminated dimension as the next variable to be eliminated. By choosing the elimination order of variables, construct the network structure, eliminate nodes from the factor graph, and construct a conditional probability table to obtain a Bayesian network representing the conditional dependencies of variables, which is used for probability modeling and inference, as follows:

[0234] By introducing the ISAM2 optimization toolkit with a sliding window, and setting the sliding window to 10 based on fusion requirements and computational capabilities, while retaining some historical information, the optimized fusion result is obtained. If not used, the optimized result after least squares method is obtained. The obtained optimization results are used to update the state vector and noise covariance matrix, and then the topic data of the inertial navigation system is subscribed to in a loop.

[0235] The information fusion problem has been derived using factor graph representation, and factors for some commonly used sensors have been defined. The portion affected by newly added factors is transformed back into a factor graph, and new factors associated with the new measurements are added to obtain a temporary factor graph.

[0236] Factor graph-encoded nonlinear optimization problems can be solved by repeatedly linearizing in a standard Gaussian-Newton nonlinear optimizer. From the set of variables... An initial estimate Initially, the Gauss-Newton method finds the estimated correction value by linearizing the system. satisfy:

[0237] (30)

[0238] In the formula, For the current linearization point A sparse Jacobian matrix, For all measured values The residuals. The Jacobian matrix is ​​equivalent to the linearized factor graph, and its block structure reflects the structure of the factor graph.

[0239] Solving for the updated value requires using Decomposition or The Jacobian matrix is ​​decomposed into an upper triangular matrix. For the above extremum problem, taking the partial differential of equation (22) and setting it to zero, we get:

[0240] (31)

[0241] Equation (23) can be obtained by matrix inversion, but the computational cost becomes very high when the data volume is large. Therefore, matrix decomposition is generally used to solve it. Let the matrix be defined. Can Decomposed into Then there is and According to equation (23), we have .because Since it is an upper triangular matrix, it can be obtained by back substitution. .get Then, the linearized points are updated to a new estimate. .

[0242] Each time the system receives a new sensor measurement, it generates a new factor in the graph. This is equivalent to adding a new row to the measurement Jacobian matrix of a linearized least squares problem. Normally, when the Jacobian matrix changes, it needs to be decomposed from scratch, but this increases the computational burden over time. To prevent the constructed model from becoming overly complex, the system incorporates regularization techniques from machine learning. L1 and L2 regularization are introduced to impose constraints on the penalty function, limiting the magnitude of the weights and making them more balanced. This facilitates feature selection, resulting in more balanced and accurate weights while reducing parameter complexity.

[0243] In the factor graph framework, optimization is achieved through variable elimination. To calculate marginal or conditional probabilities, the order of variable elimination is first determined, and then variables are eliminated sequentially from the factor graph. For a variable to be eliminated, all factors containing that variable are first identified, and then these factors are merged into a new factor to reduce computational complexity; during factor merging, chords may form in the factor graph. A chord is a path connecting multiple non-adjacent nodes in the graph. After forming chords, the variables and connections in the factor graph can be mapped to a chordal Bayesian network. In a chordal Bayesian network, nodes represent random variables, and edges represent dependencies between variables. With a chordal Bayesian network, only a small portion of the Bayesian network changes due to newly added factors, thus avoiding the need to recalculate from scratch each time. This allows for more efficient calculation of the marginal or conditional probabilities of the target variable by inferring from the graph's topology.

[0244] Therefore, when new measurements arrive, only the affected portion of the Bayesian network needs to be considered. Once the affected portion is identified, it is converted back into a factor graph, new factors are added, and the resulting factor graph is then subjected to variable elimination again. Finally, the variable-eliminated Bayesian network is merged into the original unaffected Bayesian network.

[0245] In the incremental smoothing factor plot, conditional dependencies between data are represented by constructing nodes of the graph from the data within the window and establishing relationships between these nodes. By connecting historical data within the sliding window with new data, the estimated values ​​of the nodes in the incremental smoothing factor plot can be updated, avoiding the need to recalculate the entire model when new data arrives. The sliding window reduces the impact of past residuals, improves dynamic estimation capabilities, and reduces computational and data storage requirements by maintaining a certain amount of residuals.

[0246] Step 6: If there is new observation information, repeat step 4; otherwise, end the process, obtain the optimal state estimate, and output the navigation information.

[0247] The above are merely preferred embodiments of the present invention. It should be noted that those skilled in the art can make various improvements and modifications without departing from the principle of the present invention, and these improvements and modifications should also be considered within the scope of protection of the present invention.

Claims

1. A factor graph multi-source information fusion method based on incremental learning, characterized in that, Includes the following steps: Step 1: Perform self-test on the sensors and programmable power supply of the unmanned vehicle, identify the current environment, and match the corresponding factor graph framework; Step 2: Collect sensor measurement information in real time, calculate navigation parameters synchronously, and transmit the data to the central processing unit; Step 3: Establish factor graph models and corresponding cost functions for different navigation sensors; Step 4: If new observation information is available, convert the portion affected by the newly added factors back into a factor graph, add the factors associated with the new measurements, add them to the training set for incremental learning, select feature terms and set initial weights, dynamically adjust the confidence factor and correct the cost function, as follows: Step 4.1, the definition of domain incremental learning is: in, , Tasks Data sample set and category label set, superscript , Indicates the task category, and Representing tasks Data probability distribution and label probability distribution; Step 4.2, Sensor Factor Information Category The probability is, : in, For class Conditional probability, whose value belongs to the class The ratio of the number of factor information items to the total number of training factor information items. For characteristic terms In category The probability of it appearing in; Step 4.3: In the incremental Bayesian model, let the prior probability of the event be C. Let represent its probability density function, where This represents prior information. According to Bayes' principle, when there are new samples... Enter the training set, based on prior information The posterior information was calculated. The formula is: in, Let f(x) represent the probability density function of a sample occurring given prior information and prior probabilities. This represents the probability density function that indicates the occurrence of a prior probability given prior information. The probability density function representing the occurrence of a sample given prior information; in: The Bayesian incremental learning model combines prior information with the newly added sample information S to obtain posterior information. ,Right now: Step 5: Select the variable with the smallest uneliminated dimension as the next variable to be eliminated. By selecting the elimination order of the variables, construct the network structure, eliminate nodes from the factor graph and construct a conditional probability table to obtain a Bayesian network representing the conditional dependencies of the variables, which is used for probability modeling and inference. Step 6: If there is new observation information, return to step 4; otherwise, obtain the optimal state estimate and output navigation information.

2. The factor graph multi-source information fusion method based on incremental learning according to claim 1, characterized in that, The sensors mentioned in step 1 include an inertial navigation IMU, a lidar, a visual sensor, a magnetic sensor (Magnet), an odometer (Odo), an altimeter (Alt), and an ultrasonic radar (Ultra).

3. The factor graph multi-source information fusion method based on incremental learning according to claim 2, characterized in that, Step 1 describes the self-testing of the sensors and programmable power supply on the unmanned vehicle, identifying the current environment, and matching the corresponding factor graph framework, as detailed below: Step 1.1: Install each sensor on the vehicle body using tooling, connect the power supply for preheating, initialize navigation parameters, display the positioning information in real time on the monitoring terminal, and perform self-tests on the sensors and programmable power supply of the unmanned vehicle. Step 1.2: Based on the mission requirements and environment of the unmanned vehicle, determine the sensor type and its feasibility, and decide on the corresponding factor graph framework.

4. The factor graph multi-source information fusion method based on incremental learning according to claim 3, characterized in that, In step 3, factor graph models for different navigation sensors are established, as follows: The factor graph is represented as follows: ,in For the set of factor nodes, local functions in factorization ; For the set of variable nodes, the variables in the global multivariate function. ; Edge lines represent factor nodes. With variable nodes The set of connecting edges, ; The factor graph G is defined as a function Factorization: in It is related to factor nodes The set of all relevant variable nodes. ; In the factor graph model, variable nodes interact with factor nodes or factor nodes interact with variable nodes through information exchange, and changes and additions or deletions of nodes are reflected through information transmission. Factor graph probabilistic graphical models are used to describe the joint probability distribution of navigation state variables, transforming the solution of the maximum a posteriori estimate of navigation state into factor graph inference. The calculation method is as follows: In the formula, Indicates the requirement to satisfy Maximum The value; Navigation system Time's up All state quantities between time points, express Time's up All measurements received within this time period; Indicates that all known observations Lower estimated state The probability function; The navigation state of the autonomous vehicle is abstracted as variable nodes, and the asynchronous and heterogeneous measurement information from different navigation sensors is abstracted as factor nodes. Represents the navigation status of the system. Represents the measurement information from each sensor; Represents prior information, This indicates measurement information from the IMU, and Time and Related to the navigation status at any given time; , , , , and These represent measurement information from Mag, Alt, Odo, Visual, Lidar, and Ultra, respectively.

5. The factor graph multi-source information fusion method based on incremental learning according to claim 4, characterized in that, In step 3, the cost functions corresponding to different navigation sensors are established, as follows: Step 3.1: Set prior information; Step 3.2: Perform pre-integration processing on the observation information of the inertial navigation IMU within the pre-integration interval, and add inertial navigation IMU factor nodes. Sum of deviation factor nodes ; Step 3.3: Add the corresponding factor nodes directly to the observation information from other sensors.

6. The factor graph multi-source information fusion method based on incremental learning according to claim 5, characterized in that, The setting of prior information in step 3.1 is as follows: The prior information for the autonomous vehicle localization factor graph model includes the vehicle's initial position and initial velocity, for certain variables... The prior information is that the mean is variance is Gaussian distribution model, prior factors Represented as: In the formula, for The cost function of the prior factors.

7. The factor graph multi-source information fusion method based on incremental learning according to claim 6, characterized in that, Step 3.2 involves pre-integrating the observation information from the inertial navigation IMU within the pre-integration interval and adding inertial navigation IMU factor nodes. Sum of deviation factor nodes The details are as follows: Predicting navigation state This is described by the following continuous nonlinear differential equation: In the formula, The current navigation status. and These represent the specific force and angular velocity measured by the accelerometer and gyroscope, respectively. For the measurement function, predict the initial value. ; For IMU calibration parameters, The time propagation is described by its nonlinear model: For a given IMU measurement The standard factor for IMU is: In the formula, Represent the corresponding cost function; IMU measurement and current estimated value Used to predict states The predicted value and the current estimate The difference between them is the error function expressed by factors. ; Bias factor associated with IMU error calculation model for: in, In a factor graph, this is represented as a variable node; The new IMU-measured motion state update formula is: in, , , They are time points At the time Updated by attitude, velocity, and position, index and Representing time respectively and time subscript Indicates fixed at Historical framework in inertial coordinate system World coordinate system, subscript Indicates fixed at Historical framework in inertial coordinate system The carrier coordinate system represents the transformation from a subscript to an exponent coordinate system. Indicates at time Compared to rotation, quaternion Or rotation matrix It is caused by the Earth's rotation. Represents the identity matrix. Represents gravitational acceleration. It is the angular velocity mapped from the geocentric inertial coordinate system to the world coordinate system. Indicates the IMU sampling time interval; To eliminate the dependence on the initial motion state, at time... Will Fixed to Earth coordinate system, i.e. Derivation Reference framework for time-refined pre-integral models: in, , and These are attitude pre-integration, velocity pre-integration, and position pre-integration, respectively. The superscript indicates mapping to the target coordinate system, and the subscript indicates mapping to the initial coordinate system. The attitude term is used in fine pre-integration. This is because it requires an absolute attitude to account for the Earth's rotation. and These are the Coriolis correction terms for velocity pre-integration and position pre-integration, respectively. It is a rotation matrix.

8. The factor graph multi-source information fusion method based on incremental learning according to claim 7, characterized in that, Step 3.3 involves directly adding the corresponding factor nodes to the observation information from other sensors, as detailed below: Step 3.3.1, the factor graph model of the LiDAR is as follows: In the formula, This is the current measurement value from the lidar. It is a lidar measurement function. It is the first The pose state at any given moment. It is the first The pose state at any given moment. To measure noise for lidar; The cost function for LiDAR is: in, It is a cost function; Step 3.3.2, the factor graph model of Ultra ultrasonic radar is as follows: In the formula, This is the current measurement value from the ultrasonic radar. It is an ultrasonic radar measurement function. It is the first The pose state at any given moment. It is the first The pose state at any given moment. For measuring noise using ultrasonic radar; The cost function for Ultrasonic radar is: Step 3.3.3, the factor graph model of the stereo camera Visual factor is as follows: in, for Time-of-flight depth camera measurements For depth camera measurement functions, Measure noise for depth cameras; The cost function for the stereo camera Visual is: Step 3.3.4, the factor graph model of the Mag factor of the magnetic sensor is as follows: in, for The magnetic sensor measurement value at any time. For magnetic sensor measurement functions, To measure noise for magnetic sensors; The cost function of the magnetic sensor Mag is: Step 3.3.5, the factor graph model of the odometer Odo factor is as follows: in, for Time-mileage measurement value For mileage measurement function, Noise is measured for odometers; The cost function for the Odo odometer is: Step 3.3.6, the factor graph model of the altimeter Alt factor is as follows: in, for Time-based altitude measurement value For altimeter measurement function, For height measurement of noise; The cost function for the altimeter Alt is:

9. The factor graph multi-source information fusion method based on incremental learning according to claim 8, characterized in that, Step 5 describes selecting the variable with the smallest uneliminated dimension as the next variable to be eliminated. By selecting the elimination order of variables, a network structure is constructed. Nodes are eliminated from the factor graph, and a conditional probability table is built to obtain a Bayesian network representing the conditional dependencies of variables, which is used for probabilistic modeling and inference, as detailed below: Step 5.1: Using the minimum dimension method in heuristic sorting, select a variable with the smallest uneliminated dimension as the next variable to be eliminated; The heuristic sorting method is as follows: ① Calculate the number of comparisons between each element in the original sequence and other elements. The number of comparisons is defined based on the Euclidean distance. ②The initial sorting order is the same as the original sequence order; ③ For each element, calculate the average number of comparisons between that element and other elements, that is, count the number of comparisons between that element and other elements in the current sorting order, and calculate the average value; ④ Select the element with the smallest average number of comparisons and place it at the beginning of the current sort order; ⑤ Repeat the above steps until all elements are sorted; Step 5.2: Temporary factor graph: By selecting a variable with the smallest uneliminated dimension as the elimination order of the next variable to be eliminated, a network structure is constructed. Nodes are eliminated from the factor graph and a conditional probability table is constructed to obtain a Bayesian network representing the conditional dependencies of variables, which is used for probability modeling and inference.

Images