A parking path planning method for trailer tractor based on HybridA* algorithm

Abstract

The application discloses a trailer tractor parking path planning method based on a Hybrid A* algorithm, first constructs a trailer tractor kinematic model, and describes abstract motion states into specific data; then based on the Hybrid A* algorithm, the trailer tractor kinematic model is brought into node updating, a reasonable evaluation function is set to screen the best driving node, and a trailer tractor kinematic parking path is solved. The method can make the trailer tractor quickly plan a trailer tractor kinematic parking path in any pose, and improve the parking efficiency and accuracy.

Description

Technical Field

[0001] The present invention belongs to the technical field of autonomous driving, and in particular relates to a parking path planning method for a tractor-trailer based on a Hybrid A* algorithm. Background Art

[0002] For most drivers, reversing a tractor-trailer is a difficult skill to master. This is because the trailer and tractor are traveling in opposite directions, and the trailer's large body, numerous blind spots, and relatively heavy weight increase the difficulty of estimating distance and angles. With the continuous advancement of autonomous driving technology, this paper addresses the practical needs of users who face difficulties parking a tractor-trailer. This paper proposes an automated parking path planning algorithm for tractors, which not only improves parking efficiency but also ensures parking safety. Summary of the Invention

[0003] The present invention proposes a parking path planning method for a tractor-trailer based on the Hybrid A* algorithm, which can realize rapid parking path planning of the tractor-trailer, provide a reference path for autonomous parking, and improve the parking efficiency of the tractor-trailer.

[0004] For parking path planning of a tractor-trailer, a trajectory that satisfies the vehicle's kinematics is particularly important. Therefore, it is necessary to build a kinematic model that conforms to the tractor-trailer's motion as the basis for subsequent algorithm development.

[0005] The following is a simple analysis of the formulas required for kinematics:

[0006] (1) Traction trajectory of the tractor:

[0007] Update the tractor vehicle yaw angle:

[0008] Update the coordinates of the center point of the tractor's rear axle:

[0009]

[0010]

[0011] (2) Trailer movement status:

[0012] Update trailer curvature:

[0013] Update trailer yaw angle:

[0014] The parameters represents the yaw angle of the tractor, represents the trailer yaw angle, s represents the simulation step length, ρ i represents the curvature of the tractor, ρ j represents the trailer curvature, λi Indicates the length from the saddle point to the rear axle of the tractor, L j Indicates the trailer length.

[0015] Specifically, the present invention provides a method for parking path planning for a tractor trailer based on a Hybrid A* algorithm, comprising:

[0016] Obtain the following vehicle parameters: the distance between the center of the tractor's rear axle and the front, rear, and saddle points of the tractor, the width and wheelbase of the tractor; the distance between the front and rear ends of the trailer and the saddle points, the width and wheelbase of the trailer; and the maximum steering angle of the tractor's wheels.

[0017] Obtaining the starting point and target point position information; the position information consists of the tractor position coordinates, the tractor yaw angle, and the trailer yaw angle;

[0018] Define planning simulation parameters: tractor driving step length, steering step length, grid map step length, yaw angle retention step length, and penalty cost coefficients;

[0019] Coordinate conversion: converting the coordinates of the tractor's starting point and target point to the corresponding coordinates on the grid map according to the grid map step size;

[0020] ReedsShepp connection: Use the ReedsShepp curve to connect the current motion node and the target point, perform collision detection on the connection curve, and retain valid connection lines; set the first penalty cost of the connection line; select the connection line with the smallest first penalty cost among the retained valid connection lines, and the path planning is completed.

[0021] As a further illustration of the present invention, the first penalty cost includes a vehicle driving direction change cost, a wheel steering change cost, and a vehicle driving distance.

[0022] As a further illustration of the present invention, the path planning method further includes: if there is no valid connection line, continuing to update the node.

[0023] As a further illustration of the present invention, the process of updating the node specifically includes:

[0024] Taking the current posture as the starting point of the tractor's movement, based on the tractor's kinematic model, the tractor is traversed in terms of steering and movement directions to obtain the vehicle's reachable node posture;

[0025] Perform collision detection on paths that can reach nodes and retain valid nodes; perform second penalty cost calculation on the valid nodes, filter and merge them, and sort them in ascending order according to the second penalty cost.

[0026] As a further illustration of the present invention, the collision detection includes: whether the tractor collides with the trailer when the tractor collides with the trailer; whether the tractor collides with a known obstacle point; and whether the tractor collides with the parking space envelope.

[0027] As a further illustration of the present invention, the second penalty cost = cumulative cost + heuristic cost;

[0028] The accumulated cost includes: vehicle direction change cost, wheel steering cost, wheel steering change cost, vehicle travel distance, and previous node accumulated cost;

[0029] The heuristic cost includes: the distance between the tractor and trailer and the target tractor and trailer, the difference between the yaw angle of the tractor and trailer and the yaw angle of the tractor and trailer at the target point, and the angle between the tractor and trailer.

[0030] Compared with the prior art, the present invention has the following beneficial technical effects:

[0031] Compared with single-rigid-body path planning, multi-rigid-body path planning algorithms are more complex and require more considerations. The present invention, based on dual-rigid-body kinematic analysis, can achieve rapid parking path planning for a trailer tractor, and can enable the trailer tractor to quickly plan a parking path that conforms to the kinematics of the trailer tractor in any position, thereby improving parking efficiency and accuracy, providing a reference path for autonomous parking, improving the parking efficiency of the trailer tractor, and effectively solving the parking path planning problem for the trailer tractor. BRIEF DESCRIPTION OF THE DRAWINGS

[0032] Figure 1 A flowchart of the implementation of the parking path planning method for a trailer tractor based on the Hybrid A* algorithm provided by the present invention;

[0033] Figure 2 Iterative logic block diagram of kinematic trajectory calculation of trailer tractor;

[0034] Figure 3 Trailer tractor hybrid a* path planning logic diagram;

[0035] Figure 4 Traditional Hybrid A* algorithm plans the path graph;

[0036] Figure 5 The embodiment of the present invention plans a path map for a tractor trailer based on a hybrid A* algorithm. DETAILED DESCRIPTION

[0037] To make the objectives, technical solutions, and advantages of the embodiments of the present invention more clear, the technical solutions of the embodiments of the present invention will be clearly and completely described below in conjunction with the accompanying drawings of the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Generally, the components of the embodiments of the present invention described and shown in the drawings herein can be arranged and designed in various different configurations.

[0038] Therefore, the following detailed description of the embodiments of the present invention provided in the accompanying drawings is not intended to limit the scope of the invention as claimed, but rather merely represents selected embodiments of the present invention. All other embodiments derived by persons of ordinary skill in the art based on the embodiments of the present invention without creative effort shall fall within the scope of protection of the present invention.

[0039] The technical solution of the present invention will be explained below with reference to specific embodiments.

[0040] An embodiment of the present invention provides a method for parking path planning of a tractor trailer based on the Hybrid A* algorithm, the process is as follows: Figure 1 As shown, the following steps are included:

[0041] Step 1. Obtain the following vehicle parameters for the towing tractor: the length from the center of the tractor's rear axle to the front of the tractor is 6 meters, the length from the center of the tractor's rear axle to the rear of the tractor is 1.5 meters, the tractor width is 2.4 meters, the tractor wheelbase is 5 meters, and the distance from the center of the tractor's rear axle to the saddle point is 0.5 meters; the distance from the front of the trailer to the saddle point is 1 meter, the distance from the rear of the trailer to the saddle point is 12 meters, the trailer width is 2.5 meters, and the trailer axle length is 8; the maximum steering angle of the tractor wheels is 30 degrees.

[0042] Step 2: Obtain the vehicle starting point posture information (tractor position x-coordinate 23, tractor position y-coordinate 35, tractor yaw angle 0°, trailer yaw angle 0°) and target parking point posture information (tractor position x-coordinate 0, tractor position y-coordinate 15, tractor yaw angle 90°, trailer yaw angle 90°); parking line topology data.

[0043] Step 3. Define the planning simulation parameters: the tractor driving step size is 1.5m, the trailer kinematic simulation step size is 0.1m, the wheel steering step size is 2°, the grid map step size is 1m, and the yaw angle retention step size is 5°; the cumulative cost coefficient is 2 except for the vehicle driving distance coefficient, which is 1; the distance coefficient of the tractor and trailer from the target point is 0.5, the difference coefficient between the yaw angle of the tractor and trailer and the target yaw angle is 1; and the angle coefficient between the tractor and trailer is 1.

[0044] Step 4: Convert the coordinates of the tractor's starting point and target point to corresponding coordinates on the grid map according to the grid map step size in the simulation parameters planned in step 3, and check whether a collision occurs.

[0045] Step 5: Create open and close lists for node storage. Push the starting point into the open list first.

[0046] Step 6. Take the first point in the open list as the current node currentNode, and put currentNode into the close list.

[0047] Step 7, Reeds-Shepp Connection: Use the Reeds-Shepp curve to connect the current motion node and the target point. Perform collision detection on the connection curve and retain valid connection lines. Set the first penalty cost of the connection line, including the cost of changing the vehicle's direction of travel, the cost of changing the wheel steering, and the vehicle's travel distance. Select the connection line with the lowest first penalty cost from the retained valid connection lines, and the path planning is complete. If there are no valid connection lines, continue to update the nodes.

[0048] Step 8. Update the node: The currentNode node is the starting point of the tractor's movement. Based on the tractor's kinematic model, the forward and backward movement direction and steering sampling values ​​of the tractor are traversed. The kinematic simulation step parameter is used as the sampling step to iterate the tractor's driving step to obtain a new node that the tractor can drive to.

[0049] Perform collision detection on the driving path of the new node and retain the valid nodes; calculate the second penalty cost of the valid nodes, filter them, merge them into the open list, and sort the open list in ascending order of penalty cost.

[0050] The collision detection in steps 7 and 8 includes: whether the tractor collides with the trailer when the tractor collides with the known obstacle point; and whether the tractor collides with the parking space envelope.

[0051] The second penalty cost in step 8 = cumulative cost + heuristic cost. The cumulative cost includes: vehicle direction change cost, wheel steering cost, wheel steering change cost, vehicle distance, and the cumulative cost at the previous node. The heuristic cost includes: the distance between the tractor and trailer and the target tractor and trailer, the difference between the tractor and trailer yaw angle and the target tractor and trailer yaw angle, and the tractor and trailer angle.

[0052] Execute step 6 and perform a new round of search until the path planning is completed or the open list is empty.

[0053] Figure 4This is a graph showing the path planned by the traditional Hybrid A* algorithm. Start represents the starting point, and End represents the destination. As can be seen from the figure, the traditional Hybrid A* algorithm solves the path planning problem that satisfies the kinematics of a single rigid body vehicle. Its motion conforms to simple circular motion, making the path simple and easy to search.

[0054] Figure 5 This is a diagram of a tractor-trailer parking path planned using the hybrid A* algorithm according to an embodiment of the present invention. Start represents the starting point, and End represents the destination. As shown in the figure, the solid line connecting the starting point and the destination is the tractor's path, and the dashed line is the trailer's path. The path is smooth and conforms to the kinematic principles of the tractor-trailer. Compared to single-rigid-body path planning, multi-rigid-body path planning algorithms are more complex and require more considerations. This present invention addresses the parking path planning problem for tractors, based on dual-rigid-body kinematic analysis.

[0055] The above embodiments are preferred examples for implementing the present invention, and the present invention is not limited to the above embodiments. Any non-essential additions or replacements made by those skilled in the art based on the technical features of the present invention fall within the scope of protection of the present invention.

Claims

1. A parking path planning method for a tractor trailer based on the Hybrid A* algorithm, characterized in that: include: Obtain the following vehicle parameters: the distance between the center of the tractor's rear axle and the front, rear, and saddle points of the tractor, the width and wheelbase of the tractor; the distance between the front and rear ends of the trailer and the saddle points, the width and wheelbase of the trailer; and the maximum steering angle of the tractor's wheels. Obtaining the starting point and target point position information; the position information consists of the tractor position coordinates, the tractor yaw angle, and the trailer yaw angle; Define planning simulation parameters: tractor driving step length, steering step length, grid map step length, yaw angle retention step length, and penalty cost coefficients; Coordinate conversion: converting the coordinates of the tractor's starting point and target point to the corresponding coordinates on the grid map according to the grid map step size; ReedsShepp connection: Use the ReedsShepp curve to connect the current motion node and the target point, perform collision detection on the connection curve, and retain valid connection lines; set the first penalty cost of the connection line; select the connection line with the smallest first penalty cost among the retained valid connection lines, and the path planning is completed; The first penalty cost includes a vehicle driving direction change cost, a wheel steering change cost, and a vehicle driving distance; The path planning method further includes: if there is no valid connection line, continuing to update the node; The process of updating the node specifically includes: Taking the current posture as the starting point of the tractor's movement, based on the tractor's kinematic model, the tractor is traversed in terms of steering and movement directions to obtain the vehicle's reachable node posture; Perform collision detection on paths that can reach nodes and retain valid nodes; perform second penalty cost calculation on the valid nodes, filter and merge them, and sort them in ascending order according to the second penalty cost.

2. The parking path planning method for a tractor trailer based on the Hybrid A* algorithm according to claim 1, characterized in that: The kinematic formula in the kinematic model of the towing tractor is: (1) Traction trajectory of the tractor: Update the tractor vehicle yaw angle: Update the coordinates of the center point of the tractor's rear axle: (2) Trailer movement status: Update trailer curvature: Update trailer yaw angle: The parameters represents the yaw angle of the tractor, represents the trailer yaw angle, s represents the simulation step length, ρ i represents the curvature of the tractor, ρ j represents the trailer curvature, λ i Indicates the length from the saddle point to the rear axle of the tractor, L j Indicates the trailer length.

3. The method for parking path planning for a tractor trailer based on the Hybrid A* algorithm according to claim 1, characterized in that: The collision detection includes: whether the tractor collides with the trailer when the tractor tractor moves; whether the tractor tractor collides with a known obstacle point; and whether the tractor tractor collides with the parking space envelope.

4. The method for parking path planning for a tractor trailer based on the Hybrid A* algorithm according to claim 1, characterized in that: The second penalty cost = cumulative cost + heuristic cost; The accumulated cost includes: vehicle direction change cost, wheel steering cost, wheel steering change cost, vehicle travel distance, and previous node accumulated cost; The heuristic cost includes: the distance between the tractor and trailer and the target tractor and trailer, the difference between the yaw angle of the tractor and trailer and the yaw angle of the tractor and trailer at the target point, and the angle between the tractor and trailer.

Images