An obstacle avoidance method and system for an unmanned multi-section tractor

Abstract

This invention discloses an obstacle avoidance method and system for an unmanned multi-section tractor unit. The method includes: projecting structured light from the tractor head to the deflection detection reference point of the secondary trailer section, acquiring the reflected line image and converting it into a three-dimensional contour point cloud, and calculating the first hinge angle of the secondary trailer section relative to the tractor head; acquiring the motion state of the tractor head, estimating the pose of each trailer section based on a nonholonomic constrained kinematic recursive model of the multi-hinged trailer chain, using the first hinge angle as the observation constraint for filtering and recursion; fitting a smooth curve and expanding it to both sides according to the trailer width to generate a dynamic vehicle safety envelope; detecting the intersection of the obstacle point cloud and the envelope, and controlling the vehicle to avoid obstacles. This allows for the estimation of the entire trailer chain pose by measuring only the first hinge angle, eliminating the need for sensors on subsequent trailers, ensuring the envelope closely matches the real trajectory, and balancing obstacle avoidance safety and traffic efficiency.

Description

Technical Field

[0001] This invention relates to the fields of mobile robots and autonomous driving technology, and in particular to an obstacle avoidance method and system for unmanned multi-section tractor based on low-position line structured light angle measurement and multibody kinematics recursion. Background Technology

[0002] In industrial logistics scenarios such as intelligent warehousing, manufacturing plants, and airport cargo, unmanned tractor vehicles (such as trackless automated guided vehicles (AGVs) and autonomous mobile robots (AMRs) are widely used due to their large tonnage and long-distance traction capabilities. To maximize the efficiency of a single transport, the tractor vehicle usually has multiple unpowered flatbeds or trailers attached to its rear via a non-rigid articulated mechanism, forming a directed multi-articulated chain with multiple nonholonomic constraints and multiple degrees of freedom.

[0003] However, in actual operation, because the rear trailer is usually a passive, unpowered mechanical structure without an independent steering shaft, its trajectory exhibits highly non-homogeneous and non-linear characteristics. Especially when navigating sharp bends or performing obstacle avoidance maneuvers, the rear trailer chain's trajectory deviates significantly from the inside of the bend due to the "inner wheel difference cutoff angle" effect. This makes it extremely difficult for the central control system of the unmanned tractor to predict the instantaneous spatial pose and dynamic motion envelope of the rear trailer chain, often rendering traditional single-unit obstacle avoidance solutions ineffective. If the envelope area is estimated too large, it will lead to frequent accidental emergency braking, severely reducing logistics efficiency; if the estimate is too small, the rear trailer will scrape against the passageway walls, equipment, or pedestrians.

[0004] To address this pain point, existing technologies have proposed several solutions, but all of them have significant application drawbacks:

[0005] One approach is to install active sensors throughout the entire trailer: This involves installing angle sensors such as precision encoders and potentiometers at each trailer joint, or adding radar and ultrasonic sensors around each trailer. This solution requires deploying an active electrical system on each previously passive trailer, resulting in extremely high modification costs. Furthermore, in industrial vibrations and outdoor rain and snow environments, cable connections are prone to wear and tear, battery maintenance is extremely cumbersome, and reliability is very low, making it unsuitable for the intelligent upgrade of hundreds or thousands of existing non-standard trailers within a factory area.

[0006] Secondly, there is the pure visual calibration reconstruction scheme: For example, in the patent with publication number CN116202424B, a passive banner with a visual calibration pattern is installed at the rear of the flatbed trailer. The rear camera recognizes the banner and fits a fifth-order polynomial curve to generate the vehicle body envelope. In this scheme, there must be a line of sight between the camera and the banner. However, in industrial sites, the materials or goods loaded on trailers are usually stacked very high (even higher than the tractor unit). Once the line of sight is blocked by the goods, the banner at the rear of the trailer cannot be photographed, and the envelope reconstruction fails.

[0007] Third, wireless positioning solutions (such as Bluetooth or UWB): Active positioning beacons are installed on the trailer, and the end-point coordinates are estimated through phase difference or time of flight. This solution also requires the deployment of active devices on the trailer side, and suffers from the drawbacks of the beacons requiring battery power and frequent maintenance.

[0008] Therefore, how to accurately calculate the motion posture of multiple trailer chains without installing active sensors or positioning beacons on each trailer section, and generate a dynamic vehicle safety envelope that fits the actual motion trajectory, so as to achieve obstacle avoidance control that balances traffic efficiency and safety, is a technical problem that the mobile robot industry urgently needs to solve. Summary of the Invention

[0009] Therefore, the main objective of this invention is to provide an obstacle avoidance method and system for unmanned multi-section tractor trucks, in order to solve the problem that it is difficult to accurately predict the instantaneous position and dynamic motion envelope of the unpowered trailer chain at the rear during multi-section tractor truck operation.

[0010] To achieve the above objectives, according to one aspect of the present invention, a method for obstacle avoidance of an unmanned multi-section tractor is provided, the method comprising the following steps:

[0011] Structured light is projected from the tractor head to the deflection detection reference point of the second trailer, the reflected structured light line image is collected and converted into a three-dimensional contour point cloud;

[0012] Based on the geometric features of the three-dimensional contour point cloud, the first articulation angle of the secondary trailer relative to the tractor head is calculated.

[0013] The motion state of the tractor head is obtained, and the pose state of each trailer section is estimated by filtering and recursion based on the nonholonomic constrained kinematic recursive model of the multi-articulated trailer chain with the first articulation angle as the observation constraint.

[0014] The smooth curve representing the center trajectory of the whole vehicle is fitted according to the position and posture of each trailer section, and the smooth curve is expanded to both sides according to the width of the trailer to generate a dynamic vehicle body safety envelope.

[0015] The intersection detection is performed between the obstacle point cloud and the dynamic vehicle body safety envelope, and the multi-section tractor is controlled to perform obstacle avoidance operation based on the detection result.

[0016] In a possible preferred embodiment, the line structured light is projected by a line structured light sensing module disposed in the space below the chassis at the front and rear of the tractor unit; the line structured light sensing module includes a narrowband line laser and a monocular camera, the monocular camera being configured with a narrowband filter that matches the laser wavelength of the narrowband line laser.

[0017] In a possible preferred embodiment, the conversion to a three-dimensional contour point cloud includes: extracting the two-dimensional pixel coordinates of the center points of the lines in the structured light line image; establishing back-projection rays for each of the line center points based on camera intrinsic parameters, and solving the spatial depth of each of the line center points by combining the back-projection rays with a preset line structured light laser plane equation; determining the three-dimensional spatial coordinates of each of the line center points based on the spatial depth and the back-projection rays, thereby obtaining the three-dimensional contour point cloud.

[0018] In a possible preferred embodiment, the deflection detection reference is an inverted V-shaped reflector disposed on the drawbar of the secondary trailer, the inverted V-shaped reflector comprising two reflective wing plates with a fixed included angle.

[0019] In a possible preferred embodiment, the deflection detection reference unit includes two reflective surfaces intersecting the central axis; the calculation of the first hinge angle includes: extracting the extension direction of the reflective line segments corresponding to the two reflective surfaces from the three-dimensional contour point cloud, calculating the angle bisector direction of the two extension directions, and taking the angle between the angle bisector direction and the longitudinal central axis direction of the tractor head as the first hinge angle.

[0020] In a possible preferred embodiment, the nonholonomic constrained kinematic recursive model uses the rear axle center coordinates of the tractor, the heading angle of the tractor, and the articulation angles of each trailer to form the system state vector. The linear velocity and angular velocity of the tractor are used as control inputs. Based on the rolling constraint without sideslip, the differential equations of the articulation angles of each trailer are established, and the motion state of the previous trailer is transmitted to the next trailer step by step through the articulation point.

[0021] In a possible preferred embodiment, the filtering recursion is implemented by extended Kalman filtering: in the prediction step, the state prediction value is obtained by forward integration of the nonholonomic constrained kinematic recursive model, and the state covariance is updated; in the update step, Kalman update is performed using only the measured first hinge angle as a one-dimensional observation; when the number of trailer sections changes dynamically, the dimension of the system state vector is reconstructed, and the geometric parameters of each trailer section are loaded.

[0022] In a possible preferred embodiment, the smooth curve is a cubic Bézier curve, with the rear axle center of the tractor and the rear axle center of the trailer as the first and last anchor points, and intermediate control points constructed based on the longitudinal unit vectors of the tractor and the trailer.

[0023] In a possible preferred embodiment, generating the dynamic vehicle body safety envelope includes: discretizing the smooth curve into multiple coordinate points, expanding outwards to both sides by half the width of the trailer along the normal direction of the tangent at each coordinate point, and connecting them sequentially to form a closed convex polygon.

[0024] In a possible preferred embodiment, the intersection detection includes: finding the intersection of the obstacle point cloud collected by the lidar located at the front of the tractor with the closed convex polygon; when there is a point cloud that falls into the closed convex polygon, controlling the multi-section tractor to decelerate in steps or brake suddenly based on the comparison result between the obstacle distance and a preset safe distance threshold.

[0025] To achieve the above objectives, corresponding to the above method, according to another aspect of the present invention, an unmanned multi-section tractor obstacle avoidance system is also provided, comprising:

[0026] The storage unit is used to store the program of the method steps as described in any of the above examples, so that the line structure light sensing module and the vehicle main control can retrieve and execute it in a timely manner.

[0027] The line structured light sensing module is installed at the tractor head and is used to project line structured light onto the deflection detection reference part of the secondary trailer and to collect the reflected structured light line images.

[0028] The onboard main controller, which is communicatively connected to the structured light sensing module, is configured to: convert the structured light line image into a three-dimensional contour point cloud; calculate the first articulation angle of the secondary trailer relative to the tractor head based on the geometric features of the three-dimensional contour point cloud; acquire the motion state of the tractor head, and estimate the pose state of each trailer by filtering and recursively estimating the pose state of each trailer based on the nonholonomic constrained kinematic recursive model of the multi-articulated trailer chain, using the first articulation angle as the observation constraint; fit a smooth curve representing the trajectory of the entire vehicle center according to the pose state of each trailer, and expand the smooth curve to both sides according to the width of the trailer to generate a dynamic vehicle safety envelope; perform intersection detection between the obstacle point cloud and the dynamic vehicle safety envelope, and control the multi-trailer to perform obstacle avoidance operations according to the detection results.

[0029] The obstacle avoidance method and system for unmanned multi-section tractor provided by this invention uses the measured first articulation angle as the observation constraint and employs a nonholonomic constraint kinematic recursive model of the multi-articulated trailer chain to filter and deduce the pose state of each trailer section. This ensures that the generated dynamic vehicle body safety envelope closely matches the actual inward deflection trajectory of the trailer chain during turning and obstacle avoidance maneuvers. This avoids efficiency losses caused by frequent accidental braking and prevents trailers from scraping against passage walls, equipment, or pedestrians, balancing traffic efficiency and obstacle avoidance safety. Furthermore, this solution only requires a passive deflection detection reference unit to be set up in the second trailer section and only requires measuring one first articulation angle. The poses of subsequent trailer sections are obtained through recursive estimation, eliminating the need for any angle sensors, positioning beacons, or cables to be installed in the second to last trailer sections. This enables trailer-end deployment without modification, wiring, or maintenance, significantly reducing the cost of intelligent upgrades and maintenance load for existing trailers. Attached Figure Description

[0030] The accompanying drawings, which form part of this application, are used to provide a further understanding of the invention. The illustrative embodiments of the invention and their descriptions are used to explain the invention and do not constitute an undue limitation of the invention. In the drawings:

[0031] Figure 1 This is a schematic diagram of the obstacle avoidance method for an unmanned multi-section tractor of the present invention;

[0032] Figure 2 This is a three-dimensional layout diagram of the connection between the unmanned tractor head and the multiple trailers, and the low-position perception integrated module and the inverted V-shaped reflective target avoiding the cargo above and performing space scanning under the chassis, as an example of the obstacle avoidance method of the unmanned multi-section tractor of the present invention.

[0033] Figure 3 This is a schematic diagram of the geometric principle of linear laser triangulation and inverted V-shaped target scanning angle calculation in an example of the obstacle avoidance method for unmanned multi-section tractor of the present invention;

[0034] Figure 4 This is a schematic diagram of a nonholonomic constrained kinematic recursive model based on a multi-articulated trailer chain, as an example of the obstacle avoidance method for unmanned multi-section tractor vehicles of the present invention.

[0035] Figure 5 This is a flowchart illustrating the obstacle avoidance method for an unmanned multi-section tractor of the present invention.

[0036] Figure 6 This is a schematic diagram of the architecture of the unmanned multi-section tractor obstacle avoidance system of the present invention.

[0037] Explanation of reference numerals in the attached figures

[0038] Tractor head 101, low-position sensing integrated module 102, line laser generator 103, monocular camera 104, traction pin articulation seat 105, first flatbed trailer 106, A-shaped traction frame 107, inverted V-shaped reflective target 108, left reflector 108a, right reflector 108b, subsequent passive flatbed trailer 109, second trailer 109a, third trailer 109b, vehicle-mounted main control unit 110. Detailed Implementation

[0039] To enable those skilled in the art to better understand the technical solutions of the present invention, the specific technical solutions of the present invention will be clearly and completely described below in conjunction with embodiments, so as to help those skilled in the art further understand the present invention. Obviously, the embodiments described in this application are merely some embodiments of the present invention, and not all embodiments. It should be noted that, for those skilled in the art, the embodiments and features in the embodiments of this application can be combined with each other without departing from the concept of the present invention and without conflict. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort should fall within the disclosure and protection scope of the present invention.

[0040] Furthermore, the terms "first," "second," "S100," "S200," etc., used in the specification, claims, and drawings of this invention are used to distinguish similar objects and are not necessarily used to describe a specific order or sequence. It should be understood that such features can be interchanged where appropriate so that embodiments of the invention described herein can be implemented in orders other than those described herein. At the same time, the stages described in each step are not necessarily to be implemented in the same step; it should be understood that the implementation order of the contents of each step stage can be adjusted and interchanged without violating the inventive concept, so that embodiments of the invention described herein can be implemented in orders other than those described herein. Additionally, the terms "comprising" and "having," and any variations thereof, are intended to cover non-exclusive inclusion. Unless otherwise expressly specified and limited, the terms "set," "arrange," "install," "connect," and "link" should be interpreted broadly. For example, they can refer to a fixed connection, a detachable connection, or an integral connection; a mechanical connection or an electrical connection; a direct connection or an indirect connection through an intermediate medium; or a connection within two elements. Those skilled in the art can understand the specific meaning of the above terms in this case based on the specific circumstances and in conjunction with existing technology.

[0041] To address the problem of unsafe obstacle avoidance in multi-unit tractor operations, where the instantaneous position and dynamic motion envelope of the unpowered trailer chain at the rear are difficult to predict accurately, such as... Figures 1 to 6 As shown, this invention proposes an obstacle avoidance method and system for an unmanned multi-section tractor. In the following embodiments, the example is a tractor with a three-section flatbed trailer (N=3) attached to it. Of course, those skilled in the art will understand that the number of trailer sections can also be implemented as one, two or more sections depending on the actual situation. Therefore, other alternative implementation methods that do not depart from the scope of the illustrative concept of this invention are all within the scope of disclosure and protection of this invention.

[0042] In addition, for ease of understanding, the terms used in the following examples are defined as follows: "Secondary trailer" refers to the first trailer section that is immediately adjacent to the tractor unit and directly attached to the rear of the tractor unit via an articulation mechanism; "Last trailer" refers to the trailer section located at the very end of the trailer chain; "Deflection detection reference" refers to the structural part located at the front end of the secondary trailer section, which deflects synchronously with the secondary trailer section relative to the tractor unit and serves as the bearing surface for line structured light projection. It can be a passive reflective target (such as an inverted V-shaped reflector or an infrared reflective strip array) set on the drawbar of the secondary trailer section, or it can be the outer side of the wheel hub or the tire cut surface of the front wheel of the secondary trailer section.

[0043] Among them, such as Figure 2 and Figure 6 As shown, the obstacle avoidance system of the unmanned multi-section tractor in this embodiment includes: a tractor head 101, a low-position perception integrated module 102, a line laser generator 103, a monocular camera 104, a towing pin articulation seat 105, a first flatbed trailer 106 (i.e., the second trailer), an A-shaped towing frame 107, an inverted V-shaped reflective target 108 (including a left reflector 108a and a right reflector 108b), a subsequent passive flatbed trailer 109 (including a second trailer 109a and a third trailer 109b, where the third trailer 109b is the last trailer), and an on-board main controller 110.

[0044] The tractor unit 101 is the drive section, equipped with active front wheel steering and rear wheel differential driving. The front end of the first flatbed trailer 106 has a rigid passive A-shaped tow frame 107, the head of which is attached to the tow pin hinge seat 105 at the center of the rear of the tractor unit. The subsequent passive flatbed trailers 109 are formed by connecting N-1 flatbed trailers of the same or different specifications in series. The on-board main control unit 110 uses a Rockchip embedded chip (such as RK3568) to communicate with the monocular camera 104 and interact with the chassis actuator of the tractor unit through the CAN bus. The on-board main control unit 110 can also have a built-in or external memory to store the program for executing the obstacle avoidance method steps.

[0045] The technical link design of the aforementioned hardware is as follows: the low-position perception integrated module 102 actively scans the deflection detection reference part at the front end of the second trailer section, measuring only the first hinge angle; the on-board main controller 110 uses this single-point measured angle as the observation constraint, and through the inherent kinematic transmission relationship between each section of the multi-hinged chain, it progressively derives the pose of each subsequent trailer section; then, based on this, it generates a dynamic vehicle body safety envelope that fits the actual trajectory of the entire vehicle, and achieves obstacle avoidance by intersecting with the obstacle point cloud. The design principle of this technical link is that although the multi-hinged trailer chain has multiple degrees of freedom, there is a definite kinematic transmission relationship between each section through the hinge point. As long as the first hinge angle at the beginning of the chain is measured with high precision, the observation information can be transmitted backward, thus eliminating the need to install any sensors on the second to Nth trailer sections, and calculating the pose of the entire chain with minimal sensing and modification costs.

[0046] Specifically, corresponding to the above system architecture design, such as Figure 1 and Figure 5 As shown, the example steps of the obstacle avoidance method for the unmanned multi-section tractor provided by the present invention include:

[0047] Step S100: Project structured light from the tractor head to the deflection detection reference point of the secondary trailer, collect the reflected structured light line image, and convert it into a three-dimensional contour point cloud.

[0048] Specifically, such as Figure 2 As shown, in this example, the low-position sensing integrated module 102 is firmly installed on the rear centerline of the tractor head 101, preferably at a height of approximately H = 25cm above the road surface. This height is above the chassis beam and the towing hook, avoiding scraping from road debris. Preferably, this installation height can also be selected within the range of 20cm to 40cm from the ground. The design principle of this low-position deployment is that both the module and the structure under test are located in a spacious, low gap under the cargo carried by the trailer. There is always an open passage between the cargo and the chassis, so the sensing line of sight will not be obstructed by stacked cargo of any height. Even if the cargo stacking height exceeds the front of the tractor, the system can still obtain the trailer's attitude 100% uninterruptedly.

[0049] The low-position sensing integrated module 102 preferably integrates an infrared narrowband line laser generator 103 and a global shutter monocular camera 104. The line laser generator 103 has an example wavelength of 940nm and uses horizontal offset projection; the monocular camera 104 is equipped with a 940nm narrowband filter, whose optical axis forms a 30° angle with the laser projection plane, tilting downwards. This allows the 940nm narrowband line laser and the narrowband filter to physically form a rigid fit: the filter only allows light in an extremely narrow band near 940nm to pass through, thereby filtering out environmental interference such as strong white light, backlight, and dust scattering light in the factory area, ensuring a high signal-to-noise ratio in harsh conditions such as darkness, dust, rain, and snow, and stabilizing measurement accuracy at the millimeter level (e.g., distance accuracy within ±2.0mm); furthermore, the use of a global shutter avoids motion distortion caused by shooting while the vehicle is moving.

[0050] With the above configuration, the line laser generator 103 and the monocular camera 104 can form an active line structured light triangulation measurement optical path. That is, the laser plane is projected onto the surface of the deflection detection reference part to form a folded line, and the camera collects the line from another angle. The geometric relationship between the baselines of the two and the laser plane constitutes the triangulation distance constraint.

[0051] In this embodiment, the deflection detection reference unit, such as Figure 3 As shown, the example is an inverted V-shaped reflective target 108, which is a sheet metal piece bent into a zigzag metal part with a fixed included angle (e.g., 120°) to form two reflective wing plates with a fixed included angle (e.g., left reflector 108a and right reflector 108b), which intersect at the central axis in physical geometry. Furthermore, in a preferred embodiment, the target's surface can be coated with an infrared highly reflective material and rigidly welded or adsorbed onto the central axis of the A-shaped tow frame 107 near the hinge point, serving as the bearing surface for laser projection. This target is a purely passive mechanical component, containing no electronic components, batteries, or cables, and can be welded in one step, achieving wiring-free and maintenance-free deployment at the trailer end.

[0052] In addition, in other alternative embodiments, following the above-described arrangement of the deflection detection reference unit, this step may also include the following parallel alternative implementations:

[0053] Passive reflective strip array: Instead of using sheet metal inverted V-shaped rigid targets, this method symmetrically attaches an array of infrared reflective strips with high reflectivity (high grayscale difference) to the left and right surfaces of the A-frame tow bar. When a line laser scans these reflective strips, it produces several extremely bright, high-contrast discontinuous line segments in the camera image. By calculating the three-dimensional relative displacement of the midpoints of these bright line segments in space, the hinge angle can be accurately calculated. This solution further eliminates any metal welding, allowing trailer upgrades to be completed simply by applying stickers.

[0054] Wheel hub / tire projection: If the A-frame of the first trailer is difficult to install an inverted V-shaped target due to special mechanical design (such as ultra-short tie rods or foldable tie rods), the low-position sensing integrated module 102 can be installed on the left and right sides of the tractor's front and rear decks to directly project lasers onto the outer side of the front wheel hub or the tangential plane of the tire of the first trailer. The camera captures the geometric curvature changes and yaw deformation of the laser lines on the tire / wheel hub surface, extracts feature points, and inversely calculates the hinge angle β1. This solution also maintains the advantage of the trailer being passive and power-free, and avoids the drawbar structure.

[0055] Multi-line diffractive optics (DOE) grid projection: The single-line laser generator 103 is replaced with a grid or multi-line infrared structured light projector based on multi-line diffractive optics to project a 3×3 infrared intersecting grid line onto the undercarriage of the trailer. By recognizing the three-dimensional perspective deformation of this grid, the camera can obtain trailer attitude with more degrees of freedom (such as pitch angle and roll angle), greatly enhancing the accuracy of multi-trailer attitude extrapolation when driving on rough roads or uphill / downhill slopes.

[0056] Step S200: Based on the geometric features of the three-dimensional contour point cloud, calculate the first articulation angle of the secondary trailer relative to the tractor unit.

[0057] Specifically, to convert a structured light line image into a 3D contour point cloud, the process in this example is as follows:

[0058] First, the center of the line is extracted: When the laser is projected onto the surface of the inverted V-shaped reflective target 108, the monocular camera 104 captures a zigzag laser line, which appears as a left and right line segment on the image plane. The GPU of the vehicle-mounted main control unit 110 performs sub-pixel edge center extraction on the acquired infrared image. In this embodiment, the gray-scale centroid method is used, that is, the centroid position of the cross-section of the laser line is calculated by weighting the pixel gray-scale value as the center of the line, thereby obtaining sub-pixel level center point positioning accuracy and obtaining the pixel set of the left laser line. And the set of pixels of the right laser line .

[0059] Then, 3D reconstruction is performed: a camera coordinate system is established with the optical center of the monocular camera (104 points) as the origin and the optical axis as the Z-c axis. The equation of the laser plane formed by the line laser generator 103 projecting in space is defined as follows:

[0060] (Equation 1)

[0061] Based on the camera's intrinsic parameter matrix:

[0062] (Equation 2)

[0063] When pixels are inversely projected back into 3D space, any point (u, v) on the image corresponds to a physical point in 3D space. The following relationship must be satisfied:

[0064] (Equation 3)

[0065] Equation 3 represents the back-projection relationship determined by the camera's intrinsic parameters: a pixel can be used to determine a back-projection ray passing through the optical center. The intersection of this ray with the laser plane uniquely determines the spatial point, which is the geometric essence of line structured light triangulation. Substituting Equation 3 into Equation 1, the three-dimensional spatial depth value corresponding to each pixel is obtained. :

[0066] (Equation 4)

[0067] Then solve for the depth value By substituting back into equation 3, the three-dimensional spatial coordinates of each pixel on the left and right laser lines can be calculated point by point, resulting in a three-dimensional contour point cloud. From this, the three-dimensional straight line equation of the left laser line in space can be obtained. and the equation of the right three-dimensional line The above solution process is an analytical operation involving point-by-point substitution, requiring no iterative optimization and involving minimal computation.

[0068] Finally, after completing the above three-dimensional contour point cloud extraction, the first hinge angle is calculated using its included angle geometric features, as shown in the example below.

[0069] Perform angle calculation: View the projection plane from the camera coordinate system ( On the plane, calculate the direction vector of the left line segment. with the direction vector of the right line segment The direction of the angle bisector of the two intersecting lines in the horizontal direction. The instantaneous central axis deviation of the inverted V-shaped reflective target 108:

[0070] (Equation 5)

[0071] The normalize() function represents normalizing the vector. Since the physical angle between the left and right reflector plates is fixed, the direction of the resultant vector obtained by summing the two normalized direction vectors is strictly along the angle bisector of the target's central axis. Therefore, the direction of this resultant vector represents the instantaneous orientation of the second trailer. This step only involves vector algebra operations to extract the angle information from the central axis offset.

[0072] like Figure 4 As shown, the first hinge angle That is, direction With respect to the longitudinal center axis of the tractor head 101 The included angle:

[0073] (Equation 6)

[0074] Because line lasers have extremely high signal-to-noise ratio and resolution within a 2m ranging range, the calculated... The resolution can reach 0.05°, and the measurement accuracy is better than 0.1°. For example, when the tractor unit is moving straight and the trailer begins to turn to the right, the inverted V-shaped target deflects accordingly. The spatial directions of the left and right segments of the broken line captured by the camera change synchronously, and the direction of the angle bisector obtained by Equation 5 deflects accordingly. Equation 6 outputs the direction after the deflection in real time. This allows for high-frequency and smooth tracking of the swaying of the secondary trailer.

[0075] Step S300: Obtain the motion state of the tractor head, and based on the nonholonomic constrained kinematic recursive model of the multi-articulated trailer chain, perform filtering recursion with the first articulation angle as the observation constraint to estimate the pose state of each trailer section.

[0076] Specifically, the nonholonomic constrained kinematic recursive model of the multi-articulated trailer chain on which this step is based is explained below. The multi-articulated trailer chain referred to in this example refers to the directed series structure formed by the tractor unit 101 and the next trailer section, as well as the adjacent two trailer sections, all connected by a traction pin hinge point. That is, the tractor unit is the active driving segment, and each trailer section is a driven segment without power and without an independent steering axle. Its movement is entirely generated by the dragging of the previous trailer section through the hinge point.

[0077] The nonholonomic constraint referred to in this example means that the trailer wheels satisfy the no-slip rolling assumption with respect to the ground, that is, the wheels can only roll in the direction perpendicular to their axles (i.e., the longitudinal direction of the trailer body), and cannot slide laterally in the direction of the axles. This constraint restricts the velocity direction of each trailer section, but cannot be converted into a position constraint condition through integration, hence it is called a nonholonomic constraint. The so-called recursion means that, with the motion state of the tractor unit as known input, the motion state of the next trailer section is first calculated based on the geometric relationship between the tractor unit and the articulation point of the next trailer section, and then the motion state of the previous trailer section is used as the input of the next trailer section, and so on, step by step, along the trailer chain from front to back, until the motion state of the last trailer section is calculated.

[0078] Therefore, this nonholonomic constrained kinematic recursive model consists of a set of differential equations (see Equations 7 to 10) that characterize the pose of each car, the control input of the tractor, and describe the hierarchical transmission relationship of motion states between adjacent cars.

[0079] Specifically, refer to Figure 4 As shown, the motion state variables of the tractor unit include: the heading angle obtained based on the SLAM algorithm. And linear velocity directly measured by wheel speed gauge (encoder) and IMU. and angular velocity ,by As the system control input, multi-trailer systems are typically mounted off-axle (off-axle hitching), defining the system state vector. ,in This refers to the position of the rear axle center of the tractor unit in the world coordinate system. The heading angle of the vehicle's front end. Let be the hinge angle of the i-th section.

[0080] Based on the nonholonomic kinematic constraints (no sideslip rolling assumption), the attitude derivative of the second-section trailer can be written as:

[0081] (Equation 7)

[0082] in: This refers to the length of the drawbar of the second trailer section (i.e., the distance from the draw pin to the center of the axle of the second trailer section). The off-axle offset is the length of the rear overhang from the center of the rear axle of the tractor head to the hinge point.

[0083] For the subsequent i-th trailer ( Its linear velocity and angular velocity are iteratively transferred according to the following formula:

[0084] (Equation 8)

[0085] (Equation 9)

[0086] Its hinge angle state differential equation is:

[0087] (Equation 10)

[0088] in, Let be the equivalent wheelbase of the i-th trailer. The rear suspension of the preceding car is offset. Equations 8 to 10 are the specific expressions of the above-mentioned step-by-step transmission relationship: the linear velocity and angular velocity of the preceding car are transmitted to the following car through the hinge point using a determined trigonometric function relationship. Therefore, as long as the state at the beginning is measured and constrained, the motion of the entire chain is uniquely determined step by step.

[0089] A state estimation extended Kalman filter (EKF) is established in the vehicle main controller 110:

[0090] Predict step: Using the above continuous kinematic equations, perform forward integration using the fourth-order Runge-Kutta method (RK4) to calculate the current control input. The predicted state value And update the system covariance matrix:

[0091] (Equation 11)

[0092] in Let be the system Jacobian matrix, and Q be the process noise covariance.

[0093] Update step: The observation equations of this system are extremely concise, using only the first hinge angle measured by line structured light. As a one-dimensional observation:

[0094] (Equation 12)

[0095] That is, the observation matrix The observation matrix selects exactly the fourth component of the state vector. .

[0096] Calculate Kalman gain Status Update:

[0097] (Equation 13)

[0098] (Equation 14)

[0099] Through the above steps, since only the first hinge angle in the observation equation H is constrained by actual measurements, while there are strong physical constraints (non-homogeneous differential constraints) between the N chain sections, the high-frequency, high-precision correction information of the first hinge angle will be immediately transmitted to the next section through the covariance cross-update of EKF. This constrains and corrects the angle yaw of the hidden state of the following N-1 sections, so that the cumulative error of the tail car pose derived in the event of wheel-rail slippage or uneven ground is forcibly limited to the convergence range, and the pose calculation accuracy of the last trailer section is kept within the decimeter level. At the same time, since the observation is only one-dimensional, the computational amount of the update step is extremely small, and the filter can converge at high frequency and extremely fast on the embedded chip, compressing the time consumption of the single-cycle algorithm to the microsecond level.

[0100] Furthermore, regarding adaptive loading of trailer sections, for example, when the on-site dispatch changes the number of trailer sections from 3 to 5, the on-board main controller 110 only needs to reconstruct the dimensions of the state vector X according to the current actual number of sections and load the wheelbase of each newly added trailer section. With rear suspension offset Geometric parameters can be used to estimate the state of a new fleet without adding or reconfiguring any hardware or manually modifying the code, enabling adaptive recognition and control.

[0101] Step S400: Fit a smooth curve representing the center trajectory of the whole vehicle according to the position and posture of each trailer section, and expand the smooth curve to both sides according to the width of the trailer to generate a dynamic vehicle body safety envelope.

[0102] Specifically, when EKF estimates the coordinates of the wheel axle centers of each trailer in the world coordinate system... Then, the vehicle-mounted main controller 110 uses these coordinate points as geometric control nodes to fit a smooth cubic Bézier curve. Reconstruct the center trajectory of the entire vehicle.

[0103] The cubic Bessel parametric equation is defined as follows:

[0104] (Equation 15)

[0105] Among them: first anchor point Coordinates of the rear axle center of the tractor unit ; final anchor point The coordinates of the rear axle center of the last trailer (tail trailer) Intermediate control points , The longitudinal unit vector of the vehicle's front; the intermediate control point. , The longitudinal unit axial vector of the tail vehicle is determined by the tail heading angle calculated by EKF; To adjust the smoothing factor (0.5 in this embodiment), two anchor points fix the start and end positions of the curve, and two intermediate control points constructed by the headings of the front and rear vehicles constrain the tangential direction at the start and end of the curve, so that the fitted curve smoothly fits the actual center direction of each section of the vehicle. This is the geometric principle of reconstructing the trajectory of the entire vehicle using Bézier curves.

[0106] By rapidly discretizing and solving in a microcontroller The corresponding B(t) yields 21 discrete coordinate points on the central trajectory line. Since cubic Bézier curves are evaluated purely algebraically, there is no need for high-dimensional matrix inversion or iterative solution of the cost function. Compared with fifth-order polynomial fitting, this significantly saves computational power, and the time for generating a single curve and envelope can be compressed to within 1.0 ms.

[0107] For each discrete point Centered on the point, expand outwards to both sides along the normal direction of the tangent at that point. ( (Given the physical width of the trailer), symmetrical envelope boundary points on both sides are obtained. These boundary points are then connected sequentially to the beginning and end points, directly generating a dynamic vehicle body boundary envelope region composed of closed convex polygons. This envelope region is updated frame-by-frame based on the recursive results of the pose of each trailer section. Its shape closely matches the actual inward trajectory of the trailer chain during turning, thus preventing frequent accidental braking due to overestimation and avoiding scraping due to underestimation.

[0108] In addition, in the optional implementation, following the curve fitting method described above, this step also includes the following alternative implementation: In curve generation and envelope reconstruction, if the main control computing power is extremely limited (such as a very low-end 8-bit microcontroller), cubic Bézier curves can be omitted, and the coordinate points of each wheel axle calculated by EKF can be directly interpolated and connected by multiple tangential continuous circular arcs, B-splines or spirals, and expanded to both sides, which can also achieve the generation of dynamic vehicle body safety envelope.

[0109] Step S500: Perform intersection detection between the obstacle point cloud and the dynamic vehicle body safety envelope, and control the multi-section tractor to perform obstacle avoidance operation based on the detection result.

[0110] Specifically, the obstacle avoidance calculation in this step can be implemented by an external obstacle avoidance / path planning system or by the onboard main controller 110. The obstacle avoidance calculation method involves high-speed geometric intersection of the real-time point cloud collected by the two-dimensional anti-collision lidar located in front of the tractor unit with the closed convex polygon dynamic vehicle body boundary envelope area generated in step S400. If the point cloud falls within the envelope area, its distance is calculated, and based on a set safe distance threshold, a stepped deceleration or emergency braking control command is sent to the tractor unit's drive system to achieve highly safe multi-stage obstacle avoidance. If the obstacle distance is greater than the safe distance threshold, the vehicle maintains its current path tracking and navigation.

[0111] For example, when the trailer chain swings inward in a sharp bend and the envelope expands to the inside of the bend, once the device point cloud located at the edge of the inner channel invades the expanded envelope, the system will first decelerate in steps according to the distance threshold, and then brake urgently when the distance gets closer, thereby preventing the rear of the trailer from scraping against the equipment.

[0112] In summary, the obstacle avoidance method and system for unmanned multi-section tractor provided by this invention constructs a closed-loop technology combining single-point measurement of the second section, multi-body kinematic recursion, and geometric curve envelope. It sets a passive deflection detection reference section only in the second section of the trailer, using low-position active line structured light to measure the first hinge angle with high precision. Leveraging the defined kinematic transmission relationship formed by the hinge points between the sections of the multi-hinged trailer chain, filtering and recursion are performed using this single-point observation as a constraint, transmitting the observation information from the first end backward to calculate the pose of the entire trailer chain. Then, a dynamic vehicle safety envelope that conforms to the real trajectory is generated by low-order geometric curve fitting and expansion, and its intersection with the obstacle point cloud is used to achieve obstacle avoidance control.

[0113] Therefore, this invention enables the following advantages: First, it requires only the measurement of a single hinge angle to estimate the position of the entire trailer chain. The second to the last trailer section is completely passive, power-free, wiring-free, and maintenance-free, reducing trailer upgrade and modification costs by over 95%. It is suitable for mobile scenarios involving frequent dynamic splicing of hundreds or thousands of existing trailer sections. Second, the dynamic envelope closely matches the actual motion envelope of the trailer chain, avoiding frequent mis-braking caused by overestimation and scraping caused by underestimation, thus balancing obstacle avoidance safety and logistics efficiency. Third, the sensing line of sight extends to the open gaps below the chassis where cargo cannot obstruct the view, combined with a 940nm narrowband sensor. The physical filtering of laser and narrowband filter maintains millimeter-level ranging accuracy and better than 0.1° angle measurement accuracy under harsh conditions such as arbitrary loading height and backlight, darkness, dust, rain and snow. Fourth, the EKF one-dimensional observation update suppresses the cumulative drift caused by wheel-rail slippage, and the position and posture accuracy of the last trailer is maintained within decimeter level. It can adapt to dynamic changes in the number of trailer sections without modifying the hardware code. Fifth, the cubic Bezier algebra solution and the intersection of closed convex polygons compress the time consumption of the single-cycle algorithm to the microsecond to millisecond level, which can be deployed on low-cost, low-computing-power embedded chips to ensure the real-time performance of emergency braking.

[0114] The preferred embodiments of the present invention disclosed above are merely illustrative of the invention. These preferred embodiments do not exhaustively describe all details, nor do they limit the invention to the specific implementations described. Clearly, many modifications and variations can be made based on the content of this specification. This specification selects and specifically describes these embodiments to better explain the principles and practical applications of the invention, thereby enabling those skilled in the art to better understand and utilize the invention. The present invention is limited only by the claims and their full scope and equivalents. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the invention should be included within the protection scope of the invention.

[0115] Those skilled in the art will understand that, besides implementing the system, apparatus, unit, and its modules provided by this invention in purely computer-readable program code, the same program can be implemented in the form of logic gates, switches, application-specific integrated circuits, programmable logic controllers, and embedded microcontrollers by logically programming the method steps. Therefore, the system, apparatus, and its modules provided by this invention can be considered a hardware component, and the modules included therein for implementing various programs can also be considered structures within the hardware component; alternatively, modules for implementing various functions can be considered both software programs implementing the method and structures within the hardware component.

[0116] Furthermore, all or part of the steps in the methods of the above embodiments can be implemented by a program instructing related hardware. This program is stored in a storage medium and includes several instructions to cause a microcontroller, chip, or processor to execute all or part of the steps of the methods described in the various embodiments of this application. The aforementioned storage medium includes various media capable of storing program code, such as a USB flash drive, a portable hard drive, a read-only memory (ROM), a random access memory (RAM), a magnetic disk, or an optical disk.

[0117] Furthermore, various different implementations of the present invention can be combined arbitrarily, as long as they do not violate the spirit of the present invention, they should also be regarded as the content disclosed in the present invention.

Claims

1. A method for obstacle avoidance of an unmanned multi-section tractor, characterized in that the steps include: include: Structured light is projected from the tractor head to the deflection detection reference point of the second trailer, the reflected structured light line image is collected and converted into a three-dimensional contour point cloud; Based on the geometric features of the three-dimensional contour point cloud, the first articulation angle of the secondary trailer relative to the tractor head is calculated. The motion state of the tractor head is obtained, and the pose state of each trailer section is estimated by filtering and recursion based on the nonholonomic constrained kinematic recursive model of the multi-articulated trailer chain with the first articulation angle as the observation constraint. The smooth curve representing the center trajectory of the whole vehicle is fitted according to the position and posture of each trailer section, and the smooth curve is expanded to both sides according to the width of the trailer to generate a dynamic vehicle body safety envelope. The intersection detection is performed between the obstacle point cloud and the dynamic vehicle body safety envelope, and the multi-section tractor is controlled to perform obstacle avoidance operation based on the detection result.

2. The obstacle avoidance method for an unmanned multi-section tractor according to claim 1, characterized in that, The line structured light is projected by a line structured light sensing module located in the space below the chassis at the front and rear of the tractor unit. The line structured light sensing module includes a narrowband line laser and a monocular camera, wherein the monocular camera is equipped with a narrowband filter that matches the laser wavelength of the narrowband line laser.

3. The obstacle avoidance method for an unmanned multi-section tractor according to claim 1, characterized in that, The conversion steps of the three-dimensional contour point cloud include: extracting the two-dimensional pixel coordinates of the center points of the lines in the structured light line image; establishing the back projection rays of each line center point according to the camera intrinsic parameters, and solving the spatial depth of each line center point by combining the back projection rays with the preset line structured light laser plane equation; determining the three-dimensional spatial coordinates of each line center point according to the spatial depth and the back projection rays to obtain the three-dimensional contour point cloud.

4. The obstacle avoidance method for an unmanned multi-section tractor according to claim 1, characterized in that, The deflection detection reference part is an inverted V-shaped reflector set on the drawbar of the second trailer, and the inverted V-shaped reflector includes two reflective wing plates with a fixed included angle.

5. The obstacle avoidance method for an unmanned multi-section tractor according to claim 1, characterized in that, The deflection detection reference section includes two reflective surfaces intersecting the central axis; The first hinge angle calculation step includes: extracting the extension direction of the two reflection line segments corresponding to the two reflection surfaces from the three-dimensional contour point cloud, calculating the angle bisector direction of the two extension directions, and taking the angle between the angle bisector direction and the longitudinal centerline direction of the tractor head as the first hinge angle.

6. The obstacle avoidance method for an unmanned multi-section tractor according to claim 1, characterized in that, The nonholonomic constrained kinematic recursive model uses the rear axle center coordinates of the tractor, the heading angle of the tractor, and the articulation angles of each trailer to form the system state vector. The linear velocity and angular velocity of the tractor are used as control inputs. Based on the no-side-slip rolling constraint, the differential equations of the articulation angles of each trailer are established, and the motion state of the previous trailer is transmitted to the next trailer step by step through the articulation point.

7. The obstacle avoidance method for an unmanned multi-section tractor according to claim 6, characterized in that, The filtering recursive steps include: in the prediction step, performing forward integration on the nonholonomic constrained kinematic recursive model to obtain the state prediction value and updating the state covariance; in the update step, performing Kalman update using only the measured first hinge angle as a one-dimensional observation; and when the number of trailer sections changes dynamically, reconstructing the dimension of the system state vector and loading the geometric parameters of each trailer section.

8. The obstacle avoidance method for an unmanned multi-section tractor according to claim 1, characterized in that, The smooth curve is a cubic Bézier curve, with the rear axle center of the tractor and the rear axle center of the trailer as the first and last anchor points, and intermediate control points constructed based on the longitudinal unit vectors of the tractor and the trailer.

9. The obstacle avoidance method for an unmanned multi-section tractor according to claim 1, characterized in that, The step of generating the dynamic vehicle body safety envelope includes: discretizing the smooth curve into multiple coordinate points, expanding outwards to both sides by half the width of the trailer along the normal direction of the tangent at each coordinate point, and connecting them sequentially to form a closed convex polygon.

10. The obstacle avoidance method for an unmanned multi-section tractor according to claim 9, characterized in that, The intersection detection step includes: finding the intersection of the obstacle point cloud collected by the lidar set at the front of the tractor with the closed convex polygon; when there is a point cloud that falls into the closed convex polygon, controlling the multi-section tractor to decelerate in steps or brake suddenly based on the comparison result between the obstacle distance and the preset safe distance threshold.

11. An obstacle avoidance system for an unmanned multi-section tractor, characterized in that, include: A storage unit is used to store a program of the method steps as described in any one of claims 1 to 10, for the vehicle-mounted main control unit to retrieve and execute; The line structured light sensing module is installed at the tractor head and is used to project line structured light onto the deflection detection reference part of the secondary trailer and to collect the reflected structured light line images. The onboard main controller, which is communicatively connected to the structured light sensing module, is configured to: convert the structured light line image into a three-dimensional contour point cloud; calculate the first articulation angle of the secondary trailer relative to the tractor head based on the geometric features of the three-dimensional contour point cloud; acquire the motion state of the tractor head, and estimate the pose state of each trailer by filtering and recursively estimating the pose state of each trailer based on the nonholonomic constrained kinematic recursive model of the multi-articulated trailer chain, using the first articulation angle as the observation constraint; fit a smooth curve representing the trajectory of the entire vehicle center according to the pose state of each trailer, and expand the smooth curve to both sides according to the width of the trailer to generate a dynamic vehicle safety envelope; perform intersection detection between the obstacle point cloud and the dynamic vehicle safety envelope, and control the multi-trailer to perform obstacle avoidance operations according to the detection results.

Images