Real-Time Control for Autonomous Racing Based on Viability Theory

TL;DR

This paper introduces a real-time autonomous racing control method using viability theory and a novel numerical scheme based on game theory, enabling safe and efficient miniature race car navigation.

Contribution

It develops a robust viability kernel computation method with a discriminating kernel approach, enhancing safety and real-time performance in autonomous racing.

Findings

The discriminating kernel provides a safer driving style.

Simulation and experiments show minimal lap time increase with improved safety.

The method enables real-time control for miniature autonomous race cars.

Abstract

In this paper we consider autonomous driving of miniature race cars. The viability kernel is used to efficiently generate finite look-ahead trajectories that maximize progress while remaining recursively feasible with respect to static obstacles (e.g., stay inside the track). Together with a low-level model predictive controller, this method makes real-time autonomous racing possible. The viability kernel computation is based on space discretization. To make the calculation robust against discretization errors, we propose a novel numerical scheme based on game theoretical methods, in particular the discriminating kernel. We show that the resulting algorithm provides an inner approximation of the viability kernel and guarantees that, for all states in the cell surrounding a viable grid point, there exists a control that keeps the system within the kernel. The performance of the proposed control method is studied in simulation where we determine the effects of various design choices and parameters and in experiments on an autonomous racing set-up maintained at the Automatic Control Laboratory of ETH Zurich. Both simulation and experimental results suggest that the more conservative approximation using the discriminating kernel results in safer driving style at the cost of a small increase in lap time.

Figures

Tables

Table 1. TABLE I: Influence of the viability constraints

Controller				mean lap time [s]	$\#$constr. violations	online comp. time [ms]		comp. time kernel [s]	memory [MB]
abbr.	kernel	grid				median	max		kernel	control
${𝒞}^{nv}$	No	N/A		8.77	10	43.71	334.23	N/A	N/A	N/A
${𝓒}_{𝒄}^{𝒗}$	Viab	coarse		8.57	0	0.904	7.968	17957	65	1769
${𝒞}_f^v$		fine		8.39	3	0.870	7.557	43238	157	4246
${𝒞}_c^d$	Disc	coarse		8.60	1	0.870	7.533	156776	65	1769
${𝒞}_f^d$		fine		8.41	6	1.032	6.518	397674	157	4246

Table 2. TABLE II: Influence of T p ​ p subscript 𝑇 𝑝 𝑝 T_{pp}

	mean lap time [s]	$\#$constr. violations	comp. time [ms]
$T_{pp}$			median	max
$0.12$	8.51	1	1.656	11.061
0.16	8.57	0	0.904	7.968
$0.2$	8.79	1	0.720	6.169
$0.24$	9.25	1	0.599	3.473

Table 3. TABLE III: Influence of N S subscript 𝑁 𝑆 N_{S}

	mean lap time [s]	$\#$constr. violations	comp. time [ms]
$N_S$			median	max
$2$	8.83	0	0.179	2.435
3	8.57	0	0.904	7.968
$4$	8.56	1	17.015	114.278

Table 4. TABLE IV: Experimental implementation of 𝒞 c v subscript superscript 𝒞 𝑣 𝑐 \mathcal{C}^{v}_{c} and 𝒞 c d subscript superscript 𝒞 𝑑 𝑐 \mathcal{C}^{d}_{c} .

	median lap time [s]	$\#$constr. violations	comp. time [ms]
Kernel			median	max
Viab	8.86	42	1.125	12.936
Disc	8.94	46	1.176	11.055

Table 5. TABLE V: Used gridding variables, for corresponding N m subscript 𝑁 𝑚 N_{m}

$\#$modes	$v_x$ range	$v_x$ spacing	$\delta$ grid points	drifting modes
89	$[0.5,3.5]$	0.25	5	12
99	$[0.6,3.4]$	0.2	5	12
115	$[0.5,3.5]$	0.25	7	12
129	$[0.6,3.4]$	0.2	7	12

Equations

$$\dot{X}$$

$$\dot{Y}$$

$$\dot{\phi }$$

$${\dot{v}}_x$$

$${\dot{v}}_y$$

$$\dot{\omega }$$

$$x_{k+1}\in F(x_k),\text{with }F(x)=\{f(x,u)|u\in U\}.$$

$$K^0=K,$$

$$K^{n+1}=\{x\in K^n|F(x)\cap K^n\ne \varnothing \}.$$

$$\forall x\in {\mathbb{R}}^n,\exists x_h\in X_h\text{s.t.}\Vert x-x_h{\Vert }_{\infty }\le \alpha (h),$$

$$x_{h,k+1}\in F_h^r(x_{h,k}).$$

$${\text{Viab}}_{F_h^r}(K_h)\subset ({\text{Viab}}_{F^r}(K)\cap X_h).$$

$$x_{k+1}\in G(x,v)\text{with }G(x,v)=\{g(x,u,v)|u\in U\}.$$

$$K^0=K,$$

$$K^{n+1}=\{x\in K^n|\forall v\in V,G(x,v)\cap K^n\ne \varnothing \}.$$

$$\forall x,y\in {\mathbb{R}}^n:\Vert f(y,u)-f(x,u){\Vert }_{\infty }\le L\Vert y-x{\Vert }_{\infty }.$$

$$f(x,u)\in f(x_h,u)+Lr{B}_{\infty }.$$

$$x_{k+1}=f(x_k,u_k)+v_k,\text{mbox}with{v}_k\in Lr{B}_{\infty }.$$

$$x_{k+1}\in G(x_k,v_k)=F(x_k)+v_k{v}_k\in Lr{B}_{\infty },$$

$$(F(x)+v)\cap {\text{Disc}}_G(K)\ne \varnothing \forall v\in V=Lr{B}_{\infty },$$

$$\forall x\in {\text{Disc}}_G(K),F(x)\cap {\text{Disc}}_G(K)\ne \varnothing .$$

$$f(\hat{x},u)\in f(x,u)+Lr{B}_{\infty },$$

$$x_{h,k+1}\in G_h^r(x_{h,k})=(F(x_{h,k})+v_h+r{B}_{\infty })\cap X_h,$$

$$\underset{{\mathbf{u}}_h,\mathbf{x}}{\min }\sum _{k=0}^{N_S}J(x_k,u_{h,k}),$$

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

$$x_{k+1}=f(x_k,u_{h,k}),u_{h,k}\in U_h,$$

$$(f(x_k,u_{h,k})+r{B}_{\infty })\cap X_h\subset {\text{Disc}}_{G_h^r}(K_h).$$

$$\displaystyle U_{\text{D}}(x_{h})=\begin{cases}\begin{Bmatrix}u_{h}\in U_{h}\,|\exists v_{h}\in V_{h}\\ f(x_{h},u_{h})+v_{h}+rB_{\infty}\\ \cap\textnormal{Disc}_{G_{h}^{r}}(K_{h})\neq\emptyset\end{Bmatrix}&\begin{array}[]{l}\text{if }x_{h}\in\\ \textnormal{Disc}_{G_{h}^{r}}(K_{h})\end{array}\\ \\ \quad\emptyset&\text{otherwise}\end{cases}.$$

$$\dot{X}$$

$$\dot{Y}$$

$$\dot{\phi }$$

$$\dot{T}$$

$$x_{k+1}=f(x_k,u_k)$$

$$F(x_c,q)=\{\begin{array}{ll}\tilde{F}(x_c,m),& \text{if }q\in (m-\frac{1}{2},m+\frac{1}{2})\\ \left\{\begin{array}{c}\tilde{F}(x_c,m),\\ \tilde{F}(x_c,m+1)\end{array}\right\},& \text{if }q=m+\frac{1}{2}\end{array},$$

$$\mathrm{Mod}(\phi ,2\pi )=\{\begin{array}{ll}\mathrm{mod}(\phi ,2\pi )& \text{if }\phi \ne k2\pi \\ [0,2\pi ]& \text{otherwise}\end{array},$$

$$\displaystyle K:=\left\{\begin{array}[]{l}(X,Y)\in\mathcal{X}_{\text{Track}}\,,\\ \varphi\in[0,2\pi]\,,\\ q\in\{1,...,N_{m}\}\,.\end{array}\right.$$

$$x_{h,k+1}\in {\tilde{G}}_h^r(x_{h,k},v_k),$$

Full text

I Introduction

Control design for autonomous driving has attracted considerable attention from the research community and has been successfully demonstrated on several occasions, e.g., [1, 2]. One form of autonomous driving is autonomous racing where the objective is to drive a car as fast as possible around a predefined track [3]. A popular control method for autonomous driving is receding horizon optimal control that has successfully been used for both autonomous driving in general, and for autonomous racing in particular [4, 5, 6, 7, 8]. In both cases the requirement for the car to stay on the track is typically encoded through state constraints. Without appropriate modifications, these constraints can lead to a loss of feasibility of the underlying optimization problem when the control algorithm is implemented in receding horizon, which may lead to accidents. In receding horizon control, recursive feasibility can be achieved by imposing terminal set constraints [9]. In autonomous driving, however, this issue is often neglected because terminal set constraints can be difficult to compute. In this work, we employ viability theory to derive recursively feasible controllers for autonomous racing.

Viability theory was developed to characterize states of a dynamical system for which solutions exist that satisfy its state constraints [10]. Though the use of viability theory in model predictive control has been considered [11], most applications of the theory limit themselves to establishing safe/viable regions in the state space [12, 13, 14, 15, 16] and reconstructing safe feedback controls [17]. At the heart of all these applications is the computation of the viability kernel, the largest subset of states for which the state constraints can be satisfied indefinitely. The viability kernel is typically approximated numerically using either the viability kernel algorithm introduced in [18], the discriminating kernel algorithm in the case of an additional disturbance input [19], or by exploiting the link to optimal control through viscosity solutions for Hamilton-Jacobi partial differential equations [20, 21]. All these numerical methods are based on gridding the state space and hence their computational complexity grows exponentially in the dimension of the state space. To reduce the computational load, the link to reachable set calculation has been exploited in [22], that allows for the development of efficient algorithms for linear systems [23, 24, 25, 26]. Similarly, if the dynamical system is polynomial and the constraints are semi-algebraic sets, gridding can be avoided by using methods based on linear matrix inequality (LMI) hierarchies [27].

In this paper we show how viability theory can be used to guarantee recursive feasibility of a path planner for miniature race cars. Our contributions are threefold: First, we extend the control scheme of [8] to efficiently generate viable trajectories during the path planning stage. Model Predictive Control (MPC) is then used to track the trajectory, generated in the path planning stage which maximizes the progress. We show that apart from guaranteeing safety, resorting to the viability kernel also reduces online computation time, enabling the use of longer prediction horizons. A related approach was proposed in [28], where the authors exhaustively generate trajectories and then discard the infeasible ones a posteriori. Viability theory was used in [14, 29] to construct safe trajectories and speed up trajectory planning processes, respectively. Similarly, viability and reachability analysis have also been used in autonomous driving to guarantee safety [30, 31, 16]. The main difference between those approaches and our method is that we use the viability kernel in a receding horizon fashion to generate viable trajectories only, which not only ensures recursive feasibility but also speeds up the computation.

The second contribution of this paper concerns the computation of the viability kernel. In particular, we propose a method which takes into account the discretization error due to the gridding in the viability kernel algorithm. More specifically, we propose to model the discretization error as an additive uncertainty, and then formulate the viability computation as a dynamic game between the uncertainty and the control input. The victory domain of this game is then computed using the discriminating kernel algorithm [19]. This stands in contrast to other algorithms for computing the viability kernel, which mainly establish inner or outer approximation thereof. For example, the effects due to the discretization are considered in [18, 19] by “extending” the set-valued dynamics, which results in an outer approximation of the viability kernel; an error bound for this approximation is given in [32]. A different approach was proposed in [33], where interval analysis is used to find inner and outer approximations of the viability kernel. Compared to those methods our game theoretical approach, which is an inner approximation of the viability kernel, does not only guarantee viability of the grid points, but also viability of points “close” to viable grid points. This property is of special interest in real applications, where the state of a system rarely coincides with a grid point.

As a third contribution of this paper, we examine the performance of the proposed controller in an extensive simulation study where we perform a detailed sensitivity analysis with respect to parameters of our viability-based controller. We demonstrate that the proposed controller scheme not only improves the overall performance of the race cars, but also dramatically decreases the online computation time compared to the original controller of [8]; the price to pay for this reduction in online computation is a significant increase in offline computations. Finally we verify the controller’s performance experimentally which shows that our viability-based controller is indeed suited for autonomous racing.

We remark that the current paper is an extended version of our previous publication [34], where we presented partial results of this paper’s first contribution.

This paper is organized as follows. Section II presents the hierarchical control structure of [8] for the miniature race cars, and Section III summarizes the viability and discriminating kernel algorithm of [18, 19]. In Section IV the new viability kernel approximation is introduced. Section V formulates the path planning step of the hierarchical controller as a discrete-time system. The viability and discriminating kernel based on this model are then analyzed in Section VI. In Sections VII and VIII we study the performance of the resulting controller both in simulation and in experiment. Conclusions and directions of future work are provided in Section IX. Appendices A and B contain technical results and proofs.

II Autonomous racing control hierarchy

We consider a miniature race car driving along a track with known boundaries (Fig. 1) and our autonomous racing controller is based on [8], where an optimization-based receding horizon control was proposed. The two level hierarchical structure of [8] is designed such that, in the higher level, many possible finite horizon trajectories are generated based on the current state, track layout and a simplified car model. In the lower level, the trajectory with the largest progress is tracked using a MPC subject to the full car dynamics. In the following section we review the proposed controller and outline how viability theory may improve the implementation of the higher level. The material of this section is based on [8] and [34].

II-A Vehicle model

The lower level control design is based on a nonlinear bicycle model (Fig. 2) using the Pacejka tire model [35]. This model captures important dynamics, such as the saturation of the nonlinear tire force, and the steering behavior at different forward velocities.

Following the literature, the equations of motion are derived around the center of gravity (CoG), and the states are the positions X and Y, and the orientation $\varphi$ relative to the inertial frame (Fig. 2). These three states characterize the kinematic part of the model. The second part of the model is derived in a body fixed frame centered at the CoG. The states are the longitudinal and lateral velocities $v_{x}$ and $v_{y}$ as well as the yaw rate $\omega$. The control inputs are the steering angle $\delta$ and the pulse width modulation duty cycle $d$ of the drive train motor. The complete equations of motion are

[TABLE]

where $m$ is the mass of the vehicle, $l_{r}$ and $l_{f}$ are the distances from the CoG to the rear and the front wheel, respectively, and $I_{z}$ is the moment of inertia. $F_{r,x}(v_{x},d)$ is the force produced by the drive train, $F_{r,y}(v_{x},v_{y},\omega)$ and $F_{f,y}(v_{x},v_{y},\omega,\delta)$ are the lateral forces at the rear and the front wheel, given by the Pacejka tire model [35]. For more details on the modeling as well as the exact tire model formulation we refer to [8].

II-B Path planning based on constant velocities

As the model (1) is complex and has a fast changing dynamic, the higher level of the hierarchical controller, which we call the “path planning controller”, uses a simplified model to determine an optimal trajectory. Unlike most of the path planning algorithms in the literature, e.g. [36], the goal of our path planning controller is not to reach a target while avoiding collision, but to find a finite horizon trajectory that stays within the track and which maximizes progress.

The bicycle model (1) is simplified by only considering trajectories that consist of segments of duration $T_{pp}$ with constant velocities. In vehicle dynamics such trajectories are known as steady state cornering, whereas in aeronautics they are referred to as trimmed flight. The segments are then directly linked under the assumption that the new velocity can be reached immediately. To ensure that the low-level controller can track trajectories with velocity discontinuities, constraints are imposed on the segments that can be concatenated. This approach is motivated by the time scale separation present in the system where a system’s velocity changes significantly faster than its position and orientation. A similar approach was adopted in [12, 37, 15] to reduce the number of states of the model. In our case, this approximation leads to a system with only four states, the three physical states $X$,$Y$ and $\varphi$ and a discrete state representing the constant velocity points, instead of the six states in equation (1).

We calculate constant velocity points by fixing the steering angle and the longitudinal velocity, and determine the remaining velocities and inputs $(v_{y},\omega,d)$ such that the accelerations of the model are zero. This corresponds to gridding the manifold describing the stationary velocities (Fig. 3) to generate a grid of $N_{m}$ constant velocity points, see Appendix B.

To ensure that the velocities in concatenated segments are not unrealistic, we only allow for transitions that are achievable by the nonlinear dynamical model ($v_{x}$, $v_{y}$ and $\omega$) in a certain small time step $T_{t}\approx T_{pp}/2$. This problem can be posed as a feasibility problem where the goal is to find control inputs that allow the transition from one velocity to another within $T_{t}$ seconds. This allows one to find all admissible transitions between two stationary points. Note that the resulting concatenation constraints can be encoded through a finite state automaton and that together with the continuous dynamics of (1) this implies that the path planning problem can be cast as a hybrid control problem[34][Section 3].

The path planner generates all trajectories that consist of $N_{s}$ constant velocity segments, each of duration $T_{pp}$, that satisfy the aforedescribed concatenation constraints. Once all the state trajectories have been computed the path planning algorithm discards those that violate the state constraints (i.e., leave the track). Among the remaining feasible trajectories, the path planner selects the trajectory with the greatest progress, calculated by projecting the end point of the trajectory on the center line of the track (Fig. 4). This is accomplished by first generating offline a piecewise affine approximation of the center line, comprising 488 pieces with end points distributed regularly on the center line. Online the projection can then be computed by finding the closest affine piece and taking an inner product.

A similar path planning concept based on constant velocity segments (trims) was derived in [36], and applied to autonomous driving in [38]. Our method is different since we do not plan a path from a starting point to a target set.

II-C Reference tracking MPC

The selected trajectory from the higher level path planner is passed over to a lower level MPC that tracks the given trajectory based on the full model (1). To render the MPC problem tractable, first the nonlinear model (1) is linearized around the reference state and input trajectory from the path planner and then discretized. Second, the track constraints are approximated by constraining each $X-Y$ state along the horizon to lie between two half-spaces approximating the track. These reformulations, together with a quadratic state reference tracking cost and a quadratic input rate cost, allow to formulate the problem as a convex quadratic program. The resulting optimization problem can be solved within milliseconds using tailored MPC solvers such as [39]. Since the focus of the paper is on the higher level path planner, we refer the interested reader to [8], where the MPC problem is discussed in more detail.

II-D Limitations and motivations of present work

While the hierarchical controller outlined above has enabled us to perform real-time racing of the RC cars, it faces two major drawbacks that limits its performance. The first drawback is the exponential growth of possible trajectories as a function of the number of segments, $N_{s}$. The second drawback is that feasibility does not imply recursive feasibility. Indeed it has been observed in [34], that the maximal progress trajectory is often not recursive feasible, when only feasibility is enforced.

To tackle these drawbacks we calculate the viability kernel of the path planner. By restricting the path planner to trajectories that keep the system within the viability kernel (instead of just the track), recursive feasibility of the path planning step is guaranteed under the assumption of no model mismatch. Moreover, the use of the viability kernel has two computational advantages that can be exploited to dramatically reduce the computation time. First, it allows one to consider fewer trajectories (only those that remain in the viability kernel instead of all trajectories that remain in the track) and second, it suffices to verify that at any switching point stays within the viability kernel instead of checking that the whole trajectory stays within the track. Note that using the viability kernel does not fundamentally tackle the exponential growth of the path planning computation in the horizon length. Empirically, however, this additional pruning of candidate trajectories does lead to a significant reduction in the online computation times. Preliminary results reported in [34] suggest that this reduction is by a factor of ten, making it possible to extend the paths considered by more segments, leading to a better closed-loop performance. In Section VII we show that a more efficient implementation allows a further reduction in computation time and allow for even more segments.

III Viability kernel and discriminating kernel

In this section we briefly discuss discrete-time viability theory which will later on be used to construct recursive feasible trajectories. Discrete-time viability theory addresses the question for which initial conditions does there exists a solution to a difference inclusion, which stays within a constraint set forever [10]. Consider a controlled discrete-time system $x_{k+1}=f(x_{k},u_{k})$, where $x\in\mathbb{R}^{n}$ is the state, $u\in U\subset\mathbb{R}^{m}$ is the control input and $f:\mathbb{R}^{n}\times U\rightarrow\mathbb{R}^{n}$ a continuous function. This system can be formulated as a difference inclusion,

[TABLE]

We next briefly summarize the standard viability kernel algorithm of [18] for the above class of systems.

III-A Viability kernel algorithm

Given a constraint set $K\subset\mathbb{R}^{n}$, solutions to the difference inclusion (2) which stay in $K$ forever are known as viable solutions.

Definition 1** ([18])**

A set $D\subset\mathbb{R}^{n}$ is a discrete viability domain of $F$ if $F(x)\cap D\neq\emptyset$ for all $x\in D$. The discrete viability kernel of a set $K\subset\mathbb{R}^{n}$ under $F$, denoted by $\textnormal{Viab}_{F}(K)$, is the largest closed discrete viability domain contained in $K$.

Under mild assumptions on $F$ (discussed below in Proposition 1) standard viability theory arguments ensure the existence of the discrete viability kernel and establish the existence of viable solutions for all the states contained in it.

Conceptually speaking, the viability kernel can be calculated through the so called viability kernel algorithm:

[TABLE]

Proposition 1** ([18])**

Let $F$ be an upper-semicontinuous set-valued map with closed values and let $K$ be a closed subset of $\textnormal{Dom}(F)$. Then, $\bigcap_{n=0}^{\infty}K^{n}=\textnormal{Viab}_{F}(K)$.

The viability kernel algorithm requires one to perform operations with arbitrary sets and as such is not implementable. In practice, viability kernels are approximated by discretizing/gridding the state space. Consider a family of countable subsets $X_{h}$ of $\mathbb{R}^{n}$, parameterized by $h\in\mathbb{R}$, such that

[TABLE]

for some continuous function $\alpha:\mathbb{R}_{+}\rightarrow\mathbb{R}_{+}$ with $\lim_{h\rightarrow 0}\alpha(h)=0$. Given a grid point $x_{h}\in X_{h}$, we will call the set of points $\{x\in\mathbb{R}^{n}\;|\;\|x-x_{h}\|_{\infty}\leq\alpha(h)\}$ the cell of $x_{h}$. Notice that the cells of different grid points may overlap. We note that, unlike [18], we resort here to the infinity norm, as this will facilitate the presentation of subsequent results.

To ensure that the discretized set-valued map $F(x_{h})\cap X_{h}$ is non-empty it is necessary to enlarge the set-valued map before discretization, a process that is known as “expansion” [18]. The expanded set-valued map is defined as $F^{r}(x)=F(x)+rB_{\infty}$, where $B_{\infty}$ is a closed unit ball in the infinity norm centered at the origin and $r$ is the radius of the ball. It can be shown that, for $r\geq\alpha(h)$, $F^{r}(x)\cap X_{h}$ is non empty for all $x$. Throughout we denote $F_{h}^{r}(x_{h})=F^{r}(x_{h})\cap X_{h}$ and assume for simplicity that $r=\alpha(h)$. When restricted to $X_{h}$ the set-valued map $F_{h}^{r}$ defines the following discrete time system over the countable set by

[TABLE]

If the set $K$ is compact, then the set $X_{h}$ can be assumed to be finite and the viability kernel algorithm (3) applied to (4) converges after a finite number of iterations, since $K_{h}=K\cap X_{h}$ is finite. Moreover, under appropriate technical assumptions [18], the resulting set inner-approximates the viability kernel, i.e.,

[TABLE]

Note that the guaranteed inner approximation is only with respect to the viability kernel of the extended dynamics $F^{r}$. The extension limits the granularity/resolution of features that the kernel can represent due to extension, but is necessary in the finite case. Finally, the finite viability kernel converges to the discrete viability kernel as the space discretization $h$ goes to zero, [18].

III-B Discriminating kernel algorithm

The discriminating kernel algorithm of[19] extends viability computations to systems where the state evolution $x_{k+1}=g(x_{k},u_{k},v_{k})$ is additionally affected by a disturbance $v_{k}\in V\subset\mathbb{R}^{d}$, where $g:\mathbb{R}^{n}\times U\times V\rightarrow\mathbb{R}^{n}$ is assumed to be a continuous function. The discriminating kernel algorithm returns all states (“victory domain”) where there exists a control input, which is able to prevent the disturbance from driving the system to the open set $\mathbb{R}^{n}\setminus K$. Notice that, in the discriminating kernel the control is able to access the disturbance’s current action, which is not the case if the related leadership kernel is used, for more details we refer to [40].

The difference equation can again be written as a difference inclusion

[TABLE]

Definition 2** ([19])**

A set $Q\subset\mathbb{R}^{n}$ is a discrete discriminating domain of $G$, if for all $x\in Q$, we have that $G(x,v)\cap Q\neq\emptyset$ for all $v\in V$. The discrete discriminating kernel of a set $K\subset\mathbb{R}^{n}$ under $G$, denoted by $\textnormal{Disc}_{G}(K)$, is the largest closed discrete discriminating domain contained in $K$.

One can show that the discriminating kernel $\textnormal{Disc}_{G}(K)$ exists if $K$ is a closed subset of $\mathbb{R}^{n}$ and $G:\mathbb{R}^{n}\times V\rightsquigarrow\mathbb{R}^{n}$ is an upper-semicontinuous set-valued map with compact values [19]. The discrete discriminating kernel can be calculated using an algorithm similar to the viability kernel algorithm, by considering a sequence of nested closed sets

[TABLE]

To numerically implement the discriminating kernel algorithm, the state space $\mathbb{R}^{n}$, the disturbance space $V$ and the difference inclusion $G(x,v)$ have to be discretized [19]. Whenever $K$ and $V$ are compact the algorithm (6) terminates after a finite number of iterations. Similar to the viability kernel it can be shown that the finite approximation of the discriminating kernel converges to the discrete discriminating kernel as the space discretization $h$ goes to zero [19].

IV Robustifying the viability kernel algorithm against space discretization errors

IV-A Errors due to the space discretization

We start by noting that the inner approximation property of the viability kernel algorithm (5) only holds for viable grid points and not for states in their cells. In practice it is desirable that the viability property of a grid point is passed on to its cell. In the following section we present a rigorous approach to ensure this property.

Assumption 1

The function $f(x,u)$ is continuous in $u$ and globally Lipschitz in $x$ with Lipschitz constant $L$, that is there exists a positive number $L$, such that for all $u\in U$,

[TABLE]

Consider $x_{h}\in X_{h}$, and a point $x\in x_{h}+rB_{\infty}$ in the cell of $x_{h}$, then under Assumption 1, it holds that for all $u\in U$

[TABLE]

Therefore, to robustify the viability kernel algorithm against the space discretization, we can model the discretization error as an additive disturbance on our nominal system, i.e.,

[TABLE]

IV-B Inner approximation of viability kernel

To calculate an inner approximation of the viability kernel that guarantees viability of the entire cell of a viable point, we propose to use the discriminating kernel algorithm (6).

Proposition 2

Under Assumption 1, the discriminating kernel $\textnormal{Disc}_{G}(K)$ of the difference inclusion,

[TABLE]

enjoys the following properties:

$\lim_{r\rightarrow 0}\textnormal{Disc}_{G}(K)=\textnormal{Viab}_{F}(K)$.

• 2)

$\textnormal{Disc}_{G}(K)$* is a viability domain of $F$.*

• 3)

For all $x\in\textnormal{Disc}_{G}(K)$ and for all $\hat{x}\in x+rB_{\infty}$, there exists a $u\in U$ such that $f(\hat{x},u)\in\textnormal{Disc}_{G}(K)$.

Proof:

Under Assumption 1 $F(x)$ is upper-semicontinuous, hence $G(x)$ is upper-semicontinuous, and the conditions to apply the viability and discriminating kernel algorithms are satisfied.

1. If $r\rightarrow 0$ the disturbance set vanishes, thus \bigcap_{r>0}LrB_{\infty}=\{0\}\ and using Remark 4.1 of [19], the discriminating kernel is identical to the viability kernel.

2. By definition the discriminating kernel is a discriminating domain. Thus for all $x\in\textnormal{Disc}_{G}(K)$ we have,

[TABLE]

Since $v=0\in LrB_{\infty}$ is an allowed disturbance, the discriminating kernel is also a viability domain, as

[TABLE]

3. First, by (7) we know that for all $\hat{x}\in x+rB_{\infty}$

[TABLE]

and therefore for a given $u\in U$, $f(\hat{x},u)=f(x,u)+v$, for some $v\in LrB_{\infty}$. Second, from [41][Equation (4)] we know that the discriminating kernel $\textnormal{Disc}_{G}(K)$ is the largest closest subset of $K$ such that for all $x\in\textnormal{Disc}_{G}(K)$ and for all $v\in V$, there exists a $u\in U$ such that $g(x,u,v)\in\textnormal{Disc}_{G}(K)$. Thus, together we have that for every $\hat{x}\in x+rB_{\infty}$ there exists a $u\in U$, such that $f(\hat{x},u)\in\textnormal{Disc}_{G}(K)$. ∎

Note that if the leadership kernel is used instead of the discriminating kernel, a similar property can be shown.

As discussed in the previous Section III, it is necessary to discretize space to implement the discriminating kernel algorithm. To state similar properties as in Proposition 2 if the space is discretized we need an inner approximation of the discriminating kernel $\textnormal{Disc}_{G^{r}_{h}}(K_{h})\subset\textnormal{Disc}_{G^{r}}(K)\cap X_{h}$. This is generally not given, therefore we derive a modified discriminating kernel algorithm, which guarantees an inner approximation for the given system (9). The modified discriminating kernel algorithm is derived in Appendix A and stated in Algorithm 1. The algorithm, however needs two assumptions; $(i)$ we only consider square grids and $(ii)$ the set $U$ needs to be finite. The second assumption is necessary to implement the algorithm and can be achieved by introducing a finite discretization of $U$, which we henceforth denote by $U_{h}\subset U$. Note that for our system square grids are an obvious simple choice and the path planning model has a finite input space by construction.

Proposition 3

Consider the finite dynamical system corresponding to the extended and discretized system of (9),

[TABLE]

where $v_{h}\in V_{h}$ is the discretization of $LrB_{\infty}$. If $\textnormal{Disc}_{G^{r}_{h}}(K_{h})$ is computed with Algorithm 1, then the following properties hold:

$\textnormal{Disc}_{G^{r}_{h}}(K_{h})$* is a viability domain of $F^{r}_{h}$.*

• 2)

For all $x_{h}\in\textnormal{Disc}_{{G}^{r}_{h}}(K_{h})$ and for all $\hat{x}\in x_{h}+rB_{\infty}$, there exists $u_{h}\in U_{h}$ such that $(f(\hat{x},u_{h})+rB_{\infty})\cap X_{h}\in\textnormal{Disc}_{{G}^{r}_{h}}(K_{h})$.

Proof:

See Appendix A. ∎

IV-C Reconstructing viable controls

The use of the discriminating kernel in the path planning step of the hierarchical racing controller is not directly possible as the action depends on both the state and the disturbance. Thus, the reconstruction of a viable set-valued feedback policy as proposed in [34] is not possible. However, as the disturbance is related to the position of the current state within the grid cell the disturbance is indirectly known. Furthermore, we know that if the state is within a grid cell of the discriminating kernel, there exists a control which keeps the system within a grid cell of the discriminating kernel, see Proposition 3. Thus, we can formulate a predictive controller which enforces this property. More precisely, we can consider the following optimization problem which constrains the state to stay within the discriminating kernel. Feasibility of the problem below is ensured by virtue of Proposition 3; the objective function $J(x,u)$ allows us to optimize some cost function. In our autonomous racing application $J(\cdot,\cdot)$ is chosen such that the car’s progress, measured as described in Section II, is maximized, and the system dynamics $x_{k+1}=f(x_{k},u_{h,k})$ is the path planning model described in the next section,

[TABLE]

Where, $\mathbf{u}_{h}=[u_{h,0},...,u_{h,N_{S}-1}]$ is the control sequence, $\mathbf{x}=[x_{0},...,x_{N_{S}}]$ the corresponding state sequence, $x$ the measured state and $u_{h,k}\in U_{h}$ the input constraints. We point out that the problem is solved by enumeration, which limits the horizon to few steps. To reduce the computation time related to generate all feasible input and state sequences, it is possible to replace $u_{h,k}\in U_{h}$ with $u_{h,k}\in U_{D}(x_{k}+rB_{\infty}\cap X_{h})$, where,

[TABLE]

If instead the viability kernel is used, the last constraint of (10) is replaced with $(f(x_{k},u_{h,k})+rB_{\infty})\cap X_{h}\subset\textnormal{Viab}_{F_{h}^{r}}(K_{h})$. And to reduce the computation time $u_{h,k}\in U_{h}$ can be replaced with $u_{h,k}\in U_{V}(x_{k}+rB_{\infty}\cap X_{h})$, where the set-valued feedback law $u_{k,h}\in U_{\text{V}}(x_{h})$ can be computed similar to $U_{\text{D}}$ in (11). However, there exists no guarantee that the control will keep the system within the viability kernel if the state does not always coincide with a grid point.

In the following two sections, we discuss $x_{k+1}=f(x_{k},u_{k})$ as well as $\textnormal{Disc}_{G}(K)$ and $\textnormal{Viab}_{F}(K)$, which are needed to formulate the high level path planner in terms of (10).

V Path planning model

As discussed in Section II, the path planning problem can be formalized in terms of hybrid systems, where the discrete mode $q$ of the hybrid system represents the current stationary velocity, while the continuous evolution is given by the kinematic model with the corresponding stationary velocity. A clock variable is added which allows a jump to a different discrete mode every $T_{pp}$ seconds. Thus the continuous evolution is given by,

[TABLE]

where $v_{x}(q),v_{y}(q),\omega(q)$ are the stationary velocities at the discrete mode $q$. The discrete transition takes place every $T_{pp}$ seconds, and a discrete control input respecting the concatenation constraints encoded by the finite automaton determines the discrete mode after the transition. As shown in [34], the model can be formally cast in the framework of hybrid automata [42].

Since the system is hybrid one would, strictly speaking, have to employ hybrid viability algorithms [43, 44]. This would, however, require gridding the four dimensional continuous space for each of the $N_{m}$ discrete modes, a task that is computationally very demanding. As the discrete transitions occur at fixed time intervals, the system can be interpreted as a sampled data system, with constant inputs over the time interval. Therefore, the system can be approximated by a discrete time system with a sampling time of $T_{pp}$. The discrete state $q$ of the hybrid system is considered by embedding it in the real numbers, or in other words “pretending” that $q\in\mathbb{R}$; hence the discrete dynamics can be encoded as a transition relation that depends on the current mode. This renders the time state redundant and reduces the state space dimension by one. Thus, it is only required to grid a 3 dimensional space, in addition to the coarse grid corresponding to the discrete state.

We denote the state space of the difference equation at time step $k$ by $x_{k}=(X_{k},Y_{k},\varphi_{k},q_{k})$, and the control by $u_{k}$, which determines the next mode $q_{k+1}=u_{k}$. As the control input depends on the current mode $q_{k}$, we abstractly write $u_{k}\in U(x_{k})$, where $U(x_{k})$ represents all allowed transitions in the automaton encoding the concatenation constraints, or in other words all allowed next modes $q_{k+1}$, as detailed in [34]. Let $X(x_{k},u_{k},t),\;Y(x_{k},u_{k},t),\;\varphi(x_{k},u_{k},t)$ denote the solution of (12a)-(12c) at time $t$, with $q=u_{k}$ while starting at $x_{k}=(X_{k},Y_{k},\varphi_{k},q_{k})$. Note that, since the velocities are constant the solution can be explicitly computed as a function of time. Thus, we can define the path planner as the following discrete-time model,

[TABLE]

where $T_{pp}$ is the path planning discretization time. The set-valued map needed for the viability kernel algorithm is given by $F(x)=\{f(x,u)|u\in U(x)\}$, and the output is the union of a finite number of points in the state space.

For the viability and discriminating kernel algorithm it is required that $F(x)$ is an upper-semicontinuous set-valued map. In the following, we show that by appropriately embedding the discrete state in the set of real numbers, upper-semicontinuity of the set-valued map of (13) can be ensured despite the hybrid structure of the model. To see this, recall that a set-valued map $F:\mathbb{R}^{n}\rightsquigarrow\mathbb{R}^{n}$ is called upper-semicontinuous if and only if for all $x\in\mathbb{R}^{n}$ and for all $\epsilon>0$ there exists an $\eta>0$ such that for all $x^{\prime}\in x+\eta B$ it holds that $F(x^{\prime})\subset F(x)+\epsilon B$ [45]. By embedding the discrete state $q$ in the real numbers we have $x=(X,Y,\varphi,q)\in\mathbb{R}^{4}$, where the first three states $(X,Y,\varphi)$ are continuous by virtue of the original model. Collecting them into the continuous state $x_{c}=(X,Y,\varphi)\in\mathbb{R}^{3}$ and defining the integer lattice $\mathbb{M}=\{1,...,N_{m}\}$, where $N_{m}$ is the number of modes, we can define the following upper-semicontinuous set-valued map,

[TABLE]

where $\tilde{F}(x_{c},m)=\{f([x_{c},m],u)|u\in U(m)\}$ is the set-valued map defined at the lattice point $m$. Notice that $q\in(1-1/2,N_{m}+1/2)$ is w.l.o.g. since the automaton maps the interval to itself, forming an invariant set in $\mathbb{R}$. Finally, upper-semicontinuity is ensured because the dynamics of $x_{c}$ is continuous and, using the above proposed formulation there, are no discontinuities in a set-valued sense.

To reduce the number of grid points in a practical implementation of the algorithm it is beneficial to limit the angle between [math] and $2\pi$. Instead of using $\mathrm{mod}(\varphi,2\pi)$, which would lead to a discontinuous set-valued map, we propose the set-valued $\mathrm{mod}$ function

[TABLE]

which is upper-semicontinuous.

Finally, with the model (13) it is still possible that the trajectory leaves the constraint set $K$ and re-enters during the time interval $T_{pp}$. This is an issue more generally for sampled data systems and was tackled in [46] by a sampled data system viability/discriminating kernel algorithm that discards control inputs and states for which the continuous evolution of the data sampled system leaves the constraint set $K$.

In the following section we will use the described model, including the modifications and the modified sampled data system viability/discriminating kernel algorithm to compute the kernel with respect to the track constraints.

VI Viability kernel for the track

VI-A Track constraints

The goal is to find a viable solution of the path planning model within the track (Fig. 1). To that end, we can define the constraint set $K$ as,

[TABLE]

In other words, $X,Y$ are constrained within the track, while $\varphi$ and $q$ are unconstrained. The state space is uniformly gridded such that the distance in the respective unit (meter or radian) between two grid points is identical. This leads to the smallest $r$ if the states are not normalized. Further, as $q$ is already finite no gridding is necessary.

The Lipschitz constant of the path planning model (13) can be calculated analytically by differentiating the explicit formula for $f(x_{k},u_{k})$. By using different Lipschitz constants for each mode it is possible to reduce conservatism of the proposed discriminating kernel approximation.

VI-B Viability vs. discriminating kernel

Proposition 2 statement 1) in Section IV suggests that the proposed discriminating kernel approximation converges to the viability kernel as the grid spacing $r$ goes to zero. To verify this, we compare the fraction of grid points in $K$ which fall into the respective kernels. Thus, if all grid points in $K$ are viable, then the fraction would be one. Fig. 5 shows the fraction for the viability kernel and the discriminating kernel approximation for different numbers of grid points. It is interesting to see that the proportion of the viability kernel stays approximately at 0.35, whereas the discriminating kernel approaches the viability kernel as the grid spacing gets smaller. We can make two observations from Fig. 5, first, as Proposition 2 suggests the two approximations of the viability kernel get closer the smaller the grid spacing is. Second, while the viability kernel only changes slightly as a function of the grid spacing, the proposed discriminating kernel approximation requires a relative fine grid to achieve an approximation that is similar to the one of the viability kernel algorithm. Intuitively speaking, this is due to the fact that a coarse grid introduces a larger uncertainty than a fine grid, see also (8).

Fig. 6, visualizes the two kernels by comparing slices of the four dimensional kernels, this is achieved by fixing the angle $\varphi$ and the mode $q$, resulting in just the $X-Y$ plane which is easy to visualize. Fig. 6, allows to see a qualitative difference between the viability and discriminating kernel for $r=0.02$ and $r=0.015$ (corresponding to 65’528’130 and 157’284’540 grid points respectively). Note that $r=0.02$ implies that the grid points are 4 cm apart; as a comparison the track is 37 cm wide, and the size of the cars is about $10\times 5$ cm.

Fig. 6 shows that the difference between the two kernels also qualitatively becomes smaller if we use a finer grid. More precisely Fig. 6 shows all the viable points if the car is driving straight left with a forward velocity of 2 m/s. For the illustrated angle and constant velocity, mainly the top straight and the bottom S-curve are of interest (depending on the driving direction). In this region the difference between the kernels and the effect of grid size is visible, for example, in the bottom S-curve, where the red region (which indicates that the grid point is in both kernels) gets significantly larger for the finer grid.

VII Simulation results and sensitivity analysis

We start with a simulation study to investigate the effect of the various design choices. We use the full controller introduced in Section II, which includes the viability-based path planner (10) and the MPC tracking controller. The controls are applied to the continuous time bicycle model (1), including control quantization motivated by the communication link in the real set-up, where both control inputs are represented using 8 bit each.

The effect of the following tuning parameters in the path planner are examined:

• •

use of viability kernel, discriminating kernel, and no viability constraints;

• •

grid size in $(X,Y,\varphi)$;

• •

number of modes $N_{m}$;

• •

path planning discretization time $T_{pp}$;

• •

number of constant velocity segments $N_{S}$.

To measure a controller’s performance, we consider the mean lap time, the number of constraint violations (i.e., the number of time steps the car is outside the track constraints) and the computation time required by the path planner. Each controller is simulated for 10’000 time steps; with a sampling time of 20 ms, this corresponds to 200 s or roughly 23 laps. The simulations are performed on a MacBook Pro with a 2.4 GHz Intel Core i7 processor, implemented in $\mathtt{C}$, using $\mathtt{gcc}$ as a compiler and $\mathtt{-O3}$ code optimization. Furthermore, we investigate the offline computation time and required memory for certain combinations, where the viability and discriminating kernel are computed in JULIA, on a computer running Debian equipped with 16 GB of RAM and a 3.6 GHz Intel Xeno quad-core processor.

VII-A Viability constraints and grid size

We first compare our proposed controller that uses a path planner with viability constraints ($\mathcal{C}^{v}_{f}$, $\mathcal{C}^{d}_{f}$, $\mathcal{C}^{v}_{c}$ and $\mathcal{C}^{d}_{c}$), to a naive controller without viability constraints ($\mathcal{C}^{nv}$). Specially, we consider both the viability kernel and the discriminating kernel (superscript $v$ and $d$), and two different grid spacing ($r=0.02$ and $r=0.015$) referred to as coarse and fine (subscript $c$ and $f$). All the controllers use the parameters $N_{m}=129$, $T_{pp}=0.16$ s and $N_{S}=3$ in the path planner. $\mathcal{C}^{nv}$ is based on the implementation proposed in [8] with the horizon increased from one to three and an improvement in the way constant velocities segments are concatenated in the path planner.

Table I compares all five controllers in terms of driving performance (mean lap time and constraint violations) as well as computation cost. We see that the number of constraint violation is negligible for all controllers as no controller triggers more than 10 violations (out of 10’000 steps). Focusing now on the mean lap time we can distinguish three groups: First, we see $\mathcal{C}^{nv}$ that has the highest mean lap time. Second are $\mathcal{C}^{v}_{f}$ and $\mathcal{C}^{d}_{f}$, which have very similar mean lap times, and are clearly the two fastest algorithms. Third, we have $\mathcal{C}^{v}_{c}$ and $\mathcal{C}^{d}_{c}$ whose mean lap times lie between the first two groups.

If we compare the online computation times of the controllers, then we can mainly differentiate between the controller without viability constraints ($\mathcal{C}^{nv}$) and controllers with viability constraints ($\mathcal{C}^{v}_{f}$, $\mathcal{C}^{d}_{f}$, $\mathcal{C}^{v}_{c}$ and $\mathcal{C}^{d}_{c}$) which are on average more than 40 times faster. As $\mathcal{C}^{nv}$ does not use any viability constraints to guide the path planning process, more branches of the tree have to be generated and checked, slowing down the computation, see Section II-D. Furthermore, we see from Table I that the maximal computation time of $\mathcal{C}^{nv}$ is 334 ms, which by far exceeds the sampling time of 20 ms.

When comparing $\mathcal{C}^{nv}$ with the remaining controllers using the viability constraints, it is also important to investigate the offline computation time and the memory the viability lookup tables in (10) require. First, for the controllers with viability constraints, we need to compute the corresponding kernels. Their computation time mainly depends on two factors: The choice between viability or discriminating kernel, and the used grid size. The number of grid points has a direct influence on the offline computation time, as the number of operations per grid point stays the same. The observation when comparing the chosen kernel algorithm is that the discriminating kernel algorithm is significantly slower. This has two reasons: first each iteration needs more operations than the viability kernel algorithm due to the disturbance (6); second the discriminating kernel algorithm needs more iterations to converge. For an implementation the required memory of the viability based path planner (10) is also important, as (10) requires to store a four dimensional lookup table for the kernel and a five dimensional lockup table for the viable inputs. The memory needed to store these lookup tables only depends on the grid size, and for the fine grid is 4.4 GB, and 1.8 GB for the coarse grid. Thus, we can conclude that, if the memory requirement and offline computation time are of no concern, then $\mathcal{C}^{v}_{f}$ and $\mathcal{C}^{d}_{f}$ should be preferred as they are the best performing controllers. However, if the available memory is limited, $\mathcal{C}^{v}_{c}$ and $\mathcal{C}^{d}_{c}$ with a coarse grid achieve good performance at a significantly lower memory requirement.

For the following discussions we will compare the other tuning parameters ($N_{m}$, $T_{pp}$ and $N_{S}$), of the path planner, while using $\mathcal{C}^{v}_{c}$ as the base comparison, marked as bold text in the tables.

VII-B Number of modes, $N_{m}$

We examine the influence of $N_{m}$ by varying the number of modes, which depends on the grid imposed on the stationary velocity manifold, see Appendix B for the different grids used. As expected, the lap time get slower the fewer modes are considered (mean lap time with $N_{m}=89$ is 9.69 s), but at the same time the computation time decrease by up to a factor of two. In all cases, the number of constraint violations is zero.

VII-C Discretization time, $T_{pp}$

The effect of time discretization was investigating by testing different values of $T_{pp}\in\{0.12,0.16,0.2,0.24\}$ s. The results are summarized in Table II, where we can see that larger $T_{pp}$ lead to lower computation times but the lap times getting slower. This effects come from the influence of $T_{pp}$ on the path planner model. As for the same mode (same constant velocity), a larger $T_{pp}$ implies that the car travels further. This may render this mode non-viable as the additional traveled distance may bring the car too close to the border. In contrast slower modes more likely stay inside the track. Thus, the path planner more likely chooses slow trajectories for larger $T_{pp}$, which leads to slower lap times. At the same time fewer trajectories are viable, which reduces the computation time. Thus, even though, a larger $T_{pp}$ implies a longer preview, the larger number of non-viable states mitigate this advantage.

VII-D Number of constant velocity segments, $N_{S}$

The influence of the number of constant velocity segments, or in other words the prediction horizon of the path planner is investigated by varying $N_{S}$ from two to four. In Table III we can see that more segments improve the performance but at the same time the computation time is heavily increased. The improved lap times came from the longer preview of the controller with larger $N_{S}$. The longer preview is achieved without changing the viability of a trajectory, which stands in contrast with larger $T_{pp}$. Furthermore, since both $T_{pp}$ and $N_{S}$ influence the preview window, we have observed that $N_{S}$ and $T_{pp}$ should be tuned simultaneously, e.g., for $N_{S}=2$, $T_{pp}=0.2$ s achieves a faster mean lap time than $T_{pp}=0.16$ s.

VII-E Concluding remarks

The above discussion is summarized in Fig. 7, which allows us to draw the following conclusions: First, all the tested controllers based on kernel pruning outperform $\mathcal{C}^{nv}$, the state of the art controller based on [8], which for the horizon length considered does not even meet the real-time requirements. In contrast, the controllers with viability constraints and $N_{S}\leq 3$ are real-time feasible; put another way, the viability constraints allow one to use longer horizons in real-time than would be possible with the state of the art controller. Second, when comparing the variations of $\mathcal{C}^{v}_{c}$ with different values of $N_{m}$, $T_{pp}$ and $N_{S}$, we have observed that more modes $N_{m}$, shorter $T_{pp}$ and larger $N_{S}$ are beneficial for the performance. Furthermore, the numerical study indicates that, if a less powerful computer is used and the computation time has to be reduced, the most effective way to do so is to reduce $N_{S}$ to 2, see Fig. 7. Also note that $N_{S}=4$ leads to no further improvement in the lap time, which indicates that $N_{S}=3$ is a sufficiently long prediction horizon. This is also confirmed by using a terminal cost which captures the possible long term progress of a state, which improves the performance for $N_{S}=2$, but does not help to reduce the lap time if $N_{S}\geq 3$.

Finally, we see from Table I that the viability-based controller is marginally faster than the discriminating-based controller. The latter, however, uses the proposed discriminating kernel approximation, which takes into account the discretization of the state space. Thus, the controller (10) comes with additional viability guarantees.

VII-F Closed loop behavior of $\mathcal{C}^{v}_{c}$ and $\mathcal{C}^{d}_{c}$

Since the lap time of $\mathcal{C}^{v}_{c}$ and $\mathcal{C}^{d}_{c}$ are similar, it is interesting to investigate the driving behavior of the two controllers. Fig. 8 shows one typical lap driven with $\mathcal{C}^{v}_{c}$ and $\mathcal{C}^{d}_{c}$. The beginning of the lap is in the top left corner marked with a line perpendicular to the track, and the cars race counter clockwise. The velocity is shown relative to center line, to make it possible to compare the velocity at a given point on the track; the projection on the center line is done as discussed in Section II. We see that most of the track, the two controllers drive in a similar way and even have the same velocity. However, $\mathcal{C}^{d}_{c}$ drives somewhat slower coming into the curves, allowing higher velocity on the curve itself and higher exit velocity, the most extreme cases are marked with a green bars in Fig 8. It is interesting to see that, these two different driving styles lead to practically the same lap time.

VII-G Obstacle avoidance

To further highlight the difference between the two controllers $\mathcal{C}^{v}_{c}$ and $\mathcal{C}^{d}_{c}$ we included several obstacles at challenging position. The obstacle constraints can be included by modifying the constraint set $K$ and recomputing the viability and discriminating kernel. We tested two obstacle configurations shown in Fig. 9. For the first configuration (right plot in Fig. 9) $\mathcal{C}^{v}_{c}$ successfully avoids the obstacles, but the discriminating kernel collapses to the empty set. The collapse is due to the conservatism added by considering the space discretization and the fact that the model cannot stop. The second obstacle configuration is easier to navigate and both controllers are able to find paths around the obstacles. However, $\mathcal{C}^{v}_{c}$ is significantly faster than $\mathcal{C}^{d}_{c}$ with a mean lap time of 9.066 compared to 9.272 s.

VIII Experimental results

To verify the simulation results, we implemented $\mathcal{C}^{v}_{c}$ and $\mathcal{C}^{d}_{c}$ in an experimental set-up. The set-up consists of 1:43 Kyosho dnano RC cars, which are driven autonomously around the race track shown in Fig. 1. The control signals are sent to the cars by an external computer via Bluetooth and an infrared-based vision system captures the cars’ current position, orientation and velocity. We refer the interested reader to [8] for a more detailed description of the hardware set-up. The controllers were implemented on a desktop computer running Ubuntu 14.04 OS, equipped with 4 GB of RAM and a 3.5 GHz Intel i7 quad-core processor. The tracking MPC problem is solved using FORCES Pro [47].

For the experiments we implemented the two controllers $\mathcal{C}^{v}_{c}$ and $\mathcal{C}^{d}_{c}$ due to their good performance, and because they are feasible in terms of memory (compared to $\mathcal{C}^{v}_{f}$ and $\mathcal{C}^{d}_{f}$). We ran the experiment for 200 s and extracted all completed laps, shown in Fig. 10, together with the velocity profile of one lap. We see from Table IV that constraint violations occur more frequently than in simulation. This is mainly due to a significantly larger model mismatch when using the real car compared to the bicycle model used in simulation. The increased model mismatch can also been seen by the increased spread of the trajectories between the simulation and the experimental results, compare Fig. 8 and 10.

One way to deal with the model mismatch is to tighten the track constraints of the controller to illustrate this we impose track constraints that are 1.5 cm away from the track boundary in the viability computations, and 0.5 cm in the MPC; for comparison the miniature cars are $10\times 5$ cm in size. This provides the controller a certain margin of errors but of course comes at the cost of a potential increase in lap time. Constraint violation, are counted when the car is closer than 0.5 cm to the track boundary. The model mismatch can also lead to infeasibilities in the controller. This is, for example the case for the path planner when the current state is not inside the viability kernel. In such a case we resolve infeasibilities in the path planning process using the following heuristics: First we find a viable neighboring cell and solve the path planning problem pretending to be in the closest viable neighboring grid cell. If none of the neighboring grid cells is viable, then an emergency stop is initiated, and the controller is restarted if the car is back in a viable state. Infeasibilities in the lower level MPC problem are dealt with by using soft constraints [48].

We see from Fig. 10 that $\mathcal{C}^{d}_{c}$ drives more conservatively, a feature we have also seen in the simulation studies before. By breaking earlier, $\mathcal{C}^{d}_{c}$ is able to achieve higher velocities on the curves itself and higher exit velocities. In contrast to the simulation, however, the difference in the mean lap times between $\mathcal{C}^{d}_{c}$ and $\mathcal{C}^{v}_{c}$ is larger, see Table IV, though the best and the worst laps are close with lap times in the range 8.66 to 9.54 s for $\mathcal{C}^{v}_{c}$ and 8.64 to 9.44 s for $\mathcal{C}^{d}_{c}$. Also the number of constraint violations, is nearly identical. For a video comparison of the two controllers, see https://youtu.be/RlZdMojOni0.

IX Conclusion

In this work we showed that the existing hierarchical controller of [8] can be improved by incorporating the viability kernel in the path planning phase. As a result, the path planner only generates viable trajectories that are recursively feasible, while reducing the computation time. This in turn allows the use of longer prediction horizons, which generally leads to better performance in terms of lap times and constraint violations.

To compensate for discretization errors in the computation of the viability kernel, we formulated the viability computation problem as a game between the uncertain initial condition introduced by the state discretization and the control input. The resulting kernel was calculated using the discriminating kernel algorithm, which allows us to derive an inner approximation of the normal viability kernel. Furthermore, the new kernel guarantees that there exists a control input that keeps the system within the kernel even if the current state is not on a grid point but somewhere within the cell around a grid point.

In a numerical study we investigated the influence of different parameters on the performance of the controller using the viability conststraints in the path planning phase of our controller, and compared the performance if the standard viability kernel is used compared with our inner approximation. Although the closed loop behavior of the controller using the standard viability kernel and the proposed discriminating kernel approximation are different, the performance in terms of lap time of the two controllers is very similar. The controller based on the proposed discriminating kernel approximation seems to drive with more foresight and is less aggressive. The same behavior was also observed in our experimental implementation.

This work has mainly focused on the path planning step in the hierarchical controller, whereas the lower level MPC is kept simple. In future work we will investigate how the lower level can be modified to improve the driving performance. For example by considering the model uncertainty in the MPC design, as proposed in [49]. Furthermore, we are investigating the use of the path planning model and the viability constraints in a racing game with multiple opposing cars.

Acknowledgment

The authors would like to thank Xiaojing Zhang for the helpful discussions and his advice.

Appendix A Finite inner approximation

Recall from (5) that the finite viability kernel is an inner approximation of the discrete viability kernel with respect to an extended set-valued map. For the discriminating kernel the authors are not aware of similar results. Even though the basic algorithms (3) and (6) are similar, the discretization of the disturbance space $V_{h}\subset V$ required in the discriminating kernel algorithm “weakens” the uncertainty. This may lead to points in the finite discrete discriminating kernel $\textnormal{Disc}_{G^{r}_{h}}(K_{h})$ which are outside the discrete discriminating kernel $\textnormal{Disc}_{G^{r}}(K)\cap X_{h}$.

In the following we present a method for addressing this problem for our dynamics (9). This is achieved by first showing that by only discretizing space, but not the disturbance space, the “semi-finite” discriminating kernel is an inner approximation of $\textnormal{Disc}_{G^{r}}(K)\cap X_{h}$. And second by showing that for every discrete disturbance $v_{h}\in V_{h}$ there exists a set of disturbances $\tilde{V}\subset V$ which does not change the finite dynamical system $x_{h,k+1}\in G(x_{h,k},v_{h,k})$. This inside then allows to formulate a modified discriminating kernel algorithm, which guarantees that $\textnormal{Disc}_{G^{r}_{h}}(K_{h})\subset\textnormal{Disc}_{G^{r}}(K)\cap X_{h}$.

A-A Continuous Disturbance Space

Let us first look into the “semi-finite” discriminating kernel where the state space is discretized but the disturbance input is continuous. To this end let us introduce a new set-valued map $\tilde{G}^{r}_{h}:X_{h}\times V\rightsquigarrow X_{h}$, $\tilde{G}^{r}_{h}(x_{h},v):=G^{r}(x_{h},v)\cap X_{h}$, which leads to the following finite dynamical system,

[TABLE]

where $v_{k}\in V$. The following result holds for the new “semi-finite” discriminating kernel $\textnormal{Disc}_{\tilde{G}_{h}^{r}}(K_{h})$

Proposition 4

*Let $G:\mathbb{R}^{n}\times V\rightsquigarrow\mathbb{R}^{n}$ be an upper-semicontinuous set-valued map with compact values, $K$ be a closed subset of $\textnormal{Dom}(G)$, and $V$ a compact set. Let $r$ be such that, $\forall v\in V\,\forall x\in\textnormal{Dom}(G^{r})\cap X_{h}\,,\;G^{r}(x,v)\cap X_{h}\neq\emptyset$. Then, $\textnormal{Disc}_{\tilde{G}_{h}^{r}}(K_{h})\subset\textnormal{Disc}_{G^{r}}(K)\cap X_{h}$. *

Proof:

The proof is an extension of the proof of Proposition 4.1 in [18] to the case of the discriminating kernel. We have that for any $v\in V$, $\tilde{G}^{r}_{h}(x_{h},v)\subset G^{r}(x_{h},v)$ and additionally $K_{h}\subset K$. By the definition of the discriminating kernel, we know that for all $x_{h}\in\textnormal{Disc}_{\tilde{G}_{h}^{r}}(K_{h})$ there exists a solution to the finite dynamical system (14) starting from this grid point, which forever stays within $K_{h}$, for any sequence of disturbance inputs $v_{k}$. As $\tilde{G}^{r}_{h}(x_{h},v)\subset G^{r}(x_{h},v)$ and $K_{h}\subset K$, there also exists a solution to $x_{k+1}\in G^{r}(x_{k},v_{k})$, starting at the same grid point which stays in $K$, for any disturbance input sequence. One trivial solution to $x_{k+1}\in G^{r}(x_{k},v_{k})$, which fulfills the property is same as the one of the finite dynamical system. Therefore, if $x_{h}\in\textnormal{Disc}_{\tilde{G}_{h}^{r}}(K_{h})$ the grid point is also an element of $\textnormal{Disc}_{{G}^{r}}(K)$, which concludes the proof. ∎

Proposition 4 would allow us to state the discrete space counterpart of Proposition 2. However, $\textnormal{Disc}_{\tilde{G}_{h}^{r}}(K_{h})$ can not be computed, thus we proceed by establishing a link between $\tilde{G}^{r}_{h}(x_{h},v)$ and ${G}^{r}_{h}(x_{h},v_{h})$, which allows to use Proposition 4 in the “fully finite” case.

A-B Discrete Disturbance Space

By discretizing the disturbance space $V_{h}\subset V$, the possible actions of the disturbance input are reduced, which can result in an wrong classification of states to lay within the discriminating kernel. More precisely we can only guarantee that there exists a trajectory to the difference inclusion which stays in $K_{h}$ for all sequences $v_{h,k}$. However, this does not necessary hold for all continuous disturbance sequences $v_{k}$, as illustrated in Fig. 11. Even under restrictive assumptions on the used grids, e.g., square grids, and fine disturbance grid this problem is not solved.

However, it is possible to establish a link between the continuous and discrete disturbance, for the dynamical system used in Proposition 2, $x_{k+1}\in F(x_{k})+v_{k}$ with $v_{k}\in V=LrB_{\infty}$, where the uncertainty enters additive and the disturbance space is a box. We begin with stating the assumptions on the state and disturbance grid.

Assumption 2

**

• •

$X_{h}\subset X$* is a square grid with a spacing of $2r$ in the infinity norm.*

• •

$V_{h}\subseteq V$* is a square grid, where any point $v\in V\subset\mathbb{R}^{d}$ has $2^{d}$ neighboring points in $V_{h}$ which are closer than $2r$ in the infinity norm.*

The assumption on the state grid is necessary to exclude special cases that can arise with non square grids. The assumption on the disturbance grid generates a grid where $V=LrB_{\infty}$ is gridded with $\lceil L\rceil+1$ grid points in each direction, including the corner points and grid points at the boundary of the set. Additionally the disturbance grid spacing is smaller than the state grid spacing. Therefore, it is impossible that two neighboring disturbance grid points can move $F(x_{h})$ further than to the next grid cell.

In addition to Assumption 2 we need an assumption on the set of allowed controls. As the proposed method will check every element of $F(x)$, it is necessary that $F(x)$ is a finite set, which is the case if the set of admissible controls is finite. To achieve this, the set of control inputs $U$ is discretized, leading to the following finite subset $U_{h}\subseteq U$. Notice that $U_{h}$ does not need any additional structure.

Corollary 1

If $X_{h}$ and $V_{h}$ fulfill Assumption 2, then for all $x_{h}\in X_{h}$, $v_{h}\in V_{h}$, and $u_{h}\in U_{h}$, it holds that

[TABLE]

Proof:

It is easy to see that the two statements are equivalent, as in the first statement the point is projected onto a grid point whenever the it is closer than $r$ to this grid point in the infinity norm, and the second statement states that the point is in a cell around the grid point with radius $r$ in the infinity norm. Both statements are only true if $\|x^{*}_{h}-(f(x_{h},u_{h})+v_{h})\|_{\infty}\leq r$ ∎

From Corollary 1, we can see that there exists a neighborhood around $v_{h}$, which still leads to the same $x^{*}_{h}$. Thus, for a given $x_{h}$, $u_{h}$ and $v_{h}$, we can compute the subset of all continuous disturbances $\tilde{V}(x_{h},u_{h},v_{h})\subseteq V$ which maps to the same state grid point $x_{h}^{*}=(f(x_{h},u_{h})+v_{h}+rB_{\infty})\cap X_{h}$,

[TABLE]

see Fig. 12 for an illustration of $\tilde{V}(x_{h},u_{h},v_{h})$. In the interest of readability the dependency of $\tilde{V}$ on the grid point $x_{h}$ will subsequently be left out, and we just refer to $\tilde{V}(u_{h},v_{h})$.

Thus, by only looking at finite disturbance $v_{h}$, but considering $\tilde{V}(u_{h},v_{h})$ it is possible to make a statement about the continuous disturbance. In the following we will discuss how the set $\tilde{V}(u_{h},v_{h})$ can be used to formulate a modified discriminating kernel algorithm which is equivalent to the “semi-finite” case described in Proposition 4.

Given an instance of the discriminating kernel algorithm with a grid point $x_{h}\in K_{h}^{n}$, we can compute the set of all $u_{h},\;v_{h}$ such that the intersection of $(f(x_{h},u_{h})+v_{h}+rB_{\infty})\cap X_{h}$ and $K_{h}^{n}$ is non-empty,

[TABLE]

Proposition 5

Given $x_{h}\in K_{h}^{n}$ and $I_{u,v}(x_{h})$, it holds that for all $v\in V$ there exists a $u_{h}\in U_{h}$ such that

[TABLE]

if and only if, $\bigcup_{I_{u,v}(x_{h})}\tilde{V}(u_{h},v_{h})=V$

Proof:

The if part directly follows from the definition, as all continuous disturbances $v\in V$ are considered. The only if part is satisfied by Assumption 2, which ensures that for a control $u_{h}$ the union of all $\tilde{V}$ is equal to $V$. ∎

Similar to the proof of Proposition 2, by using [41][Equation (4)], we know that for the “semi-finite” discriminating kernel it holds that if $x_{h}\in\textnormal{Disc}_{\tilde{G}_{h}^{r}}(K_{h})$, for all $v\in V$ there exists a $u_{h}\in U_{h}$ such that $(f(x_{h},u_{h})+v+rB_{\infty})\cap X_{h}\in\textnormal{Disc}_{\tilde{G}_{h}^{r}}(K_{h})$. Thus, $\bigcup_{I_{u,v}(x_{h})}\tilde{V}(u_{h},v_{h})=V$ allows us to compute $\textnormal{Disc}_{\tilde{G}_{h}^{r}}(K_{h})$ by only considering a finite number of disturbances. However, we need to compute $\bigcup_{I_{u,v}(x_{h})}\tilde{V}(u_{h},v_{h})=V$ which may be hard. Thus, we now focus on calculating and approximating of $\bigcup_{I_{u,v}(x_{h})}\tilde{V}(u_{h},v_{h})=V$. Notice first that the set $\tilde{V}(u_{h},v_{h})$ is always a box. Therefore, they can be parameterized as

[TABLE]

Where, the subscript $j$ refers to each dimension of the uncertainty $V\subset\mathbb{R}^{d}$ and for each of these dimensions the lower and upper bound can be calculated by,

[TABLE]

As the union of boxes is in general not convex, checking that $\bigcup_{I_{u,v}(x_{h})}\tilde{V}(u_{h},v_{h})=V$ is a combinatorial problem. However, by approximating the union conservatively we are able reduce the computational burden while guaranteeing an inner approximation.

To approximate the union notice that $v_{h}\in\tilde{V}(u_{h},v_{h})$ always holds. Thus we first find a box approximation for the union of the boxes around each $v_{h}$ grid point, and then calculate the union of the resulting $|V_{h}|$ boxes, which can be done by checking if neighboring boxes overlap. To compute a box approximation of the union at one disturbance grid point let us define $I_{u}(x_{h},v_{h})=\{u_{h}\in U_{h}|\;(f(x_{h},u_{h})+v_{h}+rB_{\infty})\cap X_{h}\in K_{h}^{n}\}$, which is a set similar to $I_{u,v}(x_{h})$. Here we propose two approximations of $\bigcup_{I_{u}(x_{h},v_{h})}\tilde{V}(u_{h},v_{h})$, first the maximal volume box $\max_{I_{u}(x_{h},v_{h})}\text{Vol}(\tilde{V}(u_{h},v_{h}))$ and second the intersection of all boxes $\bigcap_{I_{u}(x_{h},v_{h})}\tilde{V}(u_{h},v_{h})$. Both approximations result in a box and are an inner approximation of the union.

Thus, we propose the modified discriminating kernel algorithm outlined in Algorithm 1. The algorithm guarantees an inner approximation of the discriminating kernel in the following sense, $\textnormal{Disc}_{{G}_{h}^{r}}(K_{h})\subset\textnormal{Disc}_{\tilde{G}_{h}^{r}}(K_{h})\subset\textnormal{Disc}_{{G}^{r}}(K)\cap X_{h}$, the first inner approximation follows due to the approximation of $\bigcup_{I_{u}(x_{h},v_{h})}\tilde{V}(u_{h},v_{h})$ and the second due to Proposition 4.

Note that the union approximation is not necessary if one control input is robust with respect to all discrete disturbances. Because by virtue of Assumption 2, we know that in this case $\bigcup_{I_{u,v}(x_{h})}\tilde{V}(u_{h},v_{h})=V$.

As the proposed algorithm constructs an inner approximation in the case of a finite disturbance, it is possible to state the following finite version of Proposition (2).

Proposition 6

Consider the finite dynamical system corresponding to the extended and discretized system of (9),

[TABLE]

where $v_{h}\in V_{h}$ is the discretization of $LrB_{\infty}$ according to Assumption 2. And if $\textnormal{Disc}_{G^{r}_{h}}(K_{h})$ is computed with the Algorithm 1 and Assumption 2 holds, then the following properties hold:

$\textnormal{Disc}_{G^{r}_{h}}(K_{h})$* is a viability domain of $F^{r}_{h}$.*

• 2)

For all $x_{h}\in\textnormal{Disc}_{{G}^{r}_{h}}(K_{h})$ and for all $\hat{x}\in x_{h}+rB_{\infty}$, there exists a $u_{h}\in U_{h}$ such that $f(\hat{x},u_{h})\in\textnormal{Disc}_{{G}^{r}_{h}}(K_{h})+rB_{\infty}$.

Proof:

1. Identical to the proof of Proposition 2 statement 2), as $\textnormal{Disc}_{G^{r}_{h}}(K_{h})$ is a discriminating domain and $v=0$ is an allowed disturbance.

2. By using the finite version of [41][Equation (4)], and the guarantee of the inner approximation $\textnormal{Disc}_{{G}_{h}^{r}}(K_{h})\subset\textnormal{Disc}_{{G}^{r}}(K)\cap X_{h}$ we know that for all $v\in LrB_{\infty}$ there exists a $u_{h}\in U_{h}$ such that $(f(x_{h},u_{h})+v+rB_{\infty})\cap X_{h}\in\textnormal{Disc}_{{G}_{h}^{r}}(K_{h})$. Furthermore, by Corollary 1 we can reformulate the discriminating kernel condition as, for all $v\in LrB_{\infty}$ there exists a $u_{h}\in U_{h}$ such that $f(x_{h},u_{h})+v\in\textnormal{Disc}_{{G}_{h}^{r}}(K_{h})+rB_{\infty}$. The same Lipschitz argument as in the proof of Proposition 2, allows to conclude that for all $\hat{x}\in x_{h}+rB_{\infty}$, there exists a $u_{h}\in U_{h}$ such that $f(\hat{x},u_{h})\in\textnormal{Disc}_{{G}^{r}_{h}}(K_{h})+rB_{\infty}$ ∎

We conclude our discussion by pointing out that if instead of the discriminating kernel the leadership kernel algorithm is used, then the inner approximation is directly guaranteed. Because by virtue of Assumption 2, we know that for any robust control input $\bigcup_{I_{u,v}(x_{h})}\tilde{V}(u_{h},v_{h})=V$.

Appendix B Stationary velocity grid

The stationary velocity manifold is gridded in $v_{x}$ and $\delta$ coordinates. In the $v_{x}$ direction the space is uniformly gridded between a lower and an upper velocity, with a fixed spacing, see Table V. For every $v_{x}$ grid point, $\delta$ is gridded within the normal driving region (see [8]) with a fixed number of grid points, see Table V. Additionally, in all cases the same 12 stationary velocity points corresponding to drifting are included.