Method for performing three-dimensional lidar localization on basis of key-frame map

Abstract

A method for performing three-dimensional LiDAR localization on the basis of a key-frame map. On the basis of a self-developed map format, a point cloud of a key frame and a pose of the key frame are stored in a map, and on the basis of a search algorithm of the key frame and a positioning algorithm of the key frame, a current frame and a global map are matched during localization, so as to meet the requirements of real-time localization. The method specifically comprises the following steps: step one, loading a map; step two, parsing the map; step three, performing coarse-pose calculation; step four, generating a key-frame sub-map; step five, performing data pre-processing; step six, matching a current frame with a key frame; and step seven, performing fine-pose recycling. The method for performing three-dimensional LiDAR localization on the basis of a key-frame map has the beneficial effects of avoiding manual modification and cropping of a map after mapping is completed, thus ensuring the localization precision while reducing the computation amount during localization, not excessively occupying CPU resources, not increasing resource occupancy during localization in large-map scenarios, reducing manual errors, and improving the localization efficiency.

Description

A three-dimensional lidar positioning method based on keyframe map Technical Field

[0001] The present invention relates to the field of robot SLAM technology, and in particular to a three-dimensional laser radar positioning method based on a key frame map. Background Art

[0002] Current 3D LiDAR positioning technology in robotics relies primarily on a global map. Positioning requires matching the current frame with the global map. The larger the map, the longer the calculation time, making it inadequate for real-time positioning. Two common solutions are currently used in the industry. The first involves splitting the global map into multiple smaller maps, matching the current frame with these smaller maps during positioning. The second involves downsampling the global map, making it sparse, and then matching the current frame with the downsampled global map during positioning. The former requires manual intervention, while the latter can compromise positioning accuracy.

[0003] Summary of the Invention

[0004] The present invention proposes a three-dimensional laser radar positioning method based on key frame maps, which solves the existing problems.

[0005] To achieve the above objectives, the present invention adopts the following technical solution: a three-dimensional lidar positioning method based on a keyframe map. Based on a self-developed map format, the map stores the point cloud and pose of the keyframes. During positioning, the current frame is matched with the global map to meet the needs of real-time positioning. Specifically, the method includes the following steps:

[0006] Step 1: Load the map: The map contains the point cloud and pose of the keyframes.

[0007] Step 2: Analyze the map: extract the keyframe point cloud and keyframe poses in the map and analyze them;

[0008] Step 3: Coarse pose estimation: Input imu and odometry data. The data of these two sensors are optional and used as the coarse pose of the current frame point cloud positioning.

[0009] Step 4: Generate key frame sub-graph: Based on the obtained rough pose P t Select a sub-map in the global map, where the sub-map is composed of multiple key frames in the global map;

[0010] Step 5: Data preprocessing: Input the laser point cloud of the current frame. The data is obtained by the lidar sensor. After receiving the data, the data is first preprocessed;

[0011] Step 6: Match the current frame with the key frame: After downsampling the input laser point cloud, it can be matched with the local sub-image selected in step 4, using the GICP proposed in the paper "Generalized-ICP" as the matching algorithm;

[0012] Step 7: Recycle the fine pose: Recycle the fine pose obtained in step 6 to step 3 to update the pose P of the previous frame. t-1 =P′ t , and then calculate the next frame.

[0013] Preferably, the map uses a mapping algorithm developed by the company itself and is saved in a map format developed by the company itself.

[0014] Preferably, the IMU data is used for positioning and navigation. It is a sensor for measuring and tracking the posture of an object. By processing the posture information output by the IMU, information such as position, direction and speed in space can be obtained, thereby realizing autonomous movement and navigation. It can also be used for applications such as posture control, motion control and stability control; the odometry data can be used for speed and distance measurement, providing local accurate estimation of the robot's posture and speed.

[0015] Preferably, first determine whether odometry data is input. If so, pass the odometry data to step three; at the same time, determine whether imu data is input. If so, pass the imu data to step three, and use the two data as the coarse pose of the current frame point cloud positioning.

[0016] Preferably, the specific method of step three is: the posture P at the previous moment t-1 =(x t-1 ,y t-1 ,yaw t-1 ), if the data of imu and odometry are used, P can be obtained based on the angular velocity ω and linear velocity v provided by imu and odometry t =(x t ,y t ,yaw t )=(x t-1 +v x ·Δt,y t-1 +v y ·Δt,yaw+ω·Δt), where Δt is the time interval between two adjacent frames of laser data at time t and time t-1, and the obtained P t As the coarse value of the current frame positioning;

[0017] Preferably, if imu and odometry data are not used, the motion model is considered to be a uniform motion model, and the pose at the last moment t-1 is P t-1 =(xt-1 ,y t-1 ,yaw t-1 ), the posture at the previous moment t-2 is P t-2 =(x t-2 ,y t-2 ,yaw t-2 ), according to the postures at the two moments, the current vehicle speed can be obtained as The rough pose P of the current frame can be roughly calculated t =(x t ,y t ,yaw t )=(x t-1 +v x ·Δt,y t-1 +v y ·Δt,yaw t-1 +ω·Δt), where Δt is the time interval between the current frame at time t and the previous frame at time t-1.

[0018] Preferably, the specific method of step 4 is: first, take the current posture P t Establish the coordinate axis, the coordinate system is the right-hand coordinate system, so that the key frame poses in the global map are divided into four quadrants, each quadrant takes a pose, and the poses all meet the distance P in the quadrant t Nearest neighbor, so that at least one key frame is selected in the map, and at most four key frames are selected in the map, and the selected key frames are spliced ​​into a local sub-map.

[0019] Preferably, the specific method of step five is: converting the point cloud to the vehicle body coordinate system according to the external parameters of the laser radar, that is, the installation position of the laser radar in the vehicle body center coordinate system, which is given by the laser radar calibration algorithm developed by the company, then removing the vehicle body part from the current laser point cloud, and then performing a simple downsampling process on the point cloud according to the density of the laser point cloud, and finally converting the laser point cloud into the vehicle body coordinate system according to the obtained P t Convert to the global map coordinate system.

[0020] Preferably, the specific method of step six is: according to the pose P and the rough pose P matched between the current frame and the local sub-image t The precise pose P of the current frame in the global map coordinate system can be obtained t ′=P t +P, which is the coordinate of the current frame point cloud under the global map, that is, the current positioning pose.

[0021] The beneficial effects of the present invention are as follows: based on the self-developed map format, the point cloud and pose of the key frames are saved in the map, and based on the key frame positioning algorithm, the current frame and the global map are matched during positioning to meet the needs of real-time positioning. The present invention effectively avoids manual map segmentation after the map is built, and there is no need to manually crop the map after the map is built. During the positioning process, the calculation amount is reduced while ensuring the positioning accuracy, without occupying too much CPU resources. Positioning in large map scenarios does not increase resource usage, thereby reducing human errors and improving positioning efficiency. BRIEF DESCRIPTION OF THE DRAWINGS

[0022] FIG1 is a flow chart of a positioning system according to the present invention.

[0023] FIG2 shows the selection rules of key frame subgraphs according to the present invention.

[0024] FIG3 is the target value of this matching of the present invention.

[0025] Preferred embodiments of the present invention

[0026] The technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, rather than all the embodiments.

[0027] As shown in Figures 1-3, a keyframe map-based 3D LiDAR positioning method is based on a self-developed map format. The map stores the point cloud and pose of the keyframes. Based on the keyframe search algorithm and positioning algorithm, the current frame is matched with the global map during positioning to meet the requirements of real-time positioning. Specifically, the method includes the following steps:

[0028] Step 1: Load the map: The map contains the point cloud and pose of the keyframes.

[0029] Step 2: Analyze the map: extract the keyframe point cloud and keyframe poses in the map and analyze them;

[0030] Step 3: Coarse pose estimation: Input imu and odometry data. The data of these two sensors are optional and used as the coarse pose of the current frame point cloud positioning.

[0031] Step 4: Generate keyframe subgraphs: Select a subgraph from the global map based on the obtained coarse pose. The subgraph consists of multiple keyframes from the global map.

[0032] Step 5: Data preprocessing: Input the laser point cloud of the current frame. The data is obtained by the lidar sensor. After receiving the data, the data is first preprocessed;

[0033] Step 6: Match the current frame with the key frame: After downsampling the input laser point cloud, it can be matched with the local sub-image selected in step 4, using the GICP proposed in the paper "Generalized-ICP" as the matching algorithm;

[0034] Step 7: Recycle the fine pose: Based on the fine pose obtained in step 6, recycle to step 3 to update the pose of the previous frame, and then calculate the next frame.

[0035] The map uses the company's self-developed mapping algorithm and is saved in the company's self-developed map format. IMU data is used for positioning and navigation. It is a sensor used to measure and track the posture of objects. By processing the posture information output by the IMU, it can obtain information such as position, direction and speed in space, thereby realizing autonomous movement and navigation. It can also be used for applications such as posture control, motion control and stability control; odometry data can be used for speed and distance measurement, and can provide local accurate estimation of the robot's position and speed. Odometry information can be obtained from various sources, such as IMU, radar and wheel encoders. Since the IMU drifts over time and the wheel encoder drifts with the navigation distance, they are often used together to offset their respective negative characteristics.

[0036] First, determine whether odometry data is input. If so, pass the odometry data to step three. At the same time, determine whether imu data is input. If so, pass the imu data to step three. Use the two data as the rough pose of the current frame point cloud positioning. The pose P at the previous moment is t-1 =(x t-1 ,y t-1 ,yaw t-1 ), if the data of imu and odometry are used, P can be obtained based on the angular velocity ω and linear velocity v provided by imu and odometry t =(x t ,y t ,yaw t )=(x t-1 +v x ·Δt,y t-1 +v y ·Δt,yaw+ω·Δt), where Δt is the time interval between two adjacent frames of laser data at time t and time t-1, and the obtained P t As the rough value of the current frame positioning; if the imu and odometry data are not used, the motion model is considered to be a uniform motion model, and the pose at the last moment t-1 is P t-1 =(x t-1 ,y t-1 ,yaw t-1), the posture at the previous moment t-2 is P t-2 =(x t-2 ,y t-2 ,yaw t-2 ), according to the postures at the two moments, the current vehicle speed can be obtained as The rough pose P of the current frame can be roughly calculated t =(x t ,y t ,yaw t )=(x t-1 +v x ·Δt,y t-1 +v y ·Δt,yaw t-1 +ω·Δt), where Δt is the time interval between the current frame at time t and the previous frame at time t-1.

[0037] 2 and 3, with the current posture P t Establish the coordinate axis. The triangle in Figure 2 represents the current pose, and the five-pointed star represents the key frame pose in the global map. The coordinate system is the right-hand coordinate system. In this way, the key frame pose in the global map is divided into four quadrants. Each quadrant takes a pose, and the poses all meet the distance P in the quadrant. t Nearest neighbor. In this way, at least one key frame is selected in the map, and at most four key frames are selected in the map. The selected key frames are spliced ​​into a local sub-graph. The three key frame poses circled in Figure 3 will form a sub-graph as the target value of this matching. The point cloud is converted to the vehicle coordinate system based on the external parameters of the laser radar, that is, the installation position of the laser radar in the vehicle center coordinate system. This parameter is given by the laser radar calibration algorithm developed by our company. Then, the vehicle body part is eliminated from the current laser point cloud. Then, a simple downsampling process is performed on the point cloud according to the density of the laser point cloud. Finally, the laser point cloud is converted according to the obtained P t Convert to the global map coordinate system.

[0038] The GICP method is also known as the generalized iterative closest point method. The GICP algorithm is a point-to-surface registration method that achieves point cloud alignment by minimizing the distance between point clouds. Compared with the traditional ICP algorithm, the GICP algorithm introduces the normal information of the point cloud, which improves the stability and accuracy of the registration. Then, the pose P and the rough pose P obtained by matching the current frame and the local sub-image are calculated. t The precise pose P′ of the current frame in the global map coordinate system can be obtained t =P t +P, which is the coordinate of the current frame point cloud under the global map, that is, the current positioning pose. The obtained fine pose is recycled to the coarse pose calculation to update the pose of the previous frame. The above process is repeated to calculate the next frame to make the positioning more accurate.

[0039] The present invention effectively avoids manual map segmentation and cropping after map construction is completed. During the positioning process, it reduces the amount of calculation while ensuring positioning accuracy, does not occupy too many CPU resources, and does not increase resource usage when positioning in large map scenarios, thereby reducing human errors and improving positioning efficiency.

[0040] In operation, the present invention first saves the point cloud and pose of the key frames in the map based on the self-developed map format, extracts the key frame point cloud and the pose of the key frames in the map and parses them, and determines whether odometry data is input. If so, the odometry data is passed to the coarse pose inference; at the same time, it is determined whether imu data is input. If so, the imu data is passed to the coarse pose inference, and the two data are used as the coarse pose for the current frame point cloud positioning. According to the obtained coarse pose, a sub-map in the global map is selected, and the sub-map is composed of multiple key frames in the global map. Then, the current frame laser point cloud is input and the data is pre-processed. The current frame and the key frame are matched, and finally the precise positioning pose of the current frame is obtained. The obtained precise pose is recycled to the coarse pose inference, thereby updating the pose of the previous frame, and the above process is repeated to calculate the next frame.

[0041] The above description is only a preferred specific embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any technician familiar with the technical field, within the technical scope disclosed by the present invention, who makes equivalent replacements or changes based on the technical solution and inventive concept of the present invention, should be covered by the scope of protection of the present invention.

Claims

1. A three-dimensional laser radar positioning method based on a key frame map, characterized in that: Based on the self-developed map format, the point cloud and pose of the key frames are saved in the map. Based on the key frame search algorithm and positioning algorithm, the current frame and the global map are matched during positioning to meet the needs of real-time positioning. The specific steps include: Step 1: Load the map: The map contains the point cloud and pose of the keyframes. Step 2: Analyze the map: extract the key frame point cloud and the key frame pose in the map and analyze them; Step 3: Coarse pose estimation: Input imu and odometry data. The data of these two sensors are optional and used as the coarse pose of the current frame point cloud positioning. Step 4: Generate key frame sub-graph: Based on the obtained rough pose P t Select a sub-map in the global map, where the sub-map is composed of multiple key frames in the global map; Step 5: Data preprocessing: Input the laser point cloud of the current frame. The data is obtained by the laser radar sensor. After receiving the data, the data is preprocessed first; Step 6: Match the current frame with the key frame: After downsampling the input laser point cloud, it can be matched with the local sub-image selected in step 4, using GICP as the matching algorithm; Step 7: Recycle the fine pose: Recycle the fine pose obtained in step 6 to step 3 to update the pose P of the previous frame. t-1 =P t ′, and then calculate the next frame.

2. A three-dimensional laser radar positioning method based on key frame map according to claim 1, characterized in that: The IMU data is used for positioning and navigation. It is a sensor used to measure and track the posture of an object. By processing the posture information output by the IMU, the position, direction and speed information in space can be obtained, thereby realizing autonomous movement and navigation. It can also be used for posture control, motion control and stability control applications; the odometry data can be used for speed and distance measurement, providing local accurate estimation of the robot's posture and speed.

3. The three-dimensional laser radar positioning method based on key frame map according to claim 1 is characterized in that: First, determine whether odometry data is input. If so, pass the odometry data to step three. At the same time, determine whether imu data is input. If so, pass the imu data to step three, and use the two data as the coarse pose of the current frame point cloud positioning.

4. The three-dimensional laser radar positioning method based on key frame map according to claim 1 is characterized in that: The specific method of step three is: the posture P at the previous moment t-1 =(x t-1 ,y t-1 ,yaw t-1 ), if the data of imu and odometry are used, P can be obtained based on the angular velocity ω and linear velocity v provided by imu and odometry t =(x t ,y t ,yaw t )=(x t-1 +v x ·Δt,y t-1 +v y ·Δt,yaw+ω·Δt), where Δt is the time interval between two adjacent frames of laser data at time t and time t-1, and the obtained P t As a coarse value for the current frame positioning; If imu and odometry data are not used, the motion model is considered as a uniform motion model, and the pose at the last moment t-1 is P t-1 =(x t-1 ,y t-1 ,yaw t-1 ), the position at the previous time t-2 is P t-2 =(x t-2 ,y t-2 ,yaw t-2 ), according to the position at two moments, the current vehicle moving speed can be obtained as The rough pose P of the current frame can be roughly calculated t =(x t ,y t ,yaw t )=(x t-1 +v x ·Δt,y t-1 +v y ·Δt,yaw t-1 +ω·Δt), where Δt is the time interval between the current frame at time t and the previous frame at time t-1.

5. The three-dimensional laser radar positioning method based on key frame map according to claim 1 is characterized in that: The specific method of step 4 is: first, take the current posture P t Establish the coordinate axis, the coordinate system is a right-handed coordinate system, so that the key frame poses in the global map are divided into four quadrants, each quadrant takes a pose, and the poses all satisfy the distance P in the quadrant t Nearest neighbor, so that at least one key frame is selected in the map, and at most four key frames are selected in the map, and the selected key frames are spliced ​​into a local sub-map.

6. The three-dimensional laser radar positioning method based on key frame map according to claim 1 is characterized in that: The specific method of step 5 is: convert the point cloud to the vehicle body coordinate system according to the external parameters of the laser radar, that is, the installation position of the laser radar in the vehicle body center coordinate system. The parameters are given by the laser radar calibration algorithm developed by our company, and then the vehicle body is removed from the current laser point cloud. Then, a simple downsampling process is performed on the point cloud according to the density of the laser point cloud. Finally, the laser point cloud is converted into the vehicle body coordinate system according to the obtained P t Convert to the global map coordinate system.

7. The three-dimensional laser radar positioning method based on key frame map according to claim 1 is characterized in that: The specific method of step six is: according to the pose P and the rough pose P matched between the current frame and the local sub-image t The precise position P of the current frame in the global map coordinate system can be obtained t ′=P t +P, which is the coordinate of the current frame point cloud under the global map, that is, the current positioning posture.

Images