Semi-trailer automatic docking device, docking method and unmanned docking system

Abstract

The present invention discloses an automatic docking device for semi-trailers, a docking method and an unmanned docking system, which are used to realize automatic docking between a tractor and a semi-trailer located behind it, and the device includes a radar device, an unmanned driving controller and a safety pin controller; the radar device is arranged on the tractor, and the radar device is configured to detect the point cloud data of the semi-trailer to be docked; the unmanned driving controller is configured to receive the point cloud data detected by the radar device to calculate the reversing route of the tractor; and according to the reversing route, determine the steering angle, power driving amount and braking amount of the tractor; the safety pin controller is configured to drive the safety pin to insert into the pin hole in response to the first instruction of the unmanned driving controller. The present invention adopts a laser radar to scan the semi-trailer, and automatically calculates the reversing route to realize the automatic docking of the unmanned tractor and the semi-trailer.

Description

Technical Field

[0001] The invention relates to the field of unmanned driving, and in particular to an automatic docking device for a semitrailer. Background Art

[0002] With the continuous development of unmanned driving technology, the application of unmanned driving is becoming more and more extensive. The method of transporting goods from a designated starting point to a destination by using an unmanned tractor towing a semi-trailer can meet most of the current road transportation requirements. In the actual logistics transportation process, a small number of tractors are often used to tow multiple semi-trailers in sequence for transportation, that is, the drop-and-hook transportation method is adopted.

[0003] The specific method of drop-and-hook transport is that after the tractor tows the semi-trailer to the designated destination, the trailer is disconnected and connected to a new semi-trailer, and the new semi-trailer is towed to the next destination, and the cycle repeats. The specific disconnection process is 1.1) lowering the front support of the semi-trailer so that it can be parked on the ground stably as a whole; 1.2) disconnecting the safety pin and the wire and air pipe connection; 1.3) the tractor moves forward and disconnects from the semi-trailer.

[0004] The connection process is the opposite: 2.1) the driver observes and reverses to connect the tractor and the semi-trailer through a saddle or other means; 2.2) connect the wires and air pipes and insert the connection safety pin; 2.3) lift the front support of the semi-trailer so that only the rear wheels are supported on the ground.

[0005] At present, the connection and disconnection of semi-trailers are generally done manually, and the automatic connection and disconnection technology for unmanned semi-trailers is still blank.

[0006] The disclosure of the above background technology content is only used to assist in understanding the inventive concept and technical solution of the present invention. It does not necessarily belong to the prior art of this patent application, nor does it necessarily provide technical guidance. In the absence of clear evidence that the above content has been disclosed before the filing date of this patent application, the above background technology should not be used to evaluate the novelty and creativity of the present application. Summary of the invention

[0007] The purpose of the present invention is to provide a solution for automatic docking of a tractor and a semi-trailer located behind it, so as to realize automatic docking of an unmanned tractor and a semi-trailer. The unmanned operation of the whole process effectively reduces the personnel cost, improves the logistics transportation efficiency and reduces the logistics cost.

[0008] To achieve the above object, the technical solution adopted by the present invention is as follows:

[0009] A semitrailer automatic docking device is used to realize automatic docking between a tractor and a semitrailer located behind it, the automatic docking device comprises a radar device, an unmanned driving controller, a safety pin controller, and a semitrailer support lifter, wherein the radar device, the safety pin controller, and the semitrailer support lifter are all electrically connected to the unmanned driving controller;

[0010] Wherein, the radar device is arranged on the tractor, and the radar device is configured to detect point cloud data of the semi-trailer to be docked;

[0011] The unmanned driving controller is configured to receive the point cloud data detected by the radar device to calculate the reversing route of the tractor; and determine the steering angle, power driving amount and braking amount of the tractor according to the reversing route;

[0012] The safety pin controller is configured to drive the safety pin to be inserted into the pin hole in response to a first instruction of the unmanned driving controller, wherein one of the safety pin and the pin hole is arranged on the tractor, and the other is arranged on the semi-trailer;

[0013] The semi-trailer support lifter is configured to drive the semi-trailer supporter to lift in response to a second instruction from the unmanned driving controller.

[0014] Further, based on any one of the above-mentioned technical solutions or a combination of multiple technical solutions, the reversing route of the tractor is obtained by the following steps:

[0015] Dividing the point cloud data detected by the radar device into a plurality of point cloud data subsets according to a direction parallel to the ground where the semi-trailer is currently located;

[0016] Selecting one of the point cloud data subsets that best meets the preset front shape features of the semi-trailer from the plurality of point cloud data subsets;

[0017] Calculating the position of the connecting mechanism on the semi-trailer relative to the radar device according to the selected point cloud data subset;

[0018] According to the relative position information between the radar device and the rear axle center of the tractor, converting the position of the connecting mechanism relative to the radar device into the position of the connecting mechanism relative to the rear axle center of the tractor;

[0019] The position of the connecting mechanism relative to the center of the rear axle of the tractor is taken as the end point of the reverse route.

[0020] Further, based on any one of the above-mentioned technical solutions or a combination of multiple technical solutions, the reverse route is obtained by the following steps:

[0021] The radar device scans the three-dimensional point cloud data within a preset angle range behind the tractor to obtain a three-dimensional point set P, where pi =(x i ,y i ,z i ) T ,p i ∈P;

[0022] The unmanned driving controller uses n-1 planes parallel to the ground where the semi-trailer is currently located to divide the three-dimensional point set P into n subsets P j ,j=1,…,n;

[0023] For each subset P j Determine whether the point cloud data contains the preset front shape features of the semi-trailer. If so, count the number of valid laser data points that meet the features and evaluate the subset P by the following formula: j Matching degree: S j =N′ j / N j , where S j For the subset P j The matching degree, N j For the subset P j The number of data points, N′ j P j The number of valid laser data points in ;

[0024] Select the matching degree S j The highest subset P j , according to the subset P j Calculate the position R′ of the connecting mechanism on the semitrailer relative to the radar device r =(x r ,y r ,θ r ) T , where x r is the offset distance between the connecting mechanism and the radar device in the x-axis direction, y r is the offset distance between the connection mechanism and the radar device in the y-axis direction, θ r is the deflection angle of the vector between the radar device and the connecting mechanism relative to the plane of the xy coordinate system;

[0025] The position of the connecting mechanism relative to the rear axle center of the tractor is calculated based on the relative position information between the radar device and the rear axle center of the tractor. in, The coordinate transformation between the xy coordinate system and the rear axle center coordinate system of the tractor;

[0026] R r The calculated result is used as the end point of the reversing route.

[0027] Further, based on any one of the above-mentioned technical solutions or a combination of multiple technical solutions, the n-1 planes parallel to the ground where the semi-trailer is currently located are distributed at equal intervals, and the subset P j Calculate the position R′ of the connecting mechanism on the semi-trailer relative to the radar device r =(x r ,y r ,θ r ) T include:

[0028] According to the distribution characteristics of the connecting mechanism on the semi-trailer, from the subset P j Determine the point cloud data p corresponding to the connection mechanism obj =(x obj ,y obj ,z obj ) T ;

[0029] x r =x obj ,y r =y obj , and according to the point cloud data p obj And trigonometric function relationship to determine θ r .

[0030] Further, based on any one of the above-mentioned technical solutions or a combination of multiple technical solutions, according to the point cloud data p obj And trigonometric function relationship, the following formula is used to determine θ r :

[0031]

[0032] Further, based on any one of the above-mentioned technical solutions or a combination of multiple technical solutions, the unmanned driving controller determines the steering angle C of the tractor by the following method: S :

[0033] According to the position R′ of the connecting mechanism on the semi-trailer relative to the radar device r =(x r ,y r ,θ r ) T Calculate the orientation error E between the tractor and the semi-trailer θ , lateral error E lat :

[0034] E θ =θ r , E lat =x r sinθ r -y r ·cosθr , E lon =-x r ·cosθ r -y r sinθ r ;

[0035] Calculate the steering angle C of the tractor S =tan -1 (l f ·ω / v f ), where l f v is the distance from the center of the rear axle to the center of the front axle of the tractor, f is the real-time speed of the tractor, and the angular velocity ω=w θ ·|v f |·E θ -w lat ·v f ·E lat ·ε, where w θ 、w lat is the preset adjustment coefficient,

[0036] Further, based on any one of the above-mentioned technical solutions or a combination of multiple technical solutions, the unmanned driving controller determines the power driving amount C of the tractor by the following method: T and braking amount C B :

[0037] Among them, w k is the preset adjustment coefficient, v f is the real-time speed of the tractor, v exp is the expected reverse speed.

[0038] Further, based on any one of the above-mentioned technical solutions or a combination of multiple technical solutions, the unmanned driving controller is configured to be based on the position R′ of the connecting mechanism on the semi-trailer relative to the radar device. r =(x r ,y r ,θ r ) T Calculate the longitudinal error E between the tractor and the semitrailer lon :E lon =-x r ·cosθ r -y r sinθ r ;

[0039] In response to E lon If the distance is less than a preset distance threshold, the unmanned driving controller sends a braking command to the tractor.

[0040] Further, based on any one of the technical solutions or a combination of multiple technical solutions mentioned above, the safety pin controller is connected to the unmanned driving controller in a two-way communication manner, and the safety pin controller sends a first confirmation signal to the unmanned driving controller after completing driving the safety pin to insert into the pin hole, and the first confirmation signal is configured to trigger the unmanned driving controller to send the second instruction; or,

[0041] The semi-trailer support lifter is connected to the unmanned driving controller for two-way communication. After the semi-trailer support lifter drives the semi-trailer supporter to a preset position, it sends a second confirmation signal to the unmanned driving controller.

[0042] According to another aspect of the present invention, the present invention provides a method for automatically docking a tractor with a semitrailer located behind the tractor, comprising the following steps:

[0043] Use a radar device to obtain point cloud data behind the tractor;

[0044] Divide into several point cloud data subsets according to the direction parallel to the ground where the semi-trailer is currently located;

[0045] Selecting one of the point cloud data subsets that best meets the preset front shape features of the semi-trailer from the plurality of point cloud data subsets;

[0046] Calculating the position of the connecting mechanism on the semi-trailer relative to the radar device according to the selected point cloud data subset;

[0047] According to the relative position information between the radar device and the rear axle center of the tractor, converting the position of the connecting mechanism relative to the radar device into the position of the connecting mechanism relative to the rear axle center of the tractor;

[0048] Taking the position of the connecting mechanism relative to the center of the rear axle of the tractor as the end point of the reversing route, driving the tractor to reverse to the end point according to the reversing route;

[0049] The safety pin on the driving tractor is inserted into the pin hole on the semi-trailer, or the safety pin on the driving semi-trailer is inserted into the pin hole on the tractor.

[0050] Further, based on any one of the above-mentioned technical solutions or a combination of multiple technical solutions, the reverse route is obtained by the following steps:

[0051] The radar device scans the three-dimensional point cloud data within a preset angle range behind the tractor to obtain a three-dimensional point set P, where p i =(x i ,y i ,z i ) T ,p i ∈P;

[0052] Using n-1 planes parallel to the current ground where the semi-trailer is located, the three-dimensional point set P is divided into n subsets P j ,j=1,…,n;

[0053] For each subset P j Determine whether the point cloud data contains the preset front shape features of the semi-trailer. If so, count the number of valid laser data points that meet the features and evaluate the subset P by the following formula: j Matching degree: S j =N′ j / N j , where S j For the subset P j The matching degree, N j For the subset P j The number of data points, N′ j P j The number of valid laser data points in ;

[0054] Select the matching degree S j The highest subset P j , according to the subset P j Calculate the position R′ of the connecting mechanism on the semi-trailer relative to the radar device r =(x r ,y r ,θ r ) T , where x r is the offset distance between the connecting mechanism and the radar device in the x-axis direction, y r is the offset distance between the connection mechanism and the radar device in the y-axis direction, θ r is the deflection angle of the vector between the radar device and the connecting mechanism relative to the plane of the xy coordinate system;

[0055] The position of the connecting mechanism relative to the rear axle center of the tractor is calculated based on the relative position information between the radar device and the rear axle center of the tractor. in, The coordinate transformation between the xy coordinate system and the rear axle center coordinate system of the tractor;

[0056] R r The calculated result is used as the end point of the reversing route.

[0057] According to another aspect of the present invention, the present invention provides an unmanned automatic docking system, comprising a tractor, a semitrailer and the automatic docking device for the semitrailer as described above.

[0058] The beneficial effects brought by the technical solution provided by the present invention are as follows:

[0059] a. Realize the automatic docking between the unmanned tractor and the semi-trailer. The unmanned operation of the whole process effectively reduces the personnel cost and improves the logistics transportation efficiency;

[0060] b. The reverse route calculation method does not depend on the size or model of the semi-trailer, and the automatic docking function has a wide range of applications. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0062] Figure 1 A schematic diagram of the composition of an automatic docking device for a semitrailer provided by an exemplary embodiment of the present invention;

[0063] Figure 2 A schematic flow chart of an automatic docking method between a tractor and a semi-trailer behind the tractor provided in accordance with an exemplary embodiment of the present invention;

[0064] Figure 3 A schematic diagram of the front appearance features of a semi-trailer and valid laser data points and invalid data points provided by an exemplary embodiment of the present invention;

[0065] Figure 4 A schematic diagram of the orientation error and lateral error between a tractor and a semi-trailer provided for an exemplary embodiment of the present invention. DETAILED DESCRIPTION

[0066] In order to enable those skilled in the art to better understand the scheme of the present invention, the technical scheme in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings 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 should fall within the scope of protection of the present invention.

[0067] It should be noted that the terms "first", "second", etc. in the specification and claims of the present invention and the above-mentioned drawings are used to distinguish similar objects, and are not necessarily used to describe a specific order or sequence. It should be understood that the data used in this way can be interchanged where appropriate, so that the embodiments of the present invention described herein can be implemented in an order other than those illustrated or described herein. In addition, the terms "including" and "having" and any variations thereof are intended to cover non-exclusive inclusions, for example, a process, method, device, product or equipment that includes a series of steps or units is not necessarily limited to those steps or units clearly listed, but may include other steps or units that are not clearly listed or inherent to these processes, methods, products or equipment.

[0068] In one embodiment of the present invention, a semi-trailer automatic docking device is provided, which is used to realize automatic docking between a tractor and a semi-trailer located behind it, and the automatic docking device includes a radar device (such as a laser radar), an unmanned driving controller, a safety pin controller, and a semi-trailer support lifter. The laser radar, the safety pin controller, and the semi-trailer support lifter are all electrically connected to the unmanned driving controller. Figure 1 shown.

[0069] In this embodiment, the radar device is arranged at the rear of the tractor so as to be able to scan the point cloud data of the rear of the tractor; the unmanned driving controller can be installed in the cab of the tractor and connected to the radar device through a cable. When the unmanned driving tractor stops in front of the semi-trailer to be docked, the automatic docking mode is started, and the radar device scans and detects the point cloud data of the semi-trailer to be docked. The unmanned driving controller analyzes the point cloud data scanned by the radar device to obtain the width of the semi-trailer and the parking position and direction of the semi-trailer relative to the tractor, as shown in the figure. Figure 2 As shown:

[0070] The first step is to divide the point cloud data detected by the radar device into several point cloud data subsets according to the direction parallel to the ground where the semi-trailer is currently located; specifically, the radar device scans the three-dimensional point cloud data within a preset angle range (e.g., close to 180°) behind the tractor to obtain a three-dimensional point set P, where p i =(x i ,y i ,z i ) T ,p i ∈P; The unmanned driving controller uses n-1 planes parallel to the current ground where the semi-trailer is located to divide the three-dimensional point set P into n subsets P j ,j=1,…,n; the n-1 planes parallel to the ground where the semi-trailer is currently located are distributed at equal intervals, for example, the interval between two adjacent planes is 0.1 meter or 5 cm or 15 cm.

[0071] The second step is to select a point cloud data subset that best meets the preset front shape features of the semi-trailer from the plurality of point cloud data subsets; specifically, a matching algorithm, such as a RANSAC matching algorithm, is used to match each subset P j Determine whether the point cloud data contains the preset front shape features of the semi-trailer. The front shape features of the semi-trailer can be found in Figure 3 , Figure 3 There is a horizontal straight line and two drooping straight lines in the point cloud data subset, which belongs to the characteristic contour line of the front shape of the semi-trailer. Figure 3 The front shape feature of the semi-trailer shown in the figure is used to count the number of valid laser data points that meet the feature, and the subset P is evaluated by the following formula j Matching degree: S j =N′ j / N j , where S j For the subset P j The matching degree, N j For the subset P j The number of data points, N′ j P j The number of valid laser data points in ; for example Figure 3 The dot mark 001 in the figure is on the characteristic contour line and is a valid laser data point; while the triangle mark 002 deviates from the characteristic contour line and is an invalid laser data point. j The highest subset P j And proceed to the next step.

[0072] Step 3: According to the matching degree S j The highest subset P j , calculate the position R′ of the connecting mechanism on the semitrailer relative to the radar device r =(x r ,y r ,θ r ) T , where x r is the offset distance between the connecting mechanism and the radar device in the x-axis direction, y r is the offset distance between the connection mechanism and the radar device in the y-axis direction, θ r is the deflection angle of the vector between the radar device and the connecting mechanism relative to the xy coordinate plane; specifically, according to the distribution characteristics of the connecting mechanism on the semi-trailer, from the subset P j Determine the point cloud data p corresponding to the connection mechanism obj =(x obj ,y obj ,z obj ) TFor example, if the connecting mechanism is set at the midpoint of the front of the semi-trailer, the point corresponding to the connecting mechanism is Figure 3 The characteristic contour line (the midpoint of the horizontal line). obj Then, R' is calculated by the following formula r :

[0073]

[0074] Step 4: Based on the relative position information between the radar device and the rear axle center of the tractor, the position of the connecting mechanism relative to the radar device is converted into the position of the connecting mechanism relative to the rear axle center of the tractor. in, The coordinate transformation between the xy coordinate system and the rear axle center coordinate system of the tractor;

[0075] Step 5: The position of the connecting mechanism relative to the center of the rear axle of the tractor, i.e. R r The calculation result is used as the end point of the reversing route, that is, the parking position and direction of the semi-trailer relative to the tractor are determined.

[0076] In the embodiment of the present invention, the unmanned driving controller further determines the steering angle, power driving amount and braking amount of the tractor according to the reversing route, as follows:

[0077] According to the position R′ of the connecting mechanism on the semi-trailer relative to the radar device r =(x r ,y r ,θ r ) T Calculate the orientation error E between the tractor and the semi-trailer θ , lateral error E lat :

[0078] E θ =θ r , E lat =x r sinθ r -y r ·cosθ r , E lon =-x r ·cosθ r -y r sinθ r ;

[0079] See also Figure 4 , orientation error E θ is the angle between the current orientation of the tractor and the length direction of the semi-trailer, and the lateral error E lat It is the offset distance between the tractor and the semi-trailer on the x-axis (width direction).

[0080] The steering angle C of the tractor is calculated by the following formula: S =tan -1 (l f ·ω / v f ), where l f v is the distance from the center of the rear axle to the center of the front axle of the tractor, f is the real-time speed of the tractor, and the angular velocity ω=w θ ·|v f |·E θ -w lat ·v f ·E lat ·ε, where w θ 、w lat is the preset adjustment coefficient, In one embodiment, w θ 、w lat The values ​​can be 0.1 and 0.01 respectively.

[0081] The unmanned driving controller further determines the driving power C of the tractor by the following method: T and braking amount C B :

[0082] Among them, w k is the preset adjustment coefficient, v f is the real-time speed of the tractor, v exp is the expected reverse speed. In one embodiment, w k The possible value is 5.0.

[0083] The unmanned driving controller calculates the steering angle C S , Braking amount C B , power driving amount C T (For engine vehicles, it is the throttle control amount) is sent to the corresponding vehicle controller to drive the tractor to move slowly towards the position of the semi-trailer.

[0084] During the slow-moving process, the unmanned driving controller obtains the longitudinal error E of the semi-trailer connection mechanism relative to the rear axle center of the tractor based on the real-time detection results of the semi-trailer by the laser radar. lon , according to the position R′ of the connecting mechanism on the semitrailer relative to the radar device r =(x r ,y r ,θ r ) T Calculate the longitudinal error E between the tractor and the semitrailer lon The formula is as follows: lon =-x r·cosθ r -y r sinθ r ; when E lon When the distance is less than the preset distance threshold, it is determined that the vehicle has completed reversing, and the unmanned driving controller sends a braking command to the vehicle.

[0085] The safety pin controller is configured to drive the safety pin into the pin hole in response to the first instruction of the unmanned driving controller, such as the safety pin is set on the tractor and the pin hole is set on the semi-trailer, or vice versa; taking the safety pin controller located directly behind the tractor as an example, it includes a microcontroller and an electric push rod, the electric push rod is connected to the safety pin, the microcontroller is connected to the electric push rod and the unmanned driving controller, and when receiving the instruction sent by the unmanned driving controller, the electric push rod is driven to insert or withdraw the safety pin from the pin hole. The semi-trailer support lifter is configured to drive the semi-trailer supporter to lift in response to the second instruction of the unmanned driving controller. In a specific embodiment, the safety pin controller is connected to the unmanned driving controller in a two-way communication manner. After completing the driving of the safety pin into the pin hole, the safety pin controller sends a first confirmation signal to the unmanned driving controller. The first confirmation signal is configured to trigger the unmanned driving controller to send the second instruction, that is, after the unmanned driving controller receives the connection confirmation signal of the safety pin controller, the second instruction is sent to the semi-trailer support lifter.

[0086] In one embodiment of the present invention, before the driving safety pin is inserted into the pin hole, the electrical quick-connect device on the tractor automatically completes the connection of the corresponding wires and air pipes with the semi-trailer through the guide mechanism during the reversing process of the vehicle, and ensures the stability of the wire connection and the air pipe connection without leakage through the clamping device; the electrical quick docking device is divided into two sides, including a tractor docking seat, a semi-trailer docking seat, and a guiding device. The tractor docking seat is installed below the semi-trailer connection point at the rear of the tractor, and the semi-trailer docking seat is installed below the front connection point of the semi-trailer. The guide is loaded in front of the semi-trailer docking seat. During the reversing process of the tractor toward the semi-trailer, the tractor docking seat can be guided to automatically align with the semi-trailer docking seat. When the tractor is reversed, the spring clamping mechanism in the tractor docking seat ensures that the electrical and air connections are not loose.

[0087] In addition, the semi-trailer support lifter can also be connected to the unmanned driving controller in a two-way communication manner. The semi-trailer support lifter is located in the middle of the semi-trailer and includes a microcontroller and a drive motor. The drive motor is connected to the semi-trailer supporter through gears, and the microcontroller is connected to the drive motor. After the tractor and the semi-trailer are electrically connected, the microcontroller is connected to the unmanned driving controller. After receiving the signal sent by the unmanned driving controller, the microcontroller can drive the motor to lift or lower the semi-trailer supporter at the front of the semi-trailer. After the semi-trailer support lifter completes driving the semi-trailer supporter to the preset position, the second confirmation signal is sent to the unmanned driving controller, that is, after the unmanned driving controller receives the lifting confirmation signal of the semi-trailer support lifter, it can confirm that the automatic docking is completed, exit the automatic docking mode, and enter the general unmanned driving mode.

[0088] The embodiment of the present invention also provides a method for automatically docking a tractor with a semitrailer located behind the tractor, such as Figure 2 As shown, the following steps are included:

[0089] Use a radar device to obtain point cloud data behind the tractor;

[0090] Divide into several point cloud data subsets according to the direction parallel to the ground where the semi-trailer is currently located;

[0091] Selecting one of the point cloud data subsets that best meets the preset front shape features of the semi-trailer from the plurality of point cloud data subsets;

[0092] Calculating the position of the connecting mechanism on the semi-trailer relative to the radar device according to the selected point cloud data subset;

[0093] According to the relative position information between the radar device and the rear axle center of the tractor, converting the position of the connecting mechanism relative to the radar device into the position of the connecting mechanism relative to the rear axle center of the tractor;

[0094] Taking the position of the connecting mechanism relative to the center of the rear axle of the tractor as the end point of the reversing route, driving the tractor to reverse to the end point according to the reversing route;

[0095] The safety pin on the driving tractor is inserted into the pin hole on the semi-trailer, or the safety pin on the driving semi-trailer is inserted into the pin hole on the tractor.

[0096] Further, based on any one of the above-mentioned technical solutions or a combination of multiple technical solutions, the reverse route is obtained by the following steps:

[0097] The radar device scans the three-dimensional point cloud data within a preset angle range behind the tractor to obtain a three-dimensional point set P, where p i =(x i ,y i ,zi ) T ,p i ∈P;

[0098] Using n-1 planes parallel to the current ground where the semi-trailer is located, the three-dimensional point set P is divided into n subsets P j ,j=1,…,n;

[0099] For each subset P j Determine whether the point cloud data contains the preset front shape features of the semi-trailer. If so, count the number of valid laser data points that meet the features and evaluate the subset P by the following formula: j Matching degree: S j =N′ j / N j , where S j For the subset P j The matching degree, N j For the subset P j The number of data points, N′ j P j The number of valid laser data points in ;

[0100] Select the matching degree S j The highest subset P j , according to the subset P j Calculate the position R′ of the connecting mechanism on the semi-trailer relative to the radar device r =(x r ,y r ,θ r ) T , where x r is the offset distance between the connecting mechanism and the radar device in the x-axis direction, y r is the offset distance between the connection mechanism and the radar device in the y-axis direction, θ r is the deflection angle of the vector between the radar device and the connecting mechanism relative to the plane of the xy coordinate system;

[0101] The position of the connecting mechanism relative to the rear axle center of the tractor is calculated based on the relative position information between the radar device and the rear axle center of the tractor. in, The coordinate transformation between the xy coordinate system and the rear axle center coordinate system of the tractor;

[0102] R r The calculated result is used as the end point of the reversing route.

[0103] It should be noted that the present automatic docking method embodiment and the above-mentioned automatic docking device embodiment belong to the same concept, and the entire content of the device embodiment can be incorporated into the present method embodiment by reference.

[0104] The present invention adopts a laser radar to scan the semi-trailer, automatically calculates the reversing route, and controls the vehicle to complete the docking; for the tractor in the drop-and-hook working mode, the overall unmanned driving of the tractor can be realized, thereby reducing labor costs and improving logistics transportation efficiency; during the automatic docking process, it only relies on the detection data of the rear laser radar, the system structure is simple, is not affected by other communication, climate and other conditions, and has high stability; there is no requirement for the type of semi-trailer, and only the electric or pneumatic quick docking seat needs to be modified and a supporting lifter needs to be installed, with a low modification cost, and is suitable for most existing semi-trailers.

[0105] It should be noted that, in this article, relational terms such as first and second, etc. are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Moreover, the terms "include", "comprise" or any other variants thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or device. In the absence of further restrictions, the elements defined by the sentence "comprise a ..." do not exclude the existence of other identical elements in the process, method, article or device including the elements.

[0106] The above is only a specific implementation of the present application. It should be pointed out that for ordinary technicians in this technical field, several improvements and modifications can be made without departing from the principles of the present application. These improvements and modifications should also be regarded as the scope of protection of the present application.

Claims

1. A semi-trailer automatic docking device, characterized in that: Used to realize automatic docking between a tractor and a semi-trailer located behind it, the automatic docking device includes a radar device, an unmanned driving controller, a safety pin controller, and a semi-trailer support lifter, and the radar device, the safety pin controller, and the semi-trailer support lifter are all electrically connected to the unmanned driving controller; Wherein, the radar device is arranged on the tractor, and the radar device is configured to detect point cloud data of the semi-trailer to be docked; The unmanned driving controller is configured to receive the point cloud data detected by the radar device to calculate the reversing route of the tractor; and determine the steering angle, power driving amount and braking amount of the tractor according to the reversing route; wherein the reversing route is obtained by the following steps: the radar device scans the three-dimensional point cloud data within a preset angle range behind the tractor to obtain the three-dimensional point set P, p i =(x i ,y i ,z i ) T ,p i ∈P; The unmanned driving controller uses n-1 planes parallel to the ground where the semi-trailer is currently located to divide the three-dimensional point set P into n subsets P j ,j=1,…,n; for each subset P j Determine whether the point cloud data contains the preset front shape features of the semi-trailer. If so, count the number of valid laser data points that meet the features and evaluate the subset P by the following formula: j Matching degree: S j =N′ j / N j , where S j For the subset P j The matching degree, N j For the subset P j The number of data points, N′ j P j The number of valid laser data points in; select the matching degree S j The highest subset P j , according to the subset P j Calculate the position R′ of the connecting mechanism on the semi-trailer relative to the radar device r =(x r ,y r ,θ r ) T , including: according to the distribution characteristics of the connecting mechanism on the semi-trailer, from the subset P j Determine the point cloud data p corresponding to the connection mechanism obj =(x obj ,y obj ,z obj ) T ;x r =x obj ,y r =y obj , and according to the point cloud data p obj And trigonometric function relationship to determine θ r : Among them, x r is the offset distance between the connecting mechanism and the radar device in the x-axis direction, y r is the offset distance between the connection mechanism and the radar device in the y-axis direction, θ r is the deflection angle of the vector between the radar device and the connecting mechanism relative to the xy coordinate plane; according to the relative position information between the radar device and the rear axle center of the tractor, the position of the connecting mechanism relative to the rear axle center of the tractor is calculated. in, is the coordinate transformation between the xy coordinate system and the tractor rear axle center coordinate system; R r The calculated result is taken as the end point of the reverse route; The safety pin controller is configured to drive the safety pin to be inserted into the pin hole in response to a first instruction of the unmanned driving controller, wherein one of the safety pin and the pin hole is arranged on the tractor, and the other is arranged on the semi-trailer; The semi-trailer support lifter is configured to drive the semi-trailer supporter to lift in response to a second instruction from the unmanned driving controller.

2. The automatic docking device for semi-trailers according to claim 1, characterized in that: The n-1 planes parallel to the ground where the semi-trailer is currently located are distributed at equal intervals.

3. The automatic docking device for semi-trailers according to claim 1, characterized in that: The unmanned driving controller determines the steering angle C of the tractor by the following method: S : According to the position R of the connecting mechanism on the semi-trailer relative to the radar device ′ r =(x r ,y r ,θ r ) T Calculate the orientation error E between the tractor and the semi-trailer θ , lateral error E lat : E θ =θ r ,E lat =x r ·sinθ r -y r ·cosθ r ; Calculate the steering angle C of the tractor S =tan -1 (l f ·ω / v f ), where l f v is the distance from the center of the rear axle to the center of the front axle of the tractor, f is the real-time speed of the tractor, and the angular velocity ω=w θ ·|v f |·E θ -w lat ·v f ·E lat ·ε, where w θ 、w lat is the preset adjustment coefficient, 4. The automatic docking device for semi-trailers according to claim 1, characterized in that: The unmanned driving controller determines the driving power C of the tractor by the following method: T and braking amount C B : Among them, w k is the preset adjustment coefficient, v f is the real-time speed of the tractor, v exp is the expected reverse speed.

5. The automatic docking device for semi-trailers according to claim 1, characterized in that: The unmanned driving controller is configured to determine the position of the connecting mechanism on the semi-trailer relative to the radar device. ′ r =(x r ,y r ,θ r ) T Calculate the longitudinal error E between the tractor and the semitrailer lon :E lon =-x r ·cosθ r -y r sinθ r ; In response to E lon If the distance is less than a preset distance threshold, the unmanned driving controller sends a braking command to the tractor.

6. The automatic docking device for semi-trailers according to claim 1, characterized in that: The safety pin controller is connected to the unmanned driving controller for two-way communication. After completing driving the safety pin into the pin hole, the safety pin controller sends a first confirmation signal to the unmanned driving controller. The first confirmation signal is configured to trigger the unmanned driving controller to send the second instruction.

7. The automatic docking device for semi-trailers according to claim 1, characterized in that: The semi-trailer support lifter is connected to the unmanned driving controller for two-way communication. After the semi-trailer support lifter drives the semi-trailer supporter to a preset position, it sends a second confirmation signal to the unmanned driving controller.

8. A method for automatically docking a tractor with a semitrailer behind it, characterized in that: Based on the semi-trailer automatic docking device according to any one of claims 1 to 7, the automatic docking method comprises the following steps: Use a radar device to obtain point cloud data behind the tractor; Divide into several point cloud data subsets according to the direction parallel to the ground where the semi-trailer is currently located; Selecting one of the point cloud data subsets that best meets the preset front shape features of the semi-trailer from the plurality of point cloud data subsets; Calculating the position of the connecting mechanism on the semi-trailer relative to the radar device according to the selected point cloud data subset; According to the relative position information between the radar device and the rear axle center of the tractor, converting the position of the connecting mechanism relative to the radar device into the position of the connecting mechanism relative to the rear axle center of the tractor; Taking the position of the connecting mechanism relative to the center of the rear axle of the tractor as the end point of the reversing route, driving the tractor to reverse to the end point according to the reversing route; The safety pin on the driving tractor is inserted into the pin hole on the semi-trailer, or the safety pin on the driving semi-trailer is inserted into the pin hole on the tractor.

9. The automatic docking method according to claim 8, characterized in that: The reverse route is obtained by the following steps: The radar device scans the three-dimensional point cloud data within a preset angle range behind the tractor to obtain a three-dimensional point set P, where p i =(x i ,y i ,z i ) T ,p i ∈P; Using n-1 planes parallel to the current ground where the semi-trailer is located, the three-dimensional point set P is divided into n subsets P j ,j=1,…,n; For each subset P j Determine whether the point cloud data contains the preset front shape features of the semi-trailer. If so, count the number of valid laser data points that meet the features and evaluate the subset P by the following formula: j Matching degree: S j =N′ j / N j , where S j For the subset P j The matching degree, N j For the subset P j The number of data points, N′ j P j The number of valid laser data points in ; Select the matching degree S j The highest subset P j , according to the subset P j Calculate the position R′ of the connecting mechanism on the semi-trailer relative to the radar device r =(x r ,y r ,θ r ) T , including: according to the distribution characteristics of the connecting mechanism on the semi-trailer, from the subset P j Determine the point cloud data p corresponding to the connection mechanism obj =(x obj ,y obj ,z obj ) T ;x r =x obj ,y r =y obj , and according to the point cloud data p obj And trigonometric function relationship to determine θ r : Among them, x r is the offset distance between the connecting mechanism and the radar device in the x-axis direction, y r is the offset distance between the connection mechanism and the radar device in the y-axis direction, θ r is the deflection angle of the vector between the radar device and the connecting mechanism relative to the plane of the xy coordinate system; The position of the connecting mechanism relative to the rear axle center of the tractor is calculated based on the relative position information between the radar device and the rear axle center of the tractor. in, The coordinate transformation between the xy coordinate system and the tractor rear axle center coordinate system; R r The calculated result is used as the end point of the reversing route.

10. An unmanned automatic docking system, characterized in that: The invention comprises a tractor, a semitrailer and an automatic docking device for the semitrailer as claimed in any one of claims 1 to 7.

Images