Self-adaptive multi-mode speedometer dynamic fusion method

Abstract

The invention provides a self-adaptive multi-modal odometer dynamic fusion method which specifically comprises the following steps: extracting edge and plane features of a laser point cloud, and generating a laser pose factor by utilizing iterative motion compensation and residual optimization; multi-scale angular point features are extracted from an original image, and visual pose factors are generated in combination with optical flow tracking and local light beam adjustment. And a dynamic factor graph is constructed, laser and visual pose factors are used as constraints, an iSAM2 engine is used for optimization solution, and a global pose is obtained. Calculating the residual mean value and variance of Lagrange factors in real time, dynamically checking the state of the sensor based on chi-square test, triggering failure response during continuous overrun, gradually removing abnormal factors, keeping historical data, aligning timestamps through SE (3) manifold interpolation, and optimizing external parameter calibration. And after the sensor is recovered, factors are re-introduced through progressive weight fusion, and a sliding window mechanism is adopted to smooth the track. The method provides a robust and high-precision fusion algorithm for the field of robots and automatic driving.

Description

Technical Field

[0001] The present invention relates to the technical fields of robots and autonomous driving, and particularly relates to an adaptive multi-modal odometer dynamic fusion method. Background Art

[0002] In recent years, the Simultaneous Localization and Mapping (SLAM) technology has been widely applied in the fields of autonomous driving, robot navigation, etc. However, its performance highly depends on the environmental features and the reliability of sensors. Traditional SLAM systems are mostly implemented based on a single sensor (such as lidar or camera). However, in actual complex scenarios, a single sensor has inherent limitations: lidar is prone to matching failures due to insufficient features in an environment lacking geometric structures (such as long corridors, open spaces), and it cannot perceive semantic information such as color and texture; although visual sensors can capture rich environmental details, they are prone to feature tracking loss in areas with drastic illumination changes, motion blur, or weak texture (such as white walls, snow), and the accuracy of depth estimation is limited by the baseline length and the algorithm robustness. To improve the system generalization ability, multi-sensor fusion has become an important research direction in the SLAM field. However, its core challenge lies in how to dynamically coordinate the complementarity of heterogeneous sensors and maintain the system continuity when some sensors fail. Existing multi-modal SLAM solutions are mainly divided into two categories: loose coupling and tight coupling. The loose coupling method runs the lidar and visual SLAM threads independently, and fuses the outputs of both through backend filtering or pose graph optimization. Although this method is simple to implement, due to the lack of joint optimization at the underlying data level, the information utilization rate is low, and the linearization assumption of the filter will introduce cumulative errors, and trajectory divergence is likely to occur during long-term operation. The tight coupling method goes a step further and directly fuses the original observations such as lidar point cloud matching and visual feature tracking through a factor graph. In theory, it can improve the accuracy, but its design still has significant defects: First, the sensor weights are usually fixed or set depending on experience, and cannot be dynamically adjusted according to the real-time reliability. For example, when the vision generates an abnormal pose estimate due to the interference of dynamic objects, the system will still introduce it as a valid constraint into the optimization, contaminating the global trajectory; Second, the existing solutions are sensitive to the external parameter calibration error. Most works rely on offline calibration, and factors such as mechanical vibration and temperature drift in the actual environment may cause changes in the external parameters, thereby causing the fusion result to deviate.

[0003] Most existing technologies adopt simple judgment logics based on instantaneous residuals or fixed thresholds, and their design concepts mainly revolve around the simple idea of "abnormal means discard". For example, some open-source multi-sensor SLAM frameworks compare the relative pose differences output by the laser odometer and the visual odometer in real time. If the error exceeds a preset empirical threshold, it is directly determined that a certain sensor fails and its current data is discarded. However, this coarse-grained determination method has significant defects: First, the statistical characteristics of sensor noise are not fully modeled. In actual scenarios, lidar may experience point cloud distortion due to glass reflection or rain and fog interference, and cameras may also have feature mis-matching due to dynamic object occlusion or sudden changes in illumination. These instantaneous noises are non-Gaussian and sudden in nature. The fixed threshold method does not consider the spatio-temporal distribution characteristics of the noise, and is prone to false judgments during short-term interference, resulting in high-frequency oscillations between the "fusion - single sensor" modes of the system, and even causing non-convergence of the backend optimization. For example, when the visual odometer has a single pose jump due to a short-term occlusion, the system may mistakenly think that the vision has completely failed and remove it, but after the occlusion is removed, re-initialization is required, resulting in a break in the trajectory. Second, existing methods lack effective utilization of historical information. Most systems directly clear all associated data of abnormal sensors during switching, resulting in the instantaneous removal of the historical constraints of this sensor in the factor graph. For example, when the lidar fails due to entering an unstructured environment, if the system immediately deletes all the added lidar odometer factors, it may cause the optimization problem to lose the local smoothness constraint, resulting in a sudden change in the pose nodes at the switching moment. In addition, after the sensor recovers, the system often needs to re-initialize its data stream instead of gradually re-introducing historical information, which makes it difficult to smoothly connect the pose estimation in the recovery stage with the first half of the trajectory. More seriously, in scenarios of repeated failure - recovery, frequent initialization operations will significantly increase the computational load and even result in the loss of real-time performance. Finally, existing fault tolerance mechanisms do not achieve closed-loop optimization of the "failure - recovery" process. For example, some systems only rely on the observations of the remaining sensors to calculate the pose after switching to the single-sensor mode, without propagating the uncertainty during the failure stage to the global optimization. This may lead to two problems: one is that the single-sensor error during the failure period will accumulate with the movement distance and cannot be corrected through subsequent fusion; the other is that when the sensor recovers, the system lacks the ability to reverse-correct the pose during the failure period. This open-loop processing method severely limits the robustness of the system during long-term operation. Ideally, the fault tolerance mechanism should have the capabilities of dynamic weight adjustment, historical constraint retention, and closed-loop error compensation, while existing technologies are still in the initial stage in this regard and require the support of more refined statistical modeling and adaptive optimization frameworks.

[0004] Although the hardware synchronization mechanism can effectively reduce the initial time deviation between sensors, in actual systems, the inherent sampling frequency differences of sensors such as lidar and cameras still make it difficult to strictly synchronize multi-source data streams, forming asynchronous discrete observation sequences. Existing solutions usually use linear interpolation or nearest neighbor matching to align timestamps, but these methods essentially face two major defects: geometric distortion and information loss. For the linear interpolation method, it is assumed that the sensor moves at a uniform speed in a short time window, and the intermediate moment pose is calculated by linear weighting between the poses at adjacent moments. However, the robot motion essentially exists in the SE(3) manifold space, and its rotation component (SO(3)) has nonlinear characteristics. Simple linear interpolation will destroy the orthogonality of the rotation matrix, causing the interpolated pose to violate the rigid transformation constraint. For example, when the robot turns quickly, the spherical linear interpolation of the rotation component can maintain the angular velocity continuity, while linear interpolation will introduce false rotation jitter, which will distort the point cloud or feature matching results. For the nearest neighbor matching method, the high-frequency sensor data is directly forced to align to the low-frequency sensor timestamp. Although interpolation calculation is avoided, a large amount of valid observation information will be directly discarded. For example, when downsampling 30Hz visual data to a 10Hz laser frame rate, key motion details of the robot's acceleration or turning moment may be lost, causing the fused odometer to respond laggingly to high-frequency motion. More seriously, the above errors will be transmitted to the global trajectory through factor graph optimization, causing cumulative positioning drift in long-term operation. This problem is particularly prominent in high-speed motion scenes. Traditional methods may cause local trajectories to become "jagged" due to interpolation distortion, and even trigger nonlinear divergence of back-end optimization. Summary of the invention

[0005] In order to overcome the shortcomings of the prior art, the purpose of the present invention is to provide an adaptive multi-modal odometer dynamic fusion method to provide a comprehensive solution for the field of robotics and autonomous driving.

[0006] To achieve the above object, the present invention provides the following solutions:

[0007] An adaptive multi-modal odometer dynamic fusion method, comprising:

[0008] Extract edge feature points and plane feature points of the original laser point cloud, and generate laser pose factors through iterative motion compensation and residual optimization methods;

[0009] Extract multi-scale corner features of the original image and generate visual pose factors through optical flow tracking and local bundle adjustment method;

[0010] Construct a dynamic factor graph, use the laser pose factor and the visual pose factor as constraints, construct an optimization equation, and solve the optimization equation through the iSAM2 incremental optimization engine to obtain the global pose;

[0011] Calculate the residual mean and variance of the laser pose factor and the visual pose factor in real time, and dynamically test the sensor based on the chi-square test method;

[0012] When the sensor continuously exceeds the limit, trigger a failure response, gradually remove abnormal factors through exponentially decaying weights, retain historical data, use the SE(3) manifold interpolation algorithm to align the timestamps of multiple sensors, and correct the calibration error by jointly optimizing the external parameter calibration node;

[0013] When the sensor recovers, reintroduce the factors of the sensor through progressive weight fusion and use the sliding window mechanism to smoothly transition the trajectory.

[0014] Preferably, extract the edge feature points and plane feature points of the original laser point cloud, and generate the laser pose factor through the iterative motion compensation and residual optimization method, including:

[0015] Perform point cloud filtering on the original laser point cloud to obtain preprocessed point cloud;

[0016] Based on multi-scale curvature analysis, perform preliminary feature screening on the preprocessed point cloud to obtain candidate features;

[0017] Perform non-maximum suppression on the candidate features to obtain edge feature points and plane feature points;

[0018] Dynamically adjust the ratio of the edge feature points to the plane feature points according to the scene type;

[0019] Based on the uniform motion assumption, preliminarily estimate the relative pose transformation matrix between adjacent frame point clouds, and use the relative pose transformation matrix to perform inverse motion compensation on the current frame point cloud;

[0020] Optimize the pose and update the point cloud compensation result through multiple iterations until the pose converges, and finally output the undistorted point cloud and the final pose;

[0021] Design different matching strategies for the edge feature points and plane feature points respectively. When the matching residuals of the feature points exceed the adaptive threshold for multiple consecutive frames, they are determined as dynamic objects or noise points and excluded from the optimization process;

[0022] Solve the relative pose transformation between two frames of point clouds through the minimum feature matching residual, and solve the Euclidean distances from the edge feature points to the line and from the plane feature points to the plane respectively to construct a cost function based on the distance residual;

[0023] Use the Levenberg-Marquardt method to iteratively solve the pose transformation matrix and the laser confidence to obtain the laser pose factor.

[0024] Preferably, the different matching strategies are specifically:

[0025] For edge feature points, the KD tree is used to accelerate the search for the two nearest neighbor edge feature points in the previous frame, construct the distance residual from the point to the line, and constrain the rotational degree of freedom of the carrier.

[0026] For planar feature points, three nearest neighbor points are searched among the planar feature points in the previous frame to form a local plane, and the perpendicular distance residual from the point to the plane is calculated to constrain the translational degree of freedom of the carrier.

[0027] Preferably, the formula of the cost function l is:

[0028]

[0029] where d edge and d plane respectively represent the Euclidean distances from the edge feature point to the line and from the planar feature point to the plane, T is the pose transformation matrix to be optimized, p i is the edge feature point in the current frame, and q j is the planar feature point in the current frame.

[0030] Preferably, multi-scale corner features of the original image are extracted, and visual pose factors are generated through optical flow tracking and local bundle adjustment methods, including:

[0031] Performing preprocessing of distortion correction and histogram equalization on the original image to obtain a preprocessed image;

[0032] Adopting a multi-scale image pyramid structure, applying the FAST corner detector at different resolution levels, and screening candidate corners with scale invariance in the preprocessed image;

[0033] Generating a binary feature descriptor based on the improved BRIEF descriptor, matching candidate corners across scales through the Hamming distance, and removing mismatched points by combining the RANSAC algorithm to obtain multi-scale corner features;

[0034] Applying the KLT sparse optical flow algorithm between consecutive frames to track the motion trajectories of the matched corners, and filtering abnormal tracking points based on the forward-backward consistency test;

[0035] Introducing the local bundle adjustment method within a sliding window, constructing an optimization window containing the nearest 5 - 10 key frames, and jointly minimizing the reprojection error of all camera poses and the three-dimensional coordinates of the observed map points within the optimization window to obtain the visual pose factors.

[0036] Preferably, the formula of the optimization equation F is:

[0037]

[0038] where r i represents the residual of the i-th factor, and Σ i is the corresponding noise covariance matrix, and x is the pose of the parameter to be optimized.

[0039] Preferably, the fusion mechanism of the dynamic factor graph is specifically:

[0040] The noise covariance of the laser pose factor is dynamically adjusted based on the matching residual, with a range of 0.05 m for translation and 0.1 rad for rotation;

[0041] The visual pose factor defines the covariance by the reprojection error after coordinate system transformation;

[0042] When the sensor fails, the abnormal factors are locally removed through the Bayesian tree sparse decomposition technique, and the historical factors are retained in the sliding window buffer.

[0043] Preferably, the steps of the SE(3) manifold interpolation algorithm include:

[0044] Performing spherical linear interpolation of quaternions on the rotation component;

[0045] Performing cubic spline interpolation on the translation component; the interpolation interval is adaptively adjusted according to the difference in sensor sampling frequencies.

[0046] According to the specific embodiments provided by the present invention, the following technical effects are disclosed by the present invention:

[0047] The present invention provides an adaptive multi-modal odometry dynamic fusion method, including: extracting the edge feature points and plane feature points of the original laser point cloud, and generating the laser pose factor through the iterative motion compensation and residual optimization method; extracting the multi-scale corner features of the original image, and generating the visual pose factor through the optical flow tracking and local bundle adjustment method; constructing a dynamic factor graph, and using the laser pose factor and the visual pose factor as constraint conditions to construct an optimization equation, and solving the optimization equation through the iSAM2 incremental optimization engine to obtain the global pose; calculating the residual mean and variance of the laser pose factor and the visual pose factor in real time, and dynamically testing the sensor based on the chi-square test method; triggering a failure response when the sensor continuously exceeds the limit, gradually removing the abnormal factors through the exponentially decaying weight, and retaining the historical data, using the SE(3) manifold interpolation algorithm to align the timestamps of multi-sensors, and correcting the calibration error by jointly optimizing the external parameter calibration node; when the sensor recovers, reintroducing the factors of the sensor through the progressive weight fusion, and using the sliding window mechanism to smoothly transition the trajectory. The present invention provides a robust and high-precision fusion algorithm with adaptive adjustment, providing a comprehensive solution for the fields of robotics and autonomous driving. Description of the Drawings

[0048] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the following will briefly introduce the accompanying drawings required in the embodiments. Obviously, the accompanying drawings in the following description are only some embodiments of the present invention. For those of ordinary skill in the art, without creative efforts, other accompanying drawings can be obtained based on these drawings.

[0049] Figure 1 It is the flowchart of the method provided by the embodiment of the present invention;

[0050] Figure 2 It is the schematic diagram of the algorithm module provided by the embodiment of the present invention;

[0051] Figure 3 It is the dynamic fusion algorithm factor graph provided by the embodiment of the present invention;

[0052] Figure 4 It is the effect diagram provided by the embodiment of the present invention;

[0053] Figure 5 It is the comparison diagram of the ATE accuracy of the present invention and several other algorithms in M2DGR provided by the embodiment of the present invention

[0054] Figure 6 It is the trajectory comparison diagram of the present invention and several other algorithms in M2DGR provided by the embodiment of the present invention. Specific implementation manners

[0055] The following will clearly and completely describe the technical solutions in the embodiments of the present invention with reference to the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only some embodiments of the present invention, rather than all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative efforts belong to the scope of protection of the present invention.

[0056] The purpose of the present invention is to provide a robust and high-precision fusion algorithm with adaptive adjustment, providing a comprehensive solution for the fields of robotics and autonomous driving.

[0057] To make the above objects, features, and advantages of the present invention more clearly understood, the present invention will be further described in detail below with reference to the accompanying drawings and specific implementation manners.

[0058] Figure 1 It is the flowchart of the method provided by the embodiment of the present invention. As Figure 1 shown, the present invention provides an adaptive multi-modal odometry dynamic fusion method, including:

[0059] Step 100: Extract the edge feature points and plane feature points of the original laser point cloud, and generate laser pose factors through an iterative motion compensation and residual optimization method;

[0060] Step 200: Extract the multi-scale corner features of the original image, and generate visual pose factors through optical flow tracking and local bundle adjustment methods;

[0061] Step 300: Construct a dynamic factor graph, use the laser pose factor and the visual pose factor as constraint conditions to construct an optimization equation, and solve the optimization equation through the iSAM2 incremental optimization engine to obtain the global pose;

[0062] Step 400: Calculate the residual mean and variance of the laser pose factor and the visual pose factor in real time, and perform dynamic inspection on the sensor based on the chi-square test method;

[0063] Step 500: When the sensor continuously exceeds the limit, trigger a failure response, gradually remove abnormal factors through exponentially decaying weights, and retain historical data. Use the SE(3) manifold interpolation algorithm to align the timestamps of multi-sensors, and correct the calibration error by jointly optimizing the external parameter calibration node;

[0064] Step 600: When the sensor recovers, reintroduce the sensor factor through progressive weight fusion, and use the sliding window mechanism to smoothly transition the trajectory.

[0065] The adaptive multi-modal odometer dynamic fusion algorithm proposed by the present invention mainly includes a lidar odometer module 01, a visual odometer module 02, an algorithm fusion module 03, and an anomaly detection module 04, as Figure 2 shown.

[0066] 1. As one of the core components of the multi-sensor fusion system, the lidar odometer module is responsible for real-time calculating the carrier motion pose from the lidar point cloud, providing high-frequency and low-drift local motion constraints for subsequent fusion optimization. And obtain the optimized pose TL as the lidar odometer factor according to the input point cloud data, providing a basis for the algorithm fusion module. Its core technical process includes four core stages: feature extraction and classification, inter-frame matching, motion estimation and optimization, and loop detection. At the same time, it integrates dynamic environment adaptability and computational efficiency optimization strategies.

[0067] (1) Feature extraction and classification: The feature extraction and classification module of the laser odometer aims to screen out highly discriminative geometric features from the raw point cloud to provide robust constraint information for inter-frame matching. Its process first preprocesses the raw point cloud using the Statistical Outlier Removal (SOR) algorithm. By calculating the average distance of the K-nearest neighbors (such as a 50-neighborhood) for each point, noise points with distances exceeding the dynamic threshold (such as the mean ± 3 times the standard deviation) are removed, eliminating outliers introduced by sensor jitter or environmental suspensions (such as dust, rain, and snow). Subsequently, a preliminary feature screening of the point cloud is performed based on multi-scale curvature analysis: with each point as the center, the surface curvature is calculated within neighborhoods with radii of 0.1m, 0.3m, and 0.5m respectively. The curvature value is obtained by the eigenvalue decomposition method, that is, the local curvature is characterized by the ratio of the minimum eigenvalue to the total eigenvalue of the covariance matrix. Low values correspond to flat areas, and high values correspond to edges or corners. To avoid the bias of single-scale sensitivity to features, a multi-scale voting mechanism is adopted. If a point is determined to be high-curvature or low-curvature at least at two scales, it is classified as a candidate feature. Then, non-maximum suppression is performed on the candidate features: the point cloud is divided into multiple fan-shaped regions along the laser beam direction, and the 2 edge points with the maximum curvature and the 4 plane points with the minimum curvature are retained in each region to ensure the spatial uniformity of the features and avoid matching ambiguities caused by local overcrowding. Finally, the feature ratio is dynamically adjusted according to the scene type - by statistically analyzing the proportion of plane features in real time. If the number of plane features in N consecutive frames exceeds 60% of the total features, it is determined to be a structured scene, and the weight of the plane features is increased by 20%; otherwise, if the proportion of edge features increases significantly, it switches to the unstructured mode to strengthen the edge constraints. This strategy enables the feature extraction to have environmental self-adaptability and improves the robustness of subsequent matching from the source.

[0068] (2) Inter-frame Matching and Motion Compensation: Inter-frame matching and motion compensation are the core steps for a lidar odometer to achieve high-precision motion estimation. Its goal is to calculate the pose change of the carrier through the feature correspondence of adjacent frame point clouds and eliminate the point cloud distortion caused by the vehicle's own motion during the lidar scanning process. First, for the motion distortion problem, an iterative compensation strategy is adopted: based on the assumption of uniform motion, the relative pose transformation matrix between adjacent frames is initially estimated, and this transformation is used to perform reverse motion compensation on the current frame point cloud. That is, according to the accurate acquisition timestamp of each lidar point, it is projected onto the same reference coordinate system to eliminate the spatial misalignment caused by the vehicle's motion. Subsequently, the pose T is optimized through multiple iterations and the point cloud compensation result is updated until the pose converges, and finally, the undistorted point cloud and the accurate pose are output. In the feature matching stage, different matching strategies are designed for the extracted edge and plane feature points respectively: for edge feature points, the KD tree is used to accelerate the search for the two nearest neighbor edge points in the previous frame, and the distance residual from the point to the line is constructed to constrain the rotational degree of freedom of the carrier; for plane feature points, three nearest neighbor points are searched in the previous frame plane feature to form a local plane, and the perpendicular distance residual from the point to the plane is calculated to constrain the translational degree of freedom of the carrier. In addition, to cope with the interference of dynamic objects, a dynamic consistency check mechanism is introduced - if the matching residuals of a certain feature point exceed the adaptive threshold for multiple consecutive frames, it is determined as a dynamic object or a noise point and temporarily excluded from the optimization process, thereby improving the matching robustness. Through the above process, the system can still maintain a stable inter-frame pose tracking ability in a complex environment, providing low-drift local motion constraints for subsequent fusion and optimization.

[0069] (3) Motion Estimation and Optimization: By minimizing the feature matching residuals, the relative pose transformation between two frames is solved. The Euclidean distances from the edge points to the line and from the plane points to the plane are calculated respectively, and a cost function based on the distance residuals is constructed as shown in formula (1):

[0070]

[0071] d edge and d plane represent the Euclidean distances from the edge points to the line and from the plane points to the plane respectively. The Levenberg-Marquardt algorithm is used to iteratively solve the pose transformation matrix TL and the lidar confidence δL, and the confidence δL is calculated by formula (2). To improve efficiency, the KD tree is used to accelerate the nearest neighbor search, and the feature correspondence is updated after each iteration.

[0072]

[0073] SL and SN represent the number of plane features and the total number of points extracted in the scene respectively.

[0074] (4) Loop detection: The method of scan context is adopted to construct the loop factor, which is finally incorporated into the optimization process to eliminate the cumulative error generated by the sensor, thereby further improving the accuracy of the algorithm.

[0075] 2. As one of the core components of the multi-sensor fusion system, the visual odometry module is responsible for real-time solving the carrier motion pose from visual images, and finally taking the optimized pose Tc according to the input image sequence as the visual odometry factor to provide data support for the algorithm fusion module. It mainly includes two core modules: front-end feature extraction, tracking, and motion estimation and optimization. The main contents are as follows:

[0076] (1) Front-end feature extraction and tracking: The multi-scale FAST corner detector is adopted, and the scale invariance is realized by combining with the image pyramid. The improved BRIEF descriptor is used to calculate the binary descriptor in the neighborhood of the feature points to improve the calculation efficiency. The feature matching mainly selects the best corresponding points according to the brute-force matching method based on the Hamming distance. Although this method will increase the time complexity, it has higher accuracy. And the RANSAC algorithm is combined with the fundamental matrix to eliminate the mismatches. The feature points are tracked using sparse optical flow between consecutive frames to reduce the overhead of repeated detection.

[0077] (2) Motion estimation and optimization: First, based on the 2D feature matching of adjacent frames, the initial relative pose is calculated using the epipolar geometry. The essential matrix is iteratively estimated by the Random Sample Consensus (RANSAC) algorithm, the inliers are selected, and the rotation matrix R and translation vector t are decomposed to initially eliminate the interference of mismatches. When there is a local 3D map, the EPnP algorithm is used to associate the current frame 2D feature points with the corresponding 3D points in the map, directly solving the 6-degree-of-freedom pose Tc and the visual confidence δC, and the confidence δC is calculated by formula (3) to enhance the stability of motion estimation. Secondly, the local Bundle Adjustment (BA) optimization within the sliding window is introduced. An optimization window containing the nearest 5-10 key frames is constructed, and all the camera poses and the three-dimensional coordinates of the observed map points within the joint optimization window are optimized to minimize the reprojection error.

[0078]

[0079] S C and S N ' respectively represent the number of inliers and the total number of matching features after being screened by RANSAC.

[0080] 3. The algorithm fusion module is the most important module of the present invention. It fuses the laser odometry factor and the visual odometry factor through the factor graph, as Figure 3 shown.

[0081] The variable x iRepresents the pose of the robot at the $i$-th moment, with an integer index as the unique identifier. Each node corresponds to an object of `gtsam::Pose3`, which contains position and rotation information. Factor type: The laser odometry factor is used as a prior constraint, and the noise model is set to a diagonal Gaussian distribution, reflecting the confidence in the LIO data. The odometry factor is based on the relative motion between adjacent frames of visual odometry, constraining the pose change of adjacent frame nodes. The loop closure factor constructed by Scan context can greatly reduce the cumulative drift. In many open-source datasets, consumer-grade GPS is often used. We introduce the GPS factor, which can greatly improve the outdoor positioning accuracy when this sensor is available. Through this factor graph, we obtain the optimization equation:

[0082]

[0083] where $r$ i represents the residual of the $i$-th factor, $\Sigma_i$ is the corresponding noise covariance matrix. The laser factor and the visual factor are defined as shown in equations (5) and (6) respectively. Finally, we use iSAM2 to solve this optimization problem. The core idea is to maintain the sparse structure of the factor graph through a Bayesian tree to achieve incremental optimization. The Bayesian tree decomposes the factor graph into multiple clusters. Each time a new factor is added, only the affected clusters are updated. Subsequently, the Jacobian matrix is decomposed into an orthogonal matrix and an upper triangular matrix using QR decomposition, and the computational cost is reduced through partial re-linearization, thus accelerating the solution. This provides the key algorithm support for real-time SLAM of this method in large-scale scenarios. The sliding window mechanism is used to dynamically manage recent sensor data, supporting time synchronization interpolation, anomaly detection statistics, and optimization calculations. Sorted by timestamp, it supports efficient front-end insertion and back-end elimination.

[0084]

[0085] and are the poses calculated by the laser odometry module and the visual odometry module at time $t$ respectively, and $X$ t -1 is the pose after fusion and optimization at the previous moment.

[0086] 4. The anomaly detection module is the core safety mechanism of the AMOF-SLAM algorithm. Its goal is to detect the reliability of laser and visual odometers in real time. When the confidence of a certain odometer is lower than the dynamic threshold δthreshold, it is judged as failed and can be switched to another separate odometer. The measurement method of this threshold is the mean standard deviation method, as shown in formula (7). The interference of the failed sensor is dynamically eliminated to achieve smooth mode switching: by real-time monitoring of the residuals of the laser and visual odometers (the difference between the observed value and the system predicted value), combined with sliding window statistical tests and dynamic threshold adjustment, it is determined whether the sensor is failed. The module uses the chi-square test to analyze the historical residual distribution to avoid instantaneous noise interference, and automatically adjusts the detection sensitivity according to the dynamics of the scene. When a sensor is abnormal for many consecutive times, the system switches to the backup sensor, while retaining the recent data of the failed sensor and gradually reducing its weight to ensure a smooth transition of the trajectory; if the sensor is restored, its data is gradually reintroduced to avoid state jumps. In the process, sliding window statistics are used to add the current residual to the residual queue of the window, calculate the mean and variance, and use the chi-square test to determine whether it is abnormal. The entire detection process is lightweight in design, supports multi-threaded parallel processing, has extremely low computational overhead, and can complete decisions in milliseconds, ensuring the real-time and robustness of the system.

[0087] δ threshold =μ-3·σ#(7)

[0088] Where μ is the mean confidence value of the last ten frames, and σ is the standard deviation of the confidence value of the last ten frames.

[0089] After testing, the algorithm has better positioning accuracy on multiple data sets, including the test under the gate01 sequence of M2DGR. Figure 4 As shown, the data only relies on street lights and car lights to collect visual features.

[0090] Figure 4 The upper part of the figure shows the features of visual odometer recognition. The detection effect is good in a dark environment, which shows that the visual odometer of this scheme is very effective. The lower part is the trajectory established by the final fusion. The trajectory is smooth and complete, which shows that the algorithm has good robustness. ATE accuracy comparison of the algorithm and three open source algorithms: A-LOAM, ORB-SLAM2, and LVI-SAM Figure 5 shown.

[0091] like Figure 5 As shown in the figure, this algorithm outperforms other algorithms in all indicators except the minimum value (min), indicating that the algorithm has better accuracy in this scenario. During the entire movement process, ORB-SLAM2, which relies on the visual solution, loses visual features, resulting in only a small trajectory during the entire movement process. The trajectory comparison is shown in the figure below. Figure 6 shown.

[0092] The method proposed by the present invention is closest to the true value (dashed line), indicating that the anomaly detection module demonstrates excellent reliability and real-time performance in practical applications, significantly enhancing the system's environmental adaptability. When the sensor is working properly, the module effectively filters out instantaneous noise through sliding window statistics, with a false detection rate of less than 5%. When the sensor fails, the module can accurately identify the anomaly within 100 milliseconds, trigger the switch, and maintain the trajectory continuity. The positioning error fluctuation during the switching process is less than 30% of that of traditional methods. In addition, the module supports dynamic environment adaptation. In scenarios of high-speed movement or drastic changes in lighting, it avoids false switching caused by over-sensitivity through dynamic threshold adjustment. At the same time, when the sensor recovers, it can be smoothly fused within 10 frames, and the trajectory jump amplitude is reduced to the centimeter level. Experiments show that this module improves the overall positioning accuracy of the multi-sensor SLAM system by approximately 25% in complex scenarios of the M2DGR dataset, and in extreme tests with artificially injected sensor failures, the system survival rate reaches over 98%, fully verifying its engineering practicality.

[0093] The implementation method from laser odometry to final anomaly detection is as follows:

[0094] Multi-sensor data acquisition and preprocessing: High-precision lidar and global shutter RGB-D cameras are selected. Point cloud noise is removed through statistical filtering respectively. The motion distortion of the lidar is corrected by combining IMU data, and the original data is converted to the robot base coordinate system. On the visual side, distortion correction and CLAHE histogram equalization are used to enhance image features, ensuring stable feature extraction under low light conditions and providing high-quality input for subsequent fusion.

[0095] Optimized implementation of laser and visual odometry: Edge and plane features are extracted, frame-to-frame matching is optimized through the LM algorithm, and ScanContext loop detection is integrated to improve global consistency. On the visual side, the ORB-SLAM2 framework is improved, integrating optical flow tracking and local BA optimization. Depth fusion and triangulation strategies are adopted for RGB-D and monocular cameras respectively to ensure the pose output accuracy in different scenarios.

[0096] Time synchronization and dynamic coordinate system alignment: Hardware PPS pulse triggering is used to achieve millisecond-level synchronization of laser and vision. At the same time, the frequency difference of data streams is compensated through SE(3) pose interpolation, and a sliding window buffer is maintained to achieve multi-sensor data alignment. External parameter calibration nodes are embedded in the factor graph, and the transformation matrix from laser to camera is jointly optimized every 50 frames to dynamically correct the external parameter error caused by environmental vibration or thermal drift.

[0097] Factor Graph Fusion and Incremental Optimization: Taking the robot pose and external parameters as nodes, a tightly coupled factor graph framework is constructed. The covariance of the translational 0.05m and rotational 0.1rad of the laser odometry factor is set based on the matching residuals, and the covariance of the visual factor is defined by the reprojection error after coordinate system transformation. The iSAM2 incremental optimization engine is adopted, and local smoothing is triggered every 5 frames to balance the computational efficiency and global accuracy.

[0098] Anomaly Detection and Smooth Switching Mechanism: The residuals of the laser and visual factors are calculated in real time. The sensor anomalies are determined through the chi-square test of the sliding window with a 95% confidence level, and the failure response is triggered continuously for 3 times when exceeding the limit. The historical factors are retained during the switching to avoid trajectory jumps. When the vision fails, only the laser factor is used for optimization, and vice versa, relying on the visual prior pose. In the recovery stage, weighted fusion is gradually carried out to achieve smooth transition.

[0099] System Integration and Performance Tuning: Integrating C++ and Python modules based on the ROS2 architecture, the main thread processes optimization and data flow, and the sub-thread parallelly executes feature extraction and loop detection. The object pool is used to manage the point cloud / image memory, and the system frequency is stable above 15Hz. A semantic segmentation interface is reserved to support the future expansion of the dynamic object filtering function.

[0100] The beneficial effects of the present invention are as follows:

[0101] (1) According to the real-time confidence of the sensor, the present invention dynamically adjusts the factor weights. When the sensor fails, the corresponding factor is directly and dynamically removed instead of retaining the weighted observation, avoiding contaminating the optimization result. Constraints such as laser odometry and visual odometry are fused in the factor graph to achieve global consistency. It solves the problem that the traditional tightly coupled method cannot quickly isolate the wrong observations when the sensor fails, and improves the robustness of the algorithm.

[0102] (2) Based on the residual statistics of the chi-square test, the present invention dynamically adjusts the anomaly determination threshold according to the mean and variance of the residuals in the past ten frames, avoiding misjudgment of instantaneous noise. When the sensor recovers, its factor is gradually introduced with exponentially decaying weights to avoid trajectory jumps. When the sensor fails initially, the historical data is briefly retained for loop detection and local optimization. Compared with the traditional hard switching scheme, the false detection rate is significantly reduced, and the trajectory continuity during the switching process is ensured.

[0103] (3) The present invention designs a general sensor adaptation layer, allowing new sensors to be quickly accessed and being compatible with the semantic segmentation module, supporting the future expansion of the dynamic environment filtering function, improving the system flexibility, and meeting the customized requirements of different application scenarios.

[0104] The various embodiments in this specification are described in a progressive manner. Each embodiment focuses on the differences from other embodiments. The same or similar parts among the various embodiments can be referred to each other.

[0105] In this article, specific examples are used to illustrate the principles and implementation modes of the present invention. The description of the above embodiments is only used to help understand the method of the present invention and its core idea. At the same time, for those of ordinary skill in the art, according to the idea of the present invention, there will be changes in the specific implementation modes and application scopes. To sum up, the content of this specification should not be construed as a limitation of the present invention.

Claims

1. An adaptive multi-modal odometer dynamic fusion method, characterized in that, Including: Extract the edge feature points and plane feature points of the original laser point cloud, and generate the laser pose factor through the iterative motion compensation and residual optimization method; Extract the multi-scale corner features of the original image, and generate the visual pose factor through the optical flow tracking and local bundle adjustment method; Construct a dynamic factor graph, use the laser pose factor and the visual pose factor as constraint conditions to construct an optimization equation, and solve the optimization equation through the iSAM2 incremental optimization engine to obtain the global pose; Calculate the residual mean and variance of the laser pose factor and the visual pose factor in real time, and perform dynamic inspection on the sensor based on the chi-square test method; When the sensor continuously exceeds the limit, trigger a failure response, gradually remove abnormal factors through exponentially decaying weights, retain historical data, use the SE(3) manifold interpolation algorithm to align the timestamps of multi-sensors, and correct the calibration error by jointly optimizing the external parameter calibration node; When the sensor recovers, reintroduce the sensor factor through progressive weight fusion and use the sliding window mechanism to smoothly transition the trajectory.

2. The adaptive multi-modal odometer dynamic fusion method according to claim 1, wherein Extract the edge feature points and plane feature points of the original laser point cloud, and generate the laser pose factor through the iterative motion compensation and residual optimization method, including: Perform point cloud filtering on the original laser point cloud to obtain preprocessed point cloud; Perform feature pre-screening on the preprocessed point cloud based on multi-scale curvature analysis to obtain candidate features; Perform non-maximum suppression on the candidate features to obtain edge feature points and plane feature points; Dynamically adjust the ratio of the edge feature points and the plane feature points according to the scene type; Based on the uniform motion assumption, preliminarily estimate the relative pose transformation matrix between adjacent frame point clouds, and use the relative pose transformation matrix to perform reverse motion compensation on the current frame point cloud; Optimize the pose through multiple iterations and update the point cloud compensation result until the pose converges, and finally output the undistorted point cloud and the final pose; Design different matching strategies for the edge feature points and plane feature points respectively. When the matching residuals of the feature points in multiple consecutive frames exceed the adaptive threshold, they are determined as dynamic objects or noise points and excluded from the optimization process; Solve the relative pose transformation between two frames of point clouds through the minimum feature matching residual, and solve the Euclidean distances from the edge feature points to the line and the plane feature points to the plane respectively to construct a cost function based on the distance residual; Use the Levenberg-Marquardt method to iteratively solve the pose transformation matrix and the laser confidence to obtain the laser pose factor.

3. The adaptive multi-modal odometer dynamic fusion method according to claim 2, characterized in that The specific different matching strategies are as follows: For the edge feature points, accelerate the search for the two nearest neighbor edge feature points in the previous frame through the KD tree, construct the distance residual from the point to the line, and constrain the rotational freedom of the carrier; For the plane feature points, search for the three nearest neighbor points in the previous frame of plane feature points to form a local plane, and calculate the perpendicular distance residual from the point to the plane to constrain the translational freedom of the carrier.

4. The adaptive multi-modal odometer dynamic fusion method according to claim 2, characterized in that The formula for the cost function l is: where d edge and d plane represent the Euclidean distances from the edge feature points to the line and from the plane feature points to the plane respectively, T is the pose transformation matrix to be optimized, p i is the edge feature point in the current frame, and q j is the plane feature point in the current frame.

5. The adaptive multi-modal odometer dynamic fusion method according to claim 1, wherein, Extract the multi-scale corner features of the original image, and generate the visual pose factor through the optical flow tracking and local bundle adjustment method, including: Perform distortion correction and histogram equalization preprocessing on the original image to obtain a preprocessed image; Adopt a multi-scale image pyramid structure, apply the FAST corner detector at different resolution levels, and screen candidate corners with scale invariance in the preprocessed image; Generate a binary feature descriptor based on an improved BRIEF descriptor, match candidate corners across scales through Hamming distance, and combine the RANSAC algorithm to remove mismatched points to obtain multi-scale corner features; Apply the KLT sparse optical flow algorithm between consecutive frames to track the motion trajectories of the matched corners, and filter out abnormal tracking points based on forward-backward consistency checking; Introduce a local bundle adjustment method within a sliding window, construct an optimization window containing the nearest 5 - 10 key frames, jointly optimize all camera poses and the three-dimensional coordinates of the observed map points within the optimization window, and minimize the reprojection error to obtain the visual pose factor.

6. The adaptive multi-modal odometer dynamic fusion method according to claim 1, wherein The formula for the optimization equation F is: where r i represents the residual of the i-th factor, Σ i is the corresponding noise covariance matrix, and x is the pose of the parameter to be optimized.

7. The adaptive multi-modal odometer dynamic fusion method according to claim 1, wherein The fusion mechanism of the dynamic factor graph is specifically: The noise covariance of the laser pose factor is dynamically adjusted based on the matching residuals, with a range of translation 0.05m and rotation 0.1rad; The visual pose factor defines the covariance through reprojection error after coordinate system transformation; When the sensor fails, locally remove abnormal factors through Bayesian tree sparse decomposition technology and retain historical factors in the sliding window buffer.

8. The adaptive multi-modal odometer dynamic fusion method according to claim 1, wherein The steps of the SE(3) manifold interpolation algorithm include: Adopt spherical linear interpolation of quaternions for the rotation component; Adopt cubic spline interpolation for the translation component; the interpolation interval is adaptively adjusted according to the sensor sampling frequency difference.

Images