Automatic driving path planning method based on numerical optimization

Abstract

In recent years, research on vehicle path planning mainly comprises methods based on searching, sampling and numerical optimization. A planning problem is converted into graph search based on a search method, but the efficiency is relatively low in high dimension. The sampling method is suitable for a high-dimensional space, candidate paths are generated through random or regular sampling, but the calculation complexity is high. Compared with the prior art, according to the numerical optimization method, a path is represented through a polynomial curve, a problem is converted into a constrained optimization problem, and necessary constraint conditions are added. According to the method, the path smoothness and the solving speed can be improved, so that the method is widely accepted in academic and industrial circles. According to the path planning method based on numerical optimization, a convex polygon is used for covering a sampling point feasible region, redundant regions are reduced, and the space utilization rate is improved, so that obstacle avoidance constraints and a mathematical model are simplified, and the planning efficiency of an automatic driving automobile is improved.

Description

Technical Field

[0001] The present invention relates to the field of autonomous driving path planning, and designs an autonomous driving path planning method based on numerical optimization. Under this planning method, the driving vehicle can generate a smooth route to avoid obstacles, thereby realizing autonomous driving. Background Art

[0002] The development of autonomous driving technology began in the mid-20th century, but the real breakthrough came in the early 21st century. The earliest prototypes of autonomous vehicles were mostly developed by research institutions and universities, relying mainly on rule-driven systems and simple sensor technology. With the participation of technology giants such as Google, Tesla, and Uber, autonomous driving technology has developed rapidly. Autonomous driving is believed to improve traffic safety, reduce traffic accidents, optimize traffic flow, reduce energy consumption and environmental pollution, and may also change future urban planning and lifestyles.

[0003] Autonomous driving technology has many advantages over human drivers. First, autonomous vehicles can perceive the surrounding environment with high efficiency and quickly obtain the information needed for the driving route, which is better than humans. Second, autonomous vehicles can reasonably plan the driving route according to the predicted traffic conditions and choose the best path during driving, so as to effectively avoid congestion and busy roads. Finally, autonomous vehicles can also predict the driving direction of surrounding vehicles and avoid potential risks and dangers in advance by adjusting their own driving trajectory.

[0004] In recent years, the research on vehicle path planning mainly includes search-based methods, sampling-based methods, and numerical optimization-based path methods. Among them, the search-based local path planning method transforms the planning problem into a graph search problem, grids the solution space, builds a graph, and then searches and solves the optimal path from the starting point to the target point. The search-based planning method can obtain the optimal solution at the global level through continuous iterative search, but its search efficiency is related to the state dimension of the planning problem. When the dimension is high, the search process is time-consuming, so it is only suitable for solving planning problems with lower dimensions. The sampling-based path planning method is more suitable for path search problems in high-dimensional space. This method generates candidate paths by randomly or regularly sampling in the search space, and then selects an optimal path from these candidate paths to guide the vehicle from the starting point to the end point. However, with the improvement of the sampling speed resolution, its solution time will increase significantly, requiring more powerful computing power. When changing scenes, unreasonable cost function selection will also lead to poor stability of trajectory planning. For autonomous vehicles, it is crucial for tracking control that the planned path is smooth enough, and the results obtained by search or sampling methods are difficult to meet the vehicle kinematic and dynamic requirements. The planning method based on numerical optimization can express the planning results through polynomial curves, that is, the path planning problem is transformed into the problem of solving curve parameters. In this process, the constraints required for vehicle driving planning can be added, and finally the path planning problem is abstracted into a constrained optimization problem. The algorithm based on this method has been recognized and deeply studied by academia and industry.

[0005] Therefore, the present invention designs a path planning method based on numerical optimization. By constructing a series of path points, all obstacle avoidance constraints in the environment are converted into constraints for seeking the minimum value of the cost function, eliminating the correlation between the scale of the optimal control problem and the complexity of the environment. The feasible domain of the sampling points is covered by convex polygons, the redundant area is reduced, the space utilization rate is improved, the obstacle avoidance constraints of the autonomous driving vehicle are effectively simplified, the mathematical model of the optimal control problem is simplified, and the speed of solving the planned route is improved. Summary of the invention

[0006] The main purpose of this invention is to design an automatic driving path planning method based on numerical optimization to overcome the shortcomings of the prior art, provide a safer, more comfortable and smoother driving trajectory for the automatic driving vehicle, and solve the problem that other automatic driving methods do not meet the constraints of the vehicle dynamics model and the vehicle kinematics model. The specific implementation steps are as follows:

[0007] Step 1: Calculate the route generated by the global path planning of the vehicle in the high-precision map as the reference line to be smoothed, obtain a series of discrete points for each point on the discrete reference line, and then smooth the adjacent discrete points to obtain the smoothed reference line;

[0008] Step 2: According to the coordinate information of the current vehicle position in the Cartesian coordinate system and the information of the discrete points on the reference line in the step 1 in the Cartesian coordinate system, find the discrete points closest to the vehicle position as matching points, and find the ideal projection point of the vehicle position in the Frenet coordinate system according to the information of the matching points in the Cartesian coordinate system;

[0009] Step 3: Calculate the coordinate information of the vehicle in the Frenet coordinate system based on the information of the ideal projection point in step 2. Similarly, obtain the coordinate information of the obstacle in the Cartesian coordinate system and calculate the coordinate information of the obstacle in the Frenet coordinate system;

[0010] Step 4: Discretely sample some points on the path to be traveled, perform dynamic programming, forward optimization, and reverse solution; forward calculate the minimum cost from the planning starting point to each longitudinal sampling interval, and record the lateral position information of the previous node of the node. If the minimum cost from the starting point to the jth longitudinal sampling interval is a, and the minimum cost from the jth to j+1 is b, then the minimum cost from the starting point to j+1 is a+b, and so on, until the end point; reversely search for each parent node from the end point to obtain the shortest path, forming a feasible solution area recorded as a convex space;

[0011] Step 5: Based on the convex space opened up by dynamic programming, quadratic programming finds the optimal solution in the convex space to generate the optimal path of the vehicle in Frenet coordinates;

[0012] Step 6: Further dynamic planning and secondary planning of the speed of the path points obtained by path planning are performed. Finally, a safe, smooth and comfortable obstacle avoidance route is generated;

[0013] Step 7: Convert the coordinate points on the obstacle avoidance route from the Frenet coordinate system to the Cartesian coordinate system and send them to the control module for execution.

[0014] Furthermore, the step 1 further includes:

[0015] First, let the three adjacent discrete points on the reference line be recorded as P in vertical order. 1r , P 2r and P 3r , the corresponding three smoothed points are recorded as P1, P2 and P3 respectively. 1r , P2r and P 3r If the three points are not collinear, they can form a parallelogram. The modulus of the diagonal of the parallelogram is |P 2r P0|, the smaller the modulus, the smoother the reference line; secondly, point P 1r The modulus length to point P1 |P 1r P1|Add point P 2r The modulus length to point P2 |P 2r P2|Add a little P 3r Modulus length to point P3 |P 3r The smaller the sum of P3|, the closer the smoothed path is to the original geometric path; thirdly, for the smoothed point, the square of the modulus length from point P1 to point P2 |P1P2| 2 Add the square of the modulus from point P2 to point P3 |P2P3| 2 The smaller it is, the more compact and uniform the smoothed points are. The total smoothing cost function formula is:

[0016]

[0017]

[0018] Among them, x i is the corresponding unsmoothed point x ir Smoothed horizontal axis, y i is the corresponding unsmoothed point y ir The smoothed ordinate is in the Cartesian coordinate system. W1 is the similarity cost between the point on the smoothed reference line and the original path point. W2 is the smoothing cost and W3 is the compact cost.

[0019] The above cost function can eventually be simplified into a quadratic programming function form, and the final simplified formula is as follows:

[0020]

[0021] Among them, A1 is the coefficient matrix of similarity cost, A2 is the coefficient matrix of smoothness cost, and A3 is the coefficient matrix of compact cost. The system matrices are all known; the final cost function can be simplified into the standard form of quadratic programming,

[0022]

[0023] lb≤x≤ub

[0024] Use a quadratic programming solver to find the optimal solution x that minimizes the cost function.

[0025] Furthermore, the step 2 further includes:

[0026] Starting from the starting point on the reference line, traverse each discrete point on the reference line, and according to the distance from each discrete point on the reference line to the current position of the vehicle, find the point with the shortest distance from the current position of the vehicle from the discrete points on the reference line and record it as the minimum matching point P m , from the origin of the Cartesian coordinate system to P m The position vector is denoted as From point P m The position vector to the current position of the vehicle is recorded as Click P m The direction vector at Then the position vector of the ideal projection point of the current vehicle in the Cartesian coordinate system is for:

[0027]

[0028] According to the minimum matching point P m The curvature k at m and the tilt angle θ m ,θ m is the angle between the tangent direction of the point and the x-axis in the Cartesian coordinate system, and the curvature k at the ideal projection point can be calculated r and the tilt angle θ r , when the distance between discrete points is not too large, it can be approximately considered that k m Equal to k r , the inclination angle θ of the ideal projection point r The calculation formula is:

[0029]

[0030] At this point, the information of the projection point of the point representing the vehicle position on the reference line in the Cartesian coordinate system: the horizontal coordinate, vertical coordinate, inclination angle and curvature have all been obtained. The calculation formula for the ideal projection point of the obstacle on the reference line is the same.

[0031] Furthermore, the step three also includes:

[0032] According to the information of the ideal projection point in step 2, the coordinate information of the vehicle in the Frenet coordinate system is calculated. The vertical coordinate of the vehicle in the Frenet coordinate system is s, and the first-order derivative of the vertical coordinate of the vehicle in the Frenet coordinate system with respect to time is The second-order derivative of the vehicle's ordinate with respect to time in the Frenet coordinate system is: The horizontal coordinate of the vehicle in the Frenet coordinate system is l, the derivative of the horizontal coordinate of the vehicle in the Frenet coordinate system with respect to the arc length ds is l′, the second-order derivative of the horizontal coordinate of the vehicle in the Frenet coordinate system with respect to the arc length ds is l″, and the derivative of the horizontal coordinate of the vehicle in the Frenet coordinate system with respect to time is The second-order derivative of the vehicle's horizontal coordinate with respect to time in Frenet coordinates is The calculation formulas are as follows:

[0033] s=s r

[0034]

[0035] Among them, s r The distance from the ideal projection point of the vehicle on the reference line to the planning starting point; is the position vector of the vehicle in the Cartesian coordinate system relative to the coordinate origin; is the velocity vector of the vehicle in the Cartesian coordinate system; is the acceleration vector of the vehicle in the Cartesian coordinate system; is the projection position vector; k r is the curvature of the projection vector on the road geometry in the Cartesian coordinate system; is the tangent vector of the projection position vector on the road geometry in the Cartesian coordinate system; is the projection position vector relative to the normal vector on the road geometry in the Cartesian coordinate system; the coordinate transformation of the obstacle in the Cartesian coordinate system and the Frenet coordinate system is the same.

[0036] According to the ordinate s and abscissa l of the static obstacle in Frenet coordinates, the SL diagram of the static obstacle is established; according to the ordinate s and abscissa l of the dynamic obstacle in Frenet coordinates and the derivative of the ordinate with respect to time and the derivative of the horizontal axis with respect to time Create ST diagrams for dynamic obstacles.

[0037] Furthermore, the step 4 further includes:

[0038] The paths between adjacent sampling points are connected using trajectories generated by a quintic polynomial. The quintic polynomial formula in the Frenet coordinate system is as follows:

[0039] l=a0+a1s+a2s 2 +a3s 3 +a4s 4 +a5s 5

[0040] l′=a1+2a2s+3a3s 2 +4a4s 3 +5a5s 4

[0041] l″=2a2+6a3s+12a4s 2 +20a5s 3

[0042] Taking the longitudinal distance s of the Frenet coordinate axis as the independent variable of the quintic polynomial, when the lateral states of two adjacent nodes, such as l, l′, l″, are known, the coefficient matrix of the quintic polynomial can be obtained. When the coefficient matrix is ​​obtained, the lateral states of the two adjacent nodes with the continuous change of the longitudinal s can also be obtained, that is, the connection trajectory of the two adjacent nodes. Assuming that the lateral states at start_s are start_l, start_dl, startddl, and the lateral states at end_s are end_l, end_dl, end_ddl, respectively, the independent variable matrix A of the quintic polynomial and the known state matrix B are:

[0043]

[0044] B=[start_l, start_dl, start_ddl, end_l, end_dl, end_ddl] T

[0045] Let the coefficient matrix to be solved be coff, then the form of coff is as follows:

[0046] coff=[a0, a1, a2, a3, a4, a5] T

[0047] The matrix expression of the quintic polynomial is:

[0048] B=A×coff

[0049] According to the initial state and the end state of the vehicle in the Frenet coordinate system, that is, the B matrix and the A matrix are known, the coefficient matrix of the quintic polynomial can be obtained. According to the complete quintic polynomial, the corresponding lateral states end_l, end_dl, and end_ddl can be obtained as the longitudinal sampling frequency changes.

[0050] When the trajectory of the adjacent nodes is known, the cost of the trajectory needs to be evaluated. The corresponding cost functions include trajectory smoothing cost, obstacle collision cost and reference line distance cost, which are respectively denoted as cost smooth 、cost collision and costref , and the calculation formulas are as follows:

[0051] cost smooth =w dl ·(l′ T ·l′)+w ddl ·(l″ T ·l″)+w dddl ·(l″′ T l″′)

[0052]

[0053] cost ref =w_ref·(l T l)

[0054] Where, l′ is the first-order derivative of the horizontal coordinate with respect to the vertical coordinate in the Frenet coordinate system, l″ is the second-order derivative of the horizontal coordinate with respect to the vertical coordinate in the Frenet coordinate system, l″′ is the third-order derivative of the horizontal coordinate with respect to the vertical coordinate in the Frenet coordinate system, and w dl 、w ddl and w dddl are their corresponding weight coefficients respectively; minl is the distance from the vehicle to the obstacle, w collision is the collision cost weight coefficient; l is the horizontal coordinate in the Frenet coordinate system, and w_ref is the cost function weight coefficient of the distance from the reference line. The total cost function is the sum of the three, that is:

[0055] cost=cost smooth +cost collison +cost ref

[0056] After calculating the cost function between adjacent nodes, the cost between adjacent nodes is obtained, and then dynamic programming is performed from the sampling starting point to the sampling end point.

[0057] Given the cost between each node, find a path with the minimum sum of costs from the sampling starting point to the sampling end point as the result of dynamic programming. The steps of dynamic programming are as follows:

[0058] First, traverse the first column of nodes, find the shortest cost path from the starting point to the first column of nodes, and record the parent node of the current node on the shortest path; secondly, the outer loop traverses the second column of nodes, and the inner loop traverses the previous column of nodes to find the shortest cost path from the starting point to the second column, and records the parent node of the current node; and so on, until the last column of nodes is traversed, the shortest cost path from the starting point to the last column of nodes is found, and the parent nodes are indexed in turn to find the intermediate path nodes.

[0059] The trajectories between adjacent nodes are connected by a quintic polynomial. The result of dynamic programming has a straight line connection for adjacent nodes, but it is not smooth for the overall trajectory. Therefore, the result of dynamic programming can only be used as a rough solution to the optimal trajectory. The optimal trajectory needs to be obtained by secondary programming based on dynamic programming.

[0060] Furthermore, the step 5 further includes:

[0061] The quadratic programming further optimizes l, l′, and l″ obtained by dynamic programming. Its main cost function is as follows:

[0062]

[0063] Among them, W ref is the cost weight of the distance from the horizontal coordinate position of the vehicle to the reference line in the Frenet coordinate system, W dl is the cost weight of the first-order derivative of the horizontal coordinate in the Frenet coordinate system where the vehicle is located, W ddl is the cost weight of the second-order derivative of the horizontal coordinate in the Frenet coordinate system where the vehicle is located, W dddl is the cost weight of the third-order derivative of the horizontal coordinate in the Frenet coordinate system where the vehicle is located, W mid is the cost weight of the middle of the convex space where the vehicle is located. The smaller the value of the cost function, the smoother the trajectory of the vehicle and the closer the vehicle position is to the center of the convex space.

[0064] The cost function of the quadratic programming can eventually be transformed into the standard form of the quadratic programming, and its standard form of the quadratic programming is as follows:

[0065] x T Hx+f T x

[0066] Among them, three constraints need to be met, namely, second-order continuity constraint, collision constraint and planning starting point constraint, and the function forms are as follows:

[0067] AeqX=beq

[0068] AX≤b

[0069] lb≤X≤ub

[0070] Aeq is the second-order continuity constraint matrix, A is the collision constraint matrix, lb and ub are the boundary ranges of the convex space, and the final solved X matrix is:

[0071] X=[l1,l′1,l″1,...,l n , l′ n , l″ n ] T

[0072] The X matrix is ​​the horizontal coordinate of the vehicle in the Frenet coordinate system after quadratic programming optimization, the first-order derivative of the horizontal coordinate with respect to the vertical coordinate, and the second-order derivative of the horizontal coordinate with respect to the vertical coordinate.

[0073] Furthermore, the step six further includes:

[0074] Get the reference path calculated by the path planning module, which is the ideal route that the vehicle should travel in the current environment. Sample the reference path at a certain interval to generate multiple discrete path points, which will be used for speed planning.

[0075] Calculate the ST diagram (space-time diagram) of the obstacle. The space-time diagram is a two-dimensional diagram with time (T) as the horizontal axis and distance (S) as the vertical axis. Each point in the diagram represents the distance position that the vehicle can be at at a certain point in time. Static and dynamic obstacles in the environment are projected onto the space-time diagram. Static obstacles usually form a vertical line at a certain position, while dynamic obstacles form a parallelogram area on the space-time diagram according to their speed.

[0076] Set the target path point speed, and dynamically adjust the target speed curve based on the reference path, traffic rules (such as speed limits, etc.), and the location and speed of obstacles. For example, if a vehicle ahead slows down, the speed curve of the vehicle needs to be adjusted to avoid a collision.

[0077] Design a cost function to evaluate each possible speed curve. The cost function mainly includes the distance cost between the vehicle and the obstacle, the acceleration cost, the recommended speed cost, and the comfort cost. Use dynamic programming to select the one with the lowest cost from all possible speed curves as the optimal speed curve. The dynamic programming process is similar to step 4.

[0078] The initially generated speed curve is smoothed by quadratic programming to reduce sudden or discontinuous speed changes. During the smoothing process, it is ensured that the curve meets certain constraints, such as acceleration and jerk cannot exceed the physical limits of the vehicle.

[0079] Furthermore, the step seven further includes:

[0080] After speed planning, the optimal solution for the smooth trajectory is found. At this time, the relevant information of the horizontal coordinate l and the vertical coordinate s of the driving trajectory point in the Frenet coordinate system has been obtained. In order to send it to the control module to control the execution of the planned trajectory, the trajectory point in the Frenet coordinate system needs to be converted to the Cartesian coordinate system. The conversion formula is as follows:

[0081] x x =x r -l sinθr

[0082] y x =y r -l cosθ r

[0083]

[0084] Among them, x x and x is the horizontal and vertical coordinates converted from the Frenet coordinate system to the Cartesian coordinate system; θ x is the angle between the vehicle's velocity direction and the x-axis in the Cartesian coordinate system; vx is the vehicle's linear velocity in the Cartesian coordinate system; a x is the acceleration of the vehicle in the Cartesian coordinate system; k x is the curvature of the vehicle in the Cartesian coordinate system; θ r is the tangent vector of the ideal projection point in the Frenet coordinate system The angle between the x-axis and the Cartesian coordinate axis. The result of path planning is a series of discrete points, each of which has the above information. The path composed of this series of discrete points is the ideal route obtained by path planning.

[0085] Finally, the speed curve is converted into a control command and passed to the vehicle's execution module to control the vehicle's throttle, brakes, etc. As the vehicle moves and the environment changes, the speed plan also needs to be continuously updated to cope with new obstacles or changing road conditions.

[0086] Through these steps, a safe, smooth and efficient planning curve can be generated for the autonomous vehicle to ensure that the vehicle can drive normally in various environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0087] Figure 1 A flowchart of an automatic driving path planning method based on numerical optimization designed for the present invention.

[0088] Figure 2 A schematic diagram of the mutual conversion between the Cartesian coordinate system and the Frenet coordinate system of the vehicle coordinates provided by the present invention.

[0089] Figure 3 A schematic diagram of the coordinate positions of the vehicle corner points in the Frenet coordinate system provided by the present invention.

[0090] Figure 4 This is a schematic diagram of the dynamic programming sampling path points described in the present invention.

[0091] Figure 5 This is an example of the dynamic programming execution process provided by the present invention.

[0092] Figure 6 The present invention provides a planning roadmap after performing quadratic programming optimization based on dynamic programming.

[0093] Figure 7 This is a schematic diagram of the dynamic planning execution results of the speed planning module provided by the present invention.

[0094] Figure 8 This is a schematic diagram of the secondary planning execution results of the speed planning module provided by the present invention. DETAILED DESCRIPTION

[0095] The present invention will be further described below in conjunction with the accompanying drawings and specific examples.

[0096] The following describes the implementation of the present invention through specific examples, and those skilled in the art can easily implement it according to the contents disclosed in this specification.

[0097] The schematic diagram of the process of the present invention is as follows Figure 1 As shown, the vehicle model is Figure 3 shown.

[0098] A method for automatic driving path planning based on numerical optimization. The implementation process of the method is as follows:

[0099] Step 1: Generate a reference line by global path planning, discretize the reference line, and smooth the points on the reference line to obtain a smoothed reference line;

[0100] Step 2: Calculate the ideal projection point of the ego vehicle on the Frenet coordinate axis based on the coordinate information of the ego vehicle on the Cartesian coordinate axis and the discrete points on the smoothed reference line;

[0101] Step 3: Calculate the position of the vehicle in the Frenet coordinate system based on the information of the ideal projection point. The calculation of obstacles is similar.

[0102] Step 4: Discretely sample some points in the area where the vehicle is traveling. The paths between adjacent sampling points are connected using trajectories generated by a quintic polynomial. The cost of each trajectory is calculated using a cost evaluation function, and a path with the minimum cost is found using dynamic programming.

[0103] Step 5: Use quadratic programming to further smooth the path found by dynamic programming;

[0104] Step 6: Perform further dynamic planning and secondary planning on the speed of the path points obtained by path planning. Finally, a safe, smooth and comfortable obstacle avoidance route is generated;

[0105] Step 7: Convert the coordinate points on the obstacle avoidance route from the Frenet coordinate system to the Cartesian coordinate system and send them to the control module for execution.

[0106] Specifically, this implementation case conducts a detailed analysis of the above disclosed steps, and the analysis content is as follows:

[0107] 1. The location information of discrete points on the reference line is obtained from the high-precision map, including the horizontal coordinate, vertical coordinate, inclination angle and curvature of each point in the Cartesian coordinate system. The cost function of reference line smoothing is:

[0108]

[0109] Among them, x is the column vector composed of the horizontal and vertical coordinates of the point to be smoothed, A1 is the coefficient matrix of the similarity cost, A2 is the coefficient matrix of the smoothness cost, and A3 is the coefficient matrix of the compact cost. Except for A1 which is the unit matrix, the implementation forms of other corresponding sub-matrices are as follows:

[0110]

[0111] The corresponding large matrices A2 and A3 can be formed by continuously expanding and filling in the corresponding positions. The constraints ub and lb of the quadratic programming are the upper and lower bounds of the distance between the smoothed reference line and the original reference line, respectively, which constrain the smoothed reference line not to be too far away from the original reference line.

[0112] 2. Start from the starting point on the reference line and traverse each discrete point on the reference line. According to the distance from each discrete point on the reference line to the current position of the vehicle, find the point with the shortest distance from the current position of the vehicle from the discrete points on the reference line and record it as the minimum matching point P. m .

[0113] It should be noted that the Euclidean distance is used to measure the distance from each discrete point to the current position of the vehicle.

[0114] After finding the minimum matching point each time, the subsequent traversal starts from the minimum matching point of the previous moment, rather than from the planning starting point. The formula for measuring distance is as follows:

[0115] dx=x h -x p

[0116] dy=y h -y p

[0117]

[0118] Where dx is the vehicle horizontal coordinate xh and the horizontal coordinate x of the discrete point p The lateral distance, dy is the vehicle's ordinate y h and the ordinate y of the discrete point p The longitudinal distance is the straight-line distance between the vehicle and the discrete point.

[0119] 3. Based on the information of the ideal projection point, i.e., the position vector, inclination angle and curvature in the Cartesian coordinate system, and the position vector, inclination angle and curvature of the ego vehicle in the Cartesian coordinate system, the information of the ego vehicle in the Frenet coordinate system can be obtained. Figure 2 By transmitting different information in the two coordinate systems, the six state information of the vehicle in the Frenet coordinate system can be obtained, which are l, l′, l″, s, and in The specific calculation formula is as follows:

[0120] s=s r

[0121]

[0122] Among them, s r The distance from the ideal projection point of the vehicle on the reference line to the planning starting point; is the position vector of the vehicle in the Cartesian coordinate system relative to the coordinate origin; is the velocity vector of the vehicle in the Cartesian coordinate system; is the acceleration vector of the vehicle in the Cartesian coordinate system; is the projection position vector; k r is the curvature of the projection vector on the road geometry in the Cartesian coordinate system; is the tangent vector of the projection position vector on the road geometry in the Cartesian coordinate system; is the normal vector of the projection position vector on the road geometry in the Cartesian coordinate system; the specific conversion process is as follows:

[0123] 1) First calculate s and l;

[0124] Depend on Figure 2 It can be seen that the coordinate origin, the ideal projection point and the point where the vehicle is located form a vector triangle. It is easy to know from the relationship between the three sides of the triangle: Will Move to the left side of the equal sign and multiply both sides of the equation Available Obviously, l is the result of the dot product of two vectors. It is a scalar, which can be positive or negative. The positive direction represents above the vertical axis of the reference line, and the negative direction represents below the vertical axis of the reference line.

[0125] s represents the longitudinal distance from the ideal projection point to the planning starting point, which is approximately equal to the distance from the ideal projection point to the matching point plus the mutual distance between the matching points, and is described by the formula: where x i is the coordinate information of the matching point, x0 is the planning starting point, The dot product of the position vector from the matching point to the vehicle position and the direction vector at the matching point is numerically equal to the projection of the position vector from the matching point to the vehicle position on the direction vector at the matching point. If the two vectors form an acute angle, the result is positive; if the two vectors form an obtuse angle, the result is negative.

[0126] 2) Next, calculate and

[0127] By the vector triangle formula We know that by taking the derivative on both sides of the equation, we get By the following auxiliary formula: Substituting into the above formula we get: Multiply both sides simultaneously You can get Multiply both sides simultaneously You can get

[0128] 3) Then calculate l′ and

[0129] because Divide both the numerator and denominator by dt to get The above obtained and Substitute it into the equation to get l′; At a value equal to The derivative with respect to time is Will Substituting the time derivative, we get

[0130] 4) Final calculation and l″;

[0131] Right now Taking the derivative with respect to time, we can get the derivative of both sides of the equation with respect to time. Further calculation simplification can be obtained Also because so

[0132] Simplified so

[0133] The above is the conversion formula from the Cartesian coordinate system to the Frenet coordinate system. The coordinate information of the current position of the autonomous driving vehicle can be converted to the Frenet coordinate system through a series of transformations to prepare for subsequent calculations.

[0134] 4. The vehicle's position information has been converted to the Frenet coordinate system. At this time, some path points are randomly sampled in front of the vehicle to obtain the Frenet horizontal and vertical coordinates of the path points in preparation for dynamic planning. As shown in Figure 4, in the longitudinal direction of the vehicle, according to the longitudinal sampling frequency, some points are sampled in the horizontal direction of the vehicle's position as reference points for the vehicle's future path to be driven. Each path point has a corresponding horizontal coordinate l and vertical coordinate s. Different path points are connected by paths generated by quintic polynomials. The corresponding cost weights are calculated for the generated paths. If the cost weights between different path points are as follows: Figure 5 As shown, the entire dynamic programming process is as follows:

[0135] The starting point of the plan is located at point A. The costs from point A to the path points in the first column are 2, 5, and 1 respectively. At this time, the shortest path cost of A→B1 is recorded as 2, the shortest path cost of A→B2 is 5, and the shortest path cost of A→B3 is 1, and the pioneer path point in the first column is recorded as A; secondly, calculate the cost from the first column to the second column and add the cost from the planning starting point to the first column to take the minimum value, that is, A→C1=min(A→B1→C1, A→B2→C1, A→B3→C1)=A→B2→C1=11. Similarly, A→C2=min(A→B1→C2,A→B2→C2,A→B3→C2)=A→B3→C2=13. Similarly, A→C3=min( A→B1→C3, A→B2→C3, A→B3→C3)=A→B2→C3=9; then, calculate the cost from the second column to the third column plus the cost from the planning starting point to reach the second column to take the minimum value, that is, A→D1=min(A→C1→D1, A→C2→D1, A→C3→D1)=A→C1→D1=14. Similarly, A→D2=min(A→C1→D2, A→C2→D2, A→C3→D2)=A→C2→D2=18; finally, calculate the cost from the last column to the planning end point plus the cost from the planning starting point to the last column to take the minimum value, that is, A→E=min(A→D1→E, A→D2→E)=A→D1→E=19.

[0136] A drivable path with the lowest cost is calculated through dynamic programming, but this path does not take into account the vehicle dynamics and kinematic models, and the path connected by the path points is not smooth enough. Therefore, the path generated by dynamic programming needs to be further optimized.

[0137] 5. The quadratic programming further optimizes l, l′, and l″ obtained by dynamic programming. Its main cost function is as follows:

[0138]

[0139] Among them, W ref is the cost weight of the distance from the horizontal coordinate position of the vehicle to the reference line in the Frenet coordinate system, W dl is the cost weight of the first-order derivative of the horizontal coordinate in the Frenet coordinate system where the vehicle is located, W ddl is the cost weight of the second-order derivative of the horizontal coordinate in the Frenet coordinate system where the vehicle is located, W dddl is the cost weight of the third-order derivative of the horizontal coordinate in the Frenet coordinate system where the vehicle is located, W mid is the cost weight of the middle of the convex space where the vehicle is located. The smaller the value of the cost function, the smoother the trajectory of the vehicle and the closer the vehicle position is to the center of the convex space.

[0140] The cost function of the quadratic programming can eventually be transformed into the standard form of the quadratic programming, and its standard form of the quadratic programming is as follows:

[0141] x T Hx+f T x

[0142] Among them, three constraints need to be met, namely, second-order continuity constraint, collision constraint and planning starting point constraint, and the function forms are as follows:

[0143] AeqX=beq

[0144] AX≤b

[0145] lb≤X≤ub

[0146] Aeq is the second-order continuity constraint matrix, A is the collision constraint matrix, lb and ub are the boundary ranges of the convex space, and the final solved X matrix is:

[0147] X=[l1,l′1,l″1,...,l n , l′ n , l″ n ] T

[0148] The X matrix is ​​the horizontal coordinate of the vehicle in the Frenet coordinate system after quadratic programming optimization, the first-order derivative of the horizontal coordinate with respect to the vertical coordinate, and the second-order derivative of the horizontal coordinate with respect to the vertical coordinate.

[0149] It should be further explained that lb and ub are the boundary ranges of the convex space. The quadratic programming can only be solved when the vehicle is within the convex space. Therefore, it is necessary to ensure that the four corner points of the vehicle are contained in the convex space, not just the center of mass of the vehicle. When the center of mass of the vehicle is in Frenet coordinates (s, l), the coordinates of the four corner points of the vehicle are as follows:

[0150]

[0151] like Figure 3 As shown, d1 is the distance from the center of mass of the vehicle to the front wheel, dz is the distance from the center of mass of the vehicle to the rear wheel, w is the width of the vehicle, and θ is the angle between the velocity direction of the vehicle and the ordinate. To ensure that the vehicle is always in the convex space, it is necessary to ensure that the coordinates of the four corner points are between lb and ub.

[0152] The output of the quadratic programming is the coordinates of the smoothed path points, such as Figure 6 As shown in the figure, the SL curve after quadratic planning is also smoother, but the coordinates of the path points are still in the Frenet coordinate system and need to be converted to the Cartesian coordinate system for further speed processing.

[0153] 6. Through dynamic programming, a convex space for trajectory driving is opened up, and the optimal solution for the smooth trajectory is found in the convex space with the quadratic trajectory. At this time, the horizontal coordinates of the driving trajectory points in the Frenet coordinate system have been obtained. l In order to send the related information of the ordinate s to the control module to control the execution of the planned trajectory, the speed information of each trajectory point also needs to be obtained.

[0154] exist Figure 7 In the figure, the execution process of the dynamic programming algorithm on the space-time graph (ST graph) is shown. In this figure, the horizontal axis represents time (T) and the vertical axis represents distance (S). Through the dynamic programming algorithm, the system selects an optimal path in the space-time graph that avoids obstacles and meets physical constraints based on the input starting state and target state. The green solid line in the figure represents the planned speed curve, which passes through multiple key nodes and gradually determines the ideal position and speed of the vehicle at each moment. The influence area of ​​the obstacle is represented by the shaded block area in the figure. The dynamic programming algorithm avoids these areas through calculation, thereby ensuring that the vehicle will not collide during the planning process. In the end, the optimal speed curve generated by the system smoothly passes all obstacles and reaches the target state.

[0155] based on Figure 7The dynamic programming results in the quadratic programming further smooth and optimize the speed curve. Figure 8 In the figure, the horizontal axis also represents time (T) and the vertical axis represents distance (S). The curve in the figure represents the speed curve after quadratic programming optimization. Compared with the dynamic programming result, the curve is smoother and more continuous. Quadratic programming uses optimization objectives such as minimizing jerk to further improve ride comfort and vehicle stability. The optimization formula is as follows:

[0156] Cost = w c J c +w o J o +W d J d +w a J a

[0157] Among them, J c is the price of comfort, J o is the obstacle cost, J d Recommended speed cost, J a is the acceleration cost, w c 、w o 、w d 、w a is the weight coefficient of each cost function. The formula of comfort cost function is The obstacle cost function is d is the distance from the vehicle to the obstacle; the recommended speed cost function is where v r is the recommended speed, which is determined by the high-precision map, i.e., the lane speed limit; the acceleration cost function is

[0158] also, Figure 8 The optimization results in show that the speed curve after quadratic planning still maintains safe avoidance of obstacles, while reducing unnecessary acceleration and deceleration operations by adjusting the shape of the curve, thereby improving driving efficiency and vehicle energy consumption. Finally, the path points on the optimized speed curve are used for actual vehicle driving control after coordinate transformation.

[0159] 7. After dynamic planning, the convex space of trajectory driving is opened up, and the optimal solution of the smooth trajectory is found in the convex space with the secondary trajectory. At this time, the relevant information of the horizontal coordinate l and the vertical coordinate s of the driving trajectory point in the Frenet coordinate system has been obtained. In order to send it to the control module to control the execution of the planned trajectory, the trajectory point in the Frenet coordinate system needs to be converted to the Cartesian coordinate system. The conversion formula is as follows:

[0160] x x=x r -l sinθ r

[0161] y x =y r -l cosθ r

[0162]

[0163] Among them, x x and x is the horizontal and vertical coordinates converted from the Frenet coordinate system to the Cartesian coordinate system; θ x v is the angle between the vehicle's velocity direction and the x-axis in the Cartesian coordinate system; x is the linear velocity of the vehicle in the Cartesian coordinate system; a x is the acceleration of the vehicle in the Cartesian coordinate system; k x is the curvature of the vehicle in the Cartesian coordinate system; θ r is the tangent vector of the ideal projection point in the Frenet coordinate system The angle between the x-axis and the Cartesian coordinate axis. The result of path planning is a series of discrete points, each of which has the above information. The path composed of this series of discrete points is the ideal route obtained by path planning.

[0164] Finally, the speed curve is converted into a control command and passed to the vehicle's execution module to control the vehicle's throttle, brakes, etc. As the vehicle moves and the environment changes, the speed plan also needs to be continuously updated to cope with new obstacles or changing road conditions. Through these steps, a safe, smooth and efficient planning curve can be generated for the autonomous vehicle to ensure that the vehicle can drive normally in various environments.

[0165] It should be further explained that the cost function is a function used to measure the quality or utility of a path or decision. In path planning, the cost function is used to evaluate the pros and cons of different paths and select the path with the lowest cost as the best path. The cost function can take into account a variety of factors, such as the length, time, safety, smoothness, energy consumption, etc. of the path. By setting the weights of different factors, the cost function can be adjusted to suit specific needs.

[0166] It should be further explained that Frenet coordinates are a vehicle trajectory description method used to decompose the vehicle's movement on the road into two directions: longitudinal and lateral. It is a local coordinate system with the vehicle's current position as the origin. The longitudinal direction represents the direction of the vehicle along the road, and the lateral direction represents the lateral deviation of the vehicle relative to the center line of the road. By decomposing the three-dimensional trajectory into longitudinal and lateral components, it is easier to model and analyze the trajectory and perform path planning.

[0167] In the embodiment of the present invention, based on the principle of the numerical optimization automatic driving path planning method, a three-dimensional problem is mainly decomposed into two independent two-dimensional problems, respectively processing the path and speed of the vehicle, first planning the drivable path to obtain the SL graph, and then planning the speed of the information on the path points to obtain the ST graph. Through iterative optimization, the SL and ST graphs are coordinated to obtain the global optimal vehicle motion path. In addition, this method also constructs a trajectory quality evaluation model for quantitatively evaluating the pros and cons of each trajectory. The evaluation model takes into account multiple indicators such as vehicle stability, ride comfort, energy consumption, etc., and incorporates these indicators into the trajectory optimization process through the cost function of numerical optimization. Finally, the optimal trajectory under different driving scenarios is generated.

[0168] The series of detailed descriptions listed above are only specific descriptions of feasible implementation methods of the present invention. They are not intended to limit the scope of protection of the present invention. All equivalent methods or changes that do not deviate from the technical creation of the present invention should be included in the scope of protection of the present invention.

Claims

1. An automatic driving path planning method based on numerical optimization, including a path planning module, a speed planning module, a behavior decision module, a dynamic optimization module and a collision detection module.

2. The automatic driving path planning method based on numerical optimization according to claim 1, characterized in that: The path planning module generates a feasible driving path based on the vehicle's current position, target position, map data and perceived obstacle information.

3. The automatic driving path planning method based on numerical optimization according to claim 1, characterized in that: The speed planning module generates a speed curve for each path point based on the driving path generated by the path planning module in combination with traffic rules, road conditions and dynamic obstacles.

4. The automatic driving path planning method based on numerical optimization according to claim 1, characterized in that: The behavior decision module is used to make decisions on lane change, overtaking or parking according to traffic regulations and surrounding environment information.

5. The automatic driving path planning method based on numerical optimization according to claim 1, characterized in that: The behavior dynamic optimization module optimizes the speed curve through a quadratic programming algorithm to ensure smooth and safe driving while satisfying the physical constraints of the vehicle.

6. The automatic driving path planning method based on numerical optimization according to claim 1, characterized in that: The collision detection module is used to detect whether the generated path and speed curve collide with surrounding obstacles, and re-plan the path and speed curve when a potential collision is detected.

Images