A MAP-LIDAR-IMU fusion positioning method considering multi-mode switching

Abstract

This invention discloses a MAP-LIDAR-IMU fusion positioning method that takes into account multi-mode switching. The method includes: first, matching the LiDAR point cloud with a prior map using a normal distribution transform (NDT) algorithm to provide an accurate six-degree-of-freedom (6DoF) pose for the positioning framework. Inter-frame estimation of the laser odometry replaces the motion model to provide the initial pose for the NDT, addressing robustness issues in challenging scenarios such as high speed or rapid rotation. Second, to improve the robustness of global positioning, a multi-mode switching algorithm is designed to selectively trigger global matching and local laser odometry dead reckoning. Simultaneously, the LiDAR odometry observations, local IMU observations, and map matching absolute observations are fused to reduce cumulative error, maintain the globally optimized pose and keyframe queue, and continuously output high-precision vehicle positioning results. This method provides a reliable solution to the problem of long-term robust positioning, significantly improving both positioning robustness and accuracy compared to traditional map matching positioning methods.

Description

Technical Field

[0001] The present invention belongs to the technical field of multi-sensor fusion positioning, and in particular relates to a MAP-LIDAR-IMU (prior map-laser radar-inertial navigation unit) fusion positioning method taking into account multi-mode switching. Background Art

[0002] Traditional vehicle navigation systems using the Global Navigation Satellite System (GNSS) can achieve centimeter-level absolute positioning in open areas. However, these systems are susceptible to factors such as non-line-of-sight (NLOS) and multipath, limiting their application scenarios and posing significant challenges for vehicle navigation in complex environments. Therefore, providing continuous and effective global positioning for positioning systems in satellite signal-denied environments, such as complex underground spaces, remains a technical challenge.

[0003] With the rapid development of autonomous driving technology, positioning based on inexpensive and easy-to-deploy vehicle-mounted sensors such as lidar and cameras has made rapid progress in recent years. Compared to cameras, whose performance deteriorates in low-light and low-texture scenes, lidar has become more widely used due to its wide ranging range and rich information. Thanks to hardware iterations and increased computing power, the current mainstream lidar-based global positioning method uses a priori maps to determine the global pose by matching radar point cloud frames with the map, providing an effective alternative to GNSS.

[0004] However, this positioning method suffers from a high reliance on prior maps. In real-world scenarios, factors such as dynamic objects, dust, and human construction can cause environmental changes. These changes occur over varying timescales and are difficult to predict. Furthermore, traditional map-matching positioning methods are not easily scalable due to high maintenance costs and long update cycles. Summary of the Invention

[0005] To solve the above problems, the present invention discloses a MAP-LIDAR-IMU fusion positioning method that takes into account multi-mode switching. It utilizes the original observation information of LiDAR and IMU sensors and prior maps, fully integrates the characteristics of various positioning algorithms, and achieves complementary advantages.

[0006] To achieve the above object, the technical solution of the present invention is as follows:

[0007] A MAP-LIDAR-IMU fusion positioning method taking into account multi-mode switching includes the following steps:

[0008] Step 1: Radar-Inertial Odometry Local Pose

[0009] The motion distortion of the laser point cloud frame is removed through IMU high-frequency recursive estimation, while a lightweight radar-inertial odometry is implemented to provide a local positioning source for the system.

[0010] Step 2: NDT solves the global pose under the prior map

[0011] The odometry pose is globally recursively extrapolated, and the LiDAR is matched with the point cloud map based on the NDT algorithm, outputting the absolute positioning result of the vehicle in real time.

[0012] Step 3: Map matching positioning quality control

[0013] Map matching positioning also requires the voxel coverage ratio index and the average matching probability density index to evaluate the validity of the prior map and thus make a quality judgment on the matching results;

[0014] Step 4: Implementation of multi-mode switching mechanism

[0015] Based on the uncertainty estimation of map matching, a system mode switching mechanism is implemented, switching the local odometry positioning result as a temporary output when map matching fails;

[0016] Step 5: Factor Graph Optimization

[0017] The system uses factor graph optimization to integrate local odometry observations, IMU local observations, and provide multiple observation outputs of prior maps to solve the optimal state estimation, while maintaining the key frames of the local odometry.

[0018] The specific steps are:

[0019] Step 1: Radar-Inertial Odometry Local Pose

[0020] First, IMU high-frequency recursive estimation is used to remove motion distortion from the point cloud and simultaneously provide an initial odometry pose. Odometry is primarily achieved through a highly reusable two-stage registration process, employing scan-to-scan and scan-to-submap registration. Submaps are constructed using a keyframe queue. The keyframe queue is maintained through local odometry registration and map matching.

[0021] Step 2: NDT solves the global pose under the prior map

[0022] The map point cloud is evenly divided into m cubic grids using a preset resolution according to its size, i.e., the map is voxelized. The point cloud distribution of the i-th grid is approximated using a Gaussian function model. The normal probability density function of the point cloud mapping can be calculated as follows:

[0023]

[0024] Where μ i is the grid mean, σ i is the corresponding covariance, where q j＝1,...,nis the position of the jth point in the grid, C is a constant, (·) T Indicates transposition; during the convergence process, the initial rotation matrix R obtained according to the odometer ini and the translation vector p ini , the matching problem is modeled as a weighted least squares problem of all projection points of the laser frame:

[0025]

[0026] Where N is the predetermined number of nearest neighbors of a voxel. The Gauss-Newton method is used to optimize the convergence of the least squares problem, and the relocalization residual model of the ηth iteration is defined as follows:

[0027]

[0028] err is the residual term of all points, and J is the Jacobian matrix of the residual term with respect to the independent variable.

[0029] Step 3: Map matching positioning quality control

[0030] Voxel coverage ratio index: Based on the NDT matching algorithm, the point cloud frame is voxelized at the same resolution as the map, and the coverage between point pairs is quantified by the ratio of the repeated grid occupation of the two. map With N new Expressed as the number of repeated grids and newly added grids in the point cloud frame, the coverage point number Z is calculated with the grid as the statistical unit map and the number of uncovered points Z map :

[0031]

[0032] Where ξ is the sum of points in the grid volume, N ξ represents the number of grids with ξ as the number of inliers. Due to the discreteness of the grid boundary, there may be grids with extremely small number of inliers (p≤3), which are regarded as invalid grids, and the weight θ=0.1% is set to eliminate such errors.

[0033] Finally, the distribution can be covered by the number of points. The calculation ratio formula is as follows:

[0034]

[0035] Average matching probability density indicator: given a set of point clouds And the output pose, count the residual accumulated value Res after all the conversion points fall on the corresponding grid, and finally divide it by the number of points in the valid input point cloud to get the final average matching probability density:

[0036]

[0037] Where φ is the optimal pose transformation represented by Lie algebra, and the coordinates of the point q to be registered are transformed to the reference system as q trans .

[0038] Step 4: Implementation of multi-mode switching mechanism

[0039] Statistical voxel coverage ratio Λ after matching the kth frame k , and according to the ratio threshold Λ thd Conduct the first stage of uncertainty testing:

[0040]

[0041] Only Λ k When the threshold value Λ thd , that is, the ratio of overlapping points in the point cloud frame is high enough, it is considered that the matching has not fallen into local convergence and the prior map is reliable enough, then the second stage of detection can be entered. The confidence level of map matching is set according to the average matching probability density index required:

[0042]

[0043] In addition, the above two-stage solution is based on the convergence of the NDT algorithm. When the matching convergence fails or δ Δ =λ odom When the system switches to a temporary odometer to provide output results.

[0044] Step 5: Factor Graph Optimization

[0045] The state of the robot at the kth moment is written as a node, and the factors provide constraints on or between nodes. The formula is as follows:

[0046]

[0047] In the formula, r(·) represents the residual of each factor, The map matching factor that provides absolute constraints, the LiDAR inter-frame factor that provides relative constraints, and the measurement results of the IMU factor are respectively, is the covariance-weighted residual quadratic form.

[0048] The beneficial effects of the present invention are:

[0049] This paper proposes a MAP-LIDAR-IMU fusion positioning method that takes into account multi-mode switching. By utilizing a multi-mode switching mechanism and factor graph optimization, a multi-source fusion positioning algorithm for MAP / LiDAR / IMU is implemented, achieving real-time robust positioning output without relying on satellites. First, a lightweight laser-inertial odometry is integrated into a priori map global matching module to overcome the high degree of self-positioning in satellite-denied environments. Next, a multi-mode switching mechanism is proposed to selectively trigger a temporary odometry module based on environmental uncertainty, preventing positioning failures in areas with scene changes or insufficient map coverage, further improving positioning stability and robustness. BRIEF DESCRIPTION OF THE DRAWINGS

[0050] Figure 1 This is a flow chart of the multi-source fusion positioning algorithm based on MAP / LiDAR / IMU described in the present invention;

[0051] Figure 2 This is a schematic diagram of the implementation process of the radar-inertial odometer;

[0052] Figure 3 This is the principle diagram for implementing the voxel coverage ratio indicator;

[0053] Figure 4 This is a schematic diagram of the experimental equipment equipped with LiDAR / IMU;

[0054] Figure 5 It is a schematic diagram of the prior map used for positioning in the test example of the method of the present invention. DETAILED DESCRIPTION

[0055] The present invention will be further described below with reference to the accompanying drawings and specific embodiments. It should be understood that the following specific embodiments are only used to illustrate the present invention and are not used to limit the scope of the present invention.

[0056] As attached Figure 1 As shown, the present invention discloses a MAP-LIDAR-IMU fusion positioning method taking into account multi-mode switching, and the specific steps are as follows:

[0057] Step 1. Radar-inertial odometry local pose

[0058] First, the IMU high-frequency recursive estimation is used to remove the motion distortion of the point cloud and pass the initial pose for the odometry. The odometry is mainly implemented through a two-stage registration with high data reuse, using Scan-To-Scan registration and Scan-To-Submap registration, in which a keyframe queue is used to construct the submap. The keyframe queue is jointly maintained by local odometry registration and map matching. The specific implementation process is shown in the attached figure. Figure 2 shown.

[0059] Step 2. NDT solves the global pose under the prior map

[0060] The map point cloud is evenly divided into m cubic grids using a preset resolution according to its size, i.e., the map is voxelized. The point cloud distribution of the i-th grid is approximated using a Gaussian function model. The normal probability density function of the point cloud mapping can be calculated as follows:

[0061]

[0062] Where μ i is the grid mean, σ i is the corresponding covariance, where q j＝1,...,n is the position of the jth point in the grid, C is a constant, (·) T Indicates transposition; during the convergence process, the initial rotation matrix R obtained according to the odometer ini and the translation vector p ini , the matching problem is modeled as a weighted least squares problem of all projection points of the laser frame:

[0063]

[0064] Where N is the predetermined number of nearest neighbors of a voxel. The Gauss-Newton method is used to optimize the convergence of the least squares problem, and the relocalization residual model of the ηth iteration is defined as follows:

[0065]

[0066] err is the residual term of all points, and J is the Jacobian matrix of the residual term with respect to the independent variable.

[0067] Step 3. Map matching positioning quality control

[0068] Implement the index of point coverage ratio within voxel: Based on the NDT matching algorithm, the point cloud frame is voxelized at the same resolution as the map, and the coverage between point pairs is quantified by the ratio of the repeated grid occupation of the two, as shown in the attached figure. Figure 3 As shown. map With N new Expressed as the number of repeated grids and newly added grids in the point cloud frame, the coverage point number Z is calculated with the grid as the statistical unit map and the number of uncovered points Z map :

[0069]

[0070] Where ξ is the sum of points in the grid volume, N ξrepresents the number of grids with ξ as the number of inliers. Due to the discrete nature of grid boundaries, there may be grids with very small numbers of inliers (p≤3). These grids are considered invalid and weight θ = 0.1% is set to eliminate such errors. Finally, the distribution can be covered by the number of points. The calculation ratio formula is as follows:

[0071]

[0072] Implementing the average matching probability density indicator: Given a set of point clouds And the output pose, count the residual accumulated value Res after all the conversion points fall on the corresponding grid, and finally divide it by the number of points in the valid input point cloud to get the final average matching probability density:

[0073]

[0074] Where φ is the optimal pose transformation represented by Lie algebra, and the coordinates of the point q to be registered are transformed to the reference system as q trans .

[0075] Step 4. Implementation of multi-mode switching mechanism

[0076] Statistical voxel coverage ratio Λ after matching the kth frame k , and according to the ratio threshold Λ thd Conduct the first stage of uncertainty testing:

[0077]

[0078] Only Λ k When the threshold value Λ thd , that is, the ratio of overlapping points in the point cloud frame is high enough, it is considered that the matching has not fallen into local convergence and the prior map is reliable enough, then the second stage of detection can be entered. The confidence level of map matching is set according to the average matching probability density index required:

[0079]

[0080] In addition, the above two-stage solution is based on the convergence of the NDT algorithm. When the matching convergence fails or δ Δ =λ odom When the system switches to a temporary odometer to provide output results.

[0081] Step 5. Factor Graph Optimization

[0082] The state of the robot at the kth moment is set as a node, and the factors provide constraints on or between nodes. The formula is as follows:

[0083]

[0084] In the formula, r(·) represents the residual of each factor, The map matching factor that provides absolute constraints, the LiDAR inter-frame factor that provides relative constraints, and the measurement results of the IMU factor are respectively, is the covariance-weighted residual quadratic form.

[0085] As attached Figure 4 As shown in the figure, the 3D lidar model used is the Velodyne VLP32, with a sampling frequency of 10 Hz and 32 scan lines. The IMU device is the ADIS16488A, a six-axis inertial sensor. The prior point cloud map is obtained by processing with a Leica 3D laser scanner. In the actual vehicle-mounted experiment, the driving path contains complex scenes such as narrow corridors and complex structures that tend to degrade, and the RTK signal is severely interfered with. As shown in the attached figure, the 3D lidar model used is the Velodyne VLP32, with a sampling frequency of 10 Hz and 32 scan lines. The IMU device is the ADIS16488A, a six-axis inertial sensor. The prior point cloud map is obtained by processing with a Leica 3D laser scanner. In the actual vehicle-mounted experiment, the driving path contains complex scenes such as narrow corridors and complex structures that tend to degrade, and the RTK signal is severely interfered with. Figure 5 As shown in the figure, the test case uses part of the map before scanner stitching to simulate the extreme situation in the outdated map scene. The positioning accuracy (root mean square error) of the multi-source fusion system integrating MAP / LiDAR / IMU is 0.18m, which is an improvement of 58.1%.

[0086] It should be noted that the above content merely illustrates the technical idea of the present invention and cannot be used to limit the scope of protection of the present invention. For ordinary technicians in this technical field, several improvements and modifications can be made without departing from the principles of the present invention. These improvements and modifications all fall within the scope of protection of the claims of the present invention.

Claims

1. A MAP-LIDAR-IMU fusion positioning method taking into account multi-mode switching, characterized by: The following steps are involved: Step 1: Radar-Inertial Odometry Local Pose The motion distortion of the laser point cloud frame is removed through IMU high-frequency recursive estimation, and a lightweight radar-inertial odometer is implemented to provide a local positioning source for the system; the details are as follows First, IMU high-frequency recursive estimation is used to remove motion distortion of the point cloud and provide the initial pose for the odometry. Odometry is achieved through a two-stage registration with high data reuse, using Scan-To-Scan registration and Scan-To-Submap registration. Keyframe queues are used to construct submaps. The keyframe queues are maintained through local odometry registration and map matching. Step 2: NDT solves the global pose under the prior map The local odometer pose is globally recursively extrapolated, and the lidar and point cloud map are matched based on the NDT algorithm to output the absolute positioning result of the vehicle in real time. Step 3: Map matching positioning quality control Map matching positioning also requires the voxel coverage ratio index and the average matching probability density index to evaluate the validity of the prior map and thus make a quality judgment on the matching results; the details are as follows: Voxel coverage ratio index: Based on the NDT matching algorithm, the point cloud frame is voxelized at the same resolution as the map, and the coverage between point pairs is quantified by the ratio of the repeated grid occupation of the two. map With N new Expressed as the number of repeated grids and newly added grids in the point cloud frame, the coverage point number Z is calculated with the grid as the statistical unit map and the number of uncovered points Z new : Where ξ is the sum of points in the grid volume, N ξ represents the number of grids with inliers ξ; due to the discreteness of grid boundaries, there may be grids with inliers p≤3, which are considered invalid grids, and the weight θ = 0.1% is set to eliminate such errors; Finally, the distribution can be covered by the number of points. The calculation ratio formula is as follows: Average matching probability density indicator: given a set of point clouds And the output pose, count the residual accumulated value Res after all the conversion points fall on the corresponding grid, and finally divide it by the number of points in the valid input point cloud to get the final average matching probability density: In the formula, the coordinates of the point q to be registered are transformed into the reference system and expressed as q trans ; Step 4: Implementation of multi-mode switching mechanism Based on the uncertainty estimation of map matching, a system mode switching mechanism is implemented to switch the local odometry positioning result as a temporary output when map matching fails. The details are as follows: Statistical voxel coverage ratio Λ after matching the kth frame k , and according to the ratio threshold Λ thd Conduct the first stage of uncertainty testing: Only Λ k When the threshold value Λ thd , that is, the ratio of overlapping points in the point cloud frame is high enough, it is considered that the matching has not fallen into local convergence and the prior map is reliable enough, and then enter the second stage of detection; the confidence level of the map matching is set according to the average matching probability density index required: In addition, the above two-stage solution is based on the convergence of the NDT algorithm. When the matching convergence fails or δ Δ =λ odom When , the system will switch to the temporary odometer to provide output results; Step 5: Factor Graph Optimization The system uses factor graph optimization to integrate local odometry observations, IMU local observations, and provide multiple observation outputs of the prior map to solve the optimal state estimation while maintaining the key frames of the local odometry. The details are as follows: The state of the vehicle at the kth moment is written as a node, and the factors provide constraints on or between nodes. The formula is as follows: In the formula, r(·) represents the residual of each factor, These are the measurement results of the map matching factor that provides absolute constraints, the LiDAR inter-frame factor that provides relative constraints, and the IMU factor.

2. A MAP-LIDAR-IMU fusion positioning method taking into account multi-mode switching according to claim 1, characterized in that: The NDT described in step 2 solves the global pose under the prior map The map point cloud is evenly divided into m cubic grids using the preset resolution according to its size, i.e., the map is voxelized. The Gaussian function model is used to approximate the point cloud distribution of the i-th grid. The normal probability density function of the point cloud mapping is calculated as follows: Where μ i is the grid mean, σ i is the corresponding covariance, where q j is the position of the jth point in the grid, and C is a constant; (·) T Indicates transposition; during the convergence process, the initial rotation matrix R obtained according to the odometer ini and the translation vector p ini , the matching problem is modeled as a weighted least squares problem of all projection points of the laser frame: Where N is the predetermined number of nearest neighbors of a voxel; the Gauss-Newton method is used to optimize the convergence of the least squares problem, and the relocalization residual model of the ηth iteration is defined as follows: err is the residual term of all points, and J is the Jacobian matrix of the residual term with respect to the independent variable.

Images