Butt joint method based on multimode collaborative navigation

Abstract

The invention provides a multi-mode collaborative navigation-based docking method, which is characterized in that a two-dimensional code is arranged at the bottom of a machine table, reflectors are arranged on the two sides of the front of the machine table, a mobile trolley is positioned by scanning the reflectors, and the mobile trolley runs into the bottom of the machine table from the middle positions of the reflectors to scan the code to realize docking, and collaborative navigation positioning is realized through the reflectors and the two-dimensional code. The moving trolley can accurately run to the position below the machine table to achieve accurate butt joint, so that loading, unloading and carrying of materials are completed, and the method is reliable.

Description

Technical Field

[0001] The present invention relates to the technical field of small vehicle navigation, and in particular to a method for docking based on multi-mode collaborative navigation. Background Art

[0002] In recent years, with the continuous improvement of production technology, most modern intelligent manufacturing workshops are equipped with intelligent mobile carts. Intelligent mobile carts connect with other logistics equipment through navigation, thereby realizing the full process automation of material loading, unloading and handling. In this process, compared with the traditional rail navigation method, the positioning and navigation technology based on lidar is the key technology for intelligent mobile carts to navigate and complete docking in indoor workshops. The navigation method based on lidar has high accuracy and good stability.

[0003] For example, the Chinese patent application number is 201710571639.9, and the publication date is September 8, 2017. It discloses a laser positioning and navigation method based on double reflectors. The minimum requirement of the method is to detect two reflectors, but in actual application, multiple reflectors may be detected at the same time. By designing data fusion processing, the complex domain information of the reflectors can be maximized, thereby further improving the accuracy.

[0004] In the process of using a mobile cart to transport materials in a workshop, the mobile cart is often required to accurately drive to the bottom of the machine to achieve docking in order to complete the loading and unloading and transportation of materials. However, the double reflective plates in the above-mentioned documents require the mobile robot to obtain the light reflected on the reflective plates to determine the outline, but the confirmation effect of the reflective plates in hidden positions such as the bottom of the machine is relatively poor, so that the mobile cart cannot be driven to the precise position under the machine, and the mobile cart cannot be accurately docked with the machine to complete the loading and unloading and transportation of materials. Summary of the invention

[0005] The purpose of the present invention is to provide a method for docking based on multi-mode collaborative navigation, which facilitates positioning in various scenes through collaborative navigation and positioning of reflective plates and QR codes, and enables the mobile vehicle to accurately drive to the bottom of the machine to achieve precise docking, thereby completing the loading, unloading and transportation of materials, and the method is reliable.

[0006] To achieve the above-mentioned purpose, the present invention provides a docking method based on multi-mode collaborative navigation, wherein a two-dimensional code and a preset two-dimensional code path are provided at the bottom of the machine, and reflectors are provided on both sides in front of the machine. The mobile vehicle realizes positioning by scanning the reflectors, and drives from the middle position of the reflectors into the bottom of the machine to scan the code for docking. The following steps are involved: S1, control the mobile car to enter the reflector scanning area in front of the machine and switch to the reflector positioning mode, and then start matching the reflector contour; S1.1, Scan the point cloud data in the reflector area by the laser radar installed on the mobile vehicle, preset the point cloud intensity threshold T, weighting coefficients α, β, γ, S1.2, collect the scanned point cloud data, and then filter the point cloud data according to the preset intensity threshold T, S1.3, determine the error between the contour length of the screened point cloud data and the length of the reflector, determine the center distance error 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 of formula (1) to determine whether the reflector contour matches; cost=α* err_length+ β*err_dist+ γ*cost_intensity (1) S2. 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 car coordinate system r Get the pose matrix T of the reflector in the mobile car coordinate system L ; S3, according to the pose matrix T of the reflector in the coordinate system of the mobile car L Determine whether the mobile car is on the straight path where the center position between the reflective plates is located, and then control the mobile car to move along the straight path where the center position between the reflective plates is located to enter the bottom of the machine; S4, the mobile trolley can successfully scan the QR code within the maximum deviation range [-x, x], the corresponding adjustment angle of the mobile trolley at the maximum deviation is a, and the docking error range of the mobile trolley; S5. After the mobile car enters the scanning area at the bottom of the machine, the camera installed on the mobile car scans the QR code and identifies the QR code information. Then, a rectangular coordinate system is established with the center point of the QR code as the origin of the coordinates. Then, it is determined whether there is a deviation error between the actual position of the mobile car and the center of the QR code. The mobile car is rotated in the direction of the preset QR code path and then returned to the preset QR code path for correction. S6. After correcting the deviation error between the actual position of the mobile cart and the center point of the QR code, the mobile cart is moved to the end position, and it is determined whether the mobile cart is within the docking error range, so that the mobile cart and the machine are docked.

[0007] In the above settings, the laser radar is used to scan the point cloud data in the scanning area in front of the machine, 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; by calculating the weighted coefficient 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, and then the evaluation function cost can be obtained and the evaluation function cost can be used to determine whether the reflector contour matches, so that the mobile car 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 car coordinate system r , so that the pose matrix T of the reflector in the mobile car coordinate system can be obtained L , so that the reflector can be used in the mobile car coordinate system through the pose matrix T L It is determined whether the mobile car is on the straight path where the center position between the reflective plates is located, and it is ensured that the mobile car can drive along the straight path where the center position between the reflective plates is located to enter the code scanning area at the bottom of the machine to scan the code; by presetting the mobile car to successfully scan the QR code within the maximum deviation range [-x, x], and setting the adjustment angle corresponding to the maximum deviation of the mobile car to a, when it is determined that there is a position deviation between the mobile car and the center point of the QR code, the actual deviation between the mobile car and the center point of the QR code can be calculated and corrected, so that the mobile car can accurately drive to the center point of the QR code, thereby ensuring that the center of the mobile car is located at the starting point of the preset QR code path, and then return to the preset QR code path after adjusting the deviation value to drive to the end point, and then judge the end point position, so that the mobile car can complete the docking with the machine within the docking error range, realize the loading and unloading and handling of materials, and make full use of the advantages of QR code navigation and reflector navigation. The navigation at the bottom of the machine is carried out in the form of QR code, and the reflector is used for navigation before entering the machine, so as to ensure the accuracy of navigation.

[0008] Furthermore, S1.3 includes: calculating the error between the contour length of the filtered point cloud data and the length of the reflector through the err_length function, calculating the center distance error between the filtered point cloud data and the reflector through the err_dist function, calculating the relative intensity error between the filtered point cloud data and the reflector through the cost_intensity function, and using the evaluation function cost to determine whether the contour of the reflector matches.

[0009] The above settings calculate the length error, center distance error, and relative intensity error between the filtered point cloud data and the reflector through the err_length function, err_dist function, and cost_intensity function, respectively, which can easily determine the corresponding parameter information according to the preset function of the INS / GPS system.

[0010] Furthermore, the step S1.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 S2 also includes calculating the coordinates of the filtered point cloud data (lx i ,ly i ), the specific steps are as follows: S2.1. Obtain the distance l between the laser radar and the reflector and the scanning angle z from the laser radar scan. S2.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), S2.3. Calculate the pose matrix of the reflector in the laser radar coordinate system as follows, (4), Calculate the pose matrix T of the reflector in the odometer coordinate system g as follows, (5), Calculate the pose matrix T of the reflector in the coordinate system of the mobile car L as follows, TL =T g *T r (6).

[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 respectively; the pose matrix of the reflector in the laser radar coordinate system can be obtained 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 coordinate system of the mobile car. L .

[0014] Furthermore, the step S3 also includes steps S3.1-S3.2: S3.1. If the mobile car deviates from the straight path where the center position between the reflectors is located, then according to the position matrix T of the reflectors in the mobile car coordinate system L Calculate the deviation error, and then control the mobile car to return to the straight path. S3.1.1, take the straight path as the y-axis and the horizontal line where the mobile car is located as the x-axis, and then establish a rectangular coordinate system. S3.1.2, according to the pose matrix T of the reflector in the coordinate system of the mobile car L Determine the distance y0 that the mobile car deviates from the y-axis, then select the deviation angle b0, and use the geometric relationship to calculate the distance S that the mobile car travels to the y-axis, as follows: (7), S3.1.3, control the mobile car to travel at a speed v0 along the deviation angle b0 and stop at a distance S, then control the mobile car to rotate (90- b0)° in the direction of the straight path, so that the mobile car returns to the straight path after correcting the deviation error; S3.2. If the moving car does not deviate from the straight path where the center position between the reflective plates is located, the moving car is controlled to move along the straight path where the center position between the reflective plates is located and enter the bottom of the machine.

[0015] The above arrangement can correct the deviation error of the straight path where the center position between the moving carriage and the reflective plate is located after the moving carriage matches the reflective plate contour, so that the moving carriage can accurately move along the straight path to enter the bottom of the machine.

[0016] Furthermore, the step S5 also includes steps S5.1-S5.2: S5.1. Calculate the deviation between the horizontal coordinate of the actual position coordinate of the mobile car and the center point of the QR code, and then determine the positional relationship between the mobile car and the center point of the QR code. S5.1.1. If the deviation error x0 between the horizontal coordinate of the actual position coordinate of the mobile car and the coordinate of the center point of the QR code is 0, there is no deviation error between the actual position of the mobile car and the center point of the QR code, and the process goes to step S6. S5.1.2. If the deviation error x0 between the horizontal coordinate of the actual position coordinate of the mobile car and the horizontal coordinate of the center point of the QR code is within the range of [-x, 0), it is determined that the mobile car deviates to the left of the starting QR code, and the process goes to step S5.2. S5.1.3. If the deviation error x0 between the horizontal coordinate of the actual position coordinate of the mobile car and the horizontal coordinate of the center point of the QR code is within the range of (0, x], it is determined that the mobile car deviates to the right of the starting QR code, and the process goes to step S5.2. S5.2. Correct the deviation between the actual position coordinates of the mobile car and the coordinates of the midpoint of the QR code. S5.2.1. Calculate the angle a0 of the mobile car from the preset QR code center point through geometric relationship and using the following formula (5.1). (5.1); S5.2.2. By using the relationship and the following formula (5.2), calculate the distance s from the mobile car to the center of the QR code along the deviation angle a0. (5.2), S5.2.3. After controlling the mobile car to adjust the deviation angle a0, it travels a distance s along the deviation angle a0 at a speed v and stops, and then controls the mobile car to rotate (90-a0)° along the preset QR code path direction, so that the mobile car returns to the center point of the QR code after correcting the deviation, and enters step S6.

[0017] The above setting can calculate the deviation error between the mobile car and the center point of the QR code through the positional relationship between the two, and then enable the mobile car to correct the deviation error and return to the center point of the QR code to achieve docking.

[0018] Furthermore, the step S6 also includes the following contents: If the mobile trolley is within the docking error range, the mobile trolley completes the docking, thereby realizing the loading, unloading and transportation of materials; otherwise, the process proceeds to step S5 for re-docking.

[0019] The above arrangement enables the mobile trolley to complete docking within the docking error range, thereby realizing the loading, unloading and transportation of materials. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0021] Figure 2It is a schematic diagram of the mobile vehicle adjusting the path on a straight path of the present invention.

[0022] Figure 3 It is a schematic diagram of the mobile vehicle of the present invention adjusting the path in the center of the QR code. DETAILED DESCRIPTION

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

[0024] like Figure 1 As shown, a method for docking based on multi-mode collaborative navigation is provided. A QR code is provided at the bottom of the machine, and reflectors are provided on both sides in front of the machine. The mobile vehicle realizes positioning by scanning the reflectors, and drives from the middle position of the reflectors to the bottom of the machine to scan the code for docking. The specific implementation examples and steps are as follows: S1, control the mobile car to enter the reflector scanning area in front of the machine and switch to the reflector positioning mode, and then start matching the reflector contour. S1.1, Scan the point cloud data in the reflector area by the laser radar installed on the mobile vehicle, preset the point cloud intensity threshold T, weighting coefficients α, β, γ, S1.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; S1.3. In the INS / GPC navigation system, the function is used to calculate the contour length error of the filtered point cloud data and the length of the reflector. Specifically, the err_length function is 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 by the err_dist function. The err_dist function is to determine the error by the difference 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 by 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 reflector contour 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 S2; If the reflector profile is not matched successfully, the process returns to step S1.

[0025] S2. 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 car coordinate system r Get the pose matrix T of the reflector in the mobile car coordinate system L , S2.1, obtaining the distance l between the laser radar and the reflector and the scanning angle z in the laser radar scanning of step S1.1; S2.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); S2.3. Calculate the pose matrix of the reflector in the laser radar coordinate system as follows, (4), Calculate the pose matrix T of the reflector in the odometer coordinate system g as follows, (5), Calculate the pose matrix T of the reflector in the coordinate system of the mobile car L as follows, T L =T g *T r (6).

[0026] S3, according to the pose matrix T of the reflector in the coordinate system of the mobile car L Determine whether the mobile car is on the straight path where the center position between the reflectors is located. S3.1. If the mobile car deviates from the straight path where the center position between the reflectors is located, then according to the position matrix T of the reflectors in the mobile car coordinate system L Calculate the deviation error, and then control the mobile car to return to the straight path. S3.1.1. Take the straight path as the y-axis and the horizontal line where the mobile car is located as the x-axis, and then establish a rectangular coordinate system; S3.1.2, according to the pose matrix T of the reflector in the coordinate system of the mobile car L Determine the distance y0 that the mobile car deviates from the y-axis, and then select the deviation angle b0. In this embodiment, the deviation angle is the posture angle, which can be obtained by the posture sensor. Formula (7) is used to calculate the distance S that the mobile car travels to the y-axis. In this embodiment, the deviation angle b0 is set to 20°, as follows: (7); S3.1.3, control the mobile car to travel a distance S at a speed v0 along the deviation angle b0 and stop, and then control the mobile car to rotate (90-b0)° in the direction of the straight path, which is counterclockwise in this embodiment, so that the mobile car returns to the straight path after correcting the deviation error; Figure 2 As shown, the straight path is 1, the center of the QR code is 2, and the center 3 of the mobile car deviates from the straight path 1 by a length S and a deviation angle b0.

[0027] S3.2. If the moving car does not deviate from the straight path where the center position between the reflective plates is located, the moving car is controlled to move along the straight path where the center position between the reflective plates is located and enter the bottom of the machine.

[0028] S4. It is preset that the mobile cart can successfully scan the QR code within the maximum deviation range [-x, x]. The corresponding adjustment angle of the mobile cart at the maximum deviation is a. The docking error range of the mobile cart. In this embodiment, x is set to 300 mm and a is set to 30°.

[0029] S5. After the mobile car enters the scanning area at the bottom of the machine, the camera installed on the mobile car scans the QR code and identifies the QR code information. Then, a rectangular coordinate system is established with the center point of the QR code as the origin of the coordinates. Then, it is determined whether there is a deviation error between the actual position of the mobile car and the center of the QR code. S5.1. Calculate the deviation between the horizontal coordinate of the actual position coordinate of the mobile car and the center point of the QR code, and then determine the positional relationship between the mobile car and the center point of the QR code. S5.1.1. If the deviation error x0 between the horizontal coordinate of the actual position coordinate of the mobile car and the coordinate of the center point of the QR code is 0, there is no deviation error between the actual position of the mobile car and the center point of the QR code, and the process goes to step S6; S5.1.2. If the deviation error x0 between the horizontal coordinate of the actual position coordinate of the moving car and the horizontal coordinate of the center point of the QR code is within the range of [-x, 0), it is determined that the moving car deviates to the left of the starting QR code, and the process goes to step S5.2; S5.1.3. If the deviation error x0 between the horizontal coordinate of the actual position coordinate of the moving car and the horizontal coordinate of the center point of the QR code is within the range of (0, x], it is determined that the moving car deviates to the right side of the starting QR code, and the process goes to step S5.2; S5.2. Correct the deviation between the actual position coordinates of the mobile car and the coordinates of the midpoint of the QR code. S5.2.1. Calculate the angle a0 of the mobile car from the preset QR code center point through geometric relationship and using the following formula (5.1). (5.1); S5.2.2. By using the relationship and the following formula (5.2), calculate the distance s from the mobile car to the center of the QR code along the deviation angle a0. (5.2); S5.2.3, after controlling the mobile car to adjust the deviation angle a0, it travels along the deviation angle a0 at a speed v and stops after a distance s, and then controls the mobile car to rotate (90-a0)° along the preset QR code path direction. In this embodiment, it is counterclockwise, such as Figure 3 As shown, the starting point 4 and the end point 5 of the preset QR code path, the center of the mobile car 3 is compared with the starting point 4 of the preset QR code path to determine whether there is a deviation, so that the mobile car corrects the deviation and returns to the center point of the QR code, and enters step S6.

[0030] S6. After correcting the deviation error between the actual position of the mobile cart and the center point of the QR code, determine whether the mobile cart is within the docking error range. In this embodiment, the docking error range is 5 mm. If the mobile cart is within the docking error range, the mobile cart and the machine are docked to realize the loading, unloading and transportation of materials; otherwise, enter step S5 to re-dock.

[0031] The working principle of the present invention is as follows: the mobile vehicle is controlled to enter the reflector area in front of the machine, and then the reflector positioning mode is switched 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 error of the reflector are calculated by the err_length function, the center point error of the filtered point cloud data and the reflector are calculated by the err_dist function, and the relative intensity error of 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 , according to the transformation matrix T of the origin of the odometer coordinate system in the mobile car coordinate system r Get the pose matrix T of the reflector in the mobile car coordinate system L , and at the same time, the pose matrix T of the reflector in the coordinate system of the mobile car L It is judged whether the mobile cart is on the straight path where the center position between the reflective plates is located, so that the mobile cart can correct the deviation error from the straight path where the center position between the reflective plates is located, so that the mobile cart can accurately drive along the straight path to enter the bottom of the machine. After the mobile cart scans the code, the deviation between the mobile cart and the center point of the QR code is judged, and after the deviation is calculated, the deviation between the actual position of the mobile cart and the center point of the QR code is corrected, so that the mobile cart can complete the docking within the docking error range of the QR code scanning area after the deviation is corrected, thereby realizing the loading and unloading and transportation of materials.

Claims

1. A docking method based on multi-mode collaborative navigation, wherein a QR code is provided at the bottom of the machine, and reflectors are provided on both sides in front of the machine. The mobile vehicle realizes positioning by scanning the reflectors, and drives from the middle position of the reflectors into the bottom of the machine to scan the code for docking, characterized in that: The following steps are involved: S1, control the mobile car to enter the reflector scanning area in front of the machine and switch to the reflector positioning mode, and then start matching the reflector contour; S1.

1. Scan the point cloud data in the reflector area by using the laser radar installed on the mobile vehicle, preset the point cloud intensity threshold T, weighting coefficients α, β, γ, S1.

2. Collect the scanned point cloud data, and then filter the point cloud data according to the preset intensity threshold T. S1.

3. Determine the error between the contour length of the filtered point cloud data and the length of the reflector, determine the center distance error between the filtered point cloud data and the reflector, determine the relative intensity error between the filtered point cloud data and the reflector, and use the evaluation function cost of formula (1) to determine whether the reflector contour matches; cost = α* err_length + β*err_dist + γ*cost_intensity (1) S2. 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 car coordinate system r Get the pose matrix T of the reflector in the mobile car coordinate system L ; S3, according to the pose matrix T of the reflector in the coordinate system of the mobile car L Determine whether the mobile car is on the straight path where the center position between the reflective plates is located, and then control the mobile car to move along the straight path where the center position between the reflective plates is located to enter the bottom of the machine; S4, the mobile trolley can successfully scan the QR code within the maximum deviation range [-x, x], the corresponding adjustment angle of the mobile trolley at the maximum deviation is a, and the docking error range of the mobile trolley; S5. After the mobile car enters the scanning area at the bottom of the machine, the camera installed on the mobile car scans the QR code and identifies the QR code information. Then, a rectangular coordinate system is established with the center point of the QR code as the origin of the coordinates. Then, it is determined whether there is a deviation error between the actual position of the mobile car and the center of the QR code. The mobile car is rotated in the direction of the preset QR code path and then returned to the preset QR code path for correction. S6. After correcting the deviation error between the actual position of the mobile cart and the center point of the QR code, the mobile cart is moved to the end position, and it is determined whether the mobile cart is within the docking error range, so that the mobile cart and the machine are docked.

2. The method for docking based on multi-mode collaborative navigation according to claim 1, characterized in that: S1.3 includes: calculating the error between the contour length of the filtered point cloud data and the length of the reflector through the err_length function, calculating the center distance error between the filtered point cloud data and the reflector through the err_dist function, calculating the relative intensity error between the filtered point cloud data and the reflector through the cost_intensity function, and using the evaluation function cost to determine whether the reflector contour matches.

3. The method for docking based on multi-mode collaborative navigation according to claim 1, characterized in that: The step S1.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 method for docking based on multi-mode collaborative navigation according to claim 1, characterized in that: The step S2 also includes calculating the coordinates of the filtered point cloud data (lx i ,ly i ), the specific steps are as follows: S2.

1. Obtain the distance l between the laser radar and the reflector and the scanning angle z from the laser radar scan. S2.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), S2.

3. Calculate the pose matrix of the reflector in the laser radar coordinate system as follows, （4）， Calculate the pose matrix T of the reflector in the odometer coordinate system g as follows, （5）， Calculate the pose matrix T of the reflector in the coordinate system of the mobile car L as follows, T L =T g *T r (6)。 5. The method for docking based on multi-mode collaborative navigation according to claim 1, characterized in that: The step S3 further comprises steps S3.1-S3.2: S3.

1. If the mobile car deviates from the straight path where the center position between the reflectors is located, then according to the position matrix T of the reflectors in the mobile car coordinate system L Calculate the deviation error, and then control the mobile car to return to the straight path. S3.1.1, take the straight path as the y-axis and the horizontal line where the mobile car is located as the x-axis, and then establish a rectangular coordinate system. S3.1.2, according to the pose matrix T of the reflector in the coordinate system of the mobile car L Determine the distance y0 that the mobile car deviates from the y-axis, then select the deviation angle b0, and use the geometric relationship to calculate the distance S that the mobile car travels to the y-axis, as follows: （7）， S3.1.3, control the mobile car to travel at a speed v0 along the deviation angle b0 and stop at a distance S, then control the mobile car to rotate (90- b0)° in the direction of the straight path, so that the mobile car returns to the straight path after correcting the deviation error; S3.

2. If the moving car does not deviate from the straight path where the center position between the reflective plates is located, the moving car is controlled to move along the straight path where the center position between the reflective plates is located and enter the bottom of the machine.

6. The method for docking based on multi-mode collaborative navigation according to claim 1, characterized in that: The step S5 also includes steps S5.1-S5.2: S5.

1. Calculate the deviation between the horizontal coordinate of the actual position coordinate of the mobile car and the center point of the QR code, and then determine the positional relationship between the mobile car and the center point of the QR code. S5.1.

1. If the deviation error x0 between the horizontal coordinate of the actual position coordinate of the mobile car and the coordinate of the center point of the QR code is 0, there is no deviation error between the actual position of the mobile car and the center point of the QR code, and the process goes to step S6. S5.1.

2. If the deviation error x0 between the horizontal coordinate of the actual position coordinate of the mobile car and the horizontal coordinate of the center point of the QR code is within the range of [-x, 0), it is determined that the mobile car deviates to the left of the starting QR code, and the process goes to step S5.

2. S5.1.

3. If the deviation error x0 between the horizontal coordinate of the actual position coordinate of the mobile car and the horizontal coordinate of the center point of the QR code is within the range of (0, x], it is determined that the mobile car deviates to the right of the starting QR code, and the process goes to step S5.

2. S5.

2. Correct the deviation between the actual position coordinates of the mobile car and the coordinates of the midpoint of the QR code. S5.2.

1. Calculate the angle a0 of the mobile car from the preset QR code center point through geometric relationship and using the following formula (5.1). （5.1）； S5.2.

2. By using the relationship and the following formula (5.2), calculate the distance s from the mobile car to the center of the QR code along the deviation angle a0. （5.2）， S5.2.

3. After controlling the mobile car to adjust the deviation angle a0, it travels a distance s along the deviation angle a0 at a speed v and stops, and then controls the mobile car to rotate (90-a0)° along the preset QR code path direction, so that the mobile car returns to the center point of the QR code after correcting the deviation, and enters step S6.

7. The method for docking based on multi-mode collaborative navigation according to claim 1, characterized in that: In step S6, Includes the following: If the mobile trolley is within the docking error range, the mobile trolley completes the docking, thereby realizing the loading, unloading and transportation of materials; Otherwise, proceed to step S5 to re-docking.

Images