Control of Automated Driving in Motion Planning

In sampling-based motion planning algorithms, the number of samples determines the complexity of the computation required to solve the problem. Conventionally, the uniform sampling method will require a large number of samples and more computational resources as well. A uniform sampling pattern can be found in Gu et al. (2016), which is shown in the first picture of Fig 8. The quality of the path generated by this uniform pattern is highly dependent on the resolution. However, more running time during dynamic programming in the next step is needed if we increased the resolution in uniform sampling to achieve a trajectory with higher performance. To solve the problem that existed in the uniform sampling method, an adaptive sampling method is proposed by us so as to dynamically adjust uniform sampling patterns based on the history information. Adaptive sampling will automatically adjust the density of sampling based on the partially-observed information and the historical experience in the real-time implementation. In our motion planning experiment, roads are always bounded by two edges, and it is easy to obtain the optimal trajectory from the last cycle and the obstacles information in the real-time implementation. Therefore, we proposed our adaptive sampling method, which will take into consideration the historical trajectory, obstacles information and road boundaries.

In the next, the details of the implementation of our Adaptive Sampling Method will be presented. First of all, uniform sampling is realized as initialization, the same as shown in Figure 8. Suppose the number of layers is $N$, and a layer is denoted by $L_{i}$ (i = 1,2,…, N), and in every layer, the number of nodes is M, so the location of a node on $L_{i}$ is denoted by $n_{ij}$ (i = 1,2,…, M). Then, inspired by Artificial Potential Field (APF), the repulsive field and attractive field are defined so as to predefine and compare the possibility of being nodes of optimal trajectory in the next cycle.

Let us define the total APF force first, which equals the sum of repulsive force and attractive force. In our problem, repulsive force is given by obstacles and road boundaries and the optimal path in the last cycle offers us the attractive field in the theory:

1. 1)

   Obstacle Repulsive Field
   The repulsive field given by static obstacles depends on the distance between sampling points and obstacles boundaries. Because the obstacle may not be symmetry with respect to the reference line, here we defined the obstacle field function based on the obstacle upper-bound $l_{obs\_up}(s)$ and obstacle lower-bound $l_{obs\_down}(s)$:

   $$U_{obs}(s,l)=\begin{cases}\frac{1}{2}*\eta*\frac{1}{d^{2}(l,l_{obs\_up}(s)+1)},&l>l_{obs\_up(s)+1}\\ +inf,&l_{obs\_down(s)}-1\leq l\leq l_{obs\_up(s)}+1\\ \frac{1}{2}*\eta*\frac{1}{d^{2}(l,l_{obs\_down(s)}-1)},&l<l_{o}bs\_down(s)-1\end{cases}$$

   (9)

   Here $\eta$ is the coefficient of obstacle repulsive field. For the consideration of safety, it is defined as 1000 in our Simulink implementation.

2. 2)

   Boundary Repulsive Field
   Then, the boundary repulsive field is given here. Similar to the form of the obstacle field, the form of the equation is defined depending on the distance between lateral deviation and lateral road upper boundary $l_{bound\_up}(s)$, the distance between that and lateral road lower boundary $l_{lbound}(s)$ as well:

   $$U_{bound}(s,l)=\begin{cases}+inf,&l\geq l_{ubound}(s)-1\\ \frac{1}{2}*\gamma*(\frac{1}{d^{2}(l,l_{ubound}(s)-1}+\frac{1}{d^{2}(l,l_{lbound}(s)+1)},&l_{lbound}(s)+1<l<l_{ubound}(s)-1\\ +inf,&l\leq l_{lbound}(s)+1\end{cases}$$

   (10)

   $\gamma$ is the coefficient of the boundary repulsive field. By testing in our experiment, if the value is chosen as 20, our adaptive sampling method performs well. In addition, it is important to mention that, the objective of forming the equation when $l_{lbound}(s)+1<l<l_{ubound}(s)+1$ like that is to make sure the continuity of equations.

3. 3)

   Trajectory Attractive Field
   In a real implementation, it is not difficult to obtain the optimal trajectory in the last cycle of motion planning. This information can be fully used to converge the unique sampling area into a more feasible and smaller space for the reason that the optimal trajectory will not vibrate too much in 100 milliseconds time intervals in a normal situation.

   $$U_{ref}(s,l)=\frac{1}{2}*\alpha*d^{2}(l,l_{ref}(s))$$

   Here $\gamma$ is the coefficient of the trajectory attractive field. In our experiment, the value is chosen as 20. In the first cycle, there is no optimal solution from the last cycle, which will lead to the deadlock problem in the Simulink simulation. To solve this problem, a delay with initial values is added to this signal. The initial value is given from the reference line.

In this Figure 9, we see the visualization of the effect of our ASAPF. Depending on the distance with obstacles, road boundaries and history optimal trajectory, the sampling points in each column are adaptively adjusted so as to focus more on the area where the next optimal trajectory is easier to appear.

Until now, we have already transformed the reference line generated based on the global route from Cartesian into Frenet coordinate system, and our proposed adaptive sampling method has already been implemented to decrease the scale of sampling meanwhile maintaining the possibility to obtain optimal path. Then, in this section, the problem becomes how to find an optimal function of lateral coordinate $l=f(s)$ w.r.t. station coordinate in non-convex SL space. Quadratic programming is fast and efficient enough to solve a 2-D optimization problem but the premise is that the problem has to be within convex space. It is obvious that our motion planning task is not a convex problem. Therefore, dynamic programming is used before quadratic programming to make path decisions, or in other words, provide a rough path profile. Then based on that rough path profile, a convex space will be built where quadratic programming could be implemented easily to take into consideration dynamic vehicle constraints and make the final path smoothing which is important for autonomous vehicles to follow in the control part.

After constructing sampling space using the adaptive sampling method, each graph edge could be evaluated by the summation of cost functions. Smoothness, obstacle avoidance and reference line should be considered together in dynamic programming. Therefore, the total cost of every edge in dynamic programming could be defined in the form below:

1. 1)

   Smoothing Cost
   Let us give the smoothing cost first. By Newton’s theory, it is easy to know that lateral velocity $f^{{}^{\prime}}(s)$ is the derivative of a lateral function $f(s)$. Due to the same reason, the derivative of lateral velocity $f^{{}^{\prime}}(s)$ means lateral acceleration $f^{{}^{\prime\prime}}(s)$. Here lateral jerk $f^{{}^{\prime\prime\prime}}(s)$ is defined as the derivative of lateral acceleration $f^{{}^{\prime\prime}}(s)$, which represents the rate at which an object’s acceleration changes with respect to time. The jerk will influence the comfort of passengers, so it should be decreased as possible as we can. The smoothing cost has the form below:

   $$C_{smooth}(f(s))=w_{1}\int(f^{{}^{\prime}}(s))^{2}ds+w_{2}\int(f^{{}^{\prime\prime}}(s))^{2}ds+w_{3}\int(f^{{}^{\prime\prime\prime}}(s))^{2}ds$$

   (12)

2. 2)

   Obstacle Cost
   If the distance between lateral derivation and the obstacle centre point is less than the boundary, it means there will be a collision at that point, so a large value will be given as obstacle cost so as to detect an infeasible path. Here in equations, for simplicity of expression, we define the large value as $+inf$. Then, if the distance is over the safety distance, the influence of that obstacle could be neglected, so the obstacle cost will become 0. Between collision distance and safety distance, the obstacle cost will be defined as a monotonically decreasing function with respect to the distance, as shown below:

   $$C_{obs}(f(s))=\begin{cases}0,&f(s)>d_{safety}\\ w*\frac{1}{d^{2}(f(s),d_{collision})},&d_{collision}<f(s)<d_{safety}\\ +inf,&f(s)<d_{collision}\end{cases}$$

   (13)

3. 3)

   Reference Cost
   If there are no obstacles on the road, our ego vehicle should try to follow our reference line. Therefore, a reference cost is defined to incrementally punish the path with respect to the distance with the reference line:

   $$C_{ref}(f(s))=\int(f(s)-l_{ref}(s))^{2}ds$$

   (14)

In the content above, the cost function is defined which could give cost to every single edge. We could set the initialization point as layer 0, and the cost of nodes in this layer is 0 too. The costs of nodes in the current layer are equal to the minimal sum of edge cost and cost of nodes in the last layer. Repeat that step until the nodes in the final layer are given values of costs as well. Then try to search the minimal cost in each layer from the final layer back to layer 0, which will give us a feasible path that is the result of dynamic programming. The details of the DP process are given in Algorithm 1.

In our implementation, it is easy to know that it is impossible to connect sampled nodes using straight lines because they can not satisfy the constraints of curvature and jerk. To solve this problem, the fifth-degree polynomial curve is used which could satisfy the requirement to jerk and does not make the problem too complex to solve at the same time. In Appendix 9.1, I will explain why the fifth-degree is chosen, rather than another degree, for example, the fourth-degree polynomial which is easier to solve, and the sixth-degree polynomial which is more accurate.

A rough path profile has already been obtained by dynamic programming (QP) in the last step. That means we have already decided to pass the obstacle from the left side or right side. Next, a convex space is built based on the obtained rough path. Quadratic Programming could be seen as a refinement of the dynamic programming path step. Normally, before implementing QP, objective functions and constraints should be defined in advance. The objective function is mainly related to a linear combination of smoothness costs which have the same form as in dynamic programming. Mathematically, the QP path step will optimize the following function:

Here $r(s)$ represents the DP path result.

The constraints in the QP path include boundary constraints and dynamic feasibility. They are defined in discrete formulation, so let us note:

Suppose the third order derivative of $f(s)$ between $l_{i}$ and $l_{i+1}$ is a constant and the higher orders of that are 0. So we could use Taylor expansion here in $l_{i}$ and $l_{i+1}$.

By simple transformation for that equation, we will have:

It could be written in matrix form:

Note the left matrix on the left side of the above equation is $A_{eq\_sub}$. This constraint is only for two neighbour points. Suppose we have $l_{1},l_{1}^{\prime},l_{1}^{\prime\prime},...,l_{n},l_{n}^{{}^{\prime}},l_{n}^{{}^{\prime\prime}}$ that needs to be optimized. If we implement this constraint for each two neighbour points, we will have the total equation constraint: $A_{eq}x=b_{eq}$

Then the simple inequality constraint could be defined as:

If we consider the volume of the vehicle, like in Figure 10, we need to make sure that all four edge points have to be within the range of the road boundary. Therefore, we have the expression of four endpoints:

Because $\theta$ is a very small value, by trigonometric functions, it is easier to do the approximation:

So the equation 21 could be rewritten as below:

For safer manoeuvre and easier implementation, we first find the minimum of $l_{m}axi$ between $s_{i}-d_{2}$ and $s_{i}+d_{1}$ and note it as $ub_{i}$. So our constraints become:

For the same reason, the same constraints are implemented for the lower boundary, therefore, in the end, we will have the inequality constraint:

Rewrite 25 in matrix form, we will obtain: ,

Then the last step is to write it in $A_{sub}x\leq b_{sub}$ to satisfy the inequality constraint formulation in Quadratic Programming:

Until now, we have already given the Objective Function 15, Equation Constraint 19 and Inequality Constraint 25 in Quadratic Programming. It is easier to solve the optimization problem with this formulation in Matlab/Simulink.

Firstly, the station coordinate system is the path in a Cartesian coordinate system. In the next several seconds, if the dynamic obstacles will be projected on the station coordinate system, that will be presented in the ST graph.

In the past, uniform sampling is implemented in the ST graph, leading to huge computation complexity. Therefore, we also proposed to implement adaptive sampling using dynamic vehicle constraints here so as to delete some impossible nodes before dynamic programming. To formalize the problem, vehicle dynamics are given in the form: $v\in[v_{min},v_{max}]$, $a\in[a_{min},a_{max}]$, $J\in[J_{min},J_{max}]$

Similar to path planning, here we defined the cost function of our ASAPF:

1. 1)

   Smoothing Cost

   $$U_{smooth}(s,t)=w_{1}\int(s^{{}^{\prime}}(t)-s^{{}^{\prime}}(t-1))^{2}dt+w_{2}\int(s^{{}^{\prime\prime}}(t))dt+w_{3}\int(s^{{}^{\prime\prime\prime}}(t))dt$$

   (29)

2. 2)

   Obstacle Cost

   $$U_{obs}(s,t)=\begin{cases}\frac{1}{2}*w*\frac{1}{d^{2}(s(t),s_{obs\_upper}(t)+1)},&s(t)>s_{obs\_upper}(t)+1\\ +inf,&s_{obs\_lower}(t)-1\leq l\leq s_{obs\_upper}(t)+1\\ \frac{1}{2}*w*\frac{1}{d^{2}(s(t),s_{obs\_lower(t)}-1)},&s(t)<s_{o}bs\_lower(t)-1\end{cases}$$

   (30)

3. 3)

   Constraint Cost

   $$U_{constraint}(s,t)=\begin{cases}0,&s(t)-s(t-1)\in[v_{min},v_{max}]\\ &and\ s^{{}^{\prime}}(t)-s^{{}^{\prime}}(t-1)\in[a_{min},a_{max}]\\ &and\ s^{{}^{\prime\prime}}(t)-s^{{}^{\prime\prime}}(t-1)\in[J_{min},J_{max}]\\ +inf,&otherwise\end{cases}$$

   (31)

For example, if $v_{0}=0m/s$, $a_{0}=0$, prediction period $T=8s$, $v\in[-5,5]$, $a\in[-2,2]$, by implementing our adaptive sampling method in ST graph, we will have Figure 11. During this figure, the blue points are the result of average sampling, and the green points are the result of our adaptive sampling. We could see from the graph that the useless nodes have already been removed before Dynamic Programming in ST graph, therefore the computational task is reduced too, same as adaptive sampling in path planning.

From the perception module, it is not difficult to obtain information on dynamic obstacles (x coordinate, y coordinate, velocity and heading angle). Then, these dynamic obstacles need to be projected on the ST graph we built in the last step to represent the prediction of the trajectory of dynamic obstacles. For the reason of simplicity, the velocity of dynamic obstacles is supposed to be constant. This assumption will not lead to the failure of speed planning, because our planning time interval is very short (around 100 milliseconds). During this short interval, speed could be approximated as constant. Similar to path planning, the goal of speed planning is to find the best speed profile on the ST graph which is a non-convex optimization problem too. Therefore, dynamic programming combined with quadratic programming is used to find a smooth speed profile on the ST graph. In detail, a piece-wise linear speed profile function is represented as $s=(s_{0},s_{1},...,s_{n})$ on the grids. The finite difference method is taken to approximate the derivatives. If $dt$ represents the time interval on the time axis between two evaluated points with equal space gap, then it is easy to give the formulation of velocity $v_{i}$, acceleration $a_{i}$, and jerk $j_{i}$:

The cost function needs to be specifically defined for the speed profile here because it is different from that in the SL graph. The idea is also to optimize a cost function in the ST graph within the constraints. In detail, the cost function for dynamic programming in speed planning is represented as follows:

If there are no obstacles, the vehicle should try to follow the reference velocity, so the first term is the velocity-keeping cost. The acceleration and jerk square integral describes the smoothness of the speed profile. The last term, $C_{obs}$ decided by the distance with obstacles’ boundaries, describes the total obstacle cost.

After Dynamic Programming in the last step, we have already known that the autonomous vehicle needs to accelerate or decelerate to avoid dynamic obstacles avoidance. Then, the next step is to refine the speed profile so as to improve the success of path following in the Control module and the comfort of passengers. Same as in path planning, we defined the cost function and constraints in Quadratic Programming too here. The cost function is described as follows:

The constraints of QP:

which is given in [5].