Path planning method for large transport vehicle

Abstract

The invention discloses a path planning method for a large transport vehicle, which is used for carrying out obstacle avoidance planning on a path of the large transport vehicle and comprises the following steps of: planning a basic path of the large transport vehicle on a two-dimensional level by adopting a mixed A * method considering trailer obstacle avoidance; sequentially performing simulation prediction on all points of the basic path based on the current driving state of the vehicle to obtain the driving state of the vehicle at the next moment until the simulation prediction of the last path point is completed, and obtaining a predicted path; on the basis of the prediction path, a delaunay triangulation collision detection algorithm is adopted to carry out whole-vehicle three-dimensional collision detection on the large transport vehicle; and performing iterative planning of a prediction path according to a three-dimensional collision detection result, thereby planning a final path for ensuring that the whole vehicle is not collided. The controller is used for simulation prediction, precise three-dimensional collision detection and path iteration planning are combined, and the final path achieves the effect of reducing tracking errors, missing detection and misjudgment under the condition that it is guaranteed that the whole vehicle does not collide.

Description

Technical Field

[0001] The present invention relates to the field of path planning, and in particular to a path planning method for a transport vehicle carrying large cargo. Background Art

[0002] Oversized transport refers to the oversized transport of indivisible items. According to the "Regulations on the Management of Highways for Oversized Transport Vehicles," revised on June 23, 2021, oversized transport vehicles are defined as freight transport vehicles that meet any of the following conditions: the total height of the vehicle and cargo exceeds 4 meters from the ground; the total width of the vehicle and cargo exceeds 2.55 meters; the total length of the vehicle and cargo exceeds 18.1 meters; or the total mass of the vehicle and cargo exceeds 49 tons.

[0003] Large-scale logistics refers to the transportation of irregular goods such as overlong, overwide, and overweight goods via tractor-trailers. Planning the travel routes for various complex scenarios during transportation is a key part of large-scale logistics automation. This is crucial for reducing safety accidents caused by human errors and lowering the costs associated with manual measurement and calculation of vehicle trafficability.

[0004] Vehicle path planning is the process of finding the optimal or feasible route from a starting point to a destination in a complex environment through algorithms and techniques. Its core goal is to balance multiple factors, including efficiency, safety, and cost. Applications include autonomous driving, logistics distribution, and traffic navigation. Existing vehicle planning methods primarily include sampling-based, graph search-based, and optimization-based approaches.

[0005] Although existing algorithms for articulated truck path planning can effectively detect collisions with articulated trucks, they still suffer from the defect of low collision detection accuracy. In large-scale logistics scenarios, we often encounter cross-type obstacles such as culverts and non-cross-type obstacles such as road signs, and large goods are usually oversized. Existing collision detection methods are usually two-dimensional and cannot well represent irregular large goods and perform collision detection on obstacles, especially cross-type obstacles, which can easily lead to misjudgment and collision. Moreover, due to the excessive size of large goods, the driving space of large-scale logistics vehicles is limited. If there is a large deviation between the actual executed path and the planned path, there is a high risk of unexpected collision. Therefore, it is also necessary to reduce tracking errors.

[0006] Furthermore, large-scale logistics vehicles typically consist of a powered tractor, a trailer attached to the tractor, and the bulky cargo it carries. Since route planning is performed for the powered tractor, the technical challenge of ensuring a smooth and safe route for both the tractor and trailer remains. Summary of the Invention

[0007] The present invention provides a path planning method for large-scale transport vehicles, aiming to effectively solve the above technical problems.

[0008] According to a first aspect of the present invention, the present invention provides a path planning method for a large-scale transport vehicle, characterized in that obstacle avoidance planning is performed on the path of the large-scale transport vehicle, and the method includes:

[0009] Step 1: A hybrid A* method that takes trailer obstacle avoidance into consideration is used to plan a basic path for large-scale transport vehicles in two dimensions. Step 2: For all points on the basic path, simulation predictions are performed in sequence based on the vehicle's current driving state to obtain the vehicle's driving state at the next moment, until the simulation prediction of the last path point is completed to obtain the predicted path. Step 3: Based on the predicted path, a collision detection algorithm based on Delaunay triangulation is used to perform three-dimensional collision detection on the large-scale transport vehicle. Step 4: Iterative planning of the predicted path is performed based on the results of the three-dimensional collision detection, thereby planning a final path that ensures that the entire vehicle does not collide.

[0010] Furthermore, the present invention divides the large-piece transport vehicle into three parts, including a tractor, a trailer and large-piece cargo.

[0011] Furthermore, the specific process of simulation prediction is as follows: for any path point, a pure tracking controller is used to calculate the front wheel angle of the tractor, the target speed of the tractor is calculated based on the longitudinal speed planning, and according to the front wheel angle and target speed, the driving state of the large-scale transport vehicle at the next moment is simulated and predicted based on the vehicle kinematic model.

[0012] Furthermore, the front wheel angle is calculated as:

[0013]

[0014] Where, δ p is the front wheel turning angle of the tractor, k is the preview distance gain value, v0 is the current speed of the tractor, L fc is the preset default preview distance, L is the wheelbase of the tractor, and α is the relative angle between the tractor's current direction and the preview vector.

[0015] Furthermore, the longitudinal speed planning method considering the maximum lateral acceleration of the vehicle and the vehicle geometric constraints is used. The target speed is calculated as follows:

[0016]

[0017] Where, v p is the target speed of the tractor, a latmax is the maximum lateral acceleration, δ p is the front wheel turning angle of the tractor.

[0018] Furthermore, in step 3, the corresponding environmental point cloud map is obtained based on the predicted path. Assuming the set length of the point cloud is n, the vehicle body including the tractor, trailer and large cargo is triangulated and the point cloud data of the whole vehicle and the surrounding environment is obtained. S = {s1, s2, s3…s n} collision detection, the specific method is as follows:

[0019] First, establish the KD tree topology of the Delaunay triangulation of the entire vehicle. By searching the triangulated tetrahedral mesh, quickly find the candidate cells that may exist in the point cloud. Use the barycentric coordinate method to determine whether the point cloud is within the tetrahedron. The barycentric coordinate solution is as follows:

[0020] For point s i =(x i ,y i ,z i ), i∈{1,2,3…n} and any tetrahedron vertex A,B,C,D, calculate the barycentric coordinates by solving the linear equations:

[0021]

[0022] Solve for λ1, λ2, λ3, λ4, if λ i ≥0, then point s i Within the tetrahedron, all collision points in the point cloud data will be added to the collision point cloud set P during multiple iterations. collision middle.

[0023] Furthermore, based on the (x, y) coordinates of the point cloud where the collision occurred, expansion or contraction calculation is performed according to the actual situation, and the elements of the corresponding coordinates in the hybrid A* grid map are set to 1, that is, set as obstacles.

[0024] Furthermore, step 4 is specifically as follows: if no collision occurs on the predicted path, the planning is terminated and the predicted path is sent to the large-scale transport vehicle as the final path for execution; if a vehicle collision occurs on the predicted path, the obstacle is redefined, and the basic path planning, simulation prediction and vehicle three-dimensional collision detection are repeated according to the new obstacle to achieve iterative planning until no collision occurs on the predicted path, and the planning is terminated to obtain the final path.

[0025] According to a second aspect of the present invention, the present invention further provides an electronic device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the above method.

[0026] According to a third aspect of the present invention, the present invention further provides a storage medium storing a computer program, which implements the above method when executed by a processor.

[0027] By using one or more of the above-mentioned embodiments of the present invention, at least the following technical effects can be achieved: the path planning method for large-scale cargo transport vehicles provided in this application first adopts a hybrid A* algorithm that takes trailer obstacle avoidance into consideration to provide a good basic path, then uses a controller to perform path simulation prediction on the basic path, and then combines three-dimensional collision detection and path iterative planning methods to plan a final path, thereby achieving the effect of reducing tracking errors while ensuring that the entire vehicle does not collide. In the three-dimensional collision detection of the entire vehicle, the present invention divides the large-scale cargo transport vehicle into three parts: a tractor, a trailer, and large-scale cargo, and adopts a collision detection algorithm based on Delaunay triangulation to perform three-dimensional collision detection with the point cloud representing the environment, thereby achieving accurate collision detection and having the technical effect of reducing missed detections and misjudgments. BRIEF DESCRIPTION OF THE DRAWINGS

[0028] The technical solutions and other beneficial effects of the present invention will be made apparent by describing in detail the specific embodiments of the present invention in conjunction with the accompanying drawings.

[0029] Figure 1 This is an overall flow chart of the path planning method for large-scale transport vehicles provided by an embodiment of the present invention;

[0030] Figure 2 This is a schematic diagram of the kinematic model of a large-scale transport vehicle provided in an embodiment of the present invention. DETAILED DESCRIPTION

[0031] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without making any creative efforts shall fall within the scope of protection of the present invention.

[0032] In the description of the present invention, it should be noted that, unless otherwise specified or limited, the term "and / or" herein is merely a description of an association relationship between associated objects, indicating that three possible relationships exist. For example, "A and / or B" can represent: A exists alone, A and B exist simultaneously, and B exists alone. Furthermore, the character " / " herein, unless otherwise specified, generally indicates that the associated objects are in an "or" relationship.

[0033] When traveling on the highway, large-scale logistics vehicles may encounter complex environments, including non-crossable obstacles such as road signs and traffic lights, crossable obstacles such as underpasses, and narrow road sections. Large-scale logistics vehicles often carry oversized cargo, such as large machinery, engineering equipment, and wind turbine blades, making them significantly larger than ordinary trucks. Their extra width, length, and height make them prone to overrunning lanes and colliding with these complex environments. This makes path planning crucial for accurate collision detection and route adjustments to minimize missed detections and misjudgments. Furthermore, ensuring accurate collision detection is crucial, as is ensuring that the planned path also ensures smooth and safe travel for the tractor and trailer.

[0034] To solve the above technical problems, the present invention provides a path planning method for large-scale transport vehicles to avoid obstacles. The path planning method provided by the present invention is described below with reference to the accompanying drawings.

[0035] Figure 1 This is an overall flow chart of the path planning method for large-scale transport vehicles provided by an embodiment of the present invention. Figure 1 ,The path planning method for large-scale transport vehicles includes the following steps:

[0036] First, basic path planning is performed. This embodiment uses a hybrid A* method that takes trailer obstacle avoidance into account to plan a basic path on a two-dimensional level.

[0037] Hybrid A* with trailer obstacle avoidance is an excellent algorithm for path planning for articulated trucks. This algorithm improves upon the traditional A* algorithm by incorporating the Reeds-Shepp Curve (RS Curve). By sampling the input space as a grid map and applying a vehicle kinematic model to expand child nodes for path planning, the hybrid A* method with trailer obstacle avoidance incorporates a jackknife cost into the hybrid A* algorithm and performs collision detection between the tractor and trailer when expanding new nodes.

[0038] The vehicle kinematic model used in this method is a bicycle model with a trailer, see Figure 2 :

[0039]

[0040] During planning, Hybrid A* is based on the current state (x0, y0, θ0, θ1), which is the center position of the tractor's rear axle, the orientation angles of the tractor and trailer, and expands multiple child nodes based on the vehicle model. It then calculates the cost value of each child node state and determines the child node based on the cost value until the destination is reached. The cost value function is as follows:

[0041] f=g+h

[0042] Where g is the actual cost from the starting point to the current node, and h is the heuristic estimated cost from the current node to the target node. After adding the jackknife cost, it becomes:

[0043]

[0044] When determining the validity of a child node, in addition to collision detection for the tractor, collision detection for the trailer is also added. Directed bounding boxes are used to represent the tractor and trailer. When an obstacle falls within the bounding boxes, a collision occurs and the child node is invalid.

[0045] The hybrid A* algorithm for trailer obstacle avoidance not only takes into account the collision detection of the trailer, and can effectively replan the obstacle based on the redefinition of the obstacle projected onto the two-dimensional layer, but also considers the actual kinematics of the vehicle, which provides a good initial solution for the next step of predictive path generation.

[0046] A predicted path is generated by simulating vehicle movement along the basic path. Starting from the first point, the tractor's front wheel angle and target speed are calculated based on the vehicle's current state. The vehicle's next state is then predicted based on the vehicle's kinematic model, continuing until the final point along the basic path, resulting in a predicted path.

[0047] In this embodiment, a pure tracking controller is used to calculate the front wheel turning angle δ of the tractor p , calculated as follows:

[0048]

[0049] Where, k is the preview distance gain value, v0 is the current speed of the tractor, L fc is the preset default preview distance, L is the wheelbase of the tractor, and α is the relative angle between the tractor's current direction and the preview vector.

[0050] The longitudinal speed planning method considering the maximum lateral acceleration of the vehicle and the vehicle geometric constraints is used to calculate the target speed v of the tractor. p , calculated as follows:

[0051]

[0052] Where a latmax is the maximum lateral acceleration.

[0053] Next, a 3D collision check is performed on the entire vehicle based on the predicted path and the relative positions of the vehicle and the large cargo. To accurately detect obstacles in large-scale logistics scenarios, this embodiment geometrically divides the large-scale transport vehicle into three parts: the tractor, the trailer, and the large cargo. A collision detection algorithm based on Delaunay triangulation is used to perform 3D collision detection on the entire vehicle against the environmental point cloud corresponding to the predicted path. Specifically:

[0054] Based on the predicted path, the corresponding environmental point cloud map is obtained. Assuming the set length of the point cloud is n, the vehicle body including the tractor, trailer and large cargo is triangulated and the point cloud data of the whole vehicle and the surrounding environment is obtained. S = {s1, s2, s3…s n} collision detection, the specific method is as follows:

[0055] First, establish the KD tree topology of the Delaunay triangulation of the entire vehicle. By searching the triangulated tetrahedral mesh, quickly find the candidate cells that may exist in the point cloud. Use the barycentric coordinate method to determine whether the point cloud is within the tetrahedron. The barycentric coordinate solution is as follows:

[0056] For point s i =(x i ,y i ,z i ), i∈{1,2,3…n} and any tetrahedron vertex A,B,C,D, calculate the barycentric coordinates by solving the linear equations:

[0057]

[0058] Solve for λ1, λ2, λ3, λ4, if λ i ≥0, then point s i Within the tetrahedron, all collision points in the point cloud data will be added to the collision point cloud set P during multiple iterations. collision middle.

[0059] According to the (x, y) coordinates of the point cloud where the collision occurred, the expansion or contraction calculation is performed according to the actual situation, and the elements of the corresponding coordinates in the hybrid A* grid map are set to 1, that is, set as obstacles.

[0060] Finally, the predicted path is iteratively planned based on the 3D collision detection results to determine the final path. Specifically:

[0061] If no collision occurs on the predicted path, the planning ends and the predicted path is sent to the large-scale transport vehicle as the final path for execution;

[0062] If a vehicle collision occurs on the predicted path, the obstacle is redefined, and basic path planning, simulation prediction, and vehicle 3D collision detection are repeated based on the new obstacle to achieve iterative planning until no collision occurs on the predicted path. Planning is then terminated to obtain the final path.

[0063] In summary, the path planning method for large-scale transport vehicles provided in the embodiment of the present application first adopts a hybrid A* algorithm that takes trailer obstacle avoidance into consideration to provide a good basic path, then divides the large-scale cargo transport vehicle into three parts: tractor, trailer and large-scale cargo after using a controller to perform path simulation prediction on the basic path, and adopts a collision detection algorithm based on Delaunay triangulation to perform three-dimensional collision detection with the point cloud representing the environment, thereby achieving accurate collision detection and having the technical effect of reducing missed detections and misjudgments. Afterwards, the predicted path is iteratively planned based on the collision detection results to obtain the final safe path, which achieves the effect of reducing tracking errors while ensuring that the entire vehicle does not collide. The path planning method provided in the embodiment of the present application is aimed at the automation of large-scale logistics vehicles, can effectively reduce transportation costs, and improve safety and transportation efficiency.

[0064] Based on any of the above embodiments, another embodiment of the present invention further provides an electronic device, which may include: a memory, a processor, and a computer program stored on the memory and executable on the processor, wherein the processor may call logic instructions in the memory to execute the above method.

[0065] In addition, the logical instructions in the above-mentioned memory can be implemented in the form of a software functional unit and can be stored in a computer-readable storage medium when sold or used as an independent product. Based on this understanding, the technical solution of the present invention, or the part that contributes to the prior art, or the part of the technical solution, can be embodied in the form of a software product. The computer software product is stored in a storage medium and includes several instructions for enabling a computer device (which can be a personal computer, a server, or a network device, etc.) to execute all or part of the steps of the method described in each embodiment of the present invention. The aforementioned storage medium includes: various media that can store program codes, such as a USB flash drive, a mobile hard disk, a read-only memory (ROM), a random access memory (RAM), a magnetic disk or an optical disk.

[0066] On the other hand, an embodiment of the present invention further provides a storage medium on which a plurality of instructions are stored, and the instructions are suitable for being loaded by a processor to execute the path planning method for a large-scale transport vehicle provided in the above embodiments.

[0067] On the other hand, the technical solution of the present invention, or the portion that contributes to the prior art, or all or part of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes instructions for causing a computer device (which may be a personal computer, server, or network device, etc.) to execute all or part of the steps of the method described in each embodiment of the present invention. The aforementioned storage medium includes various media capable of storing program code, such as a USB flash drive, a mobile hard drive, a read-only memory (ROM), a random access memory (RAM), a magnetic disk, or an optical disk.

[0068] The device embodiments described above are merely illustrative. The units described as separate components may or may not be physically separate, and the components shown as units may or may not be physical units, i.e., they may be located in one location or distributed across multiple network units. Some or all of the modules may be selected based on actual needs to achieve the objectives of the present embodiment. Persons of ordinary skill in the art will be able to understand and implement the present invention without inventive effort.

[0069] Through the description of the above embodiments, those skilled in the art can clearly understand that each embodiment can be implemented by means of software plus a necessary general hardware platform, or of course, by hardware. Based on this understanding, the essence of the above technical solution or the part that contributes to the existing technology can be embodied in the form of a software product. The computer software product can be stored in a computer-readable storage medium, such as ROM / RAM, a magnetic disk, an optical disk, etc., and includes a number of instructions for enabling a computer device (which can be a personal computer, a server, or a network device, etc.) to execute the methods described in each embodiment or certain parts of the embodiments.

[0070] In summary, although the present invention has been disclosed above with reference to preferred embodiments, the above preferred embodiments are not intended to limit the present invention. A person skilled in the art may make various changes and modifications without departing from the spirit and scope of the present invention. Therefore, the scope of protection of the present invention shall be based on the scope defined in the claims.

Claims

1. A path planning method for a large-scale transport vehicle, characterized in that: Obstacle avoidance planning for large transport vehicles, including: Step 1: Using a hybrid A* method that considers trailer obstacle avoidance, a basic path for the large-scale transport vehicle is planned in two dimensions; Step 2: For all points on the basic path, simulation prediction is performed in sequence based on the vehicle's current driving state to obtain the vehicle's driving state at the next moment, until the simulation prediction of the last path point is completed and the predicted path is obtained; Step 3: Based on the predicted path, a Delaunay triangulation collision detection algorithm is used to perform a three-dimensional collision detection on the large-scale transport vehicle; Step 4: Iteratively plan the predicted path based on the results of the three-dimensional collision detection, so as to plan a final path that ensures that the entire vehicle does not collide.

2. The method according to claim 1, characterized in that The large-piece transport vehicle includes a tractor, a trailer and large-piece cargo.

3. The method according to claim 2, characterized in that The specific process of the simulation prediction is as follows: For any path point, a pure tracking controller is used to calculate the front wheel angle of the tractor, and the target speed of the tractor is calculated based on the longitudinal speed planning. According to the front wheel angle and the target speed, the driving state of the large-scale transport vehicle at the next moment is simulated and predicted based on the vehicle kinematic model.

4. The method according to claim 3, characterized in that The front wheel angle is calculated as follows: Where, δ p is the front wheel turning angle of the tractor, k is the preview distance gain value, v0 is the current speed of the tractor, L fc is the preset default preview distance, L is the wheelbase of the tractor, and α is the relative angle between the tractor's current direction and the preview vector.

5. The method according to claim 3, characterized in that The longitudinal speed planning method considering the maximum lateral acceleration of the vehicle and the vehicle geometric constraints is as follows: Where, v p is the target speed of the tractor, a latmax is the maximum lateral acceleration, δ p is the front wheel turning angle of the tractor.

6. The method according to claim 2, characterized in that In step 3, the corresponding environmental point cloud map is obtained based on the predicted path. Assuming that the set length of the point cloud is n, the vehicle body including the tractor, trailer and large cargo is triangulated and the point cloud data of the whole vehicle and the surrounding environment is obtained. S = {s1, s2, s3…s n } collision detection, the specific method is as follows: First, a KD tree topology structure of the Delaunay triangulation of the entire vehicle is established. By searching the triangulated tetrahedral mesh, candidate cells that may exist in the point cloud are quickly found. The barycentric coordinate method is used to determine whether the point cloud is within the tetrahedron. The barycentric coordinate solution is as follows: For point s i =(x i ,y i ,z i ), i∈{1,2,3…n} and any tetrahedron vertex A,B,C,D, calculate the barycentric coordinates by solving the linear equations: Solve for λ1, λ2, λ3, λ4, if λ i ≥0, then point s i Within the tetrahedron, all collision points in the point cloud data will be added to the collision point cloud set P during multiple iterations. collision middle.

7. The method according to claim 6, characterized in that According to the (x, y) coordinates of the point cloud where the collision occurred, the expansion or contraction calculation is performed according to the actual situation, and the elements of the corresponding coordinates in the hybrid A* grid map are set to 1, that is, set as obstacles.

8. The method according to claim 2, characterized in that The step 4 is specifically as follows: If no collision occurs on the predicted path, the planning is terminated and the predicted path is sent to the heavy-duty transport vehicle as the final path for execution; If a vehicle collision occurs on the predicted path, the obstacle is redefined, and basic path planning, simulation prediction, and vehicle 3D collision detection are repeated based on the new obstacle to achieve iterative planning until no collision occurs on the predicted path. Planning is then terminated to obtain the final path.

9. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein: The processor runs the computer program to implement the method according to any one of claims 1 to 8.

10. A storage medium having a computer program stored thereon, characterized in that: When the program is executed by a processor, the method according to any one of claims 1 to 8 is implemented.

Images