Review of Vehicle Motion Planning and Control Techniques to Reproduce Human-like Curve-Driving Behavior ^†

Abstract

Among the many technological challenges of automated driving development, there is an increasing focus on the behavior of these systems. Behavior is usually associated with multiple layers of control. In this paper, we focus on motion planning and control, and how these layers can be tailored to produce different behavior. Our review aims to collect and judge the most used techniques in the field of path planning and control. It has been revealed that model predictive planning and control provides high flexibility, with the cost of high computational capacity. There are simpler algorithms, such as pure-pursuit and Stanley controllers, however, these have very few parameters, therefore, the number of possible behavior patterns is limited.

1. Introduction

In the field of Advanced Driver Assistance Systems (ADAS), one of the most widely used functions is the Lane Keep or Lane Centering System (LKS or LCS) [1]. This function monitors the drivable corridor and keeps the vehicle in the center of the lane. To do that, the path-planning module calculates a reference line, which is then handed over to the vehicle controller. Even though there are many path-planning algorithms in the field [2,3,4,5,6,7,8,9], in a lot of cases, the reference line is associated with the centerline of the lane. Then, the controller—based on its internal policy—calculates a target steering angle, which is then applied to the actuator controls. In the end, the vehicle’s lateral motion is influenced by actuating the steering wheel. The composition of path planning and control is a commonly accepted technique of building automated driving systems [10]. Different behavior patterns can be recognized by tuning the control algorithm’s free parameters. In the literature, there are control algorithms that can be classified into one of the following three types:

• Inverse-dynamic models;
• Compensatory models;
• Feedforward models.

Inverse-dynamic models use a vehicle model to calculate the target steering angle to reach a certain target state (i.e., vehicle position). Such controllers are the pure-pursuit algorithm [11] or the Stanley controller [12]. Compensatory models assume that drivers intervene in relation to the error of a certain output quantity, such as the vehicle position or orientation. Hence, these control models often act as closed-loop controllers, and the drivers are modeled using simple PID or other linear control structures [13,14,15,16]. Feedforward models also use a vehicle model to predict its behavior on a certain input trajectory. Then, based on a pre-defined objective function, they optimize the behavior and calculate the optimum input trajectory on a so-called preview horizon. Such controllers are the model predictive controls [17,18,19,20]. In this paper, we implement algorithms of each type in MATLAB and run a closed-loop simulation on a real-world road segment. We compare each algorithm by its ability to provide accuracy and flexible paths and its computation effort.

2. Materials and Methods

2.1. Methods

The model is a kinematic single-track model. The model equations are given in Equation (1), where $T_s$ is the sampling time, $L$ is the axis distance of the vehicle, ${\mathit{x}}_k={\left[x y \mathsf{\Theta }\right]}^T$ is the state vector, and $u_k={\alpha }_k$ is the input of the system. The used coordinate frame $\left(x,y\right)$, the local frame $(\xi ,\eta )$, and the related kinematic quantities are shown in Figure 1a. Finally, the output vector is ${\mathit{y}}_k={\left[y_k {\mathsf{\Theta }}_k\right]}^T$.

$$f=\left[\begin{array}{c}{x}_k\\ y_k\\ {\mathsf{\Theta }}_k\end{array}\right]=x_k=\left[\begin{array}{c}{x}_{k-1}+T_s{v}_{\xi }\cos \left({\Theta }_{k-1}\right)\\ y_{k-1}+T_s{v}_{\xi }\sin \left({\Theta }_{k-1}\right)\\ {\mathsf{\Theta }}_{k-1}+{\displaystyle \frac{T_s{v}_{\xi }\tan \left({\alpha }_f\right)}{L}}\end{array}\right]$$

(1)

The kinematic single-track model is implemented in MATLAB. The simulations are run using a real-life road section from Hungary, Road 31. The path is shown in Figure 2. The model given in Equation (1) is implemented in MATLAB, too.

2.2. Model Predictive Control

The MPC algorithms rely on the following basic steps:

• Prediction of the vehicle states $X_k=[x_k{x}_{k+1}\dots x_{k+N_p-1}]\in {\mathbb{R}}^{n\times N_p}$, $X_k=f(X_{k-1},U,)$, given the input vector $U=[u_k{u}_{k+1}\dots u_{k+N_p-1}]\in {\mathbb{R}}^{m\times N_p}$ on the prediction horizon $N_p$, and the output $Y=[y_k{y}_{k+1}\dots y_{k+N_p-1}]\in {\mathbb{R}}^{o\times N_p}$, where $o$ is the number of outputs;
• Calculating the reference output, given the prior path $Y_{ref}=g(\gamma \left(p_{driver}\right),v_{\xi }, N_p, T_h)$, where $T_h$ is the prediction horizon and $\gamma$ is the representation of the prior path;
• Calculating the cost $h(Y, Y_{ref},Y_{traffic}, w_{driver},c_{driver})$.

The optimization variables are elements of $U$, and the cost function $h(·)$ is minimized as follows:

$$\begin{gathered} \underset{U}{\min }h(·) \\ s.t.u_{min}\le U\le u_{max} \end{gathered}$$

As model Equation (1) is non-linear, a numerical non-linear optimization algorithm is used. This is carried out in MATLAB, using the ‘fmincon’ function of the optimization toolbox. The initial value of the input vector $U$ is zero, then, the previously calculated optimal input is taken as the initial point of the next optimization loop. The MPC cost function always includes a term that is associated with the error of the output(s) to the reference. Thus, the reference output vector $y_{ref}$ is calculated from the prior path and interpolated to the predicted vehicle positions in Equation (2), as follows:

$$Y_{ref}=interp(\gamma ,x_{pred}),$$

(2)

where $x_{pred}=x_0+[d{x}_{1}d{x}_2\dots d{x}_k\dots d{x}_{N_p}]$, $d{x}_k=v_{\xi }\mathrm{c}\mathrm{o}\mathrm{s}\left({\mathsf{\Theta }}_k\right)$, and $x_0$ is the vehicle position at the time of planning. The geometric relations are shown in Figure 3a. The cost function also includes a term that is controversial with the output error, namely the amplitude of the steering angle. Then, the final cost function is given by Equation (3), as follows:

$$h\left(·\right)=\sum _{i=1}^{N_p}{\left(y_{ref,i}-y_i\right)}^T{W}_y\left(y_{ref,i}-y_i\right)+u_i^2{w}_{\alpha },$$

(3)

where $W_y=diag\left(\left[w_{dy} w_{d\Theta }\right]\right)$ is the weight matrix, $w_{\alpha }$ is the input weight, and $y_{ref,i}=Y[:,i]$, i.e., the $i^{th}$ column of the output matrix.

2.3. Pure-Pursuit Controller

The pure-pursuit controller relies on a simple concept. A look-ahead point is calculated, and then a circular path is defined, which leads the vehicle from its location to this look-ahead point. Geometrical relations are shown in Figure 3b. The curvature of the target path is calculated according to Equation (4), as follows:

$${\kappa }_{tar}={\displaystyle \frac{l_0^2}{2{\eta }_0}}$$

(4)

Then, the target steering angle can be calculated according to Equation (4). The algorithm has one parameter $l_0$, which also defines $p_0({\xi }_0,{\eta }_0)$. Often—to overcome speed dependency issues—the look-ahead distance $l_0$ is replaced by look-ahead time, and then $l_0=v_{\xi }{t}_0$.

2.4. Stanley Controller

The Stanley controller is a simple but robust way of leading vehicles. There are two main differences compared to the pure-pursuit algorithm, as follows: the deviation from the reference path (often called cross-track error) is calculated for the front axis of the vehicle. Secondly, the orientation error to the tangent of the reference line at the vehicle position is also considered. Its illustration is shown in Figure 3c. Then, the target steering angle can be calculated according to Equation (5). It has one parameter, which is the Stanley coefficient $k$.

$${\alpha }_{tar}=\mathrm{atan}\left({\displaystyle \frac{ke}{v_{\xi }}}\right)+{\theta }_0$$

(5)

2.5. Compensatory Driver Model

The compensatory driver model in this implementation is a parallel PID controller, which considers the deviation from the reference path and its integral and derivative terms [13]. Its geometrical relations are shown in Figure 3d. The error quantities are calculated at the vehicle position. The target steering angle is given by Equation (6). The algorithm has 3 parameters, and the control coefficients are associated with the different terms.

$${\alpha }_{tar}=Pe+I\int edt+D{\displaystyle \frac{de}{dt}}$$

(6)

3. Results

3.1. Evaluation Criteria

In Section 3.2, the experiments are presented to show the behavior of each algorithm. The following quantities are examined:

• $\delta \left(t\right)$—lane offset to the centerline;
• ${\alpha }_f$—front road wheel angle, which is the input of the system;
• $t_{ct}$—computational time/simulation cycle, measured in MATLAB.

The results are generated using the simulation framework detailed in Section 2.1.

3.2. Experiments

The following tests are run to show the working principle of the proposed planning and control algorithm:

• Test I: Neutral behavior, which is the compromise between the steering wheel oscillation and the tracking accuracy;
• Test II: Mistune—to produce a behavior that deviates from the neutral behavior.

The parameters of each test case are given in

Table 1. Parameters and description of the test cases.

	MPC	Pure-Pursuit	Stanley	PID
Test I.	$w_{dy}=0.0$$, w_{d\Theta }=2.0$$, w_{\alpha }=1.0$	$t_0=1.5 \mathrm{s}$	$k=1$	$P=0.1$$, I=0.01, D=0.05$
Test II.	$w_{dy}=0.0$$, w_{d\Theta }=2.0$$, w_{\alpha }=1.5$	$t_0=2.25 \mathrm{s}$	$k=0.5$	$P=0.01$$, I=0.01$$, D=0.05$

.

The results of Test I are shown in Figure 4. The MPC algorithm has the highest computational capacity needs, while providing moderate error on the position output. The steering angle has low frequency oscillation. The pure-pursuit algorithm provides a moderate error on the position output but a very smooth steering angle. Its computational time need is the lowest. The Stanley controller provides the best track error; however, the steering angle has high frequency oscillation. The simple PID controller produces a very similar output as the Stanley controller, with both having higher computational time needs than pure-pursuit. Interestingly, all of the controllers result in overshoot in the curves, except pure-pursuit, which cuts the curve slightly.

A second test was run, where each controller was tuned differently. The aim was to show how the parameters influence the output behavior. The results are shown in Figure 5. Having a higher weight associated with the input signal, the output error from the NMPC algorithm increases. Having a higher look-ahead time, the pure-pursuit algorithm cuts the curves more significantly, which is also a usual behavior of drivers. It must be noted that this algorithm is not suitable to use for outer-lane driving in curves. Having a lower Stanley coefficient results in higher deviation from the reference path and causes increased computational time at certain parts of the simulation. However, the steering angle does not change significantly. Having lower proportional gain, the PID controller results in a much higher overshoot in curves, while not significantly changing the input steering angle. Interestingly, similarly to the Stanley controller, the computational time increases in this case as well.

4. Conclusions

4.1. Contribution

In this paper, we have implemented prototypically and compared the most relevant control algorithms. It has been shown that, considering a simple kinematic bicycle model, the MPC algorithm produces the worst output. However, it is also emphasized that the advantages of MPC come when the complexity of the system under control grows. Also, other terms can be added to the objective function (e.g., traffic, distance to lane edges, etc.), which increases the flexibility of this algorithm. Pure-pursuit provides the smoothest steering angle signal, however, its flexibility to provide different motion behavior is low. It can only produce curve cuts. Stanley and PID controllers are highly similar, however, while the Stanley controller is relatively ineffective on the parameter changes, the PID controller performance degraded a lot when changing the gains.

4.2. Limitations

The implementation took place in MATLAB, considering only a kinematic single-track model. This makes the value of the work lower, as the results cannot be directly taken over to practical implementation, e.g., in real-vehicle controllers. Also, the tuning was carried out empirically, which makes the comparison less reliable. However, the characteristics of the controllers could be nicely shown.

4.3. Outlook

In the future, we plan to continue the analysis with real-vehicle application, as well as further extension of the MPC algorithm, to show the real features of these controllers in terms of human-like driving of automated systems.