In this example we illustrate the simplicity of the high-level user interface on a vehicle optimal trajectory generation problem. In particular, we use a simple vehicle model described by a set of ordinary dierential equations (ODEs):

$$ \begin{align} \dot{x} = & \, v\cos(\theta) \\ \dot{y} = & \, v\sin(\theta) \\ \dot{v} = & \, F/m \\ \dot{\theta} = & \, (v\,s)/L. \end{align} $$

The model consists of four differential states: \(x\) and \(y\) are the Cartesian coordinates of the car, \(v\) is the linear velocity and is the heading angle. Next, there are two control inputs to the model: the acceleration force \(F\) and the steering torque \(s\). The two parameters are the car mass \(m = 1 \text{kg}\) and the wheel base which we assume to be \(L = 0.15m \).

The trajectory of the vehicle will be defined as an NLP. First, we define stage variable \(z\) by stacking the input and differential state variables

$$ \begin{align} z := [F, s, x, y, v, \theta]^T \end{align} $$

**Objective**

In this example the cost function is the same for all stages. We want to maximize progress in the \(y\) direction, with quadratic penalties on the inputs \(F\) and \(s\), i.e.

$$ \begin{align} f(z) = -100y + 0.1 F^2 + 0.01 s^2 \end{align} $$

The stage cost function is coded in MATLAB as the following function:

F = z (1);

s = z (2);

y = z (4);

f = -100*y + 0.1*F^2 + 0.01* s^2;

end

**Matrix equality constraints**

The matrix equality constraints in this example represent only the discretized dynamic equations of the vehicle using an explicit Runge-Kutta integrator of order 4. The vehicle dynamics defined above are represented by a function `continuous_dynamics` and the NLP constraint function \(c(.)\) as the function `dynamics`. Note
that the function `RK4` is included in the FORCES Pro client software.

F = z (1);

s = z (2);

v = z( 5 );

theta = z( 6 );

m = 1; % car mass

L = 0.15; % car inertia

xdot = [v * cos ( theta );

v * sin( theta );

F / m;

(v*s) / L];

end

function xnext = dynamics ( z )

x = z(3:6);

u = z(1:2);

% implements a RK4 integrator for the dynamics .

h = 0.1; % step size

xnext = RK4(x, u, @continuous_dynamics , h);

end

**Inequality constraints**

The maneuver is subjected to a set of constraints, involving both the simple bounds

$$ \begin{align} -5\text{N} &\leq F \leq 5\text{N},\\ -1\text{Nm} &\leq s \leq 1\text{Nm},\\ -3\text{m} &\leq x \leq 0\text{m},\\ 0\text{m} &\leq y \leq 3\text{m},\\ 0\text{m/s} &\leq v \leq 2\text{m/s},\\ 0\text{rad} &\leq \theta \leq \pi\text{rad}, \end{align} $$

as well the nonlinear nonconvex constraints

$$ \begin{align} 1\text{m}^2 &\leq x^2 + y^2 \leq 9\text{m}^2,\\ 0.9025\text{m}^2 &\leq (x+2)^2 + (y-2.5)^2. \end{align} $$

In this case, the nonlinear constraint function \(h(.)\) can be coded in MATLAB as follows.

x = z (3);

y = z (4);

h = [x^2 + y^2;

(x +2)^2 + (y -2.5)^2 ];

end

**Initial and final conditions**

The goal of the maneuver is to steer the vehicle from a set of initial conditions

$$ \begin{align} x_{\text{init}} = -2\text{m},\quad y_{\text{init}} = 0\text{m},\quad v_{\text{init}} = 0\text{m/s}, \quad \theta_{\text{init}} = 2.0944\text{rad} \end{align} $$

to another point in the state-space subjected to the final conditions

$$ \begin{align} v_{\text{final}} = 0\text{m/s},\quad \theta_{\text{final}} = 0\text{rad}. \end{align} $$

With the above defined MALTAB functions for objective, matrix equality and inequality functions, we can completely define the NLP formulation in the next code snippet. For this example, we chose to use N = 50 stages in the NLP.

nlp.N = 50; % horizon length

nlp.nvar = 6; % number of stage variables

nlp.neq = 4; % number of equality constraints

nlp.nh = 2; % number of inequality constraints

%% Objective function

nlp.objective = @objective;

%% Matrix equality constraints

nlp.eq = @dynamics;

nlp.E = [zeros(4, 2), eye( 4 )];

%% Inequality constraints

% upper/lower bounds lb <= x <= ub

% F s x y v theta

nlp.lb = [-5, -1, -3, 0 0 0 ];

nlp.ub = [+5, +1, 0, 3 2 +pi];

% Nonlinear inequalities hl <= h(x) <= hu

nlp.ineq = @inequalities;

nlp.hu = [9, +inf]';

nlp.hl = [1, 0.95^2]';

%% Initial and final conditions

nlp.xinitidx = 3:6;

nlp.xfinalidx = 5: 6;

The goal is to find a trajectory that steers the vehicle from point A to another standstill point while avoiding obstacles and maximizing the progress on the y-direction along the way. The trajectory should also be feasible with respect to the vehicle dynamics and its safety and physical limitations. The plot on the left shows the vehicle's trajectory in 2D space.

The plot on the bottom left shows the vehicle's velocity and steering angle over time, while the plot on the bottom rights shows the actuator commands. One can see that all constraints are respected.