Tractor-trailer trajectory planning method in three-dimensional space

Abstract

The invention relates to the technical field of trailer driving track calculation, and particularly discloses a tractor-trailer track planning method in a three-dimensional space, which comprises the following steps: acquiring environment information of a tractor-trailer, and establishing a data file according to the environment information; establishing a motion environment of the tractor-trailer in a three-dimensional space based on the data file; planning a tractor track in the motion environment based on geometric characteristics of a Bezier curve; and determining a position relationship between the tractor and the trailer based on the motion environment, and calculating a trailer track of the tractor-trailer in a two-dimensional plane and a three-dimensional space. The problem that an existing trajectory planning algorithm cannot perform planning in a three-dimensional space is solved, and smooth loading of large cargoes on the roll-on-roll-off ship is achieved.

Description

Technical Field

[0001] The present application relates to the technical field of trailer driving trajectory calculation, and in particular to a method for tractor-trailer trajectory planning in three-dimensional space. Background Art

[0002] Ro-ro ships are one of the most economical modes of transport for large cargo, typically requiring a tractor-trailer system for loading. To safely load the cargo onto the vessel's deck, the tractor and trailer must depart from the dock, navigate the inclined ramps and locks connecting the dock to the deck, and avoid all obstacles. However, when loading large cargo, it is difficult for the driver to control the precise positioning of the trailer and cargo using the tractor. This makes it easy for the trailer and cargo to collide with the inclined ramp, locks, or obstacles on the deck during loading.

[0003] Currently, many researchers have studied the path planning and obstacle avoidance algorithms for tractor-trailers and have achieved a series of research results. For example, existing research has analyzed the kinematic relationship between the tractor and trailer and summarized the motion relationship equations between the tractor and trailer. Leveraging the kinematic relationship between the tractor and trailer, an optimization-based path planning algorithm has been proposed to explore collision-free paths for tractor-trailers in narrow roads. Furthermore, by integrating the basic RRT algorithm with the path tracking control equation, a more accurate trajectory path is generated for the towed robot by fitting spline curves. This has enabled trajectory calculation from a single trailer to multiple trailers, motion trajectory planning for tractors and trailers on inclined ramps, and reverse control of multiple trailers.

[0004] However, these studies all assume that the tractor and trailer move on the same plane. For large cargo loading scenarios on roll-on / roll-off ships, due to the different heights of the dock and the ship's deck, the tractor-trailer will move on different planes when traversing the inclined ramp connecting the dock and the ship's deck. Therefore, it is necessary to plan the tractor-trailer's transport trajectory in three-dimensional space, an area that existing algorithms have not yet addressed.

[0005] In view of this, the present application proposes a tractor-trailer trajectory planning method in three-dimensional space to solve the above problems. Summary of the Invention

[0006] The present invention provides a tractor-trailer trajectory planning method in three-dimensional space, aiming to overcome the limitation of existing trajectory planning algorithms that cannot plan in three-dimensional space, so as to achieve smooth loading of large cargo on roll-on / roll-off ships.

[0007] In one aspect, the present application provides a remote control method based on an automated forging and stamping die, comprising:

[0008] Acquiring environmental information of the tractor-trailer and creating a data file based on the environmental information;

[0009] Establishing a motion environment of the tractor-trailer in three-dimensional space based on the data file;

[0010] Planning the trajectory of the tractor in the motion environment based on the geometric properties of the Bezier curve;

[0011] Determine the positional relationship between the tractor and the trailer based on the motion environment, and calculate the tractor-trailer trajectory in a two-dimensional plane and a three-dimensional space;

[0012] sorting the tractor-trailer trajectories in three-dimensional space based on the tractor trajectory lengths, and performing collision detection on the sorted tractor-trailer trajectories in three-dimensional space using triangle characteristics;

[0013] The target trajectory of the tractor-trailer in three-dimensional space is determined based on the detection results and the target trajectory is verified.

[0014] Furthermore, determining the positional relationship between the tractor and the trailer based on the motion environment and calculating the tractor-trailer trajectory in a two-dimensional plane and a three-dimensional space includes:

[0015] Assume that the trailer is on plane 1 and the tractor is on plane 2, the initial time is t, and the boundary line between plane 1 and plane 2 is DE;

[0016] When the tractor and trailer are on the same plane, calculate the trailer trajectory of the tractor-trailer in the two-dimensional plane;

[0017] The angle Ψ between the tractor and trailer is calculated based on the motion parameters of the tractor and trailer at time t. (t) and the angular velocity of the trailer center point ω C(t) ;

[0018] Based on the angle Ψ between the tractor and trailer (t) , trailer center angular velocity ω C(t) Calculate the trailer body direction vector at the next moment and the trailer position coordinates C at the next moment (t+Δt) ;

[0019] Iterate the trailer position coordinates at the next moment according to the time increment to generate the trailer trajectory;

[0020] When the tractor and trailer are on different planes, the trailer trajectory of the tractor-trailer in three-dimensional space is calculated based on whether the front and rear wheels of the tractor are on the same plane.

[0021] Furthermore, when the tractor and the trailer are on different planes, calculating the trailer trajectory of the tractor-trailer in three-dimensional space according to whether the front wheels and rear wheels of the tractor are on the same plane includes:

[0022] When the front and rear wheels of the tractor are on the same plane 2 and the trailer is on plane 1,

[0023] Based on the geodesic theory, the speed direction vector of the trailer on plane 1 at time t is Converted to the trailer speed direction vector of plane 2 And according to the trailer speed direction vector of plane 2 and the tractor vehicle body direction vector Calculate the angle Ψ between the tractor and trailer on plane 2 (t) and the angular velocity ω of the trailer center point on plane 2 C(t) ;

[0024] Based on the direction angle Ψ between the tractor and trailer on plane 2 (t) , trailer center angular velocity ω C(t) and the tractor steering speed ω T(t) , calculate the direction angle Ψ between the tractor and trailer on plane 2 at the next moment (t+Δt) ;

[0025] Based on the direction angle Ψ between the tractor and trailer at the next moment (t+Δt) , tractor body direction vector and the normal vector of plane 2 Calculate the speed direction of the trailer on plane 2 at the next moment

[0026] Based on the geodesic theory, the velocity direction of the trailer on plane 2 at the next moment is calculated. Converted to the speed direction of the trailer on plane 1

[0027] Based on the speed direction of the trailer on plane 1 at the next moment Calculate the trailer position coordinate C on plane 1 at the next moment (t+Δt) ;

[0028] The trailer position coordinates at the next moment are iterated according to the time increment to generate the trailer trajectory.

[0029] Furthermore, when the tractor and the trailer are in different planes, calculating the trailer trajectory of the tractor-trailer in three-dimensional space based on whether the tractor and the front wheels and the rear wheels are in the same plane also includes:

[0030] When the front wheels of the tractor are in plane 2, and the rear wheels of the tractor and the trailer are in plane 1,

[0031] Based on the speed direction of the tractor's rear wheels Construct the tractor rear wheel trajectory path Tractor rear wheel track path The intersection point with the boundary line DE is B (t) ;

[0032] Construct the speed direction vector of the tractor's rear wheel from plane 1 to plane 2 based on geodesic theory

[0033] Based on the tractor trajectory path and the speed direction vector of the tractor's rear wheel on plane 2 Calculate the position coordinate A of the front wheel of the tractor on plane 2 (t) ;

[0034] Based on the position coordinate A of the front wheel of the tractor on plane 2 (t) Construct the trajectory path from the front wheels to the rear wheels of the tractor Among them, the trajectory path The intersection point with the boundary line DE is B (t) ;

[0035] If the rear wheels of the tractor reach the intersection B (t) Distance|T (t) B (t) If the preset judgment formula is met, the trailer trajectory is calculated based on the assumption that the tractor and trailer are on the same plane 1;

[0036] If the rear wheels of the tractor reach the intersection B (t) Distance|T (t) B (t) If the preset judgment formula is not met, the trailer trajectory is calculated based on the assumption that the front and rear wheels of the tractor are on the same plane 2 and the trailer is on plane 1.

[0037] Furthermore, the angle Ψ between the tractor and the trailer is (t) The calculation formula is:

[0038]

[0039] The trailer center point angular velocity ω C(t) The calculation formula is:

[0040]

[0041] in, is the trailer body direction vector, is the direction vector of the tractor body, v T(t) is the tractor speed, ω T(t) is the steering speed of the tractor, e1 is the distance from the rear wheel of the tractor to the tractor-trailer connection point, and d2 is the distance from the trailer to the connection point;

[0042] The calculation of the trailer body direction vector at the next moment The calculation formula is:

[0043]

[0044] in, is the normal vector of plane 1, Δt is the time increment;

[0045] The trailer position coordinate C at the next moment (t+Δt) The calculation formula is:

[0046]

[0047] Among them, T (t+Δt) is the position coordinate of the tractor at the next moment, is the direction vector of the tractor vehicle body at the next moment.

[0048] Furthermore, the direction angle Ψ between the tractor and the trailer on the plane 2 is (t) The calculation formula is:

[0049]

[0050] The angular velocity ω of the trailer center point on the plane 2 C(t) The calculation formula is:

[0051]

[0052] Among them, v T(t) is the tractor speed, e1 is the distance from the tractor rear wheel to the tractor-trailer connection point, and d2 is the distance from the trailer to the connection point;

[0053] The direction angle Ψ between the tractor and the trailer on plane 2 at the next moment (t+Δt) The calculation formula is: (t+Δt) =Ψ (t) +(ω T(t) -ω C(t) )△t;

[0054] The speed direction of the trailer on plane 2 at the next moment The calculation formula is:

[0055]

[0056] Furthermore, the calculation of the trailer position coordinates C at the next moment (t+Δt) ,include:

[0057] Construct the trailer position coordinate C at the next moment (t+Δt) and the tractor-trailer connection point H at the next moment (t+Δt) The trajectory path H(t+Δt) B (t+Δt) C (t+Δt) ;

[0058] Among them, the trajectory path H (t+Δt) B (t+Δt) C (t+Δt) The intersection point with the boundary line DE is B (t+Δt) ;

[0059] The acquisition of H (t+Δt) The method is:

[0060] The acquisition B (t+Δt) The method is: Where,

[0061] Among them, T (t+Δt) is the position coordinate of the rear wheel of the tractor at the next moment, and e1 is the distance from the rear wheel of the tractor to the tractor-trailer connection point H at the next moment. (t+Δt) distance, is the normal vector of plane 1, is the speed direction of the trailer on plane 2 at the next moment;

[0062] The trailer position coordinate C at the next moment (t+Δt) The calculation formula is:

[0063] Among them, get |B (t+Δt) C (t+Δt) The calculation formula for | is:

[0064]

[0065] Furthermore, the position coordinates A of the front wheels of the tractor on the plane 2 are obtained. (t) The calculation formula is:

[0066] (d1+e1) 2 =|T (t) B (t) | 2 +|B (t) A (t) | 2 -2|T (t) B (t) ||B (t) A (t) |cos(∠T (t) B (t) A (t) ), where Among them, A (t) is the position coordinate of the front wheel of the tractor on plane 2, T (t) B(t) From the rear wheel of the tractor to the intersection B (t) The distance, B (t) A (t) From the front wheel of the tractor to the intersection B (t) The distance d1 is from the front wheel of the tractor to the tractor-trailer connection point H. (t) e1 is the distance from the rear wheel of the tractor to the tractor-trailer connection point H. (t) distance;

[0067] The judgment formula is:

[0068] Furthermore, the tractor trajectory is planned in the motion environment based on the geometric characteristics of the Bezier curve, including: setting passing points, passing directions and control points in the motion environment, and generating a Bezier curve based on the passing points, passing directions and control points.

[0069] Furthermore, the collision detection of the tractor-trailer trajectory in three-dimensional space using triangle characteristics includes:

[0070] Construct a tractor-trailer triangle system consisting of a tractor, trailer and cargo;

[0071] Construct an obstacle triangle system consisting of ramp side baffles, locks and obstacles on the deck;

[0072] Collision detection for tractor-trailer triangle system and obstacle triangle system;

[0073] During the tractor-trailer motion process, the triangle characteristics are used to perform collision detection on all triangles of the tractor-trailer triangle system and the obstacle triangle system according to time increments.

[0074] If there is no collision, continue to detect the subsequent trajectory;

[0075] If a collision occurs, the subsequent trajectory will no longer be detected.

[0076] Compared with the prior art, the present invention has the following advantages:

[0077] This application addresses the issue of safe loading of large cargo on roll-on / roll-off ships by utilizing the geometric properties of Bezier curves to generate a smoother trajectory path for the tractor to facilitate the driver's operation; based on geodesic theory, the motion relationship between the tractor and trailer is expanded from a two-dimensional plane to a three-dimensional space, breaking through the limitations of existing trailer trajectory calculation methods and making the resulting trajectory more accurate and reliable; this application considers the efficiency and safety of cargo loading, arranges and performs collision detection on all trajectories, and selects a collision-free path with the shortest transportation distance for the tractor-trailer system; the trajectory planning method proposed in this application can be applied to different types of transportation scenarios and has wide and universal applicability.

[0078] The technical solution of the present application is further described in detail below through the accompanying drawings and examples. BRIEF DESCRIPTION OF THE DRAWINGS

[0079] The accompanying drawings, which are incorporated in and constitute a part of the specification, illustrate embodiments of the present application and, together with the description, serve to explain the principles of the present application.

[0080] The present application can be more clearly understood from the following detailed description with reference to the accompanying drawings, in which:

[0081] Figure 1 This is a flow chart of a tractor-trailer trajectory planning method in three-dimensional space proposed in this application;

[0082] Figure 2 Schematic diagram of boundary line and velocity plane division of a tractor-trailer trajectory planning method in three-dimensional space proposed in this application;

[0083] Figure 3 A schematic diagram of generating Bezier curves between passing points of a tractor-trailer trajectory planning method in three-dimensional space proposed in this application;

[0084] Figure 4 A schematic diagram of the shapes and sizes of a tractor-trailer, cargo, and a ship in a simulation example of a tractor-trailer trajectory planning method in three-dimensional space proposed in this application;

[0085] Figure 5 A schematic diagram of the target trajectory in the three-dimensional space of the tractor-trailer for the structure of the electronic device proposed in this application;

[0086] Figure 6 This is a schematic diagram of target trajectory collision detection results of a tractor-trailer trajectory planning method in three-dimensional space proposed in this application;

[0087] Figure 7 A schematic diagram of the instantaneous motion state of a tractor and trailer on the same plane in a tractor-trailer trajectory planning method in three-dimensional space proposed in this application;

[0088] Figure 8 Schematic diagram of the motion states of the tractor and trailer on different planes in a tractor-trailer trajectory planning method in three-dimensional space proposed in this application;

[0089] Figure 9 Schematic diagram of the instantaneous motion states of the tractor and trailer on different planes in a tractor-trailer trajectory planning method in three-dimensional space proposed in this application;

[0090] Figure 10 A schematic diagram of calculating the position of the tractor's front wheels in a tractor-trailer trajectory planning method in three-dimensional space proposed in this application;

[0091] Figure 11 A schematic diagram of a collision model of a tractor-trailer triangle system and an obstacle triangle system in a tractor-trailer trajectory planning method in three-dimensional space proposed in this application;

[0092] Figure 12 A schematic diagram of collision detection between triangles and triangle edges in a tractor-trailer trajectory planning method in three-dimensional space proposed in this application. DETAILED DESCRIPTION

[0093] Exemplary embodiments of the present disclosure will be described in more detail below with reference to the accompanying drawings. Although exemplary embodiments of the present disclosure are shown in the accompanying drawings, it should be understood that the present disclosure can be implemented in various forms and should not be limited by the embodiments set forth herein. On the contrary, these embodiments are provided to enable a more thorough understanding of the present disclosure and to fully convey the scope of the present disclosure to those skilled in the art. It should be noted that, unless there is a conflict, the embodiments of the present disclosure and the features in the embodiments can be combined with each other. The present invention will be described in detail below with reference to the accompanying drawings and in conjunction with the embodiments.

[0094] In some embodiments of the present application, a method for trajectory planning of a tractor-trailer in three-dimensional space is provided, comprising:

[0095] S1. Acquire environmental information of the tractor-trailer and create a data file based on the environmental information;

[0096] S2. Establishing a motion environment of the tractor-trailer in three-dimensional space based on the data file;

[0097] S3, planning a tractor trajectory in the motion environment based on the geometric properties of the Bezier curve;

[0098] S4. Determine the positional relationship between the tractor and the trailer based on the motion environment, and calculate the tractor-trailer trajectory in a two-dimensional plane and a three-dimensional space;

[0099] S5. Sort the tractor-trailer trajectories in the three-dimensional space based on the tractor trajectory lengths, and perform collision detection on the sorted tractor-trailer trajectories in the three-dimensional space using triangle characteristics;

[0100] S6. Determine a target trajectory of the tractor-trailer in three-dimensional space based on the detection results, and verify the target trajectory.

[0101] Specifically, the data file contains shape and position data of the dock, ramps, locks, decks, and obstacles on the deck, shape and size data of tractors, trailers, and cargo, as well as information on the starting position and direction of the tractors and trailers on the dock, and the transport destination position of the cargo on the deck.

[0102] Specifically, to establish the motion environment of a tractor-trailer in three-dimensional space, we first make a scenario assumption. Since the transported cargo is large, for safety reasons, we assume the tractor's moving speed is low and constant. Therefore, when planning the tractor and trailer's trajectory, we ignore the vehicle's acceleration and deceleration processes and only consider the vehicle's geometric motion trajectory. Secondly, we ignore the potential slippage that may occur during vehicle motion. The tractor and trailer's path may change due to wheel slip. However, the friction coefficient between the wheel and the road surface depends not only on the materials of the wheels, docks, ramps, and ship decks, but also on weather factors, cargo weight, tire deformation, and wheel wear, resulting in a plethora of variables to consider. Therefore, to simplify the calculation, based on the aforementioned low and constant speed assumption, this application ignores vehicle slippage during the path calculation process. Third, for ease of calculation, we assume the trailer has only two rear wheels. The motion trajectory of the tractor and trailer can be described by the motion trajectories of the center points of the tractor and trailer's two rear wheels. Therefore, the trailer's trajectory is the trajectory of the trailer's rear wheels, and the trailer's position coordinates are the coordinates of the trailer's rear wheels. Fourth, assuming that the vehicle's turning speed is zero when it crosses the boundary connecting the pier and the inclined ramp, the vehicle's trajectory will have the same incident and exit angles, that is, it moves along the geodesic direction.

[0103] Specifically, there are countless possible paths for the tractor from its starting position to its target position. To reduce the number of paths from infinite to finite, this application divides the entire motion environment from the tractor's starting position to the target position into multiple intervals and creates a finite number of possible paths for each interval. Then, the possible paths in each interval are combined to generate a complete tractor path. To achieve tractor trajectory generation, after establishing the tractor-trailer motion environment based on the data file, as shown in the following example: Figure 2As shown, boundary lines ①, ②, and ③ are established at the ramp's edges, boundary line ④ is established at the ramp-lock junction, and boundary lines ⑤ and ⑥ are established on the deck based on the vertices of the obstacle. These boundary lines divide the ramp and deck into different zones. Because the tractor and trailer's speed directions always lie above these zone planes, these zone planes are referred to as the tractor-trailer velocity planes.

[0104] Specifically, if Figure 3 As shown, multiple passing points are set at appropriate intervals on each boundary line, and several possible passing directions of the tractor are set at each "passing point", with the interval preferably being 50 cm. The number of passing points and passing directions set is preferably three. Secondly, to generate a smooth path in each interval, two control points are created at an appropriate distance in each passing direction of each passing point on the adjacent boundary line. According to experience, in order to avoid the generation of trajectory paths with sharp curvature, the appropriate distance is taken as one-third of the distance between the two passing points. Finally, a third-order Bezier curve is generated in each area using the de-Casteljau algorithm, and the Bezier curves in each interval are connected from the starting position to the ending position of the tractor to plan all possible tractor trajectories.

[0105] It can be understood that, through the above scheme, after the tractor trajectory is planned, the instantaneous state motion parameter data on each trajectory of the tractor are known, and subsequent calculations can be performed using the known position coordinates of each point of the tractor and the body vector data.

[0106] Specifically, in the process of transporting cargo from the dock to the designated location on the deck through an inclined ramp, the trajectory of the trailer in the two-dimensional plane when the tractor and trailer are on the same plane and the trajectory of the trailer in the three-dimensional space when the tractor and trailer are on different planes should be calculated respectively.

[0107] Specifically, when the tractor and trailer move on different planes, the motion relationship of the tractor and trailer is converted between the two-dimensional plane and the three-dimensional space through the geodesic theory, so as to calculate the corresponding trajectory of the trailer in the three-dimensional space.

[0108] Specifically, to ensure the safety of the loading process, collision detection is required for the tractor-trailer trajectory. By performing collision detection on the generated tractor-trailer trajectories one by one in the order of the tractor trajectory length, the target trajectory can be found, which is the optimal trajectory with the shortest transportation distance and no collisions.

[0109] Specifically, in order to verify the rationality of the target trajectory, this application built a simulation platform on the Microsoft Windows 11 system through Microsoft Visual Studio 2017 and C++ MFC (Microsoft Foundation Class), and realized the visualization of the simulation platform through OpenGL. Figure 4 The shapes and sizes of the tractor-trailer, cargo, and vessel used in the simulation example are shown. Figure 5 The paper demonstrates the optimal trajectories of a tractor and trailer when the height difference between the dock and the deck is 4.1m, as well as the movement of a tractor-trailer system from the dock through an inclined ramp and a ship lock. The results demonstrate that a tractor-trailer can transport large cargo from the dock to a designated location on the ship's deck according to the planned path, demonstrating that the proposed planning method can achieve trajectory planning for a tractor-trailer system in three-dimensional space.

[0110] Specifically, in order to prove the safety of the loading process, Figure 4 、 Figure 5 The model and loading trajectory in the paper were scaled 42 times and experimentally verified. Figure 6 The image shows the instantaneous state of the tractor-trailer at the most likely collision location during loading. The results demonstrate that the tractor-trailer system did not collide with ramp barriers, locks, or obstacles on the deck during transport, validating that the target trajectory of this application is the optimal trajectory with the shortest transport distance and no collisions.

[0111] It can be seen that this application uses the geometric characteristics of Bezier curves to solve the problem of safe loading of large cargo on roll-on / roll-off ships, and generates a smoother trajectory path for the tractor to facilitate the driver's operation; based on the geodesic theory, the motion relationship between the tractor and the trailer is expanded from a two-dimensional plane to a three-dimensional space, breaking through the limitations of the existing trailer trajectory calculation method, and making the obtained trajectory more accurate and reliable; this application takes into account the efficiency and safety of cargo loading, arranges and detects collisions of all trajectories, and selects a collision-free path with the shortest transportation distance for the tractor-trailer system; the trajectory planning method proposed in this application can be applied to different types of transportation scenarios and has wide and universal applicability.

[0112] In some embodiments of the present application, the positional relationship between the tractor and the trailer is determined based on the motion environment, and the trailer trajectory of the tractor-trailer in a two-dimensional plane and a three-dimensional space is calculated.

[0113] Specifically, assume that the trailer is on plane 1, the tractor is on plane 2, the initial time is t, and the boundary line between plane 1 and plane 2 is DE.

[0114] Specifically, when the tractor and trailer are on the same plane, the trailer trajectory of the tractor-trailer in the two-dimensional plane is calculated; when the tractor and trailer are on different planes, the trailer trajectory of the tractor-trailer in the three-dimensional space is calculated based on whether the front and rear wheels of the tractor are on the same plane.

[0115] Specifically, when the tractor and trailer are on the same plane, the angle Ψ between the tractor and trailer is calculated based on the motion parameters of the tractor and trailer at time t. (t) and the angular velocity of the trailer center point ω C(t) .

[0116] Based on the angle Ψ between the tractor and trailer (t) , trailer center angular velocity ω C(t) Calculate the trailer body direction vector at the next moment and the trailer position coordinates C at the next moment (t+Δt) .

[0117] The trailer position coordinates at the next moment are iterated according to the time increment to generate the trailer trajectory.

[0118] Specifically, when the tractor and trailer move on the same plane, the instantaneous states of the tractor and trailer at any moment are as follows: Figure 7 At this moment, the trailer has a vector along the direction of the trailer body The driving speed and the steering angular velocity centered on the rear wheels. Combined with the motion parameters of the tractor at time t, the angle Ψ between the tractor and the trailer can be calculated (t) and the trailer center point C (t) Angular velocity ω C(t) .

[0119] Specifically, the tractor position coordinates T at the next moment are obtained according to the data file. (t+Δt) and the tractor vehicle body direction vector The position coordinates C of the trailer's rear wheels at the next moment can be calculated (t+Δt) .

[0120] Specifically, since the positions and directions of the tractor and trailer at the starting position are known, the complete trajectory of the trailer can be obtained by iterating based on the time increment Δt.

[0121] In some embodiments of the present application, when the tractor and trailer are on different planes, calculating the tractor-trailer trajectory in three-dimensional space based on whether the front wheels and rear wheels of the tractor are on the same plane includes:

[0122] When the front and rear wheels of the tractor are on the same plane 2 and the trailer is on plane 1,

[0123] Based on the geodesic theory, the speed direction vector of the trailer on plane 1 at time t is Converted to the trailer speed direction vector of plane 2 And according to the trailer speed direction vector of plane 2 and the tractor vehicle body direction vector Calculate the angle Ψ between the tractor and trailer on plane 2 (t) and the angular velocity ω of the trailer center point on plane 2 C(t) ; Based on the direction angle Ψ between the tractor and trailer on plane 2 (t) , trailer center angular velocity ω C(t) and the tractor steering speed ω T(t) , calculate the direction angle Ψ between the tractor and trailer on plane 2 at the next moment (t+Δt) ; Based on the direction angle Ψ between the tractor and trailer at the next moment (t+Δt) , tractor body direction vector and the normal vector of plane 2 Calculate the speed direction of the trailer on plane 2 at the next moment Based on the geodesic theory, the velocity direction of the trailer on plane 2 at the next moment is calculated. Converted to the speed direction of the trailer on plane 1 Based on the speed direction of the trailer on plane 1 at the next moment Calculate the trailer position coordinate C on plane 1 at the next moment (t+Δt) ; Iterate the trailer position coordinates at the next moment according to the time increment to generate the trailer trajectory.

[0124] Specifically, if Figure 8 As shown in Figure 2, when the tractor passes through the inclined ramp plane 2, the trailer is still moving on the dock plane 1, and the tractor and trailer are located on different planes. In this case, the movement of the tractor and trailer is in three-dimensional space, not on a two-dimensional plane. In addition, when only the front wheels of the tractor pass through the boundary between the ramp and the deck, as shown in Figure 2, the tractor and trailer are located on different planes. Figure 8 (a) shows that both the front and rear wheels pass through the boundary Figure 8 As shown in (b), the trailer trajectory is calculated slightly differently.

[0125] Specifically, when the front and rear wheels of the tractor pass the boundary between the ramp and the deck, that is, when the front and rear wheels of the tractor are on plane 2 and the trailer is on plane 1, the velocity direction vector of the trailer on plane 1 is calculated using geodesic theory. Convert to a vector on plane 2 like Figure 9 As shown in (a), at this moment, the angle Ψ between the tractor and trailer on plane 2 can be calculated (t) and the trailer's rotational speed ω C(t) .

[0126] Specifically, the velocity direction of the trailer on plane 1 is inversely calculated by geodesic theory. like Figure 9 (b) shows the direction vector of the tractor body obtained from the data file. and the normal vector of plane 2 The trailer position coordinates C at the next moment on plane 1 can be further calculated (t+Δt) , based on the time increment Δt, the above steps are repeated to calculate the complete trailer motion trajectory.

[0127] In some embodiments of the present application, when the tractor and the trailer are in different planes, the tractor-trailer trajectory in three-dimensional space is calculated based on whether the tractor and the front wheels and the rear wheels are in the same plane, further comprising: when the tractor front wheels are in plane 2 and the tractor rear wheels and the trailer are in plane 1,

[0128] Based on the speed direction of the tractor's rear wheels Construct the tractor rear wheel trajectory path Tractor rear wheel track path The intersection point with the boundary line DE is B (t) ; Based on the geodesic theory, the speed direction vector of the tractor's rear wheel from plane 1 to plane 2 is constructed Based on the tractor trajectory path and the speed direction vector of the tractor's rear wheel on plane 2 Calculate the position coordinate A of the front wheel of the tractor on plane 2 (t) ; Based on the position coordinate A of the front wheel of the tractor on plane 2 (t) Construct the trajectory path from the front wheels to the rear wheels of the tractor Among them, the trajectory path The intersection point with the boundary line DE is B (t) ; If the rear wheels of the tractor reach the intersection B (t) Distance|T (t) B (t) If the preset judgment formula is met, the trailer trajectory is calculated based on the assumption that the tractor and trailer are on the same plane 1;

[0129] If the rear wheels of the tractor reach the intersection B (t) Distance|T (t) B (t) If the preset judgment formula is not met, the trailer trajectory is calculated based on the assumption that the front and rear wheels of the tractor are on the same plane 2 and the trailer is on plane 1.

[0130] Specifically, when only the front wheels of the tractor pass the boundary between the ramp and the deck, e.g. Figure 10 As shown, along the tractor speed direction Constructing trajectory paths Then, the speed direction vector of the rear wheel of the tractor is constructed based on the geodesic theory Make geodesic trajectory In B (t)The incident angle and the exit angle are the same, and the position coordinate A of the front wheel of the tractor on plane 2 can be calculated. (t) .

[0131] Specifically, if at a certain moment the rear wheel of the tractor reaches the intersection B (t) Distance|T (t) B (t) |Satisfy the preset judgment formula, indicating the connection point H between the tractor and the trailer (t) On the trajectory path The corresponding point H′ on (t) On plane 1, at this time, the tractor and trailer are on the same plane 1, and the trailer trajectory is calculated based on the tractor and trailer being on the same plane 1. If the rear wheel of the tractor reaches the intersection point B at a certain moment (t) Distance|T (t) B (t) |The preset judgment formula is not satisfied, indicating that the connection point H between the tractor and the trailer (t) The corresponding point H′ on the trajectory path (t) On plane 2, at this time, the tractor and the trailer are not on the same plane, that is, the trailer trajectory is calculated based on the assumption that the front and rear wheels of the tractor are on the same plane 2 and the trailer is on plane 1.

[0132] In some embodiments of the present application, when the front wheels and rear wheels of the tractor are on the same plane 2 and the trailer is on plane 1, the angle Ψ between the tractor and the trailer is (t) The calculation formula is:

[0133] The trailer center point angular velocity ω C(t) The calculation formula is:

[0134]

[0135] in, is the trailer body direction vector, is the direction vector of the tractor body, v T(t) is the tractor speed, ω T(t) is the steering speed of the tractor, e1 is the distance from the rear wheel of the tractor to the tractor-trailer connection point, and d2 is the distance from the trailer to the connection point;

[0136] The calculation of the trailer body direction vector at the next moment The calculation formula is:

[0137]

[0138] in, is the normal vector of plane 1, Δt is the time increment;

[0139] The trailer position coordinate C at the next moment (t+Δt) The calculation formula is:

[0140]

[0141] Among them, T (t+Δt) is the position coordinate of the tractor at the next moment, is the direction vector of the tractor vehicle body at the next moment.

[0142] In some embodiments of the present application, the angle Ψ between the tractor and the trailer on plane 2 is (t) The calculation formula is:

[0143]

[0144] The angular velocity ω of the trailer center point on the plane 2 C(t) The calculation formula is:

[0145]

[0146] Among them, v T(t) is the tractor speed, e1 is the distance from the tractor rear wheel to the tractor-trailer connection point, and d2 is the distance from the trailer to the connection point;

[0147] The direction angle Ψ between the tractor and the trailer on plane 2 at the next moment (t+Δt) The calculation formula is: (t+Δt) =Ψ (t) +(ω T(t) -ω C(t) )△t;

[0148] The speed direction of the trailer on plane 2 at the next moment The calculation formula is:

[0149]

[0150] In some embodiments of the present application, when the front wheels and rear wheels of the tractor are on the same plane 2 and the trailer is on plane 1, the position coordinates C of the trailer at the next moment are calculated. (t+Δt) ,include:

[0151] Construct the trailer position coordinate C at the next moment (t+Δt) and the tractor-trailer connection point H at the next moment (t+Δt) The trajectory path H (t+Δt) B (t+Δt) C (t+Δt) ;

[0152] Among them, the trajectory path H (t+Δt) B (t+Δt) C (t+Δt) The intersection point with the boundary line DE is B(t+Δt) ;

[0153] The acquisition of H (t+Δt) The method is:

[0154] The acquisition B (t+Δt) The method is: Where,

[0155] Among them, T (t+Δt) is the position coordinate of the rear wheel of the tractor at the next moment, and e1 is the distance from the rear wheel of the tractor to the tractor-trailer connection point H at the next moment. (t+Δt) distance, is the normal vector of plane 1, is the speed direction of the trailer on plane 2 at the next moment;

[0156] The trailer position coordinate C at the next moment (t+Δt) The calculation formula is:

[0157] Among them, get |B (t+Δt) C (t+Δt) The calculation formula for | is:

[0158]

[0159] In some embodiments of the present application, when the front wheels of the tractor are on plane 2 and the rear wheels of the tractor and the trailer are on plane 1, the position coordinates A of the front wheels of the tractor on plane 2 are obtained. (t) The calculation formula is:

[0160] (d1+e1) 2 =|T (t) B (t) | 2 +|B (t) A (t) | 2 -2|T (t) B (t) ||B (t) A (t) |cos(∠T (t) B (t) A (t) ), where Among them, A (t) is the position coordinate of the front wheel of the tractor on plane 2, T (t) B (t) From the rear wheel of the tractor to the intersection B (t) The distance, B (t) A (t) From the front wheel of the tractor to the intersection B (t)The distance d1 is from the front wheel of the tractor to the tractor-trailer connection point H. (t) e1 is the distance from the rear wheel of the tractor to the tractor-trailer connection point H. (t) distance;

[0161] The judgment formula is:

[0162] In some embodiments of the present application, planning a tractor trajectory in the motion environment based on the geometric characteristics of the Bezier curve includes: setting passing points, passing directions and control points in the motion environment, and generating a Bezier curve based on the passing points, passing directions and control points.

[0163] In some embodiments of the present application, collision detection of a tractor-trailer trajectory in three-dimensional space is performed using triangle characteristics, including:

[0164] Construct a tractor-trailer triangle system consisting of a tractor, trailer and cargo;

[0165] Construct an obstacle triangle system consisting of ramp side baffles, locks and obstacles on the deck;

[0166] Collision detection for tractor-trailer triangle system and obstacle triangle system;

[0167] During the tractor-trailer motion process, the triangle characteristics are used to perform collision detection on all triangles of the tractor-trailer triangle system and the obstacle triangle system according to time increments.

[0168] If there is no collision, continue to detect the subsequent trajectory;

[0169] If a collision occurs, the subsequent trajectory will no longer be detected.

[0170] Specifically, if Figure 11 As shown in the figure, both the tractor-trailer triangle system and the obstacle triangle system are composed of STL triangle meshes. Using the characteristics of triangles, collision detection is performed on the tractor-trailer triangle system and the obstacle triangle system. If a collision occurs during loading, the triangle ΔABC that makes up the tractor-trailer system will intersect the triangle ΔDEF that makes up the obstacle system.

[0171] Specifically, if one edge of a triangle intersects another triangle, the two triangles collide. Figure 12 As shown, in order to determine the triangle

[0172] Whether a collision occurs, use vector Represents point A, sides AB, AC, and introduces two parameters m and n to represent the ΔABC plane; for ΔDEF, use vectors Represents point D and edge DE. Finally, after introducing parameter k, the formula We can determine whether DE and ABC intersect. In other words, if the parameters m, n, and k satisfy the above equation, then DE and ABC intersect; otherwise, DE and ABC do not intersect.

[0173] Specifically, during the tractor-trailer's motion, collision detection is performed on all triangles of the tractor-trailer system and the obstacle system at all locations, based on time increments. Since collisions must be avoided during transport, to save detection time, once a collision occurs, the remaining path after the collision is not checked. Finally, collision detection is performed on each generated trajectory in the order in which they are arranged, to find the target trajectory with the shortest transport distance and no collisions.

[0174] It should be noted that:

[0175] Those skilled in the art will readily appreciate other embodiments of the present application after considering the specification and practicing the invention disclosed herein. This application is intended to cover any variations, uses, or adaptations of the present application that follow the general principles of this application and include common knowledge or customary techniques in the art not disclosed herein. The description and examples are to be considered as exemplary only, and the true scope and spirit of the present application are indicated by the following claims.

[0176] It should be understood that the present application is not limited to the exact structures described above and shown in the drawings, and that various modifications and changes may be made without departing from the scope thereof. The scope of the present application is limited only by the appended claims.

Claims

1. A tractor-trailer trajectory planning method in three-dimensional space, characterized in that: Acquiring environmental information of the tractor-trailer and creating a data file based on the environmental information; Establishing a motion environment of the tractor-trailer in three-dimensional space based on the data file; Planning the trajectory of the tractor in the motion environment based on the geometric properties of the Bezier curve; Determine the positional relationship between the tractor and the trailer based on the motion environment, and calculate the tractor-trailer trajectory in a two-dimensional plane and a three-dimensional space; sorting the tractor-trailer trajectories in three-dimensional space based on the tractor trajectory lengths, and performing collision detection on the sorted tractor-trailer trajectories in three-dimensional space using triangle characteristics; The target trajectory of the tractor-trailer in three-dimensional space is determined based on the detection results and the target trajectory is verified.

2. The method for tractor-trailer trajectory planning in three-dimensional space according to claim 1, characterized in that: Determining the positional relationship between the tractor and the trailer based on the motion environment, and calculating the tractor-trailer trajectory in a two-dimensional plane and a three-dimensional space, including: Assume that the trailer is on plane 1 and the tractor is on plane 2, the initial time is t, and the boundary line between plane 1 and plane 2 is DE; When the tractor and trailer are on the same plane, calculate the trailer trajectory of the tractor-trailer in the two-dimensional plane; The angle Ψ between the tractor and trailer is calculated based on the motion parameters of the tractor and trailer at time t. (t) and the angular velocity of the trailer center point ω C(t) ; Based on the angle Ψ between the tractor and trailer (t) , trailer center angular velocity ω C(t) Calculate the trailer body direction vector at the next moment and the trailer position coordinates C at the next moment (t+Δt) ; Iterate the trailer position coordinates at the next moment according to the time increment to generate the trailer trajectory; When the tractor and trailer are on different planes, the trailer trajectory of the tractor-trailer in three-dimensional space is calculated based on whether the front and rear wheels of the tractor are on the same plane.

3. The method for tractor-trailer trajectory planning in three-dimensional space according to claim 2, characterized in that: When the tractor and the trailer are on different planes, calculating the trailer trajectory of the tractor-trailer in three-dimensional space based on whether the front wheels and the rear wheels of the tractor are on the same plane includes: When the front and rear wheels of the tractor are on the same plane 2 and the trailer is on plane 1, Based on the geodesic theory, the speed direction vector of the trailer on plane 1 at time t is Converted to the trailer speed direction vector of plane 2 And according to the trailer speed direction vector of plane 2 and the tractor vehicle body direction vector Calculate the angle Ψ between the tractor and trailer on plane 2 (t) and the angular velocity ω of the trailer center point on plane 2 C(t) ; Based on the direction angle Ψ between the tractor and trailer on plane 2 (t) , trailer center angular velocity ω C(t) and the tractor steering speed ω T(t) , calculate the direction angle Ψ between the tractor and trailer on plane 2 at the next moment (t+Δt) ; Based on the direction angle Ψ between the tractor and trailer at the next moment (t+Δt) , tractor body direction vector and the normal vector of plane 2 Calculate the speed direction of the trailer on plane 2 at the next moment Based on the geodesic theory, the velocity direction of the trailer on plane 2 at the next moment is calculated. Converted to the speed direction of the trailer on plane 1 Based on the speed direction of the trailer on plane 1 at the next moment Calculate the trailer position coordinate C on plane 1 at the next moment (t+Δt) ; The trailer position coordinates at the next moment are iterated according to the time increment to generate the trailer trajectory.

4. The method for tractor-trailer trajectory planning in three-dimensional space according to claim 3, characterized in that: When the tractor and the trailer are in different planes, calculating the trailer trajectory of the tractor-trailer in three-dimensional space based on whether the tractor and the front wheels and the rear wheels are in the same plane also includes: When the front wheels of the tractor are in plane 2, and the rear wheels of the tractor and the trailer are in plane 1, Based on the speed direction of the tractor's rear wheels Construct the tractor rear wheel trajectory path Tractor rear wheel track path The intersection point with the boundary line DE is B (t) ; Construct the speed direction vector of the tractor's rear wheel from plane 1 to plane 2 based on geodesic theory Based on the tractor trajectory path and the speed direction vector of the tractor's rear wheel on plane 2 Calculate the position coordinate A of the front wheel of the tractor on plane 2 (t) ; Based on the position coordinate A of the front wheel of the tractor on plane 2 (t) Construct the trajectory path from the front wheels to the rear wheels of the tractor Among them, the trajectory path The intersection point with the boundary line DE is B (t) ; If the rear wheels of the tractor reach the intersection B (t) Distance|T (t) B (t) If the preset judgment formula is met, the trailer trajectory is calculated based on the assumption that the tractor and trailer are on the same plane 1; If the rear wheels of the tractor reach the intersection B (t) Distance|T (t) B (t) If the preset judgment formula is not met, the trailer trajectory is calculated based on the assumption that the front and rear wheels of the tractor are on the same plane 2 and the trailer is on plane 1.

5. The method for tractor-trailer trajectory planning in three-dimensional space according to claim 2, characterized in that: The angle Ψ between the tractor and the trailer (t) The calculation formula is: The trailer center point angular velocity ω C(t) The calculation formula is: in, is the trailer body direction vector, is the direction vector of the tractor body, v T(t) is the tractor speed, ω T(t) is the steering speed of the tractor, e1 is the distance from the rear wheel of the tractor to the tractor-trailer connection point, and d2 is the distance from the trailer to the connection point; The calculation of the trailer body direction vector at the next moment The calculation formula is: in, is the normal vector of plane 1, Δt is the time increment; The trailer position coordinate C at the next moment (t+Δt) The calculation formula is: Among them, T (t+Δt) is the position coordinate of the tractor at the next moment, is the direction vector of the tractor vehicle body at the next moment.

6. The method for tractor-trailer trajectory planning in three-dimensional space according to claim 3, characterized in that: The included angle Ψ between the tractor and trailer on the plane 2 (t) The calculation formula is: The angular velocity ω of the trailer center point on the plane 2 C(t) The calculation formula is: Among them, v T(t) is the tractor speed, e1 is the distance from the tractor rear wheel to the tractor-trailer connection point, and d2 is the distance from the trailer to the connection point; The direction angle Ψ between the tractor and the trailer on plane 2 at the next moment (t+Δt) The calculation formula is: (t+Δt) =Ψ (t) +(ω T(t) -ω C(t) )Δt; The speed direction of the trailer on plane 2 at the next moment The calculation formula is:

7. The method for tractor-trailer trajectory planning in three-dimensional space according to claim 3, characterized in that: The calculation of the trailer position coordinates C at the next moment (t+Δt) ,include: Construct the trailer position coordinate C at the next moment (t+Δt) and the tractor-trailer connection point H( t+Δt) The trajectory path H (t+Δt) B (t+Δt) C (t+Δt) ; Among them, the trajectory path H (t+Δt) B (t+Δt) C (t+Δt) The intersection point with the boundary line DE is B (t+Δt) ; The acquisition of H (t+Δt) The method is: The acquisition B (t+Δt) The method is: Where, Among them, T (t+Δt) is the position coordinate of the rear wheel of the tractor at the next moment, and e1 is the distance from the rear wheel of the tractor to the tractor-trailer connection point H at the next moment. (t+Δt) distance, is the normal vector of plane 1, is the speed direction of the trailer on plane 2 at the next moment; The trailer position coordinate C at the next moment (t+Δt) The calculation formula is: Among them, get |B (t+Δt) C (t+Δt) The calculation formula for | is:

8. The method for tractor-trailer trajectory planning in three-dimensional space according to claim 4, characterized in that: Get the position coordinate A of the front wheel of the tractor on the plane 2 (t) The calculation formula is: (d1+e1) 2 =|T (t) B (t) | 2 +|B (t) A (t) | 2 -2|T (t) B (t) ||B (t) A (t) |cos(∠T (t) B (t) A (t) ), where Among them, A (t) is the position coordinate of the front wheel of the tractor on plane 2, T (t) B (t) From the rear wheel of the tractor to the intersection B (t) The distance, B (t) A (t) From the front wheel of the tractor to the intersection B (t) The distance d1 is from the front wheel of the tractor to the tractor-trailer connection point H. (t) e1 is the distance from the rear wheel of the tractor to the tractor-trailer connection point H. (t) distance; The judgment formula is:

9. The method for tractor-trailer trajectory planning in three-dimensional space according to claim 1, characterized in that: The method of planning a tractor trajectory in the motion environment based on the geometric characteristics of the Bezier curve includes setting a passing point, a passing direction and a control point in the motion environment, and generating a Bezier curve based on the passing point, the passing direction and the control point.

10. The method for tractor-trailer trajectory planning in three-dimensional space according to claim 1, characterized in that: The method of performing collision detection on the trajectory of the tractor-trailer in three-dimensional space by utilizing the triangle characteristics includes: Construct a tractor-trailer triangle system consisting of a tractor, trailer and cargo; Construct an obstacle triangle system consisting of ramp side baffles, locks and obstacles on the deck; Collision detection for tractor-trailer triangle system and obstacle triangle system; During the tractor-trailer motion process, the triangle characteristics are used to perform collision detection on all triangles of the tractor-trailer triangle system and the obstacle triangle system according to time increments. If there is no collision, continue to detect the subsequent trajectory; If a collision occurs, the subsequent trajectory will no longer be detected.

Images