Multi-mode cooperative navigation docking method

Abstract

The application provides a multi-mode cooperative navigation docking method. A two-dimensional code is arranged at the bottom of a machine table, and a reflective plate is arranged at the front of the machine table. A mobile trolley is positioned by scanning the reflective plate, and then drives into the bottom of the machine table from the middle position of the reflective plate to scan the code to realize docking. The mobile trolley can accurately drive to the bottom of the machine table to realize precise docking through the cooperative navigation positioning of the reflective plate and the two-dimensional code, so as to complete the loading, unloading and carrying of materials, and the method is reliable.

Description

Technical Field

[0001] This invention relates to the field of vehicle navigation technology, specifically to a multi-mode collaborative navigation docking method. Background Technology

[0002] In recent years, with the continuous improvement of production technology, most modern intelligent manufacturing workshops are equipped with intelligent mobile vehicles. These intelligent mobile vehicles connect with other logistics equipment through navigation, thereby realizing the full automation of the loading, unloading and handling of materials. 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 vehicles to navigate and complete docking in indoor workshops. The navigation method based on LiDAR has high accuracy and good stability.

[0003] For example, Chinese patent application No. 201710571639.9, published on September 8, 2017, discloses a laser positioning and navigation method based on dual reflectors. The minimum requirement of the method is to detect two reflectors, but in actual applications, multiple reflectors may be detected simultaneously. 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 mobile carts to transport materials in the workshop, the carts often need to accurately travel under the machine to dock with it in order to complete the loading, unloading and transportation of materials. However, the double reflectors mentioned in the above literature require the mobile robot to obtain the light reflected from the reflectors to determine the outline. However, the confirmation effect of reflectors in hidden positions such as the bottom of the machine is relatively poor, which makes it impossible for the mobile cart to travel to the precise position under the machine. Therefore, it is impossible to achieve the problem of accurate docking between the mobile cart and the machine to complete the loading, unloading and transportation of materials. Summary of the Invention

[0005] The purpose of this invention is to provide a multi-mode collaborative navigation docking method, which uses reflectors and QR codes for collaborative navigation and positioning, facilitating positioning in various scenarios and enabling mobile vehicles to accurately drive to the machine for precise docking, thereby completing the loading, unloading and handling of materials. The method is reliable.

[0006] To achieve the above objectives, this invention provides a multi-mode collaborative navigation docking method. A QR code and a preset QR code path are located at the bottom of the machine. Reflectors are located on both sides of the front of the machine. A mobile trolley achieves positioning by scanning the reflectors, and then drives into the bottom of the machine from the middle of the reflectors to scan the code and complete the docking.

[0007] Includes the following steps:

[0008] S1. Control the moving trolley to enter the reflector scanning area in front of the machine and switch to reflector positioning mode, and then start matching the reflector outline.

[0009] S1.1, The point cloud data within the reflector area is scanned by a lidar mounted on a mobile vehicle, with a preset point cloud intensity threshold T and weighting coefficients α, β, γ.

[0010] S1.2 Collect the point cloud data obtained from the scan, and then filter the point cloud data according to the preset intensity threshold T.

[0011] S1.3 Determine the error between the outline length of the filtered point cloud data and the length of the reflector, determine the error between the center distance 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 outline matches.

[0012] cost=α* err_length+ β*err_dist+ γ*cost_intensity (1)

[0013] S2. 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 odometer coordinate system origin in the moving trolley coordinate system r Obtain the pose matrix T of the reflector in the coordinate system of the moving trolley. L ;

[0014] S3. Based on the pose matrix T of the reflector in the coordinate system of the moving trolley L Determine whether the moving trolley is on the straight path where the center position between the reflectors is located, and then control the moving trolley to travel along the straight path where the center position between the reflectors is located and enter the bottom of the machine.

[0015] S4. The preset mobile trolley can successfully scan the QR code within the maximum deviation range [-x, x]. The adjustment angle corresponding to the maximum deviation of the mobile trolley is a. The docking error range of the mobile trolley.

[0016] S5. After the mobile cart enters the scanning area at the bottom of the machine, the camera on the mobile cart scans and recognizes the QR code information. Then, a Cartesian coordinate system is established with the center point of the QR code as the origin. Next, it is determined whether there is a deviation error between the actual position of the mobile cart and the center of the QR code. The mobile cart is then rotated along the preset QR code path and returned to the preset QR code path for correction.

[0017] S6. After correcting the deviation error between the actual position of the mobile trolley and the center point of the QR code, move the mobile trolley to the endpoint position, determine whether the mobile trolley is within the docking error range, and complete the docking between the mobile trolley and the machine.

[0018] The above settings involve scanning point cloud data in the scanning area in front of the machine using a LiDAR scanner, while simultaneously setting a preset point cloud intensity threshold T. This facilitates comparison between the preset threshold T and the intensity of the scanned point cloud data, thereby filtering out point cloud data that matches the reflector, eliminating other interference factors, and improving accuracy. By calculating the weighted coefficients used in the evaluation function cost, factors such as length error, center distance error, and relative intensity error all affect whether the entire contour matches. This ensures that the final determined contour is closer to the correct length, center, and intensity, thus enabling the evaluation function cost to be obtained and used to determine whether the reflector contour matches. This allows the moving carriage to complete the contour matching of the scanned reflector. Then, the coordinates (lx) of the filtered point cloud data are calculated. i ly i The pose matrix of the reflector in the lidar coordinate system is obtained. Then, based on the transformation matrix T of the lidar in the odometer coordinate system... b This allows us to obtain the pose matrix T of the reflector in the odometer coordinate system. g Simultaneously, the transformation matrix T of the odometer coordinate system origin in the moving trolley coordinate system... r Thus, the pose matrix T of the reflector in the coordinate system of the moving trolley can be obtained. L This allows us to visualize the pose matrix T of the reflector in the coordinate system of the moving vehicle. LThe system determines whether the mobile trolley is on the straight path centered between the reflectors, ensuring that the trolley can travel along this path into the scanning area at the bottom of the machine to scan the code. By pre-setting the trolley to successfully scan QR codes within the maximum deviation range [-x, x], and setting the adjustment angle 'a' corresponding to the maximum deviation, the system can calculate and correct the actual deviation when a positional deviation is detected between the trolley and the QR code's center point. This allows the trolley to accurately reach the QR code's center point, ensuring its center is at the starting point of the pre-set QR code path. After adjusting the deviation, the trolley returns to the pre-set path and travels to the endpoint. The endpoint position is then assessed, ensuring the trolley docks with the machine within the docking error range, facilitating material loading, unloading, and handling. This system fully utilizes the advantages of QR code navigation and reflector navigation, using QR codes for navigation at the bottom of the machine and reflectors for navigation before entering the machine, thus ensuring navigation accuracy.

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

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

[0021] Furthermore, step S1.2 also includes:

[0022] 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.

[0023] The above settings allow you to filter out point cloud data that matches the reflector from all the point cloud data obtained from the scan.

[0024] Furthermore, step S2 also includes calculating the coordinates (lx) of the filtered point cloud data. i ly i The specific steps are as follows:

[0025] S2.1 Obtain the distance l between the lidar and the reflector, and the scanning angle z, from the lidar scan.

[0026] S2.2 Then, use the distance l and the scanning angle z to calculate lx respectively. i ly i ,

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

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

[0029] S2.3 Calculate the pose matrix of the reflector in the lidar coordinate system. as follows,

[0030] (4),

[0031] Calculate the pose matrix T of the reflector in the odometer coordinate system. g as follows,

[0032] (5),

[0033] Calculate the pose matrix T of the reflector in the coordinate system of the moving vehicle. L as follows,

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

[0035] The above settings allow for the calculation of the filtered point cloud coordinates using the distance *l* between the lidar and the reflector and the scanning angle *z*; and enable the determination of the reflector's pose matrix in the lidar coordinate system using the point cloud data coordinates. Then, the pose matrix T of the reflector in the odometer coordinate system is calculated. g Finally, the pose matrix T of the reflector in the coordinate system of the moving trolley is obtained. L .

[0036] Furthermore, step S3 also includes steps S3.1-S3.2:

[0037] S3.1 If the moving trolley deviates from the straight path where the center position of the reflectors is located, then according to the pose matrix T of the reflectors in the coordinate system of the moving trolley... L Calculate the deviation error, and then control the moving trolley to return to the straight path.

[0038] S3.1.1. Establish a rectangular coordinate system with the straight path as the y-axis and the horizontal line where the moving car is located as the x-axis.

[0039] S3.1.2, Based on the pose matrix T of the reflector in the coordinate system of the moving trolley. L Determine the distance y0 that the moving car deviates from the y-axis, then select the deviation angle b0, and calculate the distance S that the moving car has traveled to the y-axis using geometric relationships, as follows:

[0040] (7),

[0041] S3.1.3 Control the moving trolley to travel a distance S along the deviation angle b0 at a speed v0 and then stop. Then control the moving trolley to rotate (90-b0)° in the direction of the straight path so that the moving trolley returns to the straight path after correcting the deviation error.

[0042] S3.2 If the moving trolley does not deviate from the straight path where the center position between the reflectors is located, then control the moving trolley to travel along the straight path where the center position between the reflectors is located and enter the bottom of the machine.

[0043] The above settings can correct the deviation error of the straight path between the center position of the moving trolley and the reflector after matching the reflector outline, so that the moving trolley can accurately travel along the straight path into the bottom of the machine.

[0044] Furthermore, step S5 also includes steps S5.1-S5.2:

[0045] S5.1 Calculate the deviation error between the x-coordinate of the actual position coordinates of the moving cart and the center point of the QR code, and then determine the positional relationship between the moving cart and the center point of the QR code.

[0046] S5.1.1 If the deviation error x0 between the x-coordinate of the actual position coordinates of the moving cart and the center point coordinates of the QR code is 0, then there is no deviation error between the actual position of the moving cart and the center point of the QR code. Proceed to step S6.

[0047] S5.1.2 If the deviation error x0 between the x-coordinate of the actual position coordinates of the moving cart and the x-coordinate of the center point of the QR code is within the range of [-x, 0), then it is determined that the moving cart is deviating to the left of the starting QR code, and proceed to step S5.2.

[0048] S5.1.3 If the deviation error x0 between the x-coordinate of the actual position coordinates of the moving cart and the x-coordinate of the center point of the QR code is within the range of (0, x], then it is determined that the moving cart is deviating to the right of the starting QR code, and proceed to step S5.2.

[0049] S5.2 Correct the deviation between the actual position coordinates of the moving cart and the coordinates of the midpoint of the QR code.

[0050] S5.2.1 Calculate the angle a0 of the moving trolley deviating from the preset QR code center point using geometric relationships and the following formula (5.1).

[0051] (5.1);

[0052] S5.2.2, Calculate the distance s from the moving cart to the center point of the QR code by using the following formula (5.2) and the relationship between them.

[0053] (5.2),

[0054] S5.2.3 After the control trolley adjusts the deviation angle a0, it travels a distance s along the deviation angle a0 at a speed v and then stops. Then, the control trolley rotates (90-a0)° along the preset QR code path direction so that the trolley corrects the deviation and returns to the center point of the QR code, and proceeds to step S6.

[0055] The above settings can calculate the deviation error between the mobile cart and the center point of the QR code by the positional relationship between the two, and then enable the mobile cart to correct the deviation error and return to the center point of the QR code to achieve docking.

[0056] Furthermore, step S6 also includes the following:

[0057] If the mobile trolley is within the docking error range, the mobile trolley completes the docking, thereby realizing the loading, unloading and handling of materials; otherwise, proceed to step S5 to re-dock.

[0058] The above settings enable the mobile trolley to complete docking within the docking error range, thereby realizing the loading, unloading and handling of materials. Attached Figure Description

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

[0060] Figure 2 This is a schematic diagram of the mobile vehicle of the present invention adjusting its path on a straight path.

[0061] Figure 3 This is a schematic diagram of the mobile vehicle of the present invention adjusting its path at the center of the QR code. Detailed Implementation

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

[0063] like Figure 1As shown, a multi-mode collaborative navigation docking method involves a QR code on the bottom of the machine and reflectors on both sides at the front of the machine. The mobile trolley achieves positioning by scanning the reflectors, and then drives into the bottom of the machine from the middle of the reflectors to scan the QR code and complete the docking.

[0064] The specific implementation methods and steps are as follows:

[0065] S1. Control the moving trolley to enter the reflector scanning area in front of the machine and switch to reflector positioning mode, then start matching the reflector contour.

[0066] S1.1, The point cloud data within the reflector area is scanned by a lidar mounted on a mobile vehicle, with a preset point cloud intensity threshold T and weighting coefficients α, β, γ.

[0067] S1.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.

[0068] S1.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, as determined by 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.

[0069] cost=α* err_length+ β*err_dist+ γ*cost_intensity(1),

[0070] If the reflector outline is successfully matched, proceed to step S2;

[0071] If the reflector outline does not match successfully, return to step S1.

[0072] S2. 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 odometer coordinate system origin in the moving trolley coordinate system r Obtain the pose matrix T of the reflector in the coordinate system of the moving trolley. L ,

[0073] S2.1 In the lidar scanning of step S1.1, the distance l between the lidar and the reflector and the scanning angle z are obtained;

[0074] S2.2 Then, use the distance l and the scanning angle z to calculate lx respectively. i ly i ,

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

[0076] ly i =l*sin(z)(3);

[0077] S2.3 Calculate the pose matrix of the reflector in the lidar coordinate system. as follows,

[0078] (4),

[0079] Calculate the pose matrix T of the reflector in the odometer coordinate system. g as follows,

[0080] (5),

[0081] Calculate the pose matrix T of the reflector in the coordinate system of the moving vehicle. L as follows,

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

[0083] S3. Based on the pose matrix T of the reflector in the coordinate system of the moving trolley L Determine whether the moving cart is on the straight path where the center position between the reflectors is located.

[0084] S3.1 If the moving trolley deviates from the straight path where the center position of the reflectors is located, then according to the pose matrix T of the reflectors in the coordinate system of the moving trolley... L Calculate the deviation error, and then control the moving trolley to return to the straight path.

[0085] S3.1.1 Establish a rectangular coordinate system with the straight path as the y-axis and the horizontal line where the moving car is located as the x-axis;

[0086] S3.1.2, Based on the pose matrix T of the reflector in the coordinate system of the moving trolley. L Determine the distance y0 of the moving trolley deviating from the y-axis, and then select the deviation angle b0. In this embodiment, the deviation angle is the pose angle, which can be obtained by the pose sensor. Calculate the distance S that the moving trolley travels to the y-axis using formula (7). In this embodiment, the deviation angle b0 is set to 20°, as follows:

[0087] (7);

[0088] S3.1.3. Control the moving trolley to travel a distance S along the deviation angle b0 at a speed v0 and then stop. Then control the moving trolley to rotate (90-b0)° in the direction of the straight path. In this embodiment, it is counterclockwise, so that the moving trolley 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 of the moving car 3 deviates from the straight path 1 by a length of S and an angle of b0.

[0089] S3.2 If the moving trolley does not deviate from the straight path where the center position between the reflectors is located, then control the moving trolley to travel along the straight path where the center position between the reflectors is located and enter the bottom of the machine.

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

[0091] S5. After the mobile cart enters the scanning area at the bottom of the machine, the camera on the mobile cart scans and recognizes the QR code information. Then, a Cartesian coordinate system is established with the center point of the QR code as the origin. Next, it is determined whether there is a deviation error between the actual position of the mobile cart and the center of the QR code.

[0092] S5.1 Calculate the deviation error between the x-coordinate of the actual position coordinates of the moving cart and the center point of the QR code, and then determine the positional relationship between the moving cart and the center point of the QR code.

[0093] S5.1.1 If the deviation error x0 between the horizontal coordinate of the actual position of the moving car and the coordinate of the center point of the QR code is 0, then there is no deviation error between the actual position of the moving car and the center point of the QR code, proceed to step S6.

[0094] 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), then it is determined that the moving car is deviated to the left of the starting point QR code, and proceed to step S5.2.

[0095] S5.1.3 If the deviation error x0 between the x-coordinate of the actual position coordinate of the moving car and the x-coordinate of the center point of the QR code is within the range of (0, x], then it is determined that the moving car is deviated to the right of the starting point QR code, and proceed to step S5.2.

[0096] S5.2 Correct the deviation between the actual position coordinates of the moving cart and the coordinates of the midpoint of the QR code.

[0097] S5.2.1 Calculate the angle a0 of the moving trolley deviating from the preset QR code center point using geometric relationships and the following formula (5.1).

[0098] (5.1);

[0099] S5.2.2, Calculate the distance s from the moving cart to the center point of the QR code by using the following formula (5.2) and the relationship between them.

[0100] (5.2);

[0101] S5.2.3 After adjusting the deviation angle a0, the mobile trolley travels a distance s along the deviation angle a0 at a speed v and then stops. Then, the mobile trolley rotates (90-a0)° along the preset QR code path direction. In this embodiment, this is a counter-clockwise direction. Figure 3 As shown, the starting point 4 and ending point 5 of the preset QR code path are compared with the center of the mobile car 3 to determine whether there is a deviation. After the mobile car corrects the deviation, it returns to the center point of the QR code and proceeds to step S6.

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

[0103] The working principle of this invention is as follows: A mobile trolley is controlled to enter the reflector area in front of the machine. Then, it switches to reflector positioning mode to scan the reflector. The scanned point cloud data is then filtered according to a preset intensity threshold T, and the filtered point cloud data is identified as the reflector's point cloud data. Next, the err_length function calculates the length error between the filtered point cloud data's outline and the reflector's length, the err_dist function calculates the center point error between the filtered point cloud data and the reflector, and the cost_intensity function calculates the relative intensity error between the filtered point cloud data and the reflector. After the reflector outline is successfully matched using the cost evaluation function, the coordinates (lx) of the filtered point cloud data are calculated. i ly i And obtain the pose matrix of the reflector in the lidar coordinate system. 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 odometer coordinate system origin in the moving trolley coordinate system r Obtain the pose matrix T of the reflector in the coordinate system of the moving trolley. L Simultaneously, the pose matrix T of the reflector in the coordinate system of the moving car is... L The system determines whether the mobile trolley is on the straight path between the reflectors, allowing it to correct any deviations from the center of the reflectors. This enables the trolley to accurately travel along the straight path to the bottom of the machine. After scanning the QR code, the system assesses the deviation between the trolley and the center point of the QR code. Calculating the deviation, the system continues to correct the discrepancy between the trolley's actual position and the QR code's center point. This ensures that the trolley can complete the docking within the docking error range of the QR code scanning area, thus enabling the loading, unloading, and handling 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 platform, and reflectors are provided on both sides at the front of the machine platform. A mobile trolley achieves positioning by scanning the reflectors, and drives into the bottom of the machine platform from the middle position of the reflectors to scan the QR code and achieve docking, characterized in that: Includes the following steps: S1. Control the moving trolley to enter the reflector scanning area in front of the machine and switch to reflector positioning mode, and then start matching the reflector outline. S1.

1. Scan the point cloud data in the reflector area using a laser radar set on a mobile trolley. Set a point cloud intensity threshold T and 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 outline length of the filtered point cloud data and the length of the reflector, determine the error between the center distance 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 outline matches. ， S2. 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 odometer coordinate system origin in the moving trolley coordinate system r Obtain the pose matrix T of the reflector in the coordinate system of the moving trolley. L ; S3. Based on the pose matrix T of the reflector in the coordinate system of the moving trolley L Determine whether the moving trolley is on the straight path where the center position between the reflectors is located, and then control the moving trolley to travel along the straight path where the center position between the reflectors is located and enter the bottom of the machine. S4. The preset mobile trolley can successfully scan the QR code within the maximum deviation range [-x, x]. The adjustment angle corresponding to the maximum deviation of the mobile trolley is a. The docking error range of the mobile trolley. S5. After the mobile trolley enters the scanning area at the bottom of the machine, the camera on the mobile trolley scans and recognizes the QR code information. Then, a rectangular coordinate system is established with the center point of the QR code as the origin. Next, it is determined whether there is a deviation error between the actual position of the mobile trolley and the center of the QR code. The mobile trolley is rotated along the preset QR code path and then returned to the preset QR code path for correction. This also includes steps S5.1-S5.2: S5.1 Calculate the deviation error between the x-coordinate of the actual position coordinates of the moving cart and the center point of the QR code, and then determine the positional relationship between the moving cart and the center point of the QR code. S5.1.1 If the deviation error x0 between the x-coordinate of the actual position coordinates of the moving cart and the center point coordinates of the QR code is 0, then there is no deviation error between the actual position of the moving cart and the center point of the QR code. Proceed to step S6. S5.1.2 If the deviation error x0 between the x-coordinate of the actual position coordinates of the moving cart and the x-coordinate of the center point of the QR code is within the range of [-x, 0), then it is determined that the moving cart is deviating to the left of the starting QR code, and proceed to step S5.

2. S5.1.3 If the deviation error x0 between the x-coordinate of the actual position coordinates of the moving cart and the x-coordinate of the center point of the QR code is within the range of (0, x], then it is determined that the moving cart is deviating to the right of the starting QR code, and proceed to step S5.

2. S5.2 Correct the deviation between the actual position coordinates of the moving cart and the coordinates of the midpoint of the QR code. S5.2.1 Calculate the angle a0 of the moving trolley deviating from the preset QR code center point using geometric relationships and the following formula (5.1). ； S5.2.2, Calculate the distance s from the moving cart to the center point of the QR code by using the following formula (5.2) and the relationship between them. ， S5.2.3 After adjusting the deviation angle a0, the mobile trolley travels a distance s along the deviation angle a0 at a speed v and then stops. Then, the mobile trolley rotates (90-a0)° along the preset QR code path direction so that the mobile trolley returns to the center point of the QR code after correcting the deviation, and proceeds to step S6. S6 After correcting the deviation error between the actual position of the mobile trolley and the center point of the QR code, the mobile trolley moves to the end position. It is determined whether the mobile trolley is within the docking error range, and the mobile trolley completes docking with the machine.

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

3. The multi-mode cooperative navigation docking method according to claim 1, characterized in that: 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 determined to be the point cloud data of the reflector; otherwise, it is determined to be other interfering point cloud data.

4. The multi-mode cooperative navigation docking method according to claim 1, characterized in that: Step S2 also includes calculating the coordinates (lx) of the filtered point cloud data. i ly i The specific steps are as follows: S2.1 Obtain the distance l between the lidar and the reflector, and the scanning angle z, from the lidar scan. S2.2 Then, use the distance l and the scanning angle z to calculate lx respectively. i ly i , ， ly i =l*sin(z) (3), S2.3 Calculate the pose matrix of the reflector in the lidar coordinate system. as follows, ， Calculate the pose matrix T of the reflector in the odometer coordinate system. g as follows, ， Calculate the pose matrix T of the reflector in the coordinate system of the moving vehicle. L as follows, 。 5. The multi-mode cooperative navigation docking method according to claim 1, characterized in that: Step S3 further includes steps S3.1-S3.2: S3.1 If the moving trolley deviates from the straight path where the center position of the reflectors is located, then according to the pose matrix T of the reflectors in the coordinate system of the moving trolley... L Calculate the deviation error, and then control the moving trolley to return to the straight path. S3.1.

1. Establish a rectangular coordinate system with the straight path as the y-axis and the horizontal line where the moving car is located as the x-axis. S3.1.2, Based on the pose matrix T of the reflector in the coordinate system of the moving trolley. L Determine the distance y0 that the moving car deviates from the y-axis, then select the deviation angle b0, and calculate the distance S that the moving car has traveled to the y-axis using geometric relationships, as follows: ， S3.1.3 Control the moving trolley to travel a distance S along the deviation angle b0 at a speed v0 and then stop. Then control the moving trolley to rotate (90-b0)° in the direction of the straight path so that the moving trolley returns to the straight path after correcting the deviation error. S3.2 If the moving trolley does not deviate from the straight path where the center position between the reflectors is located, then control the moving trolley to travel along the straight path where the center position between the reflectors is located and enter the bottom of the machine.

6. The multi-mode cooperative navigation docking method according to claim 1, characterized in that: In step S6, there is also 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 handling of materials; Otherwise, proceed to step S5 to re-connect.

Images