Feature normal vector-based point cloud matching positioning method

Abstract

The invention discloses a point cloud matching and positioning method based on a feature normal vector. The method comprises the following steps: 1) obtaining an indoor wall point cloud map; 2) starting a laser radar, and collecting two-dimensional point cloud data; 3) clustering the collected two-dimensional point cloud data, calculating the distance between the point clouds in the two-dimensional point cloud data, and dividing the two-dimensional point cloud data into a plurality of classes according to the distance between the point clouds in the two-dimensional point cloud data; 4) performing linear fitting on each type of two-dimensional point cloud data to generate corresponding wall vector data; 5) respectively generating two normal vectors vertical to each wall surface; (6) the included angles between the two normal vectors and the rays emitted to the wall surface by the laser radar are calculated respectively, the normal vector with the included angle larger than 90 degrees serves as the normal vector of the wall surface, and three-dimensional point cloud data are generated; and 7) matching the three-dimensional point cloud data with the indoor wall point cloud map, calculating the nearest point of the wall point cloud, and calculating the position and attitude information of the robot at the moment to realize positioning.

Description

Technical Field

[0001] The present invention relates to the field of point cloud matching and positioning, and in particular to a point cloud matching and positioning method based on a characteristic normal vector. Background Art

[0002] Intelligent robots, such as construction robots, logistics robots, and cleaning robots, need to work indoors. It is very important to determine the position of the intelligent robot itself and its relative position to other intelligent robots, that is, to perform precise positioning. However, due to the severe attenuation and multipath effect of the signal, it is difficult for general outdoor positioning facilities to work effectively in indoor environments. For example, GPS can locate which building the intelligent robot is currently in, but GPS cannot obtain a more accurate indoor position, which requires more precise map information and higher positioning accuracy. At the same time, complex and changeable indoor scenes also put forward higher requirements on the performance of each sensor.

[0003] In the prior art, intelligent robots working and moving indoors usually use the following solutions for positioning: 1. Ultrasonic positioning technology, which is used to position by sending and receiving ultrasonic waves, but it has the disadvantages of mirror reflection, multipath effect, influence of ambient temperature, obvious signal attenuation and limited beam angle, and cannot fully obtain environmental information; 2. Visual navigation positioning technology, which is used to position by collecting and processing environmental images, but it has high requirements for lighting and is troublesome to train models; 3. Positioning technology based on auxiliary nodes, which determines its own position by the position of auxiliary nodes, but the auxiliary nodes need to be manually set up and stored on site in advance, and the practicality is poor; 4. SLAM technology. Among them, SLAM technology is widely used because of its strong flexibility and the fact that it does not require auxiliary equipment to be set up in the scene in advance, and its accuracy can reach the millimeter level. SLAM technology is divided into VSLAM and LIDAR SLAM, which respectively use visual point cloud and laser point cloud matching algorithms to build maps and position. However, both VSLAM and LIDAR SLAM technologies use point cloud feature matching. Typical algorithms include ICP (Iterative Closest Point) and NDT (Normal Distributions Transform). The NDT algorithm has a fast calculation speed and lower accuracy than ICP, and is often used for coarse alignment. The ICP algorithm needs to find neighboring matching points, so it is time-consuming and has better accuracy. It is often used for fine alignment. However, in some scenarios, such as thin walls indoors and the distance between the two sides of the wall is short, when the ICP algorithm is used to find neighboring points by distance, the position error of the point cloud will cause errors in matching the wall point cloud, affecting the positioning accuracy. Summary of the invention

[0004] In view of the above-mentioned problems, the present invention provides a point cloud matching and positioning method based on feature normal vectors, which adopts the ICP algorithm of SLAM technology to solve the problem of point cloud position error existing on some walls of the algorithm and improve the positioning accuracy of the intelligent robot.

[0005] The technical solution of the present invention is: a point cloud matching and positioning method based on characteristic normal vectors, comprising the following steps:

[0006] 1) Obtain indoor wall point cloud map through SLAM technology;

[0007] 2) Start the robot's laser radar to collect a frame of two-dimensional point cloud data, where the two-dimensional point cloud data includes the horizontal coordinate x and the vertical coordinate y of each point cloud;

[0008] 3) clustering the collected two-dimensional point cloud data, calculating the distance between each point cloud in the two-dimensional point cloud data, and dividing the two-dimensional point cloud data into multiple classes according to the distance between each point cloud in the two-dimensional point cloud data;

[0009] 4) Perform straight line fitting on each type of two-dimensional point cloud data to generate corresponding wall vector data;

[0010] 5) Generate two normal vectors perpendicular to each wall surface according to the wall vector data of various two-dimensional point cloud data;

[0011] 6) respectively calculating the angles between two normal vectors perpendicular to the wall and the ray emitted by the laser radar to the wall, taking the normal vector with an angle greater than 90° as the normal vector of the wall, and generating a frame of three-dimensional point cloud data, wherein the three-dimensional point cloud data includes the horizontal coordinate x, the vertical coordinate y, and the normal vector n of each point cloud;

[0012] 7) According to the ICP point cloud matching algorithm, the three-dimensional point cloud data is matched with the indoor wall point cloud map, the nearest point of the wall point cloud is calculated, and the position and posture information of the robot at that moment is calculated to achieve positioning.

[0013] Preferably, in step 3), point clouds with a distance between each point cloud less than 5 cm are classified into one class.

[0014] Preferably, in step 5), the two normal vectors generated that are perpendicular to the wall are in opposite directions.

[0015] Preferably, in step 7), the formula for calculating the nearest point of the wall point cloud is:

[0016]

[0017] Where, distance is the sum of the distances from all point clouds in the current frame to the nearest point cloud on the wall, x and y are the coordinates of the wall point cloud position in the indoor wall point cloud map, and x is the distance from the nearest point cloud on the wall to the nearest point cloud on the wall. ref ,y ref is the position coordinate of the point cloud in the current frame, ω is the normalization coefficient, n is the characteristic normal vector angle, n ref is the normal vector of the point cloud in the current frame.

[0018] Preferably, in step 7), the formula for calculating the position and posture information of the robot at that moment is:

[0019]

[0020] In the formula, p s Represents the point cloud of the current frame, p t Represents the nearest matched wall point cloud, R and t represent the rotation and translation matrices of the robot's position and posture.

[0021] The advantages of the present invention are: by separating the indoor walls and processing them separately, the wall vector information is extracted, and the normal vector information label is marked on each wall, and multi-dimensional information is added to the wall point cloud, so that when the point cloud is looking for adjacent points, it does not rely solely on the Euclidean distance, but adds the characteristic normal vector angle parameter, expands the two-dimensional point cloud into a three-dimensional point cloud, and distinguishes the wall point clouds with similar distances but not belonging to the same plane by calculating the normalized spatial distance, solves the problem of point cloud position matching error existing in the ICP algorithm of the prior art for thin walls that are closely spaced, increases the robustness and accuracy of point cloud matching, enables the positioning algorithm to stably output millimeter-level positioning results, and improves the positioning accuracy of the intelligent robot. In addition, the method has high anti-interference, high flexibility, high accuracy, and does not require the pre-laying of auxiliary equipment. BRIEF DESCRIPTION OF THE DRAWINGS

[0022] Figure 1 It is a flow chart of the present invention. DETAILED DESCRIPTION

[0023] See also Figure 1 , a point cloud matching and positioning method based on feature normal vectors, comprising the following steps:

[0024] 1) Obtain indoor wall point cloud map through SLAM technology;

[0025] 2) Start the robot's laser radar to collect a frame of two-dimensional point cloud data, where the two-dimensional point cloud data includes the horizontal coordinate x and the vertical coordinate y of each point cloud;

[0026] 3) Clustering the collected two-dimensional point cloud data, calculating the distance between each point cloud in the two-dimensional point cloud data, and classifying the point clouds with a distance between each point cloud less than 5 cm into one class;

[0027] 4) Perform straight line fitting on each type of two-dimensional point cloud data to generate corresponding wall vector data;

[0028] 5) Generate two normal vectors perpendicular to each wall and in opposite directions according to the wall vector data of various two-dimensional point cloud data;

[0029] 6) respectively calculating the angles between two normal vectors perpendicular to the wall and the ray emitted by the laser radar to the wall, taking the normal vector with an angle greater than 90° as the normal vector of the wall, and generating a frame of three-dimensional point cloud data, wherein the three-dimensional point cloud data includes the horizontal coordinate x, the vertical coordinate y, and the normal vector n of each point cloud;

[0030] 7) According to the ICP point cloud matching algorithm, the three-dimensional point cloud data is matched with the indoor wall point cloud map, the nearest point of the wall point cloud is calculated, and the position and posture information of the robot at that moment is calculated to achieve positioning.

[0031] The formula for calculating the closest point of the wall point cloud is:

[0032]

[0033] Where, distance is the sum of the distances from all point clouds in the current frame to the nearest point cloud on the wall, x and y are the coordinates of the wall point cloud position in the indoor wall point cloud map, and x is the distance from the nearest point cloud on the wall to the nearest point cloud on the wall. ref ,y ref is the position coordinate of the point cloud in the current frame, ω is the normalization coefficient, n is the characteristic normal vector angle, n ref is the normal vector of the point cloud in the current frame.

[0034] The formula for calculating the position and posture information of the robot at this moment is:

[0035]

[0036] In the formula, p s Represents the point cloud of the current frame, p t Represents the nearest matched wall point cloud, R and t represent the rotation and translation matrices of the robot's position and posture.

[0037] The present invention tags each wall with normal vector information, adds the normal vector information to the wall point cloud, expands the two-dimensional point cloud into a three-dimensional point cloud, distinguishes the wall point clouds that are close in distance but do not belong to the same plane, solves the problem of point cloud position matching error existing in the ICP algorithm of the prior art for thin walls that are closely spaced, increases the robustness and accuracy of point cloud matching, enables the positioning algorithm to stably output millimeter-level positioning results, and improves the positioning accuracy of the intelligent robot.

[0038] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modification made to the present invention by those skilled in the art without departing from the spirit of the present invention shall fall within the protection scope of the present invention.

Claims

1. A point cloud matching and positioning method based on characteristic normal vector, characterized in that: The following steps are involved: 1) Obtain indoor wall point cloud map through SLAM technology; 2) Start the robot's laser radar to collect a frame of two-dimensional point cloud data, where the two-dimensional point cloud data includes the horizontal coordinate x and the vertical coordinate y of each point cloud; 3) clustering the collected two-dimensional point cloud data, calculating the distance between each point cloud in the two-dimensional point cloud data, and dividing the two-dimensional point cloud data into multiple classes according to the distance between each point cloud in the two-dimensional point cloud data; 4) Perform straight line fitting on each type of two-dimensional point cloud data to generate corresponding wall vector data; 5) Generate two normal vectors perpendicular to each wall surface according to the wall vector data of various two-dimensional point cloud data; 6) respectively calculating the angles between two normal vectors perpendicular to the wall and the ray emitted by the laser radar to the wall, taking the normal vector with an angle greater than 90° as the normal vector of the wall, and generating a frame of three-dimensional point cloud data, wherein the three-dimensional point cloud data includes the horizontal coordinate x, the vertical coordinate y, and the normal vector n of each point cloud; 7) According to the ICP point cloud matching algorithm, the three-dimensional point cloud data is matched with the indoor wall point cloud map, the nearest point of the wall point cloud is calculated, and the position and posture information of the robot at that moment is calculated to achieve positioning.

2. The point cloud matching and positioning method based on characteristic normal vector according to claim 1, characterized in that: In the step 3), point clouds with distances between them less than 5 cm are classified into one class.

3. The point cloud matching and positioning method based on characteristic normal vector according to claim 1, characterized in that: In the step 5), the two normal vectors generated that are perpendicular to the wall are in opposite directions.

4. The point cloud matching and positioning method based on characteristic normal vector according to claim 1, characterized in that: In step 7), the formula for calculating the nearest point of the wall point cloud is: Where, distance is the sum of the distances from all point clouds in the current frame to the nearest point cloud on the wall, x and y are the coordinates of the wall point cloud position in the indoor wall point cloud map, and x is the distance from the nearest point cloud on the wall to the nearest point cloud on the wall. ref ,y ref is the position coordinate of the point cloud in the current frame, ω is the normalization coefficient, n is the characteristic normal vector angle, n ref is the normal vector of the point cloud in the current frame.

5. The point cloud matching and positioning method based on characteristic normal vector according to claim 1, characterized in that: In step 7), the formula for calculating the position and posture information of the robot at this moment is: In the formula, p s Represents the point cloud of the current frame, p t Represents the nearest matched wall point cloud, R and t represent the rotation and translation matrices of the robot's position and posture.

Images