Automatic reversing hanging method, system and device based on RGB camera

Abstract

The invention provides an automatic backing-up hanging method, system and device based on an RGB camera, and the method comprises the steps: obtaining the position information of a set trailer key point in an RGB image through a target detection algorithm based on a deep network, and obtaining a target detection result; tracking the target detection result through a target tracking algorithm to obtain a two-dimensional target tracking result; adopting a least square optimization algorithm to restore a target tracking result to a three-dimensional space position; according to the current pose and the three-dimensional space position of the tractor, a path planning algorithm is adopted to plan the path of the tractor, and a planned path is obtained; and calculating the control quantity of the tractor by adopting a tracking control algorithm, and controlling the tractor to a target point according to the planned path. According to the invention, the position of the hooking point can be accurately obtained, low-cost and high-success-rate automatic backing and hooking are realized, and the method has important significance for improving the unmanned logistics operation efficiency of the pull type unmanned vehicle, realizing full-process automation and ensuring the safety.

Description

Technical Field

[0001] The present invention belongs to the field of unmanned driving technology, and in particular, relates to an automatic reversing and hanging method, system and device based on an RGB camera, and also relates to a corresponding computer control terminal and a computer-readable storage medium. Background Art

[0002] With the continuous development of intelligent technology and information technology, the demand for smart factories and unmanned logistics is imminent. The ultimate goal of unmanned logistics is to achieve full-process automation, thereby ensuring the safety of the entire process and improving work efficiency. At present, the basic modules of unmanned logistics vehicles such as vehicle perception, positioning, decision-making, planning, and control have been developed to a relatively mature level, and manufacturers have feasible solutions. However, for trailer mounting tasks, manual mounting solutions are still used, that is, manually towing the trailer so that the trailer hook is aligned with the rear socket of the tractor to achieve the mounting task. Using a tractor to tow a trailer can save energy and improve logistics transportation efficiency. It is a commonly used transportation method in logistics factories, but the current manual mounting solution is inefficient and has certain risks, which affects the fully automated operation 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 comprising: an unmanned vehicle, a rear-view TOF camera, a connection detection device and a main control device; the rear-view TOF camera is arranged behind the unmanned vehicle for realizing the acquisition of rear-view depth images; 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 rear-view depth image signal and the connection detection signal, and identify the position to be towed and the towing position according to the rear-view depth image signal, set the reversing path of the unmanned vehicle, so as to realize the reversing of the unmanned vehicle to prompt the to-be-towed vehicle and the towing of the unmanned vehicle to be automatically connected; it is also used to identify whether the connection between the to-be-towed and the towing is successful according to the connection detection signal. However, the system and method use a TOF camera, whose depth channel has large noise and is prone to failure under outdoor lighting, and has limited application scenarios.

[0005] With the development of computer vision technology, the accuracy of image target detection technology is constantly improving. The current mainstream target detection technology often uses deep networks. First, the network is trained using a data set with labeled true values. After that, the trained network can be used to implement real-time network reasoning on embedded devices. The current target detection algorithm can achieve accuracy comparable to that of human experts. In addition, pure visual sensors are low-cost and suitable for mass production solutions. However, the results of existing target detection using visual sensors are usually two-dimensional information and are unstable, and cannot be directly used to control the tractor to reverse and hook up. Summary of the invention

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

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

[0008] The position information of the key points of the trailer set in the RGB image is obtained by a target detection algorithm based on a deep network, and the target detection result is obtained;

[0009] Tracking the target detection result by 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] According to the current posture of the tractor and the three-dimensional spatial position, a path planning algorithm is used to plan the path of the tractor to obtain a planned path;

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

[0013] Preferably, the position information of the set 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 a target tracking algorithm comprises:

[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, the planning of the tractor path by using a path planning algorithm according to the current posture of the tractor and the three-dimensional space position includes:

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

[0021] Preferably, the method of using a tracking control algorithm to calculate the control amount of the tractor 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 amount of the tractor, and the tractor is controlled to the target point according to the planned path; wherein:

[0023] The speed of automatic reverse hooking is initially a constant -1m / s. When it reaches the second-to-last target point, it starts to decelerate with a constant acceleration until it reaches the last target point, at which time 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, which is used to obtain the position information of the key points of the trailer set 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, which is used to plan the path of the tractor according to the current posture of the tractor and the three-dimensional spatial position using a path planning algorithm to obtain a planned path;

[0029] The tractor control module 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.

[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 installed on the rear side of a tractor, an IMU sensor installed 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 including 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 steering angle and speed controller is used to control the tractor to a target point according to a 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 run the system described above in the present invention.

[0036] According to a fifth aspect of the present invention, there is provided a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, can be used to execute 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 automatic reversing and hooking method, system and device based on RGB camera provided by the present invention use RGB camera as main sensor and apply embedded controller to realize automatic reversing and hooking system.

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

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

[0041] The automatic reversing and hooking method, system and device based on RGB camera provided by the present invention can realize 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 from the detailed description of non-limiting embodiments made with reference to the following drawings:

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

[0044] Figure 2 The figure is a schematic diagram of the component modules of an automatic reversing and hanging 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 reversing hook device based on an RGB camera in one embodiment of the present invention; wherein (a) is a top view, and (b) is a side view; in the figure, 1 is an RGB camera, 2 is an IMU sensor, 3 is an embedded controller, and 4 is a front wheel steering 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 reverse 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 The present invention is a flowchart of a three-dimensional position restoration of a hook and a planning control algorithm for a tractor in a specific application example of the present invention. DETAILED DESCRIPTION

[0051] The following is a detailed description of the embodiments of the present invention: This embodiment is implemented on the premise of the technical solution of the present invention, and a detailed implementation method and a specific operation process are given. It should be pointed out that for ordinary technicians in this field, several variations and improvements can be made without departing from the concept of the present invention, which all belong to the protection scope of the present invention.

[0052] The results of existing target detection using visual sensors are usually two-dimensional information, which is unstable and cannot be directly used to control the tractor to reverse and hook up. In view of the above problems, an embodiment of the present invention provides an automatic reverse and hook up method based on an RGB camera. The method uses an RGB camera as the main sensor, and can achieve high-precision automatic reverse and hook up through target tracking algorithm, least squares optimization, path planning algorithm and tracking control algorithm. It has the advantages of low cost, good scalability, improved automation level of unmanned logistics factory, and improved work efficiency.

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

[0054] S1, obtaining the position information of the key points of the trailer in the RGB image through a target detection algorithm based on a deep network 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 space position;

[0057] S4, planning the path of the tractor using a path planning algorithm according to 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 amount of the tractor, and controlling 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 target detection algorithm based on a deep network, may further include the following operations:

[0060] S11, providing a data set for training a deep network for object detection and annotating it, wherein the annotation format of each image in the data set includes: a key point position of the left side of the trailer, a key point position of the right side of the trailer, a key point position of the trailer hook, and a 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 annotated true value detection point and the predicted detection point of the network, and s is the size of the detection box. The loss function is minimized by 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 key point positions of 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 implementations, the above S1 may further include the following operations:

[0069] Convert the target detection network into TensorRT format that can achieve real-time reasoning, and deploy it to the embedded platform NVIDIA Jetson Nano to accelerate the network. Further optimization can achieve a reasoning speed of 50ms to meet 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 tiny 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 continuous 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 points in the next frame to obtain predicted position information; further preferably:

[0073] The state quantity of each key point is:

[0074]

[0075] in, is the pixel coordinates 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 points 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, and I2=I1 is the brightness constant assumption;

[0079] S23, using the Kalman filter method to fuse the target detection result, 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 predict the 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 updated result of optical flow observation, 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 result after target tracking 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 implementations, 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 according to 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 vehicle-mounted IMU through dead reckoning). The real space height of the key point of the trailer is set (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 the least squares optimization algorithm to solve the optimization objective function, and obtaining 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 according to 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 current posture of the tractor is used as the starting posture, and the three-dimensional spatial position of the hook key point in the three-dimensional spatial position is used as the end position, and the Reeds-Shepp algorithm is used to plan the tractor path 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 implementations, the above S5, using a tracking control algorithm to calculate the control amount of the tractor, and controlling 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 by using the tracking control Pure-Pursuit algorithm:

[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 turning 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 the automatic reverse hook is initially constant at -1m / s, and reaches (path x19 ,path y19 ) and start to decelerate at a constant acceleration until reaching 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, Figure 2 As shown, the automatic reversing hook system based on RGB camera provided in this embodiment may include the following modules:

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

[0115] 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;

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

[0117] A 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 controls 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 by 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 for 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 combination with some preferred implementation modes.

[0121] In some preferred implementations, 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 frame.

[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 the requirements of real-time control;

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

[0128] In some preferred implementations, 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 the 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] According to the internal parameter K and external parameter 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 implementations, 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 to obtain a continuous target point trajectory.

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

[0138] The tracking control Pure-Pursuit 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, Figure 3 As shown in (a) and (b), the automatic reverse hooking device based on the RGB camera provided in this embodiment may include: an RGB camera installed on the rear side of the tractor, an IMU sensor installed on 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, 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 is a further clear and complete description of the automatic reverse hooking method, system and device based on RGB camera provided by the above embodiment of the present invention in combination with a specific application example and the accompanying drawings. Obviously, the described embodiment is only a 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 making creative work are within the scope of protection of the present invention.

[0147] like Figure 4As shown, the schematic diagram of the reverse hooking task in the present invention is depicted, which is the process of the tractor (powered front vehicle) engaging its rear latch with the hook of the trailer (unpowered rear vehicle) by reversing. A successful automatic reverse hooking means that the tractor latch can successfully align with the trailer hook at the end of the reversing behavior, so that the tractor can tow the trailer to move.

[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 a target detection algorithm based on a deep network;

[0151] Step 2, improving the stability of target detection results through a target tracking algorithm based on Kalman filter tracking and optical flow;

[0152] 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;

[0153] 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;

[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] Where 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 value detection point and the predicted detection point of the network, and s is the size of the detection box. The loss function is minimized by the gradient descent algorithm to obtain the optimal target 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 that can achieve real-time reasoning, and after being deployed to the embedded platform, it meets the requirements of real-time control.

[0163] Step 1d, using the embedded platform as the controller, inputting the real-time collected RGB image, and outputting 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:

[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 the target detection results through the target tracking algorithm 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 side of the trailer, the right side of the trailer, and the trailer hook obtained in step 1 are independent between each frame. Due to the error 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, the embodiment of the present invention uses two prediction algorithms, Kalman filtering and optical flow tracking, to predict the key point positions in continuous frames, and uses the Kalman filtering method to fuse the results of the target detection and the two tracking algorithms.

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

[0169]

[0170] in, is the pixel coordinates 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 locations in the next frame.

[0171] Step 2b, use the sparse optical flow tracking Lucas-Kanade method to track and observe 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, and I2=I1 is the brightness unchanged 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, and 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 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] Where L opt is the reprojection error objective function, x np ,y np is the projected pixel coordinate of the nth key point.

[0186] Step 3b, using the least squares optimization method, 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: Take the current position of the tractor as the starting position and the three-dimensional spatial position of the hook key point as the end position, and use the Reeds-Shepp algorithm to plan the tractor 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 continuous target points of the tractor planned by the Reeds-Shepp algorithm are obtained, and the tracking control Pure-Pursuit algorithm is used to calculate the control quantity:

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

[0197] Where PP is the tracking control Pure-Pursuit algorithm, δ is the turning angle of the tractor, (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 ) and start to decelerate at a constant acceleration until reaching 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 according to the planned path, the tracking control algorithm is used to control the tractor for the obtained continuous target points, where the turning angle is obtained by the tracking control algorithm, 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 hanging 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, including a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, it can be used to execute any method of the above-mentioned embodiments of the present invention, or to run any system of the above-mentioned 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 method), computer instructions, etc., and the above computer programs, computer instructions, etc. can be partitioned and stored in one or more memories. And the above computer programs, computer instructions, data, etc. can be called by the processor.

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

[0203] The processor is used 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 the memory may be independent structures or integrated structures. When the processor and the memory are independent structures, the memory and the processor may be coupled and connected 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 key point positions of hooks in the image, and perform target tracking to improve detection stability, and use the least squares optimization method to restore the three-dimensional space points of the hooks. Then, the Reeds-Shepp algorithm is used to plan the path of the tractor to obtain the target trajectory of the tractor, and the tracking control Pure-Pursuit algorithm is used to finally control the tractor to achieve the final goal of automatic reversing and hooking. The method has 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 finally realizing full-process automated operations.

[0207] The automatic reversing and hooking method, system and device based on RGB camera provided by the above-mentioned embodiment of the present invention obtain the trailer hook position by using RGB camera as the main sensor, and then control the tractor to automatically complete the reversing and hooking task, and capture the RGB picture containing the trailer hook position by using the RGB camera installed at the rear of the tractor, and detect the hook key point by using the target detection algorithm based on the deep network; 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, and 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 automatic reversing and hooking method, system and device based on RGB camera provided in the above embodiments of the present invention propose for the first time to use RGB camera to realize hook key point detection to realize automatic reversing and hooking. By using visual detection and tracking technology, the position of the hook point can be obtained more accurately, and the pure visual solution has a low cost, and can realize low-cost and high-success rate automatic reversing and hooking, which is of great significance to improving the efficiency of unmanned logistics operations of trailer-type unmanned vehicles, realizing full process automation and ensuring safety.

[0208] All matters not covered 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 modifications or variations 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: The position information of the key points of the trailer set in the RGB image is obtained by a target detection algorithm based on a deep network, and the target detection result is obtained; Tracking the target detection result by a target tracking algorithm to obtain a two-dimensional target tracking result; Restoring the target tracking result to a three-dimensional spatial position; According to the current posture of the tractor and the three-dimensional spatial position, a path planning algorithm is used to plan the path of the tractor to obtain a planned path; A tracking control algorithm is used to calculate the control amount of the tractor, and the tractor is controlled to the target point according to the planned path.

2. The automatic reversing and hanging method based on RGB camera according to claim 1 is characterized in that: The method of obtaining the position information of the key points of the trailer in the RGB image by using a target detection algorithm based on a deep network includes: A dataset for training a deep network for object detection is provided and annotated, 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; 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; 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 key point positions of the trailer hook are output in real time; Also includes any one or more of the following: -Convert the target detection network into a TensorRT format that can realize real-time reasoning, and deploy it to an embedded platform; use the embedded platform as a controller and use the real-time collected RGB images as input to the target detection network; -The object detection network is optimized using a gradient descent algorithm.

3. The automatic reversing and hanging method based on RGB camera according to claim 1 is characterized in that: 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.

4. The automatic reversing and hanging method based on RGB camera according to claim 1 is characterized in that: The step of restoring the target tracking result to a three-dimensional spatial position includes: According to 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, 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.

5. The automatic reversing and hanging method based on RGB camera according to claim 1 is characterized in that: The method of planning the path of the tractor using a path planning algorithm according to 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 current posture of the tractor is used as the starting posture, the three-dimensional spatial position of the hook key point in the three-dimensional spatial position is used as the end position, and the Reeds-Shepp algorithm is used to plan the tractor path to obtain m continuous target point trajectories.

6. The automatic reversing and hanging method based on RGB camera according to claim 1 is characterized in that: The method of using a tracking control algorithm to calculate the control amount of the tractor 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 amount of the tractor, and the tractor is controlled to the target point according to the planned path; wherein: The speed of automatic reverse hooking is initially a constant -1m / s. When it reaches the second-to-last target point, it starts to decelerate with a constant acceleration until it reaches the last target point, at which time the speed drops to 0m / s. The automatic reverse hooking action is completed and the tractor stops.

7. An automatic reverse hook system based on RGB camera, characterized in that: include: A key point detection module, which is used to obtain the position information of the key points of the trailer set 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, which is used to plan the path of the tractor according to the current posture of the tractor and the three-dimensional spatial position using a path planning algorithm to obtain a planned path; The tractor control module 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.

8. An automatic reversing hook device based on RGB camera, characterized in that: include: 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; among which: 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 performs key point detection, key point tracking, three-dimensional information acquisition and path planning based on the RGB image; The tractor front wheel steering angle and speed controller is used to control the tractor to a target point according to a planned path.

9. 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 the processor executes the computer program, it can be used to perform the method described in any one of claims 1 to 6, or to run the system described in claim 7.

10. 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 described in any one of claims 1 to 6, or to run the system described in claim 7.

Images