A positioning method based on 3D point cloud registration

Abstract

The present invention discloses a positioning method based on 3D point cloud, including: 3D laser mapping, data acquisition, feature point extraction and matching optimization, and path planning. The 3D laser mapping process uses a mobile robot to establish a point cloud map of a complete working environment. During navigation, a multi-line laser radar is used to obtain real-time point cloud data, and pre-process and extract special detection points. Laser odometer data is obtained by inter-frame matching, and then the laser odometer data is optimized. The nearest neighbor search is used to search for point cloud data adjacent to the current frame point cloud. The relative conversion relationship between the current robot posture and the robot posture in the point cloud map is calculated using an icp matching algorithm, and the relative conversion relationship is added as a constraint to the posture graph to optimize the laser odometer and obtain accurate robot posture information. The navigation tool is used to realize autonomous navigation of the robot. The present invention improves the positioning accuracy of the mobile robot during navigation, and at the same time improves the stability and reliability of positioning.

Description

Technical Field

[0001] The present invention relates to the technical field of robot autonomous positioning and navigation, and in particular to a positioning method based on 3D point cloud registration. Background Art

[0002] With the development of computer technology, mobile robots have gained wide attention. Autonomous navigation of robots is the basic function of mobile robots, and the positioning of robots is the prerequisite for realizing autonomous navigation of robots. Robots can obtain information about themselves and the environment through the sensor modules they carry. The sensors commonly used in existing positioning technologies include inertial navigation sensors (IMU), wheel encoders, GPS, laser radar, etc. Among them, GPS is suitable for positioning and navigation in outdoor environments. In indoor environments with poor signals, the GPS accuracy is difficult to meet the requirements or even cannot work normally.

[0003] At present, the positioning technology of laser radar combined with inertial navigation sensor and wheel encoder is widely used in indoor mobile robots. Among them, the wheel encoder combined with the inertial navigation sensor (IMU) can obtain the position information and posture information of the robot. When the robot moves, the original data of the wheel encoder and the inertial navigation sensor can be solved to obtain the movement information of the robot such as the movement speed and acceleration. The robot's odometer information is obtained by combining the robot's posture information and movement information. The current environment information scanned by the laser radar is matched with the saved two-dimensional map to obtain the estimated position and posture of the robot in the environment. The maximum detection range of a single-line laser radar is 10 to 20 meters. In an open large indoor environment, it cannot detect enough environmental information, resulting in the failure of robot positioning. The detection range of a multi-line laser radar can reach more than 100 meters. In the existing laser radar positioning solution, the three-dimensional laser point cloud data of the multi-line laser radar is converted into two-dimensional laser data to replace the single-line laser radar, which effectively increases the effective range of the laser data and improves the positioning accuracy. However, in an environment with sparse feature points, it is not accurate to use laser data containing only plane information to align with a two-dimensional map to obtain the robot's position information, because the plane laser data cannot accurately describe the robot's environment information, and the two-dimensional map cannot fully preserve the environment information. Therefore, in some special usage scenarios, such as long corridors, the robot's positioning may have large errors. Summary of the invention

[0004] In order to overcome the shortcomings of the prior art, the present invention provides a positioning method based on 3D point cloud registration to improve positioning accuracy and enhance applicability and robustness to special environments.

[0005] This method uses a multi-line laser radar to process the robot environment point cloud obtained by real-time scanning and match it with the three-dimensional environment point cloud map obtained by 3DSLAM technology to obtain the estimated posture of the robot in the working environment. The data is fused with the data of the inertial navigation sensor and the wheel encoder to calculate more accurate robot posture information. The posture information obtained by the positioning module is used as input data, and the robot's driving path is planned through the move_base algorithm and teb algorithm to realize the robot's navigation function.

[0006] A positioning method based on 3D point cloud registration includes the following steps:

[0007] Step 1: Calibrate sensor external parameters; install the inertial navigation sensor on the base of the robot and use this position as the origin of the base coordinate system; calculate the relative position relationship between the radar coordinate system and the base coordinate system by manually measuring the relative position relationship between the lidar and the inertial navigation sensor; use the ROS (Robot Operating System) communication mechanism on the industrial computer to publish the TF conversion relationship topic between the radar coordinate system and the base coordinate system;

[0008] Step 2: 3D mapping; control the robot to move in the working environment; obtain the 3D point cloud map of the robot's working environment through 3DSLAM technology and downsample the point cloud. The 3D point cloud map is saved in the octree data format; extract the point cloud within the set height range of the point cloud map, and project it onto the horizontal plane of the origin of the robot chassis coordinate system to form a two-dimensional grid map;

[0009] Step 3: Feature point extraction and matching;

[0010] Step 3-1: Point cloud preprocessing; use 3D laser radar to obtain real-time point cloud data; select a starting point and an end point, keep the angle between the two points between Π and 3Π, and control the angle of a frame of point cloud within the range of the starting point and the end point; save the received point cloud information in the form of a two-dimensional array, the first index of the array indicates the line number where the point is located, the second index indicates the number of points on the line where the point is located, and the data stored in the array indicates the distance between the point and the laser radar; remove point clouds that are too far or too close according to the distance of the point cloud; calculate the horizontal angle between each point in the horizontal laser beam below the multi-line laser radar and its previous point, and mark the two points whose horizontal angle between the two points is less than a certain threshold as ground points; use the point cloud clustering algorithm to cluster the point clouds that meet the number of clusters into multiple point cloud sets, and remove the point clouds that cannot form clusters as noise points;

[0011] Step 3-2: Feature point extraction; calculate the curvature values ​​of all cluster points except the ground points; the calculation formula is:

[0012]

[0013] where r i represents the distance value of the i-th point cloud, and S represents the number of calculated point clouds; the point cloud of a frame is divided into multiple regions according to the orientation, and the point cloud is sorted according to the curvature of each point in each region, and the points with curvature values ​​greater than a certain threshold are marked as corner points, and the points with curvature values ​​less than a certain threshold are marked as plane points; the points with the largest curvature and the points with the smallest curvature are marked as large corner points and large plane points;

[0014] Step 3-3: Inter-frame matching; Use the point cloud matching algorithm to match the current point cloud with the previous frame point cloud for corner points and plane points respectively; Match the large corner points in the current frame point cloud with the corner points in the previous frame point cloud point to obtain the pitch angle, roll angle and z-axis change information in the robot's posture; Use the pitch angle, roll angle and z-axis change as constraints to match the large plane points in the current frame point cloud with the plane points in the previous frame point cloud point to obtain the x, y and yaw angle change information in the robot's posture; Assume that the movement of the laser radar is uniform; After calculating the transformation matrix of the end point of a frame of data relative to the starting point, any point in this frame of data can be interpolated according to the time when it is obtained relative to the starting point to obtain the posture of each point; The interpolation formula is as follows:

[0015]

[0016] where t i Indicates the time of the i-th IMU data in a frame of laser point cloud data, It represents the position of the laser odometer when the i-th IMU data is received after the k-th frame of laser point cloud data is received; a rotation matrix R and a translation amount T are used to represent the correspondence between the points in this frame of data and the points in the previous frame of data:

[0017]

[0018] The rotation matrix is ​​differentiated to obtain the distance from the point to the line and the surface, and the error function used for optimization is obtained:

[0019]

[0020] Each row of f represents a feature point. The pose information obtained by matching point cloud frames is optimized by Newton iteration method to obtain laser odometer data and published through ROS topic.

[0021] Step 3-4: Point cloud map matching; Use the kd-tree nearest neighbor search algorithm to match the current point cloud with the point cloud map established in step 2-1; Save the map point cloud that meets the threshold condition as the historical key frame point cloud for matching; Use the point cloud matching algorithm to calculate the relative transformation relationship between the historical key frame point cloud and the current point cloud to obtain the final posture transformation relationship; Use the laser odometer calculated in step 3-2 as the optimization target, add the matched posture constraints to the posture graph to optimize the optimization target and finally obtain accurate robot posture data;

[0022] Step 4: Convert the robot posture information and point cloud data of step 3-3 into two-dimensional laser data as input data, and use the 2D grid map of step 2-1 as the global navigation map; use the ROS navigation algorithm for path planning to realize the robot's autonomous navigation;

[0023] The advantages and positive effects of the present invention are:

[0024] 1. The present invention performs downsampling compression on the map point cloud, reduces the number of point clouds in the point cloud map without affecting the positioning accuracy, and improves the speed of calculation.

[0025] 2. The present invention is based on multi-line laser radar to obtain environmental information. Compared with single-line laser radar, the detection range of multi-line laser radar is much larger than that of single-line laser radar, and it can collect point cloud information in vertical space, so it can obtain richer environmental information.

[0026] 3. The present invention uses a clustering method to segment and filter point cloud information, and registers point clouds with the same attributes, which not only improves the registration accuracy, but also reduces the registration calculation amount and improves the calculation speed.

[0027] 4. The present invention is a method based on 3D point cloud registration, which registers real-time laser point cloud data with the established environmental point cloud map, reduces the impact of sparse feature points on positioning, and improves positioning accuracy.

[0028] 5. The present invention projects multi-line laser point cloud data according to actual use requirements to obtain appropriate two-dimensional laser data, thereby improving the flexibility of navigation. BRIEF DESCRIPTION OF THE DRAWINGS

[0029] Figure 1 It is a flow chart of the present invention.

[0030] Figure 2 Point cloud map of the environment created.

[0031] Figure 3 It is the environmental point cloud information collected in real time.

[0032] Figure 4a to Figure 4bIt is a schematic diagram of point cloud information after point cloud segmentation, where Figure 4a is a schematic diagram of corner points and plane points. Figure 4b It is a ground point schematic diagram.

[0033] Figure 5 The two-dimensional laser data is obtained by converting the three-dimensional point cloud data. DETAILED DESCRIPTION

[0034] The present invention is further described in detail below with reference to the accompanying drawings.

[0035] A positioning method based on 3D point cloud, the workflow diagram is as follows Figure 1 As shown, the method consists of four steps: 3DSLAM, data collection, feature point extraction and matching optimization, and path planning. Figure 2 The displayed image is a point cloud map of the environment constructed using 3DSLAM technology, which contains location information and attribute information. Figure 3 It is the real-time point cloud data of the current environment collected by multi-line laser radar. Figure 4a to Figure 4b It is the point cloud data after point cloud segmentation, including corner points, plane points and ground points. It is used in the navigation process Figure 5 The two-dimensional laser data shown enables obstacle avoidance.

[0036] Combination Figure 1-5 , the specific implementation methods of the present invention are as follows:

[0037] Step 1: Calibrate sensor external parameters. Install the inertial navigation sensor on the base of the robot as the origin of the base coordinate system. Install the robosense 16-line laser radar 1m above the inertial navigation sensor. The industrial computer uses NVIDIA's TX2 development board, which is installed with Ubuntu 16.04 operating system + ROS Kinetic + PCL + gtsam as the software platform. By manually measuring the relative position relationship between the laser radar and the inertial navigation sensor, the relative position relationship between the radar coordinate system and the base coordinate system is calculated. Use the communication mechanism of ROS (Robot Operating System) to publish the TF conversion relationship topic between the radar coordinate system and the base coordinate system.

[0038] Step 2: 3D mapping. Use the logic remote control handle to control the robot to move in the working environment to ensure that the laser radar can scan all directions. During the movement of the robot, the IMU data of the inertial navigation sensor and the point cloud data of the laser radar are used as input data, and the Lego-loam3DSLAM algorithm is run to build a point cloud map of the experimental environment. The point cloud compression algorithm is used to compress and downsample the point cloud. After the robot mapping is completed, use the PCL point cloud tool to save the point cloud map. Extract the point cloud map. Figure 1-2The point cloud in the m height range is projected onto the horizontal plane of the origin of the robot chassis coordinate system to form a two-dimensional grid map.

[0039] Step 3: Feature point extraction and matching.

[0040] Step 3-1: Point cloud preprocessing. When the robot is navigating, a multi-line radar is used to scan the surrounding environment in real time, and the received point cloud data is preprocessed. Create a ROS topic to receive laser point cloud data and convert the laser point cloud data into the ROS format. The point cloud data received after the laser radar runs for one week is regarded as a frame of point cloud data. The first point cloud data received in a frame is regarded as the starting point, and the last point cloud data received is regarded as the last point. Taking the negative axis of the x-axis as the reference, calculate the horizontal angle between the starting point and the last point. If the difference between the starting point angle and the last point angle is greater than 3Π, subtract 2Π from the last point angle. If the starting point angle and the last point angle are less than Π, add 2Π to the last point angle. Through the above method, the angle difference between the starting point and the last point is kept between Π and 3Π. Project all point cloud data onto a two-dimensional array. The first index of the array indicates the line number where the point is located, and the second index indicates the number of points on the line where the point is located. The data stored in the array indicates the distance between the point and the laser radar. Remove point clouds that are too far or too close in the point cloud. The laser radar used in the present invention has a total of 16 laser beams. The working ground when the robot is running can be regarded as a horizontal plane, so only 8 lines from bottom to top may scan the ground. Traverse all the points of the 8 laser beams and find the midpoint that is adjacent to the first index value of the current point in the two-dimensional array and the same as the second index value, that is, the point with the same horizontal angle as the current point and adjacent to the laser beam. Calculate the horizontal angle difference between the current point cloud and the adjacent point. If the angle difference is less than 10°, both points are marked as ground points. The point cloud other than the ground point is marked using the breadth-first search method, that is, take a core point, calculate the distance between each point cloud in the eight neighborhoods of the core point and the core point, if the distance is less than the threshold, it is considered to be in a cluster, if it is greater than the threshold, it is no longer in the same cluster. Take the point in the same cluster in the eight neighborhoods as the core point, calculate the distance between the point and the unscreened point in its eight neighborhoods, if it is greater than the threshold, it is not in a cluster, and if it is less than the threshold, it is considered to be in a cluster. And so on, until all the points in the eight neighborhoods of all points are not in a cluster, then the search for a cluster ends. If the number of point clouds in a cluster is greater than 30, the points in the cluster are marked as available cluster points. If the number of point clouds in a cluster is less than 30, the point clouds in the cluster are all marked as discrete points. The clustering method can effectively remove noise points in laser point cloud data. The processed point cloud data is published through the ROS topic.

[0041] Step 3-2: Feature point extraction. Create a ROS node to receive the point cloud topic published in step 3-1. Use the fusion data of the inertial navigation sensor and the wheel encoder to interpolate the processed point cloud data to remove motion distortion. Traverse every point cloud except the ground, and calculate the deviation of each point from the five adjacent points in front and behind as the curvature value c. The calculation formula is:

[0042]

[0043] where r i Represents the distance value of the i-th point cloud, S represents the number of calculated point clouds, and in the present invention, S is 10. A circle of point clouds is equally divided into 6 sector-shaped areas, and the point clouds are sorted in each area according to the curvature of each point. The curvature values ​​greater than the threshold are regarded as corner points, and the curvature values ​​less than the threshold are plane points. The two points with the largest curvature among the corner points are marked as large corner points, and the 10 points with the smallest curvature among the plane points are marked as large plane points. The pose transformation relationship between the two frames of point clouds is found through point-to-point matching. The large corner point set of the current frame point cloud is matched with the corner point set of the previous frame point cloud, and the large plane point set of the current frame point cloud is matched with the plane point set of the previous frame point cloud. This matching method reduces the number of matched point clouds and improves the matching accuracy.

[0044] Step 3-3: Inter-frame matching. In order to further improve the matching accuracy, point-to-line and point-to-surface matching methods are adopted. For the correspondence between corner points and corner point lines, first find the two points j and l that are closest to the previous frame of point cloud data corresponding to the corner point i, and then determine whether these two points are corner points. j and l must be points on different lines, because a line can have at most one edge point in a certain segment at a time. Plane points correspond to the plane in the previous frame of point cloud. Find the nearest point of the previous frame of point cloud data, find another point on the line scan, and then find a point on the adjacent scan line, so that the three points are not on the same line to form a plane. With the correspondence between point-to-line and point-to-surface, the next step is to require the distance between point-to-line and point-to-surface. First, calculate the distance dε from the point to the line segment. The formula is as follows:

[0045]

[0046] in represents the i-th point in the k-th frame, It represents the vector from the point cloud of the i-th point in the k+1-th frame to the point cloud of the j-th point in the k-th frame. The numerator of this formula is the cross product of two vectors, and the modulus after the cross product becomes the area of ​​the triangle formed by the two vectors. The denominator of the formula is the modulus of the vector, which is equivalent to the length of the base of the triangle. The area of ​​a triangle divided by an edge can calculate the distance from the edge to the corresponding vertex, that is, the distance from the corner point to the corner line.

[0047] Next, find the distance dh from the plane point to the corresponding plane. The formula is as follows:

[0048]

[0049] The upper part of the numerator in the formula is calculated to obtain a vector, which represents the height of the cube, and the lower part is calculated to obtain a vector. The cross product of the two vectors and the modulus represents the area of ​​the triangle, and the product of the two represents the volume of the cube. The denominator represents the noodles of the triangle on the bottom of the cube, and the height obtained is the distance from the plane point to the plane. Assume that the movement of the laser radar is uniform. After calculating the transformation matrix of the end point of a frame of data relative to the starting point, any point in this frame of data can be interpolated according to the time when it is obtained relative to the starting point to obtain the position and posture of each point. The interpolation formula is as follows:

[0050]

[0051] where t i Indicates the time of the i-th IMU data in a frame of laser point cloud data, It represents the position of the laser odometer when the i-th IMU data is received after the k-th frame of laser point cloud data. A rotation matrix R and a translation amount T are used to represent the correspondence between the points in this frame of data and the points in the previous frame of data:

[0052]

[0053] The rotation matrix is ​​differentiated to obtain the distance from the point to the line and the surface, and the error function used for optimization is obtained:

[0054]

[0055] Each row of f represents a feature point. The pose information obtained by matching point cloud frames is optimized by Newton iteration method to obtain laser odometer data and published through ROS topic.

[0056] Step 3-4: Point cloud map matching. In order to reduce the amount of matching calculations, downsample the current point cloud. Use the kd-tree nearest neighbor search algorithm to match the current point cloud with the point cloud map. Save the map point cloud with a matching distance less than 0.5 as a historical keyframe point cloud for matching. Use the icp matching algorithm to calculate the relative transformation relationship between the historical keyframe point cloud and the current point cloud to obtain the final pose transformation relationship. Use the laser odometry calculated in step 3-3 as the optimization target, use gtsam to add the matched pose constraints to the pose graph, and use the graph optimization algorithm to optimize the optimization target to finally obtain accurate robot pose data.

[0057] Step 4: Use the navigation toolkit for navigation. The toolkit requires two input data: a two-dimensional global map and two-dimensional laser data. The two-dimensional global environment map is obtained by step 2, and the two-dimensional laser data is formed by projecting the point cloud data obtained by the 5th to 10th lines of the multi-line laser radar onto a two-dimensional horizontal plane. Use the robot pose calculated in steps 3-4 to replace the robot pose data obtained by the adaptive Monte Carlo algorithm. Plan the global driving path through the move_base algorithm, expand the two-dimensional laser to form a local cost map, and use the teb algorithm for local path planning to achieve autonomous navigation of the robot.

Claims

1. A positioning method based on 3D point cloud registration, the specific steps are as follows: Step 1: Calibrate sensor external parameters; install the inertial navigation sensor on the base of the robot, and use the position of the inertial navigation sensor as the origin of the base coordinate system; manually measure the relative position relationship between the lidar and the inertial navigation sensor to calculate the relative position relationship between the radar coordinate system and the base coordinate system; use the communication mechanism of the robot operating system ROS on the industrial computer to publish the TF conversion relationship topic between the radar coordinate system and the base coordinate system; Step 2: 3D mapping; control the robot to move in the working environment; obtain the 3D point cloud map of the robot's working environment through 3DSLAM technology and downsample the point cloud. The 3D point cloud map is saved in the octree data format; extract the point cloud within the set height range of the point cloud map, and project it onto the horizontal plane of the origin of the robot chassis coordinate system to form a two-dimensional grid map; Step 3: Feature point extraction and matching; Step 3-1: Point cloud preprocessing; use 3D laser radar to obtain real-time point cloud data; select a starting point and an end point, keep the angle between the two points between Π and 3Π, and control the angle of a frame of point cloud within the range of the starting point and the end point; save the received point cloud information in the form of a two-dimensional array, the first index of the array indicates the line number where the point is located, the second index indicates the number of points on the line where the point is located, and the data stored in the array indicates the distance between the point and the laser radar; remove point clouds that are too far or too close according to the distance of the point cloud; calculate the horizontal angle between each point in the horizontal laser beam below the multi-line laser radar and its previous point, and mark the two points whose horizontal angle between the two points is less than a certain threshold as ground points; use the point cloud clustering algorithm to cluster the point clouds that meet the number of clusters into multiple point cloud sets, and remove the point clouds that cannot form clusters as noise points; Step 3-2: Feature point extraction; calculate the curvature values ​​of all cluster points except the ground points; the calculation formula is: where r i represents the distance value of the i-th point cloud, and S represents the number of calculated point clouds; the point cloud of a frame is divided into multiple regions according to the orientation, and the point cloud is sorted according to the curvature of each point in each region, and the points with curvature values ​​greater than a certain threshold are marked as corner points, and the points with curvature values ​​less than a certain threshold are marked as plane points; the points with the largest curvature and the points with the smallest curvature are marked as large corner points and large plane points; Step 3-3: Inter-frame matching; Use the point cloud matching algorithm to match the current point cloud with the previous frame point cloud for corner points and plane points respectively; Match the large corner points in the current frame point cloud with the corner points in the previous frame point cloud point to obtain the pitch angle, roll angle and z-axis change information in the robot's posture; Use the pitch angle, roll angle and z-axis change as constraints to match the large plane points in the current frame point cloud with the plane points in the previous frame point cloud point to obtain the x, y and yaw angle change information in the robot's posture; Assume that the movement of the laser radar is uniform; After calculating the transformation matrix of the end point of a frame of data relative to the starting point, any point in this frame of data can be interpolated according to the time when it is obtained relative to the starting point to obtain the posture of each point; The interpolation formula is as follows: where t i Indicates the time of the i-th IMU data in a frame of laser point cloud data, It represents the position of the laser odometer when the i-th IMU data is received after the k-th frame of laser point cloud data is received; a rotation matrix R and a translation amount T are used to represent the correspondence between the points in this frame of data and the points in the previous frame of data: The rotation matrix is ​​differentiated to obtain the distance from the point to the line and the surface, and the error function used for optimization is obtained: Each row of f represents a feature point. The pose information obtained by matching point cloud frames is optimized by Newton iteration method to obtain laser odometer data and published through ROS topic. Step 3-4: Point cloud map matching; Use the kd-tree nearest neighbor search algorithm to match the current point cloud with the point cloud map established in step 2-1; Save the map point cloud that meets the threshold condition as the historical key frame point cloud for matching; Use the point cloud matching algorithm to calculate the relative transformation relationship between the historical key frame point cloud and the current point cloud to obtain the final posture transformation relationship; Use the laser odometer calculated in step 3-2 as the optimization target, add the matched posture constraints to the posture graph to optimize the optimization target and finally obtain accurate robot posture data; Step 4: Convert the robot posture information and point cloud data of step 3-3 into two-dimensional laser data as input data, and use the 2D grid map of step 2-1 as the global navigation map; use the ROS navigation algorithm for path planning to achieve autonomous navigation of the robot.

2. A positioning method based on 3D point cloud registration as claimed in claim 1, characterized in that: In step 1, NVIDIA's TX2 development board is used as the hardware platform, and the development board is installed with Ubuntu 16.04 operating system + ROS Kinetic + PCL + gtsam as the software platform.

3. A positioning method based on 3D point cloud registration as claimed in claim 1, characterized in that: The height range of the point cloud map extracted in step 2 is set to 1 to 2 meters; the algorithm for constructing the 3D point cloud map is the Lego-loam slam algorithm.

4. A positioning method based on 3D point cloud registration as claimed in claim 1, characterized in that: The 3D laser radar described in step 3-1 has a total of 16 laser beams. The working ground when the robot is running can be regarded as a horizontal plane and the laser radar is installed horizontally, so only 8 lines from bottom to top may scan the ground; the angle threshold for judging the ground point cloud is 5°; the threshold for judging whether the clustered point cloud meets the requirements is 40.

5. The positioning method based on 3D point cloud registration according to claim 1, characterized in that: The number of sector-shaped areas divided into a circle of point clouds in step 3-2 is 6; the number of large corner points extracted from the point cloud is 3, and the number of large plane points is 15.

6. A positioning method based on 3D point cloud registration as claimed in claim 1, characterized in that: Step 4 converts the robot posture information and point cloud data of step 3-3 into two-dimensional laser data, and the laser beams used are the 5th to the 10th.

Images