Vehicle

Example

Vehicle/

Problem Description

The state vector of the vehicle is $x = [y,z , v ,\theta]^{T}\in\mathbb{R}^{4}$ , where $(y,z)$ is the position coordinate, $v$ is the velocity, and $\theta$ is the heading angle.
The input vector is $u=[F,s]^T$ , where $F$ is the acceleration force and $s$ is the steering torque. The control input is bounded by $[-5,-1]^T \leq u \leq [5,1]^T$ .
The vehicle starts from position $(-2,0)$ . It's aim is to reach $(3.5,2)$ while avoiding two obstacles with positions $(0,0)$ and $(2,2)$ . These constraints are formulated as $y^2+z^2\geq 1$ and $(y-2)^2+(z-2)^2\geq 1$ .
The dynamics of the vehicle are given by the following equations ( $m$ : mass and $I$ : moment of inertia):

$\begin{align} &\dot{y} = v\cos(\theta)\\ &\dot{z} = v\sin(\theta)\\ &\dot{v} = F/m\\ &\dot{\theta} = s/I \end{align}$

The nonlinear constraints are transfered into $y^2+z^2 = C_{1}$ and $(y-2)^2+(z-2)^2 = C_{2}$ with $C_{1}\geq 1$ and $C_{2}\geq 1$ .
The state constraints $C_{1}\geq 1$ and $C_{2}\geq 1$ are softened by introducing a positive slack variable.

Closed-loop simulation result:

Example

Vehicle/NMPC_Problem_Formulation.m