Laser-radar-based mobile robot efficient robust global positioning method
By using a lidar-based approach, feature extraction is performed using Radon transform and a feature extraction network. Combined with Fourier transform and cross-correlation operations, the problem of environmental change and rotation invariance in global localization is solved, achieving efficient and robust localization results.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- ZHEJIANG UNIV
- Filing Date
- 2022-11-21
- Publication Date
- 2026-04-24
AI Technical Summary
Existing global localization methods are easily affected by lighting and seasonal changes in mobile robots, and rotational invariance is easily affected by translational changes, leading to incorrect localization results.
A lidar-based approach is adopted, which projects lidar point cloud data into a bird's-eye view. Feature extraction is performed using a feature extraction network that is invariant to rotation and translation. Combined with one-dimensional Fourier transform and cross-correlation operation, rotation- and translation-invariant location identification and pose estimation are achieved.
It achieves robustness to environmental changes, improves the accuracy and efficiency of positioning, and enhances rotation and translation invariance by using a data-driven feature extraction method, enabling rapid convergence to the correct pose.
Smart Images
Figure CN115932868B_ABST
Abstract
Description
Technical Field
[0001] This invention relates to the field of mobile robot localization, and in particular to a global localization method for mobile robots based on lidar information. Background Technology
[0002] Global localization (GLoC) technology plays a crucial role in autonomous localization and map building for mobile robots. During autonomous localization, robots face challenges such as unknown starting positions and potential failures due to various external factors. GLoC is key to achieving relocalization within a global map. In map building, GLoC identifies common locations along the robot's existing trajectory, adding constraints to the construction of a globally consistent map. Essentially, GLoC is a problem of determining the robot's pose in the map coordinate system based on its current sensor observations. This problem is highly challenging because it aims to search for solutions across the entire candidate pose space—every pose the robot can assume on the map—using only current observations and the map database. Furthermore, resource constraints necessitate lightweight computation and storage, placing higher demands on the effectiveness and efficiency of GLoC.
[0003] To address the global localization problem, most global localization methods employ strategies derived from vision-based localization, dividing the problem into two stages: first, location identification, and then pose estimation. Vision-based methods are susceptible to environmental changes such as lighting and seasonal variations. In contrast, LiDAR-based global localization methods are robust to environmental changes and less affected by lighting and seasonal variations. However, these methods tend to fail when the robot revisits locations with significant changes in perspective. Furthermore, the rotational invariance of current global localization methods is easily affected by translational changes; under large relative translations, rotational invariance is lost, leading to incorrect localization results. Summary of the Invention
[0004] The purpose of this invention is to propose an efficient and robust global localization method for mobile robots based on lidar, which enables robust global localization to changes in environment and viewpoint.
[0005] The specific technical solution adopted in this invention is as follows:
[0006] A highly efficient and robust global localization method for mobile robots based on lidar, characterized by comprising:
[0007] S1. The mobile robot collects laser point cloud data in real time during its movement using a lidar, and projects the laser point cloud data to obtain a bird's-eye view f(x,y) based on the mobile robot's odometer information and the set distance traveled each time.
[0008] S2. Using the Radon transform, integrate the bird's-eye view obtained in S1 along the line L: xcosθ + ysinθ = τ to obtain a sine graph with θ as the vertical axis and τ as the horizontal axis. During the Radon transformation, the bird's-eye view f(x,y) undergoes a rotation α and a translation α. The changes were converted into sine graphs. Translation of the vertical and horizontal axes;
[0009] S3. The sine curve obtained in S2 using a translation-equivariant feature extraction network. Feature extraction is performed on the above to obtain the sine curve. Feature maps of the same size E f ;
[0010] S4, for feature map E f Perform a one-dimensional Fourier transform on each row, and the resulting spectrum is the amplitude spectrum M. f As a translation-invariant characteristic;
[0011] S5. During the movement of the mobile robot, the amplitude spectrum corresponding to the laser point cloud data at the current position is: Iterate through all candidate locations stored in the map database and compare the amplitude spectrum of the laser point cloud data corresponding to the current location. The amplitude spectrum corresponding to the laser point cloud data of each candidate location. Perform cross-correlation operations separately This process measures the similarity between the current location and the laser point cloud data of each candidate location, and then retrieves the candidate location index n with the highest similarity to the laser point cloud data of the current location from the map database, thus achieving location identification; simultaneously, it estimates the relative rotation between the laser point cloud data of the current location and the laser point cloud data corresponding to the retrieved index n.
[0012] in n and The calculation formulas are as follows:
[0013]
[0014]
[0015]
[0016] In the formula: This represents the amplitude spectrum corresponding to the laser point cloud data of the i-th candidate location stored in the map database; express The corresponding vertical axis is θ and the discrete frequency in the one-dimensional Fourier transform is ω;
[0017] S6. The relative rotation estimated in S5 The rotation-compensated bird's-eye view f(x,y) is then applied to the current location's bird's-eye view f(x,y), resulting in a rotated-compensated bird's-eye view f′(x,y). This rotated-compensated bird's-eye view f′(x,y) and the bird's-eye view f corresponding to the candidate location index n are then compared. n Only translation transformations exist between (x, y); then cross-correlation operations are used. Solve for the bird's-eye view f′(x,y) and the bird's-eye view f n Relative translation between (x, y)
[0018]
[0019] S7. Solve the relative rotation in S5. The relative translation obtained from the solution in S6 In the point cloud registration algorithm, the initial input is used to solve the 6-DOF relative pose, thereby achieving pose estimation.
[0020] Preferably, the specific steps in S1 for projecting the laser point cloud data into a bird's-eye view are as follows:
[0021] S11. First, filter out ground points that have no information in the laser point cloud data;
[0022] S12. Divide the laser point cloud after removing ground points into independent grids along the z-axis in 3D Cartesian space for a bird's-eye view.
[0023] S13. Based on the occupancy, maximum height, or reflection intensity information of the grid in the bird's-eye view, encode the laser point cloud into a bird's-eye view image, where the occupancy, maximum height, or reflection intensity information of the grid corresponds to the pixel values in the bird's-eye view image.
[0024] Preferably, in S2, the formula for the Radon transformation is:
[0025]
[0026] In the formula: δ(·) is the Dirac function.
[0027] Preferably, in S3, the translation-equivariant feature extraction network used is Auto-Encoder or U-Net.
[0028] Preferably, the point cloud registration algorithm used in S7 is the ICP algorithm.
[0029] Preferably, the set distance should be less than the range of the lidar.
[0030] As a preferred method, the bird's-eye view and amplitude spectrum collected during the movement of the mobile robot are stored in a map database for use in the next location identification and pose estimation.
[0031] Compared with the prior art, the advantages of this invention are as follows:
[0032] 1. This invention does not rely on images acquired by a camera, thus achieving robustness to environmental changes and being less susceptible to the effects of lighting and seasonal variations.
[0033] 2. This invention is not limited to manually designed features using traditional methods, but uses a data-driven approach to extract more unique features for location identification tasks.
[0034] 3. This invention uses a sine wave to represent laser point clouds and supervises the extraction of rotation- and translation-invariant features through correlation learning. Since its rotation invariance is not affected by translation changes, it achieves rotation- and translation-invariant location identification and improves the accuracy of global positioning.
[0035] 4. In addition to location identification, this invention also estimates relative rotation and translation, providing good initial values for point cloud registration algorithms such as ICP, enabling them to converge globally quickly and obtain more accurate positioning results. Attached Figure Description
[0036] Figure 1 This is a flowchart of an efficient and robust global localization method for mobile robots based on lidar, according to an embodiment of the present invention.
[0037] Figure 2 This is a visualization of the intermediate process of end-to-end learning in an embodiment of the present invention. Detailed Implementation
[0038] To make the above-mentioned objects, features, and advantages of the present invention more apparent and understandable, the specific embodiments of the present invention will be described in detail below with reference to the accompanying drawings. Many specific details are set forth in the following description to provide a thorough understanding of the present invention. However, the present invention can be practiced in many other ways different from those described herein, and those skilled in the art can make similar modifications without departing from the spirit of the present invention. Therefore, the present invention is not limited to the specific embodiments disclosed below. Technical features in various embodiments of the present invention can be combined accordingly without mutual conflict.
[0039] This invention provides an efficient and robust global localization method based on lidar. It utilizes the Radon transform to convert rotation and translation changes into translational changes along the two axes of a sine wave. A translationally equivariant feature extraction network is used for feature extraction to ensure the equivariance of rotation and translation. Fourier transform is used to obtain the amplitude spectrum of the spectrum to achieve translation invariance. Cross-correlation operations are used to calculate rotation- and translation-invariant similarity. Simultaneously, a supervised network extracts features suitable for location identification tasks, improving the representation ability of rotation- and translation-invariant features in lidar point clouds. Furthermore, this invention uses cross-correlation operations to estimate relative rotation and translation, providing good initial values for the point cloud registration algorithm, and further solving for accurate 6-DOF relative pose.
[0040] like Figure 1 As shown, in a preferred embodiment of the present invention, the efficient and robust global localization method for mobile robots based on lidar specifically includes the following steps:
[0041] Step (1): The mobile robot collects laser point cloud data in real time during its movement using a lidar, and projects the laser point cloud data into a bird's-eye view f(x,y) based on the mobile robot's odometer information for each set distance traveled (which must be less than the range of the lidar, such as 20 meters).
[0042] As a specific implementation of this invention, the specific steps for projecting laser point cloud data into a bird's-eye view based on occupancy / maximum height / reflection intensity information are as follows:
[0043] Step (1-1): First, filter out ground points in the laser point cloud data that have no information.
[0044] Step (1-2): Divide the laser point cloud after removing ground points into a finite number of independent grids along the z-axis in 3D Cartesian space, under a bird's-eye view.
[0045] Steps (1-3): Based on the occupancy, maximum height, or reflection intensity information of the grid in the bird's-eye view, the laser point cloud is encoded into a bird's-eye view image, where the occupancy, maximum height, or reflection intensity information of the grid corresponds to the pixel values in the bird's-eye view image.
[0046] Step (2): Using the Radon transform, integrate the bird's-eye view obtained in step (1) along the line L: xcosθ + ysinθ = τ to obtain a sine graph with θ as the vertical axis and τ as the horizontal axis. During the Radon transformation, the bird's-eye view f(x,y) undergoes a rotation α and a translation α. The changes were converted into sine graphs. Translation of the vertical and horizontal axes.
[0047] As a specific implementation of this invention, the Radon transformation described above is shown in formula (1):
[0048]
[0049] In the formula: δ(·) is the Dirac function.
[0050] Rotation α and translation of the bird's-eye view f(x,y) Convert to sine graph The translational changes of the θ and τ axes can be expressed by formulas (2) and (3) respectively:
[0051]
[0052]
[0053] Therefore, the final sine curve obtained after the calculation in step (2) is... In the diagram, the vertical axis (θ axis) reflects the rotation of the laser point cloud, while the horizontal axis (τ axis) represents the translation of the point cloud at different rotation angles. That is, the rotational changes of the mobile robot at the same location are represented by cyclic translations of the vertical axis in the sine graph, while translational changes near the same location are represented by translations of the horizontal axis.
[0054] Step (3): Use the translational isomorphic feature extraction network to obtain the sine curve in S2. Feature extraction is performed on the above, ensuring that the extracted features are equally invariant in rotation and translation, to obtain a sine wave. Feature maps of the same size E f .
[0055] In this step, the translation-equivariant feature extraction network ensures the rotation and translation equivariance of the extracted features, which is crucial for achieving subsequent rotation and translation invariance. As a specific implementation of this invention, the translation-equivariant feature extraction network used above can be an Auto-Encoder or a U-Net network, etc.
[0056] Step (4): For feature map E f Perform a one-dimensional Fourier transform on each row, and the resulting spectrum is the amplitude spectrum M. f As a translation-invariant characteristic.
[0057] In this step, due to the characteristics of the Fast Fourier Transform, the feature map is transformed into a frequency domain representation. The amplitude spectrum of the obtained spectrum is not affected by translation changes. Therefore, the obtained amplitude spectrum has translation invariance, that is, the representation is consistent in the vicinity of the same location. The amplitude spectrum can be used as a translation-invariant representation.
[0058] In one example of the present invention, the visualization results of the above-mentioned map laser point cloud, bird's-eye view, sine wave, feature map, and amplitude spectrum are as follows: Figure 2 As shown.
[0059] Step (5): During the movement of the mobile robot, the amplitude spectrum corresponding to the laser point cloud data at the current position is... Iterate through all candidate locations stored in the map database and compare the amplitude spectrum of the laser point cloud data corresponding to the current location. The amplitude spectrum corresponding to the laser point cloud data of each candidate location. Perform cross-correlation operations separately This allows for the measurement of the similarity between the current location and the laser point cloud data of each candidate location, resulting in a correlation vector. Then, the candidate location index n with the highest similarity to the laser point cloud data of the current location is retrieved from the map database to achieve location identification; at the same time, the relative rotation between the laser point cloud data of the current location and the laser point cloud data corresponding to the retrieved index n is estimated.
[0060] During cross-correlation operations The correlation spectrum is obtained by cross-correlating the current amplitude spectrum of the robot with the amplitude spectrum of the candidate location along the vertical axis. The maximum value of the correlation spectrum is taken as the similarity, and the formula is as follows:
[0061]
[0062] When determining the candidate location index n, it is necessary to compare the similarity between the robot's current laser point cloud observation and all laser point cloud observations in the map database, and take the location with the highest similarity as the location closest to the current robot. The specific formula is as follows:
[0063]
[0064] In calculating relative rotation In this case, the relative rotation between the current robot and the retrieved map data can be determined based on the position corresponding to the maximum value of the correlation spectrum in the cross-correlation operation. The specific calculation formula is as follows:
[0065]
[0066] In the formula: This represents the amplitude spectrum corresponding to the laser point cloud data of the i-th candidate location stored in the map database; express The corresponding vertical axis is θ and the discrete frequency in the one-dimensional Fourier transform is ω.
[0067] Step (6): As shown in formula (7), perform the relative rotation estimated in step (5). The image is compensated onto the current location's bird's-eye view f(x,y) to obtain the rotated-compensated bird's-eye view f′(x,y). At this point, the rotated-compensated bird's-eye view f′(x,y) and the bird's-eye view f corresponding to the candidate location index n are compared.n Only translation transformations exist between (x, y). Therefore, we can further utilize cross-correlation operations. Solve for the bird's-eye view f′(x,y) and the bird's-eye view f n Relative translation between (x, y) relative translation The calculation requires applying cross-correlation operations to obtain the correlation spectrum between the compensated bird's-eye view and the retrieved bird's-eye view, and determining the relative translation between the current robot and the retrieved map data based on the position corresponding to the maximum value of the correlation spectrum. The principle formula of cross-correlation operation is shown in (8). In actual calculation, the relative translation can be calculated using formula (9).
[0068]
[0069]
[0070]
[0071] Step (7): Solve the relative rotation in step (5) The relative translation obtained in step (6) As an initial input point cloud registration algorithm, it achieves a more accurate 6-DOF relative pose solution, thereby realizing pose estimation.
[0072] As a specific implementation of this invention, the point cloud registration algorithm used above can be the ICP algorithm.
[0073] Step (8): The bird's-eye view and amplitude spectrum collected during the movement of the mobile robot are stored in the map database for the next location identification and pose estimation.
[0074] In summary, this invention uses a sine wave to represent laser point clouds and employs correlation learning to supervise the extraction of rotation- and translation-invariant features. Since the rotation invariance is unaffected by translational changes, it achieves rotation- and translation-invariant location identification, improving the accuracy of global localization. Furthermore, in addition to location identification, this invention also estimates relative rotation and translation, providing good initial values for point cloud registration algorithms such as ICP, enabling them to converge globally quickly and obtain more accurate localization results.
[0075] The embodiments described above are merely preferred embodiments of the present invention and are not intended to limit the invention. Those skilled in the art can make various changes and modifications without departing from the spirit and scope of the invention. Therefore, all technical solutions obtained through equivalent substitution or transformation fall within the protection scope of the present invention.
Claims
1. A highly efficient and robust global localization method for mobile robots based on lidar, characterized in that, include: S1. The mobile robot collects laser point cloud data in real time during its movement using LiDAR, and projects the laser point cloud data to obtain a bird's-eye view based on the mobile robot's odometer information at each set distance traveled. ; S2. Use the Radon transform to transform the bird's-eye view obtained in S1 along a straight line. Integrating, we obtain the vertical axis as... The horizontal axis is sine graph ; Bird's-eye view during the Ladon transformation rotation Peaceful relocation The changes were converted into sine graphs. Translation of the vertical and horizontal axes; S3. The sine curve obtained in S2 using a translation-equivariant feature extraction network. Feature extraction is performed on the above to obtain the sine curve. Feature maps of the same size ; S4. Feature Map Perform a one-dimensional Fourier transform on each row, and the resulting spectrum is the amplitude spectrum. As a translation-invariant characteristic; S5. During the movement of the mobile robot, the amplitude spectrum corresponding to the laser point cloud data at the current position is: Iterate through all candidate locations stored in the map database and compare the amplitude spectrum of the laser point cloud data corresponding to the current location. The amplitude spectrum corresponding to the laser point cloud data of each candidate location. Perform cross-correlation operations separately This process measures the similarity between the current location and the laser point cloud data of each candidate location, and then retrieves the candidate location index with the highest similarity to the laser point cloud data of the current location from the map database. This enables location identification; simultaneously, it estimates the laser point cloud data of the current location and the retrieved index. Relative rotation between corresponding laser point cloud data ; in , and The calculation formulas are as follows: In the formula: This represents the amplitude spectrum corresponding to the laser point cloud data of the i-th candidate location stored in the map database; express The corresponding vertical axis is And the discrete frequency during one-dimensional Fourier transform is ; Furthermore, the extraction of rotation- and translation-invariant features is supervised through correlation learning; S6. The relative rotation estimated in S5 A bird's-eye view compensated to the current location Above, the bird's-eye view after rotation compensation is obtained. The bird's-eye view after rotation compensation and candidate location index Corresponding bird's-eye view Only translation transformations exist between them; then cross-correlation operations are used. Solving the bird's-eye view and bird's-eye view Relative translation between : S7. Solve the relative rotation in S5. The relative translation obtained from the solution in S6 In the point cloud registration algorithm, the initial input is used to solve the 6-DOF relative pose, thereby achieving pose estimation.
2. The efficient and robust global localization method for mobile robots based on lidar as described in claim 1, characterized in that, In step S1, the specific steps for projecting the laser point cloud data into a bird's-eye view are as follows: S11. First, filter out ground points that have no information in the laser point cloud data; S12. Divide the laser point cloud after removing ground points into independent grids along the z-axis in 3D Cartesian space for a bird's-eye view. S13. Based on the occupancy, maximum height, or reflection intensity information of the grid in the bird's-eye view, encode the laser point cloud into a bird's-eye view image, where the occupancy, maximum height, or reflection intensity information of the grid corresponds to the pixel values in the bird's-eye view image.
3. The efficient and robust global localization method for mobile robots based on lidar as described in claim 1, characterized in that, In S2, the formula for the Radon transformation is: In the formula: This is the Dirac function.
4. The efficient and robust global localization method for mobile robots based on lidar as described in claim 1, characterized in that, In S3, the translation-equivariant feature extraction network used is either Auto-Encoder or U-Net.
5. The efficient and robust global localization method for mobile robots based on lidar as described in claim 1, characterized in that, In S7, the point cloud registration algorithm used is the ICP algorithm.
6. The efficient and robust global localization method for mobile robots based on lidar as described in claim 1, characterized in that, The set distance must be less than the range of the lidar.
7. The efficient and robust global localization method for mobile robots based on lidar as described in claim 1, characterized in that, The bird's-eye view and amplitude spectrum collected during the movement of the mobile robot are stored in the map database for the next location identification and pose estimation.