A dual matching robust laser positioning method and system applied to dynamic environment
By constructing a static prior map in a dynamic environment and combining IMU pre-integration and GTSAM optimization, the problem of inaccurate positioning of laser inertial odometry in dynamic environments is solved, realizing a highly robust laser positioning method that is applicable to autonomous moving robots such as unmanned vehicles.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- XI AN JIAOTONG UNIV
- Filing Date
- 2023-08-17
- Publication Date
- 2026-05-12
AI Technical Summary
In dynamic environments, existing laser inertial odometry systems suffer from inconsistencies between laser point clouds and prior point cloud maps due to the presence of moving objects in the environment, which reduces the robustness and accuracy of positioning. Furthermore, GNSS systems cannot effectively correct matching errors when signals are lost in urban environments and tunnels.
A dual-matching robust laser positioning method is adopted. By constructing a static prior map, prior pose information is obtained by IMU pre-integration, and the first and second odometry matching are performed. GTSAM is combined for backend optimization to enhance the robustness of positioning.
Maintaining convergence in dynamic environments improves the accuracy of laser point cloud matching and the robustness of the system, reduces matching errors, and adapts to the positioning needs of unmanned vehicles in complex environments.
Smart Images
Figure CN117075140B_ABST
Abstract
Description
Technical Field
[0001] This invention belongs to the field of robot autonomous positioning technology, specifically relating to a dual-matching robust laser positioning method and system for dynamic environments. Background Technology
[0002] Localization is a fundamental technology in robotics, especially for autonomous robots such as drones, unmanned vehicles, and AGVs. A common localization paradigm involves the robot first acquiring a map of its own operating range—a prior map—and then performing localization based on the matching relationship between the robot's environmental sensors and the map. SLAM (Simultaneous Localization and Mapping) is a technology that relies on the robot's perception of its environment to build various types of maps and perform localization on these maps without relying on artificial beacons. It has been widely applied in unmanned vehicles in recent years and is categorized into LiDAR-SLAM and Visual SLAM, depending on the sensors used. Among the many sensor combinations available, such as monocular cameras, RGB-D cameras, LiDAR, wheel speedometers, and inertial measurement units (IMUs), the combination of LiDAR and IMU inertial localization technology stands out in unmanned vehicle applications due to its high accuracy and robustness. Although LiDAR measurements are sufficiently accurate, the estimation error of laser odometry (LO) and laser inertial odometry (LIO) systems still increases over time when estimating the robot's state. Furthermore, both LO and LIO are based on the static assumption of the environment, meaning that there are no moving objects in the environment during the entire algorithm's operation. This assumption is unreasonable for real-world robots such as autonomous vehicles. When autonomous vehicles move in a real dynamic environment, the inconsistency between the real-time acquired laser point cloud and the prior point cloud map not only increases the running time of the matching algorithm but also introduces non-matching points in the laser point cloud or prior point cloud map, thereby reducing the robustness and accuracy of the odometry.
[0003] Global Navigation Satellite System (GNSS) is commonly used in both autonomous and conventional vehicles to quickly acquire global positioning information, thereby correcting matching errors between laser point clouds and positioning algorithms. GNSS is a satellite-dependent global positioning system that relies on man-made mobile beacons, i.e., satellites. Currently, differential signal-based GNSS offers high accuracy (down to the centimeter level), and commonly used SLAM datasets like KITTI and TUM utilize RTK-GNSS as the ground truth for positioning. While RTK-GNSS systems offer high positioning accuracy and low relocation overhead, their accuracy degrades in urban environments due to multipath effects, and they completely lose positioning signals in tunnels and indoor environments—unacceptable for autonomous vehicles potentially operating in complex environments. Summary of the Invention
[0004] To address the problems existing in the prior art, this invention provides a more robust localization method that simultaneously utilizes the matching relationship between laser point clouds and prior point cloud maps and the relative pose relationship between laser point clouds and local point cloud maps constructed during algorithm execution.
[0005] To achieve the above objectives, the technical solution adopted by this invention is: a dual-matching robust laser positioning method applied in dynamic environments, comprising the following steps:
[0006] Constructing static prior maps in dynamic scenarios;
[0007] The prior pose information of the next laser frame in the static prior map is obtained by IMU pre-integration;
[0008] The first odometry matching is performed using the real-time obtained laser point cloud and the prior pose information to determine whether the odometry has converged. If it has not converged, the odometry will use the result of the first matching as the initial value for the second matching.
[0009] The second matching is performed by calculating the matching relationship between the laser point cloud and the local point cloud map. When the first odometry matching is accurate, the second odometry will immediately determine convergence; when the first odometry matching is not accurate enough, the second odometry will supplement and iterate to optimize the first odometry.
[0010] Finally, GTSAM was used for backend optimization, fusing the two odometry systems and updating all poses;
[0011] Once the final odometer result is optimized, the current laser point cloud is moved to the position indicated by the odometer, a local point cloud map is constructed and saved, and used as the object for the second matching.
[0012] After a complete localization process is completed, record all local point cloud maps and corresponding pose calculation static prior maps.
[0013] Furthermore, obtaining the prior pose information of the next laser frame in the static prior map through IMU pre-integration includes:
[0014] Using the acceleration and angular velocity information obtained from the IMU in the previous period and the previous odometry data, IMU pre-integration is performed to predict the laser pose at the moment of laser point cloud acquisition.
[0015] Furthermore, a new laser point cloud frame is divided into edge points and planar points, and the errors are calculated separately with the edge points and planar points in the static prior map. The result of IMU pre-integration is used as the initial value for matching, and the first odometry matching result is given by iterative matching using the Gauss-Newton method.
[0016] Furthermore, when a new frame of laser point cloud arrives, the continuity of the laser points on each line beam is judged. If it is judged to be discontinuous, it is considered to be an edge point; if it is judged to be continuous, it is considered to be a planar point.
[0017] Furthermore, the residual of the final result of the first odometry is retained, the covariance between the poses of adjacent frames is calculated, and the covariance between the poses of adjacent frames is accumulated until a new keyframe appears. The accumulated covariance and the keyframe are saved, the covariance is then cleared to zero, and the covariance is accumulated again. The covariance of the poses of adjacent frames is used to fill the diagonal of the covariance matrix. The uncertainty of the edges in the factor graph is described by the diagonally filled covariance matrix and added to the edges. If the variance of the second odometry is lower than a set threshold, the second odometry is considered to have converged; otherwise, a second matching is performed.
[0018] Furthermore, during the second odometer matching process, the matching relationship between adjacent frames is calculated as follows:
[0019]
[0020] z t Laser point cloud at each time point t, x t The robot pose at each time point t, where M is the prior map, and z... 0:t For a series of observations, η is a constant.
[0021] Furthermore, during the construction of the static prior map, the prior point cloud map is M, the static prior map is M, and the laser point cloud at each time step is z. t Robot state x t The process of constructing a static prior map is described as follows:
[0022]
[0023] Specifically, the following steps are included:
[0024] First, the laser point cloud z at each time point t is given. t Robot pose x t And the prior map M; by using the laser point cloud z at each time point t t As a distance query object Extracted from the prior map M and the robot pose x t Local subplots with the same position and radius
[0025] Then, the distance to the query object point cloud and local subgraph are analyzed. Within a given radius r, the height z min ≤z≤z max The region is selected as the region of interest;
[0026] The region of interest is divided into a series of bins according to angle and radius. The height difference of the points in each bin along the z-axis is calculated to obtain the pseudo-occupancy descriptor of the region.
[0027] Query objects by comparing distances with local subgraph The ratio of the descriptors of each bin to the previous bin is used to determine if the latter bin is a potential dynamic region if the latter bin is greater than the former bin.
[0028] Principal component analysis is used to extract the ground plane for each potential dynamic region. All points above the dynamic region are considered dynamic points and removed. A static prior map is then constructed, which is used as the object for the first odometry matching.
[0029] Based on the above concept, this invention provides a dual-matching robust laser positioning system for dynamic environments, including a static prior map construction module, a prior pose information acquisition module, a matching module, an optimization module, and a local point cloud map acquisition module.
[0030] The static prior map construction module is used to build static prior maps in dynamic scenes, recording all local point cloud maps and corresponding pose calculation static prior maps;
[0031] The prior pose information acquisition module is used to obtain the prior pose information of the next laser frame in the static prior map through IMU pre-integration;
[0032] The matching module performs the first odometry matching using the real-time acquired laser point cloud and the prior pose information, and determines whether the odometry has converged. If it has not converged, the odometry will use the result of the first matching as the initial value for the second matching. The second matching calculates the matching relationship between the laser point cloud and the local point cloud map. When the first odometry matching is accurate, the second odometry will immediately determine convergence. When the first odometry matching is not accurate enough, the second odometry will supplement and iteratively optimize the first odometry. The optimization module is used to perform backend optimization using GTSAM, fuse the two odometry, and update all poses.
[0033] After the final odometer result is optimized, the local point cloud map acquisition module moves the current laser point cloud to the position indicated by the odometer, constructs a local point cloud map and saves it as the matching object in the matching module.
[0034] A computer device is also provided, including a processor and a memory, wherein an executable program is stored in the memory, and when the processor executes the executable program, it can execute the dual-matching robust laser positioning method for dynamic environments described in this invention.
[0035] A computer-readable storage medium is also provided, in which a computer program is stored, which, when executed by a processor, enables the implementation of the dual-matching robust laser positioning method for dynamic environments described in this invention.
[0036] Compared with existing technologies, this invention proposes a robust dual-matching laser localization method for dynamic scenes. It simultaneously utilizes the constraint relationship between the laser point cloud and a static prior map, as well as the relationship between the laser point cloud and a local point cloud map for localization. Even if the matching from the laser point cloud to the prior map degenerates, this invention can maintain the convergence of localization by relying on the relationship between the laser point cloud and the local point cloud map, helping the system overcome the degradation stage of the laser point cloud to prior map matching method. Furthermore, this invention utilizes dynamic object filtering technology to process the prior point cloud map, further enhancing the robustness of the system. Attached Figure Description
[0037] Figure 1 This is a system flowchart of the present invention.
[0038] Figure 2 This is a block diagram of the optimized system of the present invention.
[0039] Figure 3 The result diagram shows the dual robust laser positioning method of the present invention. Detailed Implementation
[0040] The specific embodiments of the present invention will be further described in detail below with reference to the accompanying drawings.
[0041] This invention designs a dual-matching localization method to specifically improve the robustness of the algorithm in dynamic environments. Unlike traditional localization methods, this invention simultaneously uses the matching relationship between laser point clouds and point cloud maps, and the matching relationship between laser point clouds and local point cloud maps, to enhance the robustness of odometry. This invention mainly includes the following processes: static prior map construction, IMU pre-integration, odometry matching, second odometry matching, and backend optimization. Before the algorithm runs, this invention first constructs a prior static map in the dynamic scene and saves it in the computer. During real-time algorithm execution, the prior pose information of the next laser frame is first obtained through IMU pre-integration. Then, the first odometry matching is performed using the real-time obtained laser point cloud to determine if the odometry has converged. If not, the odometry will be matched again using the result of the first matching as the initial value. Finally, GTSAM (Georgia Tech Smoothing and Mapping) is used for backend optimization to update all poses.
[0042] The static prior map is constructed as follows: Define the prior point cloud map as M, the static prior map as M, and the laser point cloud at each time step as z. t Robot state x t The process of constructing a static prior map can be described as follows:
[0043]
[0044]
[0045] Specifically, we first need to provide the laser point cloud z at each time point t. t Robot pose x t And the prior map M. This is achieved by using the laser point cloud z at each time point t. t As a distance query object Compared with the robot pose x extracted from the prior map M t Local subplots with the same position and radius Then, the distance to the point cloud and local submap of the query object is set to a radius of r and a height of z. min ≤z≤z max The region is designated as the volume of interest (VOI). Next, the VOI is divided into bins based on angle and radius. The height difference along the z-axis is calculated for the points in each bin, yielding the region-wise pseudo occupancy descriptor (R-POD). This is then compared to the distance to the query object. with local subgraph The ratio of the descriptors (R-POD) of each bin is used to perform a scan ratio test (SRT). If the ratio of the latter to the former is greater than a threshold (for example, the threshold in this invention is 0.2), then the bin is considered a potential dynamic region. Finally, principal component analysis (PCA) is used to extract the ground plane for each potential dynamic region. All points above the ground plane within the dynamic region are considered dynamic points and are removed. After the static prior map is constructed, it is stored in the computer as the object of the first odometry matching.
[0046] Step 1: As Figure 1 As shown, when a new frame of laser point cloud arrives, the acceleration and angular velocity information obtained by the IMU in the previous period and the previous odometry data (if there is no odometry data, it is assumed that the pose is located at the unit element of SE(3)) are first used to perform IMU pre-integration to predict the laser pose at the moment of obtaining the laser point cloud.
[0047]
[0048]
[0049]
[0050] Robot State: The robot coordinate system is defined as the lidar coordinate system, and the robot state can be written as x = [R]. T ,p T ,v T ,b T ] T Where R = R WL ∈SO(3) represents the rotation matrix. Here, v is the position vector, v is the velocity vector, and b is the IMU bias. The transformation from the world frame G to the laser frame L is T = T... WL ∈SE(3) is represented as T=[R|p];
[0051] Step 2: As Figure 1 As shown, when a new frame of laser point cloud arrives, the continuity of the laser points on each line beam is judged based on the laser point depth. If it is judged to be discontinuous, it is considered to be an edge point; if it is judged to be continuous, it is considered to be a planar point.
[0052] Step 3: As Figure 1As shown, after a new frame of laser point cloud is acquired, it is divided into edge points and planar points. Errors are calculated between these edge points and planar points in the static prior map, and the result of IMU pre-integration is used as the initial value for matching. The first odometry matching result is obtained through iterative matching using the Gauss-Newton method. This result is usually sufficient, but in highly dynamic environments, the number of iterations may not guarantee program convergence, and the overlap between the real-time acquired laser point cloud and the prior map decreases in highly dynamic environments. Therefore, a second odometry matching is used to address this issue.
[0053] Step 4: This invention also provides a method for real-time calculation of the observation residual covariance. The residuals of the final result of the first odometer are retained, and their covariance is calculated:
[0054]
[0055]
[0056]
[0057] This calculation uses the covariance between the poses of two consecutive laser frames, i.e., the covariance between the poses of adjacent frames. However, the final optimization step requires the covariance between the poses of keyframes. The keyframe algorithm selects a frame when both the spatial distance and temporal difference reach a threshold, continuously accumulating the covariance until a new keyframe appears. Then, the accumulated covariance and the keyframe are saved, the covariance is cleared to zero, and the accumulation is repeated. To maintain isotropic optimization, the variance from the above formula is used to fill the diagonal of the covariance matrix. These covariance matrices are then used in the final backend optimization process to describe the uncertainty of edges in the factor graph and are added to the edges. If the covariance of the second odometry is below a certain threshold, we consider it converged; otherwise, a second matching is performed.
[0058] Step 5: Second odometer matching: such as Figure 2 As shown, the second odometry matching improves adaptability to highly dynamic environments by calculating the matching relationship between the laser point cloud and the local point cloud map. Below is the fusion formula between the second and first odometry measurements:
[0059]
[0060] z t Laser point cloud at each time point t, x t The robot pose at each time point t, where M is the prior map, and z... 0:t For a series of observations, η is a constant.
[0061] When the first odometer matches accurately, the second odometer will immediately determine convergence. When the first odometer's matching is not accurate enough, the second odometer will perform a supplementary iterative optimization based on the first odometer.
[0062] Backend optimization: After providing the calculation results from the first and second odometers, an optimization method is used to fuse the two odometers. The optimization function is:
[0063]
[0064] This operator represents
[0065] Step 6: After the final odometer result is optimized, the current laser point cloud will be moved to the position indicated by the odometer. A local point cloud map will be constructed and saved as the matching object in Step 5.
[0066] Step 7: After a complete localization process is completed, record all local point cloud maps and corresponding poses, combine them into a new prior map, and apply the static prior map construction method described above to the new prior map to obtain a new static prior map. This completes a map update process after localization, making the static prior map stored in the computer closer to the real environment, providing a more accurate static prior map for the next localization algorithm. (Refer to...) Figure 3 .
[0067] Based on the concept of the method, the present invention also provides a dual-matching robust laser positioning system for dynamic environments, including a static prior map construction module, a prior pose information acquisition module, a matching module, an optimization module, and a local point cloud map acquisition module;
[0068] The static prior map construction module is used to build static prior maps in dynamic scenes, recording all local point cloud maps and corresponding pose calculation static prior maps;
[0069] The prior pose information acquisition module is used to obtain the prior pose information of the next laser frame in the static prior map through IMU pre-integration;
[0070] The matching module performs the first odometry matching using the real-time acquired laser point cloud and the static prior map, and determines whether the odometry has converged. If it has not converged, the odometry will use the result of the first matching as the initial value for the second matching. The second matching calculates the matching relationship between the laser point cloud and the local point cloud map. When the first odometry matching is accurate, the second odometry will immediately determine convergence. When the first odometry matching is not accurate enough, the second odometry will supplement and iteratively optimize the first odometry. The optimization module is used to perform backend optimization using GTSAM, fuse the two odometry, and update all poses.
[0071] After the final odometer result is optimized, the local point cloud map acquisition module moves the current laser point cloud to the position indicated by the odometer, constructs a local point cloud map and saves it as the matching object in the matching module.
[0072] Optionally, the present invention may also provide a computer device, including a processor and a memory, wherein the memory is used to store a computer executable program, the processor reads part or all of the computer executable program from the memory and executes it, and the processor can implement the dual-matching robust laser positioning method for dynamic environments described in the present invention when executing part or all of the computer executable program.
[0073] The computer equipment may be a laptop, desktop computer, vehicle-mounted computer, or workstation.
[0074] The processor can be a central processing unit (CPU), a graphics processing unit (GPU), a digital signal processor (DSP), an application-specific integrated circuit (ASIC), or an off-the-shelf programmable gate array (FPGA).
[0075] A computer-readable storage medium is also provided, in which a computer program is stored. When the computer program is executed by a processor, it can implement the dual-matching robust laser positioning method for dynamic environments described in this invention.
[0076] The memory described in this invention can be an internal storage unit of a laptop, desktop computer, vehicle computer, or workstation, such as memory or hard disk; or it can be an external storage unit, such as a portable hard disk or flash memory card.
[0077] Computer-readable storage media can include computer storage media and communication media. Computer storage media includes volatile and non-volatile, removable and non-removable media implemented using any method or technology for storing information such as computer-readable instructions, data structures, program modules, or other data. Computer-readable storage media can include: read-only memory.
[0078] (ROM, Read Only Memory), Random Access Memory (RAM), Solid State Drives (SSD), or Optical Discs, etc. Among them, Random Access Memory can include Resistive Random Access Memory (ReRAM) and Dynamic Random Access Memory (DRAM).
Claims
1. A robust double-matching laser positioning method for dynamic environments, characterized in that, Includes the following steps: Constructing static prior maps in dynamic scenarios; The prior pose information of the next laser frame in the static prior map is obtained by IMU pre-integration; The first odometry matching is performed using the real-time obtained laser point cloud and the prior pose information to determine whether the odometry has converged. If it has not converged, the odometry will use the result of the first matching as the initial value for the second matching. The second matching is performed by calculating the matching relationship between the laser point cloud and the local point cloud map. When the first odometry matching is accurate, the second odometry will immediately determine convergence; when the first odometry matching is not accurate enough, the second odometry will supplement and iterate to optimize the first odometry. Finally, GTSAM was used for backend optimization, fusing the two odometry systems and updating all poses; Once the final odometer result is optimized, the current laser point cloud is moved to the position indicated by the odometer, a local point cloud map is constructed and saved, and used as the object for the second matching. After a complete localization process is completed, record all local point cloud maps and corresponding pose calculation static prior maps.
2. The dual-matching robust laser positioning method for dynamic environments according to claim 1, characterized in that, The prior pose information of the next laser frame obtained through IMU pre-integration on the static prior map includes: Using the acceleration and angular velocity information obtained from the IMU in the previous period and the previous odometry data, IMU pre-integration is performed to predict the laser pose at the moment of laser point cloud acquisition.
3. The dual-matching robust laser positioning method for dynamic environments according to claim 1, characterized in that, A new laser point cloud frame is divided into edge points and planar points, and the errors are calculated with the edge points and planar points in the static prior map respectively. The result of IMU pre-integration is used as the initial value for matching, and the first odometry matching result is given by iterative matching using the Gauss-Newton method.
4. The dual-matching robust laser positioning method for dynamic environments according to claim 3, characterized in that, When a new frame of laser point cloud arrives, the continuity of the laser points on each line beam is judged. If it is judged to be discontinuous, it is considered to be an edge point; if it is judged to be continuous, it is considered to be a planar point.
5. The dual-matching robust laser positioning method for dynamic environments according to claim 1, characterized in that, The residual of the final result of the first odometry is retained. The covariance between the poses of adjacent frames is calculated and accumulated until a new keyframe appears. The accumulated covariance and the keyframe are saved, and then the covariance is cleared to zero and accumulated again. The covariance of the poses of adjacent frames is used to fill the diagonal of the covariance matrix. The uncertainty of the edges in the factor graph is described by the diagonally filled covariance matrix and added to the edges. If the variance of the second odometry is lower than the set threshold, the second odometry is considered to have converged. Otherwise, a second matching is performed.
6. The dual-matching robust laser positioning method for dynamic environments according to claim 1, characterized in that, The second odometry matching process calculates the matching relationship between adjacent frames as follows: z t Laser point cloud at each time point t, x t The robot pose at each time point t, where M is the prior map, and z is the robot pose. 0:t For a series of observations, η is a constant.
7. The dual-matching robust laser positioning method for dynamic environments according to claim 1, characterized in that, When constructing the static prior map, the prior point cloud map is M, the static prior map is M, and the laser point cloud at each time step is z. t Robot state x t The process of constructing a static prior map is described as follows: Specifically, the following steps are included: First, the laser point cloud z at each time point t is given. t Robot pose x t And the prior map M; by using the laser point cloud z at each time point t t As a distance query object Extracted from the prior map M and the robot pose x t Local subplots with the same position and radius Then, the distance to the query object point cloud and local subgraph are analyzed. Within a given radius r, the height z min ≤z≤z max The region is selected as the region of interest; The region of interest is divided into a series of bins according to angle and radius. The height difference of the points in each bin along the z-axis is calculated to obtain the pseudo-occupancy descriptor of the region. Query objects by comparing distances with local subgraph The ratio of the descriptors of each bin to the previous bin is used to determine if the latter bin is a potential dynamic region if the latter bin is greater than the former bin. Principal component analysis is used to extract the ground plane for each potential dynamic region. All points above the dynamic region are considered dynamic points and removed. A static prior map is then constructed, which is used as the object for the first odometry matching.
8. A dual-matching robust laser positioning system for use in dynamic environments, characterized in that, It includes a static prior map construction module, a prior pose information acquisition module, a matching module, an optimization module, and a local point cloud map acquisition module; The static prior map construction module is used to build static prior maps in dynamic scenes, recording all local point cloud maps and corresponding pose calculation static prior maps; The prior pose information acquisition module is used to obtain the prior pose information of the next laser frame in the static prior map through IMU pre-integration; The matching module performs the first odometry matching using the real-time acquired laser point cloud and the prior pose information, and determines whether the odometry has converged. If it has not converged, the odometry will use the result of the first matching as the initial value for the second matching. The second matching calculates the matching relationship between the laser point cloud and the local point cloud map. When the first odometry matching is accurate, the second odometry will immediately determine convergence. When the first odometry matching is not accurate enough, the second odometry will supplement and iteratively optimize the first odometry. The optimization module is used to perform backend optimization using GTSAM, fuse the two odometry, and update all poses. After the final odometer result is optimized, the local point cloud map acquisition module moves the current laser point cloud to the position indicated by the odometer, constructs a local point cloud map and saves it as the matching object in the matching module.
9. A computer device, characterized in that, It includes a processor and a memory, the memory storing an executable program, and when the processor executes the executable program, it can perform the dual-matching robust laser positioning method for dynamic environments as described in any one of claims 1-7.
10. A computer-readable storage medium, characterized in that, A computer-readable storage medium stores a computer program that, when executed by a processor, enables the implementation of the dual-matching robust laser positioning method for dynamic environments as described in any one of claims 1-7.