A dynamic programming-based intelligent vehicle global optimal trajectory planning method and system

Abstract

The application discloses a kind of intelligent vehicle global optimal trajectory planning method and system based on dynamic programming, it includes: step 1, according to map information, vehicle chassis parameter information and start point and end point pose information, the road environment in map information is converted from Cartesian coordinate system to Frenet coordinate system, the state space is discretized, then according to the first optimal control problem model of trajectory planning task, the optimal control problem is solved using dynamic programming method, and the path point sequence of global optimal path is obtained;Step 2, according to the state space of path point sequence and the dynamics parameter information of vehicle is discretized, reference trajectory planning task establishes second optimal control problem model, the optimal control problem is solved using dynamic programming method, and the optimal speed of global optimal path is obtained, to obtain global optimal trajectory.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of automatic driving, in particular to a global optimal trajectory planning method and system for intelligent vehicles based on dynamic programming. BACKGROUND

[0002] Intelligent vehicles are an important part of intelligent transportation systems, which can effectively reduce traffic accidents, make social traffic more efficient, and also save energy and reduce emissions, so they have become a research hotspot in recent years. Among them, trajectory planning is one of the core modules of intelligent vehicles, which plays a role in connecting the upper mapping and positioning, perception, decision-making, and lower motion control. Global trajectory planning refers to planning a trajectory that meets the target requirements and satisfies the kinematic principle of the vehicle based on map information and according to the start point and target point information, which contains speed information. Global trajectory planning includes path planning and speed planning, path planning solves the problem of which coordinate points the intelligent vehicle will pass through to reach the target position, and speed planning solves the problem of how the intelligent vehicle travels at each coordinate point.

[0003] Currently, the trajectory planning methods for intelligent driving vehicles mainly include: 1) curve-based methods, which mainly feature specific requirements or expressions of reference curves, and calculate the parameters of the curve according to some boundary conditions of control points. Common ones include RS curves, Bezier curves, and spline curves, etc. 2) graph search-based methods, which mainly feature rasterizing the map and then searching for paths, and strive to achieve the goals of shortest path and optimal efficiency, mainly including Dijkstra algorithm, A* algorithm, and Hybrid A* algorithm, etc. 3) sampling-based methods, which do not need to traverse all points in the map space, but instead randomly scatter points in space, and connect the points by line segments to form a road map / tree without obstacles, to abstractly express the space, mainly including RRT (Rapidly-exploring Random Tree) and PRM (Probabilistic Road Map). 4) optimal control-based methods, which need to consider actual conditions to establish an optimal control problem model, including objective function and constraint conditions, etc. Methods suitable for solving optimal control problems include indirect methods, dynamic programming methods, and direct methods. Indirect methods, also known as analytical methods, usually use the necessary conditions for the extreme value of the cost function, combined with sufficient conditions or the actual physical meaning of the proposition to indirectly determine the optimal solution. Typical indirect methods include the variational method and the Pontryagin maximum principle. Dynamic programming method divides the problem space into multiple subspaces, and determines the sequence of control variables in each interval to achieve the optimization of the objective function. Direct methods discretize the variables of the optimal control problem, and directly obtain the numerical optimal solution by solving the NLP (Nonlinear programming) problem formed after discretization.

[0004] For the global optimal trajectory planning problem of the known map, the existing method has an optimal control-based global optimal trajectory planning method using a direct method for solving. This method is to combine the obstacle and road boundary information in the map, the vehicle model and the terminal boundary constraint to design the objective function, establish the optimal control problem model, discretize the variables to convert the optimal control problem into an NLP problem and use a solver to solve, so as to obtain the numerical optimal solution of the variables in the continuous space. Although this method can solve the global optimal trajectory, the method is highly dependent on the built-in optimization algorithm of the solver, and it is difficult to use mathematical methods to represent the obstacle constraints of the vehicle in a complex map or multi-obstacle environment. When using the direct method to solve the optimal control problem, if the constraints of the problem are not accurately expressed in mathematics or the constraints are too complex, it is easy to cause the solver to fail to solve. On the other hand, when solving the optimal control problem based on the built-in optimization algorithm of the solver, there is a high requirement for the initial solution, and a poor quality initial solution may lead to suboptimality of the solution result.

[0005] And for the global optimal trajectory planning problem of the known map, the existing technology is usually to first establish the map as a topological map, discretize the map space into a plurality of topological points, and then use dynamic programming to obtain the optimal control variable sequence reaching each topological point that satisfies the objective function, thereby obtaining the optimal route of the entire problem. However, the optimal route of the topological map solved by using dynamic programming generally only considers the coordinate points of the vehicle and does not consider the yaw angle and front wheel steering angle of the vehicle. The result is a polyline route between points, which does not satisfy the minimum turning radius and curvature continuity constraints of the vehicle path, so the vehicle cannot be directly controlled and tracked. SUMMARY

[0006] The purpose of the present application is to provide a dynamic programming-based intelligent vehicle global optimal trajectory planning method and system to overcome or at least alleviate at least one of the above-mentioned deficiencies of the prior art.

[0007] To achieve the above-mentioned purpose, the present application provides a dynamic programming-based intelligent vehicle global optimal trajectory planning method, which comprises:

[0008] Step 1, according to the map information, vehicle chassis parameter information and starting point and ending point pose information, the road environment in the map information is converted from the Cartesian coordinate system to the Frenet coordinate system, the state space is discretized, and then a first optimal control problem model is established according to the trajectory planning task, and a dynamic programming method is used to solve the optimal control problem to obtain a path point sequence of the global optimal path. Further, the setting method of the Frenet coordinate system comprises:

[0009] According to the preset reference line Tref The position coordinates of the intelligent driving vehicle in the Cartesian coordinate system are set to (x, y). The distance from the vehicle's position (x, y) to the reference line T is... ref Projecting the data, let the projection point be M. Then the distance between point M and the vehicle's position (x, y) is the lateral distance l, along T. ref The longitudinal distance from the starting point to the projection point M is s. Let (s,l) represent the coordinates of the vehicle in the Frenet coordinate system. The following equation (1) represents the mapping relationship between the Frenet coordinate system and the Cartesian coordinate system:

[0010]

[0011] in, Let M be the position vector of the projection point M of the vehicle's position on the reference line in the global coordinate system. Let M be the normal vector and tangent vector of the projection point M on the reference curve;

[0012] Step 2: Discretize the state space based on the path point sequence and vehicle dynamic parameters. Establish a second optimal control problem model with reference to the trajectory planning task. Use dynamic programming to solve the optimal control problem and obtain the optimal speed of the globally optimal path, thereby obtaining the globally optimal trajectory.

[0013] Furthermore, the model for the first optimal control problem is set as follows:

[0014]

[0015] Wherein, equation (2a) is the objective function, ||x k+1 -x k || represents the distance between path points between two stages, x k =(x k ,y k )=f1(s k ,l k ), ||x k+1 -x k || max |δ represents the maximum distance between pathpoints between two stages. k+1 -δ k | represents the change in the vehicle's front wheel steering angle control value between two stages, δ max κ represents the maximum value of the front wheel steering angle. x,k κ represents the path curvature value at stage k. x,max Let ω1, ω2, ω3 represent the maximum path curvature at stage k, ω1, ω2, ω3 represent the weights, equation (2b) is the state transition equation between state variables at different stages, equation (2c) is the boundary constraint range of the control variables, and equation (2d) is the road boundary and obstacle constraint. left (sk ) represents s k The corresponding left boundary of l, l right (s k ) represents s k The corresponding right boundary of l.

[0016] Furthermore, the method for "solving the optimal control problem based on dynamic programming" in step 1 includes:

[0017] The map space is divided into multiple sub-state spaces in the Frenet coordinate system and the sub-state spaces are discretized. Then, based on the vehicle's front wheel steering angle constraint, the drivable range for the next stage is determined using the state transition equation. Finally, the objective function is optimized by determining the optimal sequence form of the control variables in each interval to obtain the globally optimal trajectory in the entire map space.

[0018] Furthermore, the method of dividing the map space into multiple sub-state spaces in the Frenet coordinate system and discretizing the sub-state spaces specifically includes:

[0019] Based on the map information provided by the mapping module, the road centerline is collected as a reference line to establish a Frenet coordinate system and the state is discretized. The discretization of the road map is based on [s,l,θ]. x Three dimensions: s, l, θ x These represent the longitudinal distance, the lateral distance, and the vehicle yaw angle in the Cartesian coordinate system, respectively.

[0020] Furthermore, the method of "determining the drivable range for the next stage using the state transition equation based on the vehicle's front wheel steering angle constraint" specifically includes:

[0021] First, the optimal control problem described by equation (2) is solved using the front wheel rotation angle δ. x To control the amount, based on the range of the vehicle's front wheel steering angle, according to dδ x For the front wheel steering angle δ x Discretize the equations, and use equation (3) to calculate the discrete curvature control sequence based on the discrete front wheel steering angle:

[0022]

[0023] Among them, κ x R represents the curvature of the vehicle's travel path. x L represents the turning radius of a vehicle. x For the vehicle wheelbase, δ x The steering angle of the vehicle's front wheels;

[0024] The drivable range of the vehicle in the next stage is then calculated based on the state transition equation obtained from the transformation relationship between the Frenet coordinate system and the Cartesian coordinate system.

[0025] The application also provides a dynamic programming-based intelligent vehicle global optimal trajectory planning system, comprising a mapping and positioning unit, a global optimal trajectory planning unit, a motion control unit and a bottom layer execution unit, wherein:

[0026] The mapping and positioning unit is used to obtain environmental information, which is processed and then issued to the global optimal trajectory planning unit; the global optimal trajectory planning unit is used to convert the road environment in the map information from a Cartesian coordinate system to a Frenet coordinate system, discretize the state space, and then establish a first optimal control problem model according to the trajectory planning task, solve the optimal control problem by using a dynamic programming method, and obtain a path point sequence of the global optimal path; the global optimal trajectory planning unit is also used to discretize the state space according to the path point sequence and the dynamic parameter information of the vehicle, refer to the trajectory planning task to establish a second optimal control problem model, solve the optimal control problem by using a dynamic programming method, obtain the optimal speed of the global optimal path, thereby obtain the global optimal trajectory, and then output the global optimal trajectory to the motion control unit; the motion control unit is used to calculate a control instruction according to the global optimal trajectory, and issue the control instruction to the bottom layer execution unit to control the vehicle.

[0027] Further, the setting method of the Frenet coordinate system comprises:

[0028] According to a preset reference line T ref , the position coordinates of the intelligent driving vehicle in the Cartesian coordinate system are set as (x, y), the vehicle position (x, y) is projected to the reference line T ref , the projection point is M, the distance between the point M and the vehicle position (x, y) is a lateral distance l, and the distance along T ref is a longitudinal distance s, the coordinate value of the vehicle in the Frenet coordinate system is represented by (s, l), and the mapping relationship between the Frenet coordinate system and the Cartesian coordinate system is represented by the following formula (1):

[0029]

[0030] wherein, is the position vector of the projection point M of the vehicle position on the reference line in the global coordinate system, is the normal vector and tangent vector of the projection point M on the reference curve.

[0031] The first optimal control problem model is set as:

[0032]

[0033] wherein, formula (2a) is an objective function, ||x k+1x k || represents the distance of path points between two stages, x k = (x k , y k ) = f1(s k , l k ), ||x k+1 -x k || max represents the maximum value of the distance of path points between two stages, |δ k+1 -δ k | represents the change amount of the front wheel steering angle control amount of the vehicle between two stages, δ max represents the maximum value of the front wheel steering angle, κ x,k represents the path curvature value of the kth stage, κ x,max represents the maximum path curvature value of the kth stage, ω1, ω2, ω3 represent weight values, formula (2b) is a state transition equation between state quantities of different stages, formula (2c) is a boundary limit range of a control quantity, formula (2d) is a road boundary and obstacle constraint, l left (s k ) represents the left boundary corresponding to s k , and l right (s k ) represents the right boundary corresponding to s k .

[0034] Further, the method for solving the optimal control problem based on the dynamic programming method comprises:

[0035] In the Frenet coordinate system, a map space is divided into a plurality of sub-state spaces, and the sub-state spaces are discretized, and then according to the front wheel steering angle constraint of the vehicle, a state transition equation is used to determine the drivable range of the next stage, and then the optimal sequence form of the control variable on each interval is determined to optimize the objective function, so as to obtain the global optimal trajectory under the whole map space.

[0036] Further, the method for dividing the map space into a plurality of sub-state spaces in the Frenet coordinate system and discretizing the sub-state spaces comprises:

[0037] According to the map information provided by the mapping module, a road center line is collected as a reference line to establish a Frenet coordinate system and perform state discretization, and the discretization of the road map is according to three dimensions of [s, l, θ x ], s, l and θ x are longitudinal distance, lateral distance and vehicle yaw angle in the Cartesian coordinate system respectively.

[0038] Further, the method of "determining the drivable range of the next stage according to the front wheel steering angle constraint of the vehicle, and using the state transition equation" comprises:

[0039] The optimal control problem described by formula (2) is used first x As a control variable, according to the range of the front wheel steering angle of the vehicle, according to dδ x The front wheel steering angle δ x is discretized, and the discrete curvature control sequence obtained according to the discretized front wheel steering angle can be calculated by formula (3):

[0040]

[0041] Where κ x represents the curvature of the vehicle driving path, R x represents the turning radius of the vehicle, L x is the wheelbase of the vehicle, and δ x is the front wheel steering angle of the vehicle.

[0042] The drivable range of the vehicle in the next stage is calculated according to the state transition equation obtained from the conversion relationship between the Frenet coordinate system and the Cartesian coordinate system.

[0043] Compared with the route planning method based on dynamic programming of the topological map, the global optimal trajectory obtained by the present application meets the curvature continuity requirement and satisfies the minimum turning radius constraint of the vehicle. After trajectory point interpolation, the trajectory can be directly issued to the motion control unit for tracking control. The dynamic programming method is used to solve the optimal control problem, the three-dimensional state quantities of the entire configuration space are discretized, thereby dividing the trajectory planning problem into several stages, the optimal strategy between stages is solved based on the state transition equation and the objective function, the optimal decision to reach each discrete state quantity and the global optimal result of the corresponding objective function can be accurately obtained, and then on the one hand, the solving failure of the solver built-in algorithm caused by too complex constraints can be avoided, and on the other hand, the local optimum caused by a poor initial solution can be avoided. The decision sequence obtained by the present application is the optimal strategy for the objective function under the discrete condition. BRIEF DESCRIPTION OF DRAWINGS

[0044] Figure 1 The framework diagram of the intelligent vehicle global optimal trajectory planning method based on dynamic programming provided by the embodiment of the present application is shown;

[0045] Figure 2 The conversion diagram of the Frenet coordinate and the Cartesian coordinate system provided by the embodiment of the present application is shown;

[0046] Figure 3 The vehicle state discretization graph under the Frenet coordinate provided by the embodiment of the present application is shown;

[0047] Figure 4 The vehicle bicycle model diagram provided by the embodiment of the present application is shown;

[0048] Figure 5 A diagram illustrating the process of finding the globally optimal path based on dynamic programming, provided in an embodiment of the present invention.

[0049] Figure 6 The velocity discretization diagram provided for embodiments of the present invention;

[0050] Figure 7 This is the distance and velocity state transition diagram provided in this embodiment;

[0051] Figure 8 This is a diagram illustrating the dynamic programming solution process for the optimal speed provided in this embodiment.

[0052] Figure 9 This is a schematic diagram of the structure of the global optimal trajectory planning system for intelligent vehicles based on dynamic programming provided in this embodiment. Detailed Implementation

[0053] The present invention will now be described in detail with reference to the accompanying drawings and embodiments.

[0054] like Figure 1 As shown, the global optimal trajectory planning method for intelligent vehicles based on dynamic programming provided in this embodiment of the invention includes:

[0055] Step 1, Dynamically plan the globally optimal path: Based on map information, vehicle chassis parameters, and start and end point pose information, the road environment in the map information is transformed from the Cartesian coordinate system to the Frenet coordinate system, and the state space is discretized. Then, a first optimal control problem model is established according to the trajectory planning task, and the optimal control problem is solved using dynamic programming to obtain the path point sequence of the globally optimal path. The map information can be obtained from the mapping and positioning module.

[0056] Step 2, Dynamic Programming of Global Optimal Speed: Discretize the state space based on the path point sequence and vehicle dynamic parameters, establish a second optimal control problem model with reference to the trajectory planning task, and use dynamic programming to solve the optimal control problem to obtain the optimal speed of the global optimal path, thereby obtaining the global optimal trajectory.

[0057] In one implementation forest, as one way to implement step 1, it specifically includes:

[0058] Step 11, Establish the Frenet coordinate system: The Frenet coordinate system is established based on the preset reference line T. ref T ref This can be any curve, but is generally defined as the lane centerline. The coordinates of the autonomous vehicle in the Cartesian coordinate system are set to (x, y), and the distance from the vehicle's position (x, y) to the reference line T is... refA projection is made, and the projection point is M. The distance between the point M and the vehicle position (x, y) is the lateral distance l. The curve along T ref The curve distance from the starting point to the projection point M is the longitudinal distance s. The coordinate values of the vehicle in the Frenet coordinate system are represented by (s, l), and the mapping relationship is represented by equation (1) as follows:

[0059]

[0060] Figure 2 The conversion relationship between the Frenet coordinate system and the Cartesian coordinate system of the intelligent driving vehicle is described. The motion state of the vehicle at any time can be described as [x, θ x , κ x , v x , a x ]; wherein x is the current vehicle position coordinate, which can be represented by the position information (x, y) in the Cartesian coordinate system, or by the longitudinal distance s and the lateral distance l in the Frenet coordinate system, i.e. x = (x, y) = f1(s, l); θ x is the vehicle yaw angle in the Cartesian coordinate system, κ x is the curvature, v x is the vehicle speed, and a x is the acceleration. is the position vector of the projection point M of the vehicle position on the reference line in the global coordinate system, and are the normal vector and the tangent vector of the projection point M on the reference curve, respectively.

[0061] Step 12, an optimal control problem is established. The optimal control refers to seeking a control under given constraint conditions, so that the given system performance index reaches a maximum (or minimum) value. It is assumed that the front wheel steering angle of the vehicle does not change within a ds length range, i.e. the vehicle moves along a curve with a fixed curvature. The vehicle front wheel steering angle δ x is the control quantity, ξ = [x, s, l, θ x , κ x ] is the state quantity, x = (x, y) represents the vehicle coordinate in the Cartesian coordinate system, s and l represent the longitudinal distance and the lateral distance in the Frenet coordinate system, respectively, θ x is the vehicle yaw angle in the Cartesian coordinate system, and κ x is the curvature. Based on this, the first optimal control problem model is established as follows:

[0062]

[0063] wherein equation (2a) is the objective function, ||x k+1 -x k|| represents the path point distance between two stages, x k = (x k , y k ) = f1(s k , l k ), ||x k+1 - x k || max represents the maximum value of the path point distance between two stages; |δ k+1 - δ k | represents the variation of the control variable between two stages, δ max represents the maximum value of the front wheel steering angle; κ x,k represents the path curvature value of the kth stage, κ x,max represents the maximum path curvature value of the kth stage; ω1, ω2, ω3 represent the weight values, the present application sets the weight values of ω1, ω2, and ω3 to be 7, 2, and 1 respectively according to the related literature; f ds (ξ k , δ k ) in formula (2b) is the state transition equation between the state variables (ξ k , δ k ) of different stages (for example, the kth stage and the k+1th stage are two different stages), that is, formulas (3)-(6) below, ξ k , δ k are the vehicle state variable and the control variable (front wheel steering angle) of the kth stage respectively, and ξ k+1 is the vehicle state variable of the k+1th stage; formula (2c) is the boundary limit range of the control variable; formula (2d) is the road boundary and obstacle constraint, l left (s k ) represents the left boundary l corresponding to s k , and l right (s k ) represents the right boundary l corresponding to s k .

[0064] Since the vehicle moves along a circular arc within one stage defined in the above embodiment, the circular arc length can also be used to represent it. Compared with other distance forms, the Euclidean distance is more convenient to calculate, so we have:

[0065]

[0066] Of course, other distances can also be applied to the first optimal control problem model in the above embodiment.

[0067] Compared to the global optimal trajectory planning methods based on optimal control in the background art, which use direct methods to solve the problem, existing methods rely on the solver's built-in algorithm, and the increased constraint complexity can easily lead to solution failure. The embodiments of this invention do not rely on the solver's built-in algorithm, but iterate between states based on the state transition equations of a dynamic programming algorithm, resulting in a more stable solution.

[0068] Compared to the methods mentioned in the background section for solving the globally optimal trajectory planning problem on a known map, existing methods, while yielding optimal routes, cannot directly control and track the vehicle. The path planning module of this invention considers the vehicle's coordinates, yaw angle, and front wheel steering angle, resulting in a globally optimal path with continuous curvature that satisfies vehicle kinematic constraints, allowing for direct vehicle tracking and control.

[0069] Step 13: Solve the optimal control problem based on dynamic programming.

[0070] This embodiment divides the map space containing the map information into multiple sub-state spaces based on the coordinate system. The drivable range for the next stage is determined according to the vehicle's front wheel steering angle constraint. The optimal sequence of control variables in each interval is determined to optimize the objective function, thus obtaining the globally optimal trajectory across the entire map space. Therefore, step 13 specifically includes:

[0071] Step 131: Divide the map space into multiple sub-state spaces in the Frenet coordinate system, and discretize the sub-state spaces: In the Frenet coordinate system, the motion state of the vehicle can be described as... Where: s is the longitudinal distance along the reference line. For longitudinal velocity, Let θ be the longitudinal acceleration, l be the lateral distance, l′ be the first derivative of the lateral distance with respect to s, and l″ be the second derivative of the lateral distance with respect to s. This section studies the global path planning of the vehicle. Based on the map information provided by the mapping module (including road width, obstacle positions, etc.), the road centerline is collected as a reference line to establish a Frenet coordinate system and perform state discretization. The discretization of the road map is based on [s, l, θ]. x Three dimensions, θ x Let be the vehicle yaw angle in Cartesian coordinates. Corresponding to ds, dl, dθ x Discrete quantities are partitioned into sub-state spaces, while removing the state space that collides with obstacles or road boundaries, such as... Figure 3 As shown. Step 132, based on the vehicle's front wheel steering angle constraint, determine the drivable range for the next stage using the state transition equation: The above optimal control problem involves the front wheel steering angle δ x To control the amount, based on the range of the vehicle's front wheel steering angle, according to dδ xDiscretize it. Simplify the vehicle into a bicycle model, such as... Figure 4 As shown:

[0072] From the vehicle model, the curvature calculation formula (3) can be obtained:

[0073]

[0074] Among them, κ x R represents the curvature of the vehicle's travel path. x L represents the turning radius of a vehicle. x For the vehicle wheelbase, δ x Let be the front wheel steering angle of the vehicle. The discrete curvature control sequence obtained from the discrete front wheel steering angle can be calculated from equation (3).

[0075] Based on the transformation relationship between the Frenet coordinate system and the Cartesian coordinate system, the following formulas (4)-(6) can be derived:

[0076]

[0077]

[0078]

[0079] Among them, κ r Let κ be the curvature of the vehicle's projection point on the reference line. r ′ represents κ r With respect to the first derivative of the longitudinal distance s, θ r Let x be the angle between the tangent direction of the projection point of the vehicle on the reference line and the x-axis. From formulas (3) and (4), it can be seen that if the current state of the vehicle is known, the value of the next state of the vehicle is only related to the curvature of the vehicle in the next stage, that is, only related to the front wheel steering strategy of the vehicle in the next stage. As mentioned above, the discretization of the Frenet coordinate system state is divided into stages according to ds along the reference line. According to the state transition formulas (3)-(6), the control quantity of the front wheel steering angle of the vehicle is discretized, and the drivable range of the vehicle in the next stage can be calculated based on the current stage state quantity.

[0080] Step 133: Optimize the objective function by determining the optimal sequence form of the control variables in each interval to obtain the globally optimal trajectory in the entire map space. The idea of ​​dynamic programming is to decompose a problem into several stages, calculate the optimal strategy for each stage based on the state transition equation and cost function, and store the results. Dynamic programming problems satisfy the requirement of no aftereffect, that is, the decision of the current stage will not affect the subsequent decision. The optimal strategy of each stage provides effective information for the subsequent stages. Solve the state for each stage in turn until the target stage, and finally backtrack the decision to obtain the optimal decision sequence.

[0081] likeFigure 4 The determination condition one guarantees the path curvature continuity between two stages, κ x,dp,k+1 and κ x,dp,k respectively represent the curvature values of the k+1 stage and the k stage obtained by dynamic programming solution; the determination conditions two and three guarantee that the deviation of the next stage state calculated by the state transition equation and the discrete state quantity is within a certain range, where l dp,s(k+1) and θ x,dp,s(k+1) represent the lateral distance and the vehicle yaw angle of s (k+1) corresponding to the k+1 stage obtained by dynamic programming solution, l s(k+1) and θ x,s(k+1) represent the lateral distance and the yaw angle of s (k+1) corresponding to the k+1 stage, and ε1, ε2, ε3 are the deviation thresholds; when the three determination conditions are met at the same time, the value of the stage is recorded. At the end of the current stage, the minimum value of the stage quantity and the optimal control strategy are calculated; after reaching the target state, the minimum cost strategy of the target function corresponding to the target state is calculated and backtracked to obtain the global optimal path in the map environment.

[0082] The embodiment is a global optimal path planning method based on dynamic programming in the Frenet coordinate system. The vehicle state quantity of each stage considers three dimensions of the lateral and longitudinal coordinates and the vehicle yaw angle [s, l, θ x ], and the control quantity considers the front wheel steering angle δ x . Unlike the traditional path planning method based on dynamic programming, which only considers two dimensions of [x, y] or [s, l], the state discretization of three dimensions more accurately expresses the vehicle pose state; and the front wheel steering angle δ x is used as the control quantity to accurately express the curvature information in the vehicle driving process.

[0083] Obviously, the global optimal trajectory planning method based on optimal control in the background art which uses the direct method for solution depends on a better initial solution, and without an initial solution or a poor initial solution may easily lead to a local optimal solution. The method provided in the embodiment does not require an initial solution and can obtain the optimal strategy of each sub-state space, and the global trajectory obtained is certainly the optimal solution in the discrete space that meets the target function.

[0084] Of course, in the above embodiment, the discretization according to ds and the setting of the state quantity to three dimensions of [s, l, θ x ] can be replaced by the discretization according to dt and the setting of the state quantity to or The corresponding steps can be briefly summarized as including: (1) deriving state calculation formulas of each variable according to the coordinate conversion formula, as shown in formulas (3)-(6); (2) discretizing with acceleration a and front wheel steering angle δ as control variables; (3) calculating the next stage state according to the formula derived in formula (1) based on the current state variable; (4) selecting the optimal decision corresponding to each state variable; (5) finally backtracking to obtain the optimal trajectory.

[0085] Step 2, planning a globally optimal speed based on dynamic programming, specifically including:

[0086] Step 21, calculating the global path S global according to the known global path, and following the global path S global discretizing the path according to dS global , and establishing an optimal control problem:

[0087] The acceleration of the vehicle is pre-set to remain unchanged within the dS global length range, that is, the vehicle makes uniform acceleration straight line motion within the dS global length range, and in each dS global length range corresponding stage, the vehicle acceleration a x is taken as a control variable, and ξ = [S x , v x ] is taken as a state variable, wherein S x is the distance value along the global path S global , that is, the length along the global path from the starting point of the global path, and v x is the vehicle speed value, thereby establishing an optimal control problem model as shown in formula (7):

[0088]

[0089] wherein formula (7a) is an objective function, S x,k+1 -S x,k represents the path distance between two stages, S x,k is the distance value along the global path S global in the kth stage, S x,k+1 is the distance value along the global path S global in the k+1th stage, (v x,k+1 +v x,k ) / 2 represents the average speed between two stages, v x,k is the speed value of the vehicle in the kth stage, v x,k+1 is the speed value of the vehicle in the k+1th stage, represents the time spent from the kth stage to the k+1th stage; formula (7b) is a state transition equation between different stage state variables; formula (7c) is a boundary limit range of the control variable, adec,max ,a acc,max These represent the upper and lower limits of acceleration, respectively; formula (7d) represents the boundary limit range of velocity.

[0090] Step 22, based on the known maximum speed value ν of the vehicle. max For each path stage at 0:ν max Between according to dν max Speed ​​discretization was performed. Since reversing is not permitted in this application environment, the resulting discrete state diagram is as follows: Figure 5 As shown.

[0091] Step 23, Solve for the optimal speed using dynamic programming: To speed up the calculation and reduce the computational load, calculate the forward speed S at each stage based on the current distance and speed state variables. pre Within the length range, reachable state quantities satisfying constraint (8) are recorded, along with the cost of reaching that state quantity. At the end of the current stage, the minimum cost of each state quantity reaching that stage and the optimal control strategy are calculated. After reaching the target state, the minimum cost strategy of the objective function corresponding to the target state is calculated and backtracking is performed to obtain the optimal velocity and acceleration control strategy for the corresponding global path point. The state transition diagram and dynamic programming solution process diagram are shown below. Figure 6 and 7 As shown:

[0092]

[0093] a dec,max ≤a x,k+1 ≤a acc,max

[0094] This invention considers the curvature continuity of the vehicle path and the consistency between the vehicle state and discrete state variables in the solution process of each stage of dynamic programming. Using the concept of differentiation, the environmental space is [s,l,θ] x After partitioning the multidimensional sub-state space, the rotation angle δ of the previous wheel... x To control the variables and discretize them, the curvature value is calculated with reference to the bicycle model to ensure that the curvature change between adjacent stages is within a certain threshold to guarantee curvature continuity. The state of the vehicle at each stage is calculated based on the discrete control variables and the state transition equation, and the consistency between the vehicle state and the discrete state variables is judged in order to obtain the optimal strategy corresponding to each discrete variable.

[0095] It should be noted that the path-velocity decoupled trajectory planning described in the above embodiments can be replaced by an optimal trajectory planning method based on dynamic programming and path-velocity coupling.

[0096] like Figure 8 and Figure 9As shown, the embodiment of the present application also provides a global optimal trajectory planning system for intelligent vehicle based on dynamic programming, which comprises a mapping and positioning unit, a global optimal trajectory planning unit, a motion control unit and a bottom execution unit, wherein:

[0097] The mapping and positioning unit is used to obtain environment information, which is processed and then issued to the global optimal trajectory planning unit.

[0098] The global optimal trajectory planning unit is used to convert the road environment in the map information from the Cartesian coordinate system to the Frenet coordinate system according to the map information, vehicle chassis parameter information and start point and end point pose information, to discretize the state space, to establish a first optimal control problem model according to the trajectory planning task, to solve the optimal control problem by using the dynamic programming method, to obtain the path point sequence of the global optimal path; the global optimal trajectory planning unit is also used to discretize the state space according to the path point sequence and the vehicle dynamics parameter information, to establish a second optimal control problem model by referring to the trajectory planning task, to solve the optimal control problem by using the dynamic programming method, to obtain the optimal speed of the global optimal path, to obtain the global optimal trajectory, and to output the global optimal trajectory to the motion control unit.

[0099] The motion control unit is used to calculate the control instruction according to the global optimal trajectory, and to issue the control instruction to the bottom execution unit to control the vehicle, so as to realize the precise and smooth motion control of the unmanned vehicle.

[0100] Finally, it should be pointed out that: the above embodiments are only used to illustrate the technical solutions of the present application, but not to limit it. Those skilled in the art should understand that the technical solutions described in the foregoing embodiments can be modified, or some technical features can be replaced by equivalents; these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the spirit and scope of the technical solutions of the embodiments of the present application.

Claims

1. A dynamic programming based intelligent vehicle global optimal trajectory planning method, characterized in that, Comprise: Step 1, according to the map information, vehicle chassis parameter information and start and end pose information, the road environment in the map information is converted from the Cartesian coordinate system to the Frenet coordinate system, the state space is discretized, and a first optimal control problem model is established according to the trajectory planning task, and the optimal control problem is solved by using the dynamic programming method, and the path point sequence of the global optimal path is obtained; The setting method of the Frenet coordinate system comprises: According to the preset reference line T ref The position coordinates of the intelligent driving vehicle in the Cartesian coordinate system are set as (x, y), the projection is made from the vehicle position (x, y) to the reference line T ref The projection point is M, the distance between the point M and the vehicle position (x, y) is the lateral distance l, and the curve distance from the starting point to the projection point M is the longitudinal distance s. ref The coordinate value of the vehicle in the Frenet coordinate system is represented by (s, l), and the mapping relationship between the Frenet coordinate system and the Cartesian coordinate system is represented by the following formula (1): wherein, is the position vector of the projection point M of the vehicle position on the reference line in the global coordinate system, is the normal vector and tangent vector of the projection point M on the reference curve; Step 2, according to the path point sequence and the dynamic parameter information of the vehicle, the state space is discretized, a second optimal control problem model is established according to the trajectory planning task, the optimal control problem is solved by using the dynamic programming method, and the optimal speed of the global optimal path is obtained, so as to obtain the global optimal trajectory; The first optimal control problem model is set as: wherein, formula (2a) is an objective function, ||x k+1 -x k || represents the distance of path points between two stages, x k = (x k , y k ) = f1(s k , l k ), ||x k+1 -x k || max represents the maximum value of the distance of path points between two stages, |δ k+1 -δ k | represents the change amount of the front wheel steering angle control amount of the vehicle between two stages, δ max represents the maximum value of the front wheel steering angle, κ x,k represents the path curvature value of the kth stage, κ x,max represents the maximum path curvature value of the kth stage, ω1, ω2, ω3 represent weight values, formula (2b) is a state transition equation between different stage state quantities, formula (2c) is a boundary limit range of the control amount, formula (2d) is a road boundary and obstacle constraint, l left (s k ) represents the left boundary of l corresponding to s k , l right (s k ) represents the right boundary of l corresponding to s k .

2. The dynamic programming based intelligent vehicle global optimal trajectory planning method of claim 1, wherein, The method of "solving the optimal control problem by using the dynamic programming method" in step 1 comprises: In the Frenet coordinate system, the map space is divided into a plurality of sub-state spaces, and the sub-state spaces are discretized, then according to the front wheel steering angle constraint of the vehicle, the state transition equation is used to determine the drivable range of the next stage, and then the optimal sequence form of the control variable on each interval is determined to optimize the objective function, so as to obtain the global optimal trajectory in the whole map space.

3. The dynamic programming based intelligent vehicle global optimal trajectory planning method of claim 2, wherein, The method of "dividing the map space into a plurality of sub-state spaces in the Frenet coordinate system and discretizing the sub-state spaces" comprises: According to the map information provided by the mapping module, the road center line is collected as a reference line to establish a Frenet coordinate system and to perform state discretization. The discretization of the road map is according to [s, l, θ x ] three dimensions, s, l, θ x respectively longitudinal distance, transverse distance, and vehicle yaw angle under the Cartesian coordinate system.

4. The dynamic programming based intelligent vehicle global optimal trajectory planning method of claim 3, wherein, The method of "determining the drivable range of the next stage according to the front wheel steering angle constraint of the vehicle and using the state transition equation" comprises: The optimal control problem described by equation (2) is first discretized with respect to the front wheel steering angle δ x is the control variable, and dδ x is the discretization of the front wheel steering angle δ x The discretized curvature control sequence resulting from the discretized front wheel steering angle δ is calculated from equation (3) wherein, κ x represents the vehicle path curvature, R x represents the vehicle turning radius, L x is the vehicle wheelbase, δ x is the vehicle front wheel steering angle; The drivable range of the vehicle in the next stage is calculated according to the state transition equation obtained from the conversion relationship between the Frenet coordinate system and the Cartesian coordinate system.

5. A dynamic programming based intelligent vehicle globally optimal trajectory planning system, characterized in that, Comprise a mapping and positioning unit, a global optimal trajectory planning unit, a motion control unit and a bottom execution unit, wherein: The mapping and positioning unit is used to obtain environmental information, which is processed and then sent to the global optimal trajectory planning unit; The global optimal trajectory planning unit is used to convert the road environment in the map information from the Cartesian coordinate system to the Frenet coordinate system, discretize the state space, and then establish a first optimal control problem model according to the trajectory planning task, solve the optimal control problem by using the dynamic programming method, and obtain the path point sequence of the global optimal path; The global optimal trajectory planning unit is also used to discretize the state space according to the path point sequence and the dynamic parameter information of the vehicle, establish a second optimal control problem model according to the trajectory planning task, solve the optimal control problem by using the dynamic programming method, and obtain the optimal speed of the global optimal path, so as to obtain the global optimal trajectory, and then output the global optimal trajectory to the motion control unit; The motion control unit is used to calculate the control instruction according to the global optimal trajectory, and send the control instruction to the bottom execution unit to control the vehicle; The setting method of the Frenet coordinate system comprises: According to the preset reference line T ref The position coordinates of the intelligent driving vehicle in the Cartesian coordinate system are set as (x, y), the projection is made from the vehicle position (x, y) to the reference line T ref The projection point is M, the distance between the point M and the vehicle position (x, y) is the lateral distance l, and the distance along T ref The curve distance from the starting point to the projection point M is the longitudinal distance s, the coordinate value of the vehicle in the Frenet coordinate system is represented as (s, l), and the mapping relationship between the Frenet coordinate system and the Cartesian coordinate system is constructed as shown in the following formula (1): wherein, is the position vector of the projection point M of the vehicle position on the reference line in the global coordinate system, is the normal vector and tangent vector of the projection point M on the reference curve; The first optimal control problem model is set as: wherein, formula (2a) is an objective function, ||x k+1 -x k || represents the distance of path points between two stages, x k =(x k ,y k )=(f1(s k ,l k ),||x k+1 -x k || max represents the maximum value of path point distance between two stages, |δ k+1 -δ k | represents the change amount of front wheel steering control amount of the vehicle between two stages, δ max represents the maximum value of front wheel steering angle, κ x,k represents the path curvature value of the kth stage, κ x,max represents the maximum path curvature value of the kth stage, ω1, ω2, ω3 represent weight values, formula (2b) is a state transition equation between state quantities of different stages, formula (2c) is a boundary limit range of control quantity, formula (2d) is a road boundary and obstacle constraint, l left (s k ) represents the left boundary of l corresponding to s k , l right (s k ) represents the right boundary of l corresponding to s k .

6. The dynamic programming based intelligent vehicle globally optimal trajectory planning system of claim 5, wherein, The method of solving the optimal control problem by using the dynamic programming method comprises: In the Frenet coordinate system, the map space is divided into multiple sub-state spaces, and the sub-state spaces are discretized, and then according to the front wheel steering angle constraint of the vehicle, the state transition equation is used to determine the drivable range of the next stage, and then the optimal sequence form of the control variable on each interval is determined to optimize the objective function, so as to obtain the global optimal trajectory under the whole map space.

7. The dynamic programming based intelligent vehicle globally optimal trajectory planning system of claim 6, wherein, The method for dividing the map space into multiple sub-state spaces in the Frenet coordinate system and discretizing the sub-state spaces specifically comprises: According to the map information provided by the mapping module, the road center line is collected as a reference line to establish a Frenet coordinate system and to perform state discretization. The discretization of the road map is according to [s, l, θ x ] three dimensions, s, l, θ x respectively longitudinal distance, transverse distance, and vehicle yaw angle under the Cartesian coordinate system.

8. The dynamic programming based intelligent vehicle globally optimal trajectory planning system of claim 7, wherein, The method for determining the drivable range of the next stage according to the front wheel steering angle constraint of the vehicle and using the state transition equation specifically comprises: The optimal control problem described by equation (2) is first discretized with respect to the front wheel steering angle δ x is the control variable, and according to the range of the front wheel steering angle of the vehicle, the dδ x is discretized with respect to the front wheel steering angle δ x The discrete curvature control sequence obtained according to the discretized front wheel steering angle can be calculated by equation (3): wherein, κ x represents the vehicle path curvature, R x represents the vehicle turning radius, L x is the vehicle wheelbase, δ x is the vehicle front wheel steering angle; The drivable range of the vehicle in the next stage is calculated according to the state transition equation obtained from the conversion relationship between the Frenet coordinate system and the Cartesian coordinate system.

Images