Automatic driving vehicle trajectory planning method in combination with surrounding vehicle prediction information

Abstract

According to the automatic driving vehicle trajectory planning method combined with prediction information of surrounding vehicles, random sampling in a road boundary is carried out under a Frenet coordinate system, an initial sampling point set is generated, and an optimal feasible path set is selected through a dynamic planning algorithm. Then, sampling points are smoothly connected by using a quintic polynomial curve to form a path cluster, and a path with the lowest cost is selected as a preliminary optimal path based on a cost function. And finally, further optimizing the preliminary optimal path by adopting a quadratic programming algorithm, and adding a cost item of the predicted trajectory of the environment vehicle into a cost function of the preliminary optimal path. The cost item is constructed by solving a distance function between a planning point and an environment vehicle prediction track point, and a negative number of the distance function is added into a quadratic programming cost function so as to maximize the distance between the vehicle and the environment vehicle. According to the method, real-time and prediction information is integrated, and the quality and safety of trajectory planning are remarkably improved.

Description

Technical Field

[0001] The present invention belongs to the technical field of intelligent transportation and autonomous driving, and particularly relates to a method for autonomous vehicle trajectory planning that combines surrounding vehicle prediction information. Background Art

[0002] The local path planning of autonomous vehicles focuses on a comprehensive consideration of decision-making and trajectory planning, aiming to minimize the travel time while ensuring that the vehicle meets the kinematic characteristics to make the driving process safe and comfortable. This complex planning process requires in-depth cross-disciplinary collaboration among system dynamics, control theory, artificial intelligence, and other disciplines. In the planning process, the limitations of road environmental conditions are naturally important factors, but equally important is the dynamic impact of other traffic participants. Especially in multi-vehicle scenarios, vehicle driving is not only restricted by static factors such as road structure and traffic rules but also needs to cope with the dynamic challenges brought by other vehicles. This means that every entity in the scenario is interconnected and mutually influential with other entities, rather than existing in isolation. Currently, relatively common trajectory planning methods include graph search methods, random sampling methods, curve interpolation methods, and numerical optimization methods, etc.

[0003] Trajectory planning methods based on graph search have always received much attention. Such methods cover various classic algorithms such as Dijkstra's algorithm, A* algorithm, and D* algorithm. Dijkstra's algorithm, as a single-source shortest path search algorithm, is based on the idea of breadth-first search. It traverses all adjacent nodes of the current node one by one and performs priority sorting according to the weight values. In this process, the algorithm always preferentially selects the node with the smallest weight value as the expansion node until the traversal ends and the path with the smallest cumulative weight value is found. However, Dijkstra's algorithm has some blindness in the search process, resulting in relatively low search efficiency. To overcome the limitations of Dijkstra's algorithm, the A* algorithm came into being. As a widely used graph search algorithm for path planning, the A* algorithm introduces heuristic information on the basis of Dijkstra's algorithm, thus significantly reducing the number of expanded nodes and improving the search speed of the shortest path. In addition, many variants of the A* algorithm have been derived, such as Field D* and Anytime D* (AD*), etc. These variant algorithms optimize and expand the A* algorithm from different perspectives. Trajectory planning methods based on graph search can achieve effective obstacle avoidance path planning when the obstacles are stationary and the vehicle speed is low. However, such methods are inadequate in high-speed scenarios or dynamic traffic environments and are difficult to adapt to complex and changing traffic conditions.

[0004] Trajectory planning methods based on random sampling have received extensive attention due to their excellent planning efficiency. Among these methods, the Rapid-exploration Random Tree (RRT) is particularly commonly used. The RRT algorithm constructs a tree in the search space through random sampling and expands from the initial state to the target state. In each iteration, the algorithm selects a collision-free sample point and finds the nearest node in the tree. If the sample point is reachable, it is connected and expanded; otherwise, a new node is found through a steering function for expansion. Collision checking is performed during the process, and the algorithm continues to iterate until a path is found or a predetermined condition is reached. This algorithm has a fast solution speed and can incorporate kinematic or dynamic constraints during the solution process, enabling the generated trajectory to meet kinematic or dynamic requirements and thus be effectively executed by the trajectory tracking module. RRT-based planning methods have strong global search capabilities and can quickly cover the entire search space. However, since they mainly rely on random search, the paths found are often not optimal and may contain redundant nodes and path segments with large tortuosity, reducing the overall planning efficiency and driving efficiency.

[0005] The method based on curve interpolation generates a fitting trajectory that meets constraints such as feasibility and safety according to given reference points. Commonly used curves include: Clothoids curves, polynomial curves, Bezier curves, etc. Clothoids curves are defined through Fresnel integrals, and a smooth and curvature-continuous trajectory that meets driving requirements can be obtained. However, the operation of Fresnel integrals will increase the overall performance requirements of the system. The curve based on polynomial functions is established through the initial state, target state of the vehicle, and a series of boundary conditions. Polynomials representing the lateral trajectory and longitudinal trajectory are established respectively, and a normal equation system is established to solve the undetermined coefficients, thereby obtaining the planned trajectory. The solution of polynomial functions is relatively simple and generally can meet real-time requirements. However, in complex scenarios, due to more constraint conditions, it becomes difficult to solve the coefficients of high-order polynomial curves that meet the conditions. The shape of the Bezier curve is determined by the position and number of control points. By adjusting the position and number of control points, the required trajectory shape can be precisely shaped. This makes the Bezier curve have the characteristics of smoothness and continuity. The trajectory planned through the Bezier curve can ensure a stable speed and acceleration during driving. However, the local control points of the Bezier curve are difficult to modify, and the curve has poor flexibility.

[0006] To address the limitations of existing methods, this embodiment designs and implements a lane-changing overtaking trajectory planning algorithm that takes into account environmental vehicle prediction information. First, the conversion of the Frenet coordinate system is performed. Then, based on the dynamic programming algorithm, under the constraints of road boundaries and obstacles, a coarse-grained trajectory that meets the obstacle avoidance conditions and boundary conditions is planned. This trajectory creates a convex space. Then, based on this convex space, the quadratic programming algorithm is used in combination with a quintic polynomial to generate a smooth lane-changing trajectory that satisfies the vehicle kinematic constraints. In addition, to consider the impact of the future trajectories of environmental vehicles, an optimization term regarding the predicted trajectories of environmental vehicles is designed and added to the cost function of the quadratic programming to reduce the potential safety risks caused by environmental vehicles during the lane-changing process of the intelligent vehicle and ensure safe and reasonable interaction between the intelligent vehicle and environmental vehicles. Summary of the Invention

[0007] To address the problems existing in the above-mentioned prior art, this embodiment provides an autonomous vehicle trajectory planning method that combines surrounding vehicle prediction information. The purpose is to design an optimization term regarding the predicted trajectories of environmental vehicles during the quadratic programming process and add it to the objective function, enabling the algorithm to adaptively avoid potential future collision risks. First, random sampling is performed within the road boundaries in the Frenet coordinate system, a distance estimation function between the current sampling point and the obstacle trajectory point is added to the objective function, and the dynamic programming method is used to select the optimal feasible set. Then, the feasible set is used as the convex space of the quadratic programming, and the vehicle trajectory is represented by a quintic polynomial. With the optimization objectives of optimizing the acceleration, jerk, distance to the current environmental vehicle, and distance to the future environmental vehicle, after obtaining the optimal polynomial coefficients, a trajectory that meets safety, smoothness, and feasibility is obtained, ensuring the safety, intelligence, and reliability of the trajectory planning result in a dynamic driving scenario.

[0008] To achieve the above objective, the specific solution of this embodiment is as follows:

[0009] An autonomous vehicle trajectory planning method that combines surrounding vehicle prediction information, comprising the following steps:

[0010] Step 1, perform random sampling based on the road boundaries in the Frenet coordinate system to generate an initial sampling point set for trajectory planning, and use the dynamic programming algorithm to process the initial sampling point set to select the optimal feasible path set;

[0011] Step 2, smoothly connect the sampling points obtained in Step 2 using a quintic polynomial curve to form a path cluster, and select the path with the lowest cost as the preliminary optimal path based on the cost function;

[0012] Step 3: Further optimize the preliminary optimal path using the quadratic programming algorithm. The cost function of the quadratic programming includes a cost term for the predicted trajectories of surrounding vehicles. The cost term is constructed by solving the distance function between all the planned points in the discrete space and the predicted trajectory points of the surrounding vehicles corresponding to each planned point, and the negative value of the distance function is added to the cost function of the quadratic programming to maximize the distance between the ego vehicle and the surrounding vehicles within the prediction time domain.

[0013] Further, the Frenet coordinate system in Step 1 is obtained by converting from the Cartesian coordinate system, and the conversion formula is as follows:

[0014] s = s r (1)

[0015]

[0016] l′ = (1 - k r l)tan(θ x - θ r ) (5)

[0017]

[0018] In the formula, s represents the longitudinal displacement; represents the longitudinal velocity; represents the longitudinal acceleration; v x represents the lateral velocity component of the vehicle; θ x represents the heading angle of the vehicle, that is, the angle between the vehicle's forward direction and the reference line; θ r represents the azimuth angle of the reference line, usually the angle between the road tangent direction and the horizontal direction; k r represents the road curvature, indicating the degree of road bending; l represents the lateral displacement, indicating the lateral distance of the vehicle relative to the reference line; a x represents the lateral acceleration of the vehicle; l′ represents the first derivative of the lateral displacement with respect to the longitudinal displacement, that is, the lateral velocity; k x represents the curvature of the vehicle trajectory; k′ x represents the derivative of the vehicle trajectory curvature, that is, the curvature change rate; l″ represents the second derivative of the lateral displacement, that is, the lateral acceleration; x x ,y x represents the position coordinates of the vehicle in the Cartesian coordinate system; x r ,y r represents the position coordinates of a point on the reference line in the Cartesian coordinate system; sign represents the sign function, which is used to determine the direction of the lateral displacement.

[0019] Further, the initial sampling point set in Step 1 uses equally spaced sampling in the transverse and longitudinal directions respectively, and the expression is as follows:

[0020] S(k) = S(1) + (k - 1) * ds, k = 1, 2, ..., m (7)

[0021] L(k) = L(1) + (k - 1) * dl, k = 1, 2, ..., n (8)

[0022] Wherein, S(k) represents the S-axis coordinate of the k-th point, L(k) represents the L-axis coordinate of the k-th point, m and n respectively represent the number of longitudinal sampling points and transverse sampling points; dl represents the transverse sampling distance; ds represents the longitudinal sampling distance.

[0023] Furthermore, the optimal feasible path set described in step one is selected by minimizing the objective function, and the formula for the minimized objective function is as follows:

[0024]

[0025] Wherein: represents the smoothness cost function, which is used to encourage the generation of a smooth trajectory; represents the distance cost function from obstacles, which is used to encourage the generation of a trajectory far from obstacles; represents the distance cost function from the reference line, which is used to encourage the vehicle to drive along the reference line;

[0026] The objective function includes a smoothness cost function, a distance cost function from obstacles, and a distance cost function from the reference line;

[0027] The formula for the smoothness cost function is as follows:

[0028]

[0029] Wherein, ω1∫(f′(s) 2 ds represents the difference between the vehicle's heading and the planned route; it may be proportional to the square of the vehicle's speed and is used to encourage the vehicle to drive along the planned route; ω2∫(f″(s)) 2 ds represents the curvature of the path; it is proportional to the square of the vehicle's jerk and is used to avoid the path being too complex and improve driving stability; ω3∫(f″′(s)) 2 ds represents the rate of change of the path curvature; it is proportional to the square of the vehicle's jerk and is used to avoid the path being too complex and improve driving stability;

[0030] The formula for the distance cost function from obstacles is as follows:

[0031]

[0032] Wherein, [d c ,d nIndicates the distance buffer. When the distance d > d n , there will be no collision between the vehicle and the obstacle, and the cost function is 0; when the distance d < d c , the vehicle may be at risk of colliding with the obstacle, which is not allowed in the actual driving process, so its cost function is set to C collision , and its value is infinite; in the buffer, the cost function C nudge is a monotonically decreasing function. The closer to the obstacle, the higher the risk of collision and the greater the cost value. The farther away from the obstacle, the safer the driving and the smaller the cost value;

[0033] The formula for the distance cost function from the reference line is as follows:

[0034]

[0035] In the formula, f(s) represents the lateral coordinate function l = f(s) in the Frenet coordinate system; g(s) represents the reference line function; ds represents the differential of the longitudinal coordinate s in the Frenet coordinate system, which is the microelement in the integral.

[0036] Furthermore, the formula for smoothly connecting the sampling points obtained in step one using a fifth-degree polynomial curve is as follows:

[0037] l = f(s) = a0 + a1s + a2s 2 + a3s 3 + a4s 4 + a5s 5 (9)

[0038] In the formula, l represents the lateral position of the vehicle in the Frenet coordinate system; f(s) is a function of the variable s, representing the position of the vehicle on the trajectory; a0, a1, a2, a3, a4, and a5 respectively represent the coefficients of the fifth-degree polynomial; s is the independent variable, representing the longitudinal position of the vehicle in the Frenet coordinate system.

[0039] Furthermore, the formula for adding the negative of the distance function to the cost function of quadratic regularization is as follows:

[0040]

[0041] In the formula, ω3 is the weight coefficient, i represents the i-th planning point, and i proj represents the index of the predicted trajectory point of the environmental vehicle at the same time as the i-th planning point, and s proj represents the index of the predicted trajectory point of the environmental vehicle at the same time as the planning point at the longitudinal distance s; K represents the total number of environmental vehicles; by minimizing C QPobs_pre, that is, by maximizing the distance between each planning point within the planning time domain and the predicted trajectory points of the surrounding vehicles at their corresponding times, to achieve avoidance of potential future obstacles.

[0042] Further, in the quadratic programming process of step three, boundary constraints are imposed. The boundary constraints include the lateral displacement l i at the longitudinal displacement s i satisfying the road boundary conditions, and the formula is as follows:

[0043] l i ∈[l mini ,l maxi (26)

[0044] In the formula, l i represents the lateral displacement at the longitudinal displacement s i ; l min represents the minimum value of the lateral displacement, that is, the lower limit of the lateral position where the vehicle can safely travel on the road; l max represents the maximum value of the lateral displacement, that is, the upper limit of the lateral position where the vehicle can safely travel on the road;

[0045] Considering the vehicle size and avoiding the edges of the vehicle from colliding with the road boundary or obstacles, the constraint formula is:

[0046]

[0047] In the formula, l i represents the lateral displacement at the longitudinal position s i ; d i l i ′ represents the first derivative of the lateral offset of the vehicle at the longitudinal position s i with respect to the longitudinal displacement, that is, the lateral speed; w represents the width of the vehicle; lb i and ub i represent the lower and upper bounds of the lateral displacement at the longitudinal position s i respectively.

[0048] Further, in the quadratic programming process of step three, dynamic feasibility constraints are imposed. The dynamic feasibility constraints include ensuring the continuity and smoothness between path points, and the constraint formula is:

[0049]

[0050] In the formula, l i represents the lateral displacement at the current longitudinal position si; l i+1 represents the lateral displacement at the longitudinal position si at the previous moment of the current moment; l i-1represents the lateral displacement at the longitudinal position si at the moment after the current moment; △s represents the differential element of the longitudinal displacement, that is, the tiny change amount.

[0051] Advantages of this embodiment

[0052] 1. The autonomous vehicle trajectory planning method combining surrounding vehicle prediction information in this embodiment models the trajectory by adopting the Frenet coordinate system. When the vehicle continuously travels along the lane reference line, it significantly reduces the complexity of trajectory planning. The selection of this coordinate system enables the driving state of the vehicle to be described in a more intuitive and concise manner, thus optimizing the design and implementation of the trajectory planning algorithm.

[0053] 2. This embodiment uses the dynamic programming method to select the optimal drivable path from the path clusters generated by the sampling points, realizes lightweight decision-making and effective obstacle avoidance, and effectively reduces the computational amount and improves the efficiency of trajectory planning, especially when the vehicle needs to avoid obstacles.

[0054] 3. This embodiment constructs a cost function for the predicted trajectory of the surrounding vehicles, enabling the planning algorithm to consider future information during the process of generating the planned trajectory. This design realizes the early avoidance of some emergencies or potential collisions, significantly improves the predictability of the algorithm, that is, the algorithm can not only consider the information obtained in real time, but also take into account the predicted future information, thereby improving the quality of the planned trajectory. And by adding a distance function between the predicted trajectory points of the self-vehicle and the surrounding vehicles to the cost function, the distance between the self-vehicle and the surrounding vehicles is maximized, reducing the potential safety risk.

[0055] 4. The method in this embodiment combines the dynamic programming and quadratic programming algorithms, reduces the computational amount, and improves the efficiency of trajectory planning. This embodiment can generate a smoother and safer trajectory on the premise of meeting the kinematic and dynamic constraints of the vehicle. During the quadratic programming process, an optimization term regarding the predicted trajectory of the surrounding vehicles is designed and added to the objective function, enabling the algorithm to adaptively avoid potential future collision risks. And by solving the distance function between all the planned points in the discrete space and the predicted trajectory points of the surrounding vehicles corresponding to each planned point moment, and adding the negative value of this distance function to the cost function, the distance between the self-vehicle and the surrounding vehicles within the prediction time domain is maximized.

[0056] 5. The method in this embodiment improves the driving safety of the autonomous vehicle by avoiding potential collision risks in advance. At the same time, a quintic polynomial is used to represent the vehicle trajectory, and by optimizing the acceleration and jerk of the trajectory, the smoothness of the trajectory is ensured, improving the reliability of vehicle driving and the riding comfort of passengers.

[0057] 6. The method of this embodiment improves the safety and reliability of autonomous vehicles by combining the prediction information of surrounding vehicles. This method not only utilizes real-time data but also considers possible future situations for trajectory planning. In addition, this embodiment verifies the effectiveness of the algorithm through simulation experiments in different scenarios, demonstrating the excellent performance of this method in practical applications. This method is applicable to a variety of complex dynamic traffic scenarios and has good flexibility and scalability. Description of the Drawings

[0058] Figure 1 It is a flowchart of the autonomous vehicle trajectory planning method that combines the prediction information of surrounding vehicles in this embodiment.

[0059] Figure 2 It is a schematic diagram of Frenet coordinate transformation in this embodiment.

[0060] Figure 3 It is a generation diagram of lane-changing trajectory clusters in this embodiment.

[0061] Figure 4 It is a schematic diagram of equidistant sampling in this embodiment.

[0062] Figure 5 It is a connection diagram of quintic polynomials in this embodiment.

[0063] Figure 6 It is a process diagram of dynamic programming in this embodiment.

[0064] Figure 7 It is a schematic diagram of constraint boundaries in this embodiment.

[0065] Figure 8 It is a schematic diagram of the overtaking scenario by borrowing a lane when there are interfering vehicles in the left lane in this embodiment.

[0066] Figure 9 It is a curve graph of the speed change of environmental vehicles in the scenario where there are interfering vehicles in the left lane in this embodiment. Among them, (a) is the speed change curve of Car1; (b) is the speed change curve of Car2.

[0067] Figure 10 It is a curve graph of the speed change of environmental vehicles in the scenario where the interfering vehicle in the left lane approaches the host vehicle. Among them, (a) is the speed change curve graph of Car1; (b) is the speed change curve of Car2.

[0068] Figure 11 It is a visualization analysis graph of the multi-frame environmental vehicle trajectory prediction results in the scenario where the vehicles in the left lane are driving normally in this embodiment.

[0069] Figure 12 It is an analysis graph of the comparison results of the planned trajectories in the scenario where the vehicles in the left lane are driving normally in this embodiment.

[0070] Figure 13 It is a visualization comparison graph of the real-time trajectory results when the host vehicle approaches Car2 in the scenario where the vehicle in the left lane of this embodiment is driving normally. Among them, (a) is the planning result graph without combining prediction information; (b) is the planning result graph combining prediction information.

[0071] Figure 14 It is a visualization comparative analysis graph of the real-time trajectory results when the host vehicle approaches Car1 in the scenario where the vehicle in the left lane of this embodiment is driving normally. Among them, (a) is the planning result graph without combining prediction information; (b) is the planning result graph combining prediction information.

[0072] Figure 15 It is a visualization comparative analysis graph of the real-time planning results at the lane change starting point in the scenario where the vehicle in the left lane of this embodiment is driving normally. Among them, (a) is the planning result without combining prediction information; (b) is the planning result combining prediction information.

[0073] Figure 16 It is a comparison graph of the lateral acceleration change results in the scenario where the vehicle in the left lane of this embodiment is driving normally.

[0074] Figure 17 It is a visualization comparison graph of the multi-frame environmental vehicle trajectory prediction results in the scenario where the vehicle in the left lane of this embodiment has a tendency to approach the host vehicle.

[0075] Figure 18 It is a comparison graph of the planned trajectory comparison results in the scenario where the vehicle in the left lane of this embodiment has a tendency to approach the host vehicle.

[0076] Figure 19 It is a visualization comparative analysis graph of the real-time trajectory results when the host vehicle approaches Car2 in the scenario where the vehicle in the left lane of this embodiment has a tendency to approach the host vehicle. Among them, (a) is the planning result without combining prediction information; (b) is the planning result combining prediction information.

[0077] Figure 20 It is a visualization comparative analysis graph of the real-time trajectory results when the host vehicle approaches Car1 in the scenario where the vehicle in the left lane of this embodiment has a tendency to approach the host vehicle. Among them, (a) is the planning result without combining prediction information; (b) is the planning result combining prediction information.

[0078] Figure 21 It is the comparison result of the lateral acceleration in the scenario where the vehicle in the left lane of this embodiment has a tendency to approach the host vehicle.

[0079] Figure 22It is a visual comparative analysis chart of the real-time planning results at the acceleration mutation point 100m - 150m in the scenario where the vehicle in the left lane of this embodiment has a tendency to approach the own vehicle. Among them, (a) is the planning result without combining prediction information; (b) is the planning result with combined prediction information. Detailed implementation manner

[0080] The following further explains and illustrates this embodiment in conjunction with the accompanying drawings and the detailed implementation manner. It should be noted that this specific embodiment is not used to limit the scope of rights of this embodiment.

[0081] As Figures 1 to 22 shown, a trajectory planning method for an autonomous vehicle that combines surrounding vehicle prediction information provided by this specific embodiment includes the following steps:

[0082] Step 1, perform random sampling based on the road boundary in the Frenet coordinate system to generate an initial sampling point set for trajectory planning, and use the dynamic programming algorithm to process the initial sampling point set to select the optimal set of feasible paths;

[0083] In the Frenet coordinate system, the motion state of the vehicle can be described in a more intuitive and concise manner, significantly reducing the complexity of trajectory planning. By performing random sampling within the road boundary to generate an initial sampling point set, it provides basic data for trajectory planning. Trajectory planning is to more accurately grasp the surrounding environment by making full use of the results of trajectory prediction, so as to plan a safe and efficient trajectory on the basis of fully considering the interaction with environmental vehicles.

[0084] The steps of the Frenet coordinate system conversion method are as follows:

[0085] In the Frenet coordinate system, the motion trajectory of the vehicle appears as a straight line. When the vehicle continuously travels along the lane reference line, this characteristic significantly reduces the complexity of trajectory planning. Using the Frenet coordinate system can describe the driving state of the vehicle in a more intuitive and concise manner, thus optimizing the design and implementation of the trajectory planning algorithm.

[0086] In the Cartesian coordinate system, the motion state of the vehicle is described by the parameters x , k x , v x , a x (representing the current position of the vehicle, the direction angle of the current point position of the vehicle, curvature, speed, and acceleration respectively)

[0087] In the Frenet coordinate system, it is represented by are described, representing longitudinal displacement, longitudinal velocity, longitudinal acceleration, lateral displacement, lateral velocity, lateral acceleration, the first derivative of lateral displacement with respect to longitudinal displacement, and the second derivative of lateral displacement with respect to longitudinal displacement respectively.

[0088] is the position of the projection point of the current vehicle position in the Cartesian coordinate system on the reference line The x in the Frenet coordinate system is expressed as θ r is the azimuth angle of the reference trajectory point.

[0089] According to the definition of curvature, k r = θ r ′ = dθ r / ds, k x = θ x ′ = dθ x / ds.

[0090] After completing the derivation of the coordinate system conversion relationship, the formula for converting from the Cartesian coordinate system to the Frenet coordinate system is:

[0091] s = s r (1)

[0092]

[0093] l′ = (1 - k r l)tan(θ x - θ r ) (5)

[0094]

[0095] In the formula, s represents longitudinal displacement; represents longitudinal velocity; represents longitudinal acceleration; v x represents the lateral velocity component of the vehicle; θ x represents the heading angle of the vehicle, that is, the angle between the vehicle's forward direction and the reference line; θ r represents the azimuth angle of the reference line, usually the angle between the road tangent direction and the horizontal direction; k r represents the road curvature, indicating the degree of road bending; l represents lateral displacement, indicating the lateral distance of the vehicle relative to the reference line; a x represents the lateral acceleration of the vehicle; l′ represents the first derivative of lateral displacement with respect to longitudinal displacement, that is, lateral velocity; k x represents the curvature of the vehicle trajectory; k′ x represents the derivative of the vehicle trajectory curvature, that is, the curvature change rate; l″ represents the second derivative of lateral displacement, that is, lateral acceleration; x x ,yx Represents the position coordinates of the vehicle in the Cartesian coordinate system; x r , y r Represents the position coordinates of a point on the reference line in the Cartesian coordinate system; sign represents the sign function, which is used to determine the direction of the lateral displacement.

[0096] The dynamic programming algorithm is used to select the optimal drivable path from the path clusters generated by the sampling points. This process can be regarded as a lightweight decision-making. When the vehicle encounters an obstacle during driving, the dynamic programming algorithm will decide the optimal path to avoid the obstacle, as Figure 4 shown. By scattering points, the dynamic programming realizes the discretization of the space and uses a quintic polynomial curve to smoothly connect the sampling points to form path clusters. Subsequently, based on a reasonable cost function, the path with the lowest cost is selected as the optimal path to achieve the best obstacle avoidance decision.

[0097] Before path planning, it is necessary to discretely sample the continuous space to reduce the complexity of the path planning algorithm. The dynamic programming algorithm performs discrete sampling in both the horizontal and vertical directions, as Figure 4 shown.

[0098] Therefore, the initial sampling point set uses equidistant sampling in the horizontal and vertical directions respectively, and the expressions are as follows:

[0099] S(k) = S(1) + (k - 1) * ds, k = 1, 2,..., m (7)

[0100] L(k) = L(1) + (k - 1) * dl, k = 1, 2,..., n (8)

[0101] In the formula, S(k) represents the S-axis coordinate of the k-th point; L(k) represents the L-axis coordinate of the k-th point; m and n respectively represent the number of longitudinal sampling points and the number of horizontal sampling points; dl represents the horizontal sampling distance; ds represents the longitudinal sampling distance.

[0102] The sampling interval is determined by experience. Usually, the planning layer plans the path for the next 5 - 8 seconds. In this embodiment, the trajectory for the next 8s is selected. The total longitudinal sampling length is 60m. The simulation scenario has a total of 2 lanes, and the width of each lane is 3.5m. The selected horizontal sampling distance is dl = 1m, and the longitudinal sampling distance is ds = 4m.

[0103] The purpose of using the dynamic programming algorithm to process the initial sampling point set is to find an optimal decision path and select the optimal feasible path set. The essence of this process is to find the global optimal solution of the lateral coordinate function l = f(s) in the non-convex space of the SL coordinate system.

[0104] The specific steps are as follows:

[0105] When a vehicle encounters an obstacle during driving, there may be two decisions: avoiding to the left or to the right, which may result in multiple local optimal solutions. Therefore, the trajectory planning is decomposed into two key steps to solve the optimal solution: First, based on dynamic programming, a preliminary path planning is carried out to determine a rough path, and the drivable area and the decision of lateral avoidance are determined based on this rough path; Then, quadratic programming is used to further smooth and optimize the rough path to generate a globally optimal planned trajectory.

[0106] First, starting from the first column (usually the column where the starting point is located), calculate the cost of each sampling point in each row. At the same time, record the parent node information of each sampling point, that is, from which previous sampling point it is transferred.

[0107] Then, enter the calculation of the second column. For each sampling point in the second column, consider all possible parent nodes in the first column, and calculate the cost of transferring from the parent node to the current point. This cost includes the path cost from the parent node to the current point and the self - cost of the current point. Select the parent node with the minimum cost as the optimal parent node of the current point, and update the minimum cost and parent node information of the current point. This process is carried out column by column until the last column is reached. In each step, the previously calculated minimum cost and parent node information are used to guide the calculation of the current column to ensure that the optimal transfer path is selected in each step. When the calculation reaches the last column, the minimum cost of all possible paths from the starting point to the target point has been obtained. At this time, the optimal path is constructed through a backtracking process. Starting from the sampling point in the last column, according to the recorded parent node information, gradually backtrack to the starting point. During the backtracking process, a quintic polynomial is used to connect the parent node and the current node in turn to form a complete path. As Figure 6 shown in the dynamic programming process.

[0108] The optimal feasible path set is selected by minimizing the objective function. In the dynamic programming, the objective function for evaluating the sampling path is composed of three parts: the smoothness cost function, the distance cost function from the obstacle, and the distance cost function from the reference line. These three cost functions are used to encourage the generation of smooth trajectories, trajectories far from obstacles, and trajectories along the reference line, specifically:

[0109] The formula of the smoothness cost function is as follows:

[0110]

[0111] In the formula, ω1∫(f′(s) 2 ds represents the difference between the vehicle's heading and the planned route; It may be proportional to the square of the vehicle speed and is used to encourage the vehicle to drive along the planned route; ω2∫(f″(s)) 2ds represents the curvature of the path; it is proportional to the square of the vehicle jerk, used to avoid overly complex paths and improve driving stability; ω3∫(f″′(s)) 2 ds represents the rate of change of the path curvature; it is proportional to the square of the vehicle jerk, used to avoid overly complex paths and improve driving stability;

[0112] The distance cost to the obstacle discretizes the path into a series of {s1, s2,..., s n} to solve the sum of the costs of each point to the obstacle. Assuming the distance between the obstacle and the ego vehicle is d, then the formula for the distance cost function to the obstacle is as follows:

[0113]

[0114] In the formula, to ensure that the ego vehicle does not collide with the obstacle, [d c , d n represents the distance buffer; when the distance d > d n , there will be no collision between the vehicle and the obstacle, and the cost function is 0; when the distance d < d c , the vehicle may be at risk of colliding with the obstacle, which is not allowed in the actual driving process, so its cost function is set to C collision , and its value is infinite; in the buffer, the cost function C nudge is a monotonically decreasing function. The closer to the obstacle, the higher the risk of collision, and the greater the cost value; the farther away from the obstacle, the safer the driving, and the smaller the cost value;

[0115] The reference line is used to guide the driving direction of the vehicle. In the obstacle-free area, it is usually desired that the driverless vehicle follows the reference line. Usually, the center line of the road is used as the reference line. Let the reference line function be g(s), then the formula for the distance cost function to the reference line is as follows:

[0116]

[0117] In the formula, f(s) represents the lateral coordinate function l = f(s) in the Frenet coordinate system; g(s) represents the reference line function; ds represents the differential of the longitudinal coordinate s in the Frenet coordinate system, and the differential element in the integral represents.

[0118] The smoothness cost, the distance cost to the obstacle, and the distance cost to the reference line are combined into a total objective function to evaluate the quality of each sampled path. Therefore, the optimal feasible path set is selected by minimizing the objective function. This path achieves the best balance in terms of smoothness, safety (away from obstacles), and following the reference line. The formula for minimizing the objective function is:

[0119]

[0120] Where: C DPtotal The function \(f\) represents the smoothness cost function, which is used to encourage the generation of smooth trajectories; represents the distance cost function from obstacles, which is used to encourage the generation of trajectories far from obstacles; represents the distance cost function from the reference line, which is used to encourage the vehicle to travel along the reference line;

[0121] In the second step, a quintic polynomial curve is used to smoothly connect the sampling points obtained in the first step to form a path cluster. That is, in the optimal feasible path set, a distance estimation function between the current sampling point and the obstacle trajectory point is added to the objective function, and the optimal feasible set is selected by using the dynamic programming method. Through the method of scattering points, dynamic programming realizes the discretization of space, and uses the quintic polynomial curve to smoothly connect the sampling points to form a path cluster. And based on the cost function, the path with the lowest cost is selected as the preliminary optimal path;

[0122] Due to its good smoothness and flexibility, the quintic polynomial curve is used to smoothly connect the sampling points to form a path cluster. This curve can effectively constrain the acceleration and jerk of the trajectory to ensure the smoothness of the trajectory. And each path in the path cluster is evaluated based on the cost function, and the path with the lowest cost is selected as the preliminary optimal path. The cost function comprehensively considers the smoothness of the path, the distance from obstacles, and the deviation from the reference line, ensuring that the preliminary optimal path achieves a good balance in terms of safety and smoothness.

[0123] Commonly used trajectory generation methods include sine function curves, Bezier curves, and polynomial curves. Sine curves usually can only simulate the driving of vehicles on straight or simple curve trajectories. For complex vehicle trajectory changes, the effect may not be good, and they cannot well adapt to various complex road conditions and vehicle driving behaviors. The mathematical representation of Bezier curves is relatively complex and requires more control points to fit complex trajectories. For some specific-shaped trajectories, Bezier curves may not be able to accurately fit. Polynomials have high flexibility and adaptability and can fit curves of various shapes, including complex vehicle trajectory changes. The fitting accuracy can be increased by increasing the order of the polynomial to meet different accuracy requirements. Therefore, the present invention selects the quintic polynomial curve as the vehicle trajectory model.

[0124] Such as Figure 5 For quintic polynomial connection, it can be found that when using quintic polynomial connection, the curve is gentle and not prone to sudden changes. The expression of the quintic polynomial curve is:

[0125] l = f(s) = a0 + a1s + a2s 2 + a3s 3 + a4s 4 + a5s5 (9)

[0126] In the formula, l represents the lateral position in the vehicle Frenet coordinate system; f(s) is a function of the variable s, representing the position of the vehicle on the trajectory; a0, a1, a2, a3, a4, a5 respectively represent the coefficients of the fifth-degree polynomial; s is the independent variable, representing the longitudinal position in the vehicle Frenet coordinate system.

[0127] Substituting the state information [s0, l0, l0′, l0″] of the starting point into equation (9), we get:

[0128]

[0129] Then the parameters of the fifth-degree polynomial curve can be expressed in matrix form as:

[0130]

[0131] Denoted as: SA = L, then the parameters of the fifth-degree polynomial curve are: A = S -1 L.

[0132] During the process of point scattering, taking the vehicle's center of mass as the starting point, uniform points are scattered forward at intervals of ds. The selection of the state between the vehicle and the first column of scattered points is different from that of the sampling points after the first column. To ensure the smoothness of the planned path, in this embodiment, the starting state of the vehicle to the first column of sampling points is selected as the current state of the vehicle, that is, s0 = s, l0 = l, l0′ = tanθ, l″ 0| = 0, and the end state is the first column of sampling points. Take l′ and l″ of each sampling point to be 0.

[0133] The core of quadratic programming is to seek an optimal solution under the linear constraint conditions of multiple variables, so that a quadratic function composed of these variables reaches the minimum or maximum value. Due to the existence of this quadratic function, quadratic programming is classified as a special non-linear programming problem.

[0134] The first part of the quadratic programming cost function is the smoothing cost:

[0135]

[0136] In the formula, ω1 and ω2 represent weight coefficients, l”(s) and l”'(s) are the second derivative and the third derivative of the lateral offset with respect to the longitudinal offset respectively. They represent the change rate of the change rate of the lateral offset of the vehicle relative to the center line of the road, that is, the acceleration of the lateral offset, and the change rate of the lateral offset acceleration of the vehicle relative to the center line of the road, that is, the jerk of the lateral offset. By constraining the change rate of the lateral movement offset and the acceleration change rate of the vehicle, the planned trajectory can be made smoother, minimizing unnecessary sharp turns or sudden lane changes to the greatest extent, thereby improving the comfort of passengers; ds is the microelement representation in the integral, and s represents the longitudinal position in the Frenet coordinate system.

[0137] Expand l”(s) with a fifth-degree polynomial and let X = [a s,0 , a s,1 , a s,2 , a s,3 , a s,4 , a s,5 T , then ∫(l”(s)) 2 ds can be expressed as:

[0138] ∫(l”(s)) 2 ds = X T H1X (17)

[0139] The expression of H1 can be obtained by calculation:

[0140]

[0141] Similarly, by performing the same operation on ∫(l”'(s)) 2 ds = X T H2X, H2 can be obtained:

[0142]

[0143] Step 3: Use the quadratic programming algorithm to further optimize the preliminary optimal path. The cost function of the quadratic programming includes the cost term of the predicted trajectory of surrounding vehicles. The purpose of constructing this cost term is to enable the planning algorithm to take future information into account during the process of generating the planned trajectory, so as to avoid sudden situations or potential collisions in advance, making the algorithm more "predictive". The cost term is constructed by solving the distance function between all planned points in the discrete space and the predicted trajectory points of surrounding vehicles corresponding to each planned point, and the negative value of the distance function is added to the cost function of the quadratic programming to maximize the distance between the ego vehicle and the surrounding vehicles within the prediction time domain, realizing the early avoidance of potential collision risks and ensuring the safety and reliability of the trajectory planning result in the dynamic traffic scenario. ​

[0144] The quadratic programming algorithm is used to further optimize the preliminary optimal path. The specific steps of including the cost term of the predicted trajectories of surrounding vehicles in the cost function of quadratic programming are as follows:

[0145] The second part of the quadratic programming cost function is the cost function for the predicted trajectories of surrounding vehicles. The goal of constructing this cost function is to enable the planning algorithm to take future information into account during the process of generating the planned trajectory, so as to avoid sudden situations or potential collisions in advance, making the algorithm more "predictive".

[0146] Assume that the predicted trajectories of surrounding vehicles are:

[0147]

[0148] In the formula, m represents the number of predicted trajectory points, and k represents the predicted trajectory of the k-th surrounding vehicle.

[0149] In the discrete space, the planned trajectory can be expressed as:

[0150] [S, L] = [(s(1), l(1)), (s(2), l(2))...(s(n), l(n))] (21)

[0151] In the formula, n represents the number of planned trajectory points. To ensure that the planned trajectory has no collisions within the planning time domain and the prediction time domain, each planned point needs to be as far away as possible from the predicted trajectory points at the corresponding time. The negative of the distance function is added to the formula of the quadratic programming cost function as follows:

[0152]

[0153] In the formula, ω3 is the weight coefficient, i represents the i-th planned point, i proj represents the index of the predicted trajectory point of the surrounding vehicle at the same time as the i-th planned point, s proj represents the index of the predicted trajectory point of the surrounding vehicle at the same time as the planned point at the longitudinal distance s, and K represents the total number of surrounding vehicles. By minimizing C QPobs_pre , that is, maximizing the distance between each planned point within the planning time domain and the predicted trajectory points of the surrounding vehicles at the corresponding time, the avoidance of future potential obstacles is achieved.

[0154] Equation (21) can be expanded using a fifth-degree polynomial and written as:

[0155] C QPobs_pre =-X T H3X + 2f3X - C (23)

[0156] During the optimization process, the constant term C can be discarded, and then the expressions of H3 and f3 can be obtained:

[0157]

[0158] In Equation (25), f3 is the gradient vector in the construction of the cost of the predicted trajectory of the surrounding vehicle; represents the lateral coordinate of the predicted trajectory of the k-th surrounding vehicle corresponding to the index of the predicted trajectory point of the surrounding vehicle at the same moment as the planned point at the longitudinal distance s (see Equation (20) for details); s represents the longitudinal coordinate of the planned trajectory.

[0159] In the quadratic programming process, the constraint conditions are the key part to ensure that the optimization result conforms to the actual driving scenario.

[0160] The constraint conditions include boundary constraints and dynamic feasibility constraints. The boundary constraints are used to ensure that the path is within the road boundaries, and the dynamic feasibility constraints are used to ensure that the path meets the dynamic and kinematic requirements of the vehicle, such as Figure 6 shown.

[0161] Assume that the driverless vehicle makes an evasive maneuver to the left. According to the road boundary and obstacle information, a drivable space, i.e., a convex space, is generated on the left side of the road. Each point on the planned path should be restricted within the drivable space, that is, the lateral displacement l i at the longitudinal displacement s i should satisfy:

[0162] l i ∈[l mini , l maxi (26)

[0163] In the formula, l i represents the lateral displacement at the longitudinal displacement s i ; l min represents the minimum value of the lateral displacement, that is, the lower limit of the lateral position where the vehicle can safely drive on the road; l max represents the maximum value of the lateral displacement, that is, the upper limit of the lateral position where the vehicle can safely drive on the road;

[0164] l i ″ is the rate of change of l i ′, similar to the lateral acceleration in the natural coordinate system. The constraint is:

[0165] l i ″ ∈[l″ min , l″ max (27)

[0166] In the actual driving environment, neither the host vehicle nor the obstacle vehicle is a particle model. It is necessary to consider the size of the vehicle to avoid the edges of the vehicle from colliding with the obstacle vehicle or driving outside the road boundary. The models of the host vehicle and the obstacle vehicle are approximated as rectangles with a length of 4.8 m and a width of 2 m. From Figure 6 it can be seen that when the vehicle reaches point s on the path at time i i , the body of the vehicle must not collide with the road boundary or obstacles. Considering the limitation of the vehicle body shape, the lateral coordinate displacement is restricted to the maximum value ub i and the minimum value lb i in the range [s i -d2 - w / 2, s i +d1 + w / 2]. That is, the upper boundary l max has a minimum value of ub i in the interval [s i -d2 - w / 2, s i +d1 + w / 2], and the lower boundary l min has a maximum value of lb i in the interval [s i -d2 - w / 2, s i . Then the inflection points P1, P2, P3, and P4 at the four corners of the vehicle need to satisfy the following constraints:

[0167]

[0168] where w is the width of the vehicle, d1 is the distance from the vehicle's center of mass to the front end of the vehicle, and d2 is the distance from the vehicle's center of mass to the rear end of the vehicle. Since θ is relatively small, the above formula is approximated as:

[0169]

[0170] where l i represents the lateral displacement at the longitudinal position s i ; d i l i ′ represents the first derivative of the lateral offset with respect to the longitudinal displacement at the longitudinal position s i , that is, the lateral velocity; w represents the width of the vehicle; lb i and ub i represent the lower and upper bounds of the lateral displacement at the longitudinal position s i , respectively.

[0171] Convert Equation (28) into matrix form:

[0172]

[0173] Denoted as b sub_li ≤ A subi xi ≤ b sub_ui Similar restrictions are imposed on each point on the path to obtain the inequality constraint Ax ≤ b.

[0174] When solving QP, the DP planned path is discretized, and dynamic feasibility constraints are imposed during the quadratic programming process to ensure that two adjacent path points should remain continuous and smooth. Then:

[0175]

[0176] In the formula, l i represents the lateral displacement at the longitudinal position si at the current moment; l i+1 represents the lateral displacement at the longitudinal position si at the previous moment of the current moment; l i-1 represents the lateral displacement at the longitudinal position si at the next moment of the current moment; △s represents the microelement of the longitudinal displacement, that is, the tiny change amount.

[0177] Denote formula (31) as A eq_subi x i = b eq_subi . By restricting the points on the entire path, the equality constraint of the quadratic programming can be obtained, Ax = b eq x = b eq . Finally, call the quadratic programming solver in MATLAB / Simulink to solve this optimization problem.

[0178] Perform the following experiments through the above-mentioned trajectory planning method for autonomous vehicles that combines surrounding wheel prediction information:

[0179] Experimental design of this embodiment: Build a structured road lane-changing scenario in Prescan, write lane-changing decision-making, trajectory planning, and tracking control algorithms in Simulink, and send the calculated control quantities to the CarSim vehicle dynamics module to achieve closed-loop simulation. For different scenarios, compare multiple indicators to verify the effectiveness of the algorithm of this embodiment.

[0180] To fully verify the effectiveness of the trajectory planning method of this embodiment in the scenario of borrowing a lane to overtake, two typical scenarios of borrowing a lane to overtake are designed on a standard two-lane road surface to test the planning situations of the planning algorithm of this embodiment and the algorithm without combining prediction information. The two experimental scenarios are as follows:

[0181] (1) The interfering vehicle in the left lane is driving normally

[0182] The scenario is set as Figure 8 (a) and (b) of, after the ego vehicle accelerates from 0 to the desired speed close to Car1, perform the action of borrowing a lane to overtake. During the lane change, there is an interfering vehicle Car2 in the left lane. The setting information of each vehicle is shown in Table 5-1.

[0183] Scenario where the interfering vehicle in the left lane disrupts the normal driving of vehicles

[0184]

[0185] The speed changes of Car1 and Car2 are as Figure 10 :

[0186] Car1 slows down slowly in the range of 10 m / s to 15 m / s, and Car2 basically maintains a constant speed.

[0187] (2) The interfering vehicle in the left lane has a tendency to approach the host vehicle

[0188] The scenario is set as in Figure 8 (a) and (b). After the host vehicle accelerates from 0 to the desired speed close to Car1, it performs a lane-changing overtaking maneuver. During the lane change, there is an interfering vehicle Car2 in the left lane, and Car2 has a tendency to approach the host vehicle. Compared with scenario (2), the interference of vehicles in this scenario environment is more obvious, and it can better test the adaptability of the algorithm to changes in the dynamic traffic environment. The setting information of each vehicle is shown in Table 5-2.

[0189] Table 5-2 Scenario settings where the vehicle in the left lane has a tendency to approach the host vehicle lane

[0190]

[0191] The speed changes of Car1 and Car2 are as Figure 10 shown.

[0192] Car1 decelerates slowly in the first 100 m and then accelerates slowly in the range of 10 m / s to 15 m / s in the second half. Car2 accelerates throughout the process, and the speeds of both vehicles are less than the desired speed of the host vehicle.

[0193] By analyzing the experimental results of the above two experimental scenarios, the effectiveness of the path planning in this embodiment is verified.

[0194] 1. Scenario where the interfering vehicle in the left lane drives normally:

[0195] Visualize and analyze the prediction results of multiple frames, as Figure 11 . Car1 and Car2 drive along their respective lanes with little lateral displacement. The predicted trajectory and the real trajectory basically coincide horizontally, indicating that the algorithm predicts the lateral movement accurately in this scenario. It is observed that at the beginning, there is a large gap between the longitudinal position of the predicted trajectory of Car2 and the real trajectory, but as time goes by, the gap gradually decreases because the trajectory prediction results are updated in real time. As time passes, the historical data is continuously updated, and the error also gradually decreases.

[0196] Generally speaking, the prediction results at times t1 to t3 are relatively accurate. After t3, the error is relatively large, which is a normal phenomenon because as the prediction horizon increases, the uncertainty also increases, making the prediction task more difficult. However, since the planning task and the prediction task update the results in real time, the data at time points too far from the current time within the prediction horizon has a relatively small impact, so it will not have a significant impact on the planning results.

[0197] Figure 12 For the comparison of the planning results of the two planning methods, as can be seen from the figure, after combining the prediction information, the algorithm plans a more conservative trajectory, specifically manifested as follows: at 150 - 200m, that is, before lane change; at 250 - 300m, during lane change; at 350 - 400m, that is, during the process of turning back to the original lane after lane change. Compared with the planning algorithm without combining prediction information, there are respectively 0.36m and 0.74m more lateral displacements, and 6.57m more longitudinal displacement.

[0198] Figure 13 (a) and (b) of Figure 14 are respectively the scenarios when the ego - vehicle approaches Car2 and when it approaches Car1. As can be seen from Figure 13 (a) and (b), when approaching Car2, although there is no obvious collision within the prediction horizon, the planning algorithm still gives a relatively conservative result. Compared with the result without prediction information, there is 0.38m more lateral offset at the future time t1 to prevent potential risks. In Figure 14 , when approaching the leading vehicle Car1, the method without combining prediction information, although it plans a trajectory without collision with Car1 at the current time, but near the future time t2, the lateral distance between the planned trajectory and the Car1 trajectory is only 1.29m, and the collision risk is relatively large. However, after combining the prediction information, the future - time trajectory of Car2 has been considered in the planning cost, that is, the algorithm has the ability to anticipate potential risks. Therefore, near the risk point t2, the lateral distance from the Car1 trajectory increases to 2m, reducing the potential collision risk.

[0199] Figure 15 For the comparison of the lane - change starting points of the two methods. As can be seen from the figure, the trajectory given by the method without combining prediction information is only 2.58m away from Car2 at the lane - change starting point. When cutting into the target lane, due to the too - short longitudinal distance, there is a collision risk. While for the planning method that combines prediction information, compared with the former, the lane - change point is postponed, and the longitudinal distance from Car2 increases to 6.63m. In contrast, the risk is smaller.

[0200] Figure 16For the lateral acceleration comparison of the two methods, it can be clearly seen from the figure that the lateral acceleration obtained by the planning method combined with prediction information is smoother and has no sudden changes. Since larger lateral and longitudinal offsets are generated before, during, and after lane changes to ensure safety, the acceleration peak is slightly larger than that of the method without combined prediction information, but the maximum value does not exceed 1.5 m / s^2, which is within an acceptable range.

[0201] Scenario where the vehicle in the left lane has a tendency to approach the host vehicle

[0202] Visualize and analyze the prediction results of multiple frames, such as Figure 17 , it can be seen from the figure that Car2 has a tendency to approach the host vehicle's lane. At this time, the prediction algorithm can still predict the general trend of its movement. From time t1 to t3, the prediction results are relatively accurate. After time t3, the prediction results of the longitudinal displacement have a large deviation because as the prediction time domain increases, the uncertainty also increases, and the prediction task becomes more difficult. However, since the planning task and the prediction task update the results in real time, the data at time points too far from the current time within the prediction time domain have little impact, so it will not have a great impact on the planning results.

[0203] Figure 18 For the comparison of the planned trajectory results of the two methods, similar to Scenario 2, after combining the prediction information, the algorithm gives more conservative results, specifically manifested as follows: at 100 - 150 m, that is, before lane change, at 250 - 300 m, that is, during lane change, and at 350 - 400 m, that is, when returning to the original lane after lane change, compared with the results without combined prediction information, there are additional lateral displacements of 0.2 m and 0.31 m, and a longitudinal displacement of 30.79 m. As known from 5.2, Car1 accelerates throughout the process and the maximum speed reaches 17.74 m / s, approaching the host vehicle's expected speed of 22 m / s. Therefore, to ensure safety, a large longitudinal displacement is generated during the stage of returning to the original lane.

[0204] Figure 19 For the visualization result of the scenario when Car2 first shows the intention to approach the host vehicle's lane, it can be seen from the figure that although the prediction algorithm gives the general trend of Car2's movement direction, as the prediction time domain increases, the deviation becomes larger and larger. In this scenario, the algorithm without combined prediction information can only obtain the state information of Car2 at the current moment, so it does not react to its behavior of approaching along the host vehicle's lane and can only drive along the DP reference line; while after combining the prediction information, the algorithm has the ability to anticipate Car2's behavior, and the reaction is that the planned trajectory has a lateral offset near time t1, improving safety.

[0205] At the same time, it can be observed that the true trajectory of Car2 is closer to the ego vehicle's lane. In this case, if no response is made and the vehicle continues to drive along the reference line or continue the lane-changing operation, there may be a collision risk. In this scenario, the effectiveness of the planning algorithm of this embodiment can be clearly seen.

[0206] Figure 20 For the visualization of the planned trajectory results when the ego vehicle approaches Car1, by comparing the trajectories of the two methods during the process of planning to return to the ego lane, that is, around the time t2, it can be seen that the method combining prediction information can always maintain a greater lateral distance from the obstacle within the prediction time domain compared to the method without combining prediction information, and has higher safety.

[0207] Figure 21 For the comparison of the lateral acceleration changes of the two methods, the method without combining prediction information can only plan based on real-time state information and cannot handle some sudden situations. Therefore, the generated lateral acceleration curve has obvious jitters. In contrast, due to the combination of prediction information, the algorithm can predict the driving state of the surrounding vehicles, so the situation of sudden changes in lateral acceleration is greatly reduced, that is, driving along the planned trajectory is more comfortable and stable.

[0208] Perform a scenario visualization analysis on the moment when the acceleration of the method without combining prediction information mutates at 100 - 150m, as Figure 22 shown. It can be seen from the figure that the distance between the ego vehicle and Car2 is very close. At the current moment, the decision reference line given by the DP algorithm based on real-time environmental information has an obvious lateral offset at the current position (the red curve at 0 - 10m), that is, it is necessary to immediately turn right to avoid Car2. It is precisely due to this instantaneous decision that the method without combining prediction information generates a mutated acceleration. Moreover, observing the 40 - 60m section in the figure, affected by the instantaneous change, the planned trajectory mutates; in contrast, the method combining prediction information makes a lateral avoidance in advance before this moment, so the lateral acceleration does not mutate, and the planned trajectory does not mutate either, verifying the effectiveness of the prediction information.

Claims

1. An autonomous vehicle trajectory planning method combined with surrounding vehicle prediction information, characterized in that It includes the following steps: Step 1: Randomly sample based on the road boundary in the Frenet coordinate system to generate an initial sampling point set for trajectory planning, and use the dynamic programming algorithm to process the initial sampling point set to select the optimal set of feasible paths; Step 2: Smoothly connect the sampling points obtained in Step 1 using a quintic polynomial curve to form a path cluster, and select the path with the lowest cost as the preliminary optimal path based on the cost function; Step 3: Further optimize the preliminary optimal path using the quadratic programming algorithm. The cost function in the quadratic programming includes a cost term for the predicted trajectories of surrounding vehicles. The cost term is constructed by solving the distance function between all planned points in the discrete space and the predicted trajectory points of surrounding vehicles corresponding to each planned point, and the negative of the distance function is added to the cost function of the quadratic programming to maximize the distance between the ego vehicle and the surrounding vehicles within the prediction time domain.

2. The method according to claim 1, wherein The Frenet coordinate system described in Step 1 is obtained by converting from the Cartesian coordinate system, and the conversion formula is as follows: s = s r (1) l′=(1 - k r l)tan(θ x - θ r ) (5) In the formula, s represents the longitudinal displacement; represents the longitudinal velocity; represents the longitudinal acceleration; v x represents the vehicle's lateral velocity component; θ x represents the vehicle's heading angle, that is, the angle between the vehicle's forward direction and the reference line; θ r represents the azimuth angle of the reference line, usually the angle between the road tangent direction and the horizontal direction; k r represents the road curvature, indicating the degree of road bending; l represents the lateral displacement, indicating the vehicle's lateral distance relative to the reference line; a x represents the vehicle's lateral acceleration; l′ represents the first derivative of the lateral displacement with respect to the longitudinal displacement, that is, the lateral velocity; k x represents the curvature of the vehicle's trajectory; k′ x represents the derivative of the vehicle's trajectory curvature, that is, the curvature change rate; l″ represents the second derivative of the lateral displacement, that is, the lateral acceleration; x x ,y x represents the vehicle's position coordinates in the Cartesian coordinate system; x r ,y r represents the position coordinates of a point on the reference line in the Cartesian coordinate system; sign represents the sign function, which is used to determine the direction of the lateral displacement.

3. The method according to claim 1, wherein In Step 1, the initial sampling point set uses equidistant sampling in the horizontal and vertical directions respectively, and the expressions are as follows: S(k) = S(1) + (k - 1) * ds, k = 1, 2,..., m (7) L(k) = L(1) + (k - 1) * dl, k = 1, 2,..., n (8) In the formula, S(k) represents the S-axis coordinate of the k-th point, L(k) represents the L-axis coordinate of the k-th point, m and n respectively represent the number of longitudinal sampling points and transverse sampling points; dl represents the transverse sampling distance; ds represents the longitudinal sampling distance.

4. The method according to claim 1, characterized in that The optimal set of feasible paths described in Step 1 is selected by minimizing the objective function, and the formula for the minimizing objective function is: Where: C DPtotal f represents the smoothness cost function, which is used to encourage the generation of smooth trajectories; represents the distance cost function with respect to obstacles, which is used to encourage the generation of trajectories far from obstacles; represents the distance cost function with respect to the reference line, which is used to encourage the vehicle to drive along the reference line; The objective function includes a smoothness cost function, a distance cost function from obstacles, and a distance cost function from the reference line; The formula for the smoothness cost function is as follows: where, ω1∫(f′(s) 2 ds represents the difference between the vehicle's heading and the planned route; it may be proportional to the square of the vehicle speed and is used to encourage the vehicle to travel along the planned route; ω2∫(f″(s)) 2 ds represents the curvature of the path; it is proportional to the square of the vehicle jerk and is used to avoid overly complex paths and improve driving stability; ω3∫(f′″(s)) 2 ds represents the rate of change of the path curvature; it is proportional to the square of the vehicle jerk and is used to avoid overly complex paths and improve driving stability; The formula for the distance cost function from obstacles is as follows: where, [d c , d n represents the distance buffer. When the distance d > d n , no collision will occur between the vehicle and the obstacle, and the cost function is 0; when the distance d < d c , there may be a risk of collision between the vehicle and the obstacle, which is not allowed in the actual driving process. Therefore, its cost function is set to C collision , and its value is infinity; in the buffer, the cost function C nudge is a monotonically decreasing function. The closer to the obstacle, the easier it is to have a collision risk, and the greater its cost value. The farther away from the obstacle, the safer the driving, and the smaller the cost value; The formula for the distance cost function from the reference line is as follows: In the formula, f(s) represents the lateral coordinate function l = f(s) in the Frenet coordinate system; g(s) represents the reference line function; ds represents the differential of the longitudinal coordinate s in the Frenet coordinate system, and the differential element in the integral represents.

5. The method according to claim 1, wherein The formula for smoothly connecting the sampling points obtained in Step 1 using a quintic polynomial curve in Step 2 is as follows: l = f(s) = a0 + a1s + a2s 2 + a3s 3 + a4s 4 + a5s 5 (9) In the formula, l represents the lateral position of the vehicle in the Frenet coordinate system; f(s) is a function of the variable s, representing the position of the vehicle on the trajectory; a0, a1, a2, a3, a4, a5 respectively represent the coefficients of the quintic polynomial; s is the independent variable, representing the longitudinal position of the vehicle in the Frenet coordinate system.

6. The method according to claim 1, characterized in that, The formula for adding the negative of the distance function to the cost function of the quadratic programming in Step 3 is as follows: Where ω3 is the weight coefficient, i represents the i-th planning point, and i proj represents the index of the predicted trajectory point of the surrounding vehicle at the same time as the i-th planning point, s proj represents the index of the predicted trajectory point of the surrounding vehicle at the same time as the planning point at the longitudinal distance s, and K represents the total number of surrounding vehicles; by minimizing C QPobs_pre , that is, maximizing the distance between each planning point within the planning horizon and the predicted trajectory point of the surrounding vehicle at its corresponding time, to achieve avoidance of potential future obstacles.

7. The method according to claim 1, characterized in that, In the quadratic programming process in Step 3, boundary constraints are imposed, and the boundary constraints include the lateral displacement l i at the longitudinal displacement s i The formula for satisfying the road boundary conditions is as follows: l i ∈[l mini ,l maxi ](26)In the formula, l i Indicates the longitudinal displacement s i The lateral displacement at min Indicates the minimum value of lateral displacement, that is, the lower limit of the lateral position at which the vehicle can safely travel on the road; l max It represents the maximum value of lateral displacement, that is, the upper limit of the lateral position at which the vehicle can travel safely on the road; Considering the size of the vehicle, to avoid the edges of the vehicle from colliding with the road boundary or obstacles, its constraint formula is: where l i represents the lateral displacement at the longitudinal position s i ; d i l i ′ represents the first derivative of the lateral offset with respect to the longitudinal displacement at the longitudinal position s i , that is, the lateral speed; w represents the width of the vehicle; lb i andub i Respectively represent the vertical position s i The lower and upper bounds of the lateral displacement at .

8. The method according to claim 1, wherein In Step 3, the quadratic programming process imposes dynamic feasibility constraints, and the dynamic feasibility constraints include ensuring the continuity and smoothness between path points, and its constraint formula is: where, l i represents the lateral displacement at the longitudinal position si at the current moment; l i+1 represents the lateral displacement at the longitudinal position si at the moment before the current moment; l i-1 represents the lateral displacement at the longitudinal position si at the moment after the current moment; △s represents the infinitesimal element of the longitudinal displacement, that is, the infinitesimal change amount.

Images