Realtime Simulation-in-the-Loop Control for Agile Ground Vehicles

Abstract

In this paper we present a system for real-time control of agile ground vehicles operating in rough 3D terrain replete with bumps, berms, loop-the-loops, skidding, banked-turns and large jumps. The proposed approach fuses local-planning and feedback trajectory-tracking in a unified, simulation-based framework that operates in real-time. Experimentally we find that fast physical simulation-in-the-loop enables impressive control over difficult 3D terrain. The success of the proposed method can be attributed to the fact that it takes advantage of the full expressiveness of the inherently non-linear, terrain-dependent, highly dynamic systems involved. Performance is experimentally validated in a motion capture lab on a high-speed non-holonomic vehicle navigating a 3D map provided by an offline perception system.

Appendix A

We define the bezier curve $\mathbf {P}(s),\, s\in [0,1]$ as $\mathbf {P}(s)=\sum _{i=0}^{n}B_{i}^{n}(s)\mathbf {P}_{i}$ where $\mathbf {P}_{i}\in \mathbb {R}^{2}$ is the $i$th control point. We have initially parametrized the bezier curve with the variable $s$ rather than the traditional $t$, as we desire the final curve to indeed be parametrized by time, which we represent as $t$. The relationship between $s$ and $t$ depends on the evolution of the vehicle velocity. As we have proposed a constant acceleration parameter $a$, we can parametrize the bezier curve w.r.t time as $\mathbf {P}(s(t))$ where $s(t)=t/t_{total}=t/(2d/(v_{i}+v_{f})$, $d$ is the distance along the bezier curve, and $v_{i}$ and $v_{f}$ are the initial and final velocities respectively. The curvature of the bezier curve at any value of $t$ defined as $\kappa \left( t\right)$ can then be calculated using the analytical first and second derivatives of the curve in $x$ and $y$ [5]. The steering angle $\theta _{s}$ can then be obtained from $\kappa$, based on a simplified symmetric bicycle steering model with zero slip [18] $\theta _{s}=tan^{-1}\left( l\kappa \right)$ where $l$ is the wheel base.

The order of the bezier curve is chosen such that sufficient expressivity exists to satisfy boundary conditions. For trajectory tracking, a major requirement is $C^{2}$ continuity. This is needed to ensure maximum feasibility of steering commands, as the actuators cannot be discontinuously controlled, when switching from one command sequence to the next. To independently constrain the curvature on the boundary of a bezier curve, three control points are required [19], therefore we use fifth degree bezier curves $\left( n=6\right)$ in order to independently satisfy the constraint at both ends of the curve.

Given the bezier curve control points, the parameter vector $\mathbf {p^{\prime }}$ can be expressed as $\mathbf {p^{\prime }}=\begin{bmatrix}\mathbf {P}_{0},&...&,\mathbf {P}_{6}&a\end{bmatrix}^{T}$ where $\mathbf {P}_{0},...,\mathbf {P}_{6}\in \mathbb {R}^{2}$ are the control points. $\mathbf {p^{\prime }}$ is defined by 13 parameters: 2 for each control point and 1 for acceleration. There are three gauge freedoms associated with transforms in $\mathbb {SE}2$, reducing the actual parameter space to 10. However, properties of the bezier curve can be exploited to further reduce the number of parameters. Given the initial projected 2D pose ($\mathbf {x}_{i_{proj}}=\begin{bmatrix}x_{i}~y_{i}~\theta _{i}~\kappa _{i}\end{bmatrix},$ as described below), the position, heading and curvature of the vehicle can be constrained (the curvature is constrained by the current curvature $\kappa _{i}$). Consequently, the first three control points can be parametrized using only $h_{i}$ and $a_{i}$ (refer to [19]). Fixing $x_{i}$, $y_{i}$ and $\theta _{i}$ also removes the gauge freedoms of the control points. At the other end of the bezier curve, $\mathbf {x}_{f_{proj}}=\begin{bmatrix}x_{f}~y_{f}~\theta _{f}~\kappa _{f}\end{bmatrix}$ is used to reduce the parameter space of the final 3 control points from 6 to 3, as the end curvature is also constrained by $k_{f}$ to enforce $C^{2}$ continuity. This leaves the parameters $h_{i}$, $a_{i}$, $h_{f}$ and $a_{f}$. To further reduce the parameter space, these values were set as follows: $h_{i}=a_{i}=h_{f}=a_{f}=\frac{1}{5}\begin{Vmatrix}\begin{bmatrix}x_{f}&y_{f}\end{bmatrix}-\begin{bmatrix}x_{i}&y_{i}\end{bmatrix}\end{Vmatrix}$ where the factor of $1/5$ times the distance between the start and end points was empirically determined to result in adequate control laws for a wide range of start and goal positions, headings and curvatures. Therefore, the initial parameter space $\mathbf {p}$ can be obtained from the reduced parameter space: $\mathbf {p^{\prime }}=\mathcal {B}(\mathbf {p,}\mathbf {x}_{i_{proj}},\kappa _{f})$ where $\mathbf {p}=\begin{bmatrix}x_{f}~y_{f}~\theta _{f}~a\end{bmatrix}^{T}$, and $\mathcal {B}$ is a function which maps the reduced parameter space to the full 5th degree bezier control points. Note that $\mathcal {B}\left( \right)$ requires the initial state $\mathbf {x_{i}}$ to constrain the first three control points, and the final curvature $\kappa _{f}$ to constrain the last 3 points according to $\mathbf {p}$. Therefore the command-sequence used in the optimization is defined as $\mathbf {c}=C\left( \mathbf {x}_{i},\mathcal {B}(\mathbf {p,}\mathbf {x}_{i_{proj}},\kappa _{f})\right)$.

The control law parametrization scheme detailed previously uses bezier curves in $\mathbb {R}^{2}$. While control commands generated from these curves would perfectly steer an ideal vehicle in $\mathbb {R}^{2}$, the same statement can not be made for a vehicle in $\mathbb {R}^{3}$ driving over rough terrain. For this reason, we are interested in initializing the steering command-sequence in a way which places us in the basin of attraction of the optimization that follows.

To initialize the bezier curve, we project the pose of the vehicle onto a plane in $\mathbb {R}^{2}$, the normal of which is calculated as the average of the terrain normals at the start and end of the trajectory. This serves to maximize the likelihood that following the curvature of the bezier curve will lead the vehicle to follow the shape of the curve. We define the projection as $\mathbf {x}_{proj}\in \mathbb {R}^{4}=\mathcal {P}(\mathbf {T}_{pw}\mathbf {x}_{w},\kappa )$. Where $x_{proj}$ is the state vector in projected $\mathbb {R}^{2}$ coordinates defined as $\begin{bmatrix}x~y~\theta ~\kappa \end{bmatrix}$, $\mathbf {T}_{pw}$ is the $\mathbb {R}^{4\times 4}$ transformation matrix from world coordinates to the projection plane coordinates (obtained from the terrain normals), and $\mathcal {P}$ is the projection function, which takes the $x$ and $y$ components, and projects the vehicle theta onto the plane. Once both $\mathbf {x}_{i_{proj}}$ and $\mathbf {x}_{f_{proj}}$ have been obtained, a bezier curve can be initialized using the aforementioned re-parametrization function $\mathcal {B}(\mathbf {p^{\prime }},\mathbf {x}_{i_{proj}},\kappa _\mathbf {f})$, which returns the initial bezier control points.