Vehicle path planning method fusing improved RRT and Trajectory Mamb-DWA algorithm

Abstract

The invention provides a vehicle path planning method fusing improved RRT and Trajectory Mamb-DWA, and the method is suitable for a dynamic traffic environment. In the global planning stage, the search efficiency of the RRT algorithm is optimized through KD-Tree, the random sampling range is limited in combination with a safe sampling area, the directivity of the path is enhanced by utilizing adaptive target gravitation bias, and a global optimal path is generated by adopting Bezier curve smoothing. In the local obstacle avoidance stage, a Trajectory Mama trajectory prediction model is embedded in a dynamic window method, the movement trends of surrounding vehicles and pedestrians are predicted, the speed of the vehicle is restrained through time sequence collision detection, and an obstacle avoidance trajectory with both real-time performance and foresight is generated. And aiming at the path connection problem after obstacle avoidance, designing a dynamic re-planning mechanism: according to the current environment state, comparing expected time for returning to an original path or generating a new path with a collision risk, and adaptively selecting an optimal scheme. The method provides key technical support for efficient navigation of the autonomous vehicle, and is of great significance to development of a driving assistance system.

Description

Technical Field

[0001] The present invention relates to the field of artificial intelligence and autonomous driving technology, and in particular to a vehicle path planning method in traffic scenarios that integrates an improved RRT algorithm and an improved DWA algorithm combined with a Trajectory Mamba trajectory detection model. Background Art

[0002] Path planning is a key technical issue in the fields of autonomous driving and intelligent transportation systems. With the acceleration of urbanization and the continuous growth of traffic volume, vehicle path planning in complex and dynamic environments faces increasing challenges. Existing path planning methods perform well when dealing with static obstacles and known environmental structures, but often fail to successfully complete obstacle avoidance and path planning tasks when dealing with complex and changing traffic environments and unpredictable dynamic obstacle behavior.

[0003] Traditional path planning methods, such as the Rapidly Exploring Random Trees (RRT) algorithm, the A* algorithm, and its variants, are typically based on static maps and predefined motion models. They are unable to perceive and predict the movement trends of surrounding dynamic obstacles in real time. Consequently, they lack the ability to effectively predict and adapt to the behavior of dynamic obstacles. As a result, in actual applications, vehicles may not be able to respond to sudden changes in obstacles in a timely manner, increasing their collision risk.

[0004] Existing local obstacle avoidance algorithms, such as the Optimized Collision Avoidance (ORCA) algorithm and the artificial potential field method, primarily rely on sensors to perceive information about surrounding dynamic obstacles and generate safe speeds and steering angles based on predefined obstacle avoidance strategies. These predefined local obstacle avoidance algorithms lack the ability to predict the future trajectory of obstacles, potentially leading to overly conservative or insufficiently proactive vehicle avoidance behaviors, impacting smoothness and efficiency. Furthermore, local obstacle avoidance algorithms lack the guidance of global path planning, which can lead to frequent route adjustments and reduce path planning efficiency.

[0005] Furthermore, existing technologies struggle to integrate global path planning with local obstacle avoidance. Lack of an efficient algorithm fusion and switching mechanism prevents efficient and accurate path planning while ensuring safety. A major challenge facing current technologies is how to quickly and effectively re-plan a vehicle's path after performing local obstacle avoidance maneuvers to adapt to dynamically changing environments.

[0006] The limitations of existing technologies not only affect the performance of autonomous vehicles but also pose new challenges to vehicle safety and the development of driver assistance systems. Therefore, overcoming these limitations and achieving accurate, stable, and real-time path planning and obstacle avoidance in dynamically changing traffic environments has become a key issue in improving the performance of autonomous driving systems. Summary of the Invention

[0007] To address the limitations of existing technologies, this paper proposes a vehicle path planning method that integrates an improved RRT algorithm with the TrajectoryMamba-DWA algorithm. By integrating an improved Rapid Exploration Random Tree (RRT) algorithm with an improved Dynamic Window Approach (DWA) based on the Trajectory Mamba trajectory prediction model, this method achieves efficient integration and dynamic switching between global path planning and local obstacle avoidance, providing an innovative solution for vehicle path planning in traffic scenarios.

[0008] The present invention adopts the following technical solutions:

[0009] The vehicle path planning method that integrates the improved RRT and Trajectory Mamba-DWA algorithms includes the following:

[0010] Step S1: Load a two-dimensional global map containing known static obstacles based on the actual map, detect the collision area of ​​the static obstacles and mark them;

[0011] Step S2: Based on the global map and the vehicle's starting and destination points, a globally optimal path is quickly planned for the vehicle using an improved Rapid Exploration Random Tree (RRT) algorithm, combined with a KD-Tree-based search optimization strategy, a sampling optimization strategy that considers safe sampling areas, a node selection strategy based on adaptive target gravity bias, and a Bezier curve-based path smoothing strategy. The vehicle then drives steadily toward the next node based on the planned globally optimal path.

[0012] Step S3: To improve obstacle avoidance during driving, the Trajectory Mamba trajectory prediction model is integrated into the DWA algorithm to predict the future movement trends of surrounding vehicles and pedestrians. Time-series collision detection is used to further constrain the safe driving speed of the ego vehicle, guiding the vehicle to avoid potential collision risks, thereby enhancing local obstacle avoidance performance.

[0013] Step S4: After the vehicle performs the obstacle avoidance operation, the subsequent path is evaluated based on the dynamic path replanning mechanism, and two options are calculated: one is to immediately return to the original planned path; the other is to plan a new path directly from the current position after obstacle avoidance to the target point. By comparing the performance indicators of the two options such as path length, estimated travel time, and obstacle avoidance effect, the optimal option is selected as the subsequent path; steps S3 and S4 are repeated until the vehicle reaches the target point.

[0014] Furthermore, the specific steps of step S2 are as follows:

[0015] Step S2.1: Use a KD-Tree-based search optimization strategy to speed up the node search process and reduce the time required to search for neighboring nodes. The specific steps are as follows:

[0016] First, simplify the three-dimensional map into a two-dimensional problem, ignore the height information (x direction), and only consider the plane coordinates (y, z). In each layer of recursion, alternately select the y direction or the z direction as the split dimension. On the current split dimension, calculate the median of all nodes as the split point. For example, in the y direction, sort all nodes by y coordinate, select the median node as the split point, and divide the node set into two parts: the left subtree contains nodes with y coordinates less than the median, and the right subtree contains nodes with y coordinates greater than the median. Then recursively perform the above splitting steps on the left subtree and the right subtree respectively until each sub-area contains only one node or the preset recursive depth is reached.

[0017] When searching for adjacent nodes, start from the root node and recursively traverse the KD-Tree downwards, selecting the left or right subtree based on the positional relationship between the query point and the split point. After reaching a leaf node, calculate the distance between the query point and the leaf node and record the current closest node. Go back to the parent node and check whether there is a closer node by calculating the distance from the query point to the split plane. Repeat the above process until the entire tree is traversed.

[0018] Step S2.2: Adopt a sampling optimization strategy that considers a safe sampling area without static obstacles, and simultaneously use a multi-step expansion method to accelerate the approach to the target point. That is, determine the static obstacle-free area near the node, first calculate the minimum distance from the node to all obstacles, and use this as the radius to construct a circular convergence area for the node. When the random tree grows into this area, use a larger step size to expand the path, especially when sampling within the convergence area of ​​the target point, directly leading to the end point. The specific steps are as follows:

[0019] In order to solve the problem that the randomness of the RRT* algorithm in the sampling process is too strong, which causes the tree growth to deviate from the target point, thus affecting the search efficiency and convergence speed, a sampling optimization strategy is introduced to improve the goal orientation. For each node, the minimum distance from the node to all obstacles is calculated, which is called the node convergence radius r. i :

[0020]

[0021] Among them, N i Indicates the node position, O j Indicates the obstacle position. With the node position as the center, r i Draw a circle with radius , and get the convergence area S of the node i The random tree is expanded using multi-level steps, and the random tree uses a regular step size l in the non-node safety area.base ; After entering the node safety area, the step length is extended to 2·l base , accelerating the convergence process; when the sampling point is located in the convergence area of ​​the target point, the final path node is generated directly with the target point as the guide.

[0022] In addition, a sampling probability mechanism is introduced during sampling to further optimize the sampling process. A sampling bias probability P∈[0,1] is generated using a random number generator function. If P is less than a pre-set threshold (P<P threshold ), the sampling point is a randomly generated point. If the sampling point is located in the target point convergence area, the target point is used as the sampling point; otherwise, the randomly generated point is used as the sampling point. If P is greater than or equal to the threshold (P≥P threshold ), the target point is directly used as the sampling point, ensuring that the algorithm can directly expand toward the target point with a certain probability. This strategy not only ensures that the random tree can grow toward the target point in most cases, but also accelerates the path planning process by using a larger step size for expansion in areas with sparse obstacle distribution, effectively improving the overall performance of the algorithm.

[0023] Step S2.3: Adopt a node selection strategy based on adaptive target gravity bias to enhance the adaptability of the path. That is, after generating a sampling point, first connect the current node with the target node and construct the target gravity component in that direction. The new node generation method is changed from the original expansion of a certain step length toward the sampling point to expansion in the direction of the resultant force. During the resultant force calculation process, the weight parameters of the target gravity component and the sampling point gravity component are adaptively adjusted according to the number of surrounding obstacles in the scene. The specific steps are as follows:

[0024] First, the direction vector between the current node and the target node is calculated, and the target gravity component is constructed in this direction. After the sampling points are generated, the sampling point gravity component is calculated in the same way. The weight parameters of the target gravity component and the sampling point gravity component are adaptively adjusted according to the number of surrounding obstacles in the scene. If the surrounding obstacles are sparse, the target gravity component is given a greater weight. The resultant force vector is calculated:

[0025] F goal =GN i

[0026] F sample =SN i

[0027]

[0028] F total =α·F goal +(1-α)·F sample

[0029]

[0030] Among them, F goal Represents the target gravity component, which is the current node N i Direction vector to target point G; F sample Represents the gravitational component of the sampling point, which is the current node N i Direction vector to the sampling point S; F total is the resultant force vector, and the weight α of the target gravitational component in the resultant force is determined by the number of surrounding obstacles n in the scene. obs And the adjustment parameter k is calculated.

[0031] From the current node N i Extend the direction of the resultant force F total Expand a certain step length l to generate new nodes N new Unlike the traditional RRT method that expands a certain step size toward the sampling point, this strategy uses the target point position information to reduce the randomness of the RRT algorithm, making the algorithm converge faster.

[0032] Step S2.4: Based on the Bezier curve path smoothing strategy, calculate the angle between two adjacent steps, that is, the angle between three adjacent nodes, and determine whether there is an unsmooth corner at that location based on the calculated angle. Use the Bezier curve to readjust the position of the sharp turning point to improve the overall smoothness of the path. The specific steps are as follows:

[0033] For every three adjacent nodes P in the path i-1 、P i and P i+1 , calculate the angle θ between two adjacent steps i :

[0034]

[0035] If the angle θ i If the degree is less than 150, the node P i There is an uneven corner at the node, which needs to be smoothed. Select node P i-1 and P i+1 As the starting and ending points of the cubic Bezier curve, calculate the intermediate control points P1 and P2:

[0036]

[0037] Recalculate the node P using the parametric equation of the cubic Bezier curve i Position:

[0038] P' i =(1-t) 3 P i-1 +3(1-t) 2tP1+3(1-t)t 2 P2+t 3 P i-1

[0039] Among them, the parameter t∈[0,1] is the curve parameter, which is used to control the shape of the smoothed curve. In this method, the value is 0.5 to ensure that the new node is located in the middle of the starting point and the end point.

[0040] Use the adjusted node P' i Replace node P in the original path i , realize P i Smoothing at the points. Perform the above angle determination and smoothing process on all nodes on the path in sequence until all adjacent angles on the path are no less than 150 degrees, improving the overall smoothness of the path.

[0041] Furthermore, the trajectory prediction enhanced local obstacle avoidance algorithm Trajectory Mamba-DWA in step S3 includes the following steps:

[0042] Trajectory Mamba improves long-term trajectory prediction and dynamic feature screening by integrating a state-space model (SSM) with an attention mechanism. Its core steps include: utilizing SSM to redesign the self-attention mechanism in the encoder-decoder architecture to achieve linear time complexity. A joint polyline encoding strategy is used to better capture the association between static and dynamic context, improving prediction accuracy. To balance prediction accuracy and inference speed, the decoder of this model adopts a completely different structure from the encoder. Through cross-state-space attention, all target agents share the scene context. During the decoding phase, the SSM is able to interact with the shared scene representation to infer different trajectories in the prediction step. Finally, a winner-takes-all strategy is used to optimize only the gradient of the best predicted trajectory to avoid overfitting.

[0043] In order to enhance the foresight and safety of local obstacle avoidance, the Trajectory Mamba model is used to improve the DWA algorithm, resulting in the Trajectory Mamba-DWA algorithm. The specific improvement steps are as follows:

[0044] Step S3.1, trajectory prediction: The historical information of surrounding vehicles and pedestrians collected by cameras and lidar sensors is input into the Trajectory Mamba trajectory prediction model. The input information includes the type attribute (vehicle / pedestrian) and its historical trajectory, so as to predict the optimal trajectory of other vehicles and pedestrians in the future. Specifically:

[0045] The Trajectory Mamba model is used to predict the optimal future trajectory for other vehicles and pedestrians in the scene. Taking individual i as an example, assuming that the optimal trajectory of individual i is Where T is the time step interval;

[0046] Step S3.2, Kinematic Modeling: To describe the planar motion of the vehicle, a bicycle kinematic model is used. This model conforms to the motion behavior of a vehicle with a non-zero turning radius. Its state includes position, heading angle, and linear velocity. The control variables are linear velocity and steering angle. Specifically:

[0047] The ego vehicle's kinematics are modeled using a bicycle kinematic model with the rear axle center as the reference point. The bicycle model is a simplified vehicle model used to describe the nonholonomic constrained motion of a vehicle. Its basic assumption is that a four-wheeled vehicle can be simplified to a front and a rear wheel, with steering acting only on the front wheel. In this model, the ego vehicle's state variables can be expressed as S = [x, y, θ] T , where (x, y) represents the current position of the ego vehicle, θ is the overall heading angle of the ego vehicle (relative to the X axis of the world coordinate system), and the control input is u = [v δ] T , where v is the linear velocity and δ is the front wheel steering angle.

[0048] Step S3.3, dynamic window construction: Taking into full consideration the acceleration and speed limits of the ego vehicle and the obstacle limits in the environment, determine the feasible linear velocity and steering angle sampling space range of the ego vehicle. Specifically:

[0049] Construct the linear velocity and steering angle space that the vehicle can sample at the current moment. This sampling space (v, δ) is subject to three types of constraints:

[0050] (1) Speed ​​limit: Due to the hardware and structural conditions of the vehicle, the vehicle's linear speed and steering angle have limits. The sampling space is:

[0051] V m ={(v,δ)|v∈[v min ,v max ],δ∈[δ min ,δ max ]}

[0052] Among them, v min 、v max are the minimum and maximum linear velocities of the vehicle, δ min , δ max are the minimum and maximum steering angles of the vehicle, respectively.

[0053] (2) Acceleration limit: Due to the power limit of the vehicle's drive motor or engine, the linear acceleration and angular acceleration of the vehicle are both subject to boundary limits. Assuming that the maximum acceleration and deceleration are the same, the sampleable space considering the acceleration boundary limit is:

[0054] V a ={(v,δ)|v∈[v c -a vmax Δt,v c +a vmax Δt],δ∈[δ c -a δmax Δt,δ c +a δmax Δt]}

[0055] Among them, v c and δ c are the linear velocity and steering angle of the vehicle at the current moment, a vmax and a δmax are the maximum linear acceleration and maximum steering angle change rate of the vehicle respectively.

[0056] (3) Environmental obstacle constraints: Considering the surrounding obstacles, the vehicle's sampled velocity space must meet the constraint of not colliding with surrounding obstacles at a certain moment:

[0057]

[0058] where dis(tv, δ) represents the closest distance between the simulated trajectory and the obstacle under the current control input. In the absence of obstacles, dis(tv, δ) is a large constant. This constraint ensures that the ego vehicle can safely decelerate at the maximum deceleration, thus preventing collisions with obstacles.

[0059] Considering the above restrictions, the final sampling space of the vehicle linear velocity and steering angle is:

[0060] V s =V m ∩V a ∩V o

[0061] Step S3.4, velocity sampling and trajectory simulation: Set the sampling resolution to sample the vehicle's linear velocity and steering angle to obtain all possible control input pairs. Perform a short-time window forward simulation on each control input pair, iteratively calculate the position and posture changes according to the kinematic model, and discretize the trajectory into several time steps to form a candidate trajectory sequence. Specifically:

[0062] After determining the sampling space, the DWA algorithm uniformly samples the space at a certain sampling interval to obtain several sets of linear velocity and steering angle inputs (v, δ)∈D, where D is the input set. Then, for each set of control inputs, a trajectory simulation is performed for N time steps based on the kinematic model. Assuming that the linear velocity and steering angle of the ego vehicle remain unchanged within a time step in a discrete time frame, the update formulas for each state variable are as follows:

[0063] x t+1 =x t +v·cos(θ t )·Δt

[0064] y t+1 =y t +v·sin(θ t )·Δt

[0065]

[0066] Among them, L is the wheelbase of the vehicle; the final simulation trajectory point sequence is

[0067] Step S3.5, Time-Series Collision Detection and Trajectory Evaluation: The optimal trajectories of other vehicles and pedestrians predicted in step S3.1 are time-synchronized with the candidate trajectories of the ego vehicle. The predicted position of the ego vehicle is detected to see if it collides with the predicted positions of other vehicles and pedestrians. The candidate trajectory sequence is then screened. Finally, the remaining multiple trajectory groups are evaluated and the optimal trajectory of the ego vehicle is selected. The speed corresponding to the optimal trajectory is used as the driving speed of the ego vehicle. Specifically:

[0068] After obtaining the candidate trajectory sequence, each ego vehicle trajectory is subjected to temporal collision detection with the predicted trajectories of other vehicles and pedestrians. All trajectories are first aligned in time. At each time step, the predicted position of the ego vehicle is checked for collision with the predicted positions of other vehicles and pedestrians (i.e., the Euclidean distance is less than a set threshold). If the ego vehicle trajectory overlaps with any vehicle or pedestrian trajectory, the trajectory is considered unsafe and removed from the candidate trajectory sequence.

[0069] Finally, the remaining trajectories are evaluated with multiple objectives, and the evaluation function is:

[0070] G(v,δ)=w1Head(v,δ)+w2Dist obs (v,δ)+w3Dist goal (v,δ)

[0071] Where Head(v,δ)=π-θ g is the target azimuth evaluation function, θ gDist is the angle between the trajectory end position direction and the target point at the current sampling speed; obs (v,δ) is the obstacle distance evaluation function, which represents the minimum Euclidean distance between the current simulation trajectory and the obstacle; Dist goal (v, δ) is the target distance evaluation function, which represents the Manhattan distance between the ego vehicle and the target point. The pair (v, δ) with the highest score is selected as the control command for this planning cycle.

[0072] Furthermore, the specific steps of the dynamic path replanning mechanism in step S4 are as follows:

[0073] Step S4.1: Path Assessment: After the vehicle completes local obstacle avoidance, the collision risk and expected travel time for returning to the next node on the original planned path are assessed based on the current vehicle state (position, speed, etc.) and surrounding environment perception data (static and dynamic obstacle density, etc.). If static obstacles are densely packed during the return process, or dynamic obstacles are close to the return path, the collision risk is high; if the distance between the current position and the next node on the original path is large, the expected travel time is long.

[0074] Step S4.2: Replanning triggering condition: If the path evaluation result shows that returning to the next node of the original planned path does not meet the collision risk and expected travel time constraints, path replanning is triggered.

[0075] Step S4.3: Path replanning: Use the improved RRT global planning algorithm proposed in this invention to plan a new path directly from the current position to the target point. The subsequent path is smoothed taking into account the vehicle's current direction of travel to reduce the dangers of sharp turns and sudden acceleration and deceleration.

[0076] Step S4.4: Update the path plan: Based on the optimized subsequent path planning data, including new path nodes and path directions, the updated next movement is sent to the vehicle, and its movement speed and steering angle are adjusted.

[0077] The beneficial effects of the present invention are as follows: by integrating the improved RRT algorithm with the Trajectory Mamba-DWA algorithm, the present invention significantly improves the performance of vehicle path planning in dynamic and complex environments. The beneficial effects are mainly reflected in the following four aspects:

[0078] (1) Improvement of path search efficiency and optimality: This paper adopts a KD-Tree-based search optimization strategy, which significantly shortens the search time for neighboring nodes. By introducing a sampling optimization strategy that considers safe areas and a multi-level step-size expansion mechanism, the probability of the path deviating from the target during planning is effectively reduced, and the path convergence speed is accelerated. Experimental results show that this improves the speed of global path planning. Experimental results show that the path planning time is reduced by more than 25%, and the path length is optimized by more than 10%, significantly improving the planning efficiency and optimality of the global path.

[0079] (2) Improved trajectory prediction accuracy: This invention uses the Trajectory Mamba algorithm, which combines a dynamic time series prediction model with environmental perception technology to accurately predict the future motion trajectories of surrounding vehicles and pedestrians. Compared with traditional prediction methods, its prediction results are more accurate and the error rate is significantly reduced, making obstacle avoidance planning more forward-looking and reliable.

[0080] (3) Safety and foresight of dynamic obstacle avoidance behavior: The present invention uses the Trajectory Mamba algorithm to improve the dynamic window method. After calculating the future trajectory of the ego vehicle, a temporal collision detection link is added to synchronize the predicted future motion trajectories of other vehicles and pedestrians with the calculated trajectory of the ego vehicle. The temporal collision detection further constrains the driving speed of the ego vehicle, greatly enhancing the safety and foresight of local obstacle avoidance.

[0081] (4) Optimization of dynamic path switching: This invention introduces an adaptive path replanning mechanism, which enables the vehicle to quickly determine whether to return to the original path or plan a new path based on environmental changes after an obstacle avoidance maneuver. This mechanism allows the vehicle to dynamically adapt to the environment, avoiding delays or risks caused by incorrect path selection and improving overall driving efficiency. BRIEF DESCRIPTION OF THE DRAWINGS

[0082] Figure 1 Schematic diagram of a simplified vehicle model based on a bicycle kinematic model of the present invention.

[0083] Figure 2 Schematic diagram of the Trajectory Mamba-DWA local obstacle avoidance algorithm combined with trajectory prediction of the present invention.

[0084] Figure 3 This is the experimental result of the path planning of the improved RRT algorithm of the present invention in scenario 1.

[0085] Figure 4 This is the experimental result of the path planning of the improved RRT algorithm of the present invention in scenario 2.

[0086] Figure 5 Flow chart of the method of the present invention. DETAILED DESCRIPTION

[0087] The following description further illustrates the principles and features of the present invention with reference to the accompanying drawings. The following detailed description is intended only to illustrate the present invention and is not intended to limit the scope of the present invention. To better illustrate the following embodiments, some components in the drawings may be omitted, enlarged, or reduced, and do not represent the actual dimensions of the products. It is understandable to those skilled in the art that some well-known structures and their descriptions may be omitted from the drawings.

[0088] like Figure 5 As shown, the present invention provides a vehicle path planning method that integrates the improved RRT and Trajectory Mamba-DWA algorithms, including the following contents:

[0089] Step S1: Load a two-dimensional global map containing known static obstacles (such as lane edges, buildings, etc.) based on the actual map, detect the collision area of ​​the static obstacles and mark them;

[0090] Step S2: Based on the global map and the vehicle's starting and destination points, a globally optimal path is quickly planned for the vehicle using an improved Rapid Exploration Random Tree (RRT) algorithm, combined with a KD-Tree-based search optimization strategy, a sampling optimization strategy that considers safe sampling areas, a node selection strategy based on adaptive target gravity bias, and a Bezier curve-based path smoothing strategy. The vehicle then drives steadily toward the next node based on the planned globally optimal path.

[0091] Step S3: To improve obstacle avoidance during driving, the Trajectory Mamba trajectory prediction model is integrated into the DWA algorithm to predict the future movement trends of surrounding vehicles and pedestrians. Time-series collision detection is used to further constrain the safe driving speed of the ego vehicle, guiding the vehicle to avoid potential collision risks, thereby enhancing local obstacle avoidance performance.

[0092] Step S4: After the vehicle performs the obstacle avoidance operation, the subsequent path is evaluated based on the dynamic path replanning mechanism, and two options are calculated: one is to immediately return to the original planned path; the other is to plan a new path directly from the current position after obstacle avoidance to the target point. By comparing the performance indicators of the two options such as path length, estimated travel time, and obstacle avoidance effect, the optimal option is selected as the subsequent path; steps S3 and S4 are repeated until the vehicle reaches the target point.

[0093] Example 1: Global path planning based on improved RRT algorithm

[0094] In this embodiment, an improved Rapidly Exploring Random Tree (RRT) algorithm is used for global path planning. First, a global map containing known static obstacles (such as lane edges and buildings) is loaded based on the actual map, and the collision areas of these static obstacles are detected and marked. Next, an improved RRT algorithm is combined with a KD-Tree-based search optimization strategy, a sampling optimization strategy that considers safe sampling areas, a node selection strategy based on adaptive target gravity bias, and a Bezier curve-based path smoothing strategy to plan an optimal global path for each vehicle.

[0095] ①Search optimization strategy——KD-Tree

[0096] When performing global vehicle path planning, the three-dimensional map is first simplified into a two-dimensional problem, that is, the height information (x direction) is ignored and only the plane coordinates (y, z) are considered. A KD-Tree is constructed recursively to achieve efficient node organization and fast search. In each layer of recursion, the y direction or z direction is alternately selected as the split dimension. On the current split dimension, the median of all nodes is calculated as the split point. For example, in the y direction, all nodes are sorted by y coordinates, and the median node is selected as the split point to divide the node set into two parts: the left subtree contains nodes with y coordinates less than the median, and the right subtree contains nodes with y coordinates greater than the median. The above splitting steps are then recursively performed on the left and right subtrees respectively until each sub-area contains only one node or the preset recursion depth is reached.

[0097] When searching for neighboring nodes, the algorithm starts at the root of the KD-Tree and recursively traverses the tree downward, selecting the left or right subtree based on the positional relationship between the query point and the split point. Upon reaching a leaf node, the algorithm calculates the distance between the query point and the leaf node and records the current closest node. The algorithm then backtracks to the parent node and checks for closer nodes by calculating the distance from the query point to the split plane. This process repeats until the entire tree has been traversed. By constructing a KD-Tree, the time required to search for neighboring nodes is significantly reduced, improving path planning efficiency.

[0098] ② Sampling optimization strategy - sampling point bias strategy

[0099] To address the problem that the RRT* algorithm's sampling process is too random, causing the tree growth to deviate from the target point, thereby affecting search efficiency and convergence speed, this embodiment introduces a sampling bias strategy to improve goal orientation. For each node, the minimum distance from the node to all obstacles is calculated, which is called the node convergence radius:

[0100]

[0101] Among them, N i Indicates the node position, O jIndicates the obstacle position. With the node position as the center, r i Draw a circle with a radius of , and get the static obstacle-free safe area for the node. During the random tree expansion process, the step size is dynamically adjusted according to the environment where the current node is located: if the current node is outside the safe area of ​​the sampling point, the normal step size l is used. base Exploration is performed to ensure wide coverage; when entering the safe area of ​​the node, the step size is expanded to 2·l base , accelerating the convergence to the target point. In particular, if the sampling point falls within the convergence region of the target point, the final path node is directly generated using the target point as a guide, thereby significantly improving the success rate and efficiency of path planning.

[0102] In addition, a random number generator function is used to generate a sampling bias probability P∈[0,1]. If P is less than a pre-set threshold (P<P threshold ), the sampling point is a randomly generated point. If the sampling point is located in the target point convergence area, the target point is used as the sampling point; otherwise, the randomly generated point is used as the sampling point. If P is greater than or equal to the threshold (P≥P threshold ), the target point is directly used as the sampling point. This strategy ensures that the random tree grows towards the target point with a certain probability and expands with a larger step size in areas with sparse obstacles, further accelerating the path planning process.

[0103] ③Node selection strategy - adaptive target gravity bias strategy

[0104] Before generating a sampling point, you must first connect the current node and the target node, and construct the target gravity component along this direction; after the sampling point is generated, its gravity component is calculated in the same way. To adapt to different scenarios, the weight parameters of the target gravity component and the sampling point gravity component are adaptively adjusted according to the number of surrounding obstacles in the scene. If the surrounding obstacles are sparse, the weight of the target gravity component is increased to enhance the target guidance of the path; conversely, when the obstacles are dense, the weight is reduced to prevent the path from guiding towards obstacles. The calculation formula for the resultant force vector is:

[0105] F goal =GN i

[0106] F sample =SN i

[0107]

[0108] F total =α·F goal +(1-α)·F sample

[0109] Among them, Fgoal Represents the target gravity component, which is the current node N i Direction vector to target point G; F sample Represents the gravitational component of the sampling point, which is the current node N i The direction vector to the sampling point S; α is the weight parameter of the target gravity component, which is calculated based on the number of surrounding obstacles n in the scene. obs And the adjustment parameter k is calculated.

[0110] From the current node N i Along the direction of the resultant force F total Expand a certain step length l to generate new nodes N new :

[0111]

[0112] Different from the traditional RRT method which only expands a certain step size toward the sampling point, this method uses the location information of the target point to reduce the randomness of the RRT algorithm, thereby improving the convergence speed of the algorithm and enhancing the adaptability of the path.

[0113] ④Path smoothing strategy - Bezier curve

[0114] After generating the complete path, for every three adjacent nodes P in the path i-1 、P i and P i+1 , calculate the angle θ between two adjacent steps i :

[0115]

[0116] If the angle θ i If the degree is less than 150, the node P i There is an uneven corner at the node, which needs to be smoothed. Select node P i-1 and P i+1 As the starting and ending points of the cubic Bezier curve, calculate the intermediate control points P1 and P2:

[0117]

[0118] Recalculate the node P using the parametric equation of the cubic Bezier curve i Position:

[0119] P' i =(1-t) 3 P i-1 +3(1-t) 2 tP1+3(1-t)t 2 P2+t 3 P i-1

[0120] Among them, the parameter t∈[0,1] is the curve parameter, which is used to control the shape of the smoothed curve. In this method, the value is 0.5 to ensure that the new node is located in the middle of the starting point and the end point.

[0121] The adjusted node P' i Replace node P in the original path i , realize the path point P i The above angle determination and smoothing process are performed on all nodes on the path in sequence until all angles on the path are not less than 150 degrees, thereby improving the overall smoothness of the path.

[0122] In order to evaluate the performance of the proposed method, verification was performed based on the two indicators of "path length" and "planning time". Using the ablation experiment method, 500 repeated planning experiments were conducted on the two scenarios respectively, and the average path length and average planning time were calculated. At the same time, the experimental results of the two optimization algorithms RRT and Informed-RRT of the traditional RRT algorithm were used for comparison. The results show that the improved RRT method proposed in this invention can achieve a speed increase of more than 25% compared with the traditional RRT, and the planning path length can be shortened by more than 10%. The schematic diagram of the experimental results is shown in the figure below. Figure 3 shown.

[0123] Example 2: Trajectory Mamba-DWA, a local obstacle avoidance algorithm combined with trajectory prediction

[0124] In this embodiment, an improved DWA algorithm combined with the Trajectory Mamba trajectory prediction model is used to achieve local obstacle avoidance. This algorithm uses the trajectory prediction model to predict the future motion trends of surrounding vehicles and pedestrians. This algorithm then performs time-series collision detection on the ego vehicle's trajectory simulated by the DWA algorithm, further constraining the ego vehicle's safe driving speed and thus achieving proactive obstacle avoidance control.

[0125] ① Trajectory prediction

[0126] Trajectory Mamba improves long-term trajectory prediction and dynamic feature screening by integrating a state-space model (SSM) with an attention mechanism. Its core steps include: leveraging SSM to redesign the self-attention mechanism in the encoder-decoder architecture to achieve linear time complexity. A joint polyline encoding strategy is used to better capture the associations between static and dynamic context, improving prediction accuracy. To balance prediction accuracy and inference speed, the model employs a decoder architecture that is fundamentally different from the encoder. Through cross-state-space attention, all target agents share scene context, allowing the SSM to interact with the shared scene representation during decoding, thereby inferring different trajectories during the prediction step. Finally, a winner-takes-all strategy is used to optimize the gradient of the best predicted trajectory to avoid overfitting.

[0127] To enhance the foresight of local obstacle avoidance, the Trajectory Mamba model is integrated to improve the DWA algorithm. The specific improvement steps are as follows:

[0128] First, the Trajectory Mamba model is used to predict the optimal future trajectory for other vehicles and pedestrians in the scene. Taking individual i as an example, assuming that the optimal trajectory of individual i is Where T is the time step interval;

[0129] Secondly, the bicycle kinematic model with the rear axle center as the reference point is used to model the kinematics of the vehicle. This model is a simplified vehicle model used to describe the non-holonomic constrained motion of the vehicle. Its assumption is that the four-wheeled vehicle can be simplified to one front wheel and one rear wheel, and the steering only acts on the front wheel, such as Figure 1 In this model, the state variables of the vehicle can be expressed as S = [x yθ] T , where (x, y) represents the current position of the vehicle, θ is the overall heading angle of the vehicle (relative to the X axis of the world coordinate system), and the control input is u = [vδ] T , where v is the linear velocity and δ is the front wheel steering angle. The sampled linear velocity and steering angle space of the vehicle at the current moment is then constructed. The sampling space (v, δ) is subject to three types of constraints:

[0130] (4) Speed ​​limit: Due to the hardware and structural conditions of the vehicle, the linear speed and steering angle of the vehicle have limits. The sampling space is:

[0131] V m ={(v,δ)|v∈[v min ,v max ],δ∈[δ min ,δ max ]}

[0132] Among them, v min 、v maxare the minimum and maximum linear velocities of the vehicle, δ min , δ max are the minimum and maximum steering angles of the vehicle, respectively.

[0133] (5) Acceleration limit: Due to the power limit of the vehicle's drive motor or engine, the linear acceleration and angular acceleration of the vehicle are both subject to boundary limits. Assuming that the maximum acceleration and deceleration are the same, the sampleable space considering the acceleration boundary limit is:

[0134] V a ={(v,δ)|v∈[v c -a vmax Δt,v c +a vmax Δt],δ∈[δ c -a δmax Δt,δ c +a δmax Δt]}

[0135] Among them, v c and δ c are the linear velocity and steering angle of the vehicle at the current moment, a vmax and a δmax are the maximum linear acceleration and maximum steering angle change rate of the vehicle respectively.

[0136] (6) Environmental obstacle constraints: Considering the surrounding obstacles, the vehicle's sampled velocity space must meet the constraint of not colliding with surrounding obstacles at a certain moment:

[0137]

[0138] Where dis(tv, δ) represents the closest distance between the simulated trajectory and an obstacle under the current control input. In the absence of obstacles, dis(tv, δ) is a large constant. This constraint limits the ego vehicle to the maximum deceleration possible to safely decelerate and prevent collisions with obstacles.

[0139] Taking all the constraints into consideration, the final sampling space of the vehicle's linear velocity and steering angle is:

[0140] V s =V m ∩V a ∩V o

[0141] After determining the sampling space, the DWA algorithm uniformly samples the space at a certain sampling interval to obtain several sets of linear velocity and steering angle inputs (v, δ)∈D, where D is the input set. For each set of control inputs, a trajectory simulation is performed for N time steps based on the kinematic model. Assuming that the linear velocity and steering angle of the vehicle remain unchanged within a discrete time step, the update formulas for each state variable are as follows:

[0142] x t+1 =x t +v·cos(θ t )·Δt

[0143] y t+1 =y t +v·sin(θ t )·Δt

[0144]

[0145] Among them, L is the wheelbase of the vehicle, and the final simulation trajectory point sequence is

[0146] After obtaining the candidate trajectory sequence, each ego vehicle trajectory is subjected to temporal collision detection with the predicted trajectories of other vehicles and pedestrians. All trajectories are first aligned in time. At each time step, the predicted position of the ego vehicle is checked for collision with the predicted positions of other vehicles and pedestrians (i.e., the Euclidean distance is less than a set threshold). If the ego vehicle trajectory overlaps with any vehicle or pedestrian trajectory, the trajectory is considered unsafe and removed from the candidate trajectory sequence.

[0147] Finally, the remaining trajectories are evaluated with multiple objectives, and the evaluation function is:

[0148] G(v,δ)=w1Head(v,δ)+w2Dist obs (v,δ)+w3Dist goal (v,δ)

[0149] Where Head(v,δ)=π-θ g is the target azimuth evaluation function, θ g Dist is the angle between the trajectory end position direction and the target point at the current sampling speed; obs (v,δ) is the obstacle distance evaluation function, which represents the minimum Euclidean distance between the current simulation trajectory and the obstacle; Dist goal (v, δ) is the target distance evaluation function, which represents the Manhattan distance between the ego vehicle and the target point. The pair (v, δ) with the highest score is selected as the control command for this planning cycle.

[0150] Example 3: Dynamic Path Replanning Mechanism

[0151] This embodiment employs a dynamic re-route mechanism to quickly and efficiently re-plan the vehicle's path after performing local obstacle avoidance maneuvers to adapt to dynamically changing environments. This mechanism dynamically selects the optimal path by evaluating the vehicle's current state and surroundings, ensuring efficient and safe driving in complex and dynamic environments.

[0152] ①Path assessment

[0153] After the vehicle completes a local obstacle avoidance maneuver, it obtains real-time data on the current vehicle state (position, speed, etc.) and surrounding environment perception data (static and dynamic obstacle density, etc.). Through sensor fusion technology, it integrates data from sensors such as cameras and lidar to ensure comprehensive perception of the surrounding environment. Based on the current vehicle state and environmental perception data, it assesses the collision risk R and expected travel time T for returning to the next node on the original planned path. The specific evaluation formula is as follows:

[0154] R=f(ρ s ,ρ d ,d)

[0155] T=g(d,v)

[0156] Where d is the distance from the current node to the next node, f and g are the evaluation functions of collision risk and expected travel time, respectively. The specific form is designed according to the actual scenario.

[0157] ②Replanning trigger conditions

[0158] If the path evaluation result shows that returning to the next node of the original planned path does not meet the collision risk and expected travel time constraints, path replanning is triggered. The specific conditions are:

[0159] R>R th

[0160] T>T th

[0161] Among them, R th and T th are the thresholds for collision risk and expected travel time, respectively.

[0162] ③ Path replanning

[0163] The improved RRT global planning algorithm proposed in this paper plans a new path directly from the current position to the target point. The subsequent path is smoothed, taking into account the vehicle's current direction of travel, to reduce the dangers of sharp turns and sudden acceleration and deceleration. Starting from the current position, the improved RRT algorithm is used to quickly generate a feasible path to the target point. This path is then smoothed to ensure path continuity and drivability.

[0164] ④Update path plan:

[0165] Based on the optimized subsequent path planning data, including new path nodes and directions, updated motion control instructions are sent to the vehicle to adjust speed and steering angle. Specifically, the new path node and direction information is converted into actionable motion instructions for the vehicle; the vehicle control system then adjusts the vehicle's speed and steering angle to ensure it follows the new path. During driving, the system monitors vehicle status and environmental changes in real time, dynamically adjusting path planning and obstacle avoidance strategies as needed to ensure safe and efficient driving.

Claims

1. A vehicle path planning method integrating improved RRT and Trajectory Mamba-DWA algorithm, characterized in that: The vehicle path planning method comprises the following steps: Step S1: Load a global map containing known static obstacles based on the actual map, detect the collision area of ​​the static obstacles and mark them; Step S2: Based on the global map and the vehicle's starting and destination points, a globally optimal path is quickly planned for the vehicle using an improved Rapid Exploration Random Tree (RRT) algorithm, combined with a KD-Tree-based search optimization strategy, a sampling optimization strategy that considers safe sampling areas, a node selection strategy based on adaptive target gravity bias, and a Bezier curve-based path smoothing strategy. The vehicle then drives steadily toward the next node based on the planned globally optimal path. Step S3: To improve obstacle avoidance during driving, the Trajectory Mamba trajectory prediction model is integrated into the DWA algorithm to predict the future movement trends of surrounding vehicles and pedestrians. Time-series collision detection is used to further constrain the safe driving speed of the ego vehicle, guiding the vehicle to avoid potential collision risks, thereby enhancing local obstacle avoidance performance. Step S4: After the ego vehicle performs the obstacle avoidance maneuver, the adaptively switched dynamic path replanning mechanism is used to evaluate the subsequent path. The estimated travel time and collision risk of the two options are compared: immediately returning to the original planned path and following a new path planned from the current position to the target point, and the optimal option is selected as the subsequent path; steps S3 and S4 are repeated until the ego vehicle reaches the target point.

2. The vehicle path planning method integrating the improved RRT and Trajectory Mamba-DWA algorithms according to claim 1 is characterized in that: The global path planning based on the improved RRT algorithm in step S2 includes the following steps: Step S2.1: Use a fast neighboring node search strategy based on KD-Tree to speed up the node search process and reduce the node search time; Step S2.2: Adopt a sampling optimization strategy that considers safe areas to improve sampling safety, and simultaneously use a multi-step expansion approach to accelerate the approach to the target point. Specifically, determine the static obstacle-free area near the node, first calculate the minimum distance from the node to all obstacles, and use this as the radius to construct a circular convergence area for the node. When the random tree grows into this area, use a larger step size to expand the path, especially when sampling within the convergence area of ​​the target point, directly leading to the end point. Step S2.3: Adopt a node selection strategy based on adaptive target gravity to enhance the adaptability of the path. That is, after generating a sampling point, first connect the current node with the target node and construct the target gravity component in that direction. The new node generation method is changed from the original expansion of a certain step length toward the sampling point to expansion in the direction of the resultant force. During the calculation of the resultant force, the weight parameters of the target gravity component and the sampling point gravity component are adaptively adjusted according to the number of surrounding obstacles in the scene. Step S2.4: Path smoothing strategy based on cubic Bezier curves: Calculate the angle between two adjacent steps, that is, the angle between three adjacent nodes, one by one. Based on the calculated angle value, determine whether there is an uneven corner at that location. Use the Bezier curve to readjust the position of the sharp turning point to improve the overall smoothness of the path.

3. The vehicle path planning method integrating the improved RRT and Trajectory Mamba-DWA algorithms according to claim 1 is characterized in that: The improved DWA local obstacle avoidance algorithm combined with the Trajectory Mamba trajectory prediction model in step S3 includes the following steps: Step S3.1, trajectory prediction: The historical information of surrounding vehicles and pedestrians collected by cameras and lidar sensors is input into the Trajectory Mamba trajectory prediction model. The input information includes type attributes and their historical trajectories, so as to predict the optimal trajectories of other vehicles and pedestrians in the future. Step S3.2, kinematic modeling: To describe the planar motion of the vehicle, a bicycle kinematic model is used. This model conforms to the motion behavior of a vehicle with a non-zero turning radius. Its state includes position, heading angle, and linear velocity, and the control variables are linear velocity and steering angle. Step S3.3, dynamic window construction: fully consider the acceleration limit, speed limit and obstacle limit of the ego vehicle, and determine the feasible linear speed and steering angle sampling space range of the ego vehicle; Step S3.4, velocity sampling and trajectory simulation: Set the sampling resolution to sample the vehicle's linear velocity and steering angle to obtain all possible control input pairs. Perform a short-time window forward simulation on each control input pair, iteratively calculate the position and posture changes according to the kinematic model, and discretize the trajectory into several time steps to form a candidate trajectory sequence. Step S3.5, Time-series collision detection and trajectory evaluation: The optimal trajectories of other vehicles and pedestrians predicted in step S3.1 are time-synchronized with the candidate trajectories of the ego vehicle. The predicted position of the ego vehicle is detected to see if it collides with the predicted positions of other vehicles and pedestrians, and the candidate trajectory sequence is screened. Finally, the remaining multiple trajectory groups are evaluated and the optimal trajectory of the ego vehicle is selected. The speed corresponding to the optimal trajectory is used as the driving speed of the ego vehicle.

4. The vehicle path planning method integrating the improved RRT and Trajectory Mamba-DWA algorithms according to claim 1 is characterized in that: The dynamic path replanning mechanism in step S4 includes the following steps: Step S4.1: Path Assessment: After the vehicle completes local obstacle avoidance, the collision risk and expected travel time for returning to the next node on the original planned path are assessed based on the current vehicle state and surrounding environment perception data. If the return path is densely populated with static obstacles or surrounded by dynamic obstacles, the collision risk will increase. If the distance between the current position and the next node on the original path is large, the expected travel time will be extended. Step S4.2: Replanning trigger condition: If the path evaluation result shows that returning to the next node of the original planned path does not meet the collision risk and expected travel time constraints, path replanning is triggered; Step S4.3: Path replanning: Use the improved RRT algorithm to plan a new path directly from the current position to the target point, and smooth the subsequent path considering the vehicle's current direction of travel to reduce the dangers of sharp turns and sudden acceleration and deceleration; Step S4.4: Update path plan: Based on the optimized subsequent path planning data, including new path nodes and path direction information, send the updated next movement to the vehicle to adjust its movement speed and steering angle.

Images