Hybrid path planning method for articulated vehicle based on improved a* algorithm

Abstract

The application discloses a kind of based on improved A* algorithm articulated vehicle hybrid path planning method, comprising:1, construct test site grid map, set the initial point and end point position of articulated vehicle;2, the kinematic model of articulated vehicle is established, the heading angle and articulated angle information of vehicle at target node are calculated;3, design the hybrid path planning method of improved A* algorithm and plan;4, path search, the articulated point of articulated vehicle is used to node expansion, and target node is collision detection;5, combined with vector field histogram (VFH*) method calculates the collision threat cost of each sector in search range;6, design cost function to select target node and expand;7, judge whether target node is in the vicinity of end point, if yes, then the algorithm ends, return result path;Otherwise, return step 2 sequentially.The application can plan out the task path that satisfies the operation of articulated vehicle, improves the efficiency and applicability of articulated vehicle in path planning occasion.

Description

Technical Field

[0001] This invention relates to the field of vehicle routing technology, and specifically to a hybrid routing method for articulated vehicles based on an improved A* algorithm. Background Technology

[0002] With the increasing demand for mineral resources and the development of the mining industry, intelligent mining has become the theme of underground mine development and will also become a cutting-edge field in the future development of the mining industry. Intelligent unmanned mining equipment can reduce the number of personnel in underground mines, and even achieve unmanned mining, while also enabling uninterrupted continuous operation, reducing mine production costs and improving production efficiency.

[0003] Articulated vehicles are widely used in harsh environments such as mines due to their flexibility and maneuverability. Therefore, the intelligent research of underground articulated trackless vehicles is of typical significance. Path planning is an important component of autonomous driving; a reasonable path plan can greatly improve the operational efficiency of articulated vehicles while ensuring their stable operation. Current path planning algorithms generally assume that the actuators can turn in place or can be treated as point masses relative to the map. To adapt to the narrow and restricted tunnel spaces underground, articulated vehicles cannot turn in place or be treated as point masses. Therefore, it is essential to ensure that the planned path conforms to the trajectory characteristics of articulated vehicles.

[0004] The A* algorithm is the most efficient method for finding the shortest path in path planning algorithms. However, the traditional A* algorithm has a series of significant problems, such as insufficient smoothness of the planned path, failure to fully consider vehicle kinematic constraints, and slow search speed for large-scale maps. In recent years, scholars at home and abroad have made many improvements to the A* algorithm, such as adding a vehicle kinematic model to the node expansion and smoothing the generated path; and proposing an improved storage array method to address the long time required for traversing the OPEN and CLOSE lists in the A* algorithm.

[0005] The above-mentioned improvement method does not take into account the trajectory characteristics of articulated vehicles, cannot solve the problem of slow large-scale map search, and cannot be directly used for subsequent trajectory tracking. Summary of the Invention

[0006] To address the shortcomings of existing technologies, this invention proposes a hybrid path planning method for articulated vehicles based on an improved A* algorithm. This method aims to solve the problems of collision hazards caused by planned paths not meeting trajectory characteristics and slow planned path speeds in test tracks when planning paths for articulated vehicles. In this way, it can improve the efficiency of path generation for articulated vehicles in path planning scenarios.

[0007] To solve the technical problem, the present invention adopts the following technical solution: The present invention provides a hybrid path planning method for articulated vehicles based on an improved A* algorithm, characterized by the following steps: Step 1: Establish a rectangular coordinate system oxy by taking any vertex of the circumscribed rectangle of the test site as the origin o, and using the two sides connected to the origin o as the x-axis and y-axis, respectively; in this rectangular coordinate system... oxy In this process, the test site is divided into an M×N grid map. ;in, S n Represents the grid map The upper coordinate is The n The state of each grid point, if , indicating the first n If each grid point is an empty point, , indicating the first n Each grid point is an obstacle point, and M represents the grid map. The horizontal grid number, N represents the vertical grid number; Define and initialize the open list, the close list, and the surroundingpoints list; Using hinge points as nodes, and defining the structure of any node to include: x-coordinate x y-axis y heading angle Hinge angle The actual cost from the starting point to the current node is G, the estimated cost from the current node to the end point is H, the total cost is F, and the parent node is parent. Step 2: Assume the vehicle is on the grid map. The starting point is the s-th grid point. S s Its coordinates are The heading angle is The starting point and the endpoint are the t-th grid point. S t Its coordinates are The heading angle is ; and the starting point S s As the initial node; the structure of the initial node includes: x-coordinate y-axis heading angle Hinge angle The actual cost from the starting point to the current node The estimated cost from the current node to the destination. Comprehensive cost Parent node If empty, store it in the open list; Third step: If the open list is not empty, take the node with the smallest comprehensive cost F from the open list as the current node B, determine whether the current node B is the destination, if so, start from the destination and gradually trace the parent node until the starting point, and output the path formed by the searched nodes in order; otherwise, delete the current node B from the open list and store it in the close list, and execute the fourth step. Step 4: Set the search scope as L Define safe distance d_safe ; Assume the current node B is within the search range L shared within n Let any node be the nth node. i One node; Step 5: Calculate the coordinates of the center point A of the front axle and the center point C of the rear axle of the articulated vehicle at the current node B according to equations (1) and (2); (1) (2) In equations (1) and (2), This represents the hinge angle of the current node B. This represents the heading angle of the current node B. This represents the distance from the current node B to the center point A of the front bridge. This represents the distance from the current node B to the center point C of the rear bridge. For raster maps The coordinates of the center point A of the front axle; For raster maps The coordinates of the current node B in the current node; For raster maps The coordinates of the center point C of the rear axle; The coordinates of the steering center O are obtained using equation (3). ; (3) Step 6: Calculate the distance of the articulated vehicle from the current node B to the [node number] according to equation (4). i The angle rotated around the steering center O at each node ; (4) In equation (4), This is the distance from the current node B to the turning center O; Step 7: Using equations (5) and (6), obtain the articulated vehicle at the first... i The center point of the front axle at each node coordinates Rear axle center point coordinates ; (5) (6) Step 8: Calculate the articulated vehicle's position according to equation (7) at the 1st... i The heading angle of each node and hinge angle : (7) In equation (7), For raster maps The Middle i The coordinates of each node; Step 9, if Then it means the first i If a node does not meet the requirements, proceed to step eleven; otherwise, proceed to step ten. This represents the maximum articulation angle of the articulated vehicle. Step 10: According to the articulated vehicle in the... i Coordinates of each node Heading angle and hinge angle , judge the first i The articulated vehicles at each node are in the safe zone. d_safe Are there any obstacle points inside? If there are no obstacles, then the first... i Store each node in the `surroundpoints` list and proceed to step eleven; otherwise, proceed directly to step eleven. Step 11, i +1 is assigned to i ,like i > n If the result is positive, proceed to step 12; otherwise, return to step 6 and proceed sequentially. Step 12: Assume there are a total of [number] points in the surroundingpoint list. m Let R be any one of the nodes; Step 13: Calculate the actual cost of the R-th node under the current node B. Estimated Costs and comprehensive costs Determine if the R-th node is the destination. If so, start from the R-th node and trace the parent node step by step until the starting point is reached. Then, output the path formed by the searched nodes in order. Otherwise, proceed to step fourteen. Step 14: Assign R+1 to R, if R> m If the condition is met, proceed to step 3 sequentially; otherwise, proceed to step 13 sequentially.

[0008] The articulated vehicle hybrid path planning method based on the improved A* algorithm described in this invention is also characterized by the fact that the actual cost of the R-th node in the thirteenth step under the current node B is... Estimated Costs and comprehensive costs The calculation is performed according to the following steps: Step 13.1: Search range L With radius B, the circular region centered at the current node B is divided into 8×L sectors, with the sector precision defined as follows: grid =360 / (8×L); Step 13.2, Traverse range 2 L All grid points within the sector, and calculate the collision threat cost for each sector: Step a, Determine the range 2 L Inner j If a grid point is an obstacle, proceed to step b; otherwise, determine range 2. L The next grid point inside; Step b: Calculate the first step using equation (8). j The sector where each grid point is located Then, the articulated vehicle at the current node B is calculated using equation (9). j Collision threat cost per grid point Thus, the first j The sector where each grid point is located Collision threat cost of all grid points : (8) (9) In equations (8)-(9), To round down, For the first j The coordinates of each grid point, where Cost1 represents the movement cost between any two adjacent grid points; Step 13.3: Define the coordinates of the R-th node as... If the R-th node exists in the open list, then use equation (10) to calculate the actual cost of the R-th node when its parent node is the current node B. If the R-th node is not found, proceed to step 13.4; otherwise, store the R-th node in the open list, define the parent node of the R-th node as the current node B, and proceed directly to step 13.5. (10) In equation (10), Cost represents the actual cost of the parent node of the R-th node, and Cost2 represents the movement cost between any two diagonally adjacent grid points; Step 13.4, if Less than the actual cost of the R-th node in its original parent node Then Assign to Then define the parent node of the Rth node as the current node B and execute step 13.5; otherwise, end the calculation. Step 13.5: Calculate the estimated cost of the articulated vehicle at the Rth node under the current node B using equation (11). ; (11) Step 13.6: Calculate the sector where the articulated vehicle is located at the Rth node under the current node B using equation (8). And use equation (12) to calculate the forward cost of the R-th node. ; (12) In equation (12), For the articulated vehicle in the sector where node B is located, This represents the sector where the articulated vehicle is located at the current node B. The sector where the R-th node is located The difference, The coefficient is given by equation (13); For the articulated vehicle at the t-th grid point under the current node B S t The target sector is calculated using equation (8). This represents the sector where the articulated vehicle is located at the Rth node. And the t-th grid point under the current node B S t The target sector The difference, For coefficients; This refers to the sector where the articulated vehicle is located before reaching the current node B. The articulated vehicle in the sector where node B is located With the previous sector The difference, The coefficient is given by equation (14); (13) (14) Step 13.7: Calculate the comprehensive cost of the articulated vehicle at the Rth node under the current node B using equation (15). ; (15) In equation (15), For the articulated vehicle, the distance from the R-th node to the t-th grid point under the current node B is... S t Expected cost The expected cost of the parent node of the R-th node The coefficient of the sum, For the articulated vehicle, the sector containing the Rth node under the current node B. The cost of the collision threat.

[0009] The present invention provides an electronic device, including a memory and a processor, wherein the memory is used to store a program that supports the processor in executing the articulated vehicle hybrid path planning method, and the processor is configured to execute the program stored in the memory.

[0010] The present invention discloses a computer-readable storage medium on which a computer program is stored, wherein the computer program, when executed by a processor, performs the steps of the articulated vehicle hybrid path planning method.

[0011] Compared with the prior art, the beneficial effects of the present invention are as follows: 1. Based on the kinematic model of the articulated vehicle, this invention establishes the constraint equations of the A* algorithm during node expansion, ensuring that node expansion satisfies the trajectory characteristics of the articulated vehicle and improving the working efficiency of the articulated vehicle on the test site. 2. This invention calculates the pose of the articulated vehicle at the target node based on the kinematic model of the articulated vehicle, and performs collision detection based on the pose information of the articulated vehicle at the target node, thus ensuring the safety of the articulated vehicle during operation. 3. The articulated vehicle hybrid path planning method of the present invention can plan a path that meets the trajectory characteristics of the articulated vehicle simply by changing the vehicle body parameters, depending on the different articulated vehicle bodies. It has strong functional versatility. 4. This invention constructs the collision risk cost of different sectors within the search range based on the Vector Field Histogram (VFH*) method, designs a cost function that considers vehicle position, attitude and steering switching, and transforms the coefficients according to the distance between the current node and the starting point and target node, thereby ensuring the smoothness of the planned path and improving the working efficiency of articulated vehicles. 5. This invention adds a cost based on the VFH* algorithm cost function to the comprehensive cost F in the traditional A* algorithm. vfh_value And the cost of collision threat considering the direction of travel at this node. ob_value By controlling the direction of node expansion, the efficiency of articulated vehicles in path planning is improved, and the time for path planning is reduced. Attached Figure Description

[0012] Figure 1 This is a schematic diagram of the path planning algorithm described in this invention; Figure 2 This is a kinematic model of an articulated vehicle according to a specific embodiment of the present invention; Figure 3 This is a diagram illustrating the path planning effect in an experimental scenario map, representing a specific embodiment of the present invention. Detailed Implementation

[0013] The present invention will now be described in more detail with reference to specific embodiments and accompanying drawings.

[0014] The underground loader platform used in this embodiment has a front body length of 4.962m and a width of 2.44m, a rear body length of 4.737m and a width of 2.26m, a hinge point distance from the center of the front axle of 1.752m, a distance from the center of the rear axle of 1.853m, and a hinge angle of ≤40°.

[0015] like Figure 1 As shown, a hybrid path planning method for articulated vehicles based on an improved A* algorithm is presented, with detailed steps as follows: Step 1: Establish a rectangular coordinate system oxy by taking any vertex of the circumscribed rectangle of the test site as the origin o, and using the two sides connected to the origin o as the x-axis and y-axis, respectively; in the rectangular coordinate system... oxy In the middle, the test site is divided into an M×N grid map. ;in, S n Represents a raster map The upper coordinate is The state of the grid points, if This indicates that the corresponding grid point is an empty point. , indicating that the corresponding grid point is an obstacle point, M represents the grid map. The horizontal grid number, N represents the vertical grid number; Define and initialize the open, close, and surroundingpoints lists; use hinge points as nodes, and define the structure of any node to include the x-coordinate. x y-axis y heading angle Hinge angle The actual cost from the starting point to the current node is G, the estimated cost from the current node to the end point is H, the total cost is F, and the parent node is parent. Step 2: Based on the experimental scene grid map, such as... Figure 3 As shown, the resolution is 0.2m, and vehicles are set on the grid map. The starting point S s coordinate for The heading angle is = The hinge angle is ,end S t coordinate for heading angle for Hinge angle ; and the starting point S s As the initial node; the structure of the initial node includes: x-coordinate =572, ordinate =313, heading angle Hinge angle The actual cost from the starting point to the current node The estimated cost from the current node to the destination. Comprehensive cost Parent node If empty, store it in the open list; Third, if the open list is not empty, take the node with the smallest overall cost F from the open list as the current node B. Determine if the current node B is the destination. If so, start from the destination and trace the parent node step by step until the starting point is reached. Then, output the path formed by the searched nodes in order. Figure 3 The final path planning result is shown; otherwise, the current node B is removed from the open list and added to the close list, and step four is executed. Step 4: Set the search scope as L= 10. Define safe distance d_safe =0.3m; Assume that the current node B has a total of [number] nodes within the search range. n Let any node be the nth node. i One node; Step 5, as Figure 2 As shown, the coordinates of the center point A of the front axle and the center point C of the rear axle of the articulated vehicle at the current node B are calculated according to equations (1) and (2). (1) (2) In equations (1) and (2), This represents the hinge angle of the current node B. This represents the heading angle of the current node B. This represents the distance from the current node B to the center point A of the front bridge. This represents the distance from the current node B to the center point C of the rear bridge. For raster maps The coordinates of the center point A of the front axle; For raster maps The coordinates of the current node B in the current node; For raster maps The coordinates of the center point C of the rear axle; The coordinates of the steering center O are obtained using equation (3). ; (3) Step 6: Calculate the distance of the articulated vehicle from the current node B to the [node number] according to equation (4). i The angle rotated around the steering center O at each node ; (4) In equation (4), The distance from node B to the turning center O; Step 7: Using equations (5) and (6), obtain the articulated vehicle at the first... i The center point of the front axle at each node coordinates Rear axle center point coordinates ; (5) (6) Step 8: Calculate the articulated vehicle's position according to equation (7) at the 1st... i The heading angle of each node and hinge angle : (7) In equation (7), For raster maps The Middle i The coordinates of each node; Step 9, if Then it means the first i If a node does not meet the requirements, proceed to step eleven; otherwise, proceed to step ten. This represents the maximum articulation angle of the articulated vehicle. Step 10: According to the articulated vehicle in the... i Coordinates of each node Heading angle and hinge angle ,like Figure 2 As shown, determine the first iAre there any obstacles within the safe area for the articulated vehicle at each node? Figure 2 The text shows a safe zone; if there are no obstacles, then the [missing information] will be [missing information]. i Store each node in the `surroundpoints` list and proceed to step eleven; otherwise, proceed directly to step eleven. Step 11, i +1 is assigned to i ,like i > n If the result is positive, proceed to step 12; otherwise, return to step 6 and proceed sequentially. Step 12: Assume there are a total of [number] points in the surroundingpoint list. m Let R be any one of the nodes; Step 13: Calculate the actual cost of the R-th node under the current node B. Estimated Costs and comprehensive costs Determine if the R-th node is the destination. If so, start from the R-th node and trace the parent nodes step by step until reaching the starting point. Then, output the path formed by the searched nodes in order. Figure 3 The final path planning result is shown; otherwise, proceed to step fourteen. In this embodiment, the actual cost of the R-th node in step thirteen under the current node B. Estimated Costs and comprehensive costs The calculation is performed according to the following steps: Step 13.1: Divide the circular region with a search range of 10 as the radius and the current node B as the center into 80 sectors. Define the precision of each sector as... grid =4.5; Step 13.2: Traverse all grid points within a range of 20 grid cells and calculate the collision threat cost for each sector: Step a: Determine the first [unit] within a range of 20 grid cells. j If a grid point is an obstacle, proceed to step b; otherwise, determine the next grid point within a range of 20 grid cells. Step b: Calculate the first step using equation (8). j The sector where each grid point is located Then, the articulated vehicle at the current node B is calculated using equation (9). j Collision threat cost per grid point Thus, the first j The sector where each grid point is located Collision threat cost of all grid points : (8) (9) In equations (8)-(9), To round down, For the first j The coordinates of each grid point, and the movement cost between any two adjacent grid points is 10; Step 13.3: Define the coordinates of the R-th node as... If the R-th node exists in the open list, then use equation (10) to calculate the actual cost of the R-th node when its parent node is the current node B. If the R-th node is not found, proceed to step 13.4; otherwise, store the R-th node in the open list, define the parent node of the R-th node as the current node B, and proceed directly to step 13.5. (10) In equation (10), Defined as the actual cost of the parent node of the R-th node, the movement cost between any two diagonally adjacent grid points is 14; Step 13.4, if Less than the actual cost of the R-th node in its original parent node Then Assign to Then define the parent node of the Rth node as the current node B and execute step 13.5; otherwise, end the calculation. Step 13.5: Calculate the estimated cost of the articulated vehicle at the Rth node under the current node B using equation (11). ; (11) Step 13.6: Calculate the sector where the articulated vehicle is located at the Rth node under the current node B using equation (8). And use equation (12) to calculate the forward cost of the R-th node. ; (12) In equation (12), For the articulated vehicle in the sector where node B is located, This represents the sector where the articulated vehicle is located at the current node B. The sector where the R-th node is located The difference, The coefficient is given by equation (13); For the articulated vehicle at the t-th grid point under the current node B S t The target sector is calculated using equation (8). This represents the sector where the articulated vehicle is located at the Rth node. And the t-th grid point under the current node B S t The target sector The difference has a coefficient of 1; This refers to the sector where the articulated vehicle is located before reaching the current node B. The articulated vehicle in the sector where node B is located With the previous sector The difference, The coefficient is given by equation (14); (13) (14) Step 13.7: Calculate the comprehensive cost of the articulated vehicle at the Rth node under the current node B using equation (15). ; (15) In equation (15), For the articulated vehicle, the distance from the R-th node to the t-th grid point under the current node B is... S t Expected cost The expected cost of the parent node of the R-th node The coefficient of the sum, For the articulated vehicle, the sector containing the Rth node under the current node B. The cost of the collision threat.

[0016] Step 14: Assign R+1 to R, if R> m If the condition is met, proceed to step 3 sequentially; otherwise, proceed to step 13 sequentially.

[0017] In this embodiment, an electronic device includes a memory and a processor. The memory stores a program that supports the processor in executing the above-described method, and the processor is configured to execute the program stored in the memory.

[0018] In this embodiment, a computer-readable storage medium stores a computer program, which is executed by a processor to perform the steps of the above method.

[0019] from Figure 3As can be seen, the traditional A* algorithm plans trajectory 1 too close to the obstacle. Since the narrowness of passages varies in real-world scenarios, an adaptive safety distance from the obstacle is added to the algorithm to ensure a path can be found. Furthermore, the trajectory planned by this method does not meet the trajectory characteristics of articulated vehicles and requires smoothing. The articulated vehicle hybrid path planning method based on the improved A* algorithm in this invention plans trajectory 2, which not only meets the trajectory characteristics of articulated vehicles but also saves 30% of the time compared to the traditional A* algorithm. It does not require smoothing, and the planned trajectory is located in the middle of the passage, reducing the risk of collisions between the articulated vehicle and the sides. This demonstrates that the method of this invention can plan trajectories that meet the trajectory characteristics of articulated vehicles, improving efficiency in path planning.

Claims

1. A hybrid path planning method for articulated vehicles based on an improved A* algorithm, characterized in that, The procedure is as follows: Step 1: Establish a rectangular coordinate system oxy by taking any vertex of the circumscribed rectangle of the test site as the origin o, and using the two sides connected to the origin o as the x-axis and y-axis, respectively; in this rectangular coordinate system... oxy In this process, the test site is divided into an M×N grid map. ; in, S n Represents the grid map The upper coordinate is The n The state of each grid point, if , indicating the first n If each grid point is an empty point, , indicating the first n Each grid point is an obstacle point, and M represents the grid map. The horizontal grid number, N represents the vertical grid number; Define and initialize the open list, the close list, and the surroundingpoints list; Using hinge points as nodes, and defining the structure of any node to include: x-coordinate x y-axis y heading angle Hinge angle The actual cost from the starting point to the current node is G, the estimated cost from the current node to the end point is H, the total cost is F, and the parent node is parent. Step 2: Assume the vehicle is on the grid map. The starting point is the s-th grid point. S s Its coordinates are The heading angle is The starting point and the endpoint are the t-th grid point. S t Its coordinates are The heading angle is ; and the starting point S s As the initial node; the structure of the initial node includes: x-coordinate y-axis heading angle Hinge angle The actual cost from the starting point to the current node The estimated cost from the current node to the destination. Comprehensive cost Parent node If empty, store it in the open list; Third step: If the open list is not empty, take the node with the smallest comprehensive cost F from the open list as the current node B, determine whether the current node B is the destination, if so, start from the destination and gradually trace the parent node until the starting point, and output the path formed by the searched nodes in order; otherwise, delete the current node B from the open list and store it in the close list, and execute the fourth step. Step 4: Set the search scope as L Define safe distance d_safe ; Assume the current node B is within the search range L shared within n Let any node be the nth node. i One node; Step 5: Calculate the coordinates of the center point A of the front axle and the center point C of the rear axle of the articulated vehicle at the current node B according to equations (1) and (2); (1) (2) In equations (1) and (2), This represents the hinge angle of the current node B. This represents the heading angle of the current node B. This represents the distance from the current node B to the center point A of the front bridge. This represents the distance from the current node B to the center point C of the rear bridge. For raster maps The coordinates of the center point A of the front axle; For raster maps The coordinates of the current node B in the current node; For raster maps The coordinates of the center point C of the rear axle; The coordinates of the steering center O are obtained using equation (3). ; (3) Step 6: Calculate the distance of the articulated vehicle from the current node B to the [node number] according to equation (4). i The angle rotated around the steering center O at each node ; (4) In equation (4), This is the distance from the current node B to the turning center O; Step 7: Using equations (5) and (6), obtain the articulated vehicle at the first... i The center point of the front axle at each node coordinates Rear axle center point coordinates ; (5) (6) Step 8: Calculate the articulated vehicle's position according to equation (7) at the 1st... i The heading angle of each node and hinge angle : (7) In equation (7), For raster maps The Middle i The coordinates of each node; Step 9, if Then it means the first i If a node does not meet the requirements, proceed to step eleven; otherwise, proceed to step ten. This represents the maximum articulation angle of the articulated vehicle. Step 10: According to the articulated vehicle in the... i Coordinates of each node Heading angle and hinge angle , judge the first i The articulated vehicles at each node are in the safe zone. d_safe Are there any obstacle points inside? If there are no obstacles, then the first... i Store each node in the `surroundpoints` list and proceed to step eleven; otherwise, proceed directly to step eleven. Step 11, i +1 is assigned to i ,like i > n If the result is positive, proceed to step 12; otherwise, return to step 6 and proceed sequentially. Step 12: Assume there are a total of [number] points in the surroundingpoint list. m Let R be any one of the nodes; Step 13: Calculate the actual cost of the R-th node under the current node B. Estimated Costs and comprehensive costs Determine if the R-th node is the destination. If so, start from the R-th node and trace the parent node step by step until the starting point is reached. Then, output the path formed by the searched nodes in order. Otherwise, proceed to step fourteen. The actual cost of the R-th node in the thirteenth step under the current node B Estimated Costs and comprehensive costs The calculation is performed according to the following steps: Step 13.1: Search range L With radius B, the circular region centered at the current node B is divided into 8×L sectors, with the sector precision defined as follows: grid =360 / (8×L); Step 13.2, Traverse range 2 L All grid points within the sector, and calculate the collision threat cost for each sector: Step a, Determine the range 2 L Inner j If a grid point is an obstacle, proceed to step b; otherwise, determine range 2. L The next grid point inside; Step b: Calculate the first step using equation (8). j The sector where each grid point is located Then, the articulated vehicle at the current node B is calculated using equation (9). j Collision threat cost per grid point Thus, the first j The sector where each grid point is located Collision threat cost of all grid points : (8) (9) In equations (8)-(9), To round down, For the first j The coordinates of each grid point, where Cost1 represents the movement cost between any two adjacent grid points; Step 13.3: Define the coordinates of the R-th node as... If the R-th node exists in the open list, then use equation (10) to calculate the actual cost of the R-th node when its parent node is the current node B. If the R-th node is not found, proceed to step 13.4; otherwise, store the R-th node in the open list, define the parent node of the R-th node as the current node B, and proceed directly to step 13.

5. (10) In equation (10), Cost represents the actual cost of the parent node of the R-th node, and Cost2 represents the movement cost between any two diagonally adjacent grid points; Step 13.4, if Less than the actual cost of the R-th node in its original parent node Then Assign to Then define the parent node of the Rth node as the current node B and execute step 13.5; otherwise, end the calculation. Step 13.5: Calculate the estimated cost of the articulated vehicle at the Rth node under the current node B using equation (11). ; (11) Step 13.6: Calculate the sector where the articulated vehicle is located at the Rth node under the current node B using equation (8). And use equation (12) to calculate the forward cost of the R-th node. ; (12) In equation (12), For the articulated vehicle in the sector where node B is located, This represents the sector where the articulated vehicle is located at the current node B. The sector where the R-th node is located The difference, The coefficient is given by equation (13); For the articulated vehicle at the t-th grid point under the current node B S t The target sector is calculated using equation (8). This represents the sector where the articulated vehicle is located at the Rth node. And the t-th grid point under the current node B S t The target sector The difference, For coefficients; This refers to the sector where the articulated vehicle is located before reaching the current node B. The articulated vehicle in the sector where node B is located With the previous sector The difference, The coefficient is given by equation (14); (13) (14) Step 13.7: Calculate the comprehensive cost of the articulated vehicle at the Rth node under the current node B using equation (15). ; (15) In equation (15), For the articulated vehicle, the distance from the R-th node to the t-th grid point under the current node B is... S t Expected cost The expected cost of the parent node of the R-th node The coefficient of the sum, For the articulated vehicle, the sector containing the Rth node under the current node B. The cost of the collision threat; Step 14: Assign R+1 to R, if R> m If the condition is met, proceed to step 3 sequentially; otherwise, proceed to step 13 sequentially.

2. An electronic device, comprising a memory and a processor, characterized in that, The memory is used to store a program that supports the processor in executing the articulated vehicle hybrid path planning method of claim 1, and the processor is configured to execute the program stored in the memory.

3. A computer-readable storage medium storing a computer program, characterized in that, The computer program, when run by the processor, executes the steps of the articulated vehicle hybrid path planning method of claim 1.

Images