Search

CN-122009183-A - Unmanned tractor control method with automatic hook supporting function

CN122009183ACN 122009183 ACN122009183 ACN 122009183ACN-122009183-A

Abstract

The invention relates to a control method of an unmanned tractor with an automatic hook supporting function, wherein the tractor comprises a vehicle body mechanism and an automatic hook releasing mechanism, a rear laser radar is arranged at the rear part of the tractor, a navigation laser and a front blind supplementing laser are arranged on a vehicle roof, and left and right blind supplementing lasers are arranged on two sides of the tractor. The control steps are as follows, the tractor advances to a predetermined position point and then reverses to approach the skip car along the planned curve. When the distance of the skip car is smaller than the recognition distance threshold value, the laser radar recognizes the precise pose of the hook after starting. The system continuously detects the distance, if the recognition is overtime, the recognition is stopped, whether the recognition distance is adjusted or not is judged, if necessary, the parameter is updated for retrying, and otherwise, the system returns to the initial state. And after the identification is successful, judging the transverse deviation, if the deviation is too large, advancing to adjust the vehicle body and reversing again, and if the deviation is within the allowable range, changing the path end point, closing the obstacle avoidance function, and continuing reversing until the butt joint is completed. The method realizes full-automatic butt joint, has fault tolerance, avoids collision risk and improves the success rate of butt joint.

Inventors

  • ZHOU SHUANG
  • YANG SONGCHAO
  • NIU PENG
  • MA XIAOQING
  • ZHANG KUNFAN
  • ZHU YONGFENG

Assignees

  • 浙江中力机械股份有限公司

Dates

Publication Date
20260512
Application Date
20260327

Claims (10)

  1. 1. The unmanned tractor control method with the automatic hook supporting function is characterized by comprising a vehicle body mechanism (100) and an automatic hook releasing mechanism (7) fixed at the rear part of the vehicle body mechanism (100), wherein a rear laser radar (6) for detecting the hook position is further arranged on the upper part of the automatic hook releasing mechanism (7) on the vehicle body mechanism (100), a navigation laser (1) and a front blind supplementing laser (2) are further arranged at the top of the vehicle body mechanism (100), and left and right blind supplementing lasers (3) are further arranged on two sides of the vehicle body mechanism (100), and the control method comprises the following specific control steps: The tractor advances to a preset position point, then starts to travel backwards from the preset position point, backs a car approaching the car parking area C along a planned curve track, issues an identification signal when the distance of the car is smaller than an identification distance threshold value, scans and detects the accurate pose of a hook of the identification car after starting, continuously detects whether the distance of the car is still within an identification range or not by a system in an industrial personal computer (5) on the tractor, waits for an identification result to be obtained if the distance is within the range, If the recognition timeout does not acquire the result, issuing a stop recognition signal and judging whether the recognition distance needs to be changed, if so, updating the recognition distance parameter, then reissuing the recognition signal for continuous attempt, and if not, returning the issuing stop recognition signal to an initial state; After the identification is successful, whether the transverse deviation of the tractor and the skip car is smaller than a threshold value is firstly judged, if the transverse deviation is too large, the operation of forward adjusting the car body to reduce the transverse deviation is executed, then the car returns to a reversing state to approach again, if the transverse deviation is within an allowable range, the path end point is directly changed, the obstacle avoidance function is closed, and the car continues to reverse until the situation that the goods are taken is detected to complete the task.
  2. 2. The unmanned tractor control method with the automatic hook supporting function according to claim 1, wherein the specific process of scanning detection and identification of the hooks of the skip by the rear laser radar (6) is as follows: Step S1, initializing berth information, namely generating a searching area of the front end face of the skip car by the tractor from berth information received by a dispatching system, wherein the connection direction of berth starting point A (xa, ya), berth ending point B (xb, yb) and berth ending point B (xb, yb) is the reference central axis of the expected direction of the trailer, and is used for judging whether the attitude of the front end face of the trailer exceeds the allowable deviation or not, defining a quadrangle A 0 A 1 B 1 B 0 as an effective trailer searching area, Step S2, filtering point cloud, comprising the following steps: The parking position area filtering, namely, based on the quadrilateral searching area generated in the step S1, only retaining the point cloud inside the quadrilateral A 0 A 1 B 1 B 0 and filtering irrelevant point cloud outside the area; Step S3, clustering the filtered point clouds based on spatial proximity, grouping adjacent point clouds according to a distance threshold, classifying the points with the distance smaller than the threshold into the same category, finally forming a plurality of independent point cloud clusters, and calculating the external outline convex hulls, the lengths and the widths of each cluster; S4, end face identification, namely, the geometric feature of the front end face of the trailer is an approximate straight line segment with known width, and the front end face of the trailer is screened out from a clustering result by utilizing the feature: Step S5, target tracking, namely performing inter-frame tracking on the identified front end face target, updating a tracking list if the candidate target of the current frame is successfully matched with the tracking target of the previous frame in position and orientation, performing time sequence filtering on the position and orientation of the front end face based on an extended Kalman filtering model, inhibiting single-frame measurement noise, and adding the target as a new target into the tracking list if the candidate target of the current frame is not matched with the tracking target of the previous frame; step S6, pose calculation comprises two stages of coarse positioning and fine calculation, Step S61, in the coarse positioning stage, linear fitting is carried out on the front end face candidate point cloud based on the RANSAC algorithm, and whether the pose of the front end face meets the butt joint requirement is primarily judged; step S62, a fine calculation stage, namely, accurately calculating the pose of the front end surface of the internal point cloud passing through the coarse positioning stage by adopting a least square method; s7, calculating the position and the appearance of the hook, and calculating the accurate position of the hook based on the position and appearance calculation result of the front end face; And S8, the tractor acquires the recognized hook pose, a reference line L is issued under the dispatching system, the tractor plans a driving path from the current position of the vehicle to the hook position along the reference line L, and automatic towing and docking operation is executed.
  3. 3. The method for controlling an unmanned tractor with an automatic hook-supporting function according to claim 2, wherein the method further comprises a reference line deviation adjusting step of causing a lateral deviation of the hook with respect to the reference line L when the parking of the trailer and the center of the garage have a lateral deviation, Let the perpendicular distance of the hook position Phook to the reference line L be the lateral offset δ, define two thresholds: the safety threshold delta safe is that the upper limit of the transverse offset exceeds the upper limit, the deviation is considered to be too large, the safe docking cannot be realized, and the system alarms and stops tasks; when δdock < δ is less than or equal to δsafe, the specific method for adjusting the reference line offset is to translate the original reference line L by a distance δ along the normal vector direction thereof, generate a new offset reference line L', and the offset direction is the direction pointing to the hook position from the reference line, namely: Wherein The direction of the unit normal vector of the reference line L points to one side of the hook, and the offset reference line L 'passes through the transverse position of the projection point of the hook position on the original reference line, so that when the tractor runs along the new reference line L', the central line of the tractor body is transversely aligned with the hook, and the butt joint can be successfully completed.
  4. 4. The unmanned tractor control method with automatic hook-supporting function of claim 3, further comprising a post-offset re-docking procedure after the reference line offset adjustment step: after the reference line offset is completed, the tractor needs to perform the following sequence of actions to enter the new reference line L' and restart the automatic docking: 1. the tractor moves forward along the current driving direction, and moves away from the docking area in front of the trailer to a safe distance; 2. transversely switching, namely transversely moving the tractor from the original reference line L to the offset reference line L 'to align the central line of the vehicle body with the L'; 3. reversing the tractor to run along the new reference line L', re-entering the trailer docking area, and enabling the tail of the tractor to face the direction of the hook; 4. And restarting the butt joint, namely after reversing in place, restarting the step of scanning, detecting and identifying the hook of the skip car by the laser radar (6) after starting, and finishing the accurate butt joint based on the new reference line L'.
  5. 5. The method according to claim 2, wherein in the step 1, the unit vector from the stop end point B to the stop start point a is (xv, yv), Is respectively of the two normal vectors (-Yv, xv) and (Yv, -xv) the berth width is w, and the search quadrilateral A 0 A 1 B 1 B 0 is generated based on the information, and the four vertex coordinates are respectively: Wherein, the berth origin A is the side of the direction of the tractor approaching the skip car, the berth destination B is the side of the skip car body in the longitudinal direction, the berth width w is larger than the skip car body width, and the connecting line of the berth origin A and the berth destination B is the central axis Central axis of And a reference of the expected orientation of the trailer is used for subsequent pose deviation judgment.
  6. 6. The method for controlling an unmanned tractor with an automatic hook supporting function according to claim 2, wherein the step S2 further comprises discrete point filtering, wherein the number N of adjacent points in the radius range is counted by taking any point P as a circle center and r as a radius, and if N is smaller than a set threshold value, the point P is considered as a discrete noise point to be filtered.
  7. 7. The method for controlling an unmanned tractor with an automatic hook-supporting function according to claim 2, wherein the rule of the end face recognition in the step S4 is as follows: 1. distance sorting, namely projecting the central points of all clustered targets to the central axis of berth The targets close to the tractor are preferentially processed according to the sequence from near to far from the berth origin A; 2. size screening, namely selecting a target with the length matched with the width of the front end face of the trailer, and removing interference objects with different sizes; 3. Direction screening based on the central axis of the berth Judging whether the included angle between the main direction angle of the candidate target and the normal direction of the central axis exceeds a threshold value, and if so, considering that the target deviates from the expected direction and eliminating the deviation; 4. And (3) straightness inspection, namely performing straight line fitting on the point cloud of the candidate target, calculating fitting residual errors, wherein the front end surface of the trailer is of a planar structure, the point cloud projection of the trailer has good straightness, and the target with the residual errors exceeding a threshold value is eliminated.
  8. 8. The method for controlling an unmanned tractor with an automatic hook supporting function according to claim 1, wherein the specific process of the step S61, coarse positioning stage is as follows: 1. Selecting a projection point pt nearest to the parking position starting point A; 2. Selecting a point cloud with a distance less than maxDist from pt from all clustered targets, wherein maxDist =w×sin (θ_max×pi/180), and θ_max is the maximum deviation angle of the trailer allowed to park; 3. Performing RANSAC straight line fitting on the point cloud meeting the requirements to obtain a direction vector of a fitting straight line and one point on the line; 4. and calculating an included angle between the fitting straight line and the normal direction of the central axis of the berth, and if the included angle exceeds the allowable maximum deviation angle theta max, considering that the detection of the current frame fails, and entering the next detection period.
  9. 9. The method for controlling an unmanned tractor with an automatic hook supporting function according to claim 1, wherein the specific process of the step S62 and the fine calculation stage is as follows: 1. Performing least square line fitting on the inner point set to obtain front end face direction vector And fitting a linear equation; 2. The midpoint calculation, namely projecting all internal points onto a fitting straight line, and taking the midpoint of the projection points as a front end face center point Pf (xf, yf); 3. The normal vector direction of the front end surface is that the trailer direction theta=atan2 (v_fit_x, -v_fit_y), and the normal vector is perpendicular to the fitting straight line and points to the inside of the vehicle body; 4. And multi-frame accumulation, namely continuously calculating the weighted average value of N frames of results by adopting a multi-frame accumulation averaging method.
  10. 10. The method for controlling an unmanned tractor with an automatic hook-supporting function according to claim 9, wherein the precise position of the hook in step S7 is calculated as follows: xhook = xf + d×cosθ, yhook = yf + d×sinθ, D is the known fixed offset of the hook from the front end face, theta is the trailer orientation angle, and finally the pose (xhook, yhook, theta) of the hook is output for the tractor path planning module to execute the automatic docking operation.

Description

Unmanned tractor control method with automatic hook supporting function Technical Field The invention relates to the technical field of automatic traction hooks, in particular to a control method of an unmanned tractor with an automatic hook supporting function. Background With the rapid development of intelligent manufacturing and intelligent logistics, unmanned tractors (Automated Guided Vehicle, AGVs) are widely applied to scenes such as factories, warehouses, airports and the like, and are used for carrying the automatic transfer tasks of unpowered vehicles such as trucks, luggage trucks, trailers and the like. The automatic hook supporting and abutting joint between the tractor and the skip car is a key link for realizing full-flow unmanned operation. At present, the existing automatic docking schemes are mainly divided into the following categories: First, docking systems based on manual assistance or semi-automatic approaches. The operator controls the tractor to retreat through a remote controller or a vehicle-mounted human-computer interface, and the hook is connected by visual observation. Although the manual strength is reduced to a certain extent by the mode, manual intervention is still needed, unmanned operation in a true sense cannot be realized, the receiving efficiency is greatly influenced by the proficiency of operators, and potential safety hazards exist. Secondly, a docking scheme based on a single sensor (such as a common camera or an ultrasonic radar). Such solutions typically achieve back access by visual identification of the skip hook or by ultrasonic ranging. However, the camera is easily influenced by environmental factors such as illumination, dust, shielding and the like, the recognition stability is not enough, the ultrasonic radar can only provide distance information, the accurate pose of the hook cannot be obtained, and the tractor is difficult to guide to realize high-precision butt joint. When a large transverse deviation or an angle deviation exists in the parking position of the skip car, the scheme cannot be automatically adjusted, and the docking failure and even the collision are easy to occur. Thirdly, a laser radar-based identification docking scheme. Part of the prior art adopts a laser radar to scan the front end surface or the hook of the skip car, and realizes pose estimation through point cloud processing. However, the existing method generally has the defects that a systematic area filtering and multi-dimensional target screening mechanism is lacking in the point cloud processing process, the error identification is easily caused by the influence of environmental interference objects (such as adjacent parked trucks, upright posts, wall surfaces and the like), the system fault tolerance is poor due to the lack of a processing mechanism for abnormal conditions such as overtime identification and overlarge transverse deviation, when the large transverse deviation exists between the truck parking and the library position center, the existing method often needs to re-plan a global path or directly suspend tasks, and the adaptability is not enough, and in addition, the existing scheme still has an optimization space in the aspects of hook pose calculation precision, multi-frame filtering smoothness, reference line dynamic adjustment and the like, and the docking success rate and the operation efficiency are difficult to be considered. In summary, the existing automatic hook supporting technology of the unmanned tractor generally has the problems of poor environmental adaptability, insufficient recognition robustness, low docking success rate, imperfect exception handling mechanism and the like in practical application. Disclosure of Invention In order to solve the technical problems, the invention aims to provide the unmanned tractor control method with the automatic hook supporting function, which can realize efficient, stable and safe automatic butt joint between the tractor and the skip car through multi-sensor fusion, systematic point cloud processing and pose calculation and flexible path adjustment strategies. In order to achieve the purpose of the invention, the invention adopts the following technical scheme: The unmanned tractor control method with the automatic hook supporting function comprises a vehicle body mechanism and an automatic hook releasing mechanism fixed at the rear part of the vehicle body mechanism, wherein a rear laser radar for detecting the hook position is further arranged on the upper part of the automatic hook releasing mechanism on the vehicle body mechanism, navigation laser and front blind supplementing laser are further arranged at the top of the vehicle body mechanism, and left and right blind supplementing lasers are further arranged at the two sides of the vehicle body mechanism, and the control method comprises the following specific control steps: The tractor advances to a preset position point, then starts to travel backward