Outdoor multi-vision sensor fusion positioning method and system based on two-factor graph

Abstract

An outdoor multi-vision sensor fusion localization method and system based on a two-factor graph is disclosed. The localization method includes the following steps: (1) establishing the relationship between the state model and the coordinate system; defined as the global coordinate system W, the basic coordinate system B, the odometry coordinate system O, the IMU coordinate system I, the RGBD coordinate system R, and the stereo vision coordinate system S, respectively; (2) constructing an improved point cloud measurement model and an improved IMU pre-integration algorithm for the multi-factor graph model; (3) defining the IMU factor, the RGBD odometry factor, and the stereo vision measurement factor; (4) solving the state estimation of the robot in the odometry reference system for localization; (5) factor graph prediction update loop; (6) two-factor graph design. The localization system includes an initialization module, an acquisition module, a calculation module, an evaluation module, and a fusion localization module. This invention improves the accuracy and robustness of outdoor localization, increases the fault tolerance of the localization system, and meets the requirements of high update rate and high latency state estimation for actual control tasks.

Description

Technical Field

[0001] This invention relates to a multi-vision sensor fusion positioning method for robot vision positioning in outdoor environments, belonging to the field of robot vision positioning technology. Background Technology

[0002] In recent years, some important applications of robotics technology have existed in unstructured fields, such as agriculture, mining, search and rescue, and national defense. The autonomous navigation capabilities of robots in field environments are still relatively underdeveloped. Onboard autonomy enables robots to perform tasks without human control, thereby improving operational efficiency. Robots capable of operating in complex natural environments have broad application prospects in forestry, agriculture, mining, and border patrol.

[0003] Robot localization methods primarily rely on sensors such as GNSS (Global Navigation Satellite), LiDAR, stereo cameras, depth cameras, and IMU (Inertial Measurement Unit). In outdoor environments, weather and environmental factors, such as obstruction by trees and bushes, can cause frequent GNSS data loss over extended periods. Furthermore, signal interference, multipath effects, and signal attenuation can severely degrade positioning accuracy. GNSS positioning also has low accuracy in measuring dynamic parameters such as velocity and acceleration, failing to meet the demands of high-speed, high-precision dynamic positioning and thus not adequately fulfilling the autonomous operation requirements of outdoor mobile robots.

[0004] LiDAR primarily collects data such as distance and reflection intensity, mainly used for measuring distance and detecting the position and outline of objects. It cannot directly obtain visual information such as surface texture and color, and this single piece of information is insufficient to meet the needs. Furthermore, because LiDAR data is limited in scope, subsequent processing and analysis are required to obtain richer information, which may increase the complexity of the algorithm and computational cost. There is also the issue of excessively high physical costs, thus presenting certain limitations.

[0005] IMU measurements are used to calculate position and orientation by integrating acceleration and angular velocity, which leads to the gradual accumulation of errors. Without error correction, these errors can cause significant odometry inaccuracies. Furthermore, IMU information is susceptible to environmental interference; therefore, it cannot be used alone as odometry data, but only as compensation information.

[0006] Compared to radar and GNSS positioning, visual sensors can provide higher-precision positioning information. By identifying and matching targets in the environment, relatively high positioning accuracy can be achieved. Visual sensors can perceive and identify obstacles, terrain, and other features in the environment by acquiring image information and make autonomous decisions. Visual sensors have stronger environmental adaptability and can adapt to more complex and changing environments. Visual sensors can also provide richer environmental information, such as the color, shape, and texture of targets, which can be used for more refined positioning and path planning.

[0007] However, the influence of external conditions such as lighting on visual sensors is also a major challenge. RGBD cameras and stereo cameras are widely used in outdoor scenes. Compared with RGBD cameras, stereo cameras are less affected by lighting and are relatively stable. However, RGBD cameras process information faster and have better image construction capabilities. The advantages and disadvantages of the two methods can compensate for each other, but currently, there is limited research on methods for integrating the two, indicating a certain technological gap.

[0008] In sensor fusion, systems need to process high-frequency IMU data and high-latency visual data, while practical control tasks require high-update-rate state estimation. Typically, filtering-based methods are used for high-update-rate state estimation due to their lightweight nature; however, their ability to handle highly delayed and nonlinear measurements is limited. Optimization-based methods are better suited for handling delayed and asynchronous measurements, but solving comprehensive nonlinear optimization problems within milliseconds remains challenging for state estimation.

[0009] Therefore, for large-scale complex outdoor environments, it is very important to develop a fast and lightweight multi-vision sensor fusion positioning method applicable to outdoor environments, taking into full account the performance of various sensors, the applicability of the system and the actual scale. Summary of the Invention

[0010] In view of the shortcomings and advantages of existing sensors, and based on the deficiencies of current robot visual positioning technology in outdoor environments, this invention provides a multi-visual sensor fusion positioning method and system for complex outdoor terrain, including an improved point feature measurement model and an IMU pre-integration algorithm based on a multi-sampling method; and constructs a multi-factor graph optimization. In order to make full use of filtering and optimization methods, a new factor graph prediction update loop design is proposed, a two-factor graph is designed to compensate for the influence of external conditions on camera positioning, and a multi-visual fusion positioning system is designed based on the above method.

[0011] The outdoor multi-visual sensor fusion positioning method based on two-factor graphs of the present invention includes the following steps:

[0012] Step 1: Establish the relationship between the state model and the coordinate system;

[0013] The established coordinate systems are defined as follows: global coordinate system W, basic coordinate system B, odometer coordinate system O, IMU coordinate system I, RGBD coordinate system R, and stereo vision coordinate system S;

[0014] The mobile robot's state during the entire fusion localization process is as follows:

[0015]

[0016] in

[0017]

[0018] I x i It is the state of the IMU coordinate system I relative to the global coordinate system W, where R∈SO(3) is the direction. It's about location. It's about location. I b g , I b a These are two noise error quantities of the IMU, T WO ∈SE(3) is the coordinate transformation from the odometer coordinate system O to the global coordinate system W;

[0019]

[0020] B x i This indicates the state of the base coordinate system B relative to the odometer coordinate system O;

[0021] t k The IMU measurements at time points in the base coordinate system B and the IMU coordinate system I are denoted as follows: and Including linear acceleration and angular acceleration The RGBD odometer measurement value in the RGBD coordinate system R is The measurement value of the stereo vision odometry in the stereo vision coordinate system S is Therefore, in t k A set of measurements at time represents in

[0022] Step 2: Construct an improved point cloud measurement model and an improved IMU pre-integration algorithm for the multi-factor graph model;

[0023] The visual information of feature points input from the camera (feature points input from stereo cameras and RGBD cameras, etc.) is processed to construct a point feature measurement model. The point feature error model is constructed using the distance difference from the projection point to the observation point, i.e., through the reprojection error. The improved IMU pre-integration algorithm is a manifold IMU pre-integration algorithm proposed for multi-factor graph models. It applies a multi-sampling algorithm to obtain more accurate pre-integration IMU measurements.

[0024] The specific process of constructing the improved point cloud measurement model is as follows:

[0025] Based on point features, given camera frame c j Feature measurement of the k-th point in The reprojection error is then defined as:

[0026]

[0027] in It is camera frame c i The first observation of the features, f k The landmark point represents the feature, and the inverse depth λ of the feature. k Also in camera frame c i Internal definition, Indicates the transformation to camera frame c j The reprojection error, i.e. the position residual, is obtained from the points in the graph using this formula and added to the multi-factor plot as part of the camera model factors.

[0028] The specific process of the improved IMU pre-integration algorithm is as follows:

[0029] When an IMU measurement is received, the first step is to correct the IMU bias; then, a multi-sampling algorithm is used to achieve more accurate pre-integration IMU measurements and create an IMU pre-integration factor, which is then added to the factor graph.

[0030] Before optimization, the pre-integration IMU measurement is first-order calibrated using the current IMU bias, and then the proposed improved manifold IMU pre-integration algorithm is applied to the navigation solution in the predictor graph.

[0031] Angular velocity of the IMU in the base coordinate system B and acceleration The measured values ​​conform to the following equation:

[0032]

[0033] Where, ω t and a t These are the theoretical values ​​of angular velocity and acceleration. and It is the actual measured value of the sensor; is the transfer matrix from the global coordinate system W to the basic coordinate system B; g is the constant gravity vector in the global coordinate system W; b t It is a slowly changing centrifugal force; n t It's white noise;

[0034] By using an improved IMU pre-integration algorithm and a multi-sampling method, combined with IMU measurements, the relative motion Δv between two timestamps i and j is obtained. ij Δp ij and ΔR ij That is, velocity, position, and direction, as shown in the following formula:

[0035]

[0036] Where φ k The equivalent rotation vector is calculated based on the multisampling method:

[0037]

[0038] Where Δθ k and They are respectively [t] k-1 ,t k ] and [t k-2 ,t k-1 The angle increment in ];

[0039] ΔV k Based on the multi-sampling algorithm, considering velocity rotation error compensation and velocity oscillation compensation, the calculation is as follows:

[0040] ΔV k =Δv k +Δv rot,k +Δv scull,k ,

[0041]

[0042]

[0043] in It is rotational error compensation, Δv k This represents speed oscillation compensation.

[0044] The initial value h provided by the IMU pre-integration ( I x i It is used for matching operations with vision sensors.

[0045] Step 3: Define the IMU factor, RGBD odometry factor, and stereo vision measurement factor;

[0046] IMU Factor: Improved manifold pre-integration discretizes continuous IMU measurements, obtaining discrete IMU state variables and creating state variable nodes in the factor graph. Since IMU acquisition is the most frequent, the IMU factor serves as an intermediate node, concatenated with other nodes to construct the entire factor graph (this factor was already modeled in step two). The error term is defined as:

[0047] in

[0048] RGBD odometry factor: This is odometry information calculated using the RTABMAP method. When the RGBD odometry is operating reliably, this information is integrated into the optimization as a global position estimate, including the position residual.

[0049] in This represents the difference between the observed quantity and the actual quantity constructed from the point feature model. W p WI Representing the observed quantity, r represents the actual quantity. f This refers to the reprojection error calculated in step two.

[0050] Stereo vision measurement factors: Stereo vision odometry measurements are calculated using the binocular vision method described in ORB-SLAM3; for different applications, stereo vision measurements are represented by two types of factors:

[0051] (1) Stereo odometry factor: In global operation, the stereo odometry factor is treated as a regular observation factor and directly added to the main graph as a six-degree-of-freedom measurement factor; the difference between the odometry prediction relative transformation and the measurement model. I h(x) is evaluated on the subplot. When the RGBD odometry factor is operating reliably, this factor needs to be combined with the RGBD odometry factor to present a considerable global position to connect adjacent active factors;

[0052] (2) Stereo pose univariate factor: In the absence of RGBD odometry observations, stereo odometry observations are transcribed into pseudo-global values, represented as a univariate pose factor; based on observability, a higher covariance is set for the pose estimation of stereo odometry to better integrate with the IMU factor. Similar to the stereo odometry factor, I The h(x) error is evaluated on the subplot.

[0053] Step 4: Solve for the robot's state estimate in the odometry reference frame using maximum a posteriori estimation for localization;

[0054] From the given measurements, the maximum a posteriori estimate of all past IMU states:

[0055]

[0056] Represents state variables. Representing the observed quantity, The current state quantity is calculated using the maximum a posteriori estimate.

[0057] By maintaining T WO Transformation to determine an accurate and locally consistent estimate of the robot's state in the odometry reference frame for localization;

[0058] t k Optimal state estimation at time 1 I x i The solution is transformed into solving for the maximum a posteriori estimate of the system. Bayes' rule is used to solve for the maximum a posteriori estimate: given the measured value, solve for the probability of state x and estimate the model; that is, given the system observations, solve for the system state variables such that the conditional probability is maximized.

[0059] According to the central limit theorem, the error of the sensor used in this invention is similar to that of a typical nonlinear measurement function h( I x i It conforms to a Gaussian distribution. This system decomposes the posterior distribution into a prior term and a likelihood term, using the optimal posterior probability of the previous frame as the prior term and the measurement value given by the external sensor model as the likelihood term. Factor graph is a method for maximum a posteriori estimation of the state based on Bayes' theorem, under the assumptions that the input and observation are independent and that the state and observation conform to a multidimensional Gaussian distribution. Factor graph solves for maximizing the product of all factors and minimizing the error of the error function, ultimately transforming the problem into a nonlinear least squares problem.

[0060] Step 5: Factor graph prediction update loop;

[0061] Information such as feature points from visual sensors is used as a likelihood distribution as a measurement model to update predictions.

[0062] The method for predicting and updating the factor map is based on a multi-threaded industrial control computer, using multiple threads to simultaneously receive measurement values ​​from different sensors. Each thread processes and adds the data to the optimization process independently, with the reception of each measurement factor independent of the others. Meanwhile, the optimization thread runs continuously, waiting for callbacks from the RGBD odometry or stereo vision odometry to activate different factor maps for optimization. When an IMU measurement value is received, only the IMU measurement result is added to the factor map without triggering optimization. The update thread updates the optimized horizontal factor map result to the total state estimate. Ix. Before running the optimization, the newly arrived factors are copied to the graph to be optimized; once the optimization is complete, the state of the last optimized state is propagated to the latest IMU state using the obtained IMU bias (this graph-based approach inherently allows delay measurements to be integrated into the smoothing operation at each time step).

[0063] Step Six: Two-Factor Plot Design;

[0064] Two-factor plot design involves two aspects: outlier suppression and recovery from inconsistencies in localization.

[0065] The outlier suppression process is as follows:

[0066] Two parallel factor graphs are designed. When the RGBD odometry is running reliably (with minimal external influence), the main graph operates normally, and the stereo vision odometry factor is optimized as a relative odometry factor. At this time, the RGBD odometry factor is used as a global estimate for localization, and other factors are optimized based on this. When external conditions affect the RGBD odometry factor and make it unreliable, the system switches to the sub-graph. In this case, the stereo attitude univariate factor acts as a pseudo-global factor, replacing the role of the RGBD odometry factor to maintain consistency with the airborne localization and mapping solution. During the operation of the sub-graph, the stereo vision odometry factor is still added to the main graph as a relative odometry factor, but it is not used for state estimation or localization purposes.

[0067] The process of restoring the inconsistency in the positioning is as follows:

[0068] After the RGBD odometry is restored, the system switches back to the main image. The RGBD odometry needs to be restored based on the stereo vision odometry measurements and the IMU readings. The relative drift between the global coordinate system and the odometry coordinate system during the loss period is estimated by calculating the RGBD interruption time. WO Updated to:

[0069] T WO =T WI ·(T OI ) -1 ,

[0070] Among them, T OI The estimated value, T, is in the subplot. WI When the sub-map is enabled, it is provided by the main map. When the main map is restored, the sub-map is reset to the state of the main map.

[0071] The present invention also provides an outdoor multi-vision sensor positioning system for implementing the above positioning method, the system comprising:

[0072] Initialization module: Initializes the IMU pose as the starting pose. Regardless of the initial pose relationship between the vehicle coordinate system and the horizontal plane, the default initial pose is horizontal. The system will then make corrections and adjustments based on the relationship between the visual positioning information and the default world coordinate system.

[0073] Acquisition Module: Acquires camera and IMU information. The RGBD camera acquires visual feature points with depth information and obtains RGBD odometry information through feature point transformation. The stereo camera calculates feature information through the parallax of the left and right cameras and solves the stereo vision odometry information. The IMU module collects angular velocity and linear acceleration as initial IMU data.

[0074] The computation module calculates the point cloud measurement model and IMU pre-integration. It constructs point feature measurement models from the feature points acquired by the RGBD camera and the stereo camera, respectively. It uses the reprojection error to construct the point feature measurement models as odometry factors for the two cameras. It solves the IMU pre-integration factor by combining the multi-sampling method with the IMU pre-integration algorithm and applies it to the state estimation of the prediction factor map.

[0075] Evaluation module: In order to improve the system's fault tolerance and positioning accuracy, the usability of RGBD factors is evaluated. By detecting the number of feature points collected by RGBD and the correspondence between feature points in two consecutive frames, it is determined whether the collected RGBD visual information can be added to the factor map as an optimization factor for fusion positioning.

[0076] Fusion positioning module: Based on the evaluation results, different fusion threads are selected for factor map optimization. When the RGBD odometry is running reliably, the RGBD odometry and the stereo camera odometry are combined with IMU factors to perform fusion optimization positioning through the main map. When the RGBD odometry is lost due to external conditions, the system switches to the sub-map, and the stereo odometry is fused with IMU factors for positioning, while waiting for the RGBD odometry to be recovered.

[0077] The two-factor graph fusion localization method proposed in this invention combines the stability of traditional binocular camera localization with the accuracy and speed of RGBD camera localization. It solves the problem of external environmental interference in the outdoor operation environment of mobile robots, and improves the accuracy and robustness of outdoor localization. On the other hand, it improves the fault tolerance of the localization system, so that the system can operate normally without significantly reducing the localization accuracy even if the interference of the external environment causes a sensor to distort or malfunction.

[0078] In the two-factor graph design of this invention, the main graph fuses all the sensors used by the system, and all sensor factors participate in the overall state estimation, which significantly improves the positioning accuracy. When the system sensor structure degrades, the subgraph will no longer consider the sensor information that has been lost, and will estimate the state of the remaining sensor factors according to the new observation values. The subgraph ensures the normal operation of the system and improves the fault tolerance of the system.

[0079] This invention combines the advantages of filtering and optimization-based methods in state estimation, and proposes a novel factor graph prediction update loop design that meets the requirements of high update rate and high latency state estimation for practical control tasks. Attached Figure Description

[0080] Figure 1 This is the overall design diagram of the system on which this invention is based.

[0081] Figure 2 This is a flowchart of the process of the outdoor multi-visual sensor fusion positioning method based on two-factor graphs of the present invention.

[0082] Figure 3 This is a schematic diagram of the operation of the two-factor graph in this invention.

[0083] In the diagram: 1. RGBD camera, 2. Stereo binocular camera, 3. Inertial Measurement Unit (IMU), 4. Industrial computer, 5. Four-wheel differential platform. Detailed Implementation

[0084] The system carried by this invention is as follows Figure 1 As shown, the system includes a data acquisition module and a control module. The data acquisition module includes an RGBD camera 1, a stereo binocular camera 2, and an inertial measurement unit (IMU) 3. The control module consists of an industrial computer 4, a communication serial port, etc. Two RealSense D455 cameras are used as the RGBD camera 1 and the stereo binocular camera 2. The inertial measurement unit 3 used is an XSENSMTi-300. The system is built on a four-wheel differential platform 5, and the mobile platform has a certain degree of outdoor off-road capability.

[0085] The outdoor multi-visual sensor fusion positioning method based on two-factor graphs of the present invention, such as... Figure 2 As shown, the specific steps include:

[0086] Step 1: Establish the relationship between the state model and the coordinate system

[0087] The coordinate systems used in this system are defined as follows: global coordinate system W, basic coordinate system B, odometer coordinate system O, IMU coordinate system I, RGBD coordinate system R, and stereo vision coordinate system S.

[0088] The state of the mobile robot during the entire fusion localization process is as follows

[0089]

[0090] in

[0091]

[0092] I x i It is the state of the IMU coordinate system relative to the global coordinate system, where R∈SO(3) is the direction. It's about location. It's about location. I b g , I b a These are two noise error quantities of the IMU, T WO ∈SE(3) is the coordinate transformation from the odometer coordinate system to the global coordinate system.

[0093]

[0094] B x i This represents the state of B relative to O.

[0095] t k The IMU measurements at time points in the base coordinate system and the IMU coordinate system are denoted as follows: and Including linear acceleration and angular acceleration The RGBD odometer measurement value in the RGBD coordinate system is The measurement value of the stereo vision odometry in the stereo vision coordinate system is Therefore, in t k A set of measurements at time t can represent in

[0096]

[0097] Step 2: Construct an improved point cloud measurement model and an improved IMU pre-integration algorithm for the multi-factor graphical model.

[0098] Visual information, such as feature points, input from stereo binocular camera 2 and RGBD camera 1 is processed to construct a point feature measurement model. This model uses the distance difference between the projection point and the observation point, i.e., the reprojection error, to construct a point feature error model. Based on the point features, given camera frame c... j Feature measurement of the k-th point in The reprojection error can then be defined as...

[0099]

[0100] in It is camera frame c i The first observation of the features, f k The landmark point represents the feature, and the inverse depth λ of the feature. k Also in camera frame c i Internal definition, Indicates the transformation to camera frame c j The points in the diagram. From this formula, we can obtain the reprojection error, i.e., the position residual, which is added to the multi-factor plot as part of the camera model factor.

[0101] Considering the Earth's rotation, as well as changes in gravity and centrifugal force, an improved manifold IMU pre-integration algorithm is proposed for multi-factor graphical models. To obtain more accurate pre-integration IMU measurements, a multi-sampling algorithm is applied.

[0102] Upon receiving IMU measurements, the first step is IMU bias correction. Then, a multi-sampling algorithm is used to achieve more accurate pre-integration IMU measurements, and an IMU pre-integration factor is created. Finally, the IMU pre-integration factor is added to the factor graph.

[0103] Before optimization, the pre-integration IMU measurements are first-order calibrated using the current IMU bias. The proposed improved manifold IMU pre-integration algorithm is then applied to the navigation solution in the predictor graph.

[0104] Angular velocity of the IMU in coordinate system B and acceleration The measured values ​​conform to the equation

[0105]

[0106] Where, ω t and a t These are the theoretical values ​​of angular velocity and acceleration. and It is the actual measured value of the sensor; is the transfer matrix from W to B; g is the constant gravity vector of W; b t It is a slowly changing centrifugal force; n t It's white noise.

[0107] By using an improved IMU pre-integration algorithm and a multi-sampling method, combined with IMU measurements, the relative motion Δv between two timestamps i and j is obtained. ij Δp ij and ΔR ij That is, velocity, position, and direction, as shown in the following formula.

[0108]

[0109] Where φ kThe equivalent rotation vector is calculated based on the multi-sampling method.

[0110]

[0111] Where Δθ k and They are respectively [t] k-1 ,t k ] and [t k-2 ,t k-1 ] angle increment

[0112] ΔV k Based on the multi-sampling algorithm, considering velocity rotation error compensation and velocity oscillation compensation, the calculation is as follows:

[0113] ΔV k =Δv k +Δv rot,k +Δv scull,k

[0114]

[0115]

[0116] in It is rotational error compensation, Δv k This represents speed oscillation compensation.

[0117] IMU pre-integration provides a good initial value h( I x i It is used for matching operations with vision sensors.

[0118] Step 3: Definition of each factor

[0119] IMU Factor: Improved manifold pre-integration discretizes continuous IMU measurements, obtaining discrete IMU state variables and creating state variable nodes in the factor graph. Since IMU acquisition is the most frequent, the IMU factor serves as an intermediate node, concatenated with other nodes to construct the entire factor graph. This factor has already been modeled previously, and the error term is defined as...

[0120] in

[0121] The RGBD odometry factor is calculated using the RTABMAP method. When the RGBD odometry is operating reliably, this information is integrated into the optimization as the RGBD odometry factor, which includes the position residual, as a global position estimate.

[0122] in This represents the difference between the observed quantity and the actual quantity constructed from the point feature model.W p WI Representing the observed quantity, r represents the actual quantity. f This is the reprojection error calculated in step two.

[0123] Stereo vision measurement factors: Stereo vision odometry measurements are calculated using the binocular vision method described in ORB-SLAM3. Depending on the application, stereo vision measurements are represented by two types of factors:

[0124] (1) Stereo odometry factor: During global operation, the stereo odometry factor is treated as a regular observation factor and directly added to the main graph as a six-degree-of-freedom measurement factor. The difference between the odometry prediction relative transformation and the measurement model is considered. I h(x) is evaluated on the subplot. It should be noted that when the RGBD odometry factor is running reliably, this factor needs to be combined with the RGBD odometry factor to present a considerable global position to connect adjacent active factors.

[0125] (2) Stereo pose univariate factor: In the absence of RGBD odometry observations, stereo odometry observations are transcribed into pseudo-global values, represented as a univariate pose factor. Based on observability, a higher covariance is set for the pose estimation of the stereo odometry to better integrate with the IMU factor. Similar to the stereo odometry factor, I The h(x) error is evaluated on the subplot.

[0126] Step 4: Maximum A posteriori estimation to solve for the robot's state estimate in the odometry reference frame for localization. From the given measurements, the maximum a posteriori estimate of all past IMU states is obtained:

[0127]

[0128] Represents state variables. Representing the observed quantity, The current state quantity is calculated using the maximum a posteriori estimate.

[0129] By maintaining T WO Transformation can determine an accurate and locally consistent estimate of the robot's state in the odometry reference frame for localization.

[0130] The purpose of the system is to obtain t k Optimal state estimation at time 1 I x iThis problem can be transformed into solving the maximum a posteriori (MAP) estimation problem for the system. Using Bayes' theorem to solve the MAP estimation: given a measurement, solve for the probability of state x and estimate the model. That is, given system observations, solve for the system state variables that maximize this conditional probability. According to the central limit theorem, the error of the sensor used in this invention is related to the general nonlinear measurement function h( I x i It conforms to a Gaussian distribution. This system decomposes the posterior distribution into a prior term and a likelihood term, using the optimal posterior probability of the previous frame as the prior term and the measurement value given by the external sensor model as the likelihood term. Factor graphs are a method for maximum a posteriori estimation of the state based on Bayes' theorem, under the assumptions that the input and observation are independent and that the state and observation conform to a multidimensional Gaussian distribution. Factor graphs solve for maximizing the product of all factors and minimizing the error of the error function, ultimately transforming the problem into a nonlinear least squares problem.

[0131] Step 5: Predictive Update Loop

[0132] The system application environment assumes that all sensors are functioning normally and that there are no abnormal measurements. Since it is necessary to process IMU and visual information, the robot's prior distribution is calculated using the linear velocity and angular velocity from the pre-integrated IMU data. This dynamic model is used for the prediction step to predict the vehicle's motion. In the update step, information such as feature points from the visual sensors is used as the likelihood distribution as the measurement model to update the prediction.

[0133] Practical control tasks require state estimation with high update rates and high latency. To combine the advantages of filtering and optimization-based methods, a novel factor graph prediction update loop design is proposed. The proposed method is based on a multi-threaded industrial control computer, using multiple threads to simultaneously receive measurements from different sensors. Each thread processes and adds the measurements to the optimization process independently, with the reception of each measurement factor independent of the others. Meanwhile, the optimization thread runs continuously, waiting for callbacks from RGBD odometry or stereo vision odometry to activate different factor graphs for optimization. When an IMU measurement is received, only the IMU measurement result is added to the factor graph without triggering optimization. The update thread updates the overall state estimate with the results of the optimized horizontal factor graph. I x. Before running the optimization, newly arrived factors are copied to the graph to be optimized. Once the optimization is complete, the last optimized state is propagated to the latest IMU state using the obtained IMU bias, similar to the update step in a filter-based method. Note that this graph-based approach inherently allows delay measurements to be integrated into the smoothing operation at each time step.

[0134] Step Six: Two-Factor Plot Design

[0135] The system application environment addresses structural degradation caused by external influences such as light, leading to RGBD sensor loss. A two-factor graph was designed to handle sensor loss and improve system fault tolerance. First, each thread processes the data independently and adds it to the optimization process. The reception of each measurement factor is independent, and the loss of one factor does not affect the overall result. Each measurement is processed and added to the optimization process in a separate thread. The two-factor graph design consists of two aspects: suppressing outliers and recovering from inconsistencies in localization.

[0136] Outlier suppression:

[0137] This method designs two parallel factor graphs. When external influences are minimal and the RGBD odometry is running reliably, the main graph operates normally, and the stereo vision odometry factor is optimized as a relative odometry factor. At this time, the RGBD odometry factor serves as a global estimate for localization, and other factors are optimized based on this. When external conditions affect the RGBD odometry factor, making it unreliable, the method switches to the sub-graph. In this case, the stereo attitude univariate factor acts as a pseudo-global factor, replacing the RGBD odometry factor to maintain consistency with the airborne localization and mapping solution. Note that during the operation of the sub-graph, the stereo vision odometry factor is still added to the main graph as a relative odometry factor, but it is not used for state estimation or localization.

[0138] Recovering from location inconsistencies:

[0139] After the RGBD odometry is restored, the system switches back to the main image. The RGBD odometry needs to be restored based on the stereo vision odometry measurements and IMU readings. To minimize positional deviations in the sub-image due to cumulative drift caused by IMU and stereo vision odometry factors, the system estimates the relative drift between the global coordinate system and the odometry coordinate system during the loss period by calculating the RGBD interruption time, T. WO Updated to:

[0140] T WO =T WI ·(T OI ) -1

[0141] Among them, T OI The estimated value, T, is in the subplot. WI When the sub-map is enabled, it is provided by the main map. When the main map is restored, the sub-map is reset to the state of the main map.

[0142] Figure 3 The process of running a two-factor graph is given.

[0143] The outdoor multi-vision sensor positioning system provided by this invention, which implements the above-mentioned outdoor multi-vision sensor fusion positioning method based on two-factor graphs, includes:

[0144] Initialization module: Initializes the IMU pose as the starting pose. Regardless of the initial pose relationship between the vehicle coordinate system and the horizontal plane, the default initial pose is horizontal. Subsequently, the system will make corrections and adjustments based on the relationship between visual positioning information and the default world coordinate system.

[0145] Acquisition Module: Acquires camera and IMU information. The RGBD camera acquires visual feature points with depth information and obtains RGBD odometry information through feature point transformation. The stereo camera calculates feature information through the parallax of the left and right cameras and solves the stereo vision odometry information. The IMU module collects angular velocity and linear acceleration as initial IMU data.

[0146] The computation module calculates the point cloud measurement model and IMU pre-integration. It constructs point feature measurement models from feature points acquired by the RGBD camera and stereo camera, respectively, and uses reprojection errors to build point feature measurement models as odometry factors for both cameras. By combining a multi-sampling method with the IMU pre-integration algorithm, it solves for the IMU pre-integration factors and applies them to state estimation in the predictor graph.

[0147] Evaluation module: In order to improve the system's fault tolerance and positioning accuracy, the usability of RGBD factors is evaluated. By detecting the number of feature points acquired by RGBD and the correspondence between feature points in two consecutive frames, it is determined whether the acquired RGBD visual information can be added to the factor map as an optimization factor for fusion positioning.

[0148] Fusion positioning module: Based on the evaluation results, different fusion threads are selected for factor map optimization. When the RGBD odometry is running reliably, the RGBD odometry and the stereo camera odometry are combined with IMU factors to perform fusion optimization positioning through the main map. When the RGBD odometry is lost due to external conditions, the system switches to the sub-map, and the stereo odometry is fused with IMU factors for positioning, while waiting for the RGBD odometry to be recovered.

Claims

1. An outdoor multi-visual sensor fusion localization method based on two-factor graphs, characterized in that, Includes the following steps: Step 1: Establish the relationship between the state model and the coordinate system; The established coordinate systems are defined as follows: global coordinate system W, basic coordinate system B, odometer coordinate system O, IMU coordinate system I, RGBD coordinate system R, and stereo vision coordinate system S; The mobile robot's state during the entire fusion localization process is as follows: ， in: ， It is the state of IMU coordinate system I relative to global coordinate system W, where It's a direction. It's about location. It's about location. These are two noise error quantities of the IMU. It is a coordinate transformation from the odometer coordinate system O to the global coordinate system W; ， This indicates the state of the base coordinate system B relative to the odometer coordinate system O; The IMU measurements at time points in the base coordinate system B and the IMU coordinate system I are denoted as follows: and Including linear acceleration and angular acceleration The RGBD odometer measurement value in the RGBD coordinate system R is... The measurement value of the stereo vision odometry in the stereo vision coordinate system S is Therefore, in A set of measurements at time represents ,in , ； Step 2: Construct an improved point cloud measurement model and an improved IMU pre-integration algorithm for the multi-factor graph model; The visual information of feature points input from the camera is processed to construct a point feature measurement model. The point feature error model is constructed using the distance difference from the projection point to the observation point, i.e., through the reprojection error. The improved IMU pre-integration algorithm is a manifold IMU pre-integration algorithm proposed for multi-factor graph models. It applies a multi-sampling algorithm to obtain more accurate pre-integration IMU measurements. Step 3: Define the IMU factor, RGBD odometry factor, and stereo vision measurement factor; IMU Factor: Improved manifold pre-integration discretizes continuous IMU measurements, obtaining discrete IMU state variables and creating state variable nodes in the factor graph. Since IMUs acquire data at the highest frequency, the IMU factor serves as an intermediate node, concatenated with other nodes to construct the entire factor graph. The error term is defined as: ； RGBD odometry factor: This is odometry information calculated using the RTABMAP method. When the RGBD odometry is operating reliably, this information is integrated into the optimization as a global position estimate, including the position residual. ， This represents the difference between the observed quantity and the actual quantity constructed from the point feature model. Representing the observed quantity, Indicates the actual quantity. This is due to reprojection error; Stereo vision measurement factor: Stereo vision odometry measurements are calculated using the binocular vision method described in ORB-SLAM3; Step 4: Solve for the robot's state estimate in the odometry reference frame using maximum a posteriori estimation for localization; From the given measurements, the maximum a posteriori estimate of all past IMU states: ， Represents state variables. Representing the observed quantity, The current state quantity is calculated using the maximum a posteriori estimate; Through maintenance Transformation to determine an accurate and locally consistent estimate of the robot's state in the odometry reference frame for localization; Optimal state estimation at time 1 The problem is transformed into solving for the maximum a posteriori (MAP) estimate of the system. Bayes' theorem is then used to solve for the MAP estimate: given the measured values, the solution is found for the state. The probability is estimated and the model is estimated; that is, given the system observations, the system state variables are solved to maximize this conditional probability. Step 5: Factor graph prediction update loop; By using the feature points and other information from the visual sensor as the likelihood distribution as the measurement model to update the prediction, and combining the advantages of filtering and optimization-based methods, a new factor graph prediction update loop design is proposed, which uses multiple threads to receive and process each factor in parallel. Step Six: Two-Factor Plot Design; Two-factor plot design involves two aspects: outlier suppression and recovery from inconsistencies in localization. The outlier suppression process in step six is ​​as follows: Two parallel factor graphs are designed. When the RGBD odometry is running reliably, the main graph operates normally, and the stereo vision odometry factor is optimized as a relative odometry factor. At this time, the RGBD odometry factor is used as a global estimate for localization, and the other factors are optimized based on this. When external conditions affect the RGBD odometry factor and make it unreliable, the system switches to the sub-graph. At this time, the stereo attitude univariate factor is used as a pseudo-global factor to replace the role of the RGBD odometry factor in order to maintain consistency with the airborne localization and mapping solution. During the operation of the sub-graph, the stereo vision odometry factor is still added to the main graph as a relative odometry factor, but it is not used for state estimation or localization purposes. The process of restoring the inconsistency in positioning in step six is ​​as follows: After the RGBD odometry is restored, the system switches back to the main image. The RGBD odometry needs to be restored based on the stereo vision odometry measurements and the IMU. The relative drift between the global coordinate system and the odometry coordinate system during the loss period is estimated by calculating the RGBD interruption time. Updated to: ， in, These are estimated values ​​in the subplot. When the sub-map is enabled, it is provided by the main map. When the main map is restored, the sub-map is reset to the state of the main map.

2. The outdoor multi-visual sensor fusion positioning method based on two-factor graphs according to claim 1, characterized in that, The specific process of constructing the improved point cloud measurement model in step two is as follows: Based on point features, given a camera frame Feature measurement of the k-th point in The reprojection error is then defined as: ， in It is a camera frame. The first observation of the characteristics of the middle The landmark points representing point features, and the inverse depth of the features. Also in camera frame Internal definition, Indicates transformation to camera frame The reprojection error, or position residual, is obtained from the points in the graph using this formula and added to the multi-factor graph as part of the camera model factors.

3. The outdoor multi-visual sensor fusion positioning method based on two-factor graphs according to claim 1, characterized in that, The specific process of the improved IMU pre-integration algorithm in step two is as follows: When an IMU measurement is received, the first step is to correct the IMU bias; then, a multi-sampling algorithm is used to achieve more accurate pre-integration IMU measurements and create an IMU pre-integration factor, which is then added to the factor graph. Before optimization, the pre-integration IMU measurement is first-order calibrated using the current IMU bias, and then the proposed improved manifold IMU pre-integration algorithm is applied to the navigation solution in the predictor graph. Angular velocity of the IMU in the base coordinate system B and acceleration The measured values ​​conform to the following equation: ， in, and These are the theoretical values ​​of angular velocity and acceleration. and It is the actual measured value of the sensor; It is the transfer matrix from the global coordinate system W to the basic coordinate system B; It is the constant gravity vector in the global coordinate system W; It is a slowly changing centrifugal force; It's white noise; By using an improved IMU pre-integration algorithm and a multi-sampling method, combined with IMU measurements, the relative motion between two timestamps i and j is obtained. , and As shown in the following formula: ， in The equivalent rotation vector is calculated based on the multisampling method: ， in and They are respectively and The angle increment in the middle; Based on the multi-sampling algorithm, considering velocity rotation error compensation and velocity oscillation compensation, the calculation is as follows: ， ， ， in It is rotational error compensation. Represents speed oscillation compensation; Initial values ​​provided by IMU pre-integration Used for matching operations with vision sensors.

4. The outdoor multi-visual sensor fusion positioning method based on two-factor graphs according to claim 1, characterized in that, The stereo vision measurement values ​​in step three are represented by two types of factors: (1) Stereo odometry factor: In global operation, the stereo odometry factor is used as a regular observation factor and is directly added to the main graph as a six-degree-of-freedom measurement factor; the difference between the odometry prediction relative transformation and the measurement model. Evaluation is performed on the subplot. When the RGBD odometry factor is operating reliably, this factor needs to be combined with the RGBD odometry factor to present a considerable global position to connect adjacent active factors. (2) Stereo pose univariate factor: In the absence of RGBD odometry observations, stereo odometry observations are transcribed into pseudo-global values, represented as a univariate pose factor; based on observability, and to better integrate with the IMU factor, the pose estimation of the stereo odometry is set with a higher covariance, similar to the stereo odometry factor. Errors are evaluated on the subplot.

5. The outdoor multi-visual sensor fusion positioning method based on two-factor graphs according to claim 1, characterized in that, The factor graph prediction update loop method in step five is based on a multi-threaded industrial control computer, using multiple threads to simultaneously receive measurement values ​​from different sensors; each thread processes and adds to the optimization separately, and the reception of each measurement factor does not affect each other; Meanwhile, the optimization thread continues to run, waiting for the RGBD odometry or stereo vision odometry callback to activate different factor maps for optimization; when an IMU measurement is received, the IMU measurement result is only added to the factor map without triggering optimization; the update thread updates the total state estimate with the result of the optimized horizontal factor map. Before running the optimization, the newly arrived factors are copied to the graph to be optimized; once the optimization is complete, the final optimized state is propagated to the latest IMU state using the obtained IMU bias.

6. An outdoor multi-vision sensor positioning system that implements the outdoor multi-vision sensor fusion positioning method based on two-factor graphs as described in any one of claims 1-5, characterized in that, include: Initialization module: Initializes the IMU pose as the starting pose. Regardless of the initial pose relationship between the vehicle coordinate system and the horizontal plane, the default initial pose is horizontal. The system will then make corrections and adjustments based on the relationship between the visual positioning information and the default world coordinate system. Acquisition Module: Acquires camera and IMU information. The RGBD camera acquires visual feature points with depth information and obtains RGBD odometry information through feature point transformation. The stereo camera calculates feature information through the parallax of the left and right cameras and solves the stereo vision odometry information. The IMU module collects angular velocity and linear acceleration as initial IMU data. The computation module calculates the point cloud measurement model and IMU pre-integration. It constructs point feature measurement models from the feature points acquired by the RGBD camera and the stereo camera, respectively. It uses the reprojection error to construct the point feature measurement models as odometry factors for the two cameras. It solves the IMU pre-integration factor by combining the multi-sampling method with the IMU pre-integration algorithm and applies it to the state estimation of the prediction factor map. Evaluation module: In order to improve the system's fault tolerance and positioning accuracy, the usability of RGBD factors is evaluated. By detecting the number of feature points collected by RGBD and the correspondence between feature points in two consecutive frames, it is determined whether the collected RGBD visual information can be added to the factor map as an optimization factor for fusion positioning. Fusion positioning module: Based on the evaluation results, different fusion threads are selected for factor map optimization. When the RGBD odometry is running reliably, the RGBD odometry and the stereo camera odometry are combined with IMU factors to perform fusion optimization positioning through the main map. When the RGBD odometry is lost due to external conditions, the system switches to the sub-map, and the stereo odometry is fused with IMU factors for positioning, while waiting for the RGBD odometry to be recovered.

Images