CN-117075140-B - Dual-matching robust laser positioning method and system applied to dynamic environment

CN117075140BCN 117075140 BCN117075140 BCN 117075140BCN-117075140-B

Abstract

The invention discloses a double-matching robust laser positioning method and a system applied to a dynamic environment, wherein the method comprises the steps of firstly constructing a static priori map in the dynamic environment, then obtaining priori pose information of a next laser frame through IMU pre-integration, carrying out primary odometer matching by utilizing laser point cloud obtained in real time, judging whether the odometer is converged or not, and if the odometer is not converged, carrying out matching again by taking a result of the primary matching as an initial value by the odometer, thereby solving the problem of matching convergence in the dynamic environment in a targeted manner. The invention simultaneously utilizes the constraint relation between the laser point cloud and the static prior map and the constraint relation between the laser point cloud and the local point cloud map to position, even if the matching of the laser point cloud to the static prior map is degraded, the invention can also maintain the positioning convergence by means of the relation between the laser point cloud and the local point cloud map, help the system to transit the degradation stage of the matching method of the laser point cloud to the prior map, and further enhance the robustness of the system by utilizing the dynamic object filtering technology to process the prior map.

Inventors

ZHENG NANNING
ZHU ZIYU
WANG DAN
CHEN SHITAO

Assignees

西安交通大学
宁波市舜安人工智能研究院

Dates

Publication Date: 20260512
Application Date: 20230817

Claims (10)

1. The double-matching robust laser positioning method applied to the dynamic environment is characterized by comprising the following steps of: Constructing a static priori map in a dynamic scene; acquiring priori pose information of the next laser frame on the static priori map through IMU pre-integration; Performing first-time odometer matching by utilizing the laser point cloud obtained in real time and the priori pose information, judging whether the odometer is converged, and performing second-time matching by taking the result of the first-time matching as an initial value if the odometer is not converged; the second matching is carried out by calculating the matching relation between the laser point cloud and the local point cloud map, when the first milemeter is matched accurately, the second milemeter immediately judges convergence; Finally, performing back-end optimization by using GTSAM, fusing the two odometers, and updating all the poses; after the final odometer result is optimized, moving the current laser point cloud to the position indicated by the odometer, constructing a local point cloud map and storing the map as a second matching object; And after the one-time complete positioning process is finished, recording all the local point cloud maps and the corresponding pose calculation static priori maps.
2. The method for dual-matching robust laser positioning applied to a dynamic environment according to claim 1, wherein obtaining prior pose information of a next laser frame on the static prior map through IMU pre-integration comprises: And carrying out IMU pre-integration by utilizing the acceleration and angular velocity information obtained by the IMU in the previous period and the previous odometer data, and predicting the laser pose at the moment of obtaining the laser point cloud.
3. The method for positioning dual matching robust laser applied to dynamic environment according to claim 1, wherein a new frame of laser point cloud is divided into edge points and plane points, errors are calculated respectively with the edge points and the plane points in a static prior map, an IMU pre-integration result is used as a matching initial value, and a Gaussian Newton method is used for providing a first odometer matching result through iterative matching.
4. The method for positioning dual-matching robust laser applied to dynamic environment according to claim 3, wherein after a new frame of laser point cloud arrives, the laser points on each wire harness are judged continuously, if the laser points are judged to be discontinuous, the laser points are regarded as edge points, and if the laser points are judged to be continuous, the laser points are regarded as plane points.
5. The method for positioning dual-matching robust laser applied to dynamic environment according to claim 1, wherein residual errors of final results of the first milestones are reserved, covariance between adjacent frame poses is calculated, covariance between adjacent frame poses is accumulated until a new key frame appears, the accumulated covariance and the key frame are reserved, covariance is cleared again, accumulation is carried out again, diagonal angles of covariance matrixes are filled by the covariance of adjacent frame poses, uncertainty of edges in factor graphs is described by the covariance matrixes filled by the diagonal angles and added into the edges, the variance of the second milestones is lower than a set threshold, and the second milestones are considered to be converged, otherwise, second matching is carried out.
6. The method for dual matching robust laser positioning for dynamic environment according to claim 1, wherein the matching relationship between adjacent frames is calculated by the second odometer matching as follows: z t is the laser point cloud for each time point t, x t is the robot pose for each time point t, M is the prior map, z 0:t is a series of observations, η is a constant,
7. The method for positioning dual-matching robust laser applied to dynamic environment according to claim 1, wherein when a static prior map is constructed, the prior point cloud map is M, the static prior map is M, the laser point cloud at each moment is z t , the robot state is x t , and the construction process of the static prior map is described as follows: The method specifically comprises the following steps: First, a laser point cloud z t , a robot pose x t and a priori map M of each time point t are given, and the laser point cloud z t of each time point t is taken as a distance query object Local subgraph extracted from prior inspection map M and having same position and radius as robot pose x t Then the distance inquiry object point cloud and the local subgraph Setting a region with a height z min ≤z≤z max as a region of interest within a radius r; dividing the region of interest into a series of bins according to angles and radii, and calculating the height difference of the point cloud of each bin in the z-axis direction to obtain a pseudo-occupation descriptor of the region; Querying objects by contrast distance And local subgraph The ratio of descriptors of each bin is considered to be a potential dynamic region if the ratio of the latter to the former is greater than a threshold; And (3) carrying out ground plane extraction on each potential dynamic region by utilizing principal component analysis, wherein all points above the dynamic region are regarded as dynamic points to be removed, and further constructing a static prior map which is used as an object matched by the first mileometer.
8. The double-matching robust laser positioning system applied to the dynamic environment is characterized by comprising a static priori map construction module, a priori 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 for constructing a static prior map in a dynamic scene, and recording all the local point cloud maps and corresponding pose calculation static prior maps; The priori pose information acquisition module is used for acquiring priori pose information of the next laser frame on the static priori map through IMU pre-integration; The matching module is used for carrying out first-time odometer matching by utilizing the laser point cloud obtained in real time and the priori pose information, judging whether the odometer converges, if not, carrying out second-time matching by taking a result of the first-time matching as an initial value by the odometer, wherein the second-time matching is carried out by calculating the matching relation between the laser point cloud and the local point cloud map, when the first-time odometer is accurately matched, the second-time odometer immediately judges convergence, when the first-time odometer is insufficiently matched, the second-time odometer carries out supplementary iterative optimization on the first-time odometer, and the optimizing module is used for carrying out back-end optimization by using GTSAM, fusing the two odometers and updating all poses; And 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 stores the local point cloud map as an object matched in the matching module.
9. A computer device comprising a processor and a memory, the memory storing an executable program, the processor being capable of executing the dual matching robust laser positioning method of any of claims 1-7 when executed by the processor.
10. A computer readable storage medium, wherein a computer program is stored in the computer readable storage medium, the computer program, when executed by a processor, is capable of implementing the dual matching robust laser positioning method applied to a dynamic environment as claimed in any one of claims 1-7.

Description

Dual-matching robust laser positioning method and system applied to dynamic environment Technical Field The invention belongs to the technical field of autonomous robot positioning, and particularly relates to a double-matching robust laser positioning method and system applied to a dynamic environment. Background Positioning tasks are a fundamental technique in robotics, especially for autonomous mobile robots, such as unmanned aerial vehicles, unmanned vehicles, AGVs, etc. At present, a common positioning paradigm is that a robot firstly acquires a map of a self motion range, namely a priori map, and then positions according to a matching relation between a sensor of a sensing environment carried by the robot and the map. SLAM (Simultaneous Localization AND MAPPING) technology is a technology that builds up various forms of maps depending on the perception of the environment by robots and performs positioning on the maps without relying on artificial beacons, and has been largely applied to unmanned vehicles in recent years, and is classified into LiDAR-SLAM and Visual SLAM, etc., depending on the sensors used. Among the many alternative sensor combinations, such as monocular cameras, RGB-D cameras, liDAR, wheel speed meters, and Inertial Measurement Units (IMUs), the combined laser inertial positioning technology of LiDAR and IMU stands out in unmanned vehicle applications due to its high accuracy and robustness of measurement. Although LiDAR measurements are sufficiently accurate, the estimation errors still increase over time as Laser Odometer (LO) and Laser Inertial Odometer (LIO) systems make state estimates of the robot. And both LO and LIO are based on the static assumption of the environment, i.e. no moving objects in the environment throughout the operation of the algorithm, which is not reasonable for real autonomous vehicles and the like robots. When an autonomous vehicle moves in a real dynamic environment, the inconsistency of the laser point cloud and the prior point cloud map obtained in real time not only leads to the increase of the running time of a matching algorithm, but also leads to the occurrence of non-matching points in the laser point cloud or the prior point cloud map, and further reduces the robustness and accuracy of the odometer. Global positioning information is also quickly acquired, typically on unmanned vehicles and on ordinary vehicles, using global satellite navigation systems (GNSS) systems to correct for laser point cloud-to-positioning algorithm matching errors. GNSS systems are a satellite-dependent global positioning system that relies on artificial mobile beacons, i.e., satellites. The accuracy of the current differential signal-based GNSS is very high (can reach centimeter level), and on both the common dataset kitti of SLAM and TUM, RTK-GNSS is utilized as the true value of the positioning system. While the RTK-GNSS system has high positioning accuracy and low repositioning overhead, he can have reduced positioning accuracy in urban environments due to multipath effects of the signals and completely lost positioning signals in tunnel and indoor scenes, which is unacceptable on unmanned vehicles that may run in complex environments. Disclosure of Invention In order to solve the problems in the prior art, the invention provides a more robust positioning method which is obtained by simultaneously utilizing the matching relationship between the laser point cloud and the prior point cloud map and the relative pose relationship between the laser point cloud and the local point cloud map formed during the operation of the algorithm. In order to achieve the purpose, the technical scheme adopted by the invention is that the double-matching robust laser positioning method applied to the dynamic environment comprises the following steps: Constructing a static priori map in a dynamic scene; acquiring priori pose information of the next laser frame on the static priori map through IMU pre-integration; Performing first-time odometer matching by utilizing the laser point cloud obtained in real time and the priori pose information, judging whether the odometer is converged, and performing second-time matching by taking the result of the first-time matching as an initial value if the odometer is not converged; the second matching is carried out by calculating the matching relation between the laser point cloud and the local point cloud map, when the first milemeter is matched accurately, the second milemeter immediately judges convergence; Finally, performing back-end optimization by using GTSAM, fusing the two odometers, and updating all the poses; after the final odometer result is optimized, moving the current laser point cloud to the position indicated by the odometer, constructing a local point cloud map and storing the map as a second matching object; And after the one-time complete positioning process is finished, recording all the local point cloud maps and the correspon