A robot repositioning method based on combination of GPS and laser radar
By combining GPS and LiDAR, and utilizing differential GPS data and LiDAR loop closure detection, the problem of inaccurate positioning during robot initialization and loss of positioning was solved, achieving efficient repositioning of outdoor robots and improving robustness and accuracy.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- SHANDONG NEW GENERATION INFORMATION IND TECH RES INST CO LTD
- Filing Date
- 2022-12-12
- Publication Date
- 2026-06-05
AI Technical Summary
Existing robot localization methods suffer from inaccurate localization and errors during initialization and when localization is lost, especially in outdoor environments where efficient and accurate relocalization is difficult to achieve when point cloud registration fails.
By combining GPS and LiDAR, the robot's repositioning pose can be determined through differential GPS data filtering, LiDAR loopback data filtering, UTM projection, and coordinate system alignment. This leverages the complementary advantages of GPS's accuracy in open environments and LiDAR's robustness in complex environments.
It improves the robustness and accuracy of robot relocalization, solves the problem of inaccurate localization of outdoor robots in the case of initialization and localization loss, and has higher robustness and accuracy.
Smart Images
Figure CN116243356B_ABST
Abstract
Description
Technical Field
[0001] This invention relates to the field of robot localization, and more specifically to a robot relocalization method based on a combination of GPS and lidar. Background Technology
[0002] The localization module provides the robot with its position information in the environment and is a crucial intermediary link between maps and navigation. Currently, most common robot localization strategies are based on point cloud registration methods, using the current point cloud frame to register with the map point cloud to obtain relatively accurate position information.
[0003] However, point cloud registration-based localization methods typically have two main problems that need to be addressed. The first is the localization initialization problem, which is to determine the robot's position on the map at the moment it is powered on and without human intervention. The second is the correction problem in case of localization loss, which is that under certain influences, point cloud registration may fail, causing errors such as jumps in localization data, which need to be corrected. Summary of the Invention
[0004] To overcome the shortcomings of the above technologies, this invention provides a method for solving the problems of positioning initialization and positioning loss correction, meeting the requirements of efficient repositioning of outdoor robots, and having high robustness and accuracy.
[0005] The technical solution adopted by this invention to overcome its technical problems is:
[0006] A robot relocalization method based on a combination of GPS and lidar includes the following steps:
[0007] a) Input GPS data and filter the data to obtain valid GPS data;
[0008] b) Filter the loopback input and loopback data of the robot's LiDAR to obtain valid loopback data;
[0009] c) Use UTM projection to project latitude and longitude onto a plane rectangular coordinate system;
[0010] d) Align the coordinate system;
[0011] e) Place the valid GPS data and valid loopback data into two separate queues and complete the queue update;
[0012] f) Extract the timestamp of the current lidar point cloud frame. Set the timestamp threshold to Extract the latest GPS timestamp from the queue of valid GPS data. Extract the latest loopback data frame timestamp from the valid loopback data queue. ,if If so, the GPS data is determined to be valid data. If so, the loopback data is determined to be valid data;
[0013] g) Determine if there is any loopback data that is determined to be valid data. If it exists, use the loopback data as the robot's repositioning pose output. If it does not exist, use the GPS data that is determined to be valid data as the valid repositioning pose output.
[0014] Furthermore, in step a), the reliability of the differential GPS data is initially assessed by comparing the number of satellites searched with the fixed solution, and the number of satellites searched is set to... Set the fixed solution flag for the number of satellites searched to be... Set the threshold for the number of satellites searched to 1. The fixed solution flag for setting the threshold for the number of satellites searched is... ,when and At that time, the input GPS data is valid GPS data.
[0015] Preferred, , .
[0016] Furthermore, in step b), the keyframe detected by the current loop closure is set to... The corresponding key pose is Set up local map construction Required keyframe number threshold Through formula
[0017] Calculate the local map Combine the current real-time lidar point cloud frame with the local map Standard scores were obtained by performing NDT registration. With registration coordinate transformation Set the registration score threshold to ,when When the input loopback data is considered valid loopback data, the key pose is determined. Convert to coordinate transformation matrix Through formula The accurate cyclic coordinate transformation matrix is calculated. The precise cyclic coordinate transformation matrix The pose is converted into a six-dimensional pose as a candidate repositioning pose.
[0018] Preferred, , .
[0019] Furthermore, in step d), the GPS trajectory of the robot transformed to a plane is collected, and the coordinate transformation matrix is obtained by aligning the map trajectory with the GPS trajectory using rqt_reconfigure in ROS. The pose representation of GPS projection onto the plane is set as follows: Through formula Calculate the coordinates transformed to the map coordinate system. .
[0020] Preferred, .
[0021] The beneficial effects of this invention are: it leverages the complementary advantages of differential GPS, which provides more accurate position output in open environments and performs better in complex environments, through loop closure detection. It also utilizes UTM projection, which transforms the latitude and longitude coordinates output by GPS into a point cloud map coordinate system. Compared to traditional methods relying on point cloud registration and search for relocation, this method offers higher robustness and accuracy. Attached Figure Description
[0022] Figure 1 This is a flowchart of the method of the present invention. Detailed Implementation
[0023] The following is in conjunction with the appendix Figure 1 The present invention will be further described below.
[0024] This invention integrates differential GPS and LiDAR to address the relocalization problem when the real-time point cloud data output by the LiDAR for outdoor robots fails to register with the point cloud map. It also solves the robot's initial localization problem upon startup. The fusion of multiple sensors increases the system's robustness and success rate.
[0025] Specifically, through the appendix Figure 1 As shown, this robot relocalization method, which combines GPS and LiDAR, employs a Huace P3 differential GPS and a Velodyne 16-line LiDAR and includes the following steps:
[0026] a) Input GPS data and filter it to obtain valid GPS data. For differential GPS data, the reliability of the data can be initially assessed by the number of satellites searched and the fixed solution.
[0027] b) Filter the loopback input and loopback data of the robot's LiDAR to obtain valid loopback data. Using LiDAR loopback detection for initial robot localization is a common method in the SLAM field, but the data obtained from loopback detection is often inaccurate, therefore it needs to be filtered.
[0028] c) Use UTM projection to project latitude and longitude onto a plane rectangular coordinate system.
[0029] d) Align the coordinate system.
[0030] e) Place the valid GPS data and valid loopback data into two separate queues and complete the queue update.
[0031] f) Extract the timestamp of the current lidar point cloud frame. Set the timestamp threshold to Extract the latest GPS timestamp from the queue of valid GPS data. Extract the latest loopback data frame timestamp from the valid loopback data queue. ,if If so, the GPS data is determined to be valid data. If so, the loopback data is determined to be valid data.
[0032] g) Determine if there is any loopback data that is determined to be valid data. If it exists, use the loopback data as the robot's repositioning pose output. If it does not exist, use the GPS data that is determined to be valid data as the valid repositioning pose output.
[0033] This invention solves the positioning loss problem that occurs when outdoor robots rely on LiDAR point cloud frames and point cloud maps for registration and localization, and also addresses the issue of robots failing to initialize their positioning at the start of registration. It leverages the complementary strengths of differential GPS, which provides more accurate position output in open environments and performs better in complex environments through loop closure detection. Furthermore, to convert the latitude and longitude coordinates output by GPS to the point cloud map coordinate system, this invention also utilizes UTM projection. Compared to traditional methods relying on point cloud registration and search for relocalization, this invention offers higher robustness and accuracy.
[0034] Example 1:
[0035] In step a), the reliability of the differential GPS data is initially assessed by comparing the number of satellites searched with the fixed solution. The number of satellites searched is set to... Set the fixed solution flag for the number of satellites searched to be... Set the threshold for the number of satellites searched to 1. The fixed solution flag for setting the threshold for the number of satellites searched is... ,when and At that time, the input GPS data is considered valid GPS data. In one embodiment of the present invention, , .
[0036] Example 2:
[0037] In step b), the keyframe detected by the current loop closure is set to... The corresponding key pose is Set up local map construction Required keyframe number threshold Through formula
[0038] Calculate the local map Combine the current real-time lidar point cloud frame with the local map Standard scores were obtained by performing NDT registration. With registration coordinate transformation Set the registration score threshold to ,when When the input loopback data is considered valid loopback data, the key pose is determined. Convert to coordinate transformation matrix Through formula The accurate cyclic coordinate transformation matrix is calculated. The precise cyclic coordinate transformation matrix The pose is converted into a six-dimensional pose as a candidate repositioning pose. In one embodiment of the present invention, , .
[0039] Example 3:
[0040] In step d), the GPS trajectory of the robot transformed to a plane is collected, and the coordinate transformation matrix is obtained by aligning the map trajectory with the GPS trajectory using rqt_reconfigure in ROS. The pose representation of GPS projection onto the plane is set as follows: Through formula Calculate the coordinates transformed to the map coordinate system. In one embodiment of the present invention, .
[0041] Finally, it should be noted that the above descriptions are merely preferred embodiments of the present invention and are not intended to limit the present invention. Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art can still modify the technical solutions described in the foregoing embodiments or make equivalent substitutions for some of the technical features. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the protection scope of the present invention.
Claims
1. A robot relocalization method based on a combination of GPS and lidar, characterized in that, Includes the following steps: a) Input GPS data and filter the data to obtain filtered GPS data; b) Filter the loop closure detection data of the robot's LiDAR to obtain the filtered loop closure data; c) Use UTM projection to project latitude and longitude onto a plane rectangular coordinate system; d) Align the coordinate system; e) Place the valid GPS data and valid loopback data into two separate queues and complete the queue update; f) Extract the timestamp of the current lidar point cloud frame. Set the timestamp threshold to Extract the latest GPS timestamp from the queue of valid GPS data. Extract the latest loopback data frame timestamp from the valid loopback data queue. ,if If so, the GPS data is determined to be valid data. If so, the loopback data is determined to be valid data; g) Determine if there is any loop data that is determined to be valid data. If it exists, use the loop data as the robot's repositioning pose output. If it does not exist, use the GPS data that is determined to be valid data as the valid repositioning pose output. In step b), the keyframe detected by the current loop closure is set to... The corresponding key pose is Set up local map construction Required keyframe number threshold Through formula Calculate the local map Combine the current real-time lidar point cloud frame with the local map Standard scores were obtained by performing NDT registration. With registration coordinate transformation Set the registration score threshold to According to the standard score Registration score threshold After obtaining the filtered loopback data, the key poses are... Convert to coordinate transformation matrix Through formula The accurate cyclic coordinate transformation matrix is calculated. The precise cyclic coordinate transformation matrix The pose is converted into a six-dimensional pose as a candidate repositioning pose. In step d), the GPS trajectory of the robot transformed to a plane is collected, and the coordinate transformation matrix is obtained by aligning the map trajectory with the GPS trajectory using rqt_reconfigure in ROS. The pose representation of GPS projection onto the plane is set as follows: Through formula Calculate the coordinates transformed to the map coordinate system. .
2. The robot relocalization method based on the combination of GPS and lidar according to claim 1, characterized in that: In step a), the reliability of the differential GPS data is initially assessed by comparing the number of satellites searched with the fixed solution. The number of satellites searched is set to... Set the fixed solution flag for the number of satellites searched to be... Set the threshold for the number of satellites searched to 1. The fixed solution flag for setting the threshold for the number of satellites searched is... ,when and At that time, the filtered GPS data was obtained.
3. The robot relocalization method based on the combination of GPS and lidar according to claim 2, characterized in that: ， 。 4. The robot relocalization method based on the combination of GPS and lidar according to claim 1, characterized in that: ， 。 5. The robot relocalization method based on the combination of GPS and lidar according to claim 1, characterized in that: 。