Real-Time Constrained Trajectory Planning and Vehicle Control for Proactive Autonomous Driving With Road Users

TL;DR

This paper introduces a real-time, proactive vehicle motion planning and control framework using Model Predictive Control that accounts for moving pedestrians, validated through simulations and experiments demonstrating stability and low computational delay.

Contribution

It presents a novel MPC-based framework incorporating pedestrian motion prediction into vehicle control for autonomous driving.

Findings

Controller remains stable under input delays

Framework achieves low computational times

Validated with real pedestrian data in simulations

Abstract

For motion planning and control of autonomous vehicles to be proactive and safe, pedestrians' and other road users' motions must be considered. In this paper, we present a vehicle motion planning and control framework, based on Model Predictive Control, accounting for moving obstacles. Measured pedestrian states are fed into a prediction layer which translates each pedestrians' predicted motion into constraints for the MPC problem. Simulations and experimental validation were performed with simulated crossing pedestrians to show the performance of the framework. Experimental results show that the controller is stable even under significant input delays, while still maintaining very low computational times. In addition, real pedestrian data was used to further validate the developed framework in simulations.

Figures

Equations

$$\displaystyle{\mathbf{x}}:=\left[\begin{array}[]{cccccc}x&y&v&\theta&\delta&\omega\end{array}\right]^{\top},$$

$$\dot{{\mathbf{x}}}=\left[\begin{array}[]{c}\dot{x}\\ \dot{y}\\ \dot{v}\\ \dot{\theta}\\ \dot{\delta}\\ \dot{\omega}\end{array}\right]=\left[\begin{array}[]{c}v\cos\theta\\ v\sin\theta\\ a\\ v/l_{w}\tan\delta\\ \omega\\ w_{0}^{2}(\delta_{\mathrm{sp}}-\delta)-2\zeta\omega\end{array}\right]=:f_{\mathrm{c}}({\mathbf{x}},{\mathbf{u}}),$$

$$\underset{\bar{\mathbf{x}},\bar{\mathbf{u}}}{\min }$$

$$\displaystyle\qquad+\left[\begin{array}[]{c}\bar{\mathbf{x}}_{N}-{\mathbf{r}}_{N}^{\mathbf{x}}\end{array}\right]^{\top}W_{N}\left[\begin{array}[]{c}\bar{\mathbf{x}}_{N}-{\mathbf{r}}_{N}^{\mathbf{x}}\end{array}\right]$$

$$\text{s.t.}{\bar{\mathbf{x}}}_0={\hat{\mathbf{x}}}_0,$$

$${\bar{\mathbf{x}}}_{k+1}=f({\bar{\mathbf{x}}}_k,{\bar{\mathbf{u}}}_k),$$

$$h({\bar{\mathbf{x}}}_k,{\bar{\mathbf{u}}}_k)\le 0,$$

$$A_{k}=\frac{\partial{}f}{\partial{}{\mathbf{x}}}\bigg{\rvert}_{\bar{\mathbf{x}}_{k}^{\mathrm{g}},\bar{\mathbf{u}}_{k}^{\mathrm{g}}},B_{k}=\frac{\partial{}f}{\partial{}{\mathbf{u}}}\bigg{\rvert}_{\bar{\mathbf{x}}_{k}^{\mathrm{g}},\bar{\mathbf{u}}_{k}^{\mathrm{g}}},b_{k}=f(\bar{\mathbf{x}}_{k}^{\mathrm{g}},\bar{\mathbf{u}}_{k}^{\mathrm{g}})-\bar{\mathbf{x}}_{k+1}^{\mathrm{g}},$$

$$\underset{\bar{\mathbf{x}},\bar{\mathbf{u}}}{\min }$$

$$\displaystyle\qquad+\left[\begin{array}[]{c}\bar{\mathbf{x}}_{N}-{\mathbf{r}}_{N}^{\mathbf{x}}\end{array}\right]^{\top}Q_{N}\left[\begin{array}[]{c}\bar{\mathbf{x}}_{N}-{\mathbf{r}}_{N}^{\mathbf{x}}\end{array}\right]$$

$$\text{s.t.}{\bar{\mathbf{x}}}_0={\hat{\mathbf{x}}}_0,$$

$${\bar{\mathbf{x}}}_{k+1}=A_k{\bar{\mathbf{x}}}_k+B_k{\bar{\mathbf{u}}}_k+b_k,$$

$${\mathbf{d}}_k^{\mathrm{l}}\le C_k{\bar{\mathbf{x}}}_k+D_k{\bar{\mathbf{u}}}_k\le {\mathbf{d}}_k^{\mathrm{u}}$$

$${\mathbf{d}}_N^{\mathrm{l}}\le C_N{\bar{\mathbf{x}}}_N\le {\mathbf{d}}_N^{\mathrm{u}}.$$

$${\sigma }_k={\int }_0^t(1-\kappa (\tau )e_y(\tau ){)}^{-1}v(\tau )\cos (e_{\psi }(\tau ))d\tau ,$$

$$r_{k}^{\theta}=\tan^{-1}\Big{(}\frac{r_{k+1}^{y}-r_{k}^{y}}{r_{k+1}^{x}-r^{x}_{k}}\Big{)},$$

$$\begin{array}{c}{Q}_k=\mathrm{diag}({\bar{q}}_1,...,{\bar{q}}_m),\\ R_k=\mathrm{diag}({\bar{r}}_1,...,{\bar{r}}_n),S=0,\end{array}$$

$$\displaystyle\left(\left[\begin{array}[]{c}0\\ 1\end{array}\right]^{\top}\left[\begin{array}[]{cc}\cos(-r^{\theta}_{k})&-\sin(-r^{\theta}_{k})\\ \sin(-r^{\theta}_{k})&\cos(-r^{\theta}_{k})\end{array}\right]\left[\begin{array}[]{c}\bar{x}_{k}-r_{k}^{x}\\ \bar{y}_{k}-r_{k}^{y}\end{array}\right]\right)^{2}.$$

$$Q_k=\mathrm{blockdiag}({\bar{q}}_{1}T(-r_k^{\theta }),{\bar{q}}_2,{\bar{q}}_3,{\bar{q}}_4,{\bar{q}}_5),$$

$$T(\theta)=\left[\begin{array}[]{cc}\sin^{2}\theta&\cos\theta\sin\theta\\ \cos\theta\sin\theta&\cos^{2}\theta\end{array}\right].$$

$${\Delta }^{\mathrm{road}}-l_d\le {\bar{x}}_k\sin (-{\hat{\theta }}_k^{\mathrm{g}})+{\bar{y}}_k\cos (-{\hat{\theta }}_k^{\mathrm{g}})\le {\Delta }^{\mathrm{road}}+l_d,$$

$$\Delta^{\mathrm{ped}}\mathbf{l}_{b}\leq{}\Delta^{\mathrm{ped}}\left[\begin{array}[]{c}\bar{x}_{k}\\ \bar{y}_{k}\end{array}\right],$$

$$\mathbf{l}_{b}=\left[\begin{array}[]{c}x_{k}^{\mathrm{ped}}-d_{k}^{\mathrm{safe}}\cos\phi_{k}-(\bar{x}_{k}^{\mathrm{ego}}-\bar{x}_{k}^{\mathrm{g}})\\ y_{k}^{\mathrm{ped}}-d_{k}^{\mathrm{safe}}\sin\phi_{k}-(\bar{y}_{k}^{\mathrm{ego}}-\bar{y}_{k}^{\mathrm{g}})\end{array}\right].$$

$$l_w=2.984\mathrm{m},w_0=20{\mathrm{s}}^{-1},\zeta =0.9{\mathrm{s}}^{-1}.$$

$$\begin{array}{c}{Q}_k=\mathrm{blockdiag}(2T(-r_k^{\theta }),0.1,10,0.1,10),\\ R_k=\mathrm{blockdiag}(2,1),S_k=0,N=100,\end{array}$$

$$\begin{array}{c}-1\le {\bar{v}}_k\le 20,|{\bar{\delta }}_k|\le 0.4942,|{\bar{\omega }}_k|\le 0.1765,\\ -2\le {\bar{a}}_k\le 1,|{\bar{\delta }}_{k,\mathrm{sp}}|\le 0.4942,\end{array}$$

$$a_1{\bar{v}}_k+b_1{\bar{\delta }}_k\le c_1,a_2{\bar{v}}_k+b_2{\bar{\delta }}_k\ge c_2.$$

Full text

Simulations and experimental validation were performed with simulated crossing pedestrians to show the performance of the framework. Experimental results show that the controller is stable even under significant input delays, while still maintaining very low computational times. In addition, real pedestrian data was used to further validate the developed framework in simulations.

I Introduction

Since a decade, autonomous driving technologies have emerged with the potential for increased safe and efficient driving [1]. While one can expect autonomous driving technologies to be deployed first in structured environments such as highway driving and low-speed parking [2], other scenarios, such as urban driving, arguably pose a greater challenge due to the presence of non-autonomous road users, e.g. pedestrians, cyclists, and other vehicles. Hence, the research focus needs to be directed to deriving models for predicting the stochastic behavior of human road users, while also avoiding collisions with them.

This paper targets the autonomous driving problem in complex urban environments where road users are present. The autonomous vehicle needs to drive along a pre-defined route, while also ensuring that collisions are avoided. To that end, it is necessary that the vehicle is provided with information about the environment, such as where a road user will most likely be in the future, in order to be proactive and plan for a collision-free path. Therefore, the vehicle motion planning and control need to be handled in real-time, hence a low computational complexity is needed.

While safety is the dominating factor when designing driving trajectories for autonomous driving, passengers’ comfort also needs to be considered. The main contributing factors to ride discomfort are considered to be high levels of jerk and acceleration [3]. Several methods have been proposed where these factors are taken in consideration and optimized for driving scenarios on highways or country roads [4, 5]. Similarly, the work in [6, 7] minimizes jerk profiles under fixed travel times set beforehand.

For urban driving, [8] presents a framework for trajectory planning by decomposing the problem in spatial path planning and velocity profile planning. However, the planner does not consider reactive behaviors such as responding to dynamic objects in the environment. Other frameworks [9, 10] use grid or graph-based methods to either plan paths around pedestrians or stop for them, while making an assumption that alternative paths exist, which may not always hold true in urban scenarios. Planners using human comfort as optimization are also able to plan paths around obstacles [11, 12], but avoid stopping as it is not always optimal. Similar to other planners, [13] uses a path-velocity decomposition to generate path and velocity profiles in parallel. While the planner is able to slow down, or come to a complete stop for pedestrians, it does not consider pedestrian movements in time, which might affect the planning capability.

In this paper we provide a general real-time framework to handle autonomous driving in urban scenarios with pedestrian crossing intersections. Unlike the common approach in literature, we do not decouple the longitudinal and lateral control of the vehicle. Instead, we solve the trajectory planning and vehicle control problem simultaneously, with the assumption that a higher level navigation layer provides a nominal driving route, e.g. center of the road lane, that the vehicle needs to track. Model Predictive Control (MPC) is used to generate optimal trajectories, while considering other road users by predicting their motion in time. When considering other road users, other approaches in the literature often predict the future motion with primitive methods, e.g. constant velocity or acceleration models. In this paper we use an environment-aware predictor, developed in [14], to obtain more accurate future predictions, and unify the collision avoidance problem simultaneously with the vehicle motion and control problem.

We ensure collision avoidance by transforming the predicted motion and uncertainty of other road users into constraints within the MPC formulation. Doing so, allows us to plan a trajectory in time, while being proactive and avoiding collisions with moving road users or other static obstacles. Fig. 1 shows an example of how predicted pedestrian positions can be used for collision avoidance. The red circles express regions at different planning times that the vehicle is not allowed to enter. It is important to note that we aim at addressing a nominal behavior which guarantees safety and comfort, but which also requires an emergency layer which would intervene in case something unexpected happens. The safety layer is the subject of ongoing research.

This paper is structured as follows. In Section II we introduce the problem formulation and our framework along with the used vehicle and pedestrian prediction models. The framework is evaluated, both in simulations and real experiments, for scenarios with simulated pedestrians moving near an intersection in Section III. Finally, we draw conclusions and outline future research directions in Section IV.

II Problem Formulation and Framework

An autonomous driving application needs above all to be safe, and in order to safely coexist with a higher level of riding comfort, the vehicle behavior needs to be proactive. By predicting the future evolution of the driving environment, one can incorporate cautiousness to minimize the risk of collisions with surrounding traffic participants (pedestrians, cyclists, cars). In the following, we first introduce the system architecture, then we explain the vehicle and pedestrian models used in our decision problem. Finally, we present the reference path generation, cost function, and system constraints used to formulate our control objectives.

II-A System Architecture

At each sampling instant, we assume that the vehicle receives estimated vehicle states $\hat{\mathbf{x}}_{0}$ from an estimator, previous guesses of state and control input trajectories $\bar{\mathbf{x}}_{k}^{\mathrm{g}}$ and $\bar{\mathbf{u}}_{k}^{\mathrm{g}}$ respectively, and a path reference $\mathbf{\xi}^{\mathrm{c}}$ and $\mathbf{v}^{\mathrm{c}}$ from a nominal path module. State measurements $\mathbf{x}_{k}^{\mathrm{meas}}$ of a detected pedestrian111Although our approach can be extended to other road users, for convenience of exposition we’ll refer to pedestrians are sent to a module that generates predictions ${\mathbf{x}}_{k}^{\mathrm{ped}}$ of the pedestrian positions in time. The planner uses the estimated states, the state and control guesses, the path references, and the pedestrian predictions to control the vehicle along the path, while avoiding collisions. This paper focuses on the planner represented as the largest dashed rectangle in Fig. 2.

II-B Vehicle Model

For urban autonomous driving, lateral, longitudinal, and yaw dynamics are quite fast [15]. Therefore, a kinematic bicycle model was chosen for its simplicity and comparable accuracy with a dynamic one [16] in the scenarios considered in this paper. A vehicle model sketch is shown in Fig. 1.

We model the vehicle motion with states $\mathbf{x}\in\mathbb{R}^{6}$, and controls $\mathbf{u}\in\mathbb{R}^{2}$, defined as

[TABLE]

where we denote the position coordinates of the rear wheel in the absolute reference frame G as $x,y$, the velocity in direction $\theta$ as $v$, and the steering angle and steering angle rate as $\delta$ and $\omega$, respectively. The acceleration and steering angle setpoint are denoted respectively by $a$ and $\delta_{\mathrm{sp}}$, where the setpoint refers to desired steering angle at the wheel base.

With the chosen state representation, the dynamics are described by the bicycle equations as

[TABLE]

where the only model parameters are: the wheel base $l_{w}$ and the parameters $w_{0}$ and $\zeta$, introduced to describe a second order model of the steering wheel actuator.

For simplicity, we chose to omit the longitudinal jerk in the model. It could, however, easily be implemented without any greater complication or performance impact.

II-C Pedestrian Model

The performance of a planner is predominantly limited by the accuracy of the information it is provided with: generating plans with highly inaccurate information about the surrounding environment will result in suboptimal and possibly unsafe or conservative plans. Since our planner solves an optimal control problem over a prediction horizon, a description of the environment, e.g. pedestrian positions and road boundaries, is necessary throughout the horizon.

We chose to use the flexible and fairly accurate method presented in [14] to generate pedestrian predictions needed for our planner. A simple representation of the road geometry is used in order to assign possible paths (hereafter referred to as references), e.g. sidewalks and crosswalks, where a pedestrian might walk. The pedestrian motion is modeled using a unicycle model, which is assumed to follow the reference, thanks to a closed-loop regulator. Process noise is added to the model, and state uncertainty is predicted by propagating its covariance. Using the closed-loop system model, pedestrian trajectories and state covariances can be predicted along the road geometry.

Moreover, if a pedestrian is walking on a road that has a bifurcation, e.g. the road splits into multiple walking paths, the prediction method takes into account all possible directions and generates predictions along these paths. As the number of paths or obstacles increase, the computational complexity may be contained through parallelization.

II-D Problem Statement

The objective of the vehicle is to safely follow any given path as close as possible, while driving comfortably and satisfying constraints arising from the physical limitations, but also other road users, e.g pedestrians, cyclists and other vehicles.

This problem can be formulated as a finite horizon, constrained optimal control problem. Since the vehicle model and constraints are nonlinear, we are faced with the following Nonlinear Model Predictive Control (NMPC) problem

[TABLE]

where $N$ is the prediction horizon, $W_{k}$ is the stage cost matrix, $\bar{\mathbf{x}}_{k}$ and $\bar{\mathbf{u}}_{k}$ are the state and control input, ${\mathbf{r}}_{k}^{\mathbf{x}}$ and ${\mathbf{r}}_{k}^{\mathbf{u}}$ are the state and control input reference, constraint (2i) enforces that the prediction starts at the current state estimate $\hat{\mathbf{x}}_{0}$, constraint (2j) enforces the system dynamics, and (2k) enforces constraints such as, e.g. actuator limits and obstacle avoidance. The system dynamics in discrete time are obtained through numerical integration of (1). Note that proactivity naturally follows in the MPC framework through consideration of predicted constraints over the horizon.

With the algorithms described in [17, 18] the Nonlinear Program (NLP) (2) can be rewritten using a Sequential Quadratic Programming (SQP) approach, which sequentially approximates the NLP with a Quadratic Program (QP) (3). Here, we only give a brief summary of the methodology. For any further details we refer the reader to [17, 18]. Following the SQP approach we linearize the system model with a previous guess $(\bar{\mathbf{x}}_{k}^{\mathrm{g}},\bar{\mathbf{u}}_{k}^{\mathrm{g}})$ and use any numerical discretization scheme to obtain the sensitivities

[TABLE]

for every prediction time step $k$. The quantities $C_{k}$, $D_{k}$, $\mathbf{d}_{k}^{\mathrm{l}}$, $\mathbf{d}_{k}^{\mathrm{u}}$ $C_{N}$, $\mathbf{d}_{N}^{\mathrm{l}}$, and $\mathbf{d}_{N}^{\mathrm{u}}$ can be computed in a similar manner from constraint (2k). With the sensitivities, the problem is then formulated into a QP and solved, where the solution is used to update the previous guess. This process is either iterated until convergence, or only done once, thus following the real time iteration (RTI) scheme [18]. At each iteration, the following QP is solved

[TABLE]

The tuning parameters here are the stage cost matrices $Q_{k}$, $R_{k}$ and $S_{k}$, and the prediction horizon $N$. Note that all matrices in this formulation are in general time-varying.

At every sampling instant, Problem (3) is solved, and only the first control input ${\mathbf{u}}_{0}$ is applied to the system. However, the solutions $\bar{\mathbf{x}}$ and $\bar{\mathbf{u}}$ are used to update the guesses $(\bar{\mathbf{x}}_{k}^{\mathrm{g}},\bar{\mathbf{u}}_{k}^{\mathrm{g}})=(\bar{\mathbf{x}}_{k+1},\bar{\mathbf{u}}_{k+1})$, where $k$ spans the prediction horizon, following the shifting approach in [17, 18].

II-E Reference Generation

The generation of the trajectory reference deserves a specific discussion, since it is a crucial component of the control scheme and, as such, has a strong effect on the closed-loop behavior. In standard MPC implementations, the reference is updated from one time step to the next by shifting: $({\mathbf{r}}_{k}^{\mathbf{x}},{\mathbf{r}}_{k}^{\mathbf{u}})=({\mathbf{r}}_{k+1}^{\mathbf{x}},{\mathbf{r}}_{k+1}^{\mathbf{u}})$. In our setup instead, we recompute the reference at every time instant in order to avoid aggressive controller behaviors when the vehicle is forced to slow down due to the presence of an obstacle.

In particular, rather than relying on a reference trajectory, we choose a reference path and a reference velocity along it. The advantage of such a choice is that, if the vehicle is forced to slow down by the presence of an obstacle, the controller will not try to recover the lost time and instead it will simply try to bring the vehicle back at the desired velocity. In our research, we adopt an approach which is very similar to the one proposed in [19] to address these issues. Differently from [19], we eliminate the curvilinear path parameter $\sigma$ from the problem formulation and we relate $\dot{\sigma}$ to the vehicle’s velocity directly.

We assume that a reference velocity $v^{\mathrm{c}}(\sigma)$ and a curve $\xi(\sigma)=(x^{\mathrm{c}}(\sigma),y^{\mathrm{c}}(\sigma),\theta^{\mathrm{c}}(\sigma),\delta^{\mathrm{c}}(\sigma),\omega^{\mathrm{c}}(\sigma),a^{\mathrm{c}}(\sigma),\delta_{\mathrm{sp}}^{\mathrm{c}}(\sigma))$ parametrized in the curvilinear coordinate $\sigma$ are available. Using the current position $(x_{0},y_{0})$, we define the initial reference coordinate $\sigma_{0}$ by projecting $(x_{0},y_{0})$ onto the curve $\xi(\sigma)$ such that $\sigma_{0}=\mathrm{arg}\min_{\sigma}\|(x_{0},y_{0})-(x^{\mathrm{c}}(\sigma),y^{\mathrm{c}}(\sigma))\|_{2}$. Then, we define reference points at time $k\Delta{t}$, where $\Delta{t}$ is the discretization time, by means of $\sigma_{k}=\sigma(k\Delta{t})$. In order to obtain a meaningful reference in the presence of obstacles, we propose to use

[TABLE]

where $\kappa$, $e_{y}$, and $e_{\psi}$ is the curvature, lateral error and orientation error w.r.t to the path. This guarantees that the predicted reference accommodates for the presence of obstacles [20]. Using the reference velocity in the computation of $\sigma(t)$, instead, would ignore the presence of obstacles and create the potential for unstable behavior, especially in the presence of sharp turns. In the context of the RTI scheme, $\sigma(t)$ is obtained by integration of the velocity computed at the previous time instant.

In practice, the curves $\xi(\sigma)$ and $v(\sigma)$ might not be available and the reference generation layer might have access only to a set of data points $\xi^{\mathrm{c}}_{p}$ and $v_{p}^{\mathrm{c}}$. Then, a curve can be obtained by interpolation. In this paper, we use a simple piecewise linear interpolation and the formula $\sigma_{k+1}=\sigma_{k}+\bar{v}_{k}^{\mathrm{g}}\cos(e_{\psi_{k}})\Delta{t}$ for approximate integration, where $\bar{v}_{k}^{\mathrm{g}}$ is the velocity component of $\bar{\mathbf{x}}^{\mathrm{g}}_{k}$. By selecting $\sigma_{k}$ according to $\bar{v}_{k}^{\mathrm{g}}$ rather than $r_{k}^{v}$, the algorithm is able to account for the presence of obstacles, as explained in Fig. 3. Note that in the second case, the orientation reference will refer to a point on the path far from the previously predicted vehicle position and, therefore, will result in an incorrect orientation reference and unwanted driving behavior.

Moreover, we assume that the only available data concerns positions and velocities, such that we need to define suitable references for the other states and controls. The position references are obtained as $(r_{k}^{x},r_{k}^{y})=(x^{c}(\sigma_{k}),y^{c}(\sigma_{k}))$, while the heading reference $r^{\theta}_{k}$ is computed by

[TABLE]

and the steering angle and steering angle rate references are set as the initial angle $\delta_{0}$ and zero, respectively, across all prediction times $k$. The reference for the control input is set as zero for the acceleration, and $\delta_{0}$ for the steering setpoint.

II-F Cost Function

Often, matrices $Q_{k}$, $R_{k}$, and $S_{k}$ are chosen to have the diagonal form

[TABLE]

where $m$ and $n$ denote the state and control input dimensions.

As mentioned in Section II-E, in the presence of obstacles, trying to force the system to catch up with a predefined evolution of the curvilinear coordinate $\sigma$ can yield an undesirably aggressive behavior. Therefore, we propose to remove the term penalizing this deviation from the cost. We do so by only penalizing the lateral deviation from the path $\xi^{\mathrm{c}}$ through the term

[TABLE]

Then, matrix $Q_{k}$ becomes

[TABLE]

with

[TABLE]

With this cost formulation we avoid undesirably aggressive behaviors and we still ensure that the cost matrices are positive semi-definite.

II-G System Constraints

The states and control inputs are subject to a set of constraints formulated in (3m)-(3n), which concern mainly safety, comfort, and physical limitations of the vehicle. These consist of the road boundary limits, but also of occupied areas of predicted pedestrians’ positions.

II-G1 Road Boundary Constraints

In order to decide on the lateral bounds of the road, we use the previous guesses $\bar{\mathbf{x}}^{\mathrm{g}}$ to project the positions $(\bar{x}^{\mathrm{g}}_{k},\bar{y}^{\mathrm{g}}_{k})$ onto the path. Knowing the projection point $(\hat{x}^{\mathrm{g}}_{k},\hat{y}^{\mathrm{g}}_{k})$, and orientation $\hat{\theta}_{k}^{\mathrm{g}}$, it is possible to express a lateral bound $l_{d}$ on the path as

[TABLE]

where $\Delta^{\mathrm{road}}=\hat{x}_{k}^{\mathrm{g}}\sin(-\hat{\theta}_{k}^{\mathrm{g}})+\hat{y}_{k}^{\mathrm{g}}\cos(-\hat{\theta}_{k}^{\mathrm{g}})$.

II-G2 Pedestrian Constraints

Information about future pedestrian states is provided by a prediction layer outside of the MPC framework. The predictions consist of positions $(x_{k}^{\mathrm{ped}},y_{k}^{\mathrm{ped}})$, the major and minor axes $\lambda_{k}^{a}$ and $\lambda_{k}^{b}$ corresponding to the covariance ellipse of a Gaussian probability density function, and the orientation $\alpha$ in a global frame. With this information, occupied pedestrian regions are defined in the time domain. Similarly, we define a safety ellipse around the vehicle that describes the space it occupies. An illustration is provided in Fig. 4.

The covariance ellipses are linearized using the previous vehicle positions $(\bar{x}_{k}^{\mathrm{g}},\bar{y}_{k}^{\mathrm{g}})$ and heading $\bar{\theta}_{k}^{\mathrm{g}}$ to find the vehicle center $(\bar{x}_{k}^{\mathrm{ego}},\bar{y}_{k}^{\mathrm{ego}})$. The predicted pedestrian positions $(x_{k}^{\mathrm{ped}},y_{k}^{\mathrm{ped}})$ and the vehicle center are used to compute the safety distances $d_{k}^{\mathrm{ego}}$ and $d_{k}^{\mathrm{ped}}$ for the vehicle and pedestrian using the relative orientation $\phi_{k}$. Considering the two safety distances, we express a linear constraint around the rear wheel of the vehicle as

[TABLE]

where \Delta^{\mathrm{ped}}=-\left[\begin{array}[]{cc}\cos\phi_{k}&\sin\phi_{k}\end{array}\right] and

[TABLE]

The resulting linearized constraint, together with a physical illustration of the parameters, is illustrated in Fig. 4.

III Simulations & Experimental Validation

In this section we present results from a traffic scenario simulation with a crossing pedestrian, and the ego vehicle. To model the pedestrian, we used a trajectory from the data set in [14]. To further verify the framework, we also provide experimental results for a traffic intersection with a simulated pedestrian.

III-A Simulation Setup

The controller was first tested in a simulated intersection scenario with a real pedestrian trajectory taken from [14]. In this scenario, the ego vehicle is traveling along a straight road towards an intersection which a pedestrian is approaching, see Fig. 6.

We generated sensitivites $A_{k}$ and $B_{k}$ and $b_{k}$ with a fourth order Runge-Kutta method, while ensuring that 5 integrator steps was sufficiently accurate, with a discretization time of $\Delta{}t=0.05$s, and the following parameter values

[TABLE]

The estimated state $\hat{\mathbf{x}}_{0}$ was obtained by integration, using an integrator with error control to guarantee the accuracy of the solution. The vehicle controller used

[TABLE]

and the state and control inputs were subject to the following constraints

[TABLE]

where all values are given in standard SI-units.

Finally, the lateral bound on the road boundary constraint (9) was set to be $l_{d}=1$m. In order to ensure feasibility of optimization problem (3), we relax constraint (9) with an L1-penalty. This ensures that if there exists a feasible solution for problem (3), then the relaxed problem yields the same solution [21, Prop. 6].

III-B Experimental Setup

The closed loop controller was experimentally validated at the Astazero222See http://www.astazero.com/ for more information test-track outside Gothenburg, Sweden. The experiment consisted of a traffic scenario including our ego vehicle, and a simulated walking pedestrian. A simulated pedestrian was used due to safety reasons. The vehicle needed to make a 90-degree left turn in the intersection, while a simulated pedestrian was moving close-by and could possibly cross. Fig. 5 shows the considered scenario, where the vehicle needs to drive along the orange line that represents the center of the driving lane, with an approaching pedestrian from the right.

III-B1 Test Vehicle

The vehicle used in the experiment was a Volvo XC90 T6 petrol-turbo SUV with the real-time open source software OpenDLV333See https://opendlv.org/ for more information. that interfaced sensors and actuators to an external computer for control. A Laptop computer (i7 2.8GHz, 16GB RAM) running Ubuntu 16.04 as a Virtual Machine was used to receive sensor data, compute the control signals, and send actuation requests to the vehicle with OpenDLV through an ethernet connection. The vehicle was equipped with a Real Time Kinematic (RTK) GPS receiver for high accuracy positioning, and the estimated state $\hat{\mathbf{x}}{}_{0}$ was obtained with an Extended Kalman Filter (EKF).

III-B2 Experimental Details

The interface between OpenDLV and the vehicle exhibited a delay of roughly $300$ms when sending signals to the steering actuator, but also reading from it. Therefore the state space vector in (1) was augmented with additional time-delayed states for the steering angle to model the input delay. Since we estimated the steering wheel actuator dynamics, dead reckoning was used for estimating the steering wheel angle and steering wheel angle rate. Furthermore, due to safety features in the vehicle, the actuator limited the steering wheel angle in a speed-dependent way while in autonomous driving mode. To model this, we implemented the linear constraint

[TABLE]

The sensitivities were generated in the same way as for the simulation, and used same parameter values as in (12) and tuning parameters as in (13).

III-C Results

We implemented the proposed framework in MATLAB first for the simulations, and then auto generated the code into C++, where it was interfaced with OpenDLV for experimental testing. Both implementations used the open source software ACADO [22] for auto generation of the sensitivities, and the solver from HPMPC [23] to solve the QP problem with the RTI scheme.

III-C1 Simulation

Fig. 6 illustrates the open-loop solutions across different time instants. The MPC controller is generating control inputs to minimize the deviation from the reference path and reference velocity $r_{k}^{v}=10\mathrm{m}/\mathrm{s}$. For time $t=1.25$s the open-loop solution is not affected by the predicted pedestrian positions, and the controller is generating inputs just to stay on the path. At time $t=7.5$s the vehicle has adapted its speed and managed to plan a path around the pedestrian instead of coming to a full stop. For times $t=10$s and $t=12.5$s, we can see how the vehicle finally passes the pedestrian, and the final closed-loop trajectory. The corresponding closed-loop states and control inputs are presented in Fig. 7.

The aggressive behavior on the steering rate is due to the deviation of the actual pedestrian position from the predicted one. When the vehicle is close to the pedestrian, the control authority is reduced and such uncertainties result in large control actions. Future research will investigate approaches to mitigate these undesirable behaviors.

Lastly, the runtime of the framework in MATLAB resulted in an average solution time of $22.4$ms with a standard deviation of $2.3$ms, and longest timing of $34.1$ms.

III-C2 Experiment

Fig. 8 shows the open-loop solutions across different time instants for the experiments at the test-track. The same controller from the simulation is used with the reference velocity $r_{k}^{v}=5\mathrm{m}/\mathrm{s}$. For time $t=13.75\mathrm{s}$ the open-loop solution is not affected by the pedestrian constraints, and the controller is generating inputs only to stay on the path. For time $t=20\mathrm{s}$ the prediction algorithm propagates pedestrian positions through the intersection, where one of the predictions crosses the vehicle’s path. Since the predicted positions are transformed into constraints, the vehicle is forced to slow down and make sure that the pedestrian passes. This can subsequently be seen in times $t=23.75\mathrm{s}$ through $t=30\mathrm{s}$. Fig. 9 shows the corresponding closed-loop states and control inputs. The computer running the controller sent actuation signals at a rate of $20$Hz. The actual runtime of the framework, including the pedestrian prediction method, resulted in an average solution time of $10.3$ms, with a standard deviation of $2$ms, and longest timing of $19.7$ms. Thus, the runtime leaves a great margin to further increase the algorithmic complexity, or the sampling rate of the control scheme. In addition, since the controller ran in a Virtual Machine and not on the native computer operating system, there is a possibility to further reduce the computational time.

IV Conclusions

In this paper we considered autonomous driving in urban environments with pedestrian crossings. We propose a general framework that solves the trajectory planning, and the longitudinal and lateral vehicle control problem both in simulations and real experiments. In addition, by combining predictions of the environment, it is shown that behaviors, such as slowing down, or stopping for a crossing pedestrian, are naturally included without any noteworthy increase in complexity. As shown, the framework already presents real-time performance without being highly optimized, except for the choice of sensitivity generation and QP solver.

Future work will aim to research situations where pedestrians suddenly appear to the framework due to sensor occlusion. In addition, we will aim to further verify the framework in more complex intersections with crossing pedestrians. Finally, future research will aim at developing a control scheme with recursive feasibility guarantees, which is robust to unexpected events.