A robot precise positioning method based on three-dimensional laser radar

Abstract

The application discloses a kind of robot accurate positioning method based on three-dimensional laser radar, belong to robot navigation technical field.The method of the present application is first placed at a certain position from target point Mark, by the three-dimensional laser radar installed on mobile robot, obtain the point cloud diagram of Mark and the space where it is located;Then by calculating curvature, the edge point cloud of Mark and the space environment where it is located is extracted;Again with the three-dimensional laser radar spatial coordinate system x, y, z axis three directions to the edge point cloud obtained by straight-through filtering, obtain the edge point cloud of Mark;Finally, straight line fitting is carried out using edge point cloud, and the positioning data of robot is obtained according to the coordinate position relationship of fitted straight line and three-dimensional laser radar and the pose of robot is adjusted.Compared with prior art, the robot accurate positioning method based on three-dimensional laser radar solves the problem that error accumulation will be caused by calculating IMU to form odometer for positioning, and only three-dimensional laser radar is needed to complete the reproduction of this method, it is convenient to use, in such as pinpoint blasting, nuclear waste recycling this kind of need accurate positioning field has wide prospect.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of robot navigation, and particularly relates to a robot accurate positioning method based on a three-dimensional laser radar. BACKGROUND

[0002] At present, people's quality of life and economic development have higher and higher requirements for production efficiency, and traditional robots have been unable to meet the demand. With the rapid development of the field of artificial intelligence, robot technology is rapidly innovating, and intelligent robots with autonomous perception and behavior decision-making gradually become the research focus of the industry due to their more extensive application and development prospects. The premise for such robots to realize free movement and work is to perceive the surrounding environment, build a map, and autonomously position, and many scenes have high requirements for the positioning accuracy of robots, but most of them have problems in dealing with complex environments due to single information source, which cannot meet the use requirements of more accurate, stable, and reliable use.

[0003] At present, the mainstream navigation and positioning method of mobile robots is to position through IMU to solve the odometry, but the accumulated error in navigation will make the final positioning error very large, greatly limiting the application field of mobile robots and affecting the safety of the movement of the robot in the space. SUMMARY

[0004] In view of the defects in the prior art, the application provides a robot accurate positioning method based on a three-dimensional laser radar. First, a marker is placed at a certain position away from a target point, and a three-dimensional laser radar installed on a mobile robot is used to obtain a point cloud map of the marker and the space where the marker is located; then edge point clouds of the marker and the space environment where the marker is located are extracted by calculating the curvature; the obtained edge point clouds are filtered in three directions of the spatial coordinate system x, y and z of the three-dimensional laser radar to obtain edge point clouds of the marker; finally, linear fitting is performed on the edge point clouds, and the positioning data of the robot is obtained according to the relationship between the fitted straight line and the coordinate position of the three-dimensional laser radar, and the pose of the robot is adjusted.

[0005] The technical scheme adopted by the application is as follows:

[0006] A robot accurate positioning method based on a three-dimensional laser radar, characterized by comprising the following steps:

[0007] S1. A marker is placed at a certain position away from a target point, and when the robot is preliminarily positioned to the target point, a three-dimensional laser radar is used to scan to obtain a point cloud map of the space and the marker; then edge point clouds of the space and the marker are extracted by calculating the curvature.

[0008] S2. The edge point clouds are filtered in three directions of the three-axis of the three-dimensional laser radar spatial coordinate system to obtain the edge point clouds of the marker.

[0009] S3. Linear fitting is performed on the edge point cloud of the marker to obtain two linear equations in the three-dimensional laser radar spatial coordinate system as a spatial rectangular coordinate system.

[0010] S4. The intersection coordinates of the two straight lines with the xoy plane of the three-dimensional laser radar spatial coordinate system are calculated, and then the center point coordinates of the two intersection coordinates are calculated.

[0011] S5. According to the positional relationship between the two intersection coordinates, the center point coordinates, and the three-dimensional laser radar spatial coordinate system, the offset and the offset angle of the robot to the marker are obtained; the robot adjusts the robot pose according to the offset data to reach the target point, thereby completing the precise positioning of the robot.

[0012] Further, in the S1, the formula for calculating the curvature is:

[0013]

[0014] where i represents a point in the kth frame of point cloud data; S is a point set composed of the five most adjacent continuous points j of point i, j≠i; F L (k,i) , F L (k,j) are the coordinates of points i and j in the laser radar spatial coordinate system, respectively.

[0015] When the curvature c corresponding to point i is greater than a set threshold, point i is defined as an edge point.

[0016] Further, in the S4, the formula for calculating the center point coordinates is:

[0017]

[0018] where (x0, y0) are the center point coordinates, (x1, y1) and (x2, y2) are the two intersection coordinates, and L is the distance from the center point to the radar; the offset of the marker relative to the three-dimensional laser radar spatial coordinate system is known from L.

[0019] Further, in the S5, the positional relationship between the two intersection coordinates, the center point coordinates, and the three-dimensional laser radar spatial coordinate system is divided into the following four cases:

[0020] A. When the line connecting the center point and the coordinate origin is perpendicular to the line connecting the two intersection points, the offset and the offset angle are calculated by formula (3) at this time:

[0021]

[0022] Wherein, k1 represents the slope of the line connecting the two intersection points, L0 is the distance from the center point to the target point; the robot is accurately moved to the target point by rotating θ1 and translating L1.

[0023] B. When the line connecting the two intersection points is perpendicular to the x-axis, the offset and offset angle are calculated by formula (4) at this time:

[0024]

[0025] The robot is accurately moved to the target point by rotating θ1 for the first time, translating L1 for the first time, rotating θ2 for the second time, and translating L2 for the second time.

[0026] C. When the line connecting the center point and the coordinate origin is not perpendicular to the line connecting the two intersection points, and x2>x1, the offset and offset angle are calculated by formula (5) at this time:

[0027]

[0028] Wherein, k2 represents the reciprocal of the slope of the line connecting the center point and the coordinate origin; β represents the angle between the line connecting the center point and the coordinate origin and the x-axis; the robot is accurately moved to the target point by rotating θ1 for the first time, translating L1 for the first time, rotating θ2 for the second time, and translating L2 for the second time.

[0029] D. When the line connecting the center point and the coordinate origin is not perpendicular to the line connecting the two intersection points, and x2<x1, the offset and offset angle are calculated by formula (6) at this time:

[0030]

[0031] The robot is accurately moved to the target point by rotating θ1 for the first time, translating L1 for the first time, rotating θ2 for the second time, and translating L2 for the second time.

[0032] Further, in the S3, the threshold of the outlier point and the threshold of the inner point are set to perform straight line fitting.

[0033] The robot accurate positioning method based on three-dimensional laser radar provided by the application is the second stage accurate positioning after the robot preliminary navigation positioning to the target position, the edge points of the target point marker are extracted by the three-dimensional laser radar, the left and right profiles are displayed, then straight line fitting is performed, so that the relative position of the marker to the radar coordinate system can be calculated by the two fitted straight lines and then adjusted. In addition, in the process of fitting the straight line, the threshold of the outlier point and the threshold of the inner point can be set to retain more points for fitting, so that different, non-standard shapes on both sides of the marker can be adapted to meet different requirements.

[0034] Compared with the prior art, the robot precise positioning method based on the three-dimensional laser radar solves the problem of error accumulation caused by positioning by solving the IMU to form a odometer, and only three-dimensional laser radar is needed to complete the reproduction of the method, which is convenient to use and has a wide prospect in the field of precise positioning such as point blasting and nuclear waste recycling. BRIEF DESCRIPTION OF DRAWINGS

[0035] Figure 1 A flowchart of the robot precise positioning method based on the three-dimensional laser radar provided by the present application is shown in the figure.

[0036] Figure 2 The figure is an edge point extraction effect diagram of the three-dimensional laser radar in the embodiment of the present application on the space environment;

[0037] Figure 3 The figure is a landmark edge point cloud diagram after straight-through filtering of the space edge point cloud in the embodiment of the present application;

[0038] Figure 4 The figure is an effect diagram of straight line fitting on the two profiles of the landmark in the embodiment of the present application;

[0039] Figure 5(a) is a schematic diagram of the first type of position relationship between the landmark and the robot and the adjustment of the pose operation;

[0040] Figure 5(b) is a schematic diagram of the second type of position relationship between the landmark and the robot and the adjustment of the pose operation;

[0041] Figure 5(c) is a schematic diagram of the third type of position relationship between the landmark and the robot and the adjustment of the pose operation;

[0042] Figure 5(d) is a schematic diagram of the fourth type of position relationship between the landmark and the robot and the adjustment of the pose operation. DETAILED DESCRIPTION

[0043] The present application will be further described in detail below in combination with the drawings and specific embodiments.

[0044] The specific process of the robot precise positioning method based on the three-dimensional laser radar is shown in Figure 1 The key steps are described in detail as follows.

[0045] Referring to Figure 1 , the present embodiment randomly sets a space environment, and the robot precise positioning method based on the three-dimensional laser radar comprises the following steps:

[0046] S1. Place a rectangular plate as a landmark one meter behind the target point, refer to Figure 2When the robot is initially positioned to the target point, a point cloud of the space and the marker is obtained by using a three-dimensional laser radar scan; then edge point clouds of the space and the marker are extracted by calculating the curvature.

[0047] The formula for calculating the curvature is:

[0048]

[0049] wherein i represents a point in the kth frame of point cloud data; S is a point set composed of 5 continuous points j most adjacent to the point i, j≠i; F L (k,i) , F L (k,j) are coordinates of the points i and j in the three-dimensional laser radar space coordinate system.

[0050] When the curvature c corresponding to the point i is greater than a set threshold value 0.1, the point i is defined as an edge point. If a point is calculated to be greater, it represents that the bending degree of the line connecting the point and the surrounding 5 points is greater, which further indicates that the curvature of the point is greater, and the point can be defined as an edge point.

[0051] S2. Refer to Figure 3 In the three-axis direction of the three-dimensional laser radar space coordinate system, straight-through filtering is performed on the edge point cloud to obtain the edge point cloud of the marker.

[0052] S3. Refer to Figure 4 The threshold value of the outlier point is set to 0.5 and the threshold value of the inner point is set to 0.03, straight line fitting is performed on the edge point cloud of the marker to obtain two straight line equations in the three-dimensional laser radar space coordinate system as a space rectangular coordinate system.

[0053] S4. The intersection coordinates of the two straight lines and the xoy plane of the three-dimensional laser radar space coordinate system are calculated, and then the center point coordinates of the two intersection coordinates are calculated.

[0054] The formula for calculating the center point coordinates is:

[0055]

[0056] wherein (x0, y0) is the center point coordinates, (x1, y1) and (x2, y2) are the two intersection coordinates, and L is the distance from the center point to the radar; the offset of the marker relative to the three-dimensional laser radar space coordinate system is known from L.

[0057] S5. According to the positional relationship between the two intersection coordinates, the center point coordinates, and the three-dimensional laser radar space coordinate system, the offset and the offset angle of the robot to the marker are obtained; the robot adjusts the robot pose according to the offset data to reach the target point, thereby completing the accurate positioning of the robot.

[0058] The positional relationship between the coordinates of the two intersection points, the coordinate of the center point and the spatial coordinate system of the three-dimensional laser radar is divided into the following four cases:

[0059] A. Referring to FIG. 5(a), when the line connecting the center point and the coordinate origin is perpendicular to the line connecting the two intersection points, the offset and the offset angle are calculated by formula (3) at this time:

[0060]

[0061] Wherein, k1 represents the slope of the line connecting the two intersection points, and L0 is the distance from the center point to the target point; the robot is accurately moved to the target point by rotating θ1 and translating L1.

[0062] The rotation angle is counterclockwise rotation, and the offset is forward translation, at this time the robot only needs to rotate and translate once.

[0063] B. Referring to FIG. 5(b), when the line connecting the two intersection points is perpendicular to the x-axis, the offset and the offset angle are calculated by formula (4) at this time:

[0064]

[0065] The robot is accurately moved to the target point by first rotating θ1, first translating L1, second rotating θ2, and second translating L2.

[0066] When the center point is on the left of the x-axis, the robot needs to first rotate counterclockwise by 90°, and then rotate clockwise by 90° to reach the target point; when the center point is on the right of the x-axis, the robot needs to first rotate clockwise by 90°, and then rotate counterclockwise by 90° to reach the target point.

[0067] C. Referring to FIG. 5(c), when the line connecting the center point and the coordinate origin is not perpendicular to the line connecting the two intersection points, and x2>x1, the offset and the offset angle are calculated by formula (5) at this time:

[0068]

[0069] Wherein, k2 represents the reciprocal of the slope of the line connecting the center point and the coordinate origin; β represents the angle between the line connecting the center point and the coordinate origin and the x-axis; the robot is accurately moved to the target point by first rotating θ1, first translating L1, second rotating θ2, and second translating L2.

[0070] D. Referring to FIG. 5(d), when the line connecting the center point and the coordinate origin is not perpendicular to the line connecting the two intersection points, and x2

[0071]

[0072] The robot is precisely moved to the target point by first rotation θ1, first translation L1, second rotation θ2 and second translation L2.

[0073] The robot pose adjustment technical solution has the beneficial effects that the present application provides all possible position relationship conditions of the robot and the marker, when the robot reaches a certain position at the target point, the robot pose adjustment formula can be selected according to the position relationship of the robot and the marker, and the fault tolerance and the stability of the precise positioning of the mobile robot are ensured.

[0074] From the above specific embodiments, it can be seen that the positioning error of the robot can be controlled within centimeters by the precise positioning method of the present application, and the pose adjustment mode involved in the present application is realized by publishing the ROS speed topic, without additional workload, so that the present application has high applicability in robot positioning.

[0075] The above only describes the preferred embodiments of the present application and is not intended to limit the present application, and any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present application shall be included in the protection scope of the present application.

Claims

1. A method for precise robot positioning based on three-dimensional lidar, characterized in that, Includes the following steps: S1. Place a marker at a certain distance from the target point. When the robot initially locates the target point, use a 3D LiDAR scanner to obtain a point cloud map of the space it is in and the marker. Then, extract the edge point cloud of the space where the robot is in and the marker by calculating the curvature. S2. Perform pass-through filtering on the edge point cloud in the three-axis direction of the three-dimensional lidar spatial coordinate system to obtain the edge point cloud of the marker; S3. Perform line fitting on the edge point cloud of the marker to obtain two line equations with the three-dimensional lidar spatial coordinate system as the spatial rectangular coordinate system; S4. Calculate the coordinates of the intersection points of the two straight lines with the xoy plane of the three-dimensional lidar spatial coordinate system, and then calculate the coordinates of the center point of the two intersection points; S5. Based on the positional relationship between the coordinates of the two intersection points, the coordinates of the center point, and the three-dimensional lidar spatial coordinate system, the robot's offset and offset angle relative to the marker are obtained; the robot adjusts its pose according to the offset data to reach the target point, thereby completing the robot's precise positioning.

2. The robot precise positioning method based on three-dimensional lidar as described in claim 1, characterized in that, In S1, the formula for calculating curvature is: Where i represents a point in the point cloud data of the k-th frame; S is the set of points consisting of the 5 nearest neighboring points j of point i, j ≠ i; F L (k,i) F L (k,j) Let i and j be the coordinates of points i and j in the lidar spatial coordinate system, respectively. When the curvature c corresponding to point i is greater than a set threshold, point i is defined as an edge point; In S4, the formula for calculating the coordinates of the center point is: Where (x0,y0) are the coordinates of the center point, (x1,y1) and (x2,y2) are the coordinates of the two intersection points, and L is the distance from the center point to the radar; L indicates the offset of the marker relative to the three-dimensional lidar spatial coordinate system.

3. The robot precise positioning method based on three-dimensional lidar as described in claim 2, characterized in that, In S5, the positional relationship between the coordinates of the two intersection points, the coordinates of the center point, and the three-dimensional lidar spatial coordinate system can be categorized into the following four cases: A. When the line connecting the center point and the origin is perpendicular to the line connecting the two intersection points, the offset and offset angle are calculated using formula (3): Where k1 represents the slope of the line connecting the two intersection points, and L0 is the distance from the center point to the target point; the robot is precisely moved to the target point by rotating θ1 and translating L1. B. When the line connecting the two intersection points is perpendicular to the x-axis, the offset and offset angle are calculated using formula (4): The robot is precisely moved to the target point by first rotation θ1, first translation L1, second rotation θ2, and second translation L2. C. When the line connecting the center point and the origin is not perpendicular to the line connecting the two intersection points, and x2 > x1, the offset and offset angle are calculated using formula (5): Where k2 represents the reciprocal of the slope of the line connecting the center point and the origin; β represents the angle between the line connecting the center point and the origin and the x-axis; the robot is precisely moved to the target point by the first rotation θ1, the first translation L1, the second rotation θ2, and the second translation L2; D. When the line connecting the center point and the origin is not perpendicular to the line connecting the two intersection points, and x2 < x1, the offset and offset angle are calculated using formula (6): The robot is precisely moved to the target point by first rotation θ1, first translation L1, second rotation θ2, and second translation L2.

4. The robot precise positioning method based on three-dimensional lidar as described in claim 3, characterized in that, In S3, linear fitting is performed by setting thresholds for outliers and inliers.

Images