CN-121994206-A - Long-range positioning method, device and medium based on dynamic voxel point cloud map and ESDF

CN121994206ACN 121994206 ACN121994206 ACN 121994206ACN-121994206-A

Abstract

The application provides a long-range positioning method, a device and a medium based on a dynamic voxel point cloud map and ESDF, and belongs to the technical field of mobile robot positioning and map construction. The method comprises the steps of obtaining a current sensor frame of the mobile robot based on a priori global voxel map, matching the current sensor frame with the priori global voxel map, judging the environmental change degree based on a matching result, adaptively updating the priori global voxel map according to the environmental change degree to obtain an updated global voxel map, incrementally updating an Euclidean symbol distance field based on a change area in the updated global voxel map, and optimizing and solving the current pose of the mobile robot by minimizing registration errors of the current sensor frame and the Euclidean symbol distance field.

Inventors

DING JIANXUN
ZHAO XIANGRONG
TANG WEI
WU FAN
YANG JUN

Assignees

中科云谷科技有限公司

Dates

Publication Date: 20260508
Application Date: 20251231

Claims (12)

1. The long-range positioning method based on the dynamic voxel point cloud map and ESDF is characterized by comprising the following steps of: Acquiring a current sensor frame of the mobile robot based on the prior global voxel map; matching the current sensor frame with the prior global voxel map, and judging the environmental change degree based on a matching result; According to the environmental change degree, the prior global voxel map is adaptively updated, and an updated global voxel map is obtained; Incrementally updating the Euclidean symbol distance field based on the changed region in the updated global voxel map; And optimizing and solving the current pose of the mobile robot by minimizing the registration error of the current sensor frame and the Euclidean symbol distance field.
2. The long-range positioning method based on the dynamic voxel point cloud map and ESDF as set forth in claim 1, wherein the matching the current sensor frame with the prior global voxel map and determining the environmental change degree based on the matching result includes: performing coordinate transformation on the current sensor frame to unify the coordinate systems of the current sensor frame and the prior global voxel map; Solving an intersection of the current sensor frame after coordinate conversion and the prior global voxel map to obtain a first point cloud which is not matched with the prior global voxel map in the current sensor frame and a second point cloud which is matched with the prior global voxel map; and determining the duty ratio of the total point cloud number of the first point cloud in the current sensor frame.
3. The long-range positioning method based on the dynamic voxel point cloud map and ESDF as set forth in claim 2, wherein the adaptively updating the prior global voxel map according to the environmental change degree, to obtain an updated global voxel map includes: Removing occupied voxels in a sensor scanning path in the current sensor frame and merging the first point cloud into a dynamic voxel map under the condition that a sensor observation range corresponding to the current sensor frame is not located in the prior global voxel map; And merging the updated dynamic voxel map into the prior global voxel map, and distributing a first weight for the first point cloud.
4. The long-range positioning method based on the dynamic voxel point cloud map and ESDF as set forth in claim 2, wherein the adaptively updating the prior global voxel map according to the environmental change degree, to obtain an updated global voxel map further includes: Removing occupied voxels in a sensor scanning path in the current sensor frame and merging the first point cloud into a dynamic voxel map under the condition that a sensor observation range corresponding to the current sensor frame is positioned in the prior global voxel map and the occupancy rate is smaller than a map consistency threshold; And merging the updated dynamic voxel map into the prior global voxel map, and distributing a second weight for the second point cloud.
5. The dynamic voxel point cloud map and ESDF-based long-range localization method of any one of claims 3 or 4, wherein the global voxel map is managed using a hash table, the incorporating the updated dynamic voxel map into the prior global voxel map comprising: for each point in the dynamic voxel map, determining a corresponding three-dimensional index based on the position coordinates of the point and map resolution; Mapping the three-dimensional index into a one-dimensional hash key value by utilizing a hash function; Judging whether the one-dimensional hash key value exists in the hash table or not; and updating the state of the corresponding voxel in the hash table based on the point type as the corresponding weight under the condition that the one-dimensional hash key value exists in the hash table.
6. The dynamic voxel point cloud map and ESDF-based long-range localization method of claim 5, wherein the incorporating the updated dynamic voxel map into the prior global voxel map further comprises: creating a new voxel entry in the hash table and giving an initial preset initial occupation probability and weight under the condition that the one-dimensional hash key value does not exist in the hash table; updating the new voxel entry based on the type of the point being the corresponding weight.
7. The long-range positioning method based on the dynamic voxel point cloud map and ESDF as set forth in claim 2, wherein the adaptively updating the prior global voxel map according to the environmental change degree, to obtain an updated global voxel map includes: And under the condition that the sensor observation range corresponding to the current sensor frame is positioned in the prior global voxel map and the duty ratio is larger than a map consistency threshold value, not updating the prior global voxel map.
8. The long range localization method based on dynamic voxel point cloud map and ESDF of any one of claim 4 or claim 7, wherein the map consistency threshold is determined based on the following formula: Wherein, the For the scanning distance of the sensor, For the effective distance of the sensor, As a ratio of the effective point cloud range to the radar scan range, For the resolution of the map voxels, Is the map consistency threshold.
9. The long-range localization method based on dynamic voxel point cloud map and ESDF of claim 1, wherein the euclidean symbol distance field is incrementally updated based on the following formula: Wherein, the Representing the voxels to be updated, For the euclidean distance of voxel p to the nearest obstacle, For a neighborhood voxel set of voxel p, The updated voxel set for the current sensor frame, And the distance field is the Euclidean sign.
10. The long-range positioning method based on the dynamic voxel point cloud map and ESDF of claim 1, wherein optimizing the solving for the current pose of the mobile robot by minimizing the registration error of the current sensor frame and the euclidean symbol distance field comprises: minimizing registration errors of the current sensor frame and the Euclidean symbol distance field according to the following formula: Wherein, the For the coordinates of the i-th point in the current sensor frame under the global voxel map coordinate system, Is that Distance to nearest voxels in the updated global voxel map, For the current pose of the mobile robot to be solved, As a second point cloud of points of the image, As a result of the second weight being set, As a first point cloud of points, Is a first weight; Iteration Until the registration error reaches a minimum.
11. A long-range positioning device based on a dynamic voxel point cloud map and ESDF, comprising: a memory configured to store instructions; a processor configured to invoke the instructions from the memory and when executing the instructions enable the dynamic voxel point cloud map and ESDF-based long-range localization method of any one of claims 1-10.
12. A machine-readable storage medium having instructions stored thereon, which when executed by a processor, cause the processor to be configured to perform the dynamic voxel point cloud map and ESDF-based long-range localization method of any one of claims 1-10.

Description

Long-range positioning method, device and medium based on dynamic voxel point cloud map and ESDF Technical Field The application relates to the technical field of mobile robot positioning and map construction, in particular to a long-range positioning method, device and medium based on a dynamic voxel point cloud map and ESDF. Background In the technical field of mobile robot positioning and map construction, mobile robot accurate positioning is a basis for completing navigation. However, the existing mobile robot positioning technology can significantly decrease positioning accuracy when facing dynamically changing objects or long-term environmental changes. For example ICP, NDT, GICP, the pose transformation is usually solved by directly matching the current frame point cloud with the global map, but when the environment changes or the map has a stale area, the matching effect is significantly reduced. However, ESDF can provide dense distance information, but the calculation amount is large, the update usually depends on the complete global reconstruction, and the real-time requirement in a large-scale dynamic scene is difficult to adapt. Disclosure of Invention The embodiment of the application aims to provide a long-range positioning method, a device and a medium based on a dynamic voxel point cloud map and ESDF. In order to achieve the above object, a first aspect of the present application provides a long-range positioning method based on a dynamic voxel point cloud map and ESDF, including: Acquiring a current sensor frame of the mobile robot based on the prior global voxel map; matching the current sensor frame with the prior global voxel map, and judging the environmental change degree based on a matching result; According to the environmental change degree, the prior global voxel map is adaptively updated, and an updated global voxel map is obtained; Based on the updated change area in the global voxel map, incrementally updating the Euclidean symbol distance field; and (3) optimizing and solving the current pose of the mobile robot by minimizing the registration error of the current sensor frame and the Euclidean symbol distance field. In the embodiment of the application, the current sensor frame is matched with the prior global voxel map, and the environmental change degree is judged based on a matching result, wherein the method comprises the steps of carrying out coordinate transformation on the current sensor frame to unify the coordinate system of the current sensor frame and the prior global voxel map, solving an intersection of the current sensor frame after coordinate transformation and the prior global voxel map to obtain a first point cloud which is not matched with the prior global voxel map in the current sensor frame and a second point cloud which is matched with the prior global voxel map, and determining the occupation ratio of the total point cloud number of the first point cloud in the current sensor frame. In the embodiment of the application, the prior global voxel map is adaptively updated according to the environmental change degree, and the obtaining of the updated global voxel map comprises the steps of removing occupied voxels in a sensor scanning path in a current sensor frame and integrating a first point cloud into a dynamic voxel map under the condition that a sensor observation range corresponding to the current sensor frame is not located in the prior global voxel map, integrating the updated dynamic voxel map into the prior global voxel map, and distributing a first weight for the first point cloud. In the embodiment of the application, the prior global voxel map is adaptively updated according to the environmental change degree, and the method for obtaining the updated global voxel map further comprises the steps of removing occupied voxels in a sensor scanning path in a current sensor frame and integrating a first point cloud into a dynamic voxel map under the condition that a sensor observation range corresponding to the current sensor frame is positioned in the prior global voxel map and the occupancy rate is smaller than a map consistency threshold value, integrating the updated dynamic voxel map into the prior global voxel map, and distributing a second weight for a second point cloud. The method comprises the steps of managing a global voxel map by utilizing a hash table, and integrating an updated dynamic voxel map into the prior global voxel map, wherein the method comprises the steps of determining a corresponding three-dimensional index based on position coordinates and map resolution of points for each point in the dynamic voxel map, mapping the three-dimensional index into one-dimensional hash key values by utilizing a hash function, judging whether the one-dimensional hash key values exist in the hash table, and updating states of corresponding voxels in the hash table based on the corresponding weights of the types of the points when the one-dimension