A decoupled real-time trajectory planning method, device and system for autonomous driving vehicles

Abstract

The present invention relates to a decoupled real-time trajectory planning method, device and system for autonomous driving vehicles, which belongs to the field of autonomous driving. The method decouples a trajectory planning problem in a three-dimensional space into two two-dimensional trajectory planning problems, and uses a cell decomposition method and a dynamic programming algorithm to obtain a rough path and a trapezoidal corridor in each two-dimensional problem. At the same time, a quadratic programming algorithm is used to optimize and solve each two-dimensional problem in the trapezoidal corridor, and finally a feasible trajectory is obtained by merging trajectories. The method solves the problems that the three-dimensional problem of autonomous driving vehicles is difficult to solve during trajectory planning, the trajectory obtained by using a sampling method is not optimal and is not suitable for complex urban road scenes, and small obstacles cannot be handled in a discrete planning space, and can plan a collision-free, executable and efficient trajectory in real time.

Description

Technical Field

[0001] The present invention relates to the field of autonomous driving, and in particular to a decoupled real-time trajectory planning method, device and system for an autonomous driving vehicle. Background Art

[0002] The task of trajectory planning for autonomous vehicles is to use map data, perception data, positioning data, and vehicle status information to calculate a collision-free and executable trajectory to ensure that the vehicle drives safely from the starting point to the destination as efficiently as possible. The essence of the problem is a multi-objective mathematical optimization problem, and its main optimization goals include safety, stability, comfort, and driving efficiency. However, in actual driving scenarios, various physical constraints such as vehicle acceleration and deceleration constraints, non-integrity constraints, and dynamic constraints should also be included.

[0003] If the movement of the autonomous vehicle in the z direction is not considered, the trajectory planning problem is a three-dimensional problem, that is, the trajectory output by the planning module should be a function of position over time. However, three-dimensional optimization problems are often difficult to solve directly, so the current mainstream approach is to decouple the three-dimensional trajectory planning problem into two two-dimensional planning problems using the Frenet coordinate system, namely the lateral planning problem and the longitudinal planning problem.

[0004] The existing "A method and system for autonomous driving planning for multiple scenarios" samples different combinations of lateral displacement and longitudinal displacement in lateral planning, and samples different combinations of time and longitudinal trajectories in longitudinal planning, and connects them using polynomials, and then uses a specific cost function to select the trajectory with the lowest cost among the candidate trajectories for merging. If there is no optimal trajectory, the reference route is regenerated. However, this method has the problem that even if the trajectory is successfully generated, it cannot guarantee the optimality of the trajectory, because it uses a sampling method, so there is a feasible trajectory, but it is not sampled, which leads to the possibility of planning failure. In addition, this method cannot guarantee whether there are obstacles between the sampled trajectories, because it can only detect the safety of the sampled trajectories, so if there are uncertain small obstacles between the sampled trajectories, it cannot handle it. Furthermore, after its planning fails, it fails to find a safe trajectory, and its sampling method of regenerating the reference line is used for processing. However, in actual scenarios, after the planning fails, the backup trajectory should be called to stop the vehicle and then execute subsequent related instructions.

[0005] Whether planning in a Cartesian coordinate system or under Frenet, most existing technologies use sampling methods to obtain trajectories. However, the trajectories obtained by sampling are suboptimal and cannot meet optimality. There may be feasible trajectories that were originally available, but because they were not sampled, the planning may fail. In addition, the sampling method cannot guarantee whether there are obstacles between the sampled trajectories, because it can only detect the safety of the sampled trajectories. Therefore, if there are small obstacles with uncertainty between the sampled trajectories, it cannot be handled. Furthermore, the sampling method can only be applied to simpler scenarios, such as highways or simple urban roads. It is difficult to sample feasible trajectories in complex scenarios.

[0006] It can be seen that trajectory planning is a continuous problem. No matter whether the trajectory is obtained by sampling or discrete longitudinal displacement, the trajectory obtained in either lateral planning or longitudinal planning is suboptimal and cannot reach the optimal level. Moreover, it cannot guarantee the safety between sampled trajectories or discrete longitudinal displacement points, so it may obtain an unsafe trajectory. Summary of the invention

[0007] The purpose of the present invention is to provide a decoupled real-time trajectory planning method, device and system for autonomous driving vehicles, so as to plan a collision-free, executable and efficient trajectory in real time.

[0008] To achieve the above object, the present invention provides the following solutions:

[0009] A decoupled real-time trajectory planning method for an autonomous driving vehicle, the planning method comprising:

[0010] Obtain the position information of the vehicle and the obstacles around it in real time;

[0011] A section of the path including the current posture of the vehicle is intercepted on the global path of the vehicle as a reference line, and the reference line is converted from the Cartesian coordinate system to the Frenet coordinate system to obtain the longitudinal displacement-lateral displacement diagram;

[0012] The obstacles whose moving speed is less than the speed threshold are projected into the longitudinal displacement-lateral displacement map;

[0013] Using the cell decomposition method, a decomposition line is established for each vertex of each obstacle in the longitudinal displacement-lateral displacement graph;

[0014] Arrange multiple sampling points on each decomposition line, and connect the sampling points on the decomposition line in sequence from the starting point to form multiple candidate paths; the candidate paths are composed of multiple straight line segments connected end to end in sequence, and the straight line segment is a line segment formed by connecting the starting point with the sampling points on the adjacent decomposition line or by connecting the sampling points on the adjacent decomposition lines;

[0015] A dynamic programming algorithm is used to calculate the cost of each candidate path, and the candidate path with the smallest cost is used as the rough path in the longitudinal displacement-lateral displacement graph;

[0016] The borders of the quadrilaterals where each straight line segment on the rough path is located form a trapezoidal corridor, and the rough path is optimized in the trapezoidal corridor using a piecewise Bezier path curve to obtain a lateral optimized trajectory;

[0017] constructing a longitudinal displacement-time graph according to the lateral optimized trajectory;

[0018] Project obstacles whose moving speed is greater than or equal to the speed threshold into the longitudinal displacement-time graph, and replace the longitudinal displacement-lateral displacement graph with the longitudinal displacement-time graph, and return to the step "Use the cell decomposition method to establish a decomposition line for each vertex of each obstacle in the longitudinal displacement-lateral displacement graph" to obtain the longitudinal optimized trajectory;

[0019] The lateral optimization trajectory and the longitudinal optimization trajectory are merged, and a safety check is performed on the merged optimization trajectory, and the merged optimization trajectory that passes the safety check is determined as the optimal feasible trajectory.

[0020] Optionally, projecting an obstacle whose moving speed is less than a speed threshold into a longitudinal displacement-lateral displacement map specifically includes:

[0021] When an obstacle whose moving speed is less than the speed threshold is a static obstacle, each vertex in the bounding box of the static obstacle is converted from the Cartesian coordinate system to the Frenet coordinate system, and the static obstacle is projected in the longitudinal displacement-lateral displacement map;

[0022] When an obstacle whose moving speed is less than the speed threshold is a low-speed obstacle, each vertex in the bounding box of the low-speed obstacle is converted from the Cartesian coordinate system to the Frenet coordinate system, and the low-speed obstacle is projected in the longitudinal displacement-lateral displacement map; the low-speed obstacle has the same driving direction as the vehicle, and the moving speed of the low-speed obstacle is greater than zero and less than the speed threshold;

[0023] When an obstacle whose moving speed is less than the speed threshold is an oncoming obstacle, each vertex in the bounding box at the position where the predicted trajectory of the oncoming obstacle collides with the current trajectory of the vehicle is converted from the Cartesian coordinate system to the Frenet coordinate system, and the oncoming obstacle is projected in the longitudinal displacement-lateral displacement map.

[0024] Optionally, the method of using the cell decomposition method to establish a decomposition line for each vertex of each obstacle in the longitudinal displacement-lateral displacement graph specifically includes:

[0025] Establish a vertical line at each vertex of each obstacle;

[0026] Select the intersection point of the vertical line and the longitudinal displacement-lateral displacement map boundary box and the intersection point of the vertical line and the obstacle boundary box as candidate intersection points;

[0027] Select the candidate intersection points closest to each vertex on both sides of each vertex along the vertical line;

[0028] Connect each vertex with the two selected candidate intersection points respectively to obtain two connecting line segments for each vertex;

[0029] Delete the connected line segments that intersect with obstacles to obtain a decomposition line for each vertex of each obstacle.

[0030] Optionally, the method of using a dynamic programming algorithm to calculate the cost of each candidate path and using the candidate path with the smallest cost as a rough path in the longitudinal displacement-lateral displacement graph specifically includes:

[0031] The decomposition lines are numbered in sequence starting from the first layer along the positive direction of the s-axis of the longitudinal displacement-lateral displacement diagram;

[0032] Initialize the number of layers u = 1;

[0033] Using formula C total (L v )=C smooth (L v )+C obs (L v )+C guidance (L v ), calculate the cost value of each straight line segment between the starting point and the uth layer in the longitudinal displacement-lateral displacement diagram; where C total (L v ) is the cost value of the vth straight line segment between the starting point and the uth layer, C smooth (L v ) is the smoothness cost of the vth straight line segment between the starting point and the uth layer, C obs (L v ) is the distance cost between the starting point and the u-th layer and the obstacle, C guidance (L v ) is the cost of the vth straight line segment between the starting point and the uth layer and the reference line; C smooth (L v )=w1*L′ v 2 +w2*L″ v 2 +w3*L″′ v 2 , w1, w2 and w3 are the weights of the first-order derivative, second-order derivative and third-order derivative respectively, L′ v , L″ v and L″′v are the first-order derivative, second-order derivative, and third-order derivative of the vth straight line segment, respectively. p is the number of discrete straight line segments of the vth line, l j is the lateral displacement of the jth discrete point, and ds is the discrete longitudinal interval; and are the lengths of the long side and short side of the decomposition line where the right endpoint of the vth straight line segment is located; C guidance (L v )=L v -G v (s), G v (s) is the lateral displacement of the reference line at the vth straight line segment between the starting point and the uth layer at s, L v is the lateral displacement of the vth straight line segment between the starting point and the uth layer,

[0034] The straight line segment with the smallest cost value is taken as the optimal straight line segment between the starting point and the u-th layer;

[0035] Take the intersection of the optimal straight line segment on the uth layer as the starting point, increase the value of u by 1, and return to step "Use formula C total (L v )=C smooth (L v )+C obs (L v )+C guidance (L v ), calculate the cost value of each straight line segment between the starting point and the u-th layer, until the value of u is greater than the maximum number of layers, stop the loop, and obtain the rough path in the longitudinal displacement-lateral displacement diagram.

[0036] Optionally, a dynamic programming algorithm is used to calculate the cost of each candidate path, and the candidate path with the smallest cost is used as a rough path in the longitudinal displacement-time graph, specifically including:

[0037] The decomposition lines are numbered sequentially starting from the first layer along the positive direction of the t-axis of the longitudinal displacement-time diagram;

[0038] Initialize the number of layers u = 1;

[0039] Using formula C total (S v )=w1*(S′ v -V ref )+w2*S″ v 2 +w3*S″′ v 2 +w4*C obs(s), calculate the cost value of each straight line segment between the starting point and the uth layer in the longitudinal displacement-time diagram; where C total (S v ) is the cost of the vth straight line segment between the starting point and the uth layer in the longitudinal displacement-time diagram, w4 is the cost of the distance to the obstacle, V ref is the reference speed of the current road segment, C obs (s) is the distance cost between the vth straight line segment and the obstacle between the starting point and the uth layer in the longitudinal displacement-time graph, and are the lengths of the long side and short side of the decomposition line where the right endpoint of the vth straight line segment in the longitudinal displacement-time diagram is located, S′ v , S″ v and S″′ v are the first-order derivative, second-order derivative, and third-order derivative of the vth straight line segment, respectively. s j is the longitudinal displacement of the jth discrete point, dt is the discrete longitudinal interval;

[0040] The straight line segment with the smallest cost value is taken as the optimal straight line segment between the starting point and the u-th layer;

[0041] Take the intersection of the optimal straight line segment on the uth layer as the starting point, increase the value of u by 1, and return to step "Use formula C total (S v )=w1*(S′ v -V ref )+w2*S″ v 2 +w3*S″′ v 2 +w4*C obs (s), calculate the cost value of each straight line segment between the starting point and the uth layer in the longitudinal displacement-time graph, until the value of u is greater than the maximum number of layers, stop the loop, and obtain the rough path in the longitudinal displacement-time graph.

[0042] Optionally, the step of optimizing the trajectory of the rough path in the trapezoidal corridor by using a piecewise Bezier path curve specifically includes:

[0043] When optimizing the lateral trajectory of a coarse path in a trapezoidal corridor using a piecewise Bezier path curve:

[0044] The cost function for constructing lateral trajectory optimization is Among them, J path represents the cost of trajectory optimization, where trajectory optimization is horizontal trajectory optimization or vertical trajectory optimization; k is the number of the piecewise Bezier curve, which corresponds to the number of the trapezoid in the trapezoidal corridor; s is the longitudinal displacement, l is the lateral displacement, m is the total number of segments of the piecewise Bezier curve, and Sk is the width of the kth trapezoid, l k (s) is the kth Bezier curve, w l is the weight of the lateral displacement, w l′ is the weight of the first-order derivative of the lateral displacement with respect to the longitudinal displacement, w l″ is the weight of the second-order derivative of the lateral displacement with respect to the longitudinal displacement, w l″′ is the weight of the third-order derivative of the lateral displacement with respect to the longitudinal displacement, w ref is the weight of the lateral displacement and the reference error, is the straight line segment of the rough path in the kth trapezoid; S1, S2 and S m are the widths of the 1st, 2nd and mth trapezoids, respectively, T m-1 is the left boundary of the mth trapezoid, T m is the right boundary of the mth trapezoid, Respectively represent the i-th control point of the 1st, 2nd, and mth Bezier curves, Represents the coefficient of the i-th Bezier curve control point, which is in the form of in t represents the independent variable, t is the longitudinal displacement s in the path planning cycle, t is the time in the speed planning cycle, and n represents the number of control points;

[0045] The constraints for constructing the lateral trajectory optimization are:

[0046]

[0047]

[0048]

[0049]

[0050]

[0051]

[0052] in, is the i-th control point of the r-th derivative of the k-th Bezier curve, is the nth control point of the rth derivative of the kth Bezier curve, is the 0th control point of the rth derivative of the k+1th Bezier curve, S k is the width of the kth trapezoid, S k , S k+1 to the (1-r)th power, Indicates that when s=s kWhen k The r-order derivative of (s) with respect to s, is the lower boundary function of the kth trapezoid, is the upper boundary function of the kth trapezoid, l′ min and l″ max are the minimum and maximum values ​​of the first-order derivative of the lateral displacement with respect to the longitudinal displacement; l″ min and l″ max are the minimum and maximum values ​​of the second-order derivative of the lateral displacement with respect to the longitudinal displacement; l″ min and l″ max are the minimum and maximum values ​​of the third-order derivative of the lateral displacement with respect to the longitudinal displacement;

[0053] When optimizing the longitudinal trajectory of a rough path in a trapezoidal corridor using a piecewise Bezier path curve:

[0054] The cost function for constructing longitudinal trajectory optimization is Among them, J speed is the cost of optimizing the longitudinal trajectory, s is the longitudinal displacement, t is the lateral displacement, and it is also the total number of trapezoids in the trapezoidal corridor, T k is the width of the kth trapezoid, s k (t) is the kth Bezier curve, is the straight line segment of the rough path in the kth trapezoid in the longitudinal displacement-time diagram, w s is the weight of the longitudinal displacement close to the reference longitudinal displacement, w s is the weight of the longitudinal speed approaching the reference speed, is the weight of the longitudinal acceleration, is the weight of longitudinal acceleration;

[0055] The constraints for constructing the longitudinal trajectory optimization are:

[0056]

[0057]

[0058]

[0059]

[0060]

[0061]

[0062]

[0063] in, is the i-th control point of the r-th derivative of the k-th Bezier curve, T k is the width of the kth trapezoid, is the lower boundary function of the kth trapezoid, is the upper boundary function of the kth trapezoid, v min With v max are the minimum and maximum longitudinal speed respectively; a min with a max are the minimum and maximum values ​​of longitudinal acceleration respectively; j min With j max are the minimum and maximum values ​​of longitudinal acceleration respectively; a cm is the maximum centrifugal acceleration; κ k is the maximum curvature of the kth Bezier curve;

[0064] The cost function and constraints of lateral trajectory optimization and the cost function and constraints of longitudinal trajectory optimization are transformed into a second-order programming expression respectively:

[0065]

[0066] stl≤A c x≤u

[0067] Where x is l(s) or s in trajectory optimization k (t), P is the quadratic term of the cost function in trajectory optimization, q T is the linear term of the cost function in trajectory optimization, l and u are the lower and upper bounds of the constraint conditions in trajectory optimization, respectively, the lower and upper bounds of the equality constraint are both 0, A c The constraint matrix formed by the constraints in trajectory optimization;

[0068] Solve the second-order programming expression to obtain the lateral optimization trajectory or the longitudinal optimization trajectory.

[0069] Optionally, the merging of the lateral optimization trajectory and the longitudinal optimization trajectory specifically includes:

[0070] The time period on the longitudinal displacement-time graph is evenly divided to obtain a series of sampling time points;

[0071] The sampling time point t is obtained from the longitudinal optimization trajectory in the longitudinal displacement-time diagram i The longitudinal displacement s i , longitudinal speed and longitudinal acceleration

[0072] The longitudinal displacement s is obtained from the lateral optimization trajectory in the longitudinal displacement-lateral displacement diagram. i The lateral displacement l i, transverse velocity l′ i and the lateral acceleration l″ i ;

[0073] Determine a trajectory point in the frenet coordinate system

[0074] The determined trajectory point is converted into the Cartesian coordinate system to obtain a trajectory point in the Cartesian coordinate system (t i , x i ,y i ,s i ,θ i , κ i , v i , a i ), where x i With y i is the pose in the Cartesian coordinate system, θ i is the orientation in Cartesian coordinates, κ i is the curvature, v i with a i are velocity and acceleration respectively;

[0075] Repeat the above steps to obtain the trajectory points of all sampling time points in the Cartesian coordinate system, and connect all trajectory points in sequence to form a merged optimized trajectory.

[0076] Optionally, the performing safety detection on the merged optimized trajectory specifically includes:

[0077] Determine whether each trajectory point on the combined optimized trajectory meets the safety condition to obtain a first determination result; the safety condition is that the speed is less than or equal to the speed threshold, the acceleration is less than or equal to the acceleration threshold, the jerk is less than or equal to the jerk threshold, and the curvature is less than or equal to the curvature threshold;

[0078] If the first judgment result indicates yes, then judging whether each trajectory point collides with an obstacle to obtain a second judgment result;

[0079] If the second judgment result indicates no, then determining that the combined optimized trajectory passes the safety detection;

[0080] If the first judgment result indicates no or the second judgment result indicates yes, it is determined that the merged optimized trajectory does not pass the safety detection, and the merged optimized trajectory is determined as a backup trajectory.

[0081] A decoupled real-time planning device for an autonomous driving vehicle, the planning device comprising: a routing module, a positioning module, a perception module, a planning module and a control module;

[0082] The routing module, positioning module, and perception module are all connected to the input end of the planning module, and the control module is connected to the output end of the planning module;

[0083] The routing module is used to send the global path of the vehicle to the planning module;

[0084] The positioning module is used to send the current position of the vehicle to the planning module;

[0085] The perception module is used to send obstacle information around the vehicle to the planning module;

[0086] The planning module is used to obtain the optimal feasible trajectory according to the global path of the vehicle, the current posture and the obstacle information around the vehicle, using the aforementioned decoupled automatic driving vehicle real-time trajectory planning method, and send the optimal feasible trajectory to the control module;

[0087] The control module is used to control the vehicle to execute the optimal feasible trajectory.

[0088] A decoupled autonomous driving vehicle real-time planning system, the planning system comprising:

[0089] Real-time information acquisition module, used to obtain the position information of the vehicle and obstacles around the vehicle in real time;

[0090] The longitudinal displacement-lateral displacement map acquisition module is used to intercept a section of the path including the current posture of the ego vehicle on the global path of the ego vehicle as a reference line, and convert the reference line from the Cartesian coordinate system to the Frenet coordinate system to obtain the longitudinal displacement-lateral displacement map;

[0091] A projection module, used for projecting obstacles whose moving speed is less than a speed threshold into a longitudinal displacement-lateral displacement map;

[0092] A decomposition module, for establishing a decomposition line for each vertex of each obstacle in the longitudinal displacement-lateral displacement graph using a cell decomposition method;

[0093] The alternative path forming module is used to arrange multiple sampling points on each decomposition line, and connect the sampling points on the decomposition line in sequence from the starting point to form multiple alternative paths; the alternative paths are composed of multiple straight line segments connected end to end in sequence, and the straight line segment is a line segment formed by straight line connection between the starting point and the sampling points on the adjacent decomposition line or by straight line connection between the sampling points on the adjacent decomposition lines;

[0094] A rough path determination module is used to calculate the cost of each candidate path using a dynamic programming algorithm, and use the candidate path with the smallest cost as the rough path in the longitudinal displacement-lateral displacement graph;

[0095] A lateral optimization trajectory acquisition module is used to form a trapezoidal corridor with the border of the quadrilateral where each straight line segment on the rough path is located, and to optimize the trajectory of the rough path in the trapezoidal corridor using a piecewise Bezier path curve to obtain a lateral optimization trajectory;

[0096] A longitudinal displacement-time graph construction module, used to construct a longitudinal displacement-time graph according to the lateral optimized trajectory;

[0097] A longitudinal optimization trajectory acquisition module is used to project obstacles whose movement speed is greater than or equal to the speed threshold into the longitudinal displacement-time graph, and replace the longitudinal displacement-lateral displacement graph with the longitudinal displacement-time graph, and return to the step of "using the cell decomposition method to establish a decomposition line for each vertex of each obstacle in the longitudinal displacement-lateral displacement graph" to obtain the longitudinal optimization trajectory;

[0098] The optimal feasible trajectory determination module is used to merge the lateral optimized trajectory and the longitudinal optimized trajectory, and perform safety detection on the merged optimized trajectory, and determine the merged optimized trajectory that passes the safety detection as the optimal feasible trajectory.

[0099] According to the specific embodiments provided by the present invention, the present invention discloses the following technical effects:

[0100] The present invention discloses a decoupled real-time trajectory planning method, device and system for autonomous driving vehicles, which decouples a trajectory planning problem in a three-dimensional space into two two-dimensional trajectory planning problems, and uses a cell decomposition method and a dynamic programming algorithm to obtain a rough path and a trapezoidal corridor in each two-dimensional problem, and uses a quadratic programming algorithm to optimize and solve each two-dimensional problem in the trapezoidal corridor, and finally obtains a feasible trajectory by merging trajectories. The method solves the problems that the three-dimensional problem of autonomous driving vehicles is difficult to solve during trajectory planning, the trajectory obtained by using a sampling method is not optimal and is not suitable for complex urban road scenes, and small obstacles cannot be handled in a discrete planning space, and can plan a collision-free, executable and efficient trajectory in real time. BRIEF DESCRIPTION OF THE DRAWINGS

[0101] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative labor.

[0102] Figure 1 A schematic diagram of a decoupled real-time trajectory planning method for an autonomous driving vehicle provided in Embodiment 1 of the present invention;

[0103] Figure 2A schematic diagram of the conversion relationship between the Cartesian coordinate system and the Frenet coordinate system provided in the first embodiment of the present invention;

[0104] Figure 3 A schematic diagram of the principle of constructing a longitudinal displacement-lateral displacement map provided in the first embodiment of the present invention;

[0105] Figure 4 A schematic diagram of reference line smoothing provided in the first embodiment of the present invention;

[0106] Figure 5 A schematic diagram of a cell decomposition principle provided in the first embodiment of the present invention; Figure 5 (a) is a schematic diagram of the intersection formation. Figure 5 (b) is a schematic diagram of the formation of decomposition lines;

[0107] Figure 6 A schematic diagram of a trapezoidal corridor provided in Embodiment 1 of the present invention;

[0108] Figure 7 A schematic diagram of alternative paths in the longitudinal displacement-lateral displacement diagram provided in the first embodiment of the present invention;

[0109] Figure 8 A schematic diagram of a rough path in the longitudinal displacement-lateral displacement diagram provided in the first embodiment of the present invention;

[0110] Fig. 9 A schematic diagram of alternative paths in the longitudinal displacement-time diagram provided in the first embodiment of the present invention;

[0111] Fig.10 A schematic diagram of a rough path in the longitudinal displacement-time diagram provided in the first embodiment of the present invention;

[0112] Fig.11 A schematic diagram of the structure of a decoupled real-time planning device for an autonomous driving vehicle provided in Embodiment 2 of the present invention. DETAILED DESCRIPTION

[0113] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0114] The purpose of the present invention is to provide a decoupled real-time trajectory planning method, device and system for autonomous driving vehicles, so as to plan a collision-free, executable and efficient trajectory in real time.

[0115] In order to make the above-mentioned objects, features and advantages of the present invention more obvious and easy to understand, the present invention is further described in detail below with reference to the accompanying drawings and specific embodiments.

[0116] Embodiment 1

[0117] The present invention provides a decoupled real-time trajectory planning method for an autonomous driving vehicle, such as Figure 1 As shown, planning methods include:

[0118] Step 1: Obtain the position information of the vehicle and the obstacles around the vehicle in real time.

[0119] In addition to obtaining the position information of the vehicle itself, it can also obtain information such as the vehicle's geometry, speed, acceleration, chassis, and the position, geometry, type (such as pedestrians, vehicles, etc.), speed, predicted trajectory, and other information of obstacles.

[0120] Step 2: On the global path of the ego vehicle, a section of the path including the current posture of the ego vehicle is intercepted as a reference line, and the reference line is converted from the Cartesian coordinate system to the Frenet coordinate system to obtain a longitudinal displacement-lateral displacement diagram.

[0121] Based on the global path provided by the routing module and the ego vehicle information obtained in S1, a section of the path is intercepted on the global path as the reference line for each planning, and the intercepted reference line is path smoothed; the reference line is obtained by finding the path point closest to the current position of the ego vehicle on the global path, and then intercepting the global path from a certain distance behind the ego vehicle to a certain distance in front of the ego vehicle. The distance behind the ego vehicle and the distance in front of the ego vehicle can be adjusted according to the actual scene; the reference line smoothing method is not limited to a specific method, but the method should be able to ensure that the smoothed reference line itself is sufficiently smooth, as short as possible, as close as possible to the reference line before smoothing, and as continuous as possible. The smoothing diagram is shown in the figure below. Figure 4 shown.

[0122] The smoothed reference line is converted from the Cartesian coordinate system to the Frenet coordinate system to construct the sl diagram (longitudinal displacement-lateral displacement diagram), as shown in Figure 2-3 As shown in the figure, the s-axis of the Frenet coordinate system is the direction along the (smoothed) reference line, and the l-axis (d-axis in the figure) is the normal direction of the (smoothed) reference line.

[0123] Step 3: Project obstacles whose moving speed is less than the speed threshold into the longitudinal displacement-lateral displacement map.

[0124] Obstacles with a moving speed less than the speed threshold mainly refer to static obstacles, low-speed dynamic obstacles and obstacles coming from the opposite direction.

[0125] Exemplarily, obstacle projection specifically includes:

[0126] When the obstacle whose moving speed is less than the speed threshold is a static obstacle, each vertex in the bounding box of the static obstacle is converted from the Cartesian coordinate system to the Frenet coordinate system, and the static obstacle is projected in the longitudinal displacement-lateral displacement map. When the obstacle whose moving speed is less than the speed threshold is a low-speed obstacle, each vertex in the bounding box of the low-speed obstacle is converted from the Cartesian coordinate system to the Frenet coordinate system, and the low-speed obstacle is projected in the longitudinal displacement-lateral displacement map; the low-speed obstacle has the same driving direction as the ego vehicle, and the moving speed of the low-speed obstacle is greater than zero and less than the speed threshold. When the obstacle whose moving speed is less than the speed threshold is an incoming obstacle, each vertex in the bounding box at the position where the predicted trajectory of the incoming obstacle collides with the current trajectory of the ego vehicle is converted from the Cartesian coordinate system to the Frenet coordinate system, and the incoming obstacle is projected in the longitudinal displacement-lateral displacement map.

[0127] Based on the relevant information of the ego vehicle, the starting point of each path planning cycle is projected into the SL map. The starting point of each path planning cycle can be determined according to the actual scenario, but the starting point should not be too far away from the projection of the ego vehicle's posture in the SL map at this time.

[0128] Step 4: Use the cell decomposition method to establish a decomposition line for each vertex of each obstacle in the longitudinal displacement-lateral displacement graph.

[0129] As an example, refer to Figure 5 and Figure 6 , the specific process of cell decomposition method is:

[0130] Establish a vertical line at each vertex of each obstacle; select the intersection of the vertical line and the longitudinal displacement-lateral displacement map boundary box and the intersection of the vertical line and the obstacle boundary box as candidate intersection points; select the candidate intersection points closest to each vertex on both sides of each vertex along the vertical line; connect each vertex with the two selected candidate intersection points to obtain two connecting line segments for each vertex; delete the connecting line segments intersecting with the obstacle to obtain a decomposition line for each vertex of each obstacle.

[0131] That is, the cell decomposition method for decomposing obstacles is to first establish a vertical line at each vertex of each obstacle, then select the intersection of the vertical line and the sl graph (st graph) or the point of the intersection of the vertical line and the obstacle that is closest to the current obstacle vertex and whose line connecting the current obstacle vertex does not intersect with the current obstacle, and then use the nearest intersection point to establish a decomposition line with the current obstacle vertex. Using this method, the decomposition line of each vertex is established for each vertex of each obstacle from left to right.

[0132] Step 5, arrange multiple sampling points on each decomposition line, and connect the sampling points on the decomposition line in sequence starting from the starting point to form multiple alternative paths; the alternative paths are composed of multiple straight line segments connected end to end in sequence, and the straight line segment is a line segment formed by connecting the starting point with the sampling points on the adjacent decomposition line or by connecting the sampling points on the adjacent decomposition lines.

[0133] Points are scattered at a certain interval on each obstacle decomposition line (if the length of a decomposition line is less than the lateral boundary buffer value, no points are scattered on the decomposition line). The interval of the points and the lateral buffer value can be changed according to the actual situation; then the starting point of the path planning cycle and the sampling point on the decomposition line closest to the starting point are connected with a straight line, and the sampling points between every two adjacent decomposition lines are also connected with a straight line, such as Figure 7 shown.

[0134] Step 6: Use a dynamic programming algorithm to calculate the cost of each candidate path, and use the candidate path with the smallest cost as the rough path in the longitudinal displacement-lateral displacement graph.

[0135] The dynamic programming algorithm is used to evaluate the cost of each straight line segment, and finally replace the one with the smallest value as the rough path in the SL graph, such as Figure 8 As shown. Dynamic programming is to first obtain the optimal path (the path with the least cost) between the planning starting point and the point of the first layer (the decomposition line closest to the starting point) (the planning starting point is the 0th layer), and then use only the end point of the optimal path in the first layer and the points in the second layer to form a series of candidate paths, and then find the optimal path in the second layer; and so on, until the optimal path of the last layer is obtained.

[0136] Exemplarily, the specific steps of calculating the rough path using the dynamic programming algorithm are:

[0137] S6-1, number the decomposition lines starting from the first layer in the positive direction of the s-axis of the longitudinal displacement-transverse displacement diagram;

[0138] S6-2, initialize the number of layers u = 1;

[0139] S6-3, using the cost evaluation function of each straight line segment, calculate the cost value of each straight line segment between the starting point and the u-th layer;

[0140] S6-4, taking the straight line segment with the smallest cost value as the optimal straight line segment between the starting point and the u-th layer;

[0141] S6-5, starting from the intersection of the optimal straight line segment on the uth layer, increase the value of u by 1, and return to step "Use formula C total (L v )=C smooth (L v )+Cobs (L v )+C guidance (L v ), calculate the cost value of each straight line segment between the starting point and the u-th layer, until the value of u is greater than the maximum number of layers, stop the loop, and obtain the rough path in the longitudinal displacement-lateral displacement diagram.

[0142] The cost evaluation function of each straight line segment is expressed as:

[0143] C total (L v )=C smooth (L v )+C obs (L v )+C guidance (L v )

[0144] C total (L v ) is the cost of the vth straight line segment between the starting point and the uth layer.

[0145] C smooth (L v ) is the smoothness cost of the vth straight line segment between the starting point and the uth layer, C smooth (L v )=w1*L′ v 2 +w2*L″ v 2 +w3*L″′ v 2 , w1, w2 and w3 are the weights of the first-order derivative, second-order derivative and third-order derivative respectively, L′ v , L″ v and L″′ v are the first-order derivative, second-order derivative and third-order derivative of the vth straight line segment respectively;

[0146]

[0147]

[0148]

[0149] p is the number of discrete straight line segments of the vth line, l j is the lateral displacement of the jth discrete point, d s is a discrete longitudinal interval;

[0150] C obs (L v ) is the distance cost between the vth straight line segment and the obstacle between the starting point and the uth layer, and are the lengths of the long side and short side of the decomposition line where the right endpoint of the vth straight line segment is located;

[0151] C guidance (L v ) is the cost of the vth straight line segment between the starting point and the uth layer and the reference line; C guidance (L v )=L v -G v (s), G v (s) is the lateral displacement of the reference line at the vth straight line segment between the starting point and the uth layer at s, L v is the lateral displacement of the vth straight line segment between the starting point and the uth layer,

[0152] Step 7: The borders of the quadrilaterals where each straight line segment on the rough path is located form a trapezoidal corridor, and the rough path is optimized in the trapezoidal corridor using a piecewise Bezier path curve to obtain a lateral optimized trajectory, such as Figure 7 shown.

[0153] Trapezoidal corridors Figure 6 shown.

[0154] Exemplarily, the trajectory optimization of the rough path in the trapezoidal corridor is performed using a piecewise Bezier path curve, specifically including:

[0155] The cost function for trajectory optimization is constructed as Among them, J path represents the cost of trajectory optimization, where trajectory optimization is horizontal trajectory optimization or vertical trajectory optimization; k is the number of the piecewise Bezier curve, which corresponds to the number of the trapezoid in the trapezoidal corridor; s is the longitudinal displacement, l is the lateral displacement, m is the total number of segments of the piecewise Bezier curve, and S k is the width of the kth trapezoid, l k (s) is the kth Bezier curve, w l is the weight of the lateral displacement, w l′ is the weight of the first-order derivative of the lateral displacement with respect to the longitudinal displacement, w l″ is the weight of the second-order derivative of the lateral displacement with respect to the longitudinal displacement, w l″′ is the weight of the third-order derivative of the lateral displacement with respect to the longitudinal displacement, w ref is the weight of the lateral displacement and the reference error, is the straight line segment of the rough path in the kth trapezoid; S1, S2 and S m are the widths of the 1st, 2nd and mth trapezoids, respectively, T m-1 is the left boundary of the mth trapezoid, Tm is the right boundary of the mth trapezoid, Respectively represent the i-th control point of the 1st, 2nd, and mth Bezier curves, Represents the coefficient of the i-th Bezier curve control point, which is in the form of in t is represented as an independent variable, which is the longitudinal displacement s in the path planning cycle and the time t in the speed planning cycle, and n represents the number of control points;

[0156] The constraints for constructing trajectory optimization are:

[0157]

[0158]

[0159]

[0160]

[0161]

[0162]

[0163] in, is the i-th control point of the r-th derivative of the k-th Bezier curve, is the nth control point of the rth derivative of the kth Bezier curve, is the 0th control point of the rth derivative of the k+1th Bezier curve, They are respectively r for, is the lower boundary function of the kth trapezoid, is the upper boundary function of the kth trapezoid, l′ min and l′ max are the minimum and maximum values ​​of the first-order derivative of the lateral displacement with respect to the longitudinal displacement; l″ min and l″ max are the minimum and maximum values ​​of the second-order derivative of the lateral displacement with respect to the longitudinal displacement; l″ min and l″ max are the minimum and maximum values ​​of the third-order derivative of the lateral displacement with respect to the longitudinal displacement;

[0164] Convert the cost function and constraints of trajectory optimization into a second-order programming expression:

[0165]

[0166] stl≤A c x≤u

[0167] Where x is l(s) in trajectory optimization, P is the quadratic term of the cost function in lateral trajectory optimization (or longitudinal trajectory optimization), and q T is the linear term of the cost function in the lateral trajectory optimization (or longitudinal trajectory optimization), l and u are the lower and upper boundaries of the constraints in the lateral trajectory optimization (or longitudinal trajectory optimization), respectively, where the lower and upper boundaries of the equality constraint are both 0, and A c The constraint matrix formed by the constraint conditions in the lateral trajectory optimization (or longitudinal trajectory optimization);

[0168] Solve the second-order programming expression and obtain the optimization trajectory.

[0169] The lateral optimization trajectory is as follows Figure 8 shown.

[0170] Step 8: Construct a longitudinal displacement-time graph based on the lateral optimized trajectory.

[0171] The construction principle of the longitudinal displacement-time diagram is the same as that of the longitudinal displacement-lateral displacement diagram in step 2.

[0172] Step 9, project obstacles whose movement speed is greater than or equal to the speed threshold into the longitudinal displacement-time graph, and replace the longitudinal displacement-lateral displacement graph with the longitudinal displacement-time graph, and return to step "use the cell decomposition method to establish a decomposition line for each vertex of each obstacle in the longitudinal displacement-lateral displacement graph" to obtain the longitudinal optimized trajectory.

[0173] Obstacles whose moving speed is greater than or equal to the speed threshold mainly refer to high-speed dynamic obstacles.

[0174] The projection process of a high-speed dynamic obstacle is as follows: each vertex in the bounding box (BoundingBox) of the high-speed dynamic obstacle at time 0 is transferred from the xy coordinate system to the sl coordinate system, and the s in the sl coordinate at time 0 (from s min With s max ) as the lower edge of the st graph ((0, s min ) and (0, s max )), will be in t end (t end t last With t max The minimum value of t last The time at the last point in the predicted trajectory of a high-speed dynamic obstacle, t max Each vertex in the bounding box at the maximum future time considered in the planning cycle is converted from the xy coordinate system to the sl coordinate system, and t end The s in the sl coordinate at the time (given by s min With smax ) as the upper edge of the st graph ((t end ,s min ) and (t max ,s max )).

[0175] The method for determining the longitudinal optimization trajectory in the longitudinal displacement-time diagram is the same as the method for determining the lateral optimization trajectory in the longitudinal displacement-lateral displacement diagram. The brief steps are:

[0176] According to the relevant information of the ego vehicle obtained in S1, the starting point of each speed planning cycle is projected into st. The starting point of each speed planning cycle can be determined according to the actual scenario, but the starting point should not be too far away from the projection of the ego vehicle's posture in the st graph at this time;

[0177] The st graph is decomposed using the cell decomposition method to obtain a series of obstacle decomposition lines, which is the same as the steps in S4.

[0178] Use the dynamic programming algorithm to evaluate the cost of each straight line segment, and finally replace the one with the smallest value as the rough path in the ST graph, such as Fig. 9 shown.

[0179] The cost evaluation function of each straight line segment is expressed as:

[0180]

[0181] Where n is the total number of straight line segments, w1 is the error weight between the first-order derivative and the reference speed, w2 and w3 are the weights of the first-order derivative, second-order derivative and third-order derivative, w4 is the cost of the distance to the obstacle, S′ i , S″ i and s″′ i are the first-order derivative, second-order derivative and third-order derivative of the i-th straight line segment, respectively, which are expressed as:

[0182]

[0183]

[0184]

[0185] Where p is the number of discrete straight line segments of the i-th line, s j is the longitudinal displacement of the jth discrete point, dt is the discrete longitudinal interval; and C obs (S) is the same as in the formula.

[0186] According to the rough path in the ST graph, the trapezoidal corridor of the ST graph is obtained; then the speed curve is optimized inside the trapezoidal corridor of the ST graph using longitudinal trajectory optimization and piecewise Bezier speed curve.

[0187] The cost function of longitudinal trajectory optimization is expressed as:

[0188]

[0189] Among them, J speed represents the cost of longitudinal trajectory optimization, s is the longitudinal displacement, t is the lateral displacement, k is the number of a certain segment of the piecewise Bezier curve, and also the number of a certain trapezoid in the trapezoidal corridor, m is the total number of segments of the piecewise Bezier curve, and also the total number of trapezoids in the trapezoidal corridor, T k is the width of the kth trapezoid, s k (t) is the k-th Bezier curve, that is, the function of the k-th longitudinal displacement with respect to time displacement, is the kth segment of the rough path obtained in S5, w s is the weight of the longitudinal displacement close to the reference longitudinal displacement, is the weight of the longitudinal speed approaching the reference speed, is the weight of the longitudinal acceleration, is the weight of the longitudinal acceleration.

[0190] The constraints for longitudinal trajectory optimization are expressed as:

[0191]

[0192]

[0193]

[0194]

[0195]

[0196]

[0197]

[0198] The first equation is a boundary constraint; the second equation is a continuity constraint; the third equation is a safety constraint; the fourth equation and are physical constraints; represents the i-th control point of the r-th derivative of the k-th Bezier curve, T k is the width of the kth trapezoid, is the lower boundary function of the kth trapezoid, is the upper boundary function of the kth trapezoid, vmin With v max are the minimum and maximum longitudinal speed respectively; a min with a max are the minimum and maximum values ​​of longitudinal acceleration respectively; j min With j max are the minimum and maximum values ​​of longitudinal acceleration respectively; a cm is the maximum centrifugal acceleration; κ k is the maximum curvature of the kth Bezier curve.

[0199] The longitudinal optimization trajectory is as follows Fig.10 shown.

[0200] Step 10: merge the lateral optimization trajectory and the longitudinal optimization trajectory, and perform a safety check on the merged optimization trajectory, and determine the merged optimization trajectory that passes the safety check as the optimal feasible trajectory.

[0201] Exemplarily, merging the horizontal optimization trajectory and the vertical optimization trajectory specifically includes:

[0202] The time period on the longitudinal displacement-time graph is evenly divided to obtain a series of sampling time points;

[0203] The sampling time point t is obtained from the longitudinal optimization trajectory in the longitudinal displacement-time diagram i The longitudinal displacement s i , longitudinal speed and longitudinal acceleration

[0204] The longitudinal displacement s is obtained from the lateral optimization trajectory in the longitudinal displacement-lateral displacement diagram. i The lateral displacement l i , transverse velocity l′ i and the lateral acceleration l″ i ;

[0205] Determine a trajectory point in the frenet coordinate system

[0206] The determined trajectory point is converted into the Cartesian coordinate system to obtain a trajectory point in the Cartesian coordinate system (t i , x i ,y i ,s i ,θ i , κ i , v i , a i ), where x i With y i is the pose in the Cartesian coordinate system, θ i is the orientation in Cartesian coordinates, κi is the curvature, v i with a i are velocity and acceleration respectively;

[0207] Repeat the above steps to obtain the trajectory points of all sampling time points in the Cartesian coordinate system, and connect all trajectory points in sequence to form a merged optimized trajectory.

[0208] As an example, the merged optimized trajectory is subjected to security detection, including:

[0209] Determine whether each trajectory point on the combined optimized trajectory meets the safety condition to obtain a first determination result; the safety condition is that the speed is less than or equal to the speed threshold, the acceleration is less than or equal to the acceleration threshold, the jerk is less than or equal to the jerk threshold, and the curvature is less than or equal to the curvature threshold;

[0210] If the first judgment result indicates yes, then judging whether each trajectory point collides with an obstacle to obtain a second judgment result;

[0211] If the second judgment result indicates no, then it is determined that the combined optimized trajectory passes the safety test;

[0212] If the first judgment result indicates no or the second judgment result indicates yes, it is determined that the merged optimized trajectory does not pass the safety detection, and the merged optimized trajectory is determined as a backup trajectory.

[0213] The present invention proposes a decoupled real-time trajectory planning method for autonomous driving vehicles. The method decouples a three-dimensional trajectory planning problem into two two-dimensional trajectory planning problems, and uses the cell decomposition method and dynamic programming algorithm to obtain a rough path and a trapezoidal corridor in each two-dimensional problem. At the same time, quadratic programming is used to optimize and solve each two-dimensional problem in the trapezoidal corridor, and finally a feasible trajectory is obtained.

[0214] This method first converts the difficult three-dimensional planning problem into two quadratic programming problems for solution, reducing the complexity; and uses the cell decomposition method to decompose all perceived obstacles after projection, so there is no influence of small obstacles; in addition, firstly, a series of obstacle decomposition lines obtained after decomposition by the cell decomposition method are scattered, and then a rough path is obtained by using a dynamic programming algorithm; furthermore, this method can obtain a path convex corridor composed of trapezoids through the obtained rough path; finally, this method uses a piecewise Bezier curve and uses a quadratic programming method to optimize in the path convex corridor. Because the piecewise Bezier curve is used, each curve will not exceed the range of each trapezoid, and because the derivative of the Bezier curve is still a Bezier curve, the speed, acceleration, and jerk of the control point of each Bezier curve are restricted, so that the reciprocal of each Bezier curve is also inside the convex hull composed of the speed, acceleration, and jerk of each control point; because quadratic programming is used, its solution speed can meet the real-time requirements. Therefore, this method solves the problems that three-dimensional problems are difficult to solve when planning trajectories for autonomous vehicles, the trajectories obtained using sampling methods are not optimal and are not suitable for complex urban road scenes, and small obstacles cannot be handled in discrete planning space.

[0215] Embodiment 2

[0216] This embodiment provides a decoupled real-time planning device for an autonomous driving vehicle, such as Fig.11 As shown, the planning device includes: a routing module, a positioning module, a perception module, a planning module and a control module.

[0217] The routing module, positioning module, and perception module are all connected to the input end of the planning module, and the control module is connected to the output end of the planning module. The routing module is used to send the global path of the vehicle to the planning module. The positioning module is used to send the current position of the vehicle to the planning module. The perception module is used to send the obstacle information around the vehicle to the planning module. The planning module is used to obtain the optimal feasible trajectory based on the global path, current position and obstacle information around the vehicle, using the aforementioned decoupled autonomous driving vehicle real-time trajectory planning method, and send the optimal feasible trajectory to the control module. The control module is used to control the vehicle to execute the optimal feasible trajectory.

[0218] Embodiment 3

[0219] This embodiment provides a decoupled autonomous driving vehicle real-time planning system, the planning system comprising:

[0220] Real-time information acquisition module, used to obtain the position information of the vehicle and obstacles around the vehicle in real time;

[0221] The longitudinal displacement-lateral displacement map acquisition module is used to intercept a section of the path including the current posture of the ego vehicle on the global path of the ego vehicle as a reference line, and convert the reference line from the Cartesian coordinate system to the Frenet coordinate system to obtain the longitudinal displacement-lateral displacement map;

[0222] A projection module, used for projecting obstacles whose moving speed is less than a speed threshold into a longitudinal displacement-lateral displacement map;

[0223] A decomposition module, for establishing a decomposition line for each vertex of each obstacle in the longitudinal displacement-lateral displacement graph using a cell decomposition method;

[0224] The alternative path forming module is used to arrange multiple sampling points on each decomposition line, and connect the sampling points on the decomposition line in sequence from the starting point to form multiple alternative paths; the alternative paths are composed of multiple straight line segments connected end to end in sequence, and the straight line segment is a line segment formed by connecting the starting point with the sampling points on the adjacent decomposition line or by connecting the sampling points on the adjacent decomposition lines;

[0225] A rough path determination module is used to calculate the cost of each candidate path using a dynamic programming algorithm, and use the candidate path with the smallest cost as the rough path in the longitudinal displacement-lateral displacement graph;

[0226] A lateral optimization trajectory acquisition module is used to form a trapezoidal corridor with the border of the quadrilateral where each straight line segment on the rough path is located, and to optimize the trajectory of the rough path in the trapezoidal corridor using a piecewise Bezier path curve to obtain a lateral optimization trajectory;

[0227] A longitudinal displacement-time graph construction module, used to construct a longitudinal displacement-time graph according to a lateral optimized trajectory;

[0228] A longitudinal optimization trajectory acquisition module is used to project obstacles whose movement speed is greater than or equal to the speed threshold into the longitudinal displacement-time graph, and replace the longitudinal displacement-lateral displacement graph with the longitudinal displacement-time graph, and return to the step of "using the cell decomposition method to establish a decomposition line for each vertex of each obstacle in the longitudinal displacement-lateral displacement graph" to obtain the longitudinal optimization trajectory;

[0229] The optimal feasible trajectory determination module is used to merge the lateral optimized trajectory and the longitudinal optimized trajectory, and perform safety detection on the merged optimized trajectory, and determine the merged optimized trajectory that passes the safety detection as the optimal feasible trajectory.

[0230] Projection module, specifically including:

[0231] The static obstacle projection submodule is used to convert each vertex in the bounding box of the static obstacle from the Cartesian coordinate system to the Frenet coordinate system when the obstacle whose moving speed is less than the speed threshold is a static obstacle, and project the static obstacle in the longitudinal displacement-lateral displacement map;

[0232] The low-speed obstacle projection submodule is used to convert each vertex in the bounding box of the low-speed obstacle from the Cartesian coordinate system to the Frenet coordinate system when the obstacle whose moving speed is less than the speed threshold is a low-speed obstacle, and project the low-speed obstacle in the longitudinal displacement-lateral displacement map; the low-speed obstacle has the same driving direction as the ego vehicle, and the moving speed of the low-speed obstacle is greater than zero and less than the speed threshold;

[0233] The oncoming obstacle projection submodule is used to convert each vertex in the bounding box at the position where the predicted trajectory of the oncoming obstacle collides with the current trajectory of the vehicle from the Cartesian coordinate system to the Frenet coordinate system when the obstacle with a moving speed less than the speed threshold is the oncoming obstacle, and project the oncoming obstacle in the longitudinal displacement-lateral displacement map.

[0234] In this specification, each embodiment is described in a progressive manner, and each embodiment focuses on the differences from other embodiments. The same or similar parts between the embodiments can be referred to each other. For the system disclosed in the embodiment, since it corresponds to the method disclosed in the embodiment, the description is relatively simple, and the relevant parts can be referred to the method part.

[0235] The principles and implementation methods of the present invention are described in this article using specific examples. The description of the above embodiments is only used to help understand the method and core idea of ​​the present invention. At the same time, for those skilled in the art, according to the idea of ​​the present invention, there will be changes in the specific implementation methods and application scope. In summary, the content of this specification should not be understood as limiting the present invention.

Claims

1. A decoupled real-time trajectory planning method for an autonomous driving vehicle, characterized in that: The planning method includes: Obtain the position information of the vehicle and the obstacles around it in real time; A section of the path including the current posture of the vehicle is intercepted on the global path of the vehicle as a reference line, and the reference line is converted from the Cartesian coordinate system to the Frenet coordinate system to obtain the longitudinal displacement-lateral displacement diagram; The obstacles whose moving speed is less than the speed threshold are projected into the longitudinal displacement-lateral displacement map; Using the cell decomposition method, a decomposition line is established for each vertex of each obstacle in the longitudinal displacement-lateral displacement graph; Arrange multiple sampling points on each decomposition line, and connect the sampling points on the decomposition line in sequence from the starting point to form multiple candidate paths; the candidate paths are composed of multiple straight line segments connected end to end in sequence, and the straight line segment is a line segment formed by connecting the starting point with the sampling points on the adjacent decomposition line or by connecting the sampling points on the adjacent decomposition lines; A dynamic programming algorithm is used to calculate the cost of each candidate path, and the candidate path with the smallest cost is used as the rough path in the longitudinal displacement-lateral displacement graph; The borders of the quadrilaterals where each straight line segment on the rough path is located form a trapezoidal corridor, and the rough path is optimized in the trapezoidal corridor using a piecewise Bezier path curve to obtain a lateral optimized trajectory; constructing a longitudinal displacement-time graph according to the lateral optimized trajectory; Project obstacles whose moving speed is greater than or equal to the speed threshold into the longitudinal displacement-time graph, and replace the longitudinal displacement-lateral displacement graph with the longitudinal displacement-time graph, and return to step "Use the cell decomposition method to establish a decomposition line for each vertex of each obstacle in the longitudinal displacement-lateral displacement graph" to obtain the longitudinal optimization trajectory; Merge the lateral optimization trajectory and the longitudinal optimization trajectory, perform safety detection on the merged optimization trajectory, and determine the merged optimization trajectory that passes the safety detection as the optimal feasible trajectory; The method of using the cell decomposition method to establish a decomposition line for each vertex of each obstacle in the longitudinal displacement-lateral displacement diagram specifically includes: Establish a vertical line at each vertex of each obstacle; Select the intersection point of the vertical line and the longitudinal displacement-lateral displacement map boundary box and the intersection point of the vertical line and the obstacle boundary box as candidate intersection points; Select the candidate intersection points closest to each vertex on both sides of each vertex along the vertical line; Connect each vertex with the two selected candidate intersection points respectively to obtain two connecting line segments for each vertex; Delete the connected line segments that intersect with obstacles to obtain a decomposition line for each vertex of each obstacle.

2. The decoupled automatic driving vehicle real-time trajectory planning method according to claim 1, characterized in that: The projecting of obstacles whose moving speed is less than the speed threshold into the longitudinal displacement-lateral displacement map specifically includes: When an obstacle whose moving speed is less than the speed threshold is a static obstacle, each vertex in the bounding box of the static obstacle is converted from the Cartesian coordinate system to the Frenet coordinate system, and the static obstacle is projected in the longitudinal displacement-lateral displacement map; When an obstacle whose moving speed is less than the speed threshold is a low-speed obstacle, each vertex in the bounding box of the low-speed obstacle is converted from the Cartesian coordinate system to the Frenet coordinate system, and the low-speed obstacle is projected in the longitudinal displacement-lateral displacement map; the low-speed obstacle has the same driving direction as the vehicle, and the moving speed of the low-speed obstacle is greater than zero and less than the speed threshold; When an obstacle whose moving speed is less than the speed threshold is an oncoming obstacle, each vertex in the bounding box at the position where the predicted trajectory of the oncoming obstacle collides with the current trajectory of the vehicle is converted from the Cartesian coordinate system to the Frenet coordinate system, and the oncoming obstacle is projected in the longitudinal displacement-lateral displacement map.

3. The decoupled automatic driving vehicle real-time trajectory planning method according to claim 1, characterized in that: The method of using a dynamic programming algorithm to calculate the cost of each candidate path and taking the candidate path with the smallest cost as the rough path in the longitudinal displacement-lateral displacement graph specifically includes: The decomposition lines are numbered in sequence starting from the first layer along the positive direction of the s-axis of the longitudinal displacement-lateral displacement diagram; Initialize the number of layers u = 1; Using formula C total (L v )=C smooth (L v )+C obs (L v )+C guidance (L v ), calculate the cost value of each straight line segment between the starting point and the uth layer in the longitudinal displacement-lateral displacement diagram; where C total (L v ) is the cost value of the vth straight line segment between the starting point and the uth layer, C smooth (L v ) is the smoothness cost of the vth straight line segment between the starting point and the uth layer, C obs (L v ) is the distance cost between the starting point and the u-th layer and the obstacle, C guidance (L v ) is the cost of the vth straight line segment between the starting point and the uth layer and the reference line; w1, w2 and w3 are the weights of the first-order derivative, second-order derivative and third-order derivative respectively. v ′, L v ″ and L v ″′ are the first-order derivative, second-order derivative and third-order derivative of the vth straight line segment, p is the number of discrete straight line segments of the vth line, l j is the lateral displacement of the jth discrete point, and ds is the discrete longitudinal interval; and are the lengths of the long side and short side of the decomposition line where the right endpoint of the vth straight line segment is located; C guidance (L v )=L v -G v (s), G v (s) is the lateral displacement of the reference line at the vth straight line segment between the starting point and the uth layer at s, L v is the lateral displacement of the vth straight line segment between the starting point and the uth layer, The straight line segment with the smallest cost value is taken as the optimal straight line segment between the starting point and the u-th layer; Take the intersection of the optimal straight line segment on the uth layer as the starting point, increase the value of u by 1, and return to step "Use formula C total (L v )=C smooth (L v )+C obs (L v )+C guidance (L v ), calculate the cost value of each straight line segment between the starting point and the u-th layer, until the value of u is greater than the maximum number of layers, stop the loop, and obtain the rough path in the longitudinal displacement-lateral displacement diagram.

4. The decoupled automatic driving vehicle real-time trajectory planning method according to claim 1, characterized in that: The dynamic programming algorithm is used to calculate the cost of each alternative path, and the alternative path with the smallest cost is used as the rough path in the longitudinal displacement-time graph, which includes: The decomposition lines are numbered sequentially starting from the first layer along the positive direction of the t-axis of the longitudinal displacement-time diagram; Initialize the number of layers u = 1; Using the formula Calculate the cost value of each straight line segment between the starting point and the uth layer in the longitudinal displacement-time diagram; where C total (S v ) is the cost of the vth straight line segment between the starting point and the uth layer in the longitudinal displacement-time diagram, w4 is the cost of the distance to the obstacle, V ref is the reference speed of the current road segment, C obs (s) is the distance cost between the vth straight line segment and the obstacle between the starting point and the uth layer in the longitudinal displacement-time graph, and are the lengths of the long and short sides of the decomposition line where the right endpoint of the vth straight line segment is located, S′ v , S″ v and S″′ v are the first-order derivative, second-order derivative, and third-order derivative of the vth straight line segment, respectively. s j is the longitudinal displacement of the jth discrete point, dt is the discrete longitudinal interval; The straight line segment with the smallest cost value is taken as the optimal straight line segment between the starting point and the u-th layer; Take the intersection of the optimal straight line segment on the uth layer as the starting point, increase the value of u by 1, and return to step "Use the formula Calculate the cost value of each straight line segment between the starting point and the uth layer in the longitudinal displacement-time graph, until the value of u is greater than the maximum number of layers, stop the loop, and obtain the rough path in the longitudinal displacement-time graph.

5. The decoupled automatic driving vehicle real-time trajectory planning method according to claim 1, characterized in that: The step of optimizing the trajectory of the rough path in the trapezoidal corridor by using the piecewise Bezier path curve specifically includes: When optimizing the lateral trajectory of a coarse path in a trapezoidal corridor using a piecewise Bezier path curve: The cost function for constructing lateral trajectory optimization is Among them, J path represents the cost of trajectory optimization, where trajectory optimization is horizontal trajectory optimization or vertical trajectory optimization; k is the number of the piecewise Bezier curve, which corresponds to the number of the trapezoid in the trapezoidal corridor; s is the longitudinal displacement, l is the lateral displacement, m is the total number of segments of the piecewise Bezier curve, and S k is the width of the kth trapezoid, w l is the weight of the lateral displacement, w l′ is the weight of the first-order derivative of the lateral displacement with respect to the longitudinal displacement, w l″ is the weight of the second-order derivative of the lateral displacement with respect to the longitudinal displacement, w l″′ is the weight of the third-order derivative of the lateral displacement with respect to the longitudinal displacement, w ref is the weight of the lateral displacement and the reference error, is the straight line segment of the rough path in the kth trapezoid; l k (s) is the kth Bezier curve, S1, S2 and S m are the widths of the 1st, 2nd and mth trapezoids, respectively, T m-1 is the left boundary of the mth trapezoid, T m is the right boundary of the mth trapezoid, Respectively represent the i-th control point of the 1st, 2nd, and mth Bezier curves, Represents the coefficient of the i-th Bezier curve control point, which is in the form of in t represents the independent variable, t is the longitudinal displacement s in the path planning cycle, t is the time in the speed planning cycle, and n represents the number of control points; The constraints for constructing the lateral trajectory optimization are: in, is the i-th control point of the r-th derivative of the k-th Bezier curve, is the nth control point of the rth derivative of the kth Bezier curve, is the 0th control point of the rth derivative of the k+1th Bezier curve, S k is the width of the kth trapezoid, S k , S k+1 to the (1-r)th power, Indicates that when s=s k When k The r-order derivative value of (s) with respect to s, is the lower boundary function of the kth trapezoid, is the upper boundary function of the kth trapezoid, l′ min and l′ max are the minimum and maximum values ​​of the first-order derivative of the lateral displacement with respect to the longitudinal displacement; l″ min and l″ max are the minimum and maximum values ​​of the second-order derivative of the lateral displacement with respect to the longitudinal displacement; l″ min and l″ max are the minimum and maximum values ​​of the third-order derivative of the lateral displacement with respect to the longitudinal displacement; When optimizing the longitudinal trajectory of a rough path in a trapezoidal corridor using a piecewise Bezier path curve: The cost function for constructing longitudinal trajectory optimization is Among them, J speed is the cost of optimizing the longitudinal trajectory, s is the longitudinal displacement, t is the lateral displacement, and it is also the total number of trapezoids in the trapezoidal corridor, T k is the width of the kth trapezoid, s k (t) is the kth Bezier curve, is the straight line segment of the rough path in the kth trapezoid in the longitudinal displacement-time diagram, w s is the weight of the longitudinal displacement close to the reference longitudinal displacement, is the weight of the longitudinal speed approaching the reference speed, is the weight of the longitudinal acceleration, is the weight of longitudinal acceleration; The constraints for constructing the longitudinal trajectory optimization are: in, is the i-th control point of the r-th derivative of the k-th Bezier curve, T k is the width of the kth trapezoid, is the lower boundary function of the kth trapezoid, is the upper boundary function of the kth trapezoid, v min With v max are the minimum and maximum longitudinal speed respectively; a min with a max are the minimum and maximum values ​​of longitudinal acceleration respectively; j min With j max are the minimum and maximum values ​​of longitudinal acceleration respectively; a cm is the maximum centrifugal acceleration; κ k is the maximum curvature of the kth Bezier curve; The cost function and constraints of lateral trajectory optimization and the cost function and constraints of longitudinal trajectory optimization are transformed into a second-order programming expression respectively: S.t.l≤A c x≤u Where x is l(s) or s in trajectory optimization k (t), P is the quadratic term of the cost function in trajectory optimization, q T is the linear term of the cost function in trajectory optimization, l and u are the lower and upper bounds of the constraint conditions in trajectory optimization, respectively, the lower and upper bounds of the equality constraint are both 0, A c The constraint matrix formed by the constraints in trajectory optimization; Solve the second-order programming expression to obtain the lateral optimization trajectory or the longitudinal optimization trajectory.

6. The decoupled automatic driving vehicle real-time trajectory planning method according to claim 1, characterized in that: The merging of the lateral optimization trajectory and the longitudinal optimization trajectory specifically includes: The time period on the longitudinal displacement-time graph is evenly divided to obtain a series of sampling time points; The sampling time point t is obtained from the longitudinal optimization trajectory in the longitudinal displacement-time diagram i The longitudinal displacement s i , longitudinal speed and longitudinal acceleration The longitudinal displacement s is obtained from the lateral optimization trajectory in the longitudinal displacement-lateral displacement diagram. i The lateral displacement l i , transverse velocity l′ i and the lateral acceleration l″ i ; Determine a trajectory point in the frenet coordinate system The determined trajectory point is converted into the Cartesian coordinate system to obtain a trajectory point in the Cartesian coordinate system (t i , x i ,y i ,s i ,θ i , k i , v i , a i ), where x i With y i is the pose in the Cartesian coordinate system, θ i is the orientation in Cartesian coordinates, κ i is the curvature, v i with a i are velocity and acceleration respectively; Repeat the above steps to obtain the trajectory points of all sampling time points in the Cartesian coordinate system, and connect all trajectory points in sequence to form a merged optimized trajectory.

7. The decoupled automatic driving vehicle real-time trajectory planning method according to claim 1, characterized in that: The security detection of the merged optimized trajectory specifically includes: Determine whether each trajectory point on the combined optimized trajectory meets the safety condition to obtain a first determination result; the safety condition is that the speed is less than or equal to the speed threshold, the acceleration is less than or equal to the acceleration threshold, the jerk is less than or equal to the jerk threshold, and the curvature is less than or equal to the curvature threshold; If the first judgment result indicates yes, then judging whether each trajectory point collides with an obstacle to obtain a second judgment result; If the second judgment result indicates no, then determining that the combined optimized trajectory passes the safety detection; If the first judgment result indicates no or the second judgment result indicates yes, it is determined that the merged optimized trajectory does not pass the safety detection, and the merged optimized trajectory is determined as a backup trajectory.

8. A decoupled real-time planning device for an autonomous driving vehicle, characterized in that: The planning device includes: a routing module, a positioning module, a sensing module, a planning module and a control module; The routing module, positioning module, and perception module are all connected to the input end of the planning module, and the control module is connected to the output end of the planning module; The routing module is used to send the global path of the vehicle to the planning module; The positioning module is used to send the current position of the vehicle to the planning module; The perception module is used to send obstacle information around the vehicle to the planning module; The planning module is used to obtain an optimal feasible trajectory according to the global path, current posture and obstacle information around the vehicle, using the decoupled automatic driving vehicle real-time trajectory planning method according to any one of claims 1 to 7, and send the optimal feasible trajectory to the control module; The control module is used to control the vehicle to execute the optimal feasible trajectory.

9. A decoupled autonomous driving vehicle real-time planning system, characterized in that: The planning system comprises: Real-time information acquisition module, used to obtain the position information of the vehicle and obstacles around the vehicle in real time; The longitudinal displacement-lateral displacement map acquisition module is used to intercept a section of the path including the current posture of the ego vehicle on the global path of the ego vehicle as a reference line, and convert the reference line from the Cartesian coordinate system to the Frenet coordinate system to obtain the longitudinal displacement-lateral displacement map; A projection module, used for projecting obstacles whose moving speed is less than a speed threshold into a longitudinal displacement-lateral displacement map; A decomposition module, for establishing a decomposition line for each vertex of each obstacle in the longitudinal displacement-lateral displacement graph using a cell decomposition method; The alternative path forming module is used to arrange multiple sampling points on each decomposition line, and connect the sampling points on the decomposition line in sequence from the starting point to form multiple alternative paths; the alternative paths are composed of multiple straight line segments connected end to end in sequence, and the straight line segment is a line segment formed by straight line connection between the starting point and the sampling points on the adjacent decomposition line or by straight line connection between the sampling points on the adjacent decomposition lines; A rough path determination module is used to calculate the cost of each candidate path using a dynamic programming algorithm, and use the candidate path with the smallest cost as the rough path in the longitudinal displacement-lateral displacement graph; A lateral optimization trajectory acquisition module is used to form a trapezoidal corridor with the border of the quadrilateral where each straight line segment on the rough path is located, and to optimize the trajectory of the rough path in the trapezoidal corridor using a piecewise Bezier path curve to obtain a lateral optimization trajectory; A longitudinal displacement-time graph construction module, used to construct a longitudinal displacement-time graph according to the lateral optimized trajectory; A longitudinal optimization trajectory acquisition module is used to project obstacles whose movement speed is greater than or equal to the speed threshold into the longitudinal displacement-time graph, and replace the longitudinal displacement-lateral displacement graph with the longitudinal displacement-time graph, and return to the step of "using the cell decomposition method to establish a decomposition line for each vertex of each obstacle in the longitudinal displacement-lateral displacement graph" to obtain the longitudinal optimization trajectory; The optimal feasible trajectory determination module is used to merge the lateral optimized trajectory and the longitudinal optimized trajectory, and perform safety detection on the merged optimized trajectory, and determine the merged optimized trajectory that passes the safety detection as the optimal feasible trajectory.

Images