Examples and Applications

This chapter contains some examples and robotic applications using PolyMPC.

Thrust Vector Control of a Rocket

Thrust Vector Control (TVC) is a key technology enabling rockets to perform complex autonomous missions, such as active stabilization, orbit insertion, or propulsive landing. This is achieved by independently controlling the thrust direction and magnitude of each of its engines. Compared to aerodynamic control such as fins or canard, it guarantees a high control authority even in the absence of an atmosphere, i.e. during high altitude launches or exploration of other planetary bodies.

Below we demonstrate how the PolyMPC software is used to buid a guidance and nonlinear stabilisation systems for a small-scale rocket mock-up. The C++ code is shown to run real-time on a low-cost ARM-based embedded platform.

_images/drone.jpg _images/drone_gimbal.jpg _images/draw_mpc_simu_demo.gif

System Dynamics

The state vector is denoted \(X\), and contains the position vector, the velocity, the orientation quaternion \(q\), and the angular speed in the body frame \(\omega^b\).

\[\begin{split}\begin{equation} \begin{split} X &= \begin{bmatrix} p& v& q& \omega^b \end{bmatrix}^T\\ \end{split} \label{eq:state} \end{equation}\end{split}\]

The control vector is denoted \(U\). The prototype is controlled through the command servo angles \(\theta_1\) and \(\theta_2\), as well as the speed of the bottom and top propellers, \(P_{B}\) and \(P_{T}\) respectively (in \(\%\) for the rest of the paper). It is more convenient, however, to consider the propellers’ average command speed as an input \(\overline{P} = \frac{P_{B} + P_{T}}{2}\) and the command speed difference \(P_{\triangle}=P_{B} - P_{T}\):

\[\begin{equation} U = \begin{bmatrix} \theta_1& \theta_2& \overline{P}& P_{\triangle} \end{bmatrix}^T \end{equation}\]

The state equations are given by the generic 6 DoF solid body dynamics. Omitting the atmosphere interaction, the forces acting on the vehicle are: gravity \(mg\), thrust \(F^b_T\) and total torque \(M^b\) in BRF. \(M^b\) comprises the torque due to the thrust vector \(F^b_T\) and the torque \(M^b_P\) caused by the speed difference between the two propellers.

\[\begin{split}\begin{equation} \begin{split} &\dot{X} = f(X, U) = \begin{bmatrix} \dot{p}\\ \dot{v}\\ \dot{q}\\ \dot{\omega^b}\\ \end{bmatrix} = \begin{bmatrix} v\\ \cfrac{R(q) F^b_T}{m} + g\\ \frac{1}{2} q \circ \omega^b\\ I^{-1} (M^b - \omega^b \times (I\omega^b))\\ \end{bmatrix}\\ &M^b = r \times F^b_T + M^b_P\\ \end{split} \end{equation}\end{split}\]

with \(I\) the inertia matrix of the drone, and \(r\) the position of the thrust \(F^b_T\) from the center of mass, and \(R(q)\) is a rotation matrix.

Both the thrust \(F^b_T\) and the torque \(M^b_P\) vectors are determined by the gimbal angles:

\[\begin{split}\begin{equation} \begin{split} \frac{F^b_T}{\lVert F^b_T \rVert}=\frac{M^b_P}{\lVert M^b_P \rVert }= \begin{bmatrix} \sin\theta_2\\ -\sin\theta_1 \cos\theta_2\\ \cos\theta_1 \cos\theta_2\\ \end{bmatrix} \end{split} \end{equation}\end{split}\]

Rocket Model Implementation: drone_model.hpp

Rocket Guidance Algorithm

The guidance algorithm is based on a point mass model and uses a minimal energy formulation where terminal time \(t_f\) is an optimization variable. The final position \(p(t_f)\) is constrained to the small neighborhood of the target position \(p_t\) with zero-velocity. The corresponding optimal control problem (OCP) is

\[\begin{split}\begin{equation} \begin{split} &\min_{F_T(t), \phi(t), \psi(t), t_f} \; \int_{t_0}^{t_f} F^2_T(t) dt + \rho \eta\\ &\begin{split} s.t. \quad & \dot{p}(t) = v(t), \; \dot{v}(t) = \frac{F_T(t)}{m} \begin{bmatrix} \sin\phi \sin\psi \\ -\cos\phi \sin\psi \\ \cos\psi \\ \end{bmatrix} + g\\ & v_{min} \leq v(t) \leq v_{max}\\ & F_{min} \leq F_T(t) \leq F_{max}\\ - & \psi_{max} \leq \psi(t) \leq \psi_{max} \\ & p(t_0) = p_0\\ & v(t_0) = v_0\\ & z(t_f) = z_{t}\\ & (x(t_f)-x_{t})^2 + (y(t_f)-y_{t})^2 \leq \eta\\ & v(t_f) =0\\ \end{split} \end{split} \label{eq:OCP} \end{equation}\end{split}\]

The propulsion vector of the rocket is defined in spherical coordinates, where \(F_T\) is an absolute value of thrust, azimuth angle \(\phi\) and polar angle \(\psi\). Symmetric constraints on the polar angle define the aperture of the reachable cone. Since the attitude of the vehicle is not explicitly considered in the guidance problem, the polar angle is usually related to the tilt angle of the rocket, thus should not be too large.

The slack variable \(\eta\) weighted by \(\rho\) is used to formulate a slack constraint on the target horizontal position, which may be violated when close to the target position.

Since the guidance problem has free terminal time, the horizon is scaled to the interval [0, 1], \(\tau \equiv \frac{t-t_0}{t_f-t_0}\), the dynamics become \(\dot{x} = (t_f-t_0) f(x, u)\), and the horizon length \((t_f-t_0)\) then becomes a variable parameter in the OCP.

\[\begin{split}\begin{equation} \begin{split} &\min_{u(t)} \; \int_{t_0}^{t_f} l(x, u, t) dt + V_f(x_f)\\ &\begin{split} s.t. \quad & \dot{x} = f(x, u)\\ & -\theta_{max} \leq \theta_1 \leq \theta_{max}\\ & -\theta_{max} \leq \theta_2 \leq \theta_{max}\\ & -\dot{\theta}_{max} \leq \dot{\theta_1} \leq \dot{\theta}_{max}\\ & -\dot{\theta}_{max} \leq \dot{\theta_2} \leq \dot{\theta}_{max}\\ & P_{min} \leq \bar{P} + {P_\delta}/2 \leq P_{max}\\ & P_{min} \leq \bar{P} - {P_\delta}/2 \leq P_{max}\\ & {P_\delta}_{min} \leq P_\delta \leq {P_\delta}_{max}\\ & 0 \leq z\\ \end{split} \end{split} \end{equation}\end{split}\]

Guidance Algorithm Implementation: drone_guidance_ocp.hpp , guidance_solver.hpp

Model Predictive Tracking Control

The nonlinear model predictive control (NMPC) algorithm uses the full state dynamics described above. It has a predictionhorizon of 2 seconds.

\[\begin{split}\begin{equation} \begin{split} &\min_{U(t)} \; \int_{t_0}^{t_f} l(X, U, t) dt + V_f(X_f)\\ &\begin{split} s.t. \quad & \dot{X} = f(X, U)\\ & -\theta_{max} \leq \theta_1 \leq \theta_{max}\\ & -\theta_{max} \leq \theta_2 \leq \theta_{max}\\ & -\dot{\theta}_{max} \leq \dot{\theta_1} \leq \dot{\theta}_{max}\\ & -\dot{\theta}_{max} \leq \dot{\theta_2} \leq \dot{\theta}_{max}\\ & P_{min} \leq \overline{P} + P_{\triangle}/2 \leq P_{max}\\ & P_{min} \leq \overline{P} - P_{\triangle}/2 \leq P_{max}\\ & {P_{\triangle}}_{min} \leq P_{\triangle} \leq {P_{\triangle}}_{max}\\ & 0 \leq z\\ \end{split} \end{split} \label{eq:control_OCP} \end{equation}\end{split}\]

The servo motors are constrained in a range of \(\pm 15^{\circ}\). We introduce derivative constraints to limit the maximum rate of inputs given by the controller and smoothen the open-loop trajectories.

The propeller speed models are directly included in the formulation, and along with the constraints on top and bottom propeller speeds \(P_T\) and \(P_B\), provide a simple way to deal with the trade-off between roll control (through \(P_{\triangle}\)) and altitude control (through \(\overline{P}\)).

Stage Cost

The tracking residual at time \(t\) corresponds to the difference between the predicted \(x(t)\) and the target guidance trajectory \(x_G(t)\). The stage cost combines the squared tracking residual, penalty on the control input and penalty on the deviation from vertical orientation. The components \(q_w q_x-q_y q_z\) and \(q_w q_y + q_x q_z\) penalize deviations of pitch and yaw angles from zero. The roll angle is not controlled directly, but rather the roll rate \(\omega^b_z\), as the final roll angle is not critical for the flight mission.

\[\begin{equation} l(X, U, t) = e(X - X_G (t))^T Q\; e(X - X_G (t)) + U^T R U \end{equation}\]
\[\begin{equation} e(X) \equiv \begin{bmatrix}p & v & q_w q_x-q_y q_z & q_w q_y + q_x q_z & \omega^b \end{bmatrix}^T \end{equation}\]

Terminal Cost

In order to improve stability, a continuous-time linear quadratic regulator (LQR) based on a linearization around the zero-speed steady-state is used as a terminal controller. The matrices \(Q\) and \(R\) for the LQR design are identical to the ones used in the stage cost.

\[\begin{equation} A = \left.\frac{\partial f(x, u)}{\partial x}\right\vert_{x_s, u_s} \; B = \left.\frac{\partial f(x, u)}{\partial u}\right\vert_{x_s, u_s} \label{eq:linearised} \end{equation}\]

Note that the states \(q_w\) and \(q_z\) are fixed in the linearization, as they are not controlled. The matrix of the terminal quadratic cost \(Q_f\) is then obtained by solving the continuous time algebraic Riccati equation (CARE).

\[\begin{equation} V_f(X_f) = e(X_f)^T Q_f \; e(X_f) \end{equation}\]

Predictive Controller Implementation: drone_tracking_ocp.hpp, drone_mpc.hpp, drone_mpc_solver.hpp

Experimental Flights and Presentation at ICRA 2023**

The control system scheme is shown on the figure below.

_images/drone_software_architecture-1.png

References

More details are available in [Linsen2022].

Race Car Path Following Control

Vehicle Dynamics

The planar diagram of the bicycle model is shown below.

_images/bicycle_model.png

We first introduce the car dynamics in the natural Cartesian reference frame:

\[\begin{split}\begin{align} \dot{v_x} &= \dot{\psi} v_y + \frac{1}{M}(F_{x,f} \cos\delta -F_{y,f} \sin\delta + F_{x,r} - F_{drag}), \nonumber\\ \dot{v_y} &= \dot{\psi} v_x + \frac{1}{M}(F_{x,f} \sin\delta + F_{y,f} \cos\delta + F_{y,r}), \\ \dot{\omega} &= \frac{1}{I_z}( - L_r F_{y,r} + L_f (F_{y,f} cos\delta + F_{x,f} \sin\delta)), \nonumber\\ \dot{X} &= v_x \cos\psi - v_y \sin\psi, \nonumber\\ \dot{Y} &= v_x \sin\psi + v_y \cos\psi, \nonumber\\ \dot{\psi} &= \omega, \nonumber \end{align}\end{split}\]

The first three equations are the dynamics of the system in the car body frame with \(x\) pointing forward along the main symmetry axis. The position and orientation in the fixed Inertial reference frame (IRF) is given by \(X\), \(Y\) and \(\phi\).

This bicycle model takes as input the steering angle \(\delta\) and the front and rear axles longitudinal forces \(F_{xf}\) and \(F_{xr}\) as summarized below

\[\begin{split}\begin{equation} \begin{split} \dot{x} &= f(x,u) \\ x &= [v_x, v_y, \omega, X, Y, \psi ], \\ u &= [\delta, F_{xf}, F_{xr}]. \end{split} \end{equation}\end{split}\]

There exist several approaches for tyre forces modelling which differ in numerical complexity. For autonomous driving at moderate speeds, one could use the linear tyre model with a constant cornering stiffness. However, for high lateral accelerations in the racing scenarios the model becomes inadequate. It is important to represent the lateral dynamics as accurately as possible, therefore, the Pacejka magic formula is used for tyre models:

\[\begin{split}\begin{equation} \begin{split} F_{yf} &= F_z D_f \sin\Big(C_f \tan{-1} \big(B_f \alpha_f - E_f (B_f \alpha_f - \tan^{-1}(B_f\alpha_f)) \big) \Big), \\ F_{yr} &= F_z D_r \sin\Big(C_r \tan^{-1}\big(B_r \alpha_r - E_r (B_r \alpha_r - \tan^{-1}(B_r\alpha_r)) \big) \Big). \end{split} \end{equation}\end{split}\]

Front and rear lateral forces each have their own set of magic formula coefficients \(B, C, D, E\) and are function of the slip angles \(\alpha\). The slip angles can be modeled as

\[\begin{split}\begin{equation} \begin{split} \alpha_f &= -\tan^{-1}\Big(\frac{v_y + \dot{\psi}L_f}{v_x}\Big) + \delta, \\ \alpha_r &= -\tan^{-1}\Big(\frac{v_y - \dot{\psi}L_r}{v_x} \Big) . \end{split} \end{equation}\end{split}\]

The navigation information is naturally provided in the Cartesian frame. For the control system, however, it is beneficial to represent the car position in the reference frame linked to the track, the so called Curvilinear reference frame. This frame is also local and attached to the car body frame. This transformation allows natural formulation of the path-following problem. Each point on a path characterised by a position in the Cartesian frame \(X_c, Y_c\), global heading \(\psi_c\) and a curvature \(\kappa_c\).

_images/curvilinear_frame_sketch.png

From the position and heading in the Cartesian frame, we calculate the distance \(w\) and the heading deviation \(\theta\) from the center line, with \(w > 0\) to the left of the center line and \(\theta > 0\) a counter-clockwise rotation with respect to the tangent to the centerline \(\psi_c\).

\[\begin{split}\begin{align} \begin{split} w &= (Y-Y_c)\cos(\psi_c) - (X-X_c)\sin(\psi_c),\\ X &= X_c - w\sin(\psi_c), \\ Y &= Y_c + w\cos(\psi_c),\\ \theta &= \psi - \psi_c. \end{split} \end{align}\end{split}\]

An additional state, \(s\), is added to track the evolution along the racing line. The resulting kinematic equations in the Curvilinear frame are:

\[\begin{split}\begin{equation} \begin{split} \dot{s} &= \frac{1}{1-\kappa_c w}(v_x \cos\theta -v_y sin\theta), \\ \dot{w} &= v_x \sin\theta + v_y \cos\theta, \\ \dot{\theta} &= \dot{\psi} - \dot{\psi_c} = \dot{\psi} - \kappa_c \dot{s}.\\ \end{split} \end{equation}\end{split}\]

From the position and heading in the Cartesian frame, we calculate the distance \(w\) and the heading deviation \(\theta\) from the center line, with \(w > 0\) to the left of the center line and \(\theta > 0\) a counter-clockwise rotation with respect to the tangent to the centerline \(\psi_c\).

#include <cmath>
#include <iostream>
#include <string>

#include "polynomials/splines.hpp"

template<typename _scalar, int Size>
using Vec = typename Eigen::Matrix<_scalar, Size, 1>

// compute the lateral forces in the wheels
template<typename _scalar> 
void LateralForces(const Eigen::Ref<const Vec<_scalar, 4>>& Vel,
		      const Eigen::Ref<const Vec<_scalar, 2>>& FzForce, const _scalar& Steering,
                      Eigen::Ref<Vec<_scalar, 2>> FYLateral) 
{
    //-------MODEL------
    // Assume all parameters (i.e., L_r, L_f, Cxx, tire parameters are defined somewhere)

    //slip angles 
    _scalar a_r = atan2(Vel(3), Vel(2) + 0.01);
    _scalar a_f = atan2(Vel(1), Vel(0) + 0.01) - Steering;
    
    // Front lateral slip force
    FYLateral(0) = -FzForce(0) * D_f * sin(C_f * atan2(B_f * a_f - E_f * (B_f * a_f - atan2(B_f * a_f,(_scalar)(1))),(_scalar)(1)));

     // Rear lateral slip force
     FYLateral(1) = -FzForce(1) * D_r * sin(C_r * atan2(B_r * a_r - E_r * (B_r * a_r - atan2(B_r * a_r,(_scalar)(1))),(_scalar)(1)));
}

// For the centreline parametrisation a cubic spline from the PolyMPC polynomial library is used in this example
// the user must provide a segment length (assumed equal) and a [4 x N] matrix containing poly. coefficients for each of [N] segments
// it is then possible to evaluate and differentiate the resulting spline using the standard autodiff interface 
polympc::EquidistantCubicSpline<double> kappa_spline;
void setSplineData(Eigen::Ref<Eigen::MatrixXd>& _data_coeff, double segment_length) 
{
    // set the number of spline segments and polynomial coefficients for each segment
    kappa_spline.segment_length() = segment_length;
    kappa_spline.coefficients()   = Eigen::Map<Eigen::Matrix<Scalar, 4, Eigen::Dynamic>>(Data.col(4).data(), 4, Data.rows() / 4);
}


// Finally, evaluate the dynamics
template<typename _scalar>
void Dynamics(const Eigen::Ref<const Vec<_scalar, NX_>>& x, const Eigen::Ref<const Vec<_scalar, NU_>>& u,
		 const Eigen::Ref<const Vec<_scalar, NP_>>& param, Eigen::Ref<Vec<_scalar, NX_>> xdot)
{
    //Look-up table to get kappa as a function of s = x[3]
    // Assume all parameters (i.e., L_r, L_f, Cxx, tire parameters are defined somewhere)
    // ================ MODEL EQUATIONS ==================

    // Rear Axis
    _scalar vx_r = x(0);
    _scalar vy_r = x(1) - x(2) * L_r;
    _scalar FZr  = (_scalar)(m * g * L_f / (L_r + L_f));

    //Front Axis
    _scalar vx_f = x(0);
    _scalar vy_f = x(1) + x(2) * L_f;
    _scalar FZf = (_scalar)(m * g * L_r / (L_f + L_r));

    // Drag Force
    _scalar Fdrag = (rollResist + Cxx * x(0)*x(0));

    Vec<_scalar, 4> velocities;
    Vec<_scalar, 2> FZForce;
    Vec<_scalar, 2> FYLateral;

    velocities << vx_f, vy_f, vx_r, vy_r;
    FZForce << FZf, FZr;

    //Get Lateral Forces
    getLateralForces<_scalar, T>(velocities, FZForce, u(0), FYLateral);
    FYr = FYLateral(1);
    FYf = FYLateral(0);

    xdot(0) = x(2) * x(1) + (u(2) + u(1) * cos(u(0)) -  FYf * sin(u(0)) - sgn(u(0)) * Fdrag) / m; //vx_dot
    xdot(1) = -x(2) * x(0) + (FYf * cos(u(0)) + u(1) * sin(u(0)) + FYr) / m;  //vy_dot
    xdot(2) = (-FYr * L_r + (FYf * cos(u(0)) + u(1) * sin(u(0))) * L_f) / Iz; //omega_dot = theta_dot_dot

    kappa_c = kappa_spline.eval(x(3));
    xdot(3) = (1.0 / (1 - kappa_c * x(4))) * (x(0) * cos(x(5)) - x(1) * sin(x(5)));	//s_dot
    xdot(4) = x(0)*sin(state(5)) + x(1)*cos(x(5));					//w_dot
    xdot(5) = x(2) - kappa_c * xdot(3); 						//theta_dot

};

Path Parametrisation

Oftentimes in practical applications, the path is represented by a set of equally spaced points, where each point is characterised by its position, curvature and reference speed. However, such discrete representation is not very efficient for the path following NMPC or local replanning as it adds constraints on the time discretisation, and requires an extra step to find the best point on the path to track. Therefore, in this example we choose to interpolate the path with custom cubic splines that can be efficiently automatically differentiated. In the following, we detail the interpolation procedure. Let \(y\) denote one of the path parameters to be approximated by a cubic spline. We divide every data chunk into \(N_s\) equidistant segments and fit a cubic spline \(y(s)\) parametrised by polynomial coefficients for each segment \(\{ P_i \}\).

\[\begin{split}\begin{equation} \begin{split} %y(s) = \sum_{i=0}^{3}P_{i, N_i}\times &s^i, \nonumber \qquad i \in [0, 1, 2, 3], N_i \in [1, \dots, N_s] \\ \min_{\{ P_i \}} \sum_{s=0}^{s_{max}} (y(s) &- \tilde{y}(s))^2, \nonumber \\ \textrm{subject to: } y(s=0) &= \tilde{y}(0), \nonumber\\ \frac{dy}{ds}\bigg\rvert_{s=0} &= \dot{\tilde{y}}(0), \nonumber \\ \frac{dy}{ds}\bigg\rvert_{s=s_{max}} &= \dot{\tilde{y}}(s_{max}), \nonumber \\ \end{split} \end{equation}\end{split}\]

Where \(s_{max}\) is the maximum spline length and \(\tilde{y}(s)\) is data evaluated at the grid points. The derivative constraints enforce continuity of the two neighbouring segments.

We formulate a quadratic problem (QP) in the form of \(\frac{1}{2}(D^TP - \tilde{Y})^2\) where P is the coefficient vector of all segments in one spline, and D is the data vector containing all the grid points.

\[\begin{split}\begin{equation*} {D_i = \begin{bmatrix} 1 & \dots & 1 \\ s_{0,N_i} & \dots & s_{max,N_i} \\ s_{0,N_i}^2 & \dots & s_{max,N_i}^2 \\ s_{0,N_i}^3 & \dots & s_{max,N_i}^3 \end{bmatrix} \in\mathbb{R}^{4\times N_{c,i}}} \\ \end{equation*}\end{split}\]
\[\begin{split}\begin{equation} {D = \begin{bmatrix} D_1 & & \\ & \ddots & \\ & & D_{N_s} \end{bmatrix}} \end{equation}\end{split}\]

where \(N_{c,i}\) is the number of points in one segment of the chunk such that \(\sum_{i=1}^{N_s}N_{c,i} = N_c\), with \(N_c\) the total number of points; \(\tilde{Y} \in\mathbb{R}^{N_c \times 1}\) is the vector of true data and \(P \in\mathbb{R}^{4\times 1}\) is the coefficients vector to optimize over.

This least squares formulation has the same minimum as the equivalent QP problem:

\[\begin{split}\begin{equation} \begin{split} \min_{X} \frac{1}{2}X^T H X + h^T X, \\ \textrm{subject to: } AX = b. \\ \end{split} \end{equation}\end{split}\]

where H, h, A are the Hessian, gradient and constraint Jacobian of the problem and b is the constant vector of data input.

\[\begin{split}\begin{equation} \begin{split} &A = \begin{bmatrix} A_1 & & \\ & \ddots & \\ & & A_{N_s} \\ \end{bmatrix} \\ &A_{N_i} = \begin{bmatrix} 1 & s_{0,N_i} & s_{0,N_i}^2 & s_{0,N_i}^3 \\ 0 & 1 & 2s_{0,N_i} &3s_{0,N_i}^2 \\ 0 & 1 & 2s_{max,N_i} &3s_{max,N_i}^2 \\ \end{bmatrix} \\ &H = DD^T \nonumber, \\ &h = -D \tilde{Y} \\ &b = [\tilde{y}_1(0),\dot{\tilde{y}}_0(0), \dot{\tilde{y}}_1(s_{max,1}), \cdots \\ &\dots,\tilde{y}_{N_s}(0),\dot{\tilde{y}}_{N_s}(0), \dot{\tilde{y}}_{N_s}(s_{max,N_s})]^T \end{split} \end{equation}\end{split}\]

We approximate with splines the center line (\(X_c\), \(Y_c\)), desired heading (tangent) (\(\psi_c\)) and curvature (\(\kappa_c\)). A cubic spline is also fitted for the reference velocity profile (\(v_{x,ref}\)). The QP problem is written and solved in the PolyMPC toolbox using the ADMM QP solver at a microsecond scale even on embedded platforms.

Frame Transformation and Path Localisation

Car navigation is typically represented in the Cartesian frame. Therefore, in order to estimate the car state in the Curvilinear frame using the equations from the section “Vehicle Dynamics”, one should first find the projection of the car onto the racing line.

This projection problem can be written as a minimization problem of the form:

\[\begin{split}\begin{equation} \begin{split} %&\min_{s}(X-X_c(s))^2 + (Y-Y_c(s))^2\\ &\min_{s} \Vert p - p_c(s) \Vert^2 \\ &\textrm{subject to}: 0 \leq s \leq s_{max} \end{split} \end{equation}\end{split}\]

Where \(p = [x, y]^T\) is the current positon of the car and \(p_c(s) = [x_c(s), y_c(s)]\) is a parametric path, i.e., the cubic splines as defined in the previous section.

Once an optimal solution \(s^{\star}\) is found, \(\kappa_c\) and \(v_{x,ref}\) can be computed and the state vector is converted to the Curvilinear frame. This NLP is solved using the PolyMPC toolbox using the dense SQP solver as shown below (example).

#include "solvers/sqp_base.hpp"
#include "polynomials/splines.hpp"
#include "solvers/box_admm.hpp"
#include "solvers/nlproblem.hpp"


// Minimise Euclidean distance to a spline curve
POLYMPC_FORWARD_NLP_DECLARATION(/*Name*/ ClosestPoint, /*NX*/ 1, /*NE*/0, /*NI*/0, /*NP*/2, /*Type*/double);
class ClosestPoint : public ProblemBase<ClosestPoint>
{
public:

    Eigen::MatrixXd m_data_coeff;
    double m_spline_length{ 0.5 }; // each segment is 0.5 [m] long

    polympc::EquidistantCubicSpline<double> m_xc_spline;
    polympc::EquidistantCubicSpline<double> m_yc_spline;

    // constructor
    ClosestPoint()
    {
        // initialise the track as a straight line (for instance)
	m_data_coeff.resize(20, 5);

	for (int i  = 0; i < 5; i++) 
	{
		m_data_coeff.row(4 * i)     << m_spline_length * i, 0, 0, 0, 0;
		m_data_coeff.row(4 * i + 1) << m_spline_length * i, 1, 0, 0, 0;
		m_data_coeff.row(4 * i + 2) << m_spline_length * i, 0, 0, 0, 0;
		m_data_coeff.row(4 * i + 3) << m_spline_length * i, 0, 0, 0, 0;
	}

        //Xc
        m_xc_spline.segment_length() = m_spline_length;
        m_xc_spline.coefficients() = Eigen::Map<Eigen::Matrix<double, 4, Eigen::Dynamic>>(m_data_coeff.col(1).data(), 4, m_data_coeff.rows() / 4);

        //Yc
        m_yc_spline.segment_length() = m_spline_length;
        m_yc_spline.coefficients() = Eigen::Map<Eigen::Matrix<double, 4, Eigen::Dynamic>>(m_data_coeff.col(2).data(), 4, m_data_coeff.rows() / 4);
    }

    ~ClosestPoint() = default;

    template<typename T>
    EIGEN_STRONG_INLINE void cost_impl(const Eigen::Ref<const variable_t<T>>& x, const Eigen::Ref<const static_parameter_t>& p, T& cost) const noexcept
    {
        T xc_ref = m_xc_spline.eval(x(0));
        T yc_ref = m_yc_spline.eval(x(0));

        cost = (p(0) - xc_ref)*(p(0) - xc_ref) + (p(1) - yc_ref)*(p(1) - yc_ref);
    }

    // set the data (if new coefficients are available)
    void set_data(const Eigen::Ref<const Eigen::MatrixXd>& _data_new, double _spline_length) 
    {
        //Xc
        m_xc_spline.segment_length() = _spline_length;
        m_xc_spline.coefficients() = Eigen::Map<Eigen::Matrix<double, 4, Eigen::Dynamic>>(m_data_coeff.col(1).data(), 4, m_data_coeff.rows() / 4);

        //Yc
        m_yc_spline.segment_length() = _spline_length;
        m_yc_spline.coefficients() = Eigen::Map<Eigen::Matrix<double, 4, Eigen::Dynamic>>(m_data_coeff.col(2).data(), 4, m_data_coeff.rows() / 4);
    }


};

Path Following NMPC

For more comfortable driving, hard rate constraints on the steering and acceleration rates can be added. For this, one can augment the system states with the steering control, delayed steering and the two longitudinal forces such that:

\[\begin{split}\begin{equation} \begin{split} &\frac{d}{dt}\delta = \dot{\delta} = v_1\\ &\frac{d}{dt}\delta_d = \frac{\delta(t) - \delta_{d}(t)}{T_{d}}\\ &\frac{d}{dt}F_{x,f} = \dot{F}_{x,f} = v_2 \\ &\frac{d}{dt}F_{x,r} = \dot{F}_{x,r} = v_3 \\ \end{split} \end{equation}\end{split}\]

The resulting augmented state vector is denoted with \(\xi\) and control with \(v\).

\[\begin{split}\begin{equation} \begin{split} \xi &= [v_x, v_y, \omega, s, w, \theta, \delta, \delta_{d}, F_{x,f}, F_{x,r}], \\ v &= [\dot{\delta}, \dot{F}_{x,f}, \dot{F}_{x,r}] \\ \dot{\xi} &= F(\xi, v) = F\big(f(\eta, \{\delta_{d},F_{x,f}, F_{x,r} \}), v\big), \\ \end{split} \end{equation}\end{split}\]

The constraints on the deviation from the center line \(w\) define the right and left boundaries of the lane. The slip angles are constrained to avoid tyre saturation. Finally, one may have:

\[\begin{split}\begin{equation} \begin{split} \min_{\xi(.),v(.)} \int_{t_0}^{t_f} l(\xi(t),v(t), p) dt + V_{f}(\xi(t_f)&)\\ \textrm{subject to: } \xi_{0} = \xi(0) \qquad& \\ \dot{\xi}(t) = F(\xi(t), v(t), p) \qquad &{t} \in [t_0, ..., t_f] \\%\textrm{ (Non-linear spatial frame dynamics)}\\ 0 \leq \frac{w(t) - w_r(t)}{w_l(t) - w_r(t)} \leq 1 \qquad & \\%\textrm{ (Road boundary constraint)} \\ \theta_{min} \leq \theta({t_i}) \leq \theta_{max } \qquad & \\%\textrm{ (Heading deviation constraint)}\\ v_{x,min} \leq v_x(t) \leq v_{x,max} \qquad & \\%\textrm{ (Longitudinal velocity constraint)} \\ v_{y,min} \leq v_y(t) \leq v_{y,max} \qquad & \\%\textrm{ (Lateral velocity constraint)} \\ \dot{\psi}_{min} \leq \dot{\psi}(t) \leq \dot{\psi}_{max} \qquad &\\% \textrm{ (Yaw rate constraint)} \\ 0 \leq s(t) \qquad \qquad &\\% \textrm{ (Evolution along centerline constraint)} \\ \alpha_{min}\leq \alpha (t) \leq \alpha_{max} \qquad & \\%\textrm{ (Slip angles constraint)}\\ \delta_{min} \leq \delta(t), \delta_d(t) \leq \delta_{max} \qquad &\\% \textrm{ (Steering angle constraint)}\\ F_{x,min} \leq F_{x,f}(t), F_{x,r}(t) \leq F_{x,max} \qquad &\\% \textrm{ (Longitudinal force constraint)}\\ \dot{\delta}_{min} \leq \dot{\delta(t)} \leq \dot{\delta}_{max} \qquad &\\% \textrm{ (Steering rate constraint)}\\ \operatorname{jerk_{x,min}} \leq \dot{F}_{x,f}(t), \dot{F}_{x,r}(t) \leq \operatorname{jerk_{x,max}} \qquad &\\% \textrm{ (Longitudinal force constraint)}\\ \xi_N \in \chi_f. \qquad & \\ \end{split} \end{equation}\end{split}\]

We do not enforce equal front and rear longitudinal forces, but other control allocation strategies can be added at a later stage. Nevertheless, large differences are penalised in the cost function. The stage cost \(l(x,u)\) is defined as:

\[\begin{split}\begin{align} l(\xi(t),v(t), p) = (\xi^*(t) - \xi^*_{ref}(t)) ^ T Q (\xi^*(t) - \xi^*_{ref}(t))\\ \;\;\; +\; v(t) ^ T R v(t) + \sigma (F_{x,r} - F_{x,f})^2, \nonumber \end{align}\end{split}\]

with \(Q \in\mathbb{R}^{10\times 10} \succeq 0, R \in\mathbb{R}^{3\times 3} \succ 0\), \(\sigma \in\mathbb{R} \geq 0\)

The tracking is equivalent to keeping \(w\) and \(\theta\) close to zero. The coarse planner provides a velocity profile which helps to keep the optimisation horizon shorter. Another benefit of the track-centered formulation is the position constraint which can be simplified to the variable box constraints. This makes tracking the local racing line within a given corridor simpler. Below is a sketch of how such problem formulation can be implemented in PolyMPC

#include "solvers/sqp_base.hpp"
#include "solvers/box_admm.hpp"
#include "polynomials/ebyshev.hpp"
#include "control/continuous_ocp.hpp"
#include "control/mpc_wrapper.hpp"
#include "polynomials/splines.hpp"

#include "models/car_model.hpp"

#define test_POLY_ORDER 5
#define test_NUM_SEG    2

using namespace Eigen;


/** choose the trajectory parametrisation and numerical quadratures */
using Polynomial = polympc::Chebyshev<test_POLY_ORDER, polympc::GAUSS_LOBATTO, double>;
using Approximation = polympc::Spline<Polynomial, test_NUM_SEG>;

POLYMPC_FORWARD_DECLARATION(/*Name*/ CarOCP, /*NX*/ 10, /*NU*/ 3, /*NP*/ 0, /*ND*/ 0, /*NG*/0, /*TYPE*/ double)
class RacingOCP : public ContinuousOCP<RacingOCP, Approximation, SPARSE>
{
public:

    RacingOCP()
    {	
	// initialise curvature somehow
        m_data_coeff.resize(20, 5);
	for (int i  = 0; i < 5; i++) //Straight line
	{
            m_data_coeff.row(4 * i)     << m_spline_length * i, 0, 0, 0, 0;
	    m_data_coeff.row(4 * i + 1) << m_spline_length * i, 1, 0, 0, 0;
	    m_data_coeff.row(4 * i + 2) << m_spline_length * i, 0, 0, 0, 0;
	    m_data_coeff.row(4 * i + 3) << m_spline_length * i, 0, 0, 0, 0;
	}

	//Define the Cost Function Matrices
	ScaleU << 1.0,  3000.0, 3000.0;
	Q  << 7.0, 0.0, 0.0, 0.0, 10.0, 100.0, 5.0, 0.0, 0.5, 0.5;
	R  << 1.0, 0.25, 0.25;
	QN << 7.0, 0.0, 0.0, 0.0, 10.0, 100.0, 5.0, 0.0, 0.5, 0.5;

	//For tracking a velocity profile
        m_velocity_coeff.resize(20, 2);
	m_velocity_coeff.setZero();
	for (int i  = 0; i < 5; i++) // Constant reference speed at 1m/s
	{
		m_velocity_coeff.row(4 * i)     << m_spline_length * i, 1; // default = 1m/s
		m_velocity_coeff.row(4 * i + 1) << m_spline_length * i, 0;
		m_velocity_coeff.row(4 * i + 2) << m_spline_length * i, 0;
		m_velocity_coeff.row(4 * i + 3) << m_spline_length * i, 0;
	}
	velocity_spline.segment_length() = m_spline_length;
	velocity_spline.coefficients() = Eigen::Map<Eigen::Matrix<double, 4, Eigen::Dynamic>>(m_velocity_coeff.col(1).data(), 4, m_velocity_coeff.rows() / 4);

    }
    ~CarOCP() = default;

    scalar_t m_spline_length{ 12.5 };
    //The Data below are is follows [Xc,Yc,Psic,Kappac]
    MatrixXd m_data_coeff;

    Eigen::Matrix<scalar_t, 3, 1> ScaleU;
    Eigen::Matrix<scalar_t, 10, 1> Q_, QN_;
    Eigen::Matrix<scalar_t, 3, 1> R_;

    //For tracking a velocity profile
    polympc::EquidistantCubicSpline<scalar_t> velocity_spline; //For tracking the velocity
    MatrixXd m_velocity_coeff;

    //inverse of steering delay 
    scalar_t m_delay_inverse{20.0};

    template<typename T>
    inline void dynamics_impl(const Eigen::Ref<const state_t<T>>& x, const Eigen::Ref<const control_t<T>>& u,
			      const Eigen::Ref<const parameter_t<T>>& p, const Eigen::Ref<const static_parameter_t> &d,
                              const T &t, Eigen::Ref<state_t<T>> xdot) const noexcept
    {
        //x = [vx,vy,r,s,w,theta, steering, Fxf, Fxr]
        //u = [steering_rate, Fxf_rate, Fxr_rate]
        Dynamics<T>(x.template head<6>(), x.template tail<3>().cwiseProduct(ScaleU), p, xdot.template head<6>());
        
        //Steering delay
        xdot.template tail<2>() = u.template tail<2>(); //Fxf rate, Fxr rate
        xdot(6) = u(0); // steering
        xdot(7) = (T)(m_delay_inverse) * (-x(7) + x(6));      // delayed steering

    }

    template<typename T>
    inline void lagrange_term_impl(const Eigen::Ref<const state_t<T>>& x, const Eigen::Ref<const control_t<T>>& u,
				   const Eigen::Ref<const parameter_t<T>>& p, const Eigen::Ref<const static_parameter_t>& d,
				   const scalar_t &t, T &lagrange) noexcept
    {
         //x = [v-v_ref, vy, phi_dot, s, w, theta]
	 //u = [steering, Fxf, Fxr]
	 state_t<T> x_aug{ x };

         T v_ref;
         v_ref = (T)(velocity_spline.eval(x(3))); //For tracking reference velocity profile
	 x_aug(0) = x(0) - v_ref; // (vx - v_ref)

         lagrange = x_aug.dot(x_aug.cwiseProduct(Q_)) + u.dot(u.cwiseProduct(R_))
	            + (x(8) - x(9)) * (x(8) - x(9)) * (T)(100.0); //Last two: differences between front and rear throttle

    }

    template<typename T>
    inline void mayer_term_impl(const Eigen::Ref<const state_t<T>>& x, const Eigen::Ref<const control_t<T>>& u,
				const Eigen::Ref<const parameter_t<T>>& p, const Eigen::Ref<const static_parameter_t>& d,
				const scalar_t &t, T &mayer) noexcept
    {
        state_t<T> x_aug{ x };

        T v_ref{ 13.2 };
	v_ref = (T)(velocity_spline.eval(x(3))); //For tracking reference velocity profile
	x_aug(0) = x(0) - v_ref; // (vx - v_ref)

	mayer = x_aug.dot(x_aug.cwiseProduct(Q_)) + (x(8) - x(9)) * (x(8) - x(9)) * (T)(100.0);
    }

    void set_data(const Eigen::Ref<const data_t>& _data_new, const scalar_t& _spline_length) noexcept
    {
        m_data_coeff = _data_new;
	m_spline_length = _spline_length;
    }

    void set_vel_data(const Eigen::Ref<const data_t>& data, const double& spline_length) noexcept
    {
	velocity_spline.segment_length() = spline_length;
	velocity_spline.coefficients() = Eigen::Map<Eigen::Matrix<double, 4, Eigen::Dynamic>>(data.col(1).data(), 4, data.rows() / 4);
    }
};

Experiments

A few videos below demonstrate numerical simulation and embedded deployment of the path following controller. The first video shows an RViz visualition of the algorithm with a nominal racing car model. Dark green is an ego-car, light-green is the predicted trajectory with a prediction horizon length of 2 seconds. We also highlight a portion of the track accessible for the car vision system.

In the second setup, the algorithm runs along with the other navigation software in the processor-in-the-loop (PiL) simulation on a Speedgoat machine with a real-time operating system (QNX 7.1). This time, the predictive controller drives a high-fidelity racing car model available in the Roborace simulator.

The third video demonstrates the algorithm deployment on an embedded Nvidia Xavier platform. A small F1tenth prototype autonomously performs a double line change manoeuvre using stereo-cameras for navigation. Some timings for these experiments can found in the Benchmark subsection below.

Benchmarks

Platform

PC

Speedgoat

Nvidia Xavier

System

Windows

QNX 7.1

Linux

Compiler

Visual C++

qcc

gcc

OCP [ms]

6.21 / 7.9

10.7 / 11.37

15.43 / 18.21

Spline fitting [ms]

0.02 /0.03

0.03 / 0.037

0.053 / 0.061

Frame Transform [ms]

0.012/0.16

0.02 / 0.023

0.044 / 0.051