A method and system for autonomous docking of a towed unmanned platform

Abstract

The present invention discloses a method and system for autonomous docking of a towing unmanned platform, belonging to the field of autonomous driving. The method comprises: S1: obtaining the coarse positioning and orientation information of the cargo platform, environmental perception information and the distance between the towing platform and the cargo platform through SLAM; S2: using the A* algorithm to perform path planning according to the coarse positioning and orientation information and environmental perception information of the cargo platform; S3: using the model predictive control algorithm to control the towing platform to approach the cargo platform according to the planned path; S4: when the towing platform approaches the cargo platform according to the planned path, if the distance between the towing platform and the cargo platform is less than the distance threshold, switching to short-range precise positioning, and accurately positioning the cargo platform by the method of laser radar and visual two-dimensional code detection fusion. Autonomous docking of the towing platform and the cargo platform is achieved by planning and re-planning the path multiple times.

Description

Technical Field

[0001] The present invention relates to the field of autonomous driving technology, and in particular to a method and system for autonomous docking of a towed unmanned platform. Background Art

[0002] In recent years, autonomous driving technology has developed rapidly and has been gradually applied to logistics, mining, port and other fields. A traction vehicle is a commonly used means of transportation in the field of transportation. It consists of a traction platform and a cargo platform, and a docking mechanism is set at the front end of the cargo platform.

[0003] At present, the autonomous docking process between the unmanned vehicle traction platform and the cargo platform is mainly based on the fixed position docking method of the global satellite positioning system, that is, the cargo platform is parked at a calibrated preset position, and the traction platform is based on real-time dynamic differential positioning technology and combined with other on-board sensors (camera, lidar, UWB, etc.) to obtain environmental information around the vehicle, control the traction platform to move to the preset position, and realize the docking of the traction platform and the cargo platform. The existing methods have the following main disadvantages:

[0004] (1) Existing methods are mainly applicable to scenarios with good satellite signals. In satellite-denied environments, the real-time dynamic differential positioning method fails, and the positioning and orientation accuracy of the traction platform is reduced, resulting in a lower success rate of autonomous docking of the unmanned platform.

[0005] (2) Existing methods assume that the distance between the traction platform and the cargo platform is relatively close (smaller than the sensing distance of the on-board sensors). However, in real applications, the distance between the two may be arbitrary, which limits the application scope of the autonomous docking system.

[0006] (3) The existing method requires the cargo platform to be accurately parked at a calibrated preset position. When the parking position of the cargo platform deviates, the success rate of autonomous docking between the traction platform and the cargo platform will be reduced, which restricts the application of the autonomous docking method in actual production. Summary of the invention

[0007] The purpose of the present invention is to provide a method and system for autonomous docking of a towed unmanned platform, so as to solve the problem of high-precision positioning of a towed unmanned vehicle with an automatic driving function in a satellite denial environment, realize the autonomous docking function of the towing platform and the cargo platform, and is not limited by the autonomous docking distance and the deviation of the docking position of the cargo platform.

[0008] To achieve the above object, the present invention provides the following solutions:

[0009] A method for autonomous docking of a towed unmanned platform, comprising:

[0010] S1: When the distance between the traction platform and the cargo platform is greater than the distance threshold, the coarse positioning and orientation information of the cargo platform, the environmental perception information and the distance between the traction platform and the cargo platform are obtained based on SLAM; the SLAM is generated by the tight coupling of the laser radar and the inertial measurement unit set on the traction platform; when the distance between the traction platform and the cargo platform is not greater than the distance threshold, execute step S4;

[0011] S2: Based on the rough positioning and orientation information of the cargo platform and the environmental perception information, the A* algorithm is used for path planning;

[0012] S3: Using a model predictive control algorithm to control the traction platform to approach the cargo platform along a planned path;

[0013] S4: When the traction platform approaches the cargo platform according to the planned path, if the distance between the traction platform and the cargo platform is not greater than the distance threshold, the first precise distance between the docking device of the traction platform and the docking device of the cargo platform at the current moment and the first angle between the longitudinal axis Y of the traction platform and the longitudinal axis y of the cargo platform are calculated through the measurement parameters of the laser radar; the forward direction of the traction platform is the positive direction of the Y axis, the horizontal right is the positive direction of the X axis, and the docking point O on the docking surface of the traction platform is the origin of the coordinate system; the forward direction of the cargo platform is the positive direction of the y axis, the horizontal right is the positive direction of the x axis, and the docking point o on the docking surface of the cargo platform is the origin of the coordinate system;

[0014] S5: using the A* algorithm to perform path planning according to the first precise distance and the first angle, and using a model predictive control algorithm to control the traction platform to approach the cargo platform along the planned path;

[0015] S6: When the first precise distance and the first angle meet the first preset condition, if the camera on the docking surface of the traction platform detects the QR code on the docking surface of the cargo platform, the camera on the docking surface of the traction platform detects the QR code on the docking surface of the cargo platform for positioning and orientation, otherwise the autonomous parking planning algorithm is used to plan the path and adjust the relative position between the traction platform and the cargo platform; when the first precise distance and the first angle do not meet the first preset condition, the A* algorithm is used to plan the path and adjust the relative position between the traction platform and the cargo platform until the first preset condition is met;

[0016] S7: Calculate the second precise distance between the docking device of the traction platform and the docking device of the cargo platform and the second angle between the longitudinal axis Y of the traction platform and the longitudinal axis y of the cargo platform at the current moment by using a visual two-dimensional code positioning and orientation method;

[0017] S8: According to the second precise distance and the second angle, the A* algorithm is used for path planning, and the model predictive control algorithm is used to control the traction platform to approach the cargo platform along the planned path; when the second precise distance and the second angle meet the second preset condition, the traction platform and the cargo platform meet the positioning accuracy requirement of autonomous docking, otherwise the autonomous parking planning algorithm is used to re-plan the path, and the relative position between the traction platform and the cargo platform is adjusted until the second precise distance and the second angle meet the second preset condition.

[0018] Optionally, when the first precise distance and the first angle do not satisfy a first preset condition, an A* algorithm is used to perform path planning according to the first precise distance and the first angle.

[0019] Optionally, the calculation formula of the first precise distance D is as follows:

[0020]

[0021]

[0022] ;

[0023] The first angle γ The calculation formula is as follows:

[0024] ;

[0025] in, Dl , Dr are the distances from the laser radar to the left and right sides of the docking surface of the cargo platform, w is the width of the cargo platform interface measured by the laser radar, α is the angle between the laser radar and the left side of the cargo platform interface and the cargo platform interface, β is the angle between the laser radar and the right side of the cargo platform interface and the cargo platform interface, DY It is the distance from the docking plane of the cargo platform in the Y-axis direction measured by the laser radar.

[0026] Optionally, the first preset condition is:

[0027] .

[0028] Optionally, the second preset condition is: ; δ is the second angle, d is the second most accurate distance.

[0029] The present invention also provides a towing unmanned platform autonomous docking system, comprising:

[0030] An information acquisition module, used for acquiring the coarse positioning and orientation information of the cargo platform, the environmental perception information and the distance between the traction platform and the cargo platform based on SLAM when the distance between the traction platform and the cargo platform is greater than a distance threshold; the SLAM is generated by tightly coupling the laser radar and the inertial measurement unit provided on the traction platform;

[0031] The first path planning module is used to perform path planning using the A* algorithm based on the rough positioning and orientation information of the cargo platform and the environmental perception information;

[0032] A control module, used for controlling the traction platform to approach the cargo platform along a planned path by using a model predictive control algorithm;

[0033] The first precise distance and first angle calculation module is used to calculate the first precise distance between the docking device of the traction platform and the docking device of the cargo platform at the current moment and the first angle between the longitudinal axis Y of the traction platform and the longitudinal axis y of the cargo platform through the measurement parameters of the laser radar when the traction platform approaches the cargo platform according to the planned path, if the distance between the traction platform and the cargo platform is not greater than the distance threshold; the forward direction of the traction platform is the positive direction of the Y axis, the horizontal right is the positive direction of the X axis, and the docking point O on the docking surface of the traction platform is the origin of the coordinate system; the forward direction of the cargo platform is the positive direction of the y axis, the horizontal right is the positive direction of the x axis, and the docking point o on the docking surface of the cargo platform is the origin of the coordinate system;

[0034] A second path planning and control module, configured to use an A* algorithm to perform path planning according to the first precise distance and the first angle, and use a model predictive control algorithm to control the traction platform to approach the cargo platform along the planned path;

[0035] A positioning and orientation module, for, when the first precise distance and the first angle meet the first preset condition, if the camera on the docking surface of the traction platform detects the QR code on the docking surface of the cargo platform, then the camera on the docking surface of the traction platform detects the QR code on the docking surface of the cargo platform for positioning and orientation, otherwise, an autonomous parking planning algorithm is used to perform a path and adjust the relative position between the traction platform and the cargo platform; and when the first precise distance and the first angle do not meet the first preset condition, an A* algorithm is used to perform a path planning and adjust the relative position between the traction platform and the cargo platform until the first preset condition is met;

[0036] A second precise distance and second angle calculation module, used to calculate the second precise distance between the docking device of the traction platform and the docking device of the cargo platform at the current moment and the second angle between the longitudinal axis Y of the traction platform and the longitudinal axis y of the cargo platform by using a visual two-dimensional code positioning and orientation method;

[0037] The third path planning and control module is used to use the A* algorithm to perform path planning according to the second precise distance and the second angle, and use the model predictive control algorithm to control the traction platform to approach the cargo platform along the planned path; when the second precise distance and the second angle meet the second preset condition, the traction platform and the cargo platform meet the positioning accuracy requirement of autonomous docking, otherwise the autonomous parking planning algorithm is used to re-plan the path and adjust the relative position between the traction platform and the cargo platform until the second precise distance and the second angle meet the second preset condition.

[0038] Optionally, the calculation formula of the first precise distance D is as follows:

[0039]

[0040]

[0041] ;

[0042] The first angle γ The calculation formula is as follows:

[0043] ;

[0044] in, Dl , Dr are the distances from the laser radar to the left and right sides of the docking surface of the cargo platform, w is the width of the cargo platform interface measured by the laser radar, α is the angle between the laser radar and the left side of the cargo platform interface and the cargo platform interface, β is the angle between the laser radar and the right side of the cargo platform interface and the cargo platform interface, DY It is the distance from the docking plane of the cargo platform in the Y-axis direction measured by the laser radar.

[0045] Optionally, the first preset condition is:

[0046] .

[0047] Optionally, the second preset condition is: ; δ is the second angle, d is the second most accurate distance.

[0048] According to the specific embodiments provided by the present invention, the present invention discloses the following technical effects:

[0049] 1) The present invention uses a multi-source information comprehensive positioning method to solve the problem of high-precision positioning of a towing unmanned vehicle in a satellite denial environment, and realizes the autonomous docking function between the towing platform and the cargo platform;

[0050] 2) The present invention proposes a comprehensive positioning method combining long-distance coarse positioning with short-distance precise positioning, which can realize long-distance autonomous docking between the traction platform and the cargo platform;

[0051] 3) The present invention proposes a close-range precise positioning method that integrates laser radar and visual QR code detection, which avoids docking failure caused by the camera's inability to perceive the QR code or the deviation of the cargo platform's docking position;

[0052] 4) The autonomous docking method of the towing unmanned platform proposed in the present invention ensures the autonomous docking success rate of the towing platform and the cargo platform, reduces the manual intervention of personnel, and improves the efficiency of transportation. BRIEF DESCRIPTION OF THE DRAWINGS

[0053] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative labor.

[0054] Figure 1 This is a schematic diagram of autonomous docking between a traction platform and a cargo platform according to an embodiment of the present invention;

[0055] Figure 2 The figure is a layout diagram of the docking surface of the traction platform and the cargo platform according to an embodiment of the present invention;

[0056] Figure 3 This is a flow chart of an autonomous docking method for a towed unmanned platform according to an embodiment of the present invention;

[0057] Figure 4 The present invention is a specific flow chart of the autonomous docking method of a towed unmanned platform according to an embodiment of the present invention. DETAILED DESCRIPTION

[0058] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0059] The purpose of the present invention is to provide a method and system for autonomous docking of a towed unmanned platform, so as to solve the problem of high-precision positioning of a towed unmanned vehicle with an automatic driving function in a satellite denial environment, realize the autonomous docking function of the towing platform and the cargo platform, and is not limited by the autonomous docking distance and the deviation of the docking position of the cargo platform.

[0060] In order to make the above-mentioned objects, features and advantages of the present invention more obvious and easy to understand, the present invention is further described in detail below with reference to the accompanying drawings and specific embodiments.

[0061] like Figure 1 As shown, considering that the cargo platform is huge and difficult to control, the cargo platform is used as the reference vehicle platform, and the traction platform is aligned and docked with the cargo platform. The present invention applies multi-source information high-precision positioning technology and adopts a positioning solution that combines long-range coarse positioning with short-range precise positioning. In a satellite-denied environment, long-range coarse positioning is combined with on-board laser radar and inertial navigation information to perform multi-source information comprehensive positioning. The coarse positioning and directional information guides the traction platform to move closer to the cargo platform. When the distance between the traction platform and the cargo platform is less than 10m, it is switched to short-range precise positioning, and precise positioning is performed by integrating the laser radar with the visual two-dimensional code detection. Autonomous docking of the traction platform and the cargo platform is achieved by planning and re-planning the path multiple times.

[0062] Figure 2 The hardware layout on the docking surface of the traction platform and the cargo platform is shown, where (a) is the layout diagram of the docking surface of the traction platform, and (b) is the layout diagram of the docking surface of the cargo platform. The width and height of the docking surface 1 of the traction platform are equal to the width and height of the docking surface 5 of the cargo platform, which are represented by w and h respectively. The two dotted lines represent the horizontal and vertical symmetry axes of the docking surface 1 of the traction platform and the docking surface 5 of the cargo platform. Figure 2 In (a), the camera 2 is installed at the center of the traction platform docking surface 1, and is used to detect the QR code 6 on the cargo platform docking surface 5 during close-range precision positioning, so as to achieve precise positioning of the cargo platform; the laser radar 3 is installed on the top of the traction platform and is located on the longitudinal symmetry axis of the traction platform docking surface, so as to sense the surrounding environmental information and achieve precise positioning of the vehicle; two pins 4 are symmetrically arranged on the transverse symmetry axis of the traction platform docking surface 1, and are 3w / 8 away from the center point of the traction platform docking surface 1. The two pins 4 are respectively connected to the pin holes 7 on the cargo platform docking surface 5, so as to guide the precise docking between the traction platform and the cargo platform. Figure 2 In (b), the QR code 6 is arranged at the center of the cargo platform docking surface 5, and the visual QR code detection method is used to achieve accurate positioning of the vehicle; two pin holes 7 are symmetrically arranged on the transverse symmetry axis of the cargo platform docking plane 5, and are 3w / 8 away from the center point of the cargo platform docking surface 5. The two pin holes 7 are respectively connected to the pin columns 4 on the cargo platform docking surface 1, which are used to guide the traction platform and the cargo platform to dock accurately.

[0063] like Figure 3-4 As shown, the autonomous docking method of a towed unmanned platform provided by the present invention comprises the following steps:

[0064] S1: When the distance between the traction platform and the cargo platform is greater than a distance threshold (called long distance), the coarse positioning and orientation information of the cargo platform, the environmental perception information and the distance between the traction platform and the cargo platform are obtained based on SLAM; the SLAM is generated by the tight coupling of the laser radar and the inertial measurement unit set on the traction platform; when the distance between the traction platform and the cargo platform is not greater than the distance threshold, execute step S4.

[0065] S2: Based on the rough positioning and orientation information of the cargo platform and the environmental perception information, the A* algorithm is used for path planning.

[0066] When the straight-line distance between the traction platform and the cargo platform is greater than 10 meters, the existing laser SLAM tightly coupled with the lidar and inertial measurement unit (IMU) is used to output the vehicle's coarse positioning and orientation information and environmental perception information. Based on the coarse positioning and orientation information and environmental perception information, the existing A* algorithm is used to plan the path from the current position of the traction platform to the cargo platform.

[0067] S3: Using a model predictive control algorithm to control the traction platform to approach the cargo platform along a planned path.

[0068] S4: When the traction platform approaches the cargo platform according to the planned path, if the distance between the traction platform and the cargo platform is not greater than the distance threshold (referred to as close distance), the first precise distance between the docking device of the traction platform and the docking device of the cargo platform at the current moment and the first angle between the longitudinal axis Y of the traction platform and the longitudinal axis y of the cargo platform are calculated through the measurement parameters of the laser radar. Let the forward direction of the traction platform be the positive direction of the Y axis, the horizontal right be the positive direction of the X axis, the docking point O on the docking surface of the traction platform be the origin of the coordinate system, and the positive direction of the Z axis can be determined according to the right-hand rule; similarly, let the forward direction of the cargo platform be the positive direction of the y axis, the horizontal right be the positive direction of the x axis, the docking point o on the docking surface of the cargo platform be the origin of the coordinate system, and the positive direction of the z axis can be determined according to the right-hand rule.

[0069] Since laser SLAM is difficult to maintain high precision and stability in unstructured scenes, the close distance between the traction platform and the cargo platform will cause the laser radar to fail, so the laser radar alone cannot achieve precise positioning of the cargo platform. Since the visual two-dimensional code detection method has the situation that the two-dimensional code is not in the camera's receptive field, the visual two-dimensional code positioning and orientation fails, so the visual two-dimensional code detection method alone cannot achieve precise positioning of the cargo platform. In response to the above problems, the present invention proposes a precise positioning and orientation method that integrates laser radar and visual two-dimensional code.

[0070] When the distance between the traction platform and the cargo platform is no more than 10 meters, laser SLAM is used to sense the environmental information around the vehicle, and laser radar information is used for positioning and orientation. The distance between the laser radar and the left and right sides of the cargo platform docking surface is represented by Dl and Dr respectively, the distance from the cargo platform docking plane measured by the laser radar in the Y-axis direction is represented by DY, and the width of the cargo platform docking surface measured by the laser radar is represented by w; on the horizontal plane, the angles between the laser radar and the left and right sides of the cargo platform docking surface and the cargo platform docking surface are α and β respectively, the angle between the traction platform longitudinal axis Y and the cargo platform longitudinal axis y is γ, and the distance between the traction platform docking device and the cargo platform docking device is D; then α, β and D can be represented by the following formulas:

[0071]

[0072]

[0073]

[0074] Then γ can be expressed as:

[0075] .

[0076] S5: According to the first precise distance D and the first angle γ, an A* algorithm is used for path planning, and a model predictive control algorithm is used to control the traction platform to approach the cargo platform along the planned path.

[0077] S6: When the first precise distance D and the first angle γ meet the first preset condition, if the camera on the docking surface of the traction platform detects the QR code on the docking surface of the cargo platform, the camera on the docking surface of the traction platform detects the QR code on the docking surface of the cargo platform for positioning and orientation, otherwise the autonomous parking planning algorithm is used to re-plan the path, and finally the detection of the QR code is achieved.

[0078] If the distance D and the angle γ between the traction platform and the cargo platform do not meet the first preset condition, the A* algorithm is used for path planning to adjust the relative position between the traction platform and the cargo platform until the first preset condition is met. The first preset condition is as follows:

[0079] .

[0080] S7: Calculate the second precise distance between the docking device of the traction platform and the docking device of the cargo platform at the current moment and the second angle between the longitudinal axis Y of the traction platform and the longitudinal axis y of the cargo platform through the visual QR code positioning and orientation method.

[0081] S8: According to the second precise distance and the second angle, the A* algorithm is used for path planning, and the model predictive control algorithm is used to control the traction platform to approach the cargo platform along the planned path; when the second precise distance and the second angle meet the second preset condition, the traction platform and the cargo platform meet the positioning accuracy condition of autonomous docking, otherwise the autonomous parking planning algorithm is used to re-plan the path, and the relative position between the traction platform and the cargo platform is adjusted until the second precise distance and the second angle meet the second preset condition.

[0082] The existing visual QR code positioning and orientation method is used to calculate the distance d between the docking device of the traction platform and the docking device of the cargo platform and the angle δ between the longitudinal axis Y of the traction platform and the longitudinal axis y of the cargo platform. Based on d and δ, the A* algorithm is used to re-plan the path, and the model predictive control algorithm is used to adjust the posture of the traction platform to provide a good posture for the automatic docking of the unmanned platform. , then the autonomous docking conditions between the traction platform and the cargo platform are met and autonomous docking can be achieved. Otherwise, the autonomous parking planning algorithm is used to replan the path.

[0083] Taking into account the satellite denial environment and the system's adaptability to different scenarios, the present invention adopts a multi-source information comprehensive positioning method, which can output stable and accurate global positioning results in unstructured scenarios, and can maintain positioning accuracy for a long time when the satellite signal is interfered with or missing, thereby ensuring the autonomous positioning accuracy of the unmanned platform in different scenarios.

[0084] Taking into account the situation that the traction platform and the cargo platform are docked at a long distance in actual applications, the present invention divides the positioning system into two stages: long-distance coarse positioning and short-distance precise positioning; the coarse positioning information guides the traction platform to move closer to the cargo platform, and after meeting the preset conditions, it switches to short-distance precise positioning, and realizes long-distance autonomous docking of the unmanned vehicle through precise positioning and orientation methods.

[0085] Considering that the cargo platform is large and difficult to control, the cargo platform is used as the reference vehicle platform (which can be parked at any angle), and the precise positioning method combining lidar and visual QR code detection is used to locate and orient the cargo platform, thereby achieving high-precision positioning of the cargo platform.

[0086] The present invention also provides a towing unmanned platform autonomous docking system, comprising:

[0087] An information acquisition module, used for acquiring the coarse positioning and orientation information of the cargo platform, the environmental perception information and the distance between the traction platform and the cargo platform based on SLAM when the distance between the traction platform and the cargo platform is greater than a distance threshold; the SLAM is generated by tightly coupling the laser radar and the inertial measurement unit provided on the traction platform;

[0088] The first path planning module is used to perform path planning using the A* algorithm based on the rough positioning and orientation information of the cargo platform and the environmental perception information;

[0089] A control module, used for controlling the traction platform to approach the cargo platform along a planned path by using a model predictive control algorithm;

[0090] The first precise distance and first angle calculation module is used to calculate the first precise distance between the docking device of the traction platform and the docking device of the cargo platform at the current moment and the first angle between the longitudinal axis Y of the traction platform and the longitudinal axis y of the cargo platform through the measurement parameters of the laser radar when the traction platform approaches the cargo platform according to the planned path, if the distance between the traction platform and the cargo platform is not greater than the distance threshold; the forward direction of the traction platform is the positive direction of the Y axis, the horizontal right is the positive direction of the X axis, and the docking point O on the docking surface of the traction platform is the origin of the coordinate system; the forward direction of the cargo platform is the positive direction of the y axis, the horizontal right is the positive direction of the x axis, and the docking point o on the docking surface of the cargo platform is the origin of the coordinate system;

[0091] a second path planning and control module, configured to use an A* algorithm to perform path planning according to the first precise distance and the first angle, and use a model predictive control algorithm to control the traction platform to approach the cargo platform along the planned path, otherwise use an autonomous parking planning algorithm to perform path planning and adjust the relative position between the traction platform and the cargo platform; and when the first precise distance and the first angle do not meet a first preset condition, use the A* algorithm to perform path planning and adjust the relative position between the traction platform and the cargo platform until the first preset condition is met;

[0092] A positioning and orientation module, configured to perform positioning and orientation by detecting the QR code on the docking surface of the cargo platform through the camera on the docking surface of the traction platform when the first precise distance and the first angle meet a first preset condition and if the camera on the docking surface of the traction platform detects the QR code on the docking surface of the cargo platform;

[0093] A second precise distance and second angle calculation module, used to calculate the second precise distance between the docking device of the traction platform and the docking device of the cargo platform at the current moment and the second angle between the longitudinal axis Y of the traction platform and the longitudinal axis y of the cargo platform by using a visual two-dimensional code positioning and orientation method;

[0094] The third path planning and control module is used to use the A* algorithm to perform path planning according to the second precise distance and the second angle, and use the model predictive control algorithm to control the traction platform to approach the cargo platform along the planned path; when the second precise distance and the second angle meet the second preset condition, the traction platform and the cargo platform meet the positioning accuracy requirement of autonomous docking, otherwise the autonomous parking planning algorithm is used to re-plan the path and adjust the relative position between the traction platform and the cargo platform until the second precise distance and the second angle meet the second preset condition.

[0095] In this specification, each embodiment is described in a progressive manner, and each embodiment focuses on the differences from other embodiments. The same or similar parts between the embodiments can be referred to each other. For the system disclosed in the embodiment, since it corresponds to the method disclosed in the embodiment, the description is relatively simple, and the relevant parts can be referred to the method part.

[0096] The principles and implementation methods of the present invention are described in this article using specific examples. The description of the above embodiments is only used to help understand the method and core idea of ​​the present invention. At the same time, for those skilled in the art, according to the idea of ​​the present invention, there will be changes in the specific implementation methods and application scope. In summary, the content of this specification should not be understood as limiting the present invention.

Claims

1. A method for autonomous docking of a towed unmanned platform, characterized in that: include: S1: When the distance between the traction platform and the cargo platform is greater than the distance threshold, the coarse positioning and orientation information of the cargo platform, the environmental perception information and the distance between the traction platform and the cargo platform are obtained based on SLAM; the SLAM is generated by the tight coupling of the laser radar and the inertial measurement unit set on the traction platform; when the distance between the traction platform and the cargo platform is not greater than the distance threshold, execute step S4; S2: Based on the rough positioning and orientation information of the cargo platform and the environmental perception information, the A* algorithm is used for path planning; S3: Using a model predictive control algorithm to control the traction platform to approach the cargo platform along a planned path; S4: When the traction platform approaches the cargo platform according to the planned path, if the distance between the traction platform and the cargo platform is not greater than the distance threshold, the first precise distance between the docking device of the traction platform and the docking device of the cargo platform at the current moment and the first angle between the longitudinal axis Y of the traction platform and the longitudinal axis y of the cargo platform are calculated through the measurement parameters of the laser radar; the forward direction of the traction platform is the positive direction of the Y axis, the horizontal right is the positive direction of the X axis, and the docking point O on the docking surface of the traction platform is the origin of the coordinate system; the forward direction of the cargo platform is the positive direction of the y axis, the horizontal right is the positive direction of the x axis, and the docking point o on the docking surface of the cargo platform is the origin of the coordinate system; S5: using the A* algorithm to perform path planning according to the first precise distance and the first angle, and using a model predictive control algorithm to control the traction platform to approach the cargo platform along the planned path; S6: When the first precise distance and the first angle meet the first preset condition, if the camera on the docking surface of the traction platform detects the QR code on the docking surface of the cargo platform, the camera on the docking surface of the traction platform detects the QR code on the docking surface of the cargo platform for positioning and orientation, otherwise the autonomous parking planning algorithm is used to perform path planning and adjust the relative position between the traction platform and the cargo platform; when the first precise distance and the first angle do not meet the first preset condition, the A* algorithm is used to perform path planning and adjust the relative position between the traction platform and the cargo platform until the first preset condition is met; S7: Calculate the second precise distance between the docking device of the traction platform and the docking device of the cargo platform and the second angle between the longitudinal axis Y of the traction platform and the longitudinal axis y of the cargo platform at the current moment by using a visual two-dimensional code positioning and orientation method; S8: According to the second precise distance and the second angle, the A* algorithm is used for path planning, and the model predictive control algorithm is used to control the traction platform to approach the cargo platform along the planned path; when the second precise distance and the second angle meet the second preset condition, the traction platform and the cargo platform meet the positioning accuracy requirement of autonomous docking, otherwise the autonomous parking planning algorithm is used to re-plan the path, and the relative position between the traction platform and the cargo platform is adjusted until the second precise distance and the second angle meet the second preset condition.

2. The autonomous docking method for a towed unmanned platform according to claim 1, characterized in that: When the first precise distance and the first angle do not satisfy a first preset condition, an A* algorithm is used to perform path planning according to the first precise distance and the first angle.

3. The autonomous docking method of a towed unmanned platform according to claim 1, characterized in that: The calculation formula of the first precise distance D is as follows: ； The first angle γ The calculation formula is as follows: ； in, Dl , Dr are the distances from the laser radar to the left and right sides of the docking surface of the cargo platform, w is the width of the cargo platform interface measured by the laser radar, α is the angle between the laser radar and the left side of the cargo platform interface and the cargo platform interface, β is the angle between the laser radar and the right side of the cargo platform interface and the cargo platform interface, DY It is the distance from the docking plane of the cargo platform in the Y-axis direction measured by the laser radar.

4. The autonomous docking method of a towed unmanned platform according to claim 3, characterized in that: The first preset condition is: 。 5. The autonomous docking method of a towed unmanned platform according to claim 1, characterized in that: The second preset condition is: ; δ is the second angle, d is the second most accurate distance.

6. A towing unmanned platform autonomous docking system, characterized in that: include: An information acquisition module, used for acquiring the coarse positioning and orientation information of the cargo platform, the environmental perception information and the distance between the traction platform and the cargo platform based on SLAM when the distance between the traction platform and the cargo platform is greater than a distance threshold; the SLAM is generated by tightly coupling the laser radar and the inertial measurement unit provided on the traction platform; The first path planning module is used to perform path planning using the A* algorithm based on the rough positioning and orientation information of the cargo platform and the environmental perception information; A control module, used for controlling the traction platform to approach the cargo platform along a planned path by using a model predictive control algorithm; The first precise distance and first angle calculation module is used to calculate the first precise distance between the docking device of the traction platform and the docking device of the cargo platform at the current moment and the first angle between the longitudinal axis Y of the traction platform and the longitudinal axis y of the cargo platform through the measurement parameters of the laser radar when the traction platform approaches the cargo platform according to the planned path, if the distance between the traction platform and the cargo platform is not greater than the distance threshold; the forward direction of the traction platform is the positive direction of the Y axis, the horizontal right is the positive direction of the X axis, and the docking point O on the docking surface of the traction platform is the origin of the coordinate system; the forward direction of the cargo platform is the positive direction of the y axis, the horizontal right is the positive direction of the x axis, and the docking point o on the docking surface of the cargo platform is the origin of the coordinate system; A second path planning and control module, configured to use an A* algorithm to perform path planning according to the first precise distance and the first angle, and use a model predictive control algorithm to control the traction platform to approach the cargo platform along the planned path; A positioning and orientation module, for, when the first precise distance and the first angle meet the first preset condition, if the camera on the docking surface of the traction platform detects the QR code on the docking surface of the cargo platform, then the camera on the docking surface of the traction platform detects the QR code on the docking surface of the cargo platform for positioning and orientation, otherwise, an autonomous parking planning algorithm is used for path planning to adjust the relative position between the traction platform and the cargo platform; and when the first precise distance and the first angle do not meet the first preset condition, an A* algorithm is used for path planning to adjust the relative position between the traction platform and the cargo platform until the first preset condition is met; A second precise distance and second angle calculation module, used to calculate the second precise distance between the docking device of the traction platform and the docking device of the cargo platform at the current moment and the second angle between the longitudinal axis Y of the traction platform and the longitudinal axis y of the cargo platform by using a visual two-dimensional code positioning and orientation method; The third path planning and control module is used to use the A* algorithm to perform path planning according to the second precise distance and the second angle, and use the model predictive control algorithm to control the traction platform to approach the cargo platform along the planned path; when the second precise distance and the second angle meet the second preset condition, the traction platform and the cargo platform meet the positioning accuracy requirement of autonomous docking, otherwise the autonomous parking planning algorithm is used to re-plan the path and adjust the relative position between the traction platform and the cargo platform until the second precise distance and the second angle meet the second preset condition.

7. The autonomous docking system for a towed unmanned platform according to claim 6 is characterized in that: The calculation formula of the first precise distance D is as follows: ； The first angle γ The calculation formula is as follows: ； in, Dl , Dr are the distances from the laser radar to the left and right sides of the docking surface of the cargo platform, w is the width of the cargo platform interface measured by the laser radar, α is the angle between the laser radar and the left side of the cargo platform interface and the cargo platform interface, β is the angle between the laser radar and the right side of the cargo platform interface and the cargo platform interface, DY It is the distance from the docking plane of the cargo platform in the Y-axis direction measured by the laser radar.

8. The autonomous docking system for a towed unmanned platform according to claim 7 is characterized in that: The first preset condition is: 。 9. The autonomous docking system for a towed unmanned platform according to claim 7, characterized in that: The second preset condition is: ; δ is the second angle, d is the second most accurate distance.

Images