Local navigation method based on reflector positioning

Abstract

The invention provides a local navigation method based on reflector positioning, which comprises the following steps: S1, controlling a mobile robot to enter a reflector scanning area, and then switching to a reflector positioning mode; s2, the mobile robot starts to match the contour of the reflector; s3, calculating coordinates (lxi, lyi) of the screened point cloud data; s4, obtaining a pose matrix # imgabs0 # of the reflector in the laser radar coordinate system through the coordinates of the point cloud data, and then obtaining a pose matrix Tg of the reflector in the odometer coordinate system according to a transformation matrix Tb of the laser radar in the odometer coordinate system; obtaining a pose matrix TL of the reflector in the coordinate system of the mobile robot according to the transformation matrix Tr of the original point of the odometer coordinate system in the coordinate system of the mobile robot; s5, controlling the mobile robot to navigate and drive to the end point of the reflector according to the pose matrix TL of the reflector in the coordinate system of the mobile robot; the method is simple and reliable.

Description

Technical Field

[0001] The invention relates to the technical field of robot navigation, and in particular to a local navigation method based on reflector positioning. Background Art

[0002] In recent years, with the continuous improvement of production technology, most modern intelligent manufacturing workshops are equipped with mobile robots. Mobile robots with the advantages of high automation and intelligence can automatically interface with other logistics equipment through the transmission mechanism they are equipped with, thereby realizing the automation of the entire process of loading and unloading and handling 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 lidar-based navigation method. The lidar-based navigation method can be divided into a reflective column-based navigation method and a contour-based navigation method. The reflective column-based lidar positioning method has high accuracy and good stability.

[0003] For example, the patent document with Chinese patent application number 201911404516.1 and publication date 2020.04.24 discloses a method for fast matching of reflective columns, which specifically includes the following steps: S1. Detect whether there is a posture at the previous moment; S2. If the detection result is yes, match the scanned reflective column with the road sign in the global map based on static matching; if the detection result is no, match the scanned reflective column with the road sign in the global map based on dynamic matching.

[0004] The above-mentioned document achieves high-precision positioning within 0.5mm through conical positioning pins and positioning bushings after the Mecanum wheel AGV scans the QR code for preliminary navigation. It cannot solve the technical problem of accurate docking of the AGV with the machine, and the existing method of realizing positioning with reflective columns does not increase the problem of strength judgment in the point cloud data, which easily leads to the inability to accurately determine the contour information of the point cloud data that does not meet the requirements. In the process of using AGV to transport materials in the workshop, the AGV is often required to determine the starting point of the driving path and then drive to the end of the machine along the path. Especially for long-distance straight paths, the AGV may have an angle deviation during the rotation process of confirming the starting point. For example, if the deviation begins to occur at the starting position, the deviation will occur during the subsequent movement, so it is impossible to achieve accurate docking with the machine to complete the material transportation. Summary of the invention

[0005] The object of the present invention is to provide a local navigation method based on reflector positioning, which can accurately form the reflector outline so that a mobile robot can accurately reach the positioning end point, and the method is simple and reliable.

[0006] To achieve the above object, the present invention provides a local navigation method based on reflector positioning, comprising the following steps: S1, control the mobile robot to enter the reflector scanning area, and then switch to the reflector positioning mode; S2, the mobile robot starts to match the reflector outline; S2.1, scan the point cloud data in the reflector area by the laser radar installed on the mobile robot, preset the point cloud intensity threshold T, weighting coefficients α, β, γ, S2.2, collect the scanned point cloud data, and then filter the point cloud data according to the preset intensity threshold T, S2.3, determine the error between the contour length of the screened point cloud data and the length of the reflector, determine the error between the center distance between the screened point cloud data and the reflector, determine the relative intensity error between the screened point cloud data and the reflector, and use the evaluation function cost in formula (1) to determine whether the reflector contour matches; cost=α* err_length+ β*err_dist+ γ*cost_intensity(1) S3, calculate the coordinates of the filtered point cloud data (lx i ,ly i ); S4. Obtain the pose matrix of the reflector in the laser radar coordinate system through the coordinates of the point cloud data , then according to the transformation matrix T of the laser radar in the odometer coordinate system b Get the pose matrix T of the reflector in the odometer coordinate system g , according to the transformation matrix T of the origin of the odometer coordinate system in the mobile robot coordinate system r Get 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 end point of the reflector.

[0007] In the above settings, the point cloud data is scanned by the laser radar in the scanning area, and the point cloud intensity threshold T is preset. This makes it easy to compare the preset point cloud intensity threshold T with the intensity of the scanned point cloud data, so as to screen out the point cloud data that matches the reflector, eliminate other interference factors, and improve accuracy; determine the length error, center spacing error, and relative intensity error between the screened point cloud data and the reflector, and then obtain the evaluation function cost and use the evaluation function cost to determine whether the reflector contour matches. Since the weighting coefficient is used in the evaluation function cost, the length error, center spacing error, and relative intensity error will affect the factors of whether the entire contour matches, so that the final determined contour can be closer to the length, center, and intensity, so that the mobile robot can complete the contour matching of the scanned reflector; then, by calculating the coordinates of the screened point cloud data (lx i ,ly i ) to obtain the pose matrix of the reflector in the laser radar coordinate system , and then according to the transformation matrix T of the laser radar in the odometer coordinate system b , so that the pose matrix T of the reflector in the odometer coordinate system can be obtained g , and the transformation matrix T of the origin of the odometer coordinate system in the mobile robot coordinate system r , so that the pose matrix T of the reflector in the mobile robot coordinate system can be obtained L ; Finally, the mobile robot can be controlled according to the pose matrix T of the reflector in the mobile robot coordinate system L Accurate positioning is achieved and the mobile robot can accurately navigate to the end point of the reflector, so that after the outline of the reflector is accurately determined, the mobile robot can be more accurately driven to the end point of the reflector for docking.

[0008] Furthermore, step S2.3 includes: determining the contour length error of the filtered point cloud data and the length of the reflector through the err_length function, determining the center distance error between the filtered point cloud data and the reflector through the err_dist function, and determining the relative intensity error between the filtered point cloud data and the reflector through the cost_intensity function.

[0009] The above settings can easily determine the corresponding parameter information according to the preset functions of the INS / GPS system.

[0010] Furthermore, the 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 judged as the point cloud data of the reflector; otherwise, it is judged as other interfering point cloud data.

[0011] The above settings can filter out the point cloud data that matches the reflector from all the scanned point cloud data.

[0012] Furthermore, the step S3 also includes the following steps: S3.1. Obtain the distance l between the laser radar and the reflector and the scanning angle z from the laser radar scan. S3.2, then use the distance l and the scanning angle z to calculate lx respectively i ,ly i , 1x i =l*cos(z)(2) ly i =l*sin(z)(3).

[0013] The above settings can calculate the coordinates of the filtered point cloud through the distance l between the laser radar and the reflector and the scanning angle z.

[0014] Furthermore, the S4 also includes: The pose matrix of the reflector in the LiDAR coordinate system as follows,

[0015] The pose matrix T of the reflector in the odometer coordinate system g as follows,

[0016] The pose matrix T of the reflector in the mobile robot coordinate system L as follows, T L =T g *T r (6).

[0017] The above settings can obtain the pose matrix of the reflector in the laser radar coordinate system through the coordinates of the point cloud data. , and then calculate the pose matrix T of the reflector in the odometer coordinate system g Finally, we get the pose matrix T of the reflector in the mobile robot coordinate system. L . BRIEF DESCRIPTION OF THE DRAWINGS

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

[0019] The present invention is further described in detail below with reference to the accompanying drawings and specific embodiments.

[0020] like Figure 1As shown, a local navigation method based on reflector positioning is used to control the robot to move on a path between the starting position of the reflector and the end position of the reflector. Reflectors are set on both sides of the walking path of the mobile robot, and a laser radar is set on the mobile robot. The laser radar sends lasers through the reflectors and then returns data to determine point cloud data. The specific implementation steps include the following: S1. Control the mobile robot to enter the reflector scanning area, and then switch to the reflector positioning mode.

[0021] S2, the mobile robot starts to match the reflector outline, S2.1. Scan the point cloud data in the reflector area by using the laser radar installed on the mobile robot, and preset the point cloud intensity threshold T, weighting coefficients α, β, γ; S2.2, collecting the scanned point cloud data, and then filtering 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 within 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 interfering point cloud data; S2.3. In the INS / GPC navigation system, the function is used to calculate the error between the contour length of the filtered point cloud data and the length of the reflector. Specifically, the err_length function is used to determine the contour length of the point cloud data through the positioning system and then calculate the length error with the length information of the preset reflector. The center distance error between the filtered point cloud data and the reflector is calculated through the err_dist function. The err_dist function is used to determine the error by the difference in the distance between the center of the point cloud data determined by the positioning system and the center of the preset reflector. The relative intensity error between the filtered point cloud data and the reflector is calculated through the cost_intensity function. The cost_intensity function determines the relative intensity error value by obtaining the intensity information of the point cloud data in the picture and the intensity information of the preset reflector. The evaluation function cost is used to determine whether the contour of the reflector matches. In this embodiment, the err_length function, the err_dist function, and the cost_intensity function are all existing functions and will not be repeated here. The formula of the evaluation function cost is as follows: cost=α* err_length+ β*err_dist+ γ*cost_intensity(1), If the reflector profile matches successfully, proceed to step S3; If the reflector profile is not matched successfully, the process returns to step S2.

[0022] S3, calculate the coordinates of the filtered point cloud data (lxi ,ly i ), in this embodiment, further comprising steps S3.1-S3.2: S3.1, obtaining the distance l between the laser radar and the reflector and the scanning angle z in the laser radar scanning of step S2.1, S3.2, then use the distance l between the laser radar and the reflector and the scanning angle z to calculate lx i ,ly i , 1x i The calculation formula is as follows: 1x i =l*cos(z)(2), ly i The calculation formula is as follows: ly i =l*sin(z)(3).

[0023] S4, then through ly i / lx i Determine the body angle, and then obtain the pose matrix of the reflector in the LiDAR coordinate system through the coordinates of the point cloud data and the body angle value The pose matrix includes the horizontal and vertical coordinates of the two-dimensional space and the body angle information. In this embodiment, the pose matrix of the reflector in the laser radar coordinate system is The calculation formula is as follows. Since the mobile robot only moves in the plane, and the plane is the surface where the XY axis is located, and the mobile robot will not climb a slope; , Then, according to the transformation matrix T of the laser radar in the odometer coordinate system b Get the pose matrix T of the reflector in the odometer coordinate system g In this embodiment, the pose matrix T of the reflector in the odometer coordinate system is g The calculation formula is as follows, T g =T b * (5), According to the transformation matrix T of the origin of the odometer coordinate system in the mobile robot coordinate system r Get the pose matrix T of the reflector in the mobile robot coordinate system L In this embodiment, the pose matrix T of the reflector in the mobile robot coordinate system is L The calculation formula is as follows, T L =T g *T r (6).

[0024] S5, control the mobile robot according to the pose matrix T of the reflector in the mobile robot coordinate system L Navigate to the end point of the reflector.

[0025] The working principle of the present invention is as follows: the mobile robot is controlled to enter the reflector area, and then switched to the reflector positioning mode to scan the reflector, and then the point cloud data obtained by the scan is filtered according to the preset intensity threshold T, and the filtered point cloud data is judged as the point cloud data of the reflector, and then the contour length of the filtered point cloud data and the length of the reflector are calculated by the err_length function, the center distance error between the filtered point cloud data and the reflector is calculated by the err_dist function, and the relative intensity error between the filtered point cloud data and the reflector is calculated by the cost_intensity function. After the reflector contour is judged to be successfully matched by the evaluation function cost, the coordinates of the filtered point cloud data (lx i ,ly i ) and obtain the pose matrix of the reflector in the laser radar coordinate system , and then according to the transformation matrix T of the laser radar in the odometer coordinate system b Get the pose matrix T of the reflector in the odometer coordinate system g Finally, according to the transformation matrix T of the origin of the odometer coordinate system in the mobile robot coordinate system r Get the pose matrix T of the reflector in the mobile robot coordinate system L , so that the mobile robot can calculate the pose matrix T of the reflector in the mobile robot coordinate system L Navigate to the end point of the reflector.

Claims

1. A local navigation method based on reflector positioning, characterized in that: The following steps are involved: S1, control the mobile robot to enter the reflector scanning area, and then switch to the reflector positioning mode; S2, the mobile robot starts to match the reflector outline; S2.1, scan the point cloud data in the reflector area by the laser radar installed on the mobile robot, preset the point cloud intensity threshold T, weighting coefficients α, β, γ, S2.2, collect the scanned point cloud data, and then filter the point cloud data according to the preset intensity threshold T, S2.

3. Determine the error between the contour length of the screened point cloud data and the length of the reflector, determine the error between the center distance of the screened point cloud data and the reflector, determine the relative intensity error between the screened point cloud data and the reflector, and use the evaluation function cost in formula (1) to determine whether the reflector contour matches; cost = α* err_length + β*err_dist + γ*cost_intensity (1) S3, calculate the coordinates of the filtered point cloud data (lx i ,ly i ); S4. Obtain the pose matrix of the reflector in the laser radar coordinate system through the coordinates of the point cloud data , then according to the transformation matrix T of the laser radar in the odometer coordinate system b Get the pose matrix T of the reflector in the odometer coordinate system g , according to the transformation matrix T of the origin of the odometer coordinate system in the mobile robot coordinate system r Get 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 end point of the reflector.

2. The local navigation method based on reflector positioning according to claim 1, characterized in that: Step S2.3 includes: determining the contour length error of the filtered point cloud data and the length of the reflector through the err_length function, determining the center distance error between the filtered point cloud data and the reflector through the err_dist function, and determining the relative intensity error between the filtered point cloud data and the reflector through the cost_intensity function.

3. The local navigation method based on reflector positioning according to claim 1, characterized in that: The 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 judged as the point cloud data of the reflector; otherwise, it is judged as other interfering point cloud data.

4. The local navigation method based on reflector positioning according to claim 1, characterized in that: The step S3 also includes the following steps: S3.

1. Obtain the distance l between the laser radar and the reflector and the scanning angle z from the laser radar scan. S3.2, then use the distance l and the scanning angle z to calculate lx respectively i ,ly i , lx i =l*cos(z)(2) ly i =l*sin(z)(3)。 5. The local navigation method based on reflector positioning according to claim 1, characterized in that: The 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