Synchronous positioning and mapping method and system based on multi-sensor fusion

Abstract

The invention discloses a synchronous positioning and mapping method and system based on multi-sensor fusion, a storage medium and electronic equipment. The method comprises the following steps: acquiring IMU data, an image and point cloud data measured by a laser radar; performing pre-integration on the IMU data to obtain real-time pose estimation, and performing laser probability updating and visual probability updating based on the real-time pose estimation to obtain an updated local map; a laser point cloud matching residual error, a visual reprojection residual error, an IMU pre-integration residual error and a marginalization residual error are constructed, a residual error equation is constructed based on the laser point cloud matching residual error, the visual reprojection residual error, the IMU pre-integration residual error and the marginalization residual error, a sliding window is adopted for nonlinear optimization, and robot pose estimation is obtained; and visual loopback detection, laser loopback detection and pose global optimization are carried out. According to the method, the calculation rate, the positioning precision and the reliability can be improved in a complex or feature degradation scene.

Description

Technical Field

[0001] This invention relates to the field of robotics and autonomous driving technology, and in particular to a method, system, storage medium, and electronic device for simultaneous localization and mapping based on multi-sensor fusion. Background Technology

[0002] Simultaneous Localization and Mapping (SLAM) is a core technology for achieving autonomous navigation and environmental perception in robots. With technological advancements, SLAM systems relying on single sensors suffer from inherent drawbacks such as low accuracy and poor reliability in complex or dynamic environments, and are prone to tracking failures or localization degradation in extreme conditions. Therefore, multi-sensor fusion SLAM technology, which integrates information from multiple sensors (such as LiDAR, visual cameras, and IMUs), has become a mainstream and hot research topic, aiming to improve the accuracy and robustness of the system in various environments.

[0003] Currently, academia and industry have proposed various multi-sensor fusion SLAM schemes. For example, some schemes use the Multi-State Constrained Kalman Filter (MSCKF) method to fuse IMU, laser, and visual features. However, this type of method is prone to cumulative errors when performing long-term, large-scale mapping, affecting the consistency of the global map. Other schemes use factor graph-based optimization frameworks to jointly optimize multi-source sensor data. Although this can significantly improve positioning accuracy, its high computational complexity often leads to a decrease in system real-time performance, especially when processing large-scale data, making it difficult to meet the high frame rate real-time requirements. Summary of the Invention

[0004] This invention provides a method, system, storage medium, and electronic device for synchronous positioning and mapping based on multi-sensor fusion, which can improve data processing speed and positioning accuracy.

[0005] This invention provides a simultaneous localization and mapping method based on multi-sensor fusion, comprising: Acquire IMU data, images, and point cloud data measured by LiDAR; The IMU data is pre-integrated to obtain a real-time pose estimate. Based on the real-time pose estimate, laser probability updates and visual probability updates are performed respectively to obtain an updated local map. The laser point cloud matching residual, visual reprojection residual, IMU pre-integration residual, and edge-forming residual are constructed. Based on the laser point cloud matching residual, the visual reprojection residual, the IMU pre-integration residual, and the edge-forming residual, a residual equation is constructed. A sliding window is used for nonlinear optimization to obtain the robot pose estimate. Perform visual loop closure detection, laser loop closure detection, and global pose optimization.

[0006] The present invention also provides a simultaneous localization and mapping system based on multi-sensor fusion, comprising: The acquisition module is used to acquire point cloud data, IMU data, and images measured by the lidar. The front-end data preprocessing module is used to pre-integrate the IMU data to obtain real-time pose estimation, and perform laser probability update and visual probability update based on the real-time pose estimation to obtain an updated local map. The backend optimization module is used to construct the laser point cloud matching residual, the visual reprojection residual, the IMU pre-integration residual, and the edge-difference residual. Based on the laser point cloud matching residual, the visual reprojection residual, the IMU pre-integration residual, and the edge-difference residual, the residual equation is constructed, and a sliding window is used for nonlinear optimization to obtain the robot pose estimate. The loop closure detection module is used for visual loop closure detection, laser loop closure detection, and global pose optimization.

[0007] The present invention also provides a computer-readable storage medium storing a plurality of instructions adapted for loading by a processor to execute any of the above-described synchronous localization and mapping methods based on multi-sensor fusion.

[0008] The present invention also provides an electronic device, including a processor and a memory, wherein the processor is electrically connected to the memory, the memory is used to store instructions and data, and the processor is used in the steps of the synchronous localization and mapping method based on multi-sensor fusion described in any of the preceding claims.

[0009] This invention provides a method, system, storage medium, and electronic device for synchronous localization and mapping based on multi-sensor fusion. The invention employs a probability-based local map update strategy, dynamically calculating the retention probability of feature points based on their stability (e.g., retention time, frequency of occurrence) and matching quality (e.g., positional deviation, matching distance) and selectively removing them. The constructed front-end odometry has lower computational burden and higher real-time operating efficiency. In the back-end optimization, this invention proposes an adaptive degradation judgment factor strategy, and dynamically adjusts the constraint weights of laser and vision based on the number of multi-source features detected in real time in the optimization model. The designed back-end optimizer possesses high-precision pose estimation capabilities and stronger system stability in scenarios with sensor information degradation. In loop closure detection, this invention proposes a hybrid detection strategy combining laser and vision, and introduces a dual confirmation mechanism of "rapid visual screening and precise laser verification." The implemented loop closure detection module has extremely high matching accuracy and a strong ability to eliminate global cumulative errors. Attached Figure Description

[0010] The technical solution and other beneficial effects of the present invention will become apparent from the following detailed description of specific embodiments of the invention, in conjunction with the accompanying drawings.

[0011] Figure 1 This is a flowchart of a synchronous localization and mapping method based on multi-sensor fusion provided in an embodiment of the present invention.

[0012] Figure 2 A flowchart for updating a local map provided in an embodiment of the present invention.

[0013] Figure 3 This is a flowchart for nonlinear optimization of multi-source data using a sliding window, provided in an embodiment of the present invention.

[0014] Figure 4 The laser line surface feature map provided for the embodiments of the present invention.

[0015] Figure 5 A schematic diagram of visual reprojection error provided for an embodiment of the present invention.

[0016] Figure 6 This is a schematic diagram of a sliding window factor graph model for multi-sensor fusion provided in an embodiment of the present invention.

[0017] Figure 7 This is a flowchart of loop closure detection provided in an embodiment of the present invention.

[0018] Figure 8 The diagram shows the execution result of sequence 07 provided in the embodiment of the present invention.

[0019] Figure 9 A comparison diagram of the trajectories of various algorithms provided in the embodiments of the present invention.

[0020] Figure 10 This is a schematic diagram of a mobile robot system provided in an embodiment of the present invention.

[0021] Figure 11 Evaluation diagrams of positioning trajectories for various algorithms in an indoor long corridor provided in this embodiment of the invention.

[0022] Figure 12 Evaluation diagrams of positioning trajectories for various algorithms in underground parking lots provided in embodiments of the present invention.

[0023] Figure 13 Evaluation diagrams of the positioning trajectories of various algorithms in an open square provided in an embodiment of the present invention. Detailed Implementation

[0024] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0025] This invention provides a method, system, storage medium, and electronic device for simultaneous localization and mapping based on multi-sensor fusion. The simultaneous localization and mapping system based on multi-sensor fusion provided by this invention can be integrated into an electronic device, such as a terminal or server. The terminal can include tablet computers, laptops, personal computers (PCs), microprocessor boxes, or other devices.

[0026] Please see Figure 1 , Figure 1 The flowchart illustrates a synchronous localization and mapping method based on multi-sensor fusion provided in an embodiment of the present invention. This method, applied in electronic devices, includes the following steps: S1 acquires IMU (Inertial Measurement Unit) data, images, and point cloud data measured by lidar.

[0027] S2, pre-integrate the IMU data to obtain real-time pose estimation, and perform laser probability update and visual probability update based on the real-time pose estimation to obtain the updated local map.

[0028] IMU pre-integration is a technique for processing IMU measurements. It integrates the IMU measurement data (angular velocity and linear acceleration) between two time points to obtain the relative motion increments (changes in position, velocity, and attitude) of the vehicle between those two time points. This method cleverly avoids repeatedly integrating from the initial time point with each state update, greatly improving computational efficiency and can be directly used as a constraint for backend optimization.

[0029] Figure 2 A flowchart for updating a local map provided in an embodiment of the present invention, such as... Figure 2 As shown, the laser probability update based on real-time pose estimation in step S2 includes the following steps: S21, real-time pose estimation is used to compensate for the point cloud data to obtain the distortion-free point cloud.

[0030] Since a single frame scan of a LiDAR radar needs to be completed within a certain time, the movement of the carrier (robot or vehicle) during this period will cause each point in the point cloud of that frame to be actually collected in a different pose, resulting in motion distortion.

[0031] To eliminate this effect, we utilize high-frequency IMU pre-integration results (i.e., real-time pose estimation) for each laser point within the scanning cycle. Accurately calculate its collection time Relative to the end time of the frame scan The relative pose transformation is then performed. Subsequently, each point is compensated to the unified coordinate system at the end of the scan based on this transformation relationship. This compensation process can be expressed by the following formula:

[0032] Here, we assume that in a point cloud frame, the radar pose at the initial moment is... , No. When collecting data from a single laser point, the radar's pose is: , No. The coordinates of the laser points are .

[0033] S22, calculate the curvature of each point cloud after distortion correction, and divide the point cloud into feature corner points and feature plane points according to the curvature threshold.

[0034] Calculate the curvature of each laser point, and divide the point cloud into feature corner points and feature plane points according to the curvature threshold. Calculate the curvature of each point separately. The calculation formula is:

[0035] in, Representing the In the first frame of the point cloud There are 1 data point, S represents 1 data point. The set of neighboring points.

[0036] S23, the distortion-free point cloud is divided into multiple sub-regions, each sub-region including a preset number of feature corner points and feature plane points, and feature points are selectively retained or removed for laser probability update.

[0037] To ensure uniform feature distribution, the point cloud is divided into multiple sub-regions, each containing a fixed number of feature corner points and feature plane points.

[0038] To optimize computational efficiency, this embodiment employs an adaptive laser odometry probability update strategy that selectively retains or removes feature points during the update process. The laser probability update formula is:

[0039] in, The current moment; For feature points The initial moment when it was observed; For feature points Distance at the current moment; In the time window Average distance within; and It is an adaptive threshold based on feature point retention time and positional deviation, and the specific calculation formula is as follows:

[0040] Where m = 1 or 2, For time window The set of feature points that meet the conditions. Indicates the number of feature points; This represents the matching time or positional deviation of feature point k; when m=1, ,in For the current moment, This represents the first observation time of feature point k, reflecting the retention time of the feature point. When m=2, , For time window The average distance within reflects the positional deviation of the feature point.

[0041] This formula comprehensively considers the retention time and matching distance of feature points, and enhances the adaptability of the laser odometry module by dynamically adjusting the retention probability. In a specific embodiment, the optical feature point retention probability threshold... It is 0.75, when If a feature point is deemed to have high reliability and stability under the current environment, it is retained; otherwise, it is removed to reduce subsequent computation and potential errors.

[0042] In one embodiment, the visual probability update based on real-time pose estimation in step S2 includes the following steps: S24, The ORB algorithm is used to create feature vectors for feature points in the image.

[0043] S25, calculate the retention probability based on the matching distance deviation and the frequency of feature point occurrence, and perform visual probability update.

[0044] An ORB (Oriented Fast and Rotated BRIEF) algorithm is employed to rapidly create feature vectors from keypoints in the image, along with an adaptive visual odometry probability update method to dynamically adjust the feature point retention strategy. To adapt to dynamic changes in different environments and improve the adaptability of the visual odometry module in complex environments, the retention probability is calculated based on the matching distance deviation and the frequency of feature point occurrence. The formula for updating the visual probability is as follows:

[0045] in, The current moment; The initial time when feature point i is observed; is the Hamming distance of the feature point descriptor; c is the number of times the feature point appears in consecutive frames; and These are the thresholds adaptively adjusted based on matching distance deviation and the thresholds adaptively adjusted based on the frequency of feature point occurrence, respectively. The specific calculation formulas are as follows:

[0046] Where m = 1 or 2, For time window The set of feature points that meet the conditions. Time window The set of feature points that meet the conditions; This represents the matching distance deviation or frequency error of feature point k; when m=1, ,in For the current moment, This represents the deviation between the Hamming distance of feature point k and the average Hamming distance within the window. When m=2, This refers to the deviation between the number of occurrences of feature point k and the average number of occurrences within the window. This formula comprehensively considers the matching distance and frequency deviation of feature points, and enhances the adaptability of the visual odometry module by dynamically adjusting the retention probability.

[0047] In one specific embodiment, the visual feature point retention probability threshold It is 0.65, when If a feature point is deemed to have high reliability and stability under the current environment, it is retained; otherwise, it is removed to reduce subsequent computation and potential errors.

[0048] S3. Construct the laser point cloud matching residual, visual reprojection residual, IMU pre-integration residual, and edge-difference residual. Based on the laser point cloud matching residual, visual reprojection residual, IMU pre-integration residual, and edge-difference residual, construct the residual equation and use a sliding window for nonlinear optimization to obtain the robot pose estimate.

[0049] Figure 3 The flowchart for sliding window multi-source data nonlinear optimization provided in the embodiments of the present invention is as follows: Figure 3 As shown, step S3 includes: S31, construct the laser point cloud matching residual, which includes point-line residual terms and point-surface residual terms.

[0050] After laser point cloud distortion correction, feature extraction, and inertial navigation data fusion, sets of line feature point clouds and area feature point clouds are obtained. In this embodiment, the current frame is matched with the constructed local map using a scan-to-map method. During the matching process, real-time pose estimation provided by the IMU is used for preliminary estimation to compensate for pose changes between laser frames. Specifically, in the optimization, the distances from points to lines and from points to areas are solved using attitude and motion predictions provided by the IMU, thereby constructing line feature point residual constraint terms (i.e., point-line residual terms) and area feature point cloud residual constraint terms (i.e., point-area residual terms). Figure 4 The laser line-surface feature map provided in the embodiments of the present invention, the line feature association and the surface feature association are respectively as follows: Figure 4 (a) and Figure 4 As shown in (b), a coarse pose estimate of the robot is obtained through Gauss-Newton nonlinear optimization. The minimum distance from a point to a line and the minimum distance from a point to a surface are solved separately.

[0051] in, The minimum distance from a point to a line. The minimum distance from a point to a surface. , , for The three nearest but non-collinear points; The residual terms of the point and line were solved using the chain derivative method. Point-to-surface residuals ;

[0052] in, For point-line residuals, For point-to-surface residuals, Optimize the pose variables.

[0053] S32, construct the visual reprojection residual.

[0054] In visual observation, the real-time pose estimation provided by IMU inertial navigation information is used as the initial value to compensate for inter-frame motion, so that the visual system can maintain consistency in the projection of the same spatial feature point in multiple frames. Figure 5 A schematic diagram of visual reprojection error provided for an embodiment of the present invention.

[0055] Three-dimensional spatial point coordinate representation The projected pixel coordinates in the two-dimensional plane are , three-dimensional space points Projected onto the i-th frame, the pixels of the j-th frame are respectively and , The estimated pixel point after coordinate transformation to frame j is: Estimate feature points The value in the camera coordinate system of the j-th frame is:

[0056] in, and These are the transformation matrices between the IMU and the world coordinate system, and the transformation matrix between the camera and the IMU. For inverse depth, For feature points The value in the camera coordinate system of the j-th frame. Let be the coordinates on the normalized plane of the camera in the i-th frame; The visual reprojection error from frame i to frame j is:

[0057] in, For feature points The actual pixel coordinates on the j-th frame of the image.

[0058] S33, construct the IMU pre-integrated residual.

[0059] Factor graph modeling is employed, consisting of laser odometry factors, visual odometry factors, IMU pre-integration factors, prior factors, and state node factors. Each factor graph node is associated with the robot's motion state. Since the IMU generates relatively smooth, noisy measurements with few outliers, the relative motion between each node is obtained through IMU pre-integration. Therefore, the relative pose estimation calculated by laser and visual odometry is used to obtain the current robot pose, rotation, and other variables as local constraints to correct IMU pre-integration bias. The formula for calculating the IMU pre-integration residual is as follows:

[0060] in, , and Translation, velocity, and rotation are obtained from laser inertial odometry and visual inertial odometry. It is the gravity vector; This is to account for the deviation of the accelerometer and gyroscope; Operators for IMU residuals; For IMU pre-integration residuals, The rotation matrix is ​​used to transform quantities in world coordinates to the coordinate system of the i-th frame. The linear velocity of the world system at time i-th frame. Let be the pose at time j. Let be the pose at time j. The acceleration at time i is zero bias. The gyroscope has zero bias at time i-th frame. The relative translation obtained by integrating the IMU over the time interval from i to j. , For the relative rotation from time interval i to j;

[0061] To correct for IMU pre-integration measurement errors, the relative pose estimated by laser and vision is used as a local constraint to further compensate for biases in IMU pre-integration. The IMU odometry residual is generated from the residual constructed by vision and IMU (VIO residual) and the residual constructed by LiDAR and IMU (LIO residual). Specifically, both the VIO residual (Visual-Inertial Odometry Residual) and the LIO residual (Lidar-Inertial Odometry Residual) are essentially correction constraints for the relative pose during IMU pre-integration. The IMU odometry residual is generated by comparing the pose obtained from IMU pre-integration with the relative poses estimated by vision (VIO) and LiDAR (LIO) respectively, forming VIO residuals and LIO residuals, and then weighted and fused.

[0062] The final IMU odometry residual is obtained by adding the VIO residual to the LIO residual. Its expression is:

[0063] in, For LIO residuals, For VIO residuals, This refers to the IMU odometry residual.

[0064] S34, Adaptive Degradation Weight Judgment Factor Strategy.

[0065] The determination of whether a degradation scene has been reached is based on the point cloud corner points, planar points, and image feature points processed by the front end. The following two formulas are the laser and the visual weight calculation formulas, respectively:

[0066] in, and These are the number of edge and planar features identified, respectively. and It is the maximum number of features in the experiment; It is a lower bound of the function to prevent the laser odometry from failing under conditions of extreme feature loss; and when The smaller the value, the richer the laser feature points and the higher the confidence level. This indicates that the number of laser feature points may be abnormal, and therefore the environment may be degraded. This feature scarcity is used to quantify the covariance matrix of laser odometry. The impact.

[0067]

[0068] in, It is the number of pixel features identified. It is the maximum number of features in the experiment. It is a lower bound of the function to prevent visual odometry from failing under conditions of extreme feature loss; and when The smaller the value, the richer the visual feature points and the higher the confidence level. This indicates that the number of visual feature points may be abnormal, suggesting a possible degradation environment. This is used to quantify the feature weight factor visual odometry covariance matrix. The impact.

[0069] Therefore, laser odometry and visual odometry can be based on the calculated... , This is used to determine the confidence levels of visual odometry and lidar odometry regarding system constraints. Their covariance expression is:

[0070] S35, Factor graph optimization solution In optimization, marginalization is often used to remove the influence of some variables on the objective function while retaining their constraint information on other variables. It is especially common in state marginalization in sliding window optimization (such as VIO or SLAM). After marginalization, the original residual is transformed into a new residual (called the marginalization residual).

[0071] definition Variables to be marginalized The variables to be retained. First, perform a first-order Taylor expansion at the current linear point. Perform a first-order approximation at:

[0072] in, Let r be the residual function, and r be the value at the current linearization point. The residual value at that point, For the residual function with respect to Jacobian matrix, For the residual function with respect to Jacobian matrix, For variables The increment, For variables The increment.

[0073] Define the Jacobian matrix concatenation form:

[0074] Therefore, the quadratic optimization problem after linearization is:

[0075] Construct the Hessian matrix H and the gradient vector b:

[0076] in, The corresponding Hessian matrix The block, The corresponding Hessian matrix and The cross block, The corresponding Hessian matrix and The cross block, The corresponding Hessian matrix The block, b is the gradient vector. For the gradient vector corresponding to Part of For the gradient vector corresponding to The part.

[0077] Therefore, the normal equation is:

[0078] Using Schur Complement, we hope to eliminate , get only with A related new optimization problem. Elimination using the Schur complement formula: Define the marginalized information matrix. and the right-hand vector :

[0079] This is the equivalent information after marginalizing the retained variables. The marginalized information matrix and vector obtained above can be re-represented as a quadratic form:

[0080] in, To preserve variables The increment vector, where const is the constant term.

[0081] We want to reconstruct a residual term , to satisfy:

[0082] This can be obtained through Cholesky decomposition:

[0083] Where L is the Cholesky decomposition factor.

[0084] The marginalization residual is constructed as follows:

[0085] Where: d is a vector. The final marginalization residual is:

[0086] A tightly coupled approach is used to fuse laser odometry, and the relative pose estimation of visual odometry constrains the IMU pre-integration measurements. Figure 6 This is a schematic diagram of a sliding window factor graph model for multi-sensor fusion provided in an embodiment of the present invention, as shown below. Figure 6 As shown, in factor graph optimization, a sliding window is introduced to backtrack historical information, avoiding the loss of a large amount of historical state information. This minimizes the visual reprojection error. Laser line surface feature residual , IMU pre-integration error and marginalized residuals The optimization problem is expressed by the residual equation as follows:

[0087] When the lidar degrades (weakens geometric features), the laser degradation factor is small, and the residual difference in the laser odometry is large. When the weighting is small and the confidence level of laser odometry is low, visual odometry should be the primary constraint. Similarly, when there is visual degradation (weak texture), the visual degradation factor is small, and the visual odometry residuals differ significantly. If the weights are small and the confidence level of visual odometry is low, then laser odometry constraints should be the primary method to improve the pose estimation accuracy of the algorithm.

[0088] S4 performs visual loop closure detection, laser loop closure detection, and global pose optimization.

[0089] Figure 7 The loop closure detection flowchart provided in the embodiments of the present invention is as follows: Figure 7 As shown, step S4 includes the following steps: S41, use the DBoW2 algorithm to compare the current frame with historical keyframes to detect whether there is a loop relationship.

[0090] Among them, the DBoW2 algorithm can effectively reduce false positives in complex or weakly textured scenes.

[0091] S422, when a candidate loop closure frame that meets the similarity threshold is detected, the laser loop closure verification thread is started. The PL-ICP (point-to-line ICP) algorithm is used to calculate the point cloud matching error between the current frame and the candidate frame. A loop closure is confirmed only when the visual similarity and the laser matching error simultaneously meet their respective thresholds.

[0092] S43 adds loop closure as a new factor to the global pose graph and performs global optimization to correct the cumulative drift of the entire motion trajectory.

[0093] The positioning accuracy will be verified through specific experiments below: To verify the effectiveness of this algorithm, this embodiment uses in-vehicle sequences from the publicly available KITTI dataset and three real-world scenarios in different environments for validation. The algorithm's performance is evaluated by comparing the estimated values ​​with the ground truth trajectories from other algorithms, using Absolute Trajectory Error (APE) and Relative Trajectory Error (RPE). APE measures the overall trajectory deviation and is the mean Euclidean distance between corresponding points on the estimated and ground truth trajectories. RPE measures the relative error between adjacent points, evaluating local trajectory accuracy. These two standards provide effective references for evaluating the algorithm's performance from both global and local perspectives. The algorithm's performance is further evaluated using four metrics: Root Mean Square Error (RMSE), Mean, Median, and Standard Deviation (STD).

[0094] Simulation experiments verified the efficiency and accuracy of the LIVE-SLAM algorithm from four aspects: algorithm performance, algorithm time consumption, algorithm positioning accuracy, and comparison of accuracy with and without loop closure detection.

[0095] First, this section's experiments select the outdoor scene 07 sequence from the KITTI dataset to test the algorithm's performance. Figure 8 The diagram shows the execution results of sequence 07 provided in this embodiment of the invention, where (a) is the trajectory evaluation result and (b) is the point cloud map. Figure 8 As shown, the trajectory of sequence 07 is 1232m long, with 1101 frames, representing a degraded scene with few features and one loop closure. The SLAM algorithm proposed in this invention is used to run this sequence, outputting a point cloud map and saving the trajectory information. Finally, the data is compared with the ground truth trajectory data from KITTI. The average position error is 1.9101%, and the average angle error is 0.0082 degrees. It can be seen that the method provided in this invention improves the localization and mapping effects, and the output trajectory closely matches the real trajectory. The results show that the algorithm in this paper still performs well in degraded scenes and can achieve good closure at the loop closure position.

[0096] The LVI-SAM algorithm is widely used in mobile robot mapping and localization due to its fast mapping speed and high localization accuracy. Therefore, Table 1 lists the time consumption of each module in the LVI-SAM algorithm when running the 07 sequence and compares it with the LVI-SAM algorithm. In the front-end and back-end modules, the time consumption is approximately 49% and 25% higher than that of the LVI-SAM algorithm, respectively.

[0097] Table 1 shows the time consumption of each module in the LIVE-SLAM algorithm when running the 07 sequence.

[0098] Furthermore, the accuracy of the LIVE-SLAM algorithm is verified by comparing it with classic open-source algorithms such as A-LOAM, LIO-SAM, and LVI-SAM. The novelty of this paper is further validated through data optimization time comparison and loop closure detection. Four sequences (06, 07, 09, and 10) are selected for comparative experiments. These sequences include urban and rural road environments, multiple complex roads and multi-layered structures, and multiple loop closure points. Figure 9 The comparison diagram of the trajectories of various algorithms provided in the embodiments of the present invention is provided by Figure 9 As can be seen, the LIVE-SLAM algorithm is closer to the real trajectory than other algorithms. Table 2 shows the trajectory error data for each algorithm, demonstrating that the LIVE-SLAM algorithm performs well on all four sequences. Experimental results show that in most datasets, the accuracy of the LIVE-SLAM algorithm is superior to several currently available open-source SLAM algorithms. The mapping effect is relatively good, and the overall pose error is relatively low.

[0099] Table 2 Comparison of trajectory error data for various algorithms based on the KITTI dataset

[0100] Physical experiment, Figure 10 This is a schematic diagram of a mobile robot system provided in an embodiment of the present invention, including an Echo mobile platform, a Livox AVIA multi-line LiDAR, a RealSense D435i camera, a built-in IMU in the camera, and an R2000 radar. The sensors are as follows: Figure 10 As shown; the computing platform is an Acer laptop with a 12th Gen Intel(R) Core(TM) i5-12500H CPU, 16GB of RAM, and an Ubuntu 18.04 system with the ROS melodic software framework.

[0101] Vehicle-mounted experiments were conducted on campus, testing in easily degraded scenarios, dimly lit scenarios, and large outdoor scenarios. To further verify the effectiveness of the proposed loop closure detection process in the algorithm, scenarios with multiple loop closures were selected to compare and verify the improvement in positioning accuracy. The algorithm of this invention was compared with LVI-SAM and LIO-SAM (with loop closure detection modules) and the proposed algorithm LIVE-SLAM* (without loop closure detection modules). Table 3 shows the APE results for a long indoor corridor. The coordinate information acquired by the R2000 radar using a reflector-based trilateration method was used as the reference ground truth for this experiment, achieving an accuracy of ±3mm. Finally, the accuracy of the algorithm was verified using the Absolute Track Error (APE) results. The algorithm performance was evaluated using four indicators: Root Mean Square Error (RMSE), Mean, Median, and Standard Deviation (STD).

[0102] Interior corridor construction plan: An indoor long corridor was selected. This corridor is a closed-loop rectangular corridor consisting of four long corridors, with a total length of approximately 270m. This corridor exhibits consistent geometric height and is a typical indoor degradation scene. The mapping and localization performance of the LIVE-SLAM algorithm in a dimly lit indoor scene was verified. A comparative analysis was conducted on the localization trajectories of the LIO-SAM, LIV-SAM, LIVE-SLAM, and LIVE-SLAM* algorithms in the indoor long corridor. Figure 11 The image shows the evaluation map of the localization trajectories of various algorithms in an indoor long corridor provided by an embodiment of the present invention. As can be seen from the image, the LIVE-SLAM algorithm can still complete high-quality mapping in geometrically similar but degraded scenarios such as indoor long corridors, and the motion trajectory has a high degree of consistency with the real trajectory. In the experiment, the RMSE value of the LIVE-SLAM algorithm was 0.20m, which improved the localization accuracy by approximately 28.6% and 9.1% compared to the LIO-SAM and LIV-SAM algorithms, respectively. Compared to LIVE-SLAM*, the addition of a laser and vision hybrid detection module improved the algorithm accuracy by 20%.

[0103] Underground parking lot: The campus underground parking lot was selected, with a mapping route approximately 200m long, to verify the mapping and localization performance of the LIVE-SLAM algorithm in dimly lit indoor scenes. The mapping trajectories of the LIO-SAM, LIV-SAM, and LIVE-SLAM algorithms in the campus underground parking lot were analyzed. Figure 12 The evaluation diagrams of the positioning trajectories of various algorithms in the underground parking lot provided in this embodiment of the invention are shown in Table 4, where the APE parameters are as follows. In the experiment, the RMSE value of the algorithm in this paper is 0.11m. Due to the greater constraints and influences on the camera in this environment, the performance of the LV system is not as good as that of LIO-SAM. However, the algorithm proposed in this invention has a very small performance difference from LIO-SAM, and its accuracy is improved by 15.4% compared to LVI-SAM. The addition of the laser and vision hybrid detection module improves the algorithm accuracy by 27%.

[0104]

[0105] Open square: The selection process involves complex, large-scale outdoor scenes, including those with structural degradation and dim lighting, such as... Figure 13 As shown, the route is approximately 420m long, designed to verify the efficient mapping and localization performance of the LIVE-SLAM algorithm in large-scale scenes and with large amounts of data. The localization trajectories of the LIO-SAM, LIV-SAM, and LIVE-SLAM algorithms in open, large-scale scenes are analyzed. Figure 13 The evaluation map of the positioning trajectory of each algorithm in an open square provided in the embodiments of the present invention is composed of... Figure 13 It is evident that the LIVE-SLAM algorithm, combined with the proposed laser-visual hybrid detection module, significantly improves loop closure performance and reduces cumulative error in complex, large-scale scenes, ensuring the localized trajectory is nearly identical to the reference trajectory. The APE results are shown in Table 5. In the experiment, the RMSE value of the proposed algorithm was 0.21m, representing improvements of approximately 72.3% and 52.2% in localization accuracy compared to the LIO-SAM and LIV-SAM algorithms, respectively. Therefore, in complex outdoor scenes, the proposed algorithm offers higher localization accuracy and lower cumulative error compared to the LIO-SAM algorithm.

[0106] Table 5 APE Results for Open Plazas

[0107] The above three sets of experimental results show that the algorithm in this paper has excellent anti-degradation performance. When facing scenarios such as degradation, dim lighting, and large outdoor scenes, its positioning accuracy is more outstanding compared with other open source algorithms.

[0108] Based on the method described in the above embodiments, this embodiment will further describe it from the perspective of a synchronous localization and mapping system based on multi-sensor fusion. Specifically, the synchronous localization and mapping system based on multi-sensor fusion can be implemented as an independent entity or integrated into an electronic device. The electronic device can be a terminal, server, or other device. The terminal can include a tablet computer, a laptop computer, a personal computer (PC), a microprocessor box, or other devices.

[0109] This invention also provides a synchronous localization and mapping system based on multi-sensor fusion, applicable to electronic devices. This synchronous localization and mapping system based on multi-sensor fusion may include: The acquisition module is used to acquire point cloud data, IMU data, and images measured by the lidar. The front-end data preprocessing module is used to pre-integrate the IMU data to obtain real-time pose estimation, and perform laser probability update and visual probability update based on the real-time pose estimation to obtain an updated local map. The backend optimization module is used to construct the laser point cloud matching residual, the visual reprojection residual, the IMU pre-integration residual, and the edge-difference residual. Based on the laser point cloud matching residual, the visual reprojection residual, the IMU pre-integration residual, and the edge-difference residual, the residual equation is constructed, and a sliding window is used for nonlinear optimization to obtain the robot pose estimate. The loop closure detection module is used for visual loop closure detection, laser loop closure detection, and global pose optimization.

[0110] In specific implementation, the above modules and / or units can be implemented as independent entities, or they can be arbitrarily combined and implemented as the same or several entities. For the specific implementation of the above modules and / or units, please refer to the previous method embodiments. For the specific beneficial effects that can be achieved, please also refer to the beneficial effects in the previous method embodiments, which will not be repeated here.

[0111] In addition, this embodiment of the invention also provides an electronic device, which may be a computer, tablet computer, or other similar device. This electronic device can implement the steps of any embodiment of the synchronous localization and mapping method based on multi-sensor fusion provided in this embodiment of the invention. Therefore, it can achieve the beneficial effects that any synchronous localization and mapping method based on multi-sensor fusion provided in this embodiment of the invention can achieve, as detailed in the preceding embodiments, and will not be repeated here.

[0112] This electronic device can be used to implement the synchronous localization and mapping method based on multi-sensor fusion provided in the above embodiments. The electronic device can be a terminal, server, or other such device. The terminal can include a tablet computer, laptop computer, personal computer (PC), microprocessor box, or other devices.

[0113] The electronic device includes at least a memory and a processor. The memory can be used to store software programs and modules, such as the corresponding program instructions / modules in the above embodiments. The processor executes various functional applications and data processing by running the software programs and modules stored in the memory 520.

[0114] Those skilled in the art will understand that all or part of the steps in the various methods of the above embodiments can be implemented by instructions, or by instructions controlling related hardware. These instructions can be stored in a computer-readable storage medium and loaded and executed by a processor. Therefore, embodiments of the present invention provide a storage medium storing multiple instructions that can be loaded by a processor to execute the steps of any embodiment of the synchronous localization and mapping method based on multi-sensor fusion provided by the present invention.

[0115] The computer-readable storage medium may include: read-only memory (ROM), random access memory (RAM), disk or optical disk, etc.

[0116] Since the instructions stored in the storage medium can execute the steps in any embodiment of the synchronous localization and mapping method based on multi-sensor fusion provided in the embodiments of the present invention, the beneficial effects that any synchronous localization and mapping method based on multi-sensor fusion provided in the embodiments of the present invention can achieve can be realized. For details, please refer to the previous embodiments, which will not be repeated here.

[0117] The foregoing has provided a detailed description of a synchronous localization and mapping method, system, storage medium, and electronic device based on multi-sensor fusion provided by the embodiments of the present invention. Specific examples have been used to illustrate the principles and implementation methods of the present invention. The descriptions of the embodiments above are only for the purpose of helping to understand the method and core ideas of the present invention. At the same time, for those skilled in the art, there will be changes in the specific implementation methods and application scope based on the ideas of the present invention. Therefore, the content of this specification should not be construed as a limitation of the present invention.

Claims

1. A method for simultaneous localization and mapping based on multi-sensor fusion, characterized in that, The method comprises: acquiring IMU data, image and point cloud data of lidar measurement; pre-integrating the IMU data to obtain real-time pose estimation, performing laser probability updating and visual probability updating based on the real-time pose estimation respectively to obtain an updated local map; constructing laser point cloud matching residual error, visual re-projection residual error, IMU pre-integration residual error and marginalization residual error, constructing residual equation based on the laser point cloud matching residual error, the visual re-projection residual error, the IMU pre-integration residual error and the marginalization residual error, performing nonlinear optimization by using a sliding window to obtain robot pose estimation; performing visual loop detection, laser loop detection and global optimization of pose.

2. The multi-sensor fusion based simultaneous localization and mapping method of claim 1, wherein, The laser probability updating based on real-time pose estimation comprises: compensating the point cloud data by using real-time pose estimation to obtain de-distorted point cloud; calculating the curvature of each de-distorted point cloud, and dividing the point cloud into feature edge corner points and feature plane points according to the curvature threshold; dividing the de-distorted point cloud into a plurality of sub-regions, each sub-region including a preset number of feature edge corner points and feature plane points, and selectively retaining or rejecting feature points to perform laser probability updating.

3. The multi-sensor fusion based simultaneous localization and mapping method of claim 1, wherein, The visual probability updating based on real-time pose estimation comprises: creating feature vectors for feature points in the image by using an ORB algorithm; calculating retention probability according to matching distance deviation and frequency of occurrence of feature points to perform visual probability updating.

4. The multi-sensor fusion based simultaneous localization and mapping method of claim 1, wherein, The formula of the laser probability updating is: wherein, is the current time, is a feature point an initial time at which the feature point was observed, is a feature point a distance at the current time, is an average distance over a time window , and is an adaptive threshold based on feature point retention time and position deviation. The formula of the visual probability updating is: wherein, is the current time, is the initial time when the feature point i is observed, is the Hamming distance of the feature point descriptor, c is the number of times the feature point appears in consecutive frames, and are the threshold values based on adaptive adjustment of the matching distance deviation and the threshold value based on adaptive adjustment of the frequency of appearance of the feature point, respectively.

5. The multi-sensor fusion based simultaneous localization and mapping method of claim 1, wherein, The laser point cloud matching residual error comprises point-line residual error and point-plane residual error, and the calculation process is: respectively solving the minimum distance from a point to a line and the minimum distance from a point to a plane: wherein, is the minimum distance from a point to a line, is the minimum distance from a point to a plane, , , is the three nearest but not collinear points; Point-line residual term is solved by chain rule Point-plane residual term; wherein, is a point-line residual term, is a point-plane residual term, is a pose optimization variable.

6. The multi-sensor fusion based simultaneous localization and mapping method of claim 1, wherein, The calculation process of the visual re-projection residual error comprises: The three-dimensional space point The pixel points of the i-th frame and the j-th frame are projected respectively as And , The estimated pixel point of the j-th frame after coordinate transformation is The estimated feature point The value in the j-th frame camera coordinate system is: wherein, and Rwi and Rci are the transformation matrices between the IMU and the world coordinate system and between the camera and the IMU, respectively, is the inverse depth, is the feature point is the value of the feature point in the jth frame camera coordinate system, is the coordinate on the normalized plane of the ith frame camera; the visual re-projection error from the i-th frame to the j-th frame is: wherein, feature point actual pixel coordinates on the jth image.

7. The multi-sensor fusion based simultaneous localization and mapping method of claim 5 or 6, characterized in that, The calculation formula of the IMU pre-integration residual error is: wherein, , and are translations, velocities and rotations, obtained from laser inertial odometry and visual inertial odometry; is the gravity vector; is the bias of the accelerometer and gyroscope; is the operator of the IMU residual; is the IMU pre-integrated residual, is the rotation matrix that transforms quantities in the world coordinate to the i-th frame coordinate system, is the linear velocity in the world coordinate at the i-th frame time, is the pose at the j-th frame time, is the pose at the j-th frame time, is the acceleration bias at the i-th time, is the gyroscope bias at the i-th frame time, is the relative translation obtained by integrating the IMU from time i to j, , is the relative rotation from time i to j; generating the final IMU odometry residual error by using the residual error constructed by vision and IMU and the residual error constructed by lidar and IMU: wherein, is the LIO residual, i.e. the residual of the laser radar and the IMU construction, is the VIO residual, i.e. the residual of the vision and the IMU construction, is the IMU odometry residual; The residual equation is: wherein, is the edge residual, , is a weight factor; wherein the weight factor is calculated based on an adaptive degeneration weight judgment factor strategy.

8. The multi-sensor fusion based simultaneous localization and mapping method of claim 1, wherein, The visual loop detection, laser loop detection and global optimization of pose comprise: comparing the current frame and the historical key frame by using a DBoW2 algorithm to detect whether there is a loop relationship; after detecting a candidate loop frame satisfying a similarity threshold, starting a laser loop verification thread, calculating the point cloud matching error between the current frame and the candidate frame by using a PL-ICP algorithm, and confirming the loop only when the visual similarity and the laser matching error simultaneously satisfy respective corresponding thresholds; adding the loop as a new factor to the global pose graph and performing global optimization to correct the cumulative drift of the entire motion trajectory.

9. A multi-sensor fusion based simultaneous localization and mapping system, characterized in that, comprise: an acquisition module, configured to acquire point cloud data of lidar measurement, IMU data and image; a front-end data preprocessing module, configured to pre-integrate the IMU data to obtain real-time pose estimation, perform laser probability updating and visual probability updating based on the real-time pose estimation respectively to obtain an updated local map; The back-end optimization module is configured to construct a laser point cloud matching residual error, a visual re-projection residual error, an IMU pre-integration residual error and an edge residual error, construct a residual error equation based on the laser point cloud matching residual error, the visual re-projection residual error, the IMU pre-integration residual error and the edge residual error, and perform nonlinear optimization by using a sliding window to obtain a robot pose estimation. The closed-loop detection module is configured to perform visual loop detection, laser loop detection and global pose optimization.

10. A computer-readable storage medium, characterized in that, The computer readable storage medium stores a plurality of instructions adapted to be loaded by the processor to execute the method of simultaneous localization and mapping based on multi-sensor fusion according to any one of claims 1 to 7.

Images