Double sampling optimization-based automatic driving vehicle track planning method for fork road scene

Abstract

The invention discloses a trajectory planning method based on double sampling optimization, and belongs to the technical field of automatic driving. The method aims at solving the technical problems that in a multi-branch unstructured scene, a traditional planning method is prone to falling into local optimum, planning fails or calculation time is too long, and system reliability is affected. According to the scheme, sampling optimization is carried out on a guide path and a smooth path at the same time, an improved WRRT *-S algorithm is adopted for searching, and an optimal guide path is selected in combination with a risk potential field, path distance cost and steering cost; generating a smooth path candidate cluster by using a quintic Bezier curve, and selecting an optimal smooth path through multi-objective function optimization of path smoothness, economy, collision penalty and the like; and discretizing a speed planning problem into a nonlinear planning problem for solving by taking the maximum speed as a constraint and utilizing a Gaussian pseudo-spectral method, so as to realize collaborative optimization of the path and the speed. According to the invention, the vehicle can plan a safe, stable and efficient track in a complex scene.

Description

Technical Field

[0001] This invention relates to the field of autonomous driving technology, and in particular to an autonomous vehicle trajectory planning method and system based on dual sampling optimization, which is especially suitable for path planning and speed coordination optimization in unstructured scenarios with multiple intersections, such as parking lots. Background Technology

[0002] In the autonomous driving technology system, path planning is one of the core modules, playing a crucial role in ensuring the safety and efficiency of vehicles. However, in unstructured scenarios such as parking lots with multi-branch structures and a lack of clear road markings and rules, trajectory planning faces severe challenges. The randomness of obstacle distribution and the high uncertainty of path selection make traditional path planning methods prone to getting trapped in local optima or failing due to excessive computation time, thus affecting the reliability and safety of the planning results.

[0003] Mainstream global path planning methods include graph search and sampling-based methods.

[0004] Graph search-based methods, represented by the A* algorithm, can efficiently find the shortest path in static environments, but they typically do not directly satisfy vehicle kinematic constraints. While improved algorithms such as hybrid A* attempt to integrate kinematic constraints, setting appropriate guide points to prevent the vehicle from getting trapped in local optima remains a key issue. Furthermore, many improved algorithms lack consideration for the continuity of path curvature.

[0005] Sampling-based methods, represented by Random Tree Quick Search (RRT) and its variants (such as RRT*), are suitable for dynamic environments and complex obstacle scenarios, and can efficiently explore free space. However, due to the randomness of the algorithm, the search efficiency is not high, and the generated paths are often tortuous, with potential discontinuities in direction. Although RRT* optimizes path cost, the generated path trajectories are still difficult to directly use for vehicle motion planning problems, requiring secondary planning.

[0006] To ensure that the path meets vehicle kinematic constraints, researchers often use geometric methods such as polynomial curves, Bézier curves, or B-spline curves to smooth the original path. However, existing methods, such as using B-spline interpolation on RRT* paths, still carry the risk of collision after interpolation and do not adequately consider curvature constraints.

[0007] In unstructured scenarios such as parking lots with dense branching structures and narrow passages, the accuracy, consistency, and feasibility of trajectory planning algorithms are extremely important when vehicles switch paths and adjust their attitudes. Existing methods, when dealing with such multi-branching scenarios, struggle to effectively balance trajectory smoothness, vehicle dynamics constraints, and obstacle avoidance feasibility while ensuring global path connectivity, and still have certain limitations that require further research and optimization.

[0008] This disclosure addresses, but is not limited to, the many factors mentioned above. Summary of the Invention

[0009] The purpose of this invention is to provide a trajectory planning method for autonomous vehicles based on dual sampling optimization, to address the technical problem that traditional path planning methods are prone to getting trapped in local optima, excessively long computation times, or planning failures due to the complexity of unstructured scenarios such as parking lots with multiple branching paths, leading to reduced system reliability. This invention aims to plan a final trajectory that can effectively search for multiple feasible paths in complex scenarios and ensure smooth, efficient, and safe vehicle operation.

[0010] To achieve the above objectives, this invention proposes a trajectory planning method for autonomous vehicles based on dual sampling optimization. Its core technical solution is to apply a sampling optimization strategy to both the guided path planning and local interpolation smoothing path planning stages, thereby forming a dual optimization mechanism. The specific technical solution adopted by this invention is as follows:

[0011] Define the vehicle trajectory planning problem, and establish the vehicle kinematics model and obstacle risk potential field map:

[0012] 1. For the multi-branching scenario in the parking lot shown, define the trajectory planning problem for autonomous vehicles.

[0013] 2. Based on the Ackermann front wheel steering principle, a vehicle kinematic model is established, and the vehicle's motion state is described as a vector containing the vehicle's lateral position, longitudinal position, and heading angle. The center of the vehicle's rear axle is selected as the state observation point.

[0014] 3. The occupied map is constructed as an obstacle risk potential field map. While avoiding collisions between vehicles and obstacles, the risk value of the trajectory among multiple path trajectories is used as a safety evaluation index. This controls the algorithm to select the trajectory with the lower risk value, ensuring that the vehicle stays away from obstacles and drives safely.

[0015] Guiding path planning and optimization (first-level sampling optimization):

[0016] 1. Algorithm Basics and Improvements: The Wide Search Rapidly-exploring Random Tree-Smart (WRRT*-S) algorithm is used for guided path planning. This algorithm uses the RRT* algorithm as its basic structure and improves the path expansion and evaluation mechanisms by introducing a path diversity preservation mechanism and a risk potential field guidance strategy.

[0017] 2. Search Mechanism: To address the issues of limited path search direction and single planned path in multi-branch scenarios, the WRRT*-S algorithm can perform breadth-first search and record all found feasible paths to form a candidate set of guiding paths.

[0018] Intelligent sampling: Sampling points are generated based on the field strength distribution of the obstacle risk potential field, following a Gaussian distribution and preferentially generated in the empty area with low risk value to improve obstacle avoidance efficiency.

[0019] Path optimization: The Path Optimization function is used to directly connect nodes without gaps or obstacles in the path, so as to effectively reduce redundant inflection points in the path.

[0020] 3. Multidimensional Cost Evaluation Function: A weighted cost function is proposed. The function comprehensively evaluates the candidate guidance paths. It integrates three key metrics and normalizes the cost design.

[0021] Risk field strength assessment This approach uses a weighted average risk and maximum risk value based on path points to replace the traditional collision cost, ensuring path safety and avoiding collisions. The risk potential field is described using a second-order higher-order Gaussian function.

[0022] Path distance cost The ratio of the current path to the longest path among all paths, used to evaluate path length.

[0023] Path turning cost Defined as the normalized average of all steering angle changes in the path, used to measure the smoothness of the path.

[0024] 4. Optimal Selection: Through weighted comprehensive cost function evaluation, the optimal guiding path that balances motion smoothness and spatial rationality is finally selected.

[0025] Planning and optimization of locally smooth trajectories (second resampling optimization):

[0026] 1. Smoothing primitive: A quintic Bézier curve is selected as the path construction primitive, which satisfies G 2 Continuity provides greater control over curve shape and applies curvature constraints at both ends of the path.

[0027] 2. Interpolation and Endpoint Constraints: Interpolation is performed on the optimal guiding path, while preserving the start and end poses of the guiding path. = [x0, y0, θ0, ρ0] and = [x f y f θ f , ρ f ].

[0028] 3. Generation of smooth path candidate clusters: To ensure smoothness and vehicle driving safety, the initial and final velocity vectors are adjusted. , and acceleration vector , Normalized adjustment range and Sampling was performed by calculating six control points. The location of the control points is used to generate a smooth path candidate cluster. The setting of control points takes into account the influence of path curvature on the acceleration vector, making the settings more flexible.

[0029] 4. Multi-objective weighted cost function: Multi-objective function optimization is performed on the candidate cluster of smooth paths to ensure that the paths conform to vehicle kinematic constraints.

[0030] Smoothness Cost : Evaluate the ratio of the maximum curvature, the average rate of change of curvature to the maximum rate of change of curvature.

[0031] Economic cost : Evaluate the ratio of the planned path length to the straight-line distance between the start and end points.

[0032] Collision penalty cost The OBB enclosure detects whether a collision has occurred at any point on the vehicle, thus enhancing safety.

[0033] Cooperative optimization of speed and path:

[0034] 1. Maximum speed limit: Given an optimal smooth path, the maximum speed value at each path point is determined by calculating the path curvature and based on the limit on the maximum lateral acceleration. ).

[0035] 2. Optimal Control Problem (OCP): Transforms the velocity planning problem into finding the minimum time... The optimal control problem is addressed by the OCP, which imposes constraints on starting / ending speed, maximum speed limit, longitudinal acceleration, and jerk.

[0036] 3. High-precision discrete solution: The Gaussian pseudospectral method (GPM) is proposed to discretize the continuous OCP, transform it into a nonlinear programming (NLP) problem, and combine it with sequential quadratic programming for solution.

[0037] 4. Final Trajectory: This process achieves precise collaborative planning of path and speed, ultimately completing the coupling of time and space to obtain a final trajectory that satisfies safety, stability, and efficiency.

[0038] This invention offers the following advantages: By employing a dual-sampling optimization strategy and a path-velocity collaborative planning mechanism, it achieves a comprehensive improvement in the global optimality, high security, high smoothness, and computational efficiency of trajectory planning. Specifically, the method effectively solves the problems of limited path guidance and single planned paths by applying sampling optimization to both path search guidance and local interpolation smoothing stages. This significantly enhances the smoothness, continuity, and feasibility of the trajectory, making the method more adaptable and flexible in complex multi-branch scenarios. The WRRT*-S algorithm introduces a path diversity preservation mechanism and a risk potential field guidance strategy, and adopts a multi-dimensional cost evaluation mechanism, effectively avoiding the algorithm from getting trapped in local optima and improving the globality of path search. Simultaneously, a quintic Bézier curve is used to ensure that the path satisfies G. 2 To improve continuity, the method combines speed and path co-optimization, significantly enhancing trajectory smoothness, continuity, and vehicle stability. In terms of safety, the method integrates a dual safety mechanism of risk potential field guidance and OBB collision detection, along with curvature-based maximum speed constraints in speed planning, ensuring excellent trajectory safety in complex environments. Finally, the method achieves precise co-optimization of path and speed by solving the nonlinear optimization problem using the Gaussian pseudospectral method.

[0039] Through the aforementioned dual sampling optimization strategy, this invention not only improves the path planning direction guidance capability, but also significantly enhances the continuity of the trajectory and the stability of vehicle driving. It effectively improves the trajectory feasibility, computational efficiency, and vehicle kinematic constraint satisfaction in multi-branch scenarios, significantly improving the execution efficiency and computational feasibility of the autonomous driving system. This provides an effective technical solution for achieving efficient and reliable autonomous driving trajectory planning in unstructured environments. Attached Figure Description

[0040] Figure 1 This is a schematic diagram of the trajectory planning process for autonomous vehicles in a branch road scenario based on dual sampling optimization in the embodiments of this application;

[0041] Figure 2 This is a schematic diagram of a parking lot intersection scenario in an embodiment of the present invention;

[0042] Figure 3 This is a schematic diagram of the vehicle kinematics model in an embodiment of the present invention;

[0043] Figure 4 This is a three-dimensional diagram of the field strength distribution of the risk potential field in the branch road scenario in this embodiment of the invention;

[0044] Figure 5 This is a schematic diagram of the smooth path candidate set in an embodiment of the present invention. Detailed Implementation

[0045] The present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments.

[0046] See Figure 1 — Figure 5 A trajectory planning method for autonomous vehicles in a branching road scenario based on double sampling optimization is proposed. The execution process of the method includes the following steps:

[0047] S101. Define the trajectory planning problem and establish the vehicle model and environment model:

[0048] Define the trajectory planning problem: Figure 2 The parking lot scenario shown has multiple branching paths. The problem of trajectory planning for an autonomous vehicle is defined. As a state space For obstacle space, This is free space. Starting point. and the finish line Given the problem of vehicle trajectory planning, find a time-parameterized continuous trajectory in a parking lot with branching paths that may contain random obstacles. satisfied and At any given time, and can be expressed by a formula, find a set of... from arrive The collision-free trajectory, among which The equation for the vehicle's motion can be written as: Or using integral form .

[0049] Vehicle Model Establishment: Based on the Ackerman steering model, which can accurately describe the basic motion characteristics of a vehicle at low speeds. For collision detection during vehicle motion, a rectangle consisting of the vehicle's maximum length and maximum width can be used as its outline boundary, combined with the OBB (Oriented Bounding Box) method. By combining the vehicle's actual outline with the OBB, accurate collision detection of the vehicle in motion can be achieved. The vehicle's state variables can be represented as follows: Where x and y are the coordinates of the center point of the rear axis; θ is the rear axle center point velocity, m / s; θ is the yaw angle, deg; φ is the front wheel equivalent steering angle, rad; k is the front wheel steering angle change rate, rad / s; L is the vehicle length, m; W is the vehicle width, m; L w L is the vehicle wheelbase, in meters (m). f , L r Let be the front and rear overhang lengths of the vehicle, in meters (m); points A, B, C, and D be the endpoints of the vehicle; point E is the intersection of the rear axle centerline and the right boundary of the vehicle; Establish the vehicle kinematic model as follows: Figure 3 As shown. And the formula can be obtained based on the above variables:

[0050]

[0051] Environmental Modeling: The design of the obstacle risk potential field considers the relative distance and approach direction between the autonomous vehicle and the obstacle, used to characterize the risk that the obstacle may pose during vehicle operation. This paper uses a quadratic higher-order Gaussian function to describe it. To flatten the obstacle's peak value, the obstacle's risk is set to 1. The risk value decay equation for the free space is:

[0052]

[0053] Where U is the risk value of x. The coordinates of the obstacle in the geodetic coordinate system. is the field strength coefficient, which is a constant greater than 0, used to control the degree of risk attenuation. , The shape function of the obstacle can be expressed as: and .in , The length of the obstacle in both the horizontal and vertical directions; , It is a constant greater than 0.

[0054] Figure 4 Calculated by the risk value decay equation, compared to the occupied map, the risk value on an empty road increases as the distance to an obstacle decreases, until it reaches its peak at the obstacle, representing the degree of danger of a collision.

[0055] It is worth noting that this article also considers parking space areas as obstacles to avoid collisions between vehicles and potential vehicles parked in the spaces.

[0056] S102. Guided path planning based on sampling optimization:

[0057] The WRRT*-S algorithm proposed in this paper is an improvement on the RRT*-Smart algorithm. This algorithm can not only search for feasible paths like RRT*-Smart, but also record all the paths found during the path search process. Thus, in complex multi-branching environments, multiple feasible paths are retained to form a candidate set of guiding paths. A multi-dimensional evaluation function is introduced, which proposes to replace the traditional collision cost with a risk potential field in the cost function. The algorithm also combines multiple factors such as path length cost and turning cost to comprehensively evaluate all paths.

[0058] The function descriptions contained in its pseudocode are as follows:

[0059] Path Optimization: When the algorithm successfully finds a path, it uses this path as the initial path. Nodes without any obstacles are directly connected. Traverse the path x. arrive The algorithm iterates through the parent nodes of each node, attempting to directly connect the parent and the current node. If a direct connection is not possible, the original path structure is preserved. This process continues until no node's parent can be directly connected to. This method effectively reduces redundant inflection points in the paths generated by the RRT* algorithm, thereby improving the smoothness and applicability of the paths.

[0060] Intelligent Sample: The intelligent sampling function was proposed to further optimize the path by ensuring that the path connected by the path optimization function is as close to obstacles as possible, thereby reducing the distance cost of the planned path. Since the number of nodes in the optimized path is much smaller than that in the original path, we propose using the optimized nodes as centers... Random nodes are generated in a sphere with a radius of [radius value]. This design enhances the adaptability of path planning to complex branching environments, but at the same time requires more iterations to ensure comprehensive path planning for multiple branches.

[0061] Unlike RRT*-Smart, which only selects the trajectory tree with the optimal path cost, this paper stores all planned trajectories in a cell array `path`. The `FindBestPath` function is used to calculate the risk potential field strength, path length cost, and turning cost at discrete points along each trajectory. Based on a comprehensive evaluation using the cost function, the optimal path is finally selected. The construction methods for each evaluation index are as follows:

[0062] 1. Obstacle risk field strength assessment: on the path In this process, path points are interpolated based on the grid resolution of the risk field. The weighted average risk and maximum risk value of the path points are calculated and used as risk indicators to reflect the safety of the path. This means the planned path not only avoids collisions with obstacles but also maintains a certain distance from them, which is beneficial for subsequent interpolation smoothing, as shown below:

[0063]

[0064] in They are respectively Time and The electric field strength value at the path point at time moment. .

[0065] 2. Path Distance Cost: The ratio of the current path to the maximum path in all path cells, as shown below:

[0066]

[0067] Each path The path length is The maximum path is

[0068] 3. Path turning cost: Defined as the normalized average of all changes in turning angles along the path, its expression is:

[0069]

[0070] Among them Number of path turns and They are respectively Time and The turning angle of the path point at any given moment; The maximum turning angle is set to 90° in the text, based on the parking lot scenario. . Used to measure the smoothness of a path. The closer the value is to 0, the smoother the path and the smaller the steering changes; the closer the value is to 1, the larger the steering angle changes in the path.

[0071] To comprehensively evaluate the merits of different paths, this paper proposes a weighted cost function. The obstacle risk, path distance, and path smoothness are uniformly evaluated as optimization objectives. The mathematical form of this cost function is as follows:

[0072]

[0073] , , The weighting coefficients for each evaluation indicator are given, and the cost design is normalized to satisfy the following conditions. These weights can be adjusted according to different application scenarios or needs.

[0074] S103. Local smooth trajectory planning based on guiding path:

[0075] Fifth-order Bezier primitives: To meet the requirements of the vehicle kinematics model, the primitives used for local planning need to be able to generate continuous curvature paths to smooth the guide path planned by WRRT*-S. Therefore, in this invention, primitives that satisfy G are selected. 2 A continuous quintic Bézier curve. A quintic Bézier curve can not only effectively connect multiple curve segments to achieve curvature continuity along a path, but also flexibly control the start and end states of the curve. Especially when considering the curvature of the path endpoints, it can provide a more accurate path planning solution.

[0076] Using this primitive, control point poses are obtained by interpolating along the guide path planned based on the WRRT*-S algorithm, thus achieving smoothing of the guide path. During interpolation, the start and end points of the guide path are preserved, while ensuring that the maximum distance between interpolation points within each path segment does not exceed d. lim Based on this, corner points in the original path are removed to prevent abrupt changes in curvature when connecting curves.

[0077] Smooth path candidate set generation: The shape of a quintic Bézier curve is determined by six control points, using the same initial pose. = [x0, y0, θ0, ρ0] and the endpoint pose = [x f y f θ f , ρ f The position, orientation, and curvature of the candidate path cluster are defined by the control points. Therefore, it is necessary to generate candidate paths with the same pose by changing the other four control points. Thus, to generate a set of candidate paths with the same start and end poses, the initial and final velocity vectors can be adjusted. , and acceleration vector , This controls the orientation angle and curvature of the path.

[0078] To determine the distance d between the extreme values ​​of different curves 0fTo maintain consistency, and to ensure that the velocity and acceleration vectors for different curve scenarios do not lead to inconsistent control point calculations due to differences in the original scale when generating candidate paths, the lengths of the velocity and acceleration vectors are normalized, and adjustment ranges are set for the velocity and acceleration vectors respectively. and And generate N during this period t N K A set of points, where the minimum and maximum normalized lengths are respectively... and .

[0079] Unit tangent vector between the start and end points , With normal vector , It can be represented as:

[0080]

[0081] Tangent vector of velocity vectors at the starting and ending points , Tangent vector to acceleration vector The length can be determined by the normalization factor and the distance d between the pose and the normalization factor. 0f The adjustment can be calculated using the following formula:

[0082]

[0083]

[0084] Considering the effect of path curvature on acceleration vectors allows for more flexible control point settings. However, it's important to note that curvature only affects the normal vector of the acceleration vector. Therefore, the acceleration vectors at the start and end points... , It can be expressed by the following formula,

[0085]

[0086] Once the velocity and acceleration vectors for two given poses are calculated, a set of path candidates can be generated by applying all combinations of velocity and acceleration vectors at the two control points. Let... = [x0, y0] and = [x f y f [ ] represents the coordinates of the start and end points. The positions of the six control points for each Bézier curve segment can be obtained as follows:

[0087]

[0088] in These are the control points of the Bézier curve. , The adaptive adjustment coefficient for the control point can be derived from... , Even if it is found that adjustments can be made based on different starting and ending poses to generate a more suitable set of path candidates, such as... Figure 5 As shown, the colored curves represent trajectory clusters, and the green hollow circles represent control points. .

[0089] Multi-objective function design for candidate path clusters: Three evaluation metrics are set for the smooth path candidate set: path smoothness, economy, and collision penalty. Smoothness is defined as the ratio of the maximum curvature of the curvature path curve to the average rate of change of curvature and the maximum rate of change of curvature. Economy refers to the planned path length. Collision penalty is determined by detecting whether a collision occurs at each endpoint of the vehicle using an OBB bounding box. After normalization, the cost function equations for path smoothness, economy, and collision penalty are established as follows:

[0090]

[0091]

[0092]

[0093] in The curvature of the nth path in the path candidate set and The maximum curvature under vehicle steering constraints; The straight-line distance between the starting point and the ending point. The length of the nth path in the candidate path set.

[0094] Establish a cost function for selecting the path candidate set:

[0095] In the formula, Using the weighting coefficients, by introducing collision detection, the evaluation of the smooth path candidate set not only considers the smoothness and economy of the path, but also strengthens the focus on safety.

[0096] S104. Speed ​​and Path Co-optimization:

[0097] The optimal problem is as follows: After smoothing and selecting the optimal path, a velocity curve needs to be calculated on the given path to couple the longitudinal velocity value with each path point. Furthermore, the velocity planning process must satisfy constraints on starting and ending velocities, linear velocity, lateral and longitudinal accelerations, and jerk. Considering safety during lateral travel, the curvature of the generated path is calculated, and the lateral acceleration is... Set constraints and calculate the maximum velocity value corresponding to the curvature at each path point. Use this to constrain the maximum velocity during the turning process.

[0098]

[0099] These maximum velocity values ​​at the reference point will be used to set the final velocity for the final trajectory calculation:

[0100]

[0101] The length of the planned path is represented as , The curvature is represented by the discrete points along the path. Let be the total time required for the intelligent vehicle to travel along this path. The speed planning process must satisfy constraints on the starting and ending speeds, linear velocity, lateral and longitudinal accelerations, and jerk. Therefore, solving the speed planning problem can be transformed into solving the optimal control problem of minimizing the time required.

[0102]

[0103] The initial and final speeds are given according to the driving requirements, and the maximum speed limit is obtained using the speed limit based on lateral acceleration mentioned above, with longitudinal acceleration and jerk applied. The limits of the maximum and minimum limits.

[0104] Discrete solution using Gaussian pseudospectral method: This invention uses the Gaussian pseudospectral method for discretization to transform the continuous optimal control problem into a nonlinear programming problem (NLP), and combines it with sequential quadratic programming for solution.

[0105] First, the Gaussian pseudospectral method is used to calculate the N pseudospectral points of the global interpolation polynomial. Evenly distributed in Between, the time domain Convert to time domain :

[0106]

[0107] Secondly, control variables , Discretize at pseudospectral points as :

[0108]

[0109] in It is the first The Lagrange polynomial of order X, in explicit form is:

[0110]

[0111] The relationship between discrete path points and velocities can be expressed by a continuous state equation as follows:

[0112]

[0113] The state equation discretized by the Gaussian pseudospectral method becomes the following at the pseudospectral points:

[0114]

[0115] To transform the constraint equations into algebraic constraints, the derivatives of the state variables need to be defined:

[0116]

[0117] The aforementioned state variables are then discretized.

[0118]

[0119] After defining constraints on the discretized control variables, the expression is rearranged into the following form, which is then solved using sequential quadratic programming.

[0120]

[0121] in .

[0122] In summary, the method of this invention introduces sampling optimization strategies in both the path search guidance stage and the local interpolation smoothing stage: Addressing the issues of limited path search direction and singular path planning results in multi-branch scenarios, an improved WRRT*-S algorithm is used to generate multiple feasible path candidate sets in complex environments, thereby enhancing path diversity and providing a basis for subsequent guidance; a multi-dimensional cost evaluation mechanism is introduced, integrating multiple indicators such as risk potential field, path length, and turning cost to construct a comprehensive path evaluation function, effectively guiding sampling to expand to better regions, achieving a balance between path movement rationality and spatial distribution, and improving the globality and environmental adaptability of path search.

[0123] Building upon this, a fifth-order Bézier curve is used to perform G² continuous interpolation on the selected guiding path, ensuring that the path satisfies vehicle kinematic constraints. Simultaneously, by introducing a control point sampling strategy for velocity and acceleration vectors during the interpolation process, a smooth path candidate set is constructed and optimized, further improving the smoothness and feasibility of the path. This dual sampling mechanism not only effectively alleviates the problem of restricted path guidance but also significantly enhances the continuity of the trajectory and the stability of vehicle movement.

[0124] Furthermore, for the generated smooth path, a speed-optimal control model based on the maximum speed constraint is constructed. The Gaussian pseudospectral method is combined to solve the nonlinear optimization problem, realizing the collaborative planning of speed and path, completing the coupling of time and space, and significantly improving the execution efficiency of the autonomous driving system while ensuring trajectory safety and controllability.

[0125] It is worth noting that the proposed WRRT*-S algorithm can be used not only as an independent path planning module, but also as a guiding mechanism for other trajectory planning methods, to explore all reachable paths and set search guidance information such as guide points.

[0126] This specification details the technical solution of the autonomous vehicle trajectory planning method for branching road scenarios based on dual sampling optimization according to the present invention. It should be noted that these specific embodiments are only used to illustrate the principles and core ideas of the present invention, and are not intended to limit the scope of protection of the present invention. Any equivalent modifications or variations made based on the concept of the present invention should be considered to fall within the scope of protection of the present invention. The scope of protection of the present invention is ultimately determined by the contents of the appended claims.

Claims

1. A trajectory planning method for autonomous vehicles in a branching road scenario based on dual sampling optimization, characterized in that, The target scenario is a parking lot scenario in an unstructured environment with multiple branching roads, and includes the following steps: A. Guiding Path Planning: An improved Fast Search Random Tree (WRRT*-S) algorithm is used to search and generate a candidate set of guiding paths in the target scene. B. Optimal Guiding Path Selection: A multi-dimensional cost evaluation is performed on the candidate guiding paths, and the optimal path is selected as the optimal guiding path. C. Local Smoothing Trajectory Planning: Interpolation is performed on the optimal guiding path, and the interpolation points are used as the endpoints of the path segments. A cluster of smooth paths is generated using quintic Bézier curves. D. Optimal Smoothing Path Selection: The cluster of smooth paths is optimized using a multi-objective function, and the optimal path is selected as the optimal smooth path. E. Velocity Planning and Co-optimization: For the optimal smooth path, an optimal control problem is constructed based on the maximum velocity constraint. The co-optimization of the path and velocity is achieved by solving the problem to obtain the final trajectory.

2. The method according to claim 1, characterized in that, In step A, the improved fast search random tree algorithm (WRRT*-S) is based on the RRT* algorithm, with the addition of a path diversity preservation mechanism and a risk potential field guidance strategy. In the path search process, all found paths are recorded to form the candidate set of guiding paths.

3. The method according to claim 2, characterized in that, The WRRT*-S algorithm includes: a. Sampling is performed based on the field strength distribution of the risk potential field, with priority given to generating random points in the idle areas with low risk values; b. Use the path optimization function to directly connect nodes without gaps or obstacles in the searched path to reduce redundant inflection points; c. Employ the Intelligent Sample function to generate random nodes around the optimized path nodes, so that the planned path is closer to the obstacles; d. The WRRT*-S algorithm is used to record all planned trajectories and store them in the cell array path, forming the candidate set of the guiding path.

4. The method according to claim 1, characterized in that, In step B, the multidimensional cost assessment includes the following indicators: a. obstacle risk field strength assessment, used to reflect the safety of the path; b. path distance cost, used to assess the path length; c. Path turning cost, used to measure the smoothness of the path; the selection of the optimal guiding path is based on a weighted cost function of this indicator. The results of the comprehensive evaluation ,in, , , These are obstacle risk field strength, path distance cost, and path turning cost, respectively. , , These are the corresponding weighting coefficients.

5. The method according to claim 4, characterized in that, The obstacle risk field strength assessment is based on calculating the weighted average risk and maximum risk value at the path points; the obstacle risk field is described using a quadratic higher-order Gaussian function. The path turning cost is the normalized average of all turning angle changes in the path.

6. The method according to claim 1, characterized in that, In step C, the quintic Bézier curve is capable of providing G. 2 The smooth path candidate cluster is generated by applying different combinations of velocity and acceleration vectors at the start and end points of the path segment, and by calculating the position of the six control points of the quintic Bézier curve according to the formula. During the generation of the smooth path candidate cluster, the lengths of the velocity and acceleration vectors are normalized.

7. The method according to claim 1, characterized in that, In step D, the multi-objective function optimization includes the following evaluation metrics: a. Path smoothness, used to evaluate the maximum value of path curvature, the ratio of the average rate of change of curvature to the maximum rate of change of curvature; b. Economic efficiency, referring to the actual length of the planned route; c. Collision penalty: Based on OBB bounding box detection, determine whether each endpoint of the vehicle collides with an obstacle. The selection of the optimal smooth path is based on a weighted cost function. The results of the comprehensive evaluation ,in, , , These are the path smoothness, economy, and collision penalty cost functions, respectively. , , These are the corresponding weighting coefficients.

8. The method according to claim 1, characterized in that, In step E, the optimal control problem aims to minimize the time required for solution. This optimal control problem is discretized into a nonlinear programming (NLP) problem using the Gaussian pseudospectral method (GPM) and solved accordingly. The optimal control problem is subject to the following constraints: a. initial and final velocity constraints; b. maximum velocity constraint based on path curvature; c. linear velocity constraint; d. lateral and longitudinal acceleration constraints; e. jerk constraint. The maximum velocity constraint is determined by calculating the maximum lateral acceleration based on the path curvature to determine the maximum velocity value at each path point.

9. An autonomous vehicle trajectory planning system, characterized in that, include: a. Map building module: used to rasterize the map information collected by sensors and build a risk potential field; b. Path planning layer: Used to receive the map information, perform guided path planning, optimal guided path selection, local smooth trajectory planning and optimal smooth path selection, and output the optimal smooth path; c. Velocity planning layer: This layer receives the optimal smooth path, plans the velocity based on the path curvature, performs co-optimization of the path and velocity, and outputs the final trajectory. d. Control system: used to receive the final trajectory and output longitudinal and lateral maneuvering signals to the vehicle actuators.

10. The system according to claim 9, characterized in that, The path planning layer executes the trajectory planning method according to any one of claims 1 to 7, and the velocity planning layer executes the velocity planning and collaborative optimization method according to claim 8.

Images