Automatic backing up hitching method, system and device based on rgb camera

Abstract

The application provides an automatic trailer hitching method, system and device based on an RGB camera, wherein the method comprises the following steps: obtaining the position information of the set hitching key points in the RGB image through a target detection algorithm based on a deep network to obtain a target detection result; tracking the target detection result through a target tracking algorithm to obtain a two-dimensional target tracking result; restoring the target tracking result to a three-dimensional space position through a least square optimization algorithm; planning the path of the towing vehicle through a path planning algorithm according to the current pose of the towing vehicle and the three-dimensional space position to obtain a planned path; and calculating the control amount of the towing vehicle through a tracking control algorithm to control the towing vehicle to the target point according to the planned path. The application can accurately obtain the position of the hitching point, realize low-cost and high-success-rate automatic trailer hitching, and has important significance for improving the efficiency of the towing unmanned vehicle and the unmanned flow operation, realizing the full-process automation and ensuring the safety.

Description

Technical Field

[0001] The present invention belongs to the field of unmanned driving technology, and specifically relates to an automatic reverse hooking method, system and device based on an RGB camera, as well as a corresponding computer control terminal and computer-readable storage medium. Background Art

[0002] With the continuous development of intelligent and information technologies, the demand for smart factories and unmanned logistics is imminent. The ultimate goal of unmanned logistics is to achieve fully automated operations, thereby ensuring safety and improving efficiency. Currently, the basic modules of unmanned logistics vehicles, such as vehicle perception, positioning, decision-making, planning, and control, have matured, and manufacturers have viable solutions. However, manual attachment methods are still used for trailer attachment, where the trailer is manually towed and aligned with the rear receptacle of the tractor. Using a tractor to tow a trailer can save energy and improve logistics efficiency, making it a common method of transportation in logistics factories. However, current manual attachment methods are inefficient and dangerous, hindering the full automation of unmanned logistics.

[0003] After searching, we found:

[0004] The Chinese invention patent application "An automatic reversing and hanging system and method based on TOF camera" with application number 202110265840.0 and application date 2021-03-11 discloses an automatic reversing and hanging system and method based on TOF camera, the system including: an unmanned vehicle, a rearview TOF camera, a connection detection device and a main control device; the rearview TOF camera is arranged behind the unmanned vehicle for realizing the acquisition of rearview depth image; the connection detection device is arranged at the to-be-towed position of the vehicle to be towed and / or the towing position of the unmanned vehicle for realizing connection detection; the main control device is used to receive the rearview depth image signal and the connection detection signal, and identify the position to be towed and the towing position according to the rearview depth image signal, set the reversing path of the unmanned vehicle, so as to realize the reversing of the unmanned vehicle and prompt the towed position of the vehicle to be towed and the towing of the unmanned vehicle to be automatically connected; it is also used to identify whether the connection between the towed vehicle and the towing is successful according to the connection detection signal. However, this system and method uses a TOF camera, whose depth channel has large noise and is prone to failure under outdoor lighting, and its application scenarios are limited.

[0005] With the advancement of computer vision technology, the accuracy of image object detection continues to improve. Current mainstream object detection techniques often utilize deep neural networks. These networks are first trained using datasets with annotated ground truth values. The trained networks can then be used to perform real-time network inference on embedded devices. Current object detection algorithms can achieve accuracy comparable to that of human experts. Furthermore, pure vision sensors are relatively low-cost and suitable for mass production. However, existing object detection methods using visual sensors typically produce two-dimensional information, which is unstable and cannot be directly used to control a tractor for reverse hooking. Summary of the Invention

[0006] In view of the above-mentioned deficiencies in the prior art, the present invention provides a method, system and device for automatic reverse hooking based on an RGB camera, and also provides a corresponding computer control terminal and computer-readable storage medium.

[0007] According to a first aspect of the present invention, a method for automatic reverse hooking based on an RGB camera is provided, comprising:

[0008] A target detection algorithm based on a deep network is used to obtain the position information of the key points of the trailer in the RGB image and obtain the target detection result;

[0009] Tracking the target detection result using a target tracking algorithm to obtain a two-dimensional target tracking result;

[0010] Restoring the target tracking result to a three-dimensional spatial position;

[0011] Planning a path for the tractor using a path planning algorithm according to the current posture of the tractor and the three-dimensional spatial position to obtain a planned path;

[0012] A tracking control algorithm is used to calculate the control quantity of the tractor, and the tractor is controlled to the target point according to the planned path.

[0013] Preferably, the setting of the position information of the trailer key points includes: the position of the left key point of the trailer, the position of the right key point of the trailer and the position of the trailer hook key point;

[0014] Preferably, tracking the target detection result by using a target tracking algorithm includes:

[0015] The Kalman filter method is used to predict the position information of the key points of the trailer in the next frame using a uniform motion model to obtain the predicted position information;

[0016] The sparse optical flow tracking method is used to track and observe the position information of the set trailer key points in the next frame to obtain the observed position information;

[0017] The Kalman filtering method is used to fuse the target detection result, predicted position information and observed position information to obtain the final target tracking result.

[0018] Preferably, a least squares optimization algorithm is used to restore the target tracking result to a three-dimensional spatial position.

[0019] Preferably, planning the tractor path using a path planning algorithm according to the tractor's current posture and the three-dimensional spatial position includes:

[0020] According to the current posture of the tractor and the three-dimensional spatial position, the Reeds-Shepp algorithm is used to plan the tractor path, with the current posture of the tractor as the starting posture and the three-dimensional spatial position of the hook key point in the three-dimensional spatial position as the end position, to obtain m continuous target point trajectories.

[0021] Preferably, the method of calculating the control amount of the tractor by using a tracking control algorithm and controlling the tractor to the target point according to the planned path includes:

[0022] A pure tracking control algorithm is used to calculate the control variable of the tractor and control the tractor to the target point according to the planned path; wherein:

[0023] The speed of automatic reverse hooking is initially constant at -1m / s. When it reaches the second-to-last target point, it starts to decelerate at a constant acceleration until it reaches the last target point, at which point the speed drops to 0m / s. The automatic reverse hooking action is completed and the tractor stops.

[0024] According to a second aspect of the present invention, there is provided an automatic reverse hooking system based on an RGB camera, comprising:

[0025] A key point detection module is used to obtain the position information of the trailer key points in the RGB image through a deep network-based target detection algorithm to obtain the target detection result;

[0026] A key point tracking module, which is used to track the target detection result through a target tracking algorithm to obtain a two-dimensional target tracking result;

[0027] A three-dimensional information acquisition module, which is used to restore the target tracking result to a three-dimensional spatial position;

[0028] A path planning module is used to plan the path of the tractor using a path planning algorithm according to the current posture of the tractor and the three-dimensional spatial position to obtain a planned path;

[0029] The tractor control module uses a tracking control algorithm to calculate the control quantity of the tractor and controls the tractor to the target point according to the planned path.

[0030] According to a third aspect of the present invention, an automatic reverse hooking device based on an RGB camera is provided, comprising: an RGB camera mounted on the rear side of a tractor, an IMU sensor mounted on the center of the tractor axle, an embedded controller, and a tractor front wheel angle and speed controller; wherein:

[0031] The RGB camera is used to obtain a real-time RGB image containing the trailer;

[0032] The IMU sensor is used to obtain the real-time acceleration and angular velocity of the tractor;

[0033] The embedded controller performs key point detection, key point tracking, three-dimensional information acquisition and path planning based on the RGB image;

[0034] The tractor front wheel angle and speed controller is used to control the tractor to the target point according to the planned path.

[0035] According to a fourth aspect of the present invention, there is provided a computer control terminal comprising a memory, a processor and a computer program stored in the memory and executable on the processor, wherein the processor, when executing the computer program, can be used to execute any one of the methods described above in the present invention, or to execute the system described above in the present invention.

[0036] According to a fifth aspect of the present invention, a computer-readable storage medium is provided, on which a computer program is stored. When the computer program is executed by a processor, it can be used to perform any of the methods described above in the present invention, or to run the system described above in the present invention.

[0037] Due to the adoption of the above technical solution, the present invention has at least one of the following beneficial effects compared with the prior art:

[0038] The present invention provides an automatic reverse hooking method, system and device based on an RGB camera, which uses an RGB camera as a main sensor and applies an embedded controller to realize an automatic reverse hooking system.

[0039] The RGB camera-based automatic reversing hooking method, system, and device provided by the present invention first use a target tracking algorithm to improve the stability of the detection results, then use least squares optimization to restore the three-dimensional spatial position of the hook point, and finally combine the Reeds-Shepp path planning algorithm and the tracking control Pure-Pursuit algorithm to achieve automatic reversing hooking of the tractor.

[0040] The RGB camera-based automatic reversing and hooking method, system, and device provided by the present invention can be effectively deployed in any unmanned logistics factory, and is of great significance for the implementation of fully automated work stacks in unmanned logistics factories in the future.

[0041] The RGB camera-based automatic reversing and hooking method, system and device provided by the present invention can achieve high-precision automatic reversing and hooking, and has the advantages of low cost and good scalability. It can be applied to various existing unmanned logistics trailer systems and is of great significance to improving the automation level of unmanned logistics factories and improving work efficiency. BRIEF DESCRIPTION OF THE DRAWINGS

[0042] Other features, objects and advantages of the present invention will become more apparent upon reading the detailed description of non-limiting embodiments with reference to the following drawings:

[0043] Figure 1 The figure is a workflow diagram of an automatic reversing and hooking method based on an RGB camera in one embodiment of the present invention.

[0044] Figure 2 The figure is a schematic diagram of the components of an automatic reverse hooking system based on an RGB camera in one embodiment of the present invention.

[0045] Figure 3 Schematic diagram of the installation structure of an automatic reverse hook-up device based on an RGB camera in one embodiment of the present invention; (a) is a top view, and (b) is a side view. In the figure, 1 is the RGB camera, 2 is the IMU sensor, 3 is the embedded controller, and 4 is the front wheel angle and speed controller of the tractor.

[0046] Figure 4 It is a schematic top view of a reverse hooking task in a specific application example of the present invention.

[0047] Figure 5 The figure is a structural framework diagram of an automatic reversing and hooking system in a specific application example of the present invention.

[0048] Figure 6 The figure is a schematic diagram of a data set annotation format in a specific application example of the present invention.

[0049] Figure 7 The figure is a flow chart of a target detection and tracking algorithm in a specific application example of the present invention.

[0050] Figure 8 This is a flow chart of the hook three-dimensional position restoration and tractor planning control algorithm in a specific application example of the present invention. DETAILED DESCRIPTION

[0051] The following is a detailed description of an embodiment of the present invention. This embodiment is implemented based on the technical solution of the present invention, and provides a detailed implementation method and specific operation process. It should be noted that those skilled in the art may make various modifications and improvements without departing from the scope of the present invention, and these modifications and improvements fall within the scope of protection of the present invention.

[0052] Existing target detection systems using visual sensors typically produce two-dimensional information, which is unstable and cannot be directly used to control a tractor for reverse hooking. To address this issue, one embodiment of the present invention provides an automatic reverse hooking method based on an RGB camera. This method uses an RGB camera as the primary sensor and utilizes a target tracking algorithm, least squares optimization, path planning, and tracking control algorithms to achieve high-precision automatic reverse hooking. This method offers the advantages of low cost, good scalability, improved automation in unmanned logistics factories, and increased work efficiency.

[0053] Specifically, if Figure 1 As shown, the automatic reverse hooking method based on the RGB camera provided in this embodiment may include the following operations:

[0054] S1, obtains the position information of the trailer key points in the RGB image through a deep network-based target detection algorithm to obtain the target detection result;

[0055] S2, tracking the target detection result through a target tracking algorithm to obtain a two-dimensional target tracking result;

[0056] S3, restore the target tracking result to the three-dimensional spatial position;

[0057] S4, planning the path of the tractor using a path planning algorithm based on the current posture and three-dimensional spatial position of the tractor to obtain a planned path;

[0058] S5, using the tracking control algorithm to calculate the control quantity of the tractor, and control the tractor to the target point according to the planned path.

[0059] In some preferred embodiments, the above S1, obtaining the position information of the key points of the trailer in the RGB image by a deep network-based object detection algorithm, may further include the following operations:

[0060] S11, providing a dataset for training a deep network for object detection and annotating it, wherein the annotation format of each image in the dataset includes: the key point position of the left side of the trailer, the key point position of the right side of the trailer, the key point position of the trailer hook, and the trailer detection frame;

[0061] S12, providing a deep network, and training the deep network using a target detection loss function and a key point loss detection function to obtain a target detection network; further preferably:

[0062] The target detection loss function and key point loss detection function are:

[0063]

[0064] Among them, L obj is the target detection loss function, is the annotated true value detection box, is the predicted detection box of the network, IOU is the intersection-over-union ratio between the two predicted detection boxes; L oks is the key point loss detection function, the key points are (left, right, hitch), d n is the distance between the labeled true detection point and the network's predicted detection point, and s is the size of the detection box. The loss function is minimized through the gradient descent algorithm to obtain the optimal target detection network.

[0065] S13, using the real-time collected RGB image as the input of the target detection network, outputting the key point positions of the left side of the trailer, the right side of the trailer, and the trailer hook in real time, and restoring the three-dimensional position by solving the position information of the key points of the trailer to ensure the stability of the solution due to multiple constraints; further preferably:

[0066] (]left x ,left y ],[right x ,right y ],[hitch x ,hitch y ],[x,y,w,h])

[0067] Among them, [left x ,left t ] is the pixel position of the key point on the left side of the trailer, [right x ,right y ] is the pixel position of the key point on the right side of the trailer, [hitch x ,hitch y ] is the pixel position of the trailer hitch key point, and [x, y, w, h] is the center pixel coordinate [x, y] and width and height [w, h] of the trailer detection frame.

[0068] In some preferred embodiments, the above S1 may further include the following operations:

[0069] Convert the object detection network into the TensorRT format for real-time inference and deploy it on the embedded platform NVIDIA Jetson Nano to accelerate the network. Furthermore, it is optimized to achieve an inference speed of 50ms, meeting the requirements of real-time control.

[0070] The embedded platform is used as the controller, and the real-time collected RGB images are used as the input of the target detection network.

[0071] The key point positions of the left side of the trailer, the right side of the trailer, and the key point positions of the trailer hook obtained by S1 are independent between each frame. Due to the errors of the target detection algorithm each time, some small jumps will occur, resulting in jumps in the subsequent three-dimensional position recovery, thereby affecting the control effect. Due to the continuity of the tractor's movement, the two-dimensional key point detection positions in the image should also be continuous. Therefore, in some preferred embodiments of the present invention, two prediction algorithms, Kalman filter tracking and optical flow tracking, are used to predict the key point positions in consecutive frames, and Kalman filtering is used to fuse the results of target detection and the two tracking algorithms. Based on this, in some preferred embodiments, the above S2, which tracks the target detection results through a target tracking algorithm, can further include the following operations:

[0072] S21, using a Kalman filter method and a uniform motion model to predict the position information of the set trailer key point in the next frame to obtain predicted position information; further preferably:

[0073] The state of each key point is:

[0074]

[0075] in, is the pixel coordinate of the nth key point [x n ,y n ] and movement speed Using the Kalman filter method, using the state quantity x n Predict the key point positions in the next frame;

[0076] S22, using the sparse optical flow tracking Lucas-Kanade method to track and observe the position information of the set trailer key point in the next frame to obtain the observed position information; further preferably:

[0077] I2(x n2 ,y n2 )=I1(x n1 ,y n1 )

[0078] Among them, (x n1 ,y n1) is the key point in the current frame image, (x n2 ,y n2 ) is the key point in the next frame, I2=I1 is the brightness constant assumption;

[0079] S23, using the Kalman filter method to fuse the target detection results, the predicted position information and the observed position information to obtain the final target tracking result; further preferably:

[0080] The observation quantity of each key point is:

[0081] z n =[z xc ,z yc ] T

[0082] Among them, [z xc ,z yc ] is the target detection algorithm for the key point z n Deep network object detection results.

[0083] Using the Kalman filter method, the observation results of S21, S22 and the target detection algorithm are weighted and combined. First, the results of S21 are used to make predictions using the uniform motion model:

[0084]

[0085] in, To obtain the predicted result, we use the optical flow result of S22 and Kalman filter to update the state:

[0086]

[0087] Among them, [x ni ,y ni ] T is the result after optical flow observation update, K I is the Kalman gain of the optical flow observation, and then the observation results of the target detection algorithm are used to update the state using the Kalman filter:

[0088]

[0089] Among them, K O is the Kalman gain of the target detection algorithm, and the final target tracking result is expressed as:

[0090] kp n =[x ns ,y ns ]

[0091] Among them, [x ns ,y ns] represents the nth key point kp n The pixel coordinates after target tracking are the final target tracking results.

[0092] In some preferred embodiments, the above S3, restoring the target tracking result to a three-dimensional spatial position, may further include the following operations:

[0093] S31, establishing an optimization objective function that minimizes the reprojection error based on the internal parameter K and the external parameter T of the RGB camera, the real space height of the trailer key point, and the two-dimensional pixel coordinates of the trailer key point in the image; further preferably:

[0094] The internal parameter K is obtained by the checkerboard-Zhang Zhengyou calibration method, and the external parameter T is obtained by the onboard IMU through dead reckoning. The real space height of the trailer key point (left z ,right z ,hitch z ) is obtained from the trailer technical manual; the two-dimensional pixel coordinates of the trailer key points in the image are set as (kp left ,kp right ,kp hitch ), we can set 6 unknown quantities (left x ,left y ,right x ,right y ,hitch x ,hitch y ), the optimization objective function P that minimizes the reprojection error is established as:

[0095]

[0096] Among them, L opt is the reprojection error objective function, x np ,y np is the projected pixel coordinate of the nth key point, x ns ,y ns Indicates the final target tracking result;

[0097] S32, using a least squares optimization algorithm to solve the optimization objective function and obtain the real three-dimensional coordinates of the key points of the trailer; further preferably:

[0098] The real three-dimensional coordinates of the key points of the trailer are obtained as follows:

[0099]

[0100] in, The three-dimensional coordinate results of the key points of the trailer are obtained by the least squares optimization method.

[0101] In some preferred embodiments, the above S4, planning the path of the tractor using a path planning algorithm based on the current posture and three-dimensional spatial position of the tractor, may further include the following operations:

[0102] According to the current posture and three-dimensional spatial position of the tractor, the Reeds-Shepp algorithm is used to plan the tractor path, with the current posture of the tractor as the starting posture and the three-dimensional spatial position of the hook key point in the three-dimensional spatial position as the end position, to obtain m continuous target point trajectories; further preferably:

[0103] The m continuous target points obtained are:

[0104]

[0105] Among them, (ego x ,ego y ,ego yaw ) is the current position of the tractor, obtained by the dead reckoning algorithm; (hitch xq ,hitch yq ) is the three-dimensional spatial position of the hook key point, obtained by the least squares optimization algorithm; RS is the Reeds-Shepp algorithm, (path xi ,path yi ) is the i-th path point obtained by the Reeds-Shepp algorithm; further preferably, the value of m is 20.

[0106] In some preferred embodiments, the above S5, which uses a tracking control algorithm to calculate the control amount of the tractor and controls the tractor to the target point according to the planned path, may further include the following operations:

[0107] A Pure-Pursuit control algorithm is used to control the tractor to the target point according to the planned path; further preferably:

[0108] The control quantity is calculated using the tracking control Pure-Pursuit algorithm as follows:

[0109] δ=PP(ego x ,ego y ,ego yaw ,path xi ,path yi )

[0110] Among them, PP is the tracking control Pure-Pursuit algorithm, δ is the tractor angle, (ego x ,ego y ,ego yaw) is the current position of the tractor, (path xi ,path yi ) is the current target point. When the tractor reaches the current target point, it switches to the next target point (path xi+1 ,path yi+1 );

[0111] The speed of automatic reverse hooking is initially constant at -1m / s, and reaches (path x19 ,path y19 ) starts to decelerate at a constant acceleration until it reaches the last target point (path x20 ,path y20 ), the speed drops to 0m / s, the automatic reverse hooking action is completed, and the tractor stops.

[0112] An embodiment of the present invention provides an automatic reverse hooking system based on an RGB camera.

[0113] Specifically, if Figure 2 As shown, the automatic reverse hook system based on RGB camera provided in this embodiment may include the following modules:

[0114] A key point detection module is used to obtain the position information of the trailer key points in the RGB image through a deep network-based target detection algorithm to obtain the target detection result;

[0115] A key point tracking module is used to track the target detection result through a target tracking algorithm to obtain a two-dimensional target tracking result;

[0116] A three-dimensional information acquisition module, which is used to restore the target tracking results to a three-dimensional spatial position;

[0117] The path planning module is used to plan the path of the tractor according to the current posture and three-dimensional spatial position of the tractor using a path planning algorithm to obtain a planned path;

[0118] The tractor control module uses a tracking control algorithm to calculate the control quantity of the tractor and control the tractor to the target point according to the planned path.

[0119] It should be noted that the steps in the method provided by the present invention can be implemented using corresponding modules in the system, and those skilled in the art can refer to the technical solution of the method to implement the composition of the system. That is, the embodiments in the method can be understood as preferred examples of constructing the system, which will not be elaborated here.

[0120] The specific working contents of each functional module in the system provided by the above embodiment of the present invention are further described in detail below in conjunction with some preferred implementation modes.

[0121] In some preferred embodiments, the key point detection module further includes:

[0122] Annotate the dataset used to train the deep network for object detection. The annotation format of each image includes the key point position of the left side of the trailer, the key point position of the right side of the trailer, the key point position of the trailer hook, and the trailer detection box.

[0123] Use the target detection loss function and the key point loss detection function to train the deep network:

[0124] Input the real-time collected RGB image and output the key point positions of the left side of the trailer, the right side of the trailer, and the key point positions of the trailer hook in real time.

[0125] It may further preferably include:

[0126] Deploy the trained object detection network to the embedded platform NVIDIA Jetson Nano to accelerate the network and meet real-time control requirements;

[0127] The embedded platform is used as the controller and the real-time collected RGB images are input.

[0128] In some preferred embodiments, the key point tracking module further includes:

[0129] Use the Kalman filter method and the uniform motion model to predict the key point position in the next frame;

[0130] Use sparse optical flow tracking method to track and observe the key point positions in the next frame;

[0131] The Kalman filter method is used to fuse the results of target detection and the two tracking algorithms to output the final tracking result.

[0132] In some preferred embodiments, the three-dimensional information acquisition module further includes:

[0133] Based on the internal parameters K and external parameters T of the RGB camera, the real space height of the three key points of the trailer, and the two-dimensional pixel coordinates of the three key points of the trailer in the image, an optimization objective function is established to minimize the reprojection error;

[0134] The least squares optimization method is used to solve the objective function of minimizing the reprojection error and obtain the true three-dimensional coordinates of the three key points of the trailer.

[0135] In some preferred embodiments, the path planning module further includes:

[0136] According to the current posture of the tractor and the three-dimensional spatial position of the target hook key point, the Reeds-Shepp algorithm is used to plan the tractor path and obtain a continuous target point trajectory.

[0137] In some preferred embodiments, the above-mentioned tractor control module further includes the following working contents:

[0138] The Pure-Pursuit tracking control algorithm is used to control the tractor to the target point along the planned path.

[0139] An embodiment of the present invention provides an automatic reverse hooking device based on an RGB camera.

[0140] Specifically, if Figure 3 As shown in (a) and (b) of FIG. 1 , the RGB camera-based automatic reverse hooking device provided in this embodiment may include: an RGB camera installed on the rear side of a tractor, an IMU sensor installed at the center of the tractor axle, an embedded controller, and a tractor front wheel angle and speed controller; wherein:

[0141] RGB camera, used to obtain real-time RGB images including the trailer;

[0142] IMU sensor, used to obtain the real-time acceleration and angular velocity of the tractor;

[0143] Embedded controller, performs key point detection, key point tracking, 3D information acquisition and path planning based on RGB images;

[0144] The tractor's front wheel steering angle and speed controller is used to control the tractor to the target point according to the planned path.

[0145] It should be noted that the steps in the method provided by the present invention can be implemented using corresponding components in the device, and those skilled in the art can refer to the technical solution of the method to implement the composition of the device. That is, the embodiments in the method can be understood as preferred examples of constructing the device, which will not be elaborated here.

[0146] The following, combined with a specific application example and accompanying drawings, further clearly and completely describes the RGB camera-based automatic reverse hooking method, system, and device provided by the above-mentioned embodiment of the present invention. Obviously, the described embodiment is only a part of the embodiments of the present invention, not all of them. All other embodiments derived by persons of ordinary skill in the art based on the embodiments of the present invention without inventive effort are within the scope of protection of the present invention.

[0147] like Figure 4The figure shows a schematic diagram of the reverse hooking task in the present invention, which depicts the process of the tractor (powered front vehicle) engaging its rear latch with the trailer (unpowered rear vehicle) by reversing. A successful automatic reverse hooking operation means that the tractor latch is successfully aligned with the trailer hook at the end of the reverse operation, allowing the tractor to tow the trailer.

[0148] The method flow chart of the embodiment of the present invention is as follows Figure 5 shown.

[0149] The above method comprises the following steps:

[0150] Step 1: Obtain the position information of the set key points (key points on the left side of the trailer, key points on the right side of the trailer, and key points on the trailer hook) in the RGB image through the target detection algorithm based on the deep network;

[0151] Step 2: Improve the stability of target detection results by using a target tracking algorithm based on Kalman filter tracking and optical flow;

[0152] Step 3: Use the least squares optimization algorithm to restore the two-dimensional image detection hook key points to the real three-dimensional space position;

[0153] Step 4: Based on the current posture of the tractor and the three-dimensional spatial position of the target hook key point, the Reeds-Shepp algorithm is used to plan the tractor path;

[0154] Step 5: Use the tracking control Pure-Pursuit algorithm to control the tractor to the target point along the planned path.

[0155] First, perform step 1 to obtain the position information of the set key points in the RGB image through the target detection algorithm based on the deep network.

[0156] Step 1a: Annotate the dataset for training the deep network for object detection. The annotation format of each image includes the key point position of the left side of the trailer, the key point position of the right side of the trailer, the key point position of the trailer hook, and the trailer detection frame, such as Figure 6 As shown:

[0157] ([left x ,left y ],[right x ,right y ],[hitch x ,hitch y ],[x,y,w,h])

[0158] Where [left x ,left y ] is the pixel position of the key point on the left side of the trailer, [rightx ,right y ] is the pixel position of the key point on the right side of the trailer, [hitch x ,hitch y ] is the pixel position of the trailer hitch key point, and [x, y, w, h] is the center pixel coordinate [x, y], width and height [w, h] of the trailer detection frame.

[0159] Step 1b: Train the detection deep network. Use the YOLOv8 target detection network for training. The loss function includes target detection loss and key point loss:

[0160]

[0161] Among them L obj is the target detection loss, is the annotated true value detection box, is the predicted detection box of the network, IOU is the intersection-over-union ratio between the two predicted detection boxes; L oks is the key point loss, the three key points are (left, right, hitch), d n is the distance between the annotated true detection point and the network's predicted detection point, and s is the size of the detection box. The loss function is minimized through the gradient descent algorithm to obtain the optimal object detection network.

[0162] In step 1c, the trained object detection network is ported to the embedded platform NVIDIA Jetson Nano. The network obtained in step 1b is converted into the TensorRT format for real-time inference and deployed to the embedded platform to meet real-time control requirements.

[0163] In step 1d, the embedded platform is used as the controller, the real-time collected RGB image is input, and the key points on the left side of the trailer, the right side of the trailer, and the trailer hook are output in real time:

[0164] ([ldft x ,left y ],[right x ,right y ],[hitch x ,hitch y ])

[0165] Where [left x ,left y ] is the key point position on the left side of the trailer, [right x ,right y ] is the key point position on the right side of the trailer, [hitch x ,hitchy ] is the key point position of the trailer hook.

[0166] The next step is to proceed to step 2, which improves the stability of target detection results by tracking the target based on Kalman filter tracking and optical flow. The schematic diagram of this process is as follows Figure 7 shown.

[0167] The key point positions of the left, right, and hook of the trailer obtained in step 1 are independent from frame to frame. Due to errors in the target detection algorithm, slight jumps may occur, leading to jumps in the subsequent three-dimensional position recovery, thus affecting the control effect. Due to the continuity of the tractor's motion, the two-dimensional key point detection positions in the image should also be continuous. Therefore, this embodiment of the present invention uses two prediction algorithms, Kalman filtering and optical flow tracking, to predict the key point positions in consecutive frames. Kalman filtering is then used to fuse the results of the target detection and tracking algorithms.

[0168] Step 2a: Use the Kalman filter method and the uniform motion model to predict the key point position in the next frame. The state of each key point is:

[0169]

[0170] in, is the pixel coordinate of the nth key point [x n ,y n ] and movement speed Using the Kalman filter method, using the state quantity x n Predict the keypoint positions in the next frame.

[0171] In step 2b, the Lucas-Kanade method is used to track the key point positions in the next frame.

[0172] I2(x n2 ,y n2 )=I1(x n1 ,y n1 )

[0173] Among them, (x n1 ,y n1 ) is the key point in the current frame image, (x n2 ,y n2 ) is the key point of the next frame image, and I2=I1 is the brightness constant assumption.

[0174] Step 2c: Use the Kalman filter method to fuse the results of target detection and the two tracking algorithms to obtain the stable key point positions of the left side of the trailer, the right side of the trailer, and the trailer hook. The observation value of each key point is:

[0175] z n =[z xc ,z yc ] T

[0176] Among them, [z xc ,z yc ] is the target detection algorithm for the key point z n Deep network object detection results.

[0177] Using Kalman filtering, the observation results of step 2a, step 2b and step 2c are weighted and combined. The final target tracking result is expressed as:

[0178] kp n =[x ns ,y ns ]

[0179] Among them, [x ns ,y ns ] represents the nth key point kp n The pixel coordinates of the target after tracking.

[0180] In step 2, the state quantities are the position and speed of the key points on the left side of the trailer, the position and speed of the key points on the right side of the trailer, and the position and speed of the key points of the trailer hook, which are predicted using a uniform motion model; the observation quantities are the position of the key points on the left side of the trailer, the position of the key points on the right side of the trailer, and the position of the key points of the trailer hook, and the two observation methods are target detection algorithm and optical flow tracking algorithm.

[0181] The next step is to proceed to step 3, using the least squares optimization algorithm to restore the two-dimensional image detection hook key points to the real three-dimensional space position, such as Figure 8 shown.

[0182] The stable key point target tracking result obtained in step 2 is two-dimensional information and cannot be directly used for tractor control. Therefore, the embodiment of the present invention uses the least squares optimization method to restore the three-dimensional spatial information of the hook key point, which is then used for tractor control.

[0183] Step 3a, based on the internal parameters K of the RGB camera (calibrated by the checkerboard-Zhang Zhengyou calibration method), the external parameters T (obtained by the onboard IMU through dead reckoning), and the real space height of the three key points of the trailer (obtained from the trailer technical manual) (left z ,right z ,hitchz ), the two-dimensional pixel coordinates of the three key points of the trailer in the image (kp left ,kp right ,kp hitch ), we can set 6 unknown quantities (left x ,left y ,right x ,right y ,hitch x ,hitch y ), establish the optimization objective function that minimizes the reprojection error:

[0184]

[0185] Among them L opt is the reprojection error objective function, x np ,y np is the projected pixel coordinate of the nth key point.

[0186] In step 3b, the least squares optimization method is used to solve the six unknowns in the optimization objective function obtained in step 3a to obtain the true three-dimensional coordinates of the three key points of the trailer:

[0187]

[0188] in The three-dimensional coordinates of the three key points of the trailer are obtained by the least squares optimization method.

[0189] In step 3, the two-dimensional image detection hook key points are restored to the real three-dimensional space position process, the real space height (z-axis coordinate) of the three key points of the trailer is set as a known quantity, and the x and y coordinates of the three key points of the trailer are set as 6 unknown quantities. The 6 unknown quantities are solved and the method of minimizing the reprojection error is used to minimize the distance between the projection of the three key points of the trailer in the pixel coordinate system and the target tracking results of the three key points in S2.

[0190] The next step is to proceed to step 4. According to the current posture of the tractor and the three-dimensional spatial position of the target hook key point, the Reeds-Shepp algorithm is used to plan the tractor path, such as Figure 8 shown.

[0191] Step 4: Using the tractor's current position as the starting position and the three-dimensional spatial position of the hook key point as the end position, the Reeds-Shepp algorithm is used to plan the tractor's path. The result is 20 consecutive target points:

[0192]

[0193] Among them (ego x,ego y ,ego yaw ) is the current position of the tractor (obtained by the dead reckoning algorithm), (hitch xq ,hitch yq ) is the three-dimensional spatial position of the hook key point (obtained by the least squares optimization algorithm), RS is the Reeds-Shepp algorithm, (path xi ,path yi ) is the i-th path point obtained by the Reeds-Shepp algorithm.

[0194] The next step is to proceed to step 5, using the tracking control Pure-Pursuit method to control the tractor to the target point according to the planned path, such as Figure 8 shown.

[0195] Step 5: In step 4, the 20 consecutive target points of the tractor planned by the Reeds-Shepp algorithm are obtained. The control variables are calculated using the Pure-Pursuit tracking control algorithm:

[0196] δ=PP(ego x ,ego y ,ego yaw ,path xi ,path yi )

[0197] Where PP is the tracking control Pure-Pursuit algorithm, δ is the tractor angle, (ego x ,ego y ,ego yaw ) is the current position of the tractor, which is obtained by IMU dead reckoning, (path xi ,path yi ) is the current target point. When the tractor reaches the current target point, it switches to the next target point (path xi+1 ,path yi+1 The speed of the automatic reverse hook is initially constant at -1m / s, and reaches (path x19 ,path y19 ) starts to decelerate at a constant acceleration until it reaches the last target point (path x20 ,path y20 ), the speed drops to 0m / s, the automatic reverse hooking action is completed, and the tractor stops.

[0198] In the process of controlling the tractor to the target point along the planned path, the tracking control algorithm is used to control the tractor for the continuous target points obtained. The turning angle is obtained by the tracking control algorithm, and the speed is to reverse at a constant speed first, decelerate uniformly when reaching the second to last target point, and stop when reaching the last target point to complete the automatic reversing and hooking task.

[0199] The above is the entire process of the embodiment of the present invention.

[0200] An embodiment of the present invention also provides a computer control terminal, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, the processor can be used to execute any one of the methods described in the above embodiments of the present invention, or to execute any one of the systems described in the above embodiments of the present invention.

[0201] Optionally, the memory is used to store programs; the memory may include volatile memory (English: volatile memory), such as random-access memory (English: random-access memory, abbreviated: RAM), such as static random-access memory (English: static random-access memory, abbreviated: SRAM), double data rate synchronous dynamic random access memory (English: Double Data Rate Synchronous Dynamic Random Access Memory, abbreviated: DDR SDRAM), etc.; the memory may also include non-volatile memory (English: non-volatile memory), such as flash memory (English: flash memory). The memory is used to store computer programs (such as applications, functional modules, etc. that implement the above-mentioned methods), computer instructions, etc., and the above-mentioned computer programs, computer instructions, etc. can be partitioned and stored in one or more memories. In addition, the above-mentioned computer programs, computer instructions, data, etc. can be called by the processor.

[0202] The aforementioned computer programs, computer instructions, etc. may be partitioned and stored in one or more memories, and the aforementioned computer programs, computer instructions, data, etc. may be called by a processor.

[0203] The processor is configured to execute the computer program stored in the memory to implement the various steps of the method or various modules of the system involved in the above embodiments. For details, please refer to the relevant descriptions in the above method, system and device embodiments.

[0204] The processor and memory can be independent structures or integrated structures. When the processor and memory are independent structures, the memory and processor can be coupled via a bus.

[0205] An embodiment of the present invention further provides a computer-readable storage medium having a computer program stored thereon. When the computer program is executed by a processor, it can be used to execute any method of the above embodiments of the present invention, or to run any system of the above embodiments of the present invention.

[0206] In the above embodiment of the present invention, in order to solve the problem of automatic reversing and hooking in unmanned logistics factories, an automatic reversing and hooking method, system and device based on RGB camera are proposed. The above method and system can use relatively mature target detection algorithms to detect the position of hook key points in the image, and perform target tracking to improve detection stability. The least squares optimization method is used to restore the three-dimensional space point of the hook. Then, the Reeds-Shepp algorithm is used to plan the tractor path to obtain the target trajectory of the tractor. The tracking control Pure-Pursuit algorithm is used for the final control of the tractor to achieve the ultimate goal of automatic reversing and hooking. The method and system have the characteristics of low cost and good universality, can be easily transplanted to all existing unmanned logistics systems, can efficiently realize automatic reversing and hooking, thereby improving the work efficiency of unmanned logistics factories, ensuring safety, and ultimately realizing full-process automated operations.

[0207] The above-mentioned embodiments of the present invention provide an automatic reversing and hooking method, system, and device based on an RGB camera. The RGB camera is used as the main sensor to obtain the trailer hook position, and then the tractor is controlled to automatically complete the reversing and hooking task. The RGB camera installed at the rear of the tractor captures an RGB image containing the trailer hook position, and the hook key point is detected by a deep network-based target detection algorithm. Kalman filter tracking and optical flow tracking are performed on the key point results of the target detection to improve the stability of the hook point detection. According to the internal parameters of the RGB camera, the hook key point position in the dedistorted image is obtained, and the three-dimensional spatial position of the hook point relative to the tractor is obtained by the least squares optimization method. The three-dimensional position of the hook point is used as the target point, and the Reeds-Shepp algorithm is used for path planning to obtain the driving path. The tracking control Pure-Pursuit algorithm is used to control the tractor to the target point to achieve the purpose of automatic reversing and hooking. The RGB camera-based automatic reversing and hooking method, system, and device provided in the above-mentioned embodiments of the present invention propose for the first time the use of an RGB camera to detect key hooking points for automatic reversing and hooking. By using visual detection and tracking technology, the position of the hooking point can be obtained more accurately. In addition, the pure visual solution has a low cost and can achieve low-cost, high-success-rate automatic reversing and hooking, which is of great significance for improving the efficiency of unmanned logistics operations of trailer-type unmanned vehicles, realizing full-process automation, and ensuring safety.

[0208] Matters not mentioned in the above embodiments of the present invention are well known in the art.

[0209] The above describes the specific embodiments of the present invention. It should be understood that the present invention is not limited to the above specific embodiments, and those skilled in the art may make various variations or modifications within the scope of the claims, which do not affect the essence of the present invention.

Claims

1. An automatic reversing hooking method based on RGB camera, characterized in that: include: A target detection algorithm based on a deep network is used to obtain the position information of the trailer key points in the RGB image and obtain the target detection result; Tracking the target detection result using a target tracking algorithm to obtain a two-dimensional target tracking result; Restoring the target tracking result to a three-dimensional spatial position; Planning a path for the tractor using a path planning algorithm according to the current posture of the tractor and the three-dimensional spatial position to obtain a planned path; A tracking control algorithm is used to calculate the control quantity of the tractor, and the tractor is controlled to the target point according to the planned path; in: Tracking the target detection result by a target tracking algorithm includes: The Kalman filter method is used to predict the position information of the key points of the trailer in the next frame using a uniform motion model to obtain the predicted position information; The sparse optical flow tracking method is used to track and observe the position information of the set trailer key points in the next frame to obtain the observed position information; The Kalman filtering method is used to fuse the target detection result, predicted position information and observed position information to obtain the final target tracking result.

2. The automatic reverse hooking method based on RGB camera according to claim 1 is characterized in that: The method of obtaining the position information of the trailer key points in the RGB image by using a deep network-based target detection algorithm includes: Provide a dataset for training a deep object detection network and annotate it. Each image in the dataset is annotated with the following format: the location of the left key point of the trailer, the location of the right key point of the trailer, the location of the key point of the trailer hitch, and the trailer detection box. Providing a deep network, and training the deep network using an object detection loss function and a key point loss detection function to obtain an object detection network; The real-time collected RGB image is used as the input of the target detection network, and the key point positions of the left side of the trailer, the right side of the trailer, and the trailer hook are output in real time; Also includes any one or more of the following: -Convert the object detection network into the TensorRT format that enables real-time inference and deploy it to an embedded platform; use the embedded platform as a controller and the real-time RGB images as input to the object detection network; -Using gradient descent algorithm to optimize the object detection network.

3. The automatic reverse hooking method based on RGB camera according to claim 1 is characterized in that: Restoring the target tracking result to a three-dimensional spatial position includes: According to the internal parameters K and external parameters T of the RGB camera, the real space height of the trailer key points and the two-dimensional pixel coordinates of the trailer key points in the image, an optimization objective function is established to minimize the reprojection error; The least squares optimization algorithm is used to solve the optimization objective function to obtain the real three-dimensional coordinates of the key points of the trailer.

4. The automatic reversing and hooking method based on RGB camera according to claim 1 is characterized in that: The method of planning a path for the tractor using a path planning algorithm based on the current posture of the tractor and the three-dimensional spatial position includes: According to the current posture of the tractor and the three-dimensional spatial position, the Reeds-Shepp algorithm is used to plan the tractor path, with the current posture of the tractor as the starting posture and the three-dimensional spatial position of the hook key point in the three-dimensional spatial position as the end position, to obtain m continuous target point trajectories.

5. The automatic reverse hooking method based on RGB camera according to claim 1, characterized in that: The method of calculating the control amount of the tractor by using a tracking control algorithm and controlling the tractor to the target point according to the planned path includes: A pure tracking control algorithm is used to calculate the control variable of the tractor and control the tractor to the target point according to the planned path; wherein: The speed of automatic reverse hooking is initially constant at -1m / s. When it reaches the second-to-last target point, it starts to decelerate at a constant acceleration until it reaches the last target point, at which point the speed drops to 0m / s. The automatic reverse hooking action is completed and the tractor stops.

6. An automatic reverse hooking system based on RGB camera, characterized in that: include: A key point detection module is used to obtain the position information of the trailer key points in the RGB image through a deep network-based target detection algorithm to obtain the target detection result; A key point tracking module, which is used to track the target detection result through a target tracking algorithm to obtain a two-dimensional target tracking result; A three-dimensional information acquisition module, which is used to restore the target tracking result to a three-dimensional spatial position; A path planning module is used to plan the path of the tractor using a path planning algorithm according to the current posture of the tractor and the three-dimensional spatial position to obtain a planned path; A tractor control module, which uses a tracking control algorithm to calculate the control quantity of the tractor and controls the tractor to the target point according to the planned path; in: The key point tracking module tracks the target detection result using a target tracking algorithm, including: The Kalman filter method is used to predict the position information of the key points of the trailer in the next frame using a uniform motion model to obtain the predicted position information; The sparse optical flow tracking method is used to track and observe the position information of the set trailer key points in the next frame to obtain the observed position information; The Kalman filtering method is used to fuse the target detection result, predicted position information and observed position information to obtain the final target tracking result.

7. The automatic reverse hooking system based on RGB camera according to claim 6, characterized in that: Also includes: An RGB camera installed on the rear side of the tractor, an IMU sensor installed at the center of the tractor axle, an embedded controller, and a tractor front wheel angle and speed controller; The RGB camera is used to obtain a real-time RGB image including the trailer; The IMU sensor is used to obtain the real-time acceleration and angular velocity of the tractor; The embedded controller is used to arrange a key point detection module, a key point tracking module, a three-dimensional information acquisition module and a path planning module, and perform key point detection, key point tracking, three-dimensional information acquisition and path planning based on the RGB image; The tractor front wheel angle and speed controller is used to control the tractor to the target point according to the planned path.

8. A computer control terminal comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that: When executing the computer program, the processor can be used to perform the method according to any one of claims 1 to 5, or to run the system according to any one of claims 6 to 7.

9. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, it can be used to perform the method according to any one of claims 1 to 5, or to run the system according to claims 6 to 7.

Images