A local navigation method based on positioning of a retroreflective panel

Abstract

The application provides a local navigation method based on a retroreflective sheet positioning, comprising the following steps: S1, controlling a mobile robot to enter a retroreflective sheet scanning area, and then switching to a retroreflective sheet positioning mode; S2, the mobile robot starts matching a retroreflective sheet contour; S3, calculating coordinates (lx i , ly i ) of screened point cloud data; S4, obtaining a pose matrix of the retroreflective sheet in a laser radar coordinate system through the coordinates of the point cloud data, then obtaining a pose matrix T b of the retroreflective sheet in a odometer coordinate system according to a transformation matrix T g of the laser radar in the odometer coordinate system, and obtaining a pose matrix T r of the retroreflective sheet in a mobile robot coordinate system according to a transformation matrix T L of an origin of the odometer coordinate system in the mobile robot coordinate system; S5, controlling the mobile robot to navigate and drive to a retroreflective sheet end point according to the pose matrix T L of the retroreflective sheet in the mobile robot coordinate system; and the method is simple and reliable.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of robot navigation, in particular to a local navigation method based on positioning of a retroreflective panel. BACKGROUND

[0002] In recent years, with the continuous improvement of production technology, modern intelligent manufacturing workshops are mostly equipped with mobile robots. The mobile robots have the advantages of high automation and high intelligence. Through the transmission mechanism, the mobile robots can automatically interface with other logistics equipment to realize the automatic loading and unloading and carrying of goods and materials. In this process, the mobile robot needs to perform local navigation in the indoor workshop. The mobile robot can achieve local navigation through a laser radar-based navigation method. The laser radar positioning method based on the retroreflective column has high precision and good stability.

[0003] For example, a quick matching method for a retroreflective column is disclosed in Chinese Patent Application No. 201911404516.1, published on April 24, 2020. The method specifically includes the following steps: S1, detecting whether there is a pose at the previous moment; S2, if the detection result is yes, matching the scanned retroreflective column with the road sign in the global map based on static matching, and if the detection result is no, matching the scanned retroreflective column with the road sign in the global map based on dynamic matching.

[0004] The above-mentioned document achieves high-precision positioning within 0.5 mm through a conical positioning pin and a positioning bushing after the AGV scans the two-dimensional code navigation preliminary navigation, which cannot solve the technical problem of precise docking of the AGV and the machine table. In addition, the existing retroreflective column positioning method does not increase the intensity judgment in the point cloud data, which may lead to inaccurate determination of the contour information due to the non-compliance of the point cloud data. In the process of transporting materials by the AGV in the workshop, the AGV often needs to determine the starting point of the travel path and then travel to the machine table endpoint according to the path. Especially for long straight paths, the angle of the AGV may deviate during the rotation process of confirming the starting point. For example, if the deviation occurs at the starting point, the deviation will occur during the subsequent movement, which cannot achieve precise docking with the machine table to complete the material handling. SUMMARY

[0005] The present application provides a local navigation method based on positioning of a retroreflective panel, which can accurately form a retroreflective panel contour, so that the mobile robot can accurately reach the positioning endpoint. The method is simple and reliable.

[0006] To achieve the above-mentioned purpose, the present application provides a local navigation method based on positioning of a retroreflective panel, comprising the following steps:

[0007] S1, control the mobile robot to enter the retro-reflective plate scanning area, and then switch to the retro-reflective plate positioning mode;

[0008] S2, the mobile robot starts to match the retro-reflective plate contour;

[0009] S2.1, preset the point cloud intensity threshold T, the weighting coefficients a, b, and g by scanning the point cloud data in the retro-reflective plate area through the laser radar arranged on the mobile robot,

[0010] S2.2, collect the scanned point cloud data, and then screen the point cloud data according to the preset intensity threshold T,

[0011] S2.3, determine the length error of the screened point cloud data contour and the length of the retro-reflective plate, determine the center distance error of the screened point cloud data and the retro-reflective plate, and determine the relative intensity error of the screened point cloud data and the retro-reflective plate, and use the evaluation function cost in formula (1) to determine whether the retro-reflective plate contour is matched;

[0012] cost = a * err_length + b * err_dist + g * cost_intensity (1)

[0013] S3, calculate the coordinates (lx i , ly i ) of the screened point cloud data;

[0014] S4, get the pose matrix T of the retro-reflective plate in the laser radar coordinate system through the coordinates of the point cloud data, then get the pose matrix T g of the retro-reflective plate in the odometer coordinate system according to the transformation matrix T b of the laser radar in the odometer coordinate system, and get the pose matrix T L of the retro-reflective plate in the mobile robot coordinate system according to the transformation matrix T r of the odometer coordinate system origin in the mobile robot coordinate system;

[0015] S5, control the mobile robot to navigate to the end point of the retro-reflective plate according to the pose matrix T L of the retro-reflective plate in the mobile robot coordinate system.

[0016] The above setting is to scan point cloud data in the scanning area by the laser radar, and preset a point cloud intensity threshold T, so as to compare the intensity of the scanned point cloud data with the preset point cloud intensity threshold T, filter out the point cloud data matched with the reflector, exclude other interference factors, and improve the accuracy. The length error, center distance error and relative intensity error of the screened point cloud data and the reflector are determined, and then the evaluation function cost is obtained, and the evaluation function cost is used to judge whether the reflector profile matches. Since the evaluation function cost uses a weighting coefficient, the length error, center distance error and relative intensity error will all affect the matching of the entire profile, so that the finally determined profile can be closer to the length, center and intensity, so that the mobile robot can complete the profile matching of the scanned reflector. Then, the coordinates (lx i , ly i ) of the screened point cloud data are calculated to obtain the pose matrix T of the reflector in the laser radar coordinate system, and then the transformation matrix T b of the laser radar in the odometer coordinate system is obtained, so that the pose matrix T g of the reflector in the odometer coordinate system is obtained. At the same time, the transformation matrix T r of the origin of the odometer coordinate system in the mobile robot coordinate system is obtained, so that the pose matrix T L of the reflector in the mobile robot coordinate system is obtained. Finally, the mobile robot can be accurately positioned and accurately navigated to the end point of the reflector according to the pose matrix T L of the reflector in the mobile robot coordinate system, so that the mobile robot can be more accurately positioned to the end point of the reflector after the profile of the reflector is accurately determined.

[0017] Further, step S2.3 includes: determining the length error of the profile length of the screened point cloud data and the length of the reflector by the err_length function, determining the center distance error of the screened point cloud data and the reflector by the err_dist function, and determining the relative intensity error of the screened point cloud data and the reflector by the cost_intensity function.

[0018] The above setting can facilitate the determination of the corresponding parameter information according to the function preset by the INS / GPS system.

[0019] Further, the step S2.2 further includes:

[0020] The intensity threshold T0 of the scanned point cloud data is compared with the preset intensity threshold T, if T0 is in the range of [0.6*T, 0.7*T], it is judged as the point cloud data of the reflector, otherwise it is judged as other interference point cloud data.

[0021] The above settings can filter out the point cloud data matched with the reflector from the entire point cloud data obtained by scanning.

[0022] Further, the step S3 further includes the following steps:

[0023] S3.1, obtaining the distance l between the lidar and the reflector and the scanning angle z from the lidar scanning,

[0024] S3.2, then calculating lx i , ly i ,

[0025] lx i = l*cos(z) (2)

[0026] ly i = l*sin(z) (3).

[0027] The above settings can calculate the filtered point cloud coordinates by the distance l between the lidar and the reflector and the scanning angle z.

[0028] Further, the S4 further includes:

[0029] The pose matrix of the reflector in the lidar coordinate system As follows,

[0030]

[0031] The pose matrix T of the reflector in the odometry coordinate system g As follows,

[0032]

[0033] The pose matrix T of the reflector in the mobile robot coordinate system L As follows,

[0034] T L = T g * T r (6).

[0035] The above settings can obtain the pose matrix T of the reflector in the lidar coordinate system by the coordinates of the point cloud data, further calculate the pose matrix T of the reflector in the odometry coordinate system g , and finally obtain the pose matrix T of the reflector in the mobile robot coordinate system L . BRIEF DESCRIPTION OF DRAWINGS

[0036] Figure 1This is a flowchart illustrating the working method of the present invention. Detailed Implementation

[0037] The present invention will now be described in further detail with reference to the accompanying drawings and specific embodiments.

[0038] like Figure 1 As shown, a local navigation method based on reflector positioning is used to control a robot to move along a path between the starting position and the ending position of the reflector. Reflectors are set on both sides of the robot's walking path, and the robot is equipped with a LiDAR. The LiDAR emits laser light, which passes through the reflector, and then returns data to determine the point cloud data. The specific implementation includes the following steps:

[0039] S1. Control the mobile robot to enter the reflector scanning area, and then switch to reflector positioning mode.

[0040] S2. The mobile robot begins matching the reflector outline.

[0041] S2.1. Scan the point cloud data in the reflector area using a laser radar set on the mobile robot, and preset the point cloud intensity threshold T and weighting coefficients α, β, γ.

[0042] S2.2 Collect the point cloud data obtained from the scan, and then filter the point cloud data according to the preset intensity threshold T. In this embodiment, the intensity threshold T0 of the scanned point cloud data is compared with the preset intensity threshold T. If T0 is in the range of [0.6*T, 0.7*T], it is determined to be the point cloud data of the reflector; otherwise, it is determined to be other interfering point cloud data.

[0043] S2.3. The INS / GPC navigation system performs calculations using functions. The `err_length` function calculates the error between the outline length of the filtered point cloud data and the length of the reflector. Specifically, the `err_length` function determines the outline length of the point cloud data using the positioning system and then calculates the length error by comparing it with the preset reflector length information. The `err_dist` function calculates the center-to-center distance error between the filtered point cloud data and the reflector. The `err_dist` function determines the error by comparing the distance between the center of the point cloud data and the center of the preset reflector using the positioning system. The `cost_intensity` function calculates the relative intensity error between the filtered point cloud data and the reflector. The `cost_intensity` function determines the relative intensity error value by comparing the intensity information of the point cloud data in the image with the intensity information of the preset reflector. The evaluation function `cost` is used to determine whether the reflector outline matches. In this embodiment, the `err_length`, `err_dist`, and `cost_intensity` functions are existing functions and will not be described in detail here. The formula for the evaluation function `cost` is as follows.

[0044] cost = a * err_length + b * err_dist + g * cost_intensity (1),

[0045] If the retroreflective plate profile matches successfully, go to step S3;

[0046] If the retroreflective plate profile does not match successfully, return to step S2.

[0047] S3, calculate the coordinates (lx i , ly i ) of the screened point cloud data, in this embodiment, also including steps S3.1-S3.2:

[0048] S3.1, in the laser radar scan of step S2.1, obtain the distance l between the laser radar and the retroreflective plate and the scanning angle z,

[0049] S3.2, then calculate lx i , ly i , respectively, using the distance l between the laser radar and the retroreflective plate and the scanning angle z,

[0050] The calculation formula of lx i is as follows:

[0051] lx i = l * cos(z) (2),

[0052] The calculation formula of ly i is as follows:

[0053] ly i = l * sin(z) (3).

[0054] S4, then determine the vehicle body angle by ly i / lx i , so as to obtain the pose matrix of the retroreflective plate in the laser radar coordinate system by the coordinates of the point cloud data and the vehicle body angle value , the pose matrix includes two-dimensional space horizontal coordinates and vertical coordinates and vehicle body angle information, in this embodiment, the calculation formula of the pose matrix of the retroreflective plate in the laser radar coordinate system is as follows, since the mobile robot only moves in the plane, and the plane is the XY axis plane, and the mobile robot will not appear to climb the slope;

[0055] ,

[0056] Then, according to the transformation matrix T b of the laser radar in the odometer coordinate system, the pose matrix T g of the retroreflective plate in the odometer coordinate system is obtained.In the embodiment, the calculation formula of the pose matrix T g of the retroreflective plate in the odometer coordinate system is as follows,

[0057] T g =T b * (5),

[0058] According to the transformation matrix T r of the origin of the odometer coordinate system in the mobile robot coordinate system, the pose matrix T L of the retroreflective plate in the mobile robot coordinate system is obtained. L In the embodiment, the calculation formula of the pose matrix T L of the retroreflective plate in the mobile robot coordinate system is as follows,

[0059] T g =T r *T L (6).

[0060] S5, control the mobile robot to navigate and drive to the end point of the retroreflective plate according to the pose matrix T i of the retroreflective plate in the mobile robot coordinate system.

[0061] The working principle of the application is as follows: the mobile robot is controlled to enter the retroreflective plate area, and then switched to the retroreflective plate positioning mode to scan the retroreflective plate, then the point cloud data obtained by scanning is screened according to the preset intensity threshold T, and the screened point cloud data is judged as the point cloud data of the retroreflective plate, then the length error of the contour of the screened point cloud data and the length of the retroreflective plate is calculated by the err_length function, the distance error between the screened point cloud data and the center of the retroreflective plate is calculated by the err_dist function, the relative intensity error between the screened point cloud data and the retroreflective plate is calculated by the cost_intensity function, and after the evaluation function cost judges that the retroreflective plate contour matching is successful, the coordinates (lx i , ly b ) of the screened point cloud data are calculated and the pose matrix T g of the retroreflective plate in the laser radar coordinate system is obtained, then the pose matrix T r of the retroreflective plate in the odometer coordinate system is obtained according to the transformation matrix T L of the origin of the odometer coordinate system in the mobile robot coordinate system, and finally the pose matrix T L of the retroreflective plate in the mobile robot coordinate system is obtained, so that the mobile robot navigates and drives to the end point of the retroreflective plate according to the pose matrix T of the retroreflective plate in the mobile robot coordinate system.

Claims

1. A local navigation method based on reflector positioning, characterized in that: Includes the following steps: S1. Control the mobile robot to enter the reflector scanning area, and then switch to reflector positioning mode; S2. The mobile robot begins to match the reflector outline; S2.1, The point cloud data within the reflector area is scanned by a lidar mounted on the mobile robot, with preset point cloud intensity threshold T and weighting coefficients α, β, γ. S2.2 Collect the point cloud data obtained from the scan, and then filter the point cloud data according to the preset intensity threshold T. S2.

3. The error between the outline length of the filtered point cloud data and the length of the reflector is determined by the err_length function, the error between the center distance between the filtered point cloud data and the reflector is determined by the err_dist function, and the error between the relative intensity of the filtered point cloud data and the reflector is determined by the cost_intensity function. The evaluation function cost in formula (1) is used to determine whether the reflector outline matches; cost=α*err_length+ β*err_dist+ γ*cost_intensity (1) S3. Calculate the coordinates (lx) of the filtered point cloud data. i ly i ); S4. Obtain the pose matrix of the reflector in the lidar coordinate system using the coordinates of the point cloud data. Then, based on the transformation matrix T of the lidar in the odometer coordinate system... b The pose matrix T of the reflector in the odometer coordinate system is obtained. g Based on the transformation matrix T of the odometry coordinate system origin in the mobile robot coordinate system r Obtain the pose matrix T of the reflector in the mobile robot coordinate system. L ; S5. Control the mobile robot according to the pose matrix T of the reflector in the mobile robot coordinate system. L Navigate to the reflector endpoint.

2. The local navigation method based on reflector positioning according to claim 1, characterized in that: Step S2.2 also includes: The intensity threshold T0 of the scanned point cloud data is compared with the preset intensity threshold T. If T0 is within the range of [0.6*T, 0.7*T], it is determined to be the point cloud data of the reflector; otherwise, it is determined to be other interfering point cloud data.

3. The local navigation method based on reflector positioning according to claim 1, characterized in that: Step S3 also includes the following steps: S3.1 Obtain the distance l between the lidar and the reflector, and the scanning angle z, from the lidar scan. S3.2 Then, calculate lx using the distance l and the scanning angle z respectively. i ly i , lx i =l*cos(z)(2) ly i =l*sin(z)(3)。 4. The local navigation method based on reflector positioning according to claim 1, characterized in that: S4 also includes: The pose matrix of the reflector in the lidar coordinate system as follows, The pose matrix T of the reflector in the odometer coordinate system g as follows, T g =T b * (5), The pose matrix T of the reflector in the mobile robot coordinate system L as follows, T L =T g *T r (6)。

Images