Learning-driven multi-sensor adaptive weighted synchronous positioning and mapping system and method

Abstract

The invention relates to a learning-driven multi-sensor adaptive weighted synchronous positioning and mapping system and method. The system comprises a data acquisition module; a data preprocessing module; a visual inertia processing module; a laser radar inertia processing module; a depth assistance and scale correction module; an observation quality statistics construction module; a learning-driven constraint weight distribution module; a multi-sensor factor graph optimization module; and a loopback identification and consistency checking module. By combining multi-source sensor information, high-precision positioning of the facility agricultural robot in different scenes is realized, and the problems of scene dependence and insufficient precision of existing multi-sensor fixed noise and fixed strategy schemes are solved.

Description

Technical Field

[0001] This invention relates to the field of sensing and localization technology for agricultural robots, and specifically to a learning-driven multi-sensor adaptive weighted synchronous localization and mapping system and method. Background Technology

[0002] With the rapid development of facility agriculture, the demand for automated and intelligent agricultural equipment is increasing. Facility agriculture robots typically rely on multiple sensors, such as vision, lidar, and inertial measurement units, for precise positioning and mapping. However, traditional multi-sensor fusion methods often employ fixed weighting strategies, failing to dynamically adjust the weights of each sensor according to environmental changes. This results in inconsistent positioning accuracy and robustness in complex environments such as low light, textureless conditions, or dynamic interference. Therefore, how to achieve adaptive weighting of sensors in variable agricultural environments to improve positioning and mapping accuracy has become a major technological challenge.

[0003] Chinese invention patent CN120668107A discloses a map construction method and system based on dynamic weighted fusion of laser and vision. The method includes the following steps: performing visual SLAM based on ORB-SLAM2 to obtain a sparse visual point cloud; performing laser SLAM based on Gmapping to generate a two-dimensional occupancy grid; projecting the visual point cloud onto the grid; calculating the semantic occupancy probability and geometric occupancy probability respectively; and then weighted fusion of the two to form a fused map. This existing patent's weighting strategy for laser and vision is mainly based on preset and heuristic rules at the probabilistic level, lacking a learnable online weight update strategy, thus presenting certain technical bottlenecks.

[0004] While existing technologies have addressed localization and mapping issues in multi-scenario operations to some extent, they still suffer from insufficient adaptive weight adjustment across multiple sensors. Particularly in complex scenarios such as facility agriculture, sensors like vision and LiDAR are prone to degradation, leading to unstable front-end estimations and accumulated back-end optimization errors. Therefore, the ability to perceive environmental changes and sensor degradation online to continuously obtain reliable pose estimations and map construction has become a key research challenge. Summary of the Invention

[0005] This invention provides a learning-driven multi-sensor adaptive weighted synchronous localization and mapping system and method to address the shortcomings of existing technologies that rely primarily on preset and heuristic rules at the probabilistic level for weighting laser and vision sensors, lacking a learnable online weight update strategy and resulting in insufficient adaptive weight adjustment across multiple sensors.

[0006] The first aspect of this invention provides a learning-driven multi-sensor adaptive weighted synchronous localization and mapping method, wherein the multi-sensors include a camera, an inertial measurement unit (IMU), and a lidar, and includes the following steps: S1. Synchronously acquire the raw data streams from the camera, IMU, and LiDAR. Perform time synchronization, intrinsic and extrinsic parameter calibration, denoising, and point cloud motion compensation on the raw data streams. Perform distortion correction on the point cloud based on the IMU. Output image frames aligned to a unified time base (image frames are the original carriers for visual feature extraction (corner points, optical flow)), IMU pre-integration sequences for pre-integration, and time-stamped point clouds and their initial poses (point clouds are the raw data for LiDAR feature extraction). S2. Corner detection is performed on the input image and the KLT optical flow method is used to establish the feature correspondence between adjacent frames. The BRIEF descriptor is calculated for loop closure retrieval and key frame information is retained. S3. Based on the sliding window, the visual reprojection residual and the IMU pre-integration residual are jointly optimized nonlinearly to estimate the pose, velocity and zero bias of the camera and IMU. S4. Use the temporal motion estimated by IMU to process the distortion of the point cloud, unify all sampling points to the reference time coordinate system, extract corner points and plane points according to local curvature, combine scan line sparsification and voxel downsampling to form a feature set, establish the correspondence between the features of the current frame in the local map through nearest neighbor search, construct point-line residuals and point-area residuals, and jointly establish the objective function of lidar and IMU pre-integration with IMU pre-integration. S5. Register the multi-frame point cloud to the camera coordinate system to form a depth map. Search the neighborhood along the ray for visual features and obtain the depth by fitting the local plane intersection when the geometric distribution threshold is met. Remove anomalies through geometric consistency and multi-frame consistency. S6. Calculate the number of effective features (successfully tracked features), average horizontal optical flow displacement, and average vertical optical flow displacement of each grid cell in the input image according to the R×C grid, normalize and flatten them into a visual statistical matrix; S7. The success rates of corner point matching and planar point matching in the statistical point cloud, the effective pixel coverage ratio and the standard deviation of the point-to-sensor distance distribution are used to obtain the laser geometric feature statistical vector, which is then normalized and flattened into a laser geometric feature statistical matrix. S8. The visual statistical matrix and the laser geometric feature statistical matrix are used together as the state vector and input into the TD3 deep reinforcement learning algorithm (Twin Delayed Deep Deterministic Policy Gradient). The output is a two-dimensional continuous action, which is mapped to visual constraint continuous weights and laser radar constraint continuous weights via softmax (an activation function commonly used in machine learning and deep learning). The reward function is the reciprocal of the relative pose error (RPE). The visual constraint continuous weights and laser radar constraint continuous weights are directly used as the weighting coefficients of the corresponding residual terms in the factor graph, dynamically adjusting the relative contributions of visual factors and laser radar factors in factor graph optimization; and S9. Construct a joint optimization objective that includes IMU pre-integration, weighted visual reprojection, and laser point-line and point-plane residuals. Weight the corresponding residuals according to the continuous weights of the visual constraints and the continuous weights of the lidar constraints, complete the sparse nonlinear least squares solution, and output the fused pose. The fused pose serves as the robot's localization result to support the robot's operation.

[0007] Furthermore, the raw data stream includes raw image data (i.e., input images) acquired by the camera to provide environmental visual texture information; raw motion data acquired by the IMU, including angular velocity (gyroscope data) and acceleration (accelerometer data), to capture the robot's own motion state; and raw point cloud data acquired by the lidar to provide environmental three-dimensional geometric structure information.

[0008] Furthermore, in step S3, the joint nonlinear optimization of the visual reprojection residual and the IMU pre-integration residual includes the following steps: S31. Let the current sliding window be... For each keyframe Define state: in, Let be the rotation matrix of the machine relative to the world coordinate system. It is the location of the IMU origin within the world system. It is the speed of the IMU in the world system. This is the zero bias vector of the gyroscope. This is the zero bias vector of the accelerometer; S32, Features Observation in frame i Establish visual reprojection residuals: in, For perspective projection, For the external parameters from the camera to the IMU, It is the pose from the IMU to the world in the i-th frame. It is a feature The three-dimensional position; S33, For adjacent keyframes Constructing the IMU pre-integration residual: in, For perspective projection, and For two frames of pose, and It's two frames per second. and It's two frame positions. It is a time interval between two frames. Let be the world-system gravity vector. For IMU pre-integration rotation measurement from keyframe i to j, For IMU pre-integration velocity increment measurement from keyframe i to j, For IMU pre-integration displacement measurement from keyframe i to j, For pre-integral rotation measurement of gyroscope zero bias The first-order Jacobian, For pre-integral velocity measurement, zero bias of the gyroscope The first-order Jacobian, For pre-integral velocity measurement, zero bias of the accelerometer The first-order Jacobian, For pre-integral displacement measurement, zero bias of the gyroscope The first-order Jacobian, For pre-integral displacement measurement, zero bias of accelerometer The first-order Jacobian, It is the zero-bias increment of the gyroscope relative to the linearization point. It is the zero bias increment relative to the linearization point; S34. Construct a joint optimization objective function using vision and IMU pre-integration: in, The set of variables to be optimized For the set of observed "frame-feature" pairs, It is the visual observation covariance. It is a visual residual robust kernel. It is a set of IMU edges. It is the IMU pre-integral covariance. It is an IMU residual robust core. and To marginalize prior residuals and covariance, It is a feature set with deep priors. It is the depth prior weight. It is a deep residual robust kernel. It is to optimize the estimated depth. It is the prior depth derived from the point cloud and depth map; and S35. By solving the problem through sparse nonlinear least squares iteration and marginalizing the oldest keyframe, the historical constraints are compressed into prior factors that are rolled and passed within the sliding window. This not only obtains the optimal state solution at the current moment, but also maintains information accumulation without increasing the state size, thereby satisfying the real-time performance and robustness of online SLAM (Online Simultaneous Localization and Mapping), and providing consistent priors and initial values ​​for the cross-modal weighted fusion in steps S4 and S8.

[0009] Furthermore, in step S4, the construction of point-line residuals and point-surface residuals, and the establishment of a joint optimization objective function for lidar and IMU pre-integration together with IMU pre-integration, includes the following steps: S41. After IMU distortion correction and unification to the reference time coordinate system, transform the laser point from the laser system to the world system. The coordinates of the k-th laser point in the i-th frame in the world coordinate system are... Represented as: in, These are the coordinates of the point in the lidar coordinate system. Let L be the rotation matrix from the lidar coordinate system L to the IMU coordinate system B. The coordinates of the lidar origin in IMU coordinate system B; S42, Constructing point-line geometric residuals from diagonal points : in, and These are the two adjacent line feature points corresponding to the corner point in the local map; S43. Construct point-plane geometric residuals for points on a plane. : in, It is the unit normal vector corresponding to the local plane. Let be a reference point on the plane; and S44. Construct a joint optimization objective function for lidar and IMU pre-integration: in, It is a set of corner indexes. For a set of plane point indices, For robust kernels of point-to-line residuals, For robust kernels of point-to-surface residuals, For point-to-line residual covariance, The variance is the point-to-surface residual.

[0010] Further, in step S6, the step of statistically analyzing the effective feature count, average horizontal optical flow displacement, and average vertical optical flow displacement of each grid cell in the input image according to an R×C grid to obtain a visual statistical matrix includes the following steps: S61. Divide the i-th frame image into an R×C grid, and count the number of effective features and the average horizontal optical flow displacement and the average vertical optical flow displacement in each grid cell: in, For the first One grid cell, This represents the average horizontal optical flow displacement of the grid cell. This represents the average vertical optical flow displacement of the grid cell. and The normalized coordinate difference of the same feature between two frames; S62. Normalize the statistical values ​​of the grid cells according to the global maximum value of the current frame to form a three-channel visual statistical matrix: in, This indicates that the maximum value is taken across all grid cells in the frame. c >0 indicates a numerically stable term; and S63. Flatten the visual statistics matrix into a visual state vector in a fixed order for use by the policy network: in, It is a flattening operator.

[0011] Furthermore, the R×C grid is determined based on the aspect ratio of the input image. For example, in the KITTI dataset, the resolution is 1241x376, and the aspect ratio is approximately 3.3:1. Therefore, R is set to 12, and C is set to 4. This avoids the grid being too dense or too sparse.

[0012] Further, in step S7, the statistical point cloud's corner matching success rate, planar point matching success rate, effective pixel coverage ratio, and standard deviation of the point-to-sensor distance distribution are used to obtain a laser geometric feature statistical vector, including the following steps: S71. Under the correspondence between the current frame point cloud and the local map, calculate the matching success rate between corner points and planar points: in, The number of corner points participating in the matching. The number of planar points participating in the matching. and The number of successful geometric consistency checks for corner points and planar points. o >0 indicates a numerically stable term; S72. Project the point cloud onto a depth map with a resolution of R×C, and calculate the coverage ratio and the standard deviation of the distance distribution: in, It is the effective pixel coverage ratio. The standard deviation of the distance from the point to the sensor; and S73. Normalize the above statistics and concatenate them into a laser geometric feature state vector: in, It is the maximum value or preset size of the sliding window. >0 indicates a numerically stable term.

[0013] Further, in S8, the state vector is input into the TD3 deep reinforcement learning algorithm to output visual and laser weights, and these weights are directly applied to the factor graph residuals to form an overall weighted objective and iteratively optimized, including the following steps; S81, Build Status The policy network outputs two-dimensional actions. : in, For TD3 policy network, and It outputs two-dimensional continuous motion, corresponding to the vision and LiDAR channels respectively; S82. Map actions to visual weights using softmax. With laser weight : in, ; S83. Merge the two objectives based on their weights to form the overall backend objective function: S84. Use the reciprocal of the relative pose error (RPE) as the immediate reward: in, For numerically stable terms, and To minimize The estimated pose is obtained. and For reference relative pose, To evaluate the span, For the logarithmic mapping of the Lie group, It is a weighted norm; S85, Record at every moment And write to the playback buffer, This serves as a round termination flag (e.g., in case of reinitialization / large drift); the replay buffer is used for subsequent sampling training of Critic and Actor. S86. Add truncated Gaussian noise to the policy output of the next state to smooth the target action; use two Critic estimates to estimate the target Q value and take the smaller one to suppress overestimation, with discount factor and sampling batch size set according to implementation. S87. First, minimize the mean square error of the two Critic paths and update the value network; then update the Actor at a low frequency according to the "delayed update strategy"; finally, update the coefficients softly. Soft updates are performed on the target network of the Actor and the two Critics to ensure convergence stability; and S88, Output again using the updated strategy. and And minimize the overall objective Obtain the optimal solution for the current window; perform marginalization on the oldest keyframe to generate priors. Pass it to the next window.

[0014] Furthermore, in S9, sparse nonlinear least squares with robust kernel solution is used to output the fused pose. Loop closure retrieval is performed based on DBoW2 (Bag of Words 2) and BRIEF (Binary Robust Independent Elementary Features). Geometric consistency is checked using PnP (Perspective-n-Point) combined with RANSAC (Random Sample Consensus). The loop closure factors that pass are added to the graph to complete global consistency correction.

[0015] Furthermore, the robot is a facility agriculture robot.

[0016] A second aspect of the present invention provides a learning-driven multi-sensor adaptive weighted synchronous localization and mapping system, comprising: The data acquisition module is configured to simultaneously acquire raw data streams from the camera, IMU, and LiDAR; The data preprocessing module is configured to preprocess the raw data stream and output image frames aligned to a unified time base, IMU pre-integration sequences for pre-integration, time-stamped point clouds and their initial poses to solve the problems of data asynchrony, inconsistent formats and noise interference from multiple sensors, and to provide standardized and highly reliable basic data input for all subsequent modules. The visual inertial processing module is configured to tightly couple and optimize the visual reprojection constraints and IMU pre-integration constraints of image features within a sliding window, estimate the pose, velocity and zero bias of the camera and IMU, establish stable correspondences of adjacent frame features using corner detection and KLT optical flow at the front end, calculate the BRIEF descriptor and participate in candidate retrieval during the loop closure stage, trigger reinitialization when the number of effective tracked features is lower than the threshold or the zero bias is out of bounds, and receive scale and depth priors provided by the depth assistance and scale correction module. The lidar inertial processing module is configured to perform distortion correction on the point cloud based on the temporal motion estimated by the IMU, extract corner and planar point features, unify all sampling points to the reference time coordinate system, divide corner and planar points based on local curvature, and obtain a feature set with high information content and controllable scale through scan line sparsification and voxel downsampling. The features of the current frame are used to establish the correspondence in the local map through nearest neighbor search, construct point-line residuals and point-area residuals, and enter the sliding window optimization together with IMU pre-integration. The depth assistance and scale correction module is configured to generate a depth map (registering multiple frames of laser point clouds to the camera coordinate system to form a sparser but denser depth map), acquire visual feature depth information and remove outliers (searching for the neighborhood of visual features along their rays in the point cloud, and fitting a local plane to find the intersection when the geometric distribution threshold is met to obtain the depth, and removing outliers through geometric consistency and multi-frame consistency), and provide depth priors to the visual inertial module and back-end optimization when the system initializes and scale drift is detected; The observation quality statistics construction module is configured to convert the visual and lidar observation data from the visual inertial processing module and the lidar inertial processing module into stable, low-dimensional statistical vectors (the lidar geometric feature state vector is flattened into a one-dimensional array and concatenated with the visual feature vector before being input into the TD3 model). This summarizes information such as scene texture, motion, and spatial structure. Feature compression and normalization are completed through regional statistics and reliability measurement of images and point clouds. The module outputs a visual statistical matrix and a lidar geometric feature statistical matrix, and uses the visual statistical matrix and the lidar geometric feature statistical matrix together as the state input of the deep reinforcement learning algorithm. The learning-driven constraint weight allocation module is configured to take the visual statistical matrix and the laser geometric feature statistical matrix as input, and output the continuous weights of visual constraints and laser radar constraints online based on the TD3 deep reinforcement learning algorithm. The weights are then directly applied to the residual weighting terms of the factor graph to achieve adaptive fusion that changes with the environment. A multi-sensor factor map optimization module is configured to construct a joint optimization objective from IMU pre-integration, visual reprojection, and laser point-line and point-surface constraints within a unified factor map. It weights the corresponding residuals based on the continuous weights of the visual and laser radar constraints to achieve adaptive adjustment of modal contributions. A robust kernel is used to complete sparse nonlinear least squares solutions, and global consistency correction is performed when a closure factor is added. The module outputs a fused pose, which serves as the robot's localization result to support its operations. The robot in this case is a facility agriculture robot. The loop closure identification and consistency verification module is configured to use the DBoW2 bag-of-words model and BRIEF descriptors to retrieve loop closure candidates, and to complete the screening based on similarity thresholds and time constraints; and to use PnP-based pose estimation in conjunction with RANSAC for geometric consistency verification, and output the loop closure factor when the verification is successful.

[0017] Furthermore, the image frame is used for feature processing and optical flow tracking in the visual inertial processing module, and further to construct a visual statistical matrix in the observation quality statistics construction module; and in the loop closure recognition and consistency verification module, the image frame is used to calculate the BRIEF descriptor; the depth assistance and scale correction module needs to register the laser point cloud to the camera coordinate system, and the image frame provides accurate camera pose, which is a prerequisite for completing this coordinate transformation.

[0018] Furthermore, the IMU pre-integration sequence is used by the visual inertial processing module, together with the visual reprojection constraint, to construct the joint optimization objective function of visual and IMU pre-integration in S34; the IMU pre-integration sequence is also used by the lidar inertial processing module, where the point-to-line residuals and point-to-surface residuals, together with the IMU pre-integration, enter the sliding window optimization to construct the joint optimization objective function of lidar and IMU pre-integration in S44; the IMU pre-integration sequence is also used by the multi-sensor factor graph optimization module, as an IMU pre-integration factor to be optimized together with other factors.

[0019] Furthermore, the time-stamped point cloud is used by the lidar inertial processing module to perform distortion correction on the point cloud and extract corner and planar point features.

[0020] Furthermore, the initial pose provides initial values ​​for the motion model of laser point cloud distortion correction; during reinitialization, the visual inertial processing module can use the initial pose as prior information for the pose.

[0021] Furthermore, the visual statistical matrix is ​​constructed by the visual inertial module after feature extraction and optical flow tracking are completed. The generation process of the visual statistical matrix includes: The input image is divided into R×C grid cells; The number of effective features, the average horizontal optical flow displacement, and the average vertical optical flow displacement of each grid cell are counted. When a grid cell has no effective features, all three statistics are recorded as zero. The statistics of the grid cells are normalized to their intra-frame maximum values ​​to form a visual statistical tensor of size (R, C, 3); and The visual statistics tensor is flattened into a one-dimensional vector, which is then used as the visual statistics matrix.

[0022] Furthermore, the laser geometric feature statistical matrix is ​​constructed by the lidar inertial module after distortion correction, feature extraction, and matching. The generation process of the laser geometric feature statistical matrix includes: The success rates of corner point matching and planar point matching are statistically analyzed to characterize the degree of utilization of point-line constraints and point-plane constraints. The point cloud is projected onto a depth map with a resolution of R×C, and the effective pixel coverage ratio is calculated to characterize the observation integrity. Calculate the standard deviation of the distance from the point to the sensor to characterize the geometric hierarchy and complexity of the scene; and The corner point matching success rate, plane point matching success rate, effective pixel coverage ratio, and standard deviation of the distance from the point to the sensor are normalized and anomaly removed, and then concatenated in a fixed order into a fixed-length vector, which serves as the laser geometric feature statistical matrix.

[0023] Furthermore, the preprocessing includes time synchronization, intrinsic and extrinsic parameter calibration and basic denoising, and motion compensation preparation for line-by-line scan point clouds based on short-term IMU motion.

[0024] Furthermore, the TD3 deep reinforcement learning algorithm takes the state vector obtained by concatenating the visual statistical matrix and the laser geometric feature statistical matrix as input. The policy network outputs a two-dimensional continuous action at each time step, which is mapped to visual constraint weights and laser constraint weights through softmax. These weights are then directly applied to the corresponding residual terms in the unified factor graph. Under the given weights, a nonlinear least squares solution is performed on the sliding window target to obtain the current trajectory. An instant reward is then constructed using the relative pose error over a preset time span and written to the replay buffer. During training, double Q-pruning, delayed policy updates, and target policy smoothing are employed to minimize the mean square error of the two value networks and inject truncated Gaussian noise into the action on the target path. Soft updates are performed on the target networks of policy and value to ensure convergence stability. During online deployment, only forward inference is performed to output weights in real time, achieving adaptive fusion that changes with the environment.

[0025] This invention has at least the following beneficial effects: 1) This invention proposes a learning-driven multi-sensor adaptive weighted synchronous localization and mapping system and method. By combining multi-source sensor information, it achieves high-precision localization of facility agriculture robots in different scenarios, overcoming the problems of scenario dependence and insufficient accuracy of existing multi-sensor fixed noise and fixed strategy schemes; 2) This invention tightly couples IMU pre-integration, visual reprojection constraints including visual weights, and laser point-line and point-surface geometric constraints including lidar weights, as well as closure factors, within a unified factor graph framework. It also incorporates depth-assisted and scale-corrected modules, as well as PnP and R... Mechanisms such as ANSAC consistency verification, through collaborative constraints and global consistency correction at the residual level, achieve scale-robust and information-free pose and map joint optimization, significantly suppressing cumulative drift and improving global accuracy and consistency in degraded scenarios such as low texture and sparse structure; 3) This invention constructs a visual observation statistical matrix and a laser geometric feature statistical vector as state input, and uses a TD3 strategy network to output continuous visual and laser weights online and directly apply them to the residual terms of the factor graph to achieve adaptive weighted fusion, which improves cross-scene positioning accuracy and overcomes the accuracy fluctuation problem caused by the performance differences of different sensors in the prior art. Attached Figure Description

[0026] To further illustrate the above and other advantages and features of the various embodiments of the present invention, a more specific description of the embodiments of the invention will be presented with reference to the accompanying drawings. It is to be understood that these drawings depict only typical embodiments of the invention and are therefore not intended to limit its scope. In the drawings, identical or corresponding parts will be indicated by identical or similar reference numerals for clarity.

[0027] Figure 1 This diagram illustrates a learning-driven multi-sensor adaptive weighted synchronous localization and mapping system framework in some embodiments of the present invention. Figure 2 The diagram illustrates a multi-sensor factor map fusion framework implemented in some embodiments of the present invention. Figure 3 The diagram illustrates the implementation of the TD3 deep reinforcement learning algorithm in some embodiments of the present invention. Figure labels: 1-Data acquisition module, 2-Data preprocessing module, 3-Visual inertial processing module, 4-LiDAR inertial processing module, 5-Depth assistance and scale correction module, 6-Observation quality statistics construction module, 7-Learning-driven constraint weight allocation module, 8-Multi-sensor factor graph optimization module, 9-Loop closure identification and consistency verification module. Detailed Implementation

[0028] It should be noted that the components in the accompanying drawings may be shown exaggerated for illustrative purposes and may not be to scale.

[0029] In this invention, the various embodiments are merely intended to illustrate the solutions of the invention and should not be construed as limiting.

[0030] In this invention, unless otherwise specified, the quantifiers “a” and “one” do not exclude scenarios involving multiple elements.

[0031] It should also be noted that, in the embodiments of the present invention, only a portion of the parts or components may be shown for clarity and simplicity. However, those skilled in the art will understand that, under the teachings of the present invention, the required parts or components can be added as needed for specific scenarios.

[0032] It should also be noted that within the scope of this invention, the terms "same", "equal", and "equal to" do not mean that the two values ​​are absolutely equal, but allow for a certain reasonable error. In other words, the terms also cover "substantially the same", "substantially equal", and "substantially equal to".

[0033] It should also be noted that in the description of this invention, the terms "center," "longitudinal," "lateral," "upper," "lower," "front," "rear," "left," "right," "vertical," "horizontal," "top," "bottom," "inner," and "outer," etc., indicate the orientation or positional relationship based on the orientation or positional relationship shown in the accompanying drawings. They are used only for the convenience of describing the invention and for simplifying the description, and do not explicitly or implicitly suggest that the device or element referred to must have a specific orientation, or be constructed and operated in a specific orientation. Therefore, they should not be construed as limitations on the invention. Furthermore, the terms "first" and "second" are used for descriptive purposes only and should not be construed as indicating or implying relative importance.

[0034] Furthermore, the embodiments of the present invention describe the process steps in a specific order. However, this is only for the convenience of distinguishing each step, and is not a limitation on the order of each step. In different embodiments of the present invention, the order of each step can be adjusted according to the process.

[0035] The robot in the following examples is a facility agriculture robot.

[0036] The following embodiment provides a learning-driven multi-sensor adaptive weighted synchronous localization and mapping method, including the following steps: S1. Synchronously acquire the raw data streams from the camera, IMU, and LiDAR, perform time synchronization, intrinsic and extrinsic parameter calibration, denoising, and point cloud motion compensation on the raw data streams, perform distortion correction on the point cloud based on the IMU, and output image frames aligned to a unified time base, IMU pre-integration sequences for pre-integration, and point clouds with timestamps and their initial poses. S2. Corner detection is performed on the input image and the KLT optical flow method is used to establish the feature correspondence between adjacent frames. The BRIEF descriptor is calculated for loop closure retrieval and key frame information is retained. S3. Based on the sliding window, the visual reprojection residual and the IMU pre-integration residual are jointly optimized nonlinearly to estimate the pose, velocity and zero bias of the camera and IMU. S31. Let the current sliding window be... For each keyframe Define state: in, Let be the rotation matrix of the machine relative to the world coordinate system. It is the location of the IMU origin within the world system. It is the speed of the IMU in the world system. This is the zero bias vector of the gyroscope. This is the zero bias vector of the accelerometer; S32, Features Observation in frame i Establish visual reprojection residuals: in, For perspective projection, For the external parameters from the camera to the IMU, It is the pose from the IMU to the world in the i-th frame. It is a feature The three-dimensional position; S33, For adjacent keyframes Constructing the IMU pre-integration residual: in, For perspective projection, and For two frames of pose, and It's two frames per second. and It's two frame positions. It is a time interval between two frames. Let be the world-system gravity vector. For IMU pre-integration rotation measurement from keyframe i to j, For IMU pre-integration velocity increment measurement from keyframe i to j, For IMU pre-integration displacement measurement from keyframe i to j, For pre-integral rotation measurement of gyroscope zero bias The first-order Jacobian, For pre-integral velocity measurement, zero bias of the gyroscope The first-order Jacobian, For pre-integral velocity measurement, zero bias of the accelerometer The first-order Jacobian, For pre-integral displacement measurement, zero bias of the gyroscope The first-order Jacobian, For pre-integral displacement measurement, zero bias of accelerometer The first-order Jacobian, It is the zero-bias increment of the gyroscope relative to the linearization point. It is the zero bias increment relative to the linearization point; S34. Construct a joint optimization objective function using vision and IMU pre-integration: in, The set of variables to be optimized For the set of observed "frame-feature" pairs, It is the visual observation covariance. It is a visual residual robust kernel. It is a set of IMU edges. It is the IMU pre-integral covariance. It is an IMU residual robust core. and To marginalize prior residuals and covariance, It is a feature set with deep priors. It is the depth prior weight. It is a deep residual robust kernel. It is to optimize the estimated depth. It is the prior depth derived from the point cloud and depth map; and S35. By solving the problem through sparse nonlinear least squares iteration and marginalizing the oldest keyframe, the historical constraints are compressed into prior factors that are rolled and passed within the sliding window. This not only obtains the optimal state solution at the current moment, but also maintains information accumulation without increasing the state size, thereby satisfying the real-time performance and robustness of online SLAM, and providing consistent priors and initial values ​​for the cross-modal weighted fusion in steps S4 and S8.

[0037] S4. Use the temporal motion estimated by IMU to process the distortion of the point cloud, unify all sampling points to the reference time coordinate system, extract corner points and plane points according to local curvature, combine scan line sparsification and voxel downsampling to form a feature set, establish the correspondence between the features of the current frame in the local map through nearest neighbor search, construct point-line residuals and point-area residuals, and jointly establish the objective function of lidar and IMU pre-integration with IMU pre-integration. S41. After IMU distortion correction and unification to the reference time coordinate system, transform the laser point from the laser system to the world system. The coordinates of the k-th laser point in the i-th frame in the world coordinate system are... Represented as: in, These are the coordinates of the point in the lidar coordinate system. Let L be the rotation matrix from the lidar coordinate system L to the IMU coordinate system B. The coordinates of the lidar origin in IMU coordinate system B; S42, Constructing point-line geometric residuals from diagonal points : in, and These are the two adjacent line feature points corresponding to the corner point in the local map; S43. Construct point-plane geometric residuals for points on a plane. : in, It is the unit normal vector corresponding to the local plane. Let be a reference point on the plane; and S44. Construct a joint optimization objective function for lidar and IMU pre-integration: in, It is a set of corner indexes. For a set of plane point indices, For robust kernels of point-to-line residuals, For robust kernels of point-to-surface residuals, For point-to-line residual covariance, The variance of the point-to-surface residuals; S5. Register the multi-frame point cloud to the camera coordinate system to form a sparser but denser depth map. Search the neighborhood along the ray for visual features and fit the local plane to find the intersection when the geometric distribution threshold is met to obtain the depth. Remove anomalies through geometric consistency and multi-frame consistency. S6. Calculate the number of effective features, average horizontal optical flow displacement and average vertical optical flow displacement of each grid cell in the input image according to the R×C grid, normalize and flatten it into a visual statistical matrix. S61. Divide the i-th frame image into an R×C grid, and count the number of effective features and the average horizontal optical flow displacement and the average vertical optical flow displacement in each grid cell: in, For the first One grid cell, This represents the average horizontal optical flow displacement of the grid cell. This represents the average vertical optical flow displacement of the grid cell. and The normalized coordinate difference of the same feature between two frames; S62. Normalize the statistical values ​​of the grid cells according to the global maximum value of the current frame to form a three-channel visual statistical matrix: in, This indicates that the maximum value is taken across all grid cells in the frame. c >0 indicates a numerically stable term; and S63. Flatten the visual statistics matrix into a visual state vector in a fixed order for use by the policy network: in, It is a flattening operator; S7. The success rates of corner point matching and planar point matching in the statistical point cloud, the effective pixel coverage ratio and the standard deviation of the point-to-sensor distance distribution are used to obtain the laser geometric feature statistical vector, which is then normalized and flattened into a laser geometric feature statistical matrix. S71. Under the correspondence between the current frame point cloud and the local map, calculate the matching success rate between corner points and planar points: in, The number of corner points participating in the matching. The number of planar points participating in the matching. and The number of successful geometric consistency checks for corner points and planar points. o >0 indicates a numerically stable term; S72. Project the point cloud onto a depth map with a resolution of R×C, and calculate the coverage ratio and the standard deviation of the distance distribution: in, It is the effective pixel coverage ratio. The standard deviation of the distance from the point to the sensor; and S73. Normalize the above statistics and concatenate them into a laser geometric feature state vector: in, It is the maximum value or preset size of the sliding window. >0 indicates a numerically stable term; S8. The visual statistical matrix and the laser geometric feature statistical matrix are used as the state vector input to the TD3 deep reinforcement learning algorithm. The output is a two-dimensional continuous action, which is mapped to visual constraint continuous weights and laser radar constraint continuous weights through softmax. The reward function is the reciprocal of the relative pose error. The visual constraint continuous weights and laser radar constraint continuous weights are directly used as the weighting coefficients of the corresponding residual terms in the factor graph, and the relative contributions of the visual factor and the laser radar factor in the factor graph optimization are dynamically adjusted. S81, Build Status The policy network outputs two-dimensional actions. : in, For TD3 policy network, and It outputs two-dimensional continuous motion, corresponding to the vision and LiDAR channels respectively; S82. Map actions to visual weights using softmax. With laser weight : in, ; S83. Merge the two objectives based on their weights to form the overall backend objective function: S84. Use the reciprocal of the relative pose error (RPE) as the immediate reward: in, For numerically stable terms, and To minimize The estimated pose is obtained. and For reference relative pose, To evaluate the span, For the logarithmic mapping of the Lie group, It is a weighted norm; S85, Record at every moment And write to the playback buffer, This serves as a round termination flag (e.g., in case of reinitialization / large drift); the replay buffer is used for subsequent sampling training of Critic and Actor. S86. Add truncated Gaussian noise to the policy output of the next state to smooth the target action; use two Critic estimates to estimate the target Q value and take the smaller one to suppress overestimation, with discount factor and sampling batch size set according to implementation. S87. First, minimize the mean square error of the two Critic paths and update the value network; then update the Actor at a low frequency according to the "delayed update strategy"; finally, update the coefficients softly. Soft updates are performed on the target network of the Actor and the two Critics to ensure convergence stability; and S88, Output again using the updated strategy. and And minimize the overall objective Obtain the optimal solution for the current window; perform marginalization on the oldest keyframe to generate priors. Pass to the next window; and S9. Construct a joint optimization objective that includes IMU pre-integration, weighted visual reprojection, and laser point-line and point-plane residuals. Weight the corresponding residuals according to the continuous weights of visual constraints and lidar constraints, complete the sparse nonlinear least squares solution, and output the fused pose. The fused pose serves as the robot's localization result to support robot operations. In this process, sparse nonlinear least squares with robust kernels are used for solving the problem. After outputting the fused pose, loop closure is performed based on DBoW2 and BRIEF. Geometric consistency is checked using PnP combined with RANSAC. The loop closure factors that pass are added to the graph to complete global consistency correction.

[0038] The following embodiments provide a learning-driven multi-sensor adaptive weighted synchronous localization and mapping system. Figure 1 This diagram illustrates the framework of a learning-driven multi-sensor adaptive weighted simultaneous localization and mapping (SMR) system, which includes: Data acquisition module 1 is configured to simultaneously acquire raw data streams from the camera, IMU, and lidar; Data preprocessing module 2 is configured to preprocess the raw data stream and output image frames aligned to a unified time base, IMU pre-integration sequences for pre-integration, time-stamped point clouds and their initial poses to solve the problems of data asynchrony, inconsistent formats and noise interference from multiple sensors, and to provide standardized and highly reliable basic data input for all subsequent modules. The visual inertial processing module 3 is configured to tightly couple and optimize the visual reprojection constraints and IMU pre-integration constraints of image features within a sliding window, estimate the pose, velocity and zero bias of the camera and IMU, establish stable correspondence of adjacent frame features using corner detection and KLT optical flow at the front end, calculate the BRIEF descriptor and participate in candidate retrieval during the loop closure stage, trigger reinitialization when the number of effective tracked features is lower than the threshold or the zero bias is out of bounds, and receive scale and depth priors provided by the depth assistance and scale correction module 5. The lidar inertial processing module 4 is configured to perform distortion correction on the point cloud based on the temporal motion estimated by the IMU, extract corner and planar point features, unify all sampling points to the reference time coordinate system, divide corner and planar points based on local curvature, and obtain a feature set with high information content and controllable scale through scan line sparsification and voxel downsampling. The features of the current frame are used to establish a correspondence in the local map through nearest neighbor search, construct point-line residuals and point-area residuals, and enter sliding window optimization together with IMU pre-integration. The depth assistance and scale correction module 5 is configured to generate a depth map (registering multiple frames of laser point clouds to the camera coordinate system to form a sparser but denser depth map), acquire visual feature depth information and remove outliers (searching for the neighborhood of visual features along their rays in the point cloud, and fitting a local plane to find the intersection when the geometric distribution threshold is met to obtain the depth, and removing outliers through geometric consistency and multi-frame consistency), and provide depth priors to the visual inertial module and back-end optimization when the system initializes and scale drift is detected; The observation quality statistics construction module 6 is configured to convert the visual and lidar observation data from the visual inertial processing module 3 and the lidar inertial processing module 4 into stable, low-dimensional statistical vectors, summarizing information such as scene texture, motion and spatial structure. It completes feature compression and normalization through regional statistics and reliability measurement of images and point clouds, outputs visual statistical matrix and lidar geometric feature statistical matrix, and uses the visual statistical matrix and lidar geometric feature statistical matrix together as the state input of the deep reinforcement learning algorithm. The learning-driven constraint weight allocation module 7 is configured to take the visual statistical matrix and the laser geometric feature statistical matrix as inputs, based on the TD3 deep reinforcement learning algorithm. Figure 3 (A schematic diagram of implementing the TD3 deep reinforcement learning algorithm is shown.) The continuous weights of visual constraints and lidar constraints are output online, and the weights are directly applied to the residual weights of the factor graph to achieve adaptive fusion that changes with the environment. The multi-sensor factor map optimization module 8 is configured to construct a joint optimization objective from IMU pre-integration, visual reprojection, and laser point-line and point-surface constraints in a unified factor map. It weights the corresponding residuals based on the continuous weights of visual and lidar constraints to achieve adaptive adjustment of modal contributions. A robust kernel is used to complete sparse nonlinear least squares solutions, and global consistency correction is performed when a closure factor is added. The module outputs a fused pose, which serves as the robot's localization result to support its operations. The robot in this module is a facility agriculture robot. The loop closure identification and consistency verification module 9 is configured to use the DBoW2 bag-of-words model and BRIEF descriptors to retrieve loop closure candidates, and to complete the screening based on similarity thresholds and time constraints; and to use PnP-based pose estimation in conjunction with RANSAC for geometric consistency verification, and output the loop closure factor when the verification is successful. Figure 2 This diagram illustrates a multi-sensor factor graph fusion framework. Under a unified factor graph framework, IMU pre-integration, visual reprojection constraints with visual weights, and LiDAR point-to-line and point-to-surface geometric constraints with LiDAR weights, along with closure factors, are tightly coupled and modeled. Through collaborative constraints and global consistency correction at the residual level, scale-robust and information-free pose and map joint optimization is achieved. The system's data flow originates from data acquisition module 1 and data preprocessing module 2. These modules simultaneously acquire raw data streams from the camera, IMU, and LiDAR, outputting aligned image frames, IMU pre-integration sequences, timestamped point clouds, and initial poses. These data are input to visual-inertial processing module 3 and LiDAR-inertial processing module 4 for front-end pose estimation. Visual-inertial processing module 3 performs tight-coupled optimization using the IMU pre-integration sequence, while LiDAR-inertial processing module 4 uses IMU data to distort the point cloud and extract features. Simultaneously, a depth-assisted and scale-corrected module receives point cloud and image data to generate depth... The vision module is used to address scale issues. The observation outputs of the vision-inertial processing module and the lidar-inertial processing module 4 are fed into the observation quality statistics construction module 6. This module compresses feature tracking information and geometric features into a one-dimensional statistical vector and uses it as a state input to the learning-driven constraint weight allocation module 7. The latter dynamically outputs adaptive weights for vision and lidar constraints through TD3 deep reinforcement learning. These weights are ultimately applied to the multi-sensor factor graph optimization module 8. This module integrates constraints from vision, lidar, and IMU pre-integration, as well as loop closure factors detected by the loop closure recognition and consistency verification module, to perform global optimization and output accurate pose estimates for robot navigation.

[0039] Figure 3The diagram illustrates the implementation of the TD3 deep reinforcement learning algorithm. The TD3 training process is as follows: First, the state (visual statistical matrix and laser geometric feature statistical matrix) is obtained from the current environment. The Actor network outputs the weights A for the camera and radar actions. Based on weight A and the current state S, the Q-values ​​of Critic1 and Critic2 are calculated to evaluate the quality of the action. The parameters of the Critic network (actor network / policy network) are updated by calculating the TD error, and the Actor network (evaluator network / value network) is updated using the policy gradient method. In this process, TD3 employs a dual Critic network and a target network. The target network uses a soft update strategy to smooth the target value, reducing fluctuations during training. Simultaneously, a delayed update strategy and adding noise to the target network further improve training stability. All states, actions, rewards, and next-state samples are stored in an experience replay pool for subsequent training and optimization, thereby achieving stable and efficient policy learning.

[0040] In some embodiments, the visual statistical matrix is ​​constructed by the visual inertial module after feature extraction and optical flow tracking are completed. The generation process of the visual statistical matrix includes: The input image is divided into R×C grid cells; The number of effective features, the average horizontal optical flow displacement, and the average vertical optical flow displacement of each grid cell are counted. When a grid cell has no effective features, all three statistics are recorded as zero. The statistics of the grid cells are normalized by intra-frame maximum value normalization to form a visual statistical tensor of size (R,C,3); and Flatten the visual statistics tensor into a one-dimensional vector, which is then used as the visual statistics matrix.

[0041] In some embodiments, the laser geometric feature statistical matrix is ​​constructed by the lidar inertial module after distortion correction, feature extraction, and matching. The generation process of the laser geometric feature statistical matrix includes: The success rates of corner point matching and planar point matching are statistically analyzed to characterize the degree of utilization of point-line constraints and point-plane constraints. The point cloud is projected onto a depth map with a resolution of R×C, and the effective pixel coverage ratio is calculated to characterize the observation integrity. Calculate the standard deviation of the distance from the point to the sensor to characterize the geometric hierarchy and complexity of the scene; and The success rates of corner point matching, planar point matching, effective pixel coverage ratio, and standard deviation of point-to-sensor distance are normalized and anomaly-removed, and then concatenated in a fixed order into a fixed-length vector, which serves as the laser geometric feature statistical matrix. In some embodiments, the TD3 deep reinforcement learning algorithm takes the state vector obtained by concatenating the visual statistical matrix and the laser geometric feature statistical matrix as input. The policy network outputs a two-dimensional continuous action at each time step, which is mapped to visual constraint weights and laser constraint weights by softmax. These weights are then directly applied to the corresponding residual terms in the unified factor graph. Under the given weights, nonlinear least squares are performed on the sliding window target to obtain the current trajectory. An instant reward is then constructed using the relative pose error over a preset time span and written to the replay buffer. During training, double Q-pruning, delayed policy updates, and target policy smoothing are employed to minimize the mean square error of the two value networks and inject truncated Gaussian noise into the action on the target path. Soft updates are performed on the target networks of policy and value to ensure convergence stability. During online deployment, only forward inference is performed to output weights in real time, achieving adaptive fusion that changes with the environment.

[0042] While some embodiments of the present invention have been described in this application, those skilled in the art will understand that these embodiments are merely illustrative. Numerous variations, alternatives, and improvements will arise in those skilled in the art under the teachings of this invention without departing from its scope. The appended claims are intended to define the scope of the invention and thereby cover methods and structures within the scope of the claims themselves and their equivalents.

Claims

1. A learning-driven multi-sensor adaptive weighted synchronous localization and mapping method, characterized in that, Includes the following steps: S1. Synchronously acquire the raw data streams from the camera, inertial measurement unit (IMU), and lidar; perform time synchronization, intrinsic and extrinsic parameter calibration, noise reduction, and point cloud motion compensation on the raw data streams; perform distortion correction on the point cloud based on the IMU; and output image frames aligned to a unified time base, IMU pre-integration sequences for pre-integration, and time-stamped point clouds and their initial poses. S2. Corner detection is performed on the input image and the KLT optical flow method is used to establish the feature correspondence between adjacent frames. The BRIEF descriptor is calculated for loop closure retrieval and key frame information is retained. S3. Based on the sliding window, the visual reprojection residual and the IMU pre-integration residual are jointly optimized nonlinearly to estimate the pose, velocity and zero bias of the camera and IMU. S4. Use the temporal motion estimated by IMU to process the distortion of the point cloud, unify all sampling points to the reference time coordinate system, extract corner points and plane points according to local curvature, combine scan line sparsification and voxel downsampling to form a feature set, establish the correspondence between the features of the current frame in the local map through nearest neighbor search, construct point-line residuals and point-area residuals, and jointly establish the objective function of lidar and IMU pre-integration with IMU pre-integration. S5. Register the multi-frame point cloud to the camera coordinate system to form a depth map. Search the neighborhood along the ray for visual features and obtain the depth by fitting the local plane intersection when the geometric distribution threshold is met. Remove anomalies through geometric consistency and multi-frame consistency. S6. Calculate the number of effective features, average horizontal optical flow displacement and average vertical optical flow displacement of each grid cell in the input image according to the R×C grid, normalize and flatten it into a visual statistical matrix. S7. The success rates of corner point matching and planar point matching in the statistical point cloud, the effective pixel coverage ratio and the standard deviation of the point-to-sensor distance distribution are used to obtain the laser geometric feature statistical vector, which is then normalized and flattened into a laser geometric feature statistical matrix. S8. The visual statistical matrix and the laser geometric feature statistical matrix are used together as the state vector and input into the TD3 deep reinforcement learning algorithm. The output is a two-dimensional continuous action and mapped to visual constraint continuous weights and laser radar constraint continuous weights through softmax. The reward function is the reciprocal of the relative pose error. The visual constraint continuous weights and laser radar constraint continuous weights are directly used as the weighting coefficients of the corresponding residual terms in the factor graph, and the relative contributions of the visual factor and the laser radar factor in the factor graph optimization are dynamically adjusted. as well as S9. Construct a joint optimization objective that includes IMU pre-integration, visual reprojection, and laser point-line and point-plane residuals. Weight the corresponding residuals according to the continuous weights of the visual constraints and the continuous weights of the lidar constraints, complete the sparse nonlinear least squares solution, and output the fused pose. The fused pose serves as the robot's localization result to support the robot's operation.

2. The learning-driven multi-sensor adaptive weighted synchronous localization and mapping method according to claim 1, characterized in that, In step S3, the joint nonlinear optimization of the visual reprojection residual and the IMU pre-integration residual includes the following steps: S31. Let the current sliding window be... For each keyframe Define state: in, Let be the rotation matrix of the machine relative to the world coordinate system. It is the location of the IMU origin within the world system. It is the speed of the IMU in the world system. This is the zero bias vector of the gyroscope. This is the zero bias vector of the accelerometer; S32, Features Observation in frame i Establish visual reprojection residuals: in, For perspective projection, For the external parameters from the camera to the IMU, It is the pose from the IMU to the world in the i-th frame. It is a feature The three-dimensional position; S33, For adjacent keyframes Constructing the IMU pre-integration residual: in, For perspective projection, and For two frames of pose, and It's two frames per second. and It's two frame positions. It is a time interval between two frames. Let be the world-system gravity vector. For IMU pre-integration rotation measurement from keyframe i to j, For IMU pre-integration velocity increment measurement from keyframe i to j, For IMU pre-integration displacement measurement from keyframe i to j, For pre-integral rotation measurement of gyroscope zero bias The first-order Jacobian, For pre-integral velocity measurement, zero bias of the gyroscope The first-order Jacobian, For pre-integral velocity measurement, zero bias of the accelerometer The first-order Jacobian, For pre-integral displacement measurement, zero bias of the gyroscope The first-order Jacobian, For pre-integral displacement measurement, zero bias of accelerometer The first-order Jacobian, It is the zero-bias increment of the gyroscope relative to the linearization point. It is the zero bias increment relative to the linearization point; S34. Construct a joint optimization objective function using vision and IMU pre-integration: in, The set of variables to be optimized For the set of observed "frame-feature" pairs, It is the visual observation covariance. It is a visual residual robust kernel. It is a set of IMU edges. It is the IMU pre-integral covariance. It is an IMU residual robust core. and To marginalize prior residuals and covariance, It is a feature set with deep priors. It is the depth prior weight. It is a deep residual robust kernel. It is to optimize the estimated depth. It is the prior depth derived from the point cloud and depth map; and S35. By solving the problem through sparse nonlinear least squares iteration and marginalizing the oldest keyframe, the historical constraints are compressed into prior factors that are rolled and passed within the sliding window. This not only obtains the optimal state solution at the current moment, but also maintains information accumulation without increasing the state size, thereby satisfying the real-time performance and robustness of online SLAM, and providing consistent priors and initial values ​​for the cross-modal weighted fusion in steps S4 and S8.

3. The learning-driven multi-sensor adaptive weighted synchronous localization and mapping method according to claim 1, characterized in that, In step S4, the construction of point-line residuals and point-surface residuals, and the establishment of a joint optimization objective function for lidar and IMU pre-integration together with IMU pre-integration, includes the following steps: S41. After IMU distortion correction and unification to the reference time coordinate system, transform the laser point from the laser system to the world system. The coordinates of the k-th laser point in the i-th frame in the world coordinate system are... Represented as: in, These are the coordinates of the point in the lidar coordinate system. Let L be the rotation matrix from the lidar coordinate system L to the IMU coordinate system B. The coordinates of the lidar origin in IMU coordinate system B; S42, Constructing point-line geometric residuals from diagonal points : in, and These are the two adjacent line feature points corresponding to the corner point in the local map; S43. Construct point-plane geometric residuals for points on a plane. : in, It is the unit normal vector corresponding to the local plane. Let be a reference point on the plane; and S44. Construct a joint optimization objective function for lidar and IMU pre-integration: in, It is a set of corner indexes. For a set of plane point indices, For robust kernels of point-to-line residuals, For robust kernels of point-to-surface residuals, For point-to-line residual covariance, The variance is the point-to-surface residual.

4. The learning-driven multi-sensor adaptive weighted synchronous localization and mapping method according to claim 1, characterized in that, In step S6, the step of calculating the effective feature count, average horizontal optical flow displacement, and average vertical optical flow displacement of each grid cell in the input image according to an R×C grid to obtain a visual statistical matrix includes the following steps: S61. Divide the i-th frame image into an R×C grid, and count the number of effective features and the average horizontal optical flow displacement and the average vertical optical flow displacement in each grid cell: in, For the first One grid cell, This represents the average horizontal optical flow displacement of the grid cell. This represents the average vertical optical flow displacement of the grid cell. and The normalized coordinate difference of the same feature between two frames; S62. Normalize the statistical values ​​of the grid cells according to the global maximum value of the current frame to form a three-channel visual statistical matrix: in, This indicates that the maximum value is taken across all grid cells in the frame. c >0 indicates a numerically stable term; and S63. Flatten the visual statistics matrix into a visual state vector in a fixed order for use by the policy network: in, It is a flattening operator.

5. The learning-driven multi-sensor adaptive weighted synchronous localization and mapping method according to claim 1, characterized in that, In step S7, the statistical point cloud is analyzed to obtain a laser geometric feature statistical vector by considering the corner matching success rate, planar point matching success rate, effective pixel coverage ratio, and standard deviation of the point-to-sensor distance distribution. This includes the following steps: S71. Under the correspondence between the current frame point cloud and the local map, calculate the matching success rate between corner points and planar points: in, The number of corner points participating in the matching. The number of planar points participating in the matching. and The number of successful geometric consistency checks for corner points and planar points. o >0 indicates a numerically stable term; S72. Project the point cloud onto a depth map with a resolution of R×C, and calculate the coverage ratio and the standard deviation of the distance distribution: in, It is the effective pixel coverage ratio. The standard deviation of the distance from the point to the sensor; and S73. Normalize the above statistics and concatenate them into a laser geometric feature state vector: in, It is the maximum value or preset size of the sliding window. >0 indicates a numerically stable term.

6. The learning-driven multi-sensor adaptive weighted synchronous localization and mapping method according to claim 1, characterized in that, S8 includes the following steps; S81, Build Status The policy network outputs two-dimensional actions. : in, For TD3 policy network, and It outputs two-dimensional continuous motion, corresponding to the vision and LiDAR channels respectively; S82. Map actions to visual weights using softmax. With laser weight : in, ; S83. Merge the two objectives based on their weights to form the overall backend objective function: S84. Use the reciprocal of the relative pose error (RPE) as the immediate reward: in, For numerically stable terms, and To minimize The estimated pose is obtained. and For reference relative pose, To evaluate the span, For the logarithmic mapping of the Lie group, It is a weighted norm; S85, Record at every moment And write to the playback buffer, This is the round termination flag; the replay buffer is used for subsequent sampling training of Critic and Actor. S86. Add truncated Gaussian noise to the policy output of the next state to smooth the target action; use two Critic estimates to estimate the target Q value and take the smaller one to suppress overestimation, with discount factor and sampling batch size set according to implementation. S87. First, minimize the mean square error of the two Critic paths and update the value network; then update the Actor; subsequently, update the coefficients using a soft update method. Soft updates are performed on the target network of the Actor and the two Critics to ensure convergence stability; and S88, Output again using the updated strategy. and And minimize the overall objective Obtain the optimal solution for the current window; perform marginalization on the oldest keyframe to generate priors. Pass it to the next window.

7. The learning-driven multi-sensor adaptive weighted synchronous localization and mapping method according to claim 1, characterized in that, In S9, sparse nonlinear least squares with robust kernel is used to solve the problem. After the fused pose is output, loop closure is searched based on DBoW2 and BRIEF. Geometric consistency is checked by combining PnP with RANSAC. The loop closure factors that pass the check are added to the graph to complete the global consistency correction.

8. A learning-driven multi-sensor adaptive weighted synchronous localization and mapping system, characterized in that, include: The data acquisition module is configured to simultaneously acquire raw data streams from the camera, inertial measurement unit (IMU), and lidar. The data preprocessing module is configured to preprocess the raw data stream and output image frames aligned to a unified time base, IMU pre-integration sequences for pre-integration, and initial poses of point clouds with timestamps. The visual-inertial processing module is configured to tightly couple and optimize visual reprojection constraints and IMU pre-integration constraints within a sliding window to estimate the pose, velocity, and zero bias of the camera and IMU. The lidar inertial processing module is configured to perform distortion correction on the point cloud, extract corner and planar point features, construct point-line residuals and point-plane residuals, and enter them together with IMU pre-integration into sliding window optimization; The depth assistance and scale correction module is configured to generate a depth map, acquire visual feature depth information and remove outliers, and provide depth priors to the visual inertial module and backend optimization during system initialization and when scale drift is detected. The observation quality statistics construction module is configured to convert the visual and lidar observation data of the visual inertial processing module and the lidar inertial processing module into low-dimensional statistical vectors, and output a visual statistical matrix and a lidar geometric feature statistical matrix. The learning-driven constraint weight allocation module is configured to take the visual statistical matrix and the laser geometric feature statistical matrix as inputs and output the continuous weights of visual constraints and the continuous weights of laser radar constraints online. The multi-sensor factor graph optimization module is configured to construct IMU pre-integration, visual reprojection, and laser point-line and point-surface constraints as joint optimization objectives in a unified factor graph. Based on the continuous weights of the visual constraints and the continuous weights of the lidar constraints, the corresponding residuals are weighted to complete the sparse nonlinear least squares solution and output the fused pose. The fused pose serves as the robot's localization result to support the robot's operation. as well as The loop closure identification and consistency verification module is configured to retrieve loop closure candidates and perform geometric consistency verification. When the verification is successful, the loop closure factor is output.

9. The learning-driven multi-sensor adaptive weighted synchronous localization and mapping system according to claim 8, characterized in that, The process of generating the visual statistical matrix includes: The input image is divided into R×C grid cells; Statistically analyze the number of effective features, average horizontal optical flow displacement, and average vertical optical flow displacement for each grid cell. The statistics of the grid cells are normalized to their intra-frame maximum values ​​to form a visual statistical tensor of size (R, C, 3); and The visual statistics tensor is flattened into a one-dimensional vector, which is then used as the visual statistics matrix.

10. The learning-driven multi-sensor adaptive weighted synchronous localization and mapping system according to claim 8, characterized in that, The generation process of the laser geometric feature statistical matrix includes: The success rates of corner point matching and planar point matching are statistically analyzed to characterize the degree of utilization of point-line constraints and point-plane constraints. The point cloud is projected onto a depth map with a resolution of R×C, and the effective pixel coverage ratio is calculated to characterize the observation integrity. Calculate the standard deviation of the distance from the point to the sensor to characterize the geometric hierarchy and complexity of the scene; and The corner point matching success rate, plane point matching success rate, effective pixel coverage ratio, and standard deviation of the distance from the point to the sensor are normalized and anomaly removed, and then concatenated in a fixed order into a fixed-length vector, which serves as the laser geometric feature statistical matrix.

Images