A CAV dynamic path planning method and system based on improved Dstar-Lite

Abstract

The present invention relates to the technical field of path planning in road environments. A CAV dynamic path planning method and system based on an improved Dstar-Lite is disclosed. The method inputs collected obstacle data and the CAV starting point into a pre-built dynamic obstacle generation model to generate an initial obstacle distribution. Obstacles in the same lane as the CAV target point are screened and the CAV target point is updated. An initial path Path1 for the CAV is generated. When an obstacle appears within the CAV preview range, the affected nodes in the initial path Path1 are searched and the CAV path is updated. The CAV is checked to see if it has reached the target point: if so, the search is stopped; if not, the search for affected nodes is continued and the CAV path is updated. The CAV path is tracked using a pre-built MPC model to dynamically adjust the CAV's motion trajectory. The present invention can significantly improve the safety and efficiency of CAVs in complex dynamic environments.

Description

Technical Field

[0001] The present invention belongs to the technical field of path planning in a road environment, and relates to a CAV dynamic path planning method and system based on an improved Dstar-Lite. Background Art

[0002] With the rapid development of intelligent transportation systems and autonomous driving technologies, dynamic path planning and obstacle avoidance have become research hotspots. In complex and dynamic environments, efficient and secure path planning methods are needed to cope with dynamic obstacles. However, existing technologies have some limitations when dealing with dynamic obstacles.

[0003] Most existing obstacle avoidance systems rely on real-time sensor data and can only process currently known environmental information, unable to predict future changes. When obstacles appear suddenly or move rapidly, the obstacle avoidance system's response is often delayed. Traditional prediction methods, based on simplified motion models or statistical laws, struggle to simulate the complexities of the real world. For example, complex behavioral models such as lane changes in manually driven vehicles cannot be accurately described. These factors combined can result in vehicles or robots' obstacle avoidance strategies being untimely or ineffective when faced with sudden or fast-moving obstacles, increasing the risk of collision.

[0004] To address these issues, researchers have proposed various improvements. For example, an improved artificial potential field algorithm based on pre-added virtual forces can address the issues of traditional artificial potential field methods that often lead to local minima and unreachable target points. Furthermore, reinforcement learning has been used to learn gravitational repulsion field functions to improve the speed and accuracy of trajectory planning. However, these methods still lack efficiency and safety in dynamic obstacle prediction and path planning. Summary of the Invention

[0005] The purpose of this invention is to provide a CAV dynamic path planning method and system based on an improved Dstar-Lite, which can improve the safety and efficiency of CAVs (Connected and Automated Vehicles) in complex dynamic environments.

[0006] In order to solve the above technical problems, the present invention is implemented by adopting the following technical solutions.

[0007] In a first aspect, the present invention proposes a CAV dynamic path planning method based on an improved Dstar-Lite, comprising:

[0008] Collect obstacle data and input the collected obstacle data and CAV starting point into a pre-built dynamic obstacle generation model to generate the initial distribution of obstacles;

[0009] Screen obstacles in the same lane as the CAV target point, and update the CAV target point by integrating the inverse artificial potential field method and simulated annealing algorithm;

[0010] According to the initial distribution of obstacles, the CAV starting point and the updated CAV target point, the improved Dstar-Lite method is used to generate the initial path path1 of the CAV;

[0011] The obstacle and the CAV begin to move. When the obstacle appears within the CAV's preview range, the CAV searches for the affected nodes in the initial path 1 and updates the CAV path. The CAV checks whether it has reached the target point. If so, the search stops. If not, the CAV continues searching for the affected nodes and updates the CAV path.

[0012] The pre-built MPC model is used to track the CAV path and dynamically adjust the CAV's motion trajectory.

[0013] In combination with the first aspect, further, the obstacle data includes the number of rows of the obstacle generation area, the number of columns of the obstacle generation area, the number of obstacles, and the frequency of the obstacles;

[0014] The initial distribution of obstacles includes an obstacle starting point, an obstacle end point, and an obstacle path.

[0015] In combination with the first aspect, further, collecting obstacle data, inputting the collected obstacle data and the CAV starting point into a pre-built dynamic obstacle generation model to generate an initial distribution of obstacles includes:

[0016] Step S11, clarifying input parameters, which include the number of rows (rows), the number of columns (cols), the number of obstacles (numObs), the starting point of the CAV (startPos), and the frequency of the obstacle (stayFreq).

[0017] Step S12: Initialize the obstacle starting point obsStart and the obstacle end point obsGoal, that is, create two empty lists to obtain the obstacle starting point obsStart and the obstacle end point obsGoal, which are used to store the generated obstacle starting point and obstacle end point respectively;

[0018] In step S13, the dynamic obstacle generation model is run to randomly generate an obstacle start point and an obstacle end point. These are stored in the obstacle start point obsStart and obstacle end point obsGoal, respectively. To ensure that the obstacle's motion trajectory conforms to the road scenario (a three-lane road running from left to right), an obstacle start and end point constraint is added, i.e., the start point is to the left of the end point.

[0019] Step S14: Run the dynamic obstacle generation model to randomly generate a horizontal basic path (basePath). Based on the generated horizontal basic path (basePath), randomly add diagonal movement positions according to the line difference between the obstacle starting point and the obstacle end point. Then, based on the obstacle's stop frequency, insert intermediate stationary points in the path of each obstacle to obtain the obstacle path (obsPaths).

[0020] Step S15 , adding a time dimension to the obstacle path obsPaths obtained in step S14 , completing the path lengths of all obstacles, and ensuring that the time dimensions of all obstacles are unified, that is, updating the obstacle path obsPaths .

[0021] In combination with the first aspect, the architecture of the dynamic obstacle generation model is further as follows:

[0022] 1) Establish the starting and ending point constraints of the obstacle and randomly generate the starting and ending coordinates of the obstacle. Set the column coordinates of the obstacle starting point y_start∈[1,10] and the column coordinates of the obstacle ending point y_goal>y_start to ensure that the obstacle's movement path is horizontally from left to right.

[0023] 2) Generate a horizontal base path basePath. Generate a horizontal base path basePath from the column coordinates of the obstacle's starting point to the column coordinates of the obstacle's end point.

[0024] 3) Row-wise adjustment. If the row coordinates of the obstacle's endpoint differ from those of its starting point, a row-wise adjustment is inserted into the horizontal basePath. The column coordinates of the path points for the row-wise adjustment are randomly distributed, but do not exceed the obstacle path length, to obtain the obstacle path. The direction of the adjustment is determined by the difference in row coordinates between the obstacle's endpoint and starting point.

[0025] 4) Insert stationary points. Randomly select some points in the obstacle path obtained in step 3) as stationary points. The stationary time is randomly generated, and the stationary frequency depends on the obstacle's stay frequency stayFreq.

[0026] 5) Obstacle path boundary constraints. Ensure that all points in the obstacle path are within the map range (row coordinates are in [1, rows], column coordinates are in [1, cols]).

[0027] 6) Add a time dimension. Add a timestamp to each point in the obstacle path, starting at 0 and incrementing. The time lengths of all obstacle paths must be aligned to the length of the longest obstacle path. The ends of shorter obstacle paths are completed by repeating the last point until the time length of the longest obstacle path is reached.

[0028] 7) Output parameters. The parameters include the obstacle starting point obsStart, the obstacle end point obsGoal, and the obstacle path obsPaths.

[0029] In combination with the first aspect, further, screening obstacles in the same lane as the CAV target point and updating the CAV target point by integrating the inverse artificial potential field method and the simulated annealing algorithm include:

[0030] Based on the generated obstacle endpoints, screen out obstacles in the same lane as the CAV target point and store the column coordinates of the obstacles in an array. Then, sort the obstacles in the same lane in descending order based on their column coordinates to facilitate the subsequent search for the CAV target point in the reverse direction of the road (from right to left).

[0031] Check whether the array is an empty set; if it is an empty set, it means there is no obstacle in the same lane, and then directly return to the current CAV target point; if it is not an empty set, enter the step of searching for the maximum potential energy point;

[0032] The step of searching for the maximum potential energy point comprises:

[0033] The reverse artificial potential field method is used to calculate the potential energy of the search position. That is, the obstacle generates gravity to form a low potential energy field, and affects the surrounding potential energy field within a certain range. The expression of the reverse artificial potential field method is as follows:

[0034] ;

[0035] in, represents the potential energy of the search position, Indicates the number of obstacles, Indicates the The Euclidean distance between the obstacle and the search position, represents the potential energy gain value, represents the smoothing parameter, Indicates the range of influence of obstacles;

[0036] According to the calculated potential energy of the search position, a simulated annealing algorithm is used to search for the maximum potential energy point. The expression of the simulated annealing algorithm is as follows:

[0037] ;

[0038] ;

[0039] ;

[0040] in, Indicates the The potential energy of the search position at the iteration, represents the current maximum potential energy, Represents the difference between the potential energy of the search position and the current maximum potential energy, Indicates the The probability of accepting the potential energy of the search position as the maximum potential energy point at the iteration, represents the initial temperature, Indicates the The temperature at the iteration, Indicates the The annealing rate at the iteration; Indicates the number of iterations;

[0041] Determine whether the searched maximum potential energy point meets the safety distance requirement, where the safety distance requirement is that the distance between the maximum potential energy point and the obstacle is greater than or equal to a preset safety distance; if the safety distance requirement is met, update the maximum potential energy point to the CAV target point; if the safety distance requirement is not met, continue searching for the maximum potential energy point that meets the safety distance requirement.

[0042] In combination with the first aspect, further, generating the initial path path1 of the CAV using the improved Dstar-Lite method according to the initial distribution of obstacles, the CAV starting point, and the updated CAV target point includes:

[0043] Step S31, initializing node information. The method of initializing node information includes: creating a structure Nodes, storing the location of each node, the actual cost , inspiration cost and the parent node; let the actual cost of all nodes is infinite, and the heuristic cost of all non-target nodes is is infinite, let the heuristic cost of the target node is 0; the actual cost and heuristic cost The expression is as follows:

[0044] ;

[0045] ;

[0046] in, Indicates the current node; express A successor point of represents the target node, i.e. the target point of CAV; Indicates the current node The set of all successor points of ; express arrive the price; and Both represent the current node The shortest path length to the target node. The difference between the two is: Record the actual cost of the exploration, by Assignment; The actual cost at the neighboring node Update when changes occur or the state of the node itself changes to guide the search direction;

[0047] Step S32: Create a priority queue , store the nodes to be detected and updated; the position and key value of the target node Enter the priority queue ;Key value is an array, and the expression is as follows:

[0048] ;

[0049] ;

[0050] in, Indicates the priority sequence of the node to be checked. The smaller the value, the higher the priority of the node. express arrive the price; Represents a key-value modifier; Indicates the current position of CAV. When the CAV current position is taken as the starting point, the CAV current position is calculated to pass through the node The path length to the CAV target point; Indicates the previous position of CAV, It represents the cost of reaching the current position of CAV from the previous position of CAV;

[0051] Step S33: Get the minimum key value node and its key value , calculate the key value of CAV starting point startPos ; Set the inspection termination condition, the inspection termination condition is: if , indicating that the priority of the CAV starting point is higher; if the actual cost of the CAV starting point startPos and heuristic cost If they are equal, it means that the CAV path has been checked from the target node to the CAV starting point startPos; if the check termination condition is met, the check ends; if not, it means that the node If it is not the minimum key value node, continue checking;

[0052] Step S34, recalculate the node The key value k_u, and from the priority queue Delete a node Information; Processing Node The methods include: , indicating that due to environmental changes (such as obstacles appearing), the node It is no longer a node in the optimal path. Re-prioritize To be checked; if the node The actual cost > Heuristic cost , description node There is a new shortcut, , and check the node Neighboring nodes, update the heuristic cost of neighboring nodes If the node The actual cost < Heuristic cost , description node When encountering an obstacle, the node The actual cost is infinite, and the heuristic cost of the neighboring nodes is updated ;

[0053] Step S35: Update neighboring node information; obtain neighboring nodes of the target node and update the heuristic cost of the neighboring nodes and actual cost , if the heuristic cost of the neighboring nodes Changes have occurred, put it in the priority queue Awaiting inspection;

[0054] Step S36: If the priority queue If it is not an empty set, continue to select from the priority queue Get the minimum key value node , repeat steps S33 to S35 until the inspection termination condition is met;

[0055] Step S37, backtracking the initial path; using the CAV starting point as the backtracking starting point, scrolling to obtain the parent node of the current node, and generating the initial path path1.

[0056] In combination with the first aspect, further, the obstacle and the CAV begin to move. When the obstacle appears within the CAV preview distance, the affected nodes in the initial path path1 are searched and the CAV path is updated. The CAV is checked to see if it has reached the target point. If so, the search is stopped. If not, the search is continued for the affected nodes in the initial path path1 and the CAV path is updated, including:

[0057] Step S41, initialize the path and environment; set the preview distance to detect obstacles in front of the current CAV path, the current position of the CAV Initialized to the second node in the initial path path1, CAV previous step position Initialize the CAV starting point startPos; initialize the map field and mark the starting point of the obstacle on the map field;

[0058] It's important to note that the map field represents a 6x30 matrix filled with all 1s (typically, 1 represents a blank area, and 3 represents a dynamic obstacle). Marking the starting point of an obstacle means finding the location corresponding to the obstacle in the field matrix and changing the number at that location to 3. When planning a path, you need to check each node one by one: obtain the location corresponding to a node. If the number at that location in the field is not equal to 1, there is an obstacle at that location and the node needs to be avoided.

[0059] Step S42, updating the dynamic obstacle position according to the generated obstacle path obsPaths;

[0060] Step S43, check whether the obstacle is within the preview distance of the latest CAV path: if not, the CAV continues to move along the latest CAV path; if yes, update the CAV path according to steps S44 to S46;

[0061] Step S44, update the key value modifier 、CAV current location 、CAV previous position ;

[0062] Step S45: for each obstacle node, let its actual cost and heuristic cost is infinite; find all affected child nodes influNodes and update their heuristic costs and and put it in the priority queue Middle: Take the obstacle at the current moment as the initial affected parent node, obtain all non-obstacle child nodes of the affected parent node; traverse all child nodes and evaluate the actual costs of all their adjacent nodes and heuristic cost , find the parent node that minimizes the cost of the child node; if the optimal parent node is the same as the current parent node, mark the child node as an affected child node; traverse all child nodes influNodes and set their heuristic cost Set to infinity and calculate the key value according to the formula in step S32 and put it in the priority queue Awaiting inspection;

[0063] In step S46 , an updated path path2 is generated according to steps S33 to S37 , and the complete paths of the initial path path1 and the updated path path3 are stored in the path path3 .

[0064] In combination with the first aspect, further, using the pre-built MPC model to track the CAV path and dynamically adjust the motion trajectory of the CAV includes:

[0065] Step S51: interpolate the path path3 to generate a smooth path point set path_new;

[0066] Step S52, initialize CAV state parameters and MPC parameters; the CAV state parameters include the horizontal coordinate of the CAV position, the vertical coordinate of the CAV position, the moving direction of the CAV, the linear velocity of the CAV and the angular velocity of the CAV; the MPC parameters include the state weight matrix , control input weight matrix , prediction step length And control step size ;

[0067] Step S53: Enter iteration until the maximum number of iterations is reached or the CAV reaches the target point;

[0068] Step S54, calculating the preview point and the path tangent direction; the preview point is a point in path 3 that is exactly one preview distance away from the current position of the CAV; the path tangent direction is the tangent direction of the position of the preview point in path 3;

[0069] Step S55: construct an MPC model; the method for constructing the MPC model is: collecting state vectors , state matrix , control matrix , output matrix , MPC state matrix , MPC control matrix , the MPC model expression is as follows:

[0070] ;

[0071] ;

[0072] ;

[0073] ;

[0074] ;

[0075] ;

[0076] ;

[0077] ;

[0078] ;

[0079] in, represents the state vector, Indicates the current state of CAV, Indicates the expected state, represents the previous control error, Indicates the current lateral coordinate of the CAV, represents the current longitudinal coordinate of the CAV, Indicates the current moving direction of the CAV, represents the desired lateral coordinate of the CAV, represents the desired longitudinal coordinate of the CAV, represents the desired moving direction of the CAV, Indicates the previous control line speed error, Indicates the previous control angular velocity error, represents the reference speed, represents the time step, Indicates control input time;

[0080] Step S56: construct an optimization problem with the goal of minimizing the weighted average of the control input and tracking error. The objective function expression of the optimization problem is:

[0081] ;

[0082] ;

[0083] ;

[0084] in, represents the objective function, represents the control input matrix, represents the quadratic coefficient matrix of the objective function, represents the MPC control matrix, represents the state weight matrix, represents the control input weight matrix, Represents the linear term coefficient matrix of the objective function, Indicates the expected output, represents the MPC state matrix;

[0085] The constraint expressions of the optimization problem are as follows:

[0086] ;

[0087] ;

[0088] in, represents the constraint matrix, represents the constraint range of the control input, represents the lower bound of the control input variation, Indicates the upper bound of the control input change;

[0089] Step S57: Use the quadratic programming solver to solve the optimization problem and obtain the optimal control input variation. ;

[0090] Step S58, calculate the final control input matrix , the expression is as follows:

[0091] ;

[0092] ;

[0093] in, represents the optimal control input change, represents the linear velocity change of the optimal control input, represents the angular velocity change of the optimal control input, represents the reference control input;

[0094] Step S59: Output the state pose of the CAV at each time step until the CAV reaches the target point; where the state pose represents the current state The state of the next time step includes the horizontal coordinate, vertical coordinate and moving direction of the next time step.

[0095] In a second aspect, the present invention proposes a CAV dynamic path planning system based on an improved Dstar-Lite, comprising:

[0096] a dynamic obstacle generation model module configured to collect obstacle data, input the collected obstacle data and the CAV starting point into a pre-built dynamic obstacle generation model, and generate an initial distribution of obstacles;

[0097] The obstacle intention analysis module is configured to screen obstacles in the same lane as the CAV target point and update the CAV target point by integrating the inverse artificial potential field method and simulated annealing algorithm;

[0098] An initial path path1 generation module is configured to generate an initial path path1 of the CAV using an improved Dstar-Lite method according to an initial distribution of obstacles, a CAV starting point, and an updated CAV destination point;

[0099] The CAV path update module is configured to detect obstacles and start CAV movement. When an obstacle appears within the CAV preview range, it searches for affected nodes in the initial path Path1 and updates the CAV path. It checks whether the CAV has reached the target point. If so, it stops searching. If not, it continues searching for affected nodes and updates the CAV path.

[0100] The motion trajectory adjustment module is configured to track the CAV path using a pre-built MPC model and dynamically adjust the motion trajectory of the CAV.

[0101] In a third aspect, the present invention proposes a computer-readable storage medium having a computer program stored thereon. When the computer program is executed by a processor, the steps of the above-mentioned CAV dynamic path planning method based on the improved Dstar-Lite are implemented.

[0102] In a fourth aspect, the present invention provides a computer device, comprising:

[0103] memory for storing computer programs;

[0104] A processor is used to execute the computer program to implement the steps of the above-mentioned CAV dynamic path planning method based on the improved Dstar-Lite.

[0105] In a fifth aspect, the present invention proposes a computer program product, comprising a computer program, characterized in that when the computer program is executed by a processor, the steps of the above-mentioned CAV dynamic path planning method based on the improved Dstar-Lite are implemented.

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

[0107] (1) The present invention provides an efficient and safe dynamic path planning and tracking method, which can significantly improve the safety and efficiency of CAVs in complex dynamic environments.

[0108] (2) The present invention proposes a path planning method that combines the improved Dstar-Lite algorithm and the MPC optimization model. By establishing a dynamic obstacle generation model, an obstacle path with a starting point, an end point, and dynamic behavior is generated. By analyzing the intention of the obstacle, obstacles in the same lane as the CAV target position are screened, and the target point position of the CAV is dynamically updated by combining the inverse artificial potential field method and the simulated annealing algorithm. Based on the initial distribution of obstacles, the starting point and target point of the CAV, the improved Dstar-Lite method is used to generate the initial path, and the path within the preview distance is monitored in real time. If an obstacle blocks the path, the CAV path is dynamically updated to avoid the obstacle in real time. The MPC algorithm is used to track the CAV path and dynamically adjust the motion trajectory, so that the CAV can accurately track the path and adapt to dynamic environmental changes. BRIEF DESCRIPTION OF THE DRAWINGS

[0109] Figure 1 Schematic diagram of the flow of the dynamic path planning method of the present invention;

[0110] Figure 2 A schematic diagram of the potential field for updating the CAV target point position of the present invention;

[0111] Figure 3 This is a schematic diagram of the dynamic path planning of the present invention;

[0112] Figure 4 Schematic diagram of MPC path tracking of the present invention. DETAILED DESCRIPTION

[0113] The technical solution of the present invention is described in detail below through the accompanying drawings and specific embodiments. It should be understood that the embodiments of the present invention and the specific features in the embodiments are detailed descriptions of the technical solution of the present invention, rather than limitations on the technical solution of the present invention. In the absence of conflict, the embodiments of the present invention and the technical features in the embodiments can be combined with each other.

[0114] The term "and / or" simply describes a relationship between related objects, indicating that three possible relationships exist. For example, "A and / or B" can mean: A exists alone, A and B exist simultaneously, or B exists alone. Additionally, the character " / " generally indicates an "or" relationship between the related objects.

[0115] Example 1

[0116] like Figure 1 As shown, the CAV dynamic path planning method based on the improved Dstar-Lite of this embodiment includes the following steps:

[0117] Step S1: Establish a dynamic obstacle generation model to randomly generate dynamic obstacles. Based on the specified input parameters, such as the area size (including the number of rows and columns), the number of obstacles numObs, the CAV starting point startPos, and the stay frequency stayFreq, an initial obstacle distribution with obstacle starting points, obstacle ending points, and obstacle paths is generated.

[0118] Step S2: obstacle intention analysis, screening obstacles in the same lane as the CAV target position, and integrating the inverse artificial potential field method and simulated annealing algorithm to update the CAV target point.

[0119] S3, CAV target position update and CAV initial path generation. Based on the initial distribution of obstacles, the CAV starting point (startPos) and the updated CAV target point (goalPos), the improved Dstar-Lite method is used to generate the CAV initial path path1.

[0120] S4: The obstacle and the CAV begin moving. The system determines whether the obstacle blocks the path. If so, the CAV path is updated. If not, the CAV moves along the latest path. Specifically, when the obstacle appears within the CAV's preview range, the affected nodes on the initial path Path1 are searched and the CAV path is updated. The CAV then checks whether it has reached the target point. If so, the search stops. If not, the CAV continues searching for the affected nodes on the path and updates the CAV path.

[0121] S5. Use the pre-built MPC model to track the CAV path and dynamically adjust the CAV's motion trajectory.

[0122] In a specific implementation of this embodiment, step S1 is as follows:

[0123] Step S11, clarifying input parameters, which include the number of rows (rows), the number of columns (cols), the number of obstacles (numObs), the starting point of the CAV (startPos), and the frequency of obstacles (stayFreq);

[0124] Step S12: Initialize the obstacle starting point obsStart and the obstacle end point obsGoal, that is, create two empty lists to obtain the obstacle starting point obsStart and the obstacle end point obsGoal, which are used to store the generated obstacle starting point and obstacle end point respectively;

[0125] In step S13, the dynamic obstacle generation model is run to randomly generate an obstacle start point and an obstacle end point. These are stored in the obstacle start point obsStart and obstacle end point obsGoal, respectively. To ensure that the obstacle's motion trajectory conforms to the road scenario (a three-lane road running from left to right), an obstacle start and end point constraint is added, i.e., the start point is to the left of the end point.

[0126] Step S14: Run the dynamic obstacle generation model to randomly generate a horizontal basic path (basePath). Based on the generated horizontal basic path (basePath), randomly add diagonal movement positions according to the line difference between the obstacle starting point and the obstacle end point. Then, based on the obstacle's stop frequency, insert intermediate stationary points in the path of each obstacle to obtain the obstacle path (obsPaths).

[0127] Step S15 , adding a time dimension to the obstacle path obsPaths obtained in step S14 , completing the path lengths of all obstacles, and ensuring that the time dimensions of all obstacles are unified, that is, updating the obstacle path obsPaths .

[0128] In a specific implementation of this embodiment, step S2 is specifically as follows:

[0129] Step S21: Based on the generated obstacle endpoints, screen for obstacles in the same lane as the CAV target point and store the column coordinates of the obstacles in an array. The obstacles in the same lane are then sorted in descending order based on their column coordinates to facilitate subsequent searching for the CAV target point in the reverse direction of the road (from right to left).

[0130] Step S22, checking whether the array is an empty set; if it is an empty set, indicating that there is no obstacle in the same lane, then directly returning to the current CAV target point; if it is not an empty set, then entering the step of searching for the maximum potential energy point;

[0131] The steps to search for the maximum potential energy point include:

[0132] The reverse artificial potential field method is used to calculate the potential energy of the search position. That is, the obstacle generates gravity to form a low potential energy field, and affects the surrounding potential energy field within a certain range. The expression of the reverse artificial potential field method is as follows:

[0133] ;

[0134] in, represents the potential energy of the search position, Indicates the number of obstacles, Indicates the The Euclidean distance between the obstacle and the search position, represents the potential energy gain value, represents the smoothing parameter, Indicates the range of influence of obstacles;

[0135] In step S23, a simulated annealing algorithm is used to search for the maximum potential energy point according to the calculated potential energy of the search position. The expression of the simulated annealing algorithm is as follows:

[0136] ;

[0137] ;

[0138] ;

[0139] in, Indicates the The potential energy of the search position at the iteration, represents the current maximum potential energy, Represents the difference between the potential energy of the search position and the current maximum potential energy, Indicates the The probability of accepting the potential energy of the search position as the maximum potential energy point at the iteration, represents the initial temperature, Indicates the The temperature at the iteration, Indicates the The annealing rate at the iteration; Indicates the number of iterations;

[0140] Step S24, determine whether the searched maximum potential energy point meets the safety distance requirement, and the safety distance requirement is that the distance between the maximum potential energy point and the obstacle is greater than or equal to the pre-set safety distance; if the safety distance requirement is met, the maximum potential energy point is updated to the CAV target point; if the safety distance requirement is not met, continue to search for the maximum potential energy point that meets the safety distance requirement.

[0141] In a specific implementation of this embodiment, step S3 is as follows:

[0142] Step S31, initializing node information. The method of initializing node information includes: creating a structure Nodes, storing the location and actual cost of each node , inspiration cost and the parent node; let the actual cost of all nodes is infinite, and the heuristic cost of all non-target nodes is is infinite, let the heuristic cost of the target node is 0; the actual cost and heuristic cost The expression is as follows:

[0143] ;

[0144] ;

[0145] in, Indicates the current node; express A successor point of represents the target node, i.e. the target point of CAV; Indicates the current node The set of all successor points of ; express arrive the price; and Both represent the current node The shortest path length to the target node. The difference between the two is: Record the actual cost of the exploration, by Assignment; The actual cost at the neighboring node Update when changes occur or the state of the node itself changes to guide the search direction;

[0146] Step S32: Create a priority queue , store the nodes to be detected and updated; the position and key value of the target node Enter the priority queue ;Key value is an array, and the expression is as follows:

[0147] ;

[0148] ;

[0149] in, Indicates the priority sequence of the node to be checked. The smaller the value, the higher the priority of the node. express arrive the price; Represents a key-value modifier; Indicates the current position of CAV. When the CAV current position is taken as the starting point, the CAV current position is calculated to pass through the node The path length to the CAV target point; Indicates the previous position of CAV, It represents the cost of reaching the current position of CAV from the previous position of CAV;

[0150] Step S33: Get the minimum key value node and its key value , calculate the key value of CAV starting point startPos ; Set the check termination condition, the check termination condition is: if , indicating that the priority of the CAV starting point is higher; if the actual cost of the CAV starting point startPos and heuristic cost If they are equal, it means that the CAV path has been checked from the target node to the CAV starting point startPos; if the check termination condition is met, the check ends; if not, it means that the node If it is not the minimum key value node, continue checking;

[0151] Step S34, recalculate the node The key value k_u, and from the priority queue Delete a node Information; Processing Node The methods include: , indicating that due to environmental changes (such as obstacles appearing), the node It is no longer a node in the optimal path. Re-prioritize To be checked; if the node The actual cost > Heuristic cost , description node There is a new shortcut, , and check the node Neighboring nodes, update the heuristic cost of neighboring nodes If the node The actual cost < Heuristic cost , description node When encountering an obstacle, the node The actual cost is infinite, and the heuristic cost of the neighboring nodes is updated ;

[0152] Step S35: Update neighboring node information; obtain neighboring nodes of the target node and update the heuristic cost of the neighboring nodes and actual cost , if the heuristic cost of the neighboring nodes Changes have occurred, put it in the priority queue Awaiting inspection;

[0153] Step S36: If the priority queue If it is not an empty set, continue to select from the priority queue Get the minimum key value node , repeat steps S33 to S35 until the inspection termination condition is met;

[0154] Step S37, backtracking the initial path; using the CAV starting point as the backtracking starting point, scrolling to obtain the parent node of the current node, and generating the initial path path1.

[0155] In a specific implementation of this embodiment, step S4 is as follows:

[0156] Step S41, initialize the path and environment; set the preview distance to detect obstacles in front of the current CAV path, the current position of the CAV Initialized to the second node in the initial path path1, CAV previous step position Initialize the CAV starting point startPos; initialize the map field and mark the starting point of the obstacle on the map field;

[0157] Step S42, updating the dynamic obstacle position according to the generated obstacle path obsPaths;

[0158] Step S43, check whether the obstacle is within the preview distance of the latest CAV path: if not, the CAV continues to move along the latest CAV path; if yes, update the CAV path according to steps S44 to S46;

[0159] Step S44, update the key value modifier 、CAV current location 、CAV previous position ;

[0160] Step S45: for each obstacle node, let its actual cost and heuristic cost is infinite; find all affected child nodes influNodes and update their heuristic costs and and put it in the priority queue Middle: Take the obstacle at the current moment as the initial affected parent node, obtain all non-obstacle child nodes of the affected parent node; traverse all child nodes and evaluate the actual costs of all their adjacent nodes and heuristic cost , find the parent node that minimizes the cost of the child node; if the optimal parent node is the same as the current parent node, mark the child node as an affected child node; traverse all child nodes influNodes and set their heuristic cost Set to infinity and calculate the key value according to the formula in step S32 and put it in the priority queue Awaiting inspection;

[0161] In step S46 , an updated path path2 is generated according to steps S33 to S37 , and the complete paths of the initial path path1 and the updated path path3 are stored in the path path3 .

[0162] In a specific implementation of this embodiment, step S5 is as follows:

[0163] Step S51: interpolate the path path3 to generate a smooth path point set path_new;

[0164] Step S52, initialize CAV state parameters and MPC parameters; CAV state parameters include the horizontal coordinate of CAV position, the vertical coordinate of CAV position, the moving direction of CAV, the linear velocity of CAV and the angular velocity of CAV; MPC parameters include the state weight matrix , control input weight matrix , prediction step length And control step size ;

[0165] Step S53: Enter iteration until the maximum number of iterations is reached or the CAV reaches the target point;

[0166] Step S54, calculate the preview point and the path tangent direction; the preview point is a point in path 3 that is exactly one preview distance away from the current position of the CAV; the path tangent direction is the tangent direction of the position of the preview point in path 3;

[0167] Step S55: construct an MPC model. The method for constructing the MPC model is: collecting state vectors , state matrix , control matrix , output matrix , MPC state matrix , MPC control matrix , the MPC model expression is as follows:

[0168] ;

[0169] ;

[0170] ;

[0171] ;

[0172] ;

[0173] ;

[0174] ;

[0175] ;

[0176] ;

[0177] in, represents the state vector, Indicates the current state of CAV, Indicates the expected state, represents the previous control error, Indicates the current lateral coordinate of the CAV, represents the current longitudinal coordinate of the CAV, Indicates the current moving direction of the CAV, represents the desired lateral coordinate of the CAV, represents the desired longitudinal coordinate of the CAV, represents the desired moving direction of the CAV, Indicates the previous control line speed error, Indicates the previous control angular velocity error, represents the reference speed, represents the time step, Indicates control input time;

[0178] Step S56: construct an optimization problem with the goal of minimizing the weighted average of the control input and tracking error. The objective function expression of the optimization problem is:

[0179] ;

[0180] ;

[0181] ;

[0182] in, represents the objective function, represents the control input matrix, represents the quadratic coefficient matrix of the objective function, represents the MPC control matrix, represents the state weight matrix, represents the control input weight matrix, Represents the linear term coefficient matrix of the objective function, Indicates the expected output, represents the MPC state matrix;

[0183] The constraint expressions of the optimization problem are as follows:

[0184] ;

[0185] ;

[0186] in, represents the constraint matrix, represents the constraint range of the control input, represents the lower bound of the control input variation, Indicates the upper bound of the control input change;

[0187] Step S57: Use the quadratic programming solver to solve the optimization problem and obtain the optimal control input variation. ;

[0188] Step S58, calculate the final control input matrix , the expression is as follows:

[0189] ;

[0190] ;

[0191] in, represents the optimal control input change, represents the linear velocity change of the optimal control input, represents the angular velocity change of the optimal control input, represents the reference control input;

[0192] Step S59: Output the state pose of the CAV at each time step until the CAV reaches the target point; where the state pose represents the current state The state of the next time step includes the horizontal coordinate, vertical coordinate and moving direction of the next time step.

[0193] This invention adds an inverse artificial potential field algorithm and a simulated annealing algorithm (steps S22-S24) to the existing Dstar-Lite algorithm to address the CAV's target obstruction and improve its adaptability to dynamic environments. An MPC model (MPC tracking control algorithm, see steps S53-S59 for details) is then added to address path smoothness. Furthermore, the system considers parameters such as input control to address both real-time and comfortable routing.

[0194] Example 2

[0195] This embodiment further illustrates the solution and effects of the present invention through specific application examples.

[0196] The experimental scenario is set as a three-lane road upstream of the intersection, with lanes running from left to right. The three lanes are left-turn, straight-ahead, and right-turn lanes. Rasterize the map, set the number of rows to 6 and the number of columns to 30, such as Figure 3As shown in the figure, the CAV experimental task is to change lanes of vehicles upstream of the intersection, from the right-turn lane to the left-turn lane, that is, the CAV starting point startPos = [6,1] and the CAV target point goalPos = [1,30]. It should be noted that: Figure 3 The CAV target point (rose red) is the updated CAV target point [1,26]. The original CAV target point [1,30] is invalid and is no longer displayed. Using the reverse artificial potential field algorithm, the potential field distribution of the CAV target lane (rows=1) can be obtained, as shown in the following example: Figure 2 As shown, Figure 2 Where X represents the row coordinate and Y represents the column coordinate. It can be seen that the original CAV target point goalPos (green) will be blocked by the dynamic obstacle (red). The simulated annealing algorithm is used to search for the position with the maximum potential energy and update the CAV target point to obtain the new CAV target point (blue) goalPos = [1,26]. The improved Dstar-Lite algorithm is used to plan the path in real time in a dynamic environment, such as Figure 3 As shown in Figure 2. The CAV detects a dynamic obstacle (red) within the preview distance at its current position (gray), so it updates the path based on the original path to obtain the CAV updated path (blue). Figure 4 As shown in the figure, after the path planning is completed, the MPC tracking control algorithm is used for tracking control to further smooth the CAV rough path (red dotted line), and the tracking speed and angular velocity are calculated to form a smooth and stable CAV tracking path (red solid line).

[0197] We conducted an experimental comparison using three path planning and tracking methods: the Astar+PID algorithm, the Dstar-Lite+PID algorithm, and the CAV dynamic path planning method of our invention (i.e., the improved Dstar-Lite+MPC algorithm in Table 1). Three dynamic scenarios were set, with 3, 5, and 7 dynamic obstacles, respectively. In each scenario, the starting and ending points of the dynamic obstacles and the obstacle paths were randomly generated. Path planning and tracking were performed using the three algorithms. The experiment was repeated 50 times, yielding the following experimental results, as shown in Table 1.

[0198] Table 1 Experimental results

[0199]

[0200] Experimental results show that the improved Dstar-Lite+MPC algorithm of the present invention has the highest success rate in all three dynamic scenarios, demonstrating the superiority of the inverse artificial potential field algorithm. Dstar-Lite outperforms Astar in terms of expansion points because, when an obstacle suddenly appears or disappears, the Dstar-Lite algorithm only needs to modify the affected node parameters, updating the nodes based on the original planned path without having to calculate a large range of node parameters. In contrast, Astar requires clearing all node parameters and recalculating, meaning that Astar chooses to replan the path, resulting in a greater computational effort and number of iterations. In terms of smoothness and comfort, the improved Dstar-Lite+MPC algorithm of the present invention performs best because the MPC algorithm can predict the future corresponding state of the control input, enabling real-time tracking control.

[0201] Example 3

[0202] Based on the same inventive concept as Example 1, this example introduces a CAV dynamic path planning system based on an improved Dstar-Lite, including:

[0203] a dynamic obstacle generation model module configured to collect obstacle data, input the collected obstacle data and the CAV starting point into a pre-built dynamic obstacle generation model, and generate an initial distribution of obstacles;

[0204] The obstacle intention analysis module is configured to screen obstacles in the same lane as the CAV target point and update the CAV target point by integrating the inverse artificial potential field method and simulated annealing algorithm;

[0205] An initial path path1 generating module is configured to generate an initial path path1 of the CAV using a Dstar-Lite method according to an initial distribution of obstacles, a CAV starting point, and a CAV destination point;

[0206] The CAV path update module is configured to detect obstacles and start CAV movement. When an obstacle appears within the CAV preview range, it searches for affected nodes in the initial path Path1 and updates the CAV path. It checks whether the CAV has reached the target point. If so, it stops searching. If not, it continues searching for affected nodes and updates the CAV path.

[0207] The motion trajectory adjustment module is configured to track the CAV path using a pre-built MPC model and dynamically adjust the motion trajectory of the CAV.

[0208] Example 4

[0209] Based on the same inventive concept as other embodiments, this embodiment introduces a computer-readable storage medium having a computer program stored thereon. When the computer program is executed by a processor, the steps of the above-mentioned CAV dynamic path planning method based on the improved Dstar-Lite are implemented.

[0210] Example 5

[0211] Based on the same inventive concept as other embodiments, this embodiment introduces a computer device, including: a memory for storing a computer program; and a processor for executing the computer program to implement the steps of the above-mentioned CAV dynamic path planning method based on the improved Dstar-Lite.

[0212] Example 6

[0213] Based on the same inventive concept as other embodiments, this embodiment introduces a computer program product, including a computer program. When the computer program is executed by a processor, the steps of the above-mentioned CAV dynamic path planning method based on the improved Dstar-Lite are implemented.

[0214] Those skilled in the art will appreciate that embodiments of the present invention may be provided as methods, systems, or computer program products. Thus, the present invention may take the form of an entirely hardware embodiment, an entirely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, the present invention may take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to magnetic disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0215] The present invention is described with reference to flowcharts and / or block diagrams of methods, devices (systems), and computer program products according to embodiments of the present invention. It should be understood that each process and / or block in the flowcharts and / or block diagrams, as well as combinations of processes and / or blocks in the flowcharts and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowcharts and / or block diagrams. Figure 1 a process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0216] These computer program instructions may also be stored in a computer readable memory that can direct a computer or other programmable data processing device to work in a specific manner, so that the instructions stored in the computer readable memory produce an article of manufacture comprising an instruction device, which implements the process Figure 1a process or multiple processes and / or boxes Figure 1 The function specified in one or more boxes.

[0217] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operational steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing the instructions executed on the computer or other programmable device for implementing the process. Figure 1 a process or multiple processes and / or boxes Figure 1 A step that specifies a function in one or more boxes.

[0218] The embodiments of the present invention are described above in conjunction with the accompanying drawings, but the present invention is not limited to the above-mentioned specific implementation methods. The above-mentioned specific implementation methods are merely illustrative and not restrictive. Ordinary technicians in this field can also make many forms under the guidance of the present invention, which all fall within the protection of the present invention.

Claims

1. A CAV dynamic path planning method based on improved Dstar-Lite, characterized in that: include: Collect obstacle data and input the collected obstacle data and CAV starting point into a pre-built dynamic obstacle generation model to generate the initial distribution of obstacles; Screen obstacles in the same lane as the CAV target point, and update the CAV target point by integrating the inverse artificial potential field method and simulated annealing algorithm; According to the initial distribution of obstacles, the CAV starting point and the updated CAV target point, the improved Dstar-Lite method is used to generate the initial path path1 of the CAV; The obstacle and the CAV begin to move. When the obstacle appears within the CAV's preview range, the CAV searches for the affected nodes in the initial path 1 and updates the CAV path. The CAV checks whether it has reached the target point. If so, the search stops. If not, the CAV continues searching for the affected nodes and updates the CAV path. Use the pre-built MPC model to track the CAV path and dynamically adjust the CAV's motion trajectory; The expression of the reverse artificial potential field method is as follows: ； in, represents the potential energy of the search position, Indicates the number of obstacles, Indicates the The Euclidean distance between the obstacle and the search position, represents the potential energy gain value, represents the smoothing parameter, Indicates the range of influence of obstacles; The improved Dstar-Lite method is used to generate the initial path path1 of the CAV according to the initial distribution of obstacles, the CAV starting point and the updated CAV target point, including: Step S31, initializing node information. The method of initializing node information includes: creating a structure Nodes, storing the location of each node, the actual cost , inspiration cost and the parent node; let the actual cost of all nodes is infinite, and the heuristic cost of all non-target nodes is is infinite, let the heuristic cost of the target node is 0; the actual cost and heuristic cost The expression is as follows: ； ； in, Indicates the current node; express A successor point of Indicates the target node; Indicates the current node The set of all successor points of ; express arrive the price; and Both represent the current node The shortest path length to the target node. The difference between the two is: Record the actual cost of the exploration, by Assignment; The actual cost at the neighboring node Update when changes occur or the state of the node itself changes; Step S32: Create a priority queue , store the nodes to be detected and updated; the position and key value of the target node Enter the priority queue ;Key value is an array, and the expression is as follows: ； ； in, Indicates the priority sequence of the node to be checked. The smaller the value, the higher the priority of the node. express arrive the price; Represents a key-value modifier; Indicates the current position of the CAV; Indicates the previous position of CAV, It represents the cost of reaching the current position of CAV from the previous position of CAV; Step S33: Get the minimum key value node and its key value , calculate the key value of CAV starting point startPos ; Set the inspection termination condition, the inspection termination condition is: if , indicating that the priority of the CAV starting point is higher; if the actual cost of the CAV starting point startPos and heuristic cost If they are equal, it means that the CAV path has been checked from the target node to the CAV starting point startPos; if the check termination condition is met, the check ends; if not, it means that the node If it is not the minimum key value node, continue checking; Step S34, recalculate the node The key value k_u, and from the priority queue Delete a node Information; Processing Node The methods include: , indicating that due to environmental changes, nodes It is no longer a node in the optimal path. Re-prioritize To be checked; if the node The actual cost > Heuristic cost , description node There is a new shortcut, , and check the node Neighboring nodes, update the heuristic cost of neighboring nodes If the node The actual cost < Heuristic cost , description node When encountering an obstacle, the node The actual cost is infinite, and the heuristic cost of the neighboring nodes is updated ; Step S35: Update neighboring node information; obtain neighboring nodes of the target node and update the heuristic cost of the neighboring nodes and actual cost , if the heuristic cost of the neighboring nodes Changes have occurred, put it in the priority queue Awaiting inspection; Step S36: If the priority queue If it is not an empty set, continue to select from the priority queue Get the minimum key value node , repeat steps S33 to S35 until the inspection termination condition is met; Step S37, backtracking the initial path; using the CAV starting point as the backtracking starting point, scrolling to obtain the parent node of the current node, and generating the initial path path1.

2. The CAV dynamic path planning method based on the improved Dstar-Lite according to claim 1, characterized in that: The obstacle data includes the number of rows of the obstacle generation area, the number of columns of the obstacle generation area, the number of obstacles, and the frequency of the obstacles; The initial distribution of obstacles includes an obstacle starting point, an obstacle end point, and an obstacle path.

3. The CAV dynamic path planning method based on the improved Dstar-Lite according to claim 1, characterized in that: The obstacle data is collected, and the collected obstacle data and the CAV starting point are input into a pre-built dynamic obstacle generation model to generate an initial distribution of obstacles, including: Step S11, clarifying input parameters, which include the number of rows (rows), the number of columns (cols), the number of obstacles (numObs), the starting point of the CAV (startPos), and the frequency of the obstacle (stayFreq). Step S12: Initialize the obstacle starting point obsStart and the obstacle end point obsGoal, that is, create two empty lists to obtain the obstacle starting point obsStart and the obstacle end point obsGoal, which are used to store the generated obstacle starting point and obstacle end point respectively; Step S13: Run the dynamic obstacle generation model to randomly generate an obstacle start point and an obstacle end point. The generated obstacle start point and obstacle end point are stored in the obstacle start point obsStart and obstacle end point obsGoal, respectively. To ensure that the obstacle's motion trajectory conforms to the road scene, an obstacle start and end point constraint is added, that is, the start point is to the left of the end point. Step S14: Run the dynamic obstacle generation model to randomly generate a horizontal basic path (basePath). Based on the generated horizontal basic path (basePath), randomly add diagonal movement positions according to the line difference between the obstacle starting point and the obstacle end point. Then, based on the obstacle's stop frequency, insert intermediate stationary points in the path of each obstacle to obtain the obstacle path (obsPaths). Step S15 , adding a time dimension to the obstacle path obsPaths obtained in step S14 , completing the path lengths of all obstacles, and ensuring that the time dimensions of all obstacles are unified, that is, updating the obstacle path obsPaths .

4. The CAV dynamic path planning method based on the improved Dstar-Lite according to claim 2, characterized in that: The method of screening obstacles in the same lane as the CAV target point and updating the CAV target point by integrating the inverse artificial potential field method and the simulated annealing algorithm includes: According to the generated obstacle endpoint, the obstacles in the same lane as the CAV target point are filtered and the column coordinates of the obstacles are stored in an array. The obstacles in the same lane are sorted in descending order based on the column coordinates of all obstacles in the same lane. Check whether the array is an empty set; if it is an empty set, it means there is no obstacle in the same lane, and then directly return to the current CAV target point; if it is not an empty set, enter the step of searching for the maximum potential energy point; The step of searching for the maximum potential energy point comprises: The reverse artificial potential field method is used to calculate the potential energy of the search position; According to the calculated potential energy of the search position, a simulated annealing algorithm is used to search for the maximum potential energy point. The expression of the simulated annealing algorithm is as follows: ； ； ； in, Indicates the The potential energy of the search position at the iteration, represents the current maximum potential energy, Represents the difference between the potential energy of the search position and the current maximum potential energy, Indicates the The probability of accepting the potential energy of the search position as the maximum potential energy point at the iteration, represents the initial temperature, Indicates the The temperature at the iteration, Indicates the The annealing rate at the iteration; Indicates the number of iterations; Determine whether the searched maximum potential energy point meets the safety distance requirement, where the safety distance requirement is that the distance between the maximum potential energy point and the obstacle is greater than or equal to a preset safety distance; if the safety distance requirement is met, update the maximum potential energy point to the CAV target point; if the safety distance requirement is not met, continue searching for the maximum potential energy point that meets the safety distance requirement.

5. The CAV dynamic path planning method based on the improved Dstar-Lite according to claim 2, characterized in that: The obstacle and CAV start to move. When the obstacle appears within the CAV preview range, the affected nodes in the initial path path1 are searched and the CAV path is updated. The CAV is checked to see if it has reached the target point. If so, the search stops. If not, continue searching for the affected nodes in the initial path path1 and update the CAV path, including: Step S41, initialize the path and environment; set the preview distance to detect obstacles in front of the current CAV path, the current position of the CAV Initialized to the second node in the initial path path1, CAV previous step position Initialize the CAV starting point startPos; initialize the map field and mark the starting point of the obstacle on the map field; Step S42, updating the dynamic obstacle position according to the generated obstacle path obsPaths; Step S43, check whether the obstacle is within the preview distance of the latest CAV path: if not, the CAV continues to move along the latest CAV path; if yes, update the CAV path according to steps S44 to S46; Step S44, update the key value modifier 、CAV current location 、CAV previous position ; Step S45: for each obstacle node, let its actual cost and heuristic cost is infinite; find all affected child nodes influNodes and update their heuristic costs and and put it in the priority queue Middle: Take the obstacle at the current moment as the initial affected parent node, obtain all non-obstacle child nodes of the affected parent node; traverse all child nodes and evaluate the actual costs of all their adjacent nodes and heuristic cost , find the parent node that minimizes the cost of the child node; if the optimal parent node is the same as the current parent node, mark the child node as an affected child node; traverse all child nodes influNodes and set their heuristic cost Set to infinity and calculate the key value according to the formula in step S32 and put it in the priority queue Awaiting inspection; In step S46 , an updated path path2 is generated according to steps S33 to S37 , and the complete paths of the initial path path1 and the updated path path3 are stored in the path path3 .

6. A CAV dynamic path planning system based on improved Dstar-Lite, characterized in that: The method for implementing the CAV dynamic path planning method based on the improved Dstar-Lite according to any one of claims 1 to 5 comprises: a dynamic obstacle generation model module configured to collect obstacle data, input the collected obstacle data and the CAV starting point into a pre-built dynamic obstacle generation model, and generate an initial distribution of obstacles; The obstacle intention analysis module is configured to screen obstacles in the same lane as the CAV target point and update the CAV target point by integrating the inverse artificial potential field method and simulated annealing algorithm; An initial path path1 generation module is configured to generate an initial path path1 of the CAV using an improved Dstar-Lite method according to an initial distribution of obstacles, a CAV starting point, and an updated CAV destination point; The CAV path update module is configured to detect obstacles and start CAV movement. When an obstacle appears within the CAV preview range, it searches for affected nodes in the initial path Path1 and updates the CAV path. It checks whether the CAV has reached the target point. If so, it stops searching. If not, it continues searching for affected nodes and updates the CAV path. The motion trajectory adjustment module is configured to track the CAV path using a pre-built MPC model and dynamically adjust the motion trajectory of the CAV.

7. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the steps of the CAV dynamic path planning method based on the improved Dstar-Lite described in any one of claims 1 to 5 are implemented.

8. A computer device, characterized in that: include: memory for storing computer programs; A processor is configured to execute the computer program to implement the steps of the CAV dynamic path planning method based on the improved Dstar-Lite according to any one of claims 1 to 5.

9. A computer program product comprising a computer program, characterized in that: When the computer program is executed by a processor, the steps of the CAV dynamic path planning method based on the improved Dstar-Lite according to any one of claims 1 to 5 are implemented.

Images