# Path tracking¶

## move to a pose control¶

This is a simulation of moving to a pose control Ref:

## Pure pursuit tracking¶

Path tracking simulation with pure pursuit steering control and PID speed control. The red line is a target course, the green cross means the target point for pure pursuit control, the blue line is the tracking.

Ref:

## Stanley control¶

Path tracking simulation with Stanley steering control and PID speed control. Ref:

## Rear wheel feedback control¶

Path tracking simulation with rear wheel feedback steering control and PID speed control. Ref:

## Linear–quadratic regulator (LQR) steering control¶

Path tracking simulation with LQR steering control and PID speed control. Ref:

## Linear–quadratic regulator (LQR) speed and steering control¶

Path tracking simulation with LQR speed and steering control. Ref:

## Model predictive speed and steering control¶ Model predictive speed and steering control

code:

PythonRobotics/model_predictive_speed_and_steer_control.py at master · AtsushiSakai/PythonRobotics

This is a path tracking simulation using model predictive control (MPC).

The MPC controller controls vehicle speed and steering base on linealized model.

This code uses cvxpy as an optimization modeling tool.

### MPC modeling¶

State vector is:

$z = [x, y, v,\phi]$

x: x-position, y:y-position, v:velocity, φ: yaw angle

Input vector is:

$u = [a, \delta]$

a: accellation, δ: steering angle

The MPC cotroller minimize this cost function for path tracking:

$min\ Q_f(z_{T,ref}-z_{T})^2+Q\Sigma({z_{t,ref}-z_{t}})^2+R\Sigma{u_t}^2+R_d\Sigma({u_{t+1}-u_{t}})^2$

z_ref come from target path and speed.

subject to:

• Linearlied vehicle model
$z_{t+1}=Az_t+Bu+C$
• Maximum steering speed
$|u_{t+1}-u_{t}|<du_{max}$
• Maximum steering angle
$|u_{t}|<u_{max}$
• Initial state
$z_0 = z_{0,ob}$
• Maximum and minimum speed
$v_{min} < v_t < v_{max}$
• Maximum and minimum input
$u_{min} < u_t < u_{max}$

This is implemented at

PythonRobotics/model_predictive_speed_and_steer_control.py at f51a73f47cb922a12659f8ce2d544c347a2a8156 · AtsushiSakai/PythonRobotics

### Vehicle model linearization¶

Vehicle model is

$\dot{x} = vcos(\phi)$
$\dot{y} = vsin((\phi)$
$\dot{v} = a$
$\dot{\phi} = \frac{vtan(\delta)}{L}$

ODE is

$\dot{z} =\frac{\partial }{\partial z} z = f(z, u) = A'z+B'u$

where

$$\begin{equation*} A' = \begin{bmatrix} \frac{\partial }{\partial x}vcos(\phi) & \frac{\partial }{\partial y}vcos(\phi) & \frac{\partial }{\partial v}vcos(\phi) & \frac{\partial }{\partial \phi}vcos(\phi)\\ \frac{\partial }{\partial x}vsin(\phi) & \frac{\partial }{\partial y}vsin(\phi) & \frac{\partial }{\partial v}vsin(\phi) & \frac{\partial }{\partial \phi}vsin(\phi)\\ \frac{\partial }{\partial x}a& \frac{\partial }{\partial y}a& \frac{\partial }{\partial v}a& \frac{\partial }{\partial \phi}a\\ \frac{\partial }{\partial x}\frac{vtan(\delta)}{L}& \frac{\partial }{\partial y}\frac{vtan(\delta)}{L}& \frac{\partial }{\partial v}\frac{vtan(\delta)}{L}& \frac{\partial }{\partial \phi}\frac{vtan(\delta)}{L}\\ \end{bmatrix} \\ = \begin{bmatrix} 0 & 0 & cos(\bar{\phi}) & -\bar{v}sin(\bar{\phi})\\ 0 & 0 & sin(\bar{\phi}) & \bar{v}cos(\bar{\phi}) \\ 0 & 0 & 0 & 0 \\ 0 & 0 &\frac{tan(\bar{\delta})}{L} & 0 \\ \end{bmatrix} \end{equation*}$$

$$\begin{equation*} B' = \begin{bmatrix} \frac{\partial }{\partial a}vcos(\phi) & \frac{\partial }{\partial \delta}vcos(\phi)\\ \frac{\partial }{\partial a}vsin(\phi) & \frac{\partial }{\partial \delta}vsin(\phi)\\ \frac{\partial }{\partial a}a & \frac{\partial }{\partial \delta}a\\ \frac{\partial }{\partial a}\frac{vtan(\delta)}{L} & \frac{\partial }{\partial \delta}\frac{vtan(\delta)}{L}\\ \end{bmatrix} \\ = \begin{bmatrix} 0 & 0 \\ 0 & 0 \\ 1 & 0 \\ 0 & \frac{\bar{v}}{Lcos^2(\bar{\delta})} \\ \end{bmatrix} \end{equation*}$$

You can get a discrete-time mode with Forward Euler Discretization with sampling time dt.

$z_{k+1}=z_k+f(z_k,u_k)dt$

Using first degree Tayer expantion around zbar and ubar

$z_{k+1}=z_k+(f(\bar{z},\bar{u})+A'z_k+B'u_k-A'\bar{z}-B'\bar{u})dt$
$z_{k+1}=(I + dtA')z_k+(dtB')u_k + (f(\bar{z},\bar{u})-A'\bar{z}-B'\bar{u})dt$

So,

$z_{k+1}=Az_k+Bu_k +C$

where,

$$\begin{equation*} A = (I + dtA')\\ = \begin{bmatrix} 1 & 0 & cos(\bar{\phi})dt & -\bar{v}sin(\bar{\phi})dt\\ 0 & 1 & sin(\bar{\phi})dt & \bar{v}cos(\bar{\phi})dt \\ 0 & 0 & 1 & 0 \\ 0 & 0 &\frac{tan(\bar{\delta})}{L}dt & 1 \\ \end{bmatrix} \end{equation*}$$

$$\begin{equation*} B = dtB'\\ = \begin{bmatrix} 0 & 0 \\ 0 & 0 \\ dt & 0 \\ 0 & \frac{\bar{v}}{Lcos^2(\bar{\delta})}dt \\ \end{bmatrix} \end{equation*}$$

$$\begin{equation*} C = (f(\bar{z},\bar{u})-A'\bar{z}-B'\bar{u})dt\\ = dt( \begin{bmatrix} \bar{v}cos(\bar{\phi})\\ \bar{v}sin(\bar{\phi}) \\ \bar{a}\\ \frac{\bar{v}tan(\bar{\delta})}{L}\\ \end{bmatrix} - \begin{bmatrix} \bar{v}cos(\bar{\phi})-\bar{v}sin(\bar{\phi})\bar{\phi}\\ \bar{v}sin(\bar{\phi})+\bar{v}cos(\bar{\phi})\bar{\phi}\\ 0\\ \frac{\bar{v}tan(\bar{\delta})}{L}\\ \end{bmatrix} - \begin{bmatrix} 0\\ 0 \\ \bar{a}\\ \frac{\bar{v}\bar{\delta}}{Lcos^2(\bar{\delta})}\\ \end{bmatrix} )\\ = \begin{bmatrix} \bar{v}sin(\bar{\phi})\bar{\phi}dt\\ -\bar{v}cos(\bar{\phi})\bar{\phi}dt\\ 0\\ -\frac{\bar{v}\bar{\delta}}{Lcos^2(\bar{\delta})}dt\\ \end{bmatrix} \end{equation*}$$

This equation is implemented at

PythonRobotics/model_predictive_speed_and_steer_control.py at eb6d1cbe6fc90c7be9210bf153b3a04f177cc138 · AtsushiSakai/PythonRobotics

Ref:

## Nonlinear Model Predictive Control with C-GMRES¶

from IPython.display import Image
Image(filename="Figure_4.png",width=600) Image(filename="Figure_1.png",width=600) Image(filename="Figure_2.png",width=600) Image(filename="Figure_3.png",width=600)  gif

### Mathematical Formulation¶

Motion model is

$\dot{x}=vcos\theta$
$\dot{y}=vsin\theta$
$\dot{\theta}=\frac{v}{WB}sin(u_{\delta})$

(tan is not good for optimization)

$\dot{v}=u_a$

Cost function is

$J=\frac{1}{2}(u_a^2+u_{\delta}^2)-\phi_a d_a-\phi_\delta d_\delta$

Input constraints are

$|u_a| \leq u_{a,max}$
$|u_\delta| \leq u_{\delta,max}$

So, Hamiltonian　is

$\begin{split}J=\frac{1}{2}(u_a^2+u_{\delta}^2)-\phi_a d_a-\phi_\delta d_\delta\\ +\lambda_1vcos\theta+\lambda_2vsin\theta+\lambda_3\frac{v}{WB}sin(u_{\delta})+\lambda_4u_a\\ +\rho_1(u_a^2+d_a^2+u_{a,max}^2)+\rho_2(u_\delta^2+d_\delta^2+u_{\delta,max}^2)\end{split}$

Partial differential equations of the Hamiltonian are:

$$\begin{equation*} \frac{\partial H}{\partial \bf{x}}=\\ \begin{bmatrix} \frac{\partial H}{\partial x}= 0&\\ \frac{\partial H}{\partial y}= 0&\\ \frac{\partial H}{\partial \theta}= -\lambda_1vsin\theta+\lambda_2vcos\theta&\\ \frac{\partial H}{\partial v}=-\lambda_1cos\theta+\lambda_2sin\theta+\lambda_3\frac{1}{WB}sin(u_{\delta})&\\ \end{bmatrix} \\ \end{equation*}$$

$$\begin{equation*} \frac{\partial H}{\partial \bf{u}}=\\ \begin{bmatrix} \frac{\partial H}{\partial u_a}= u_a+\lambda_4+2\rho_1u_a&\\ \frac{\partial H}{\partial u_\delta}= u_\delta+\lambda_3\frac{v}{WB}cos(u_{\delta})+2\rho_2u_\delta&\\ \frac{\partial H}{\partial d_a}= -\phi_a+2\rho_1d_a&\\ \frac{\partial H}{\partial d_\delta}=-\phi_\delta+2\rho_2d_\delta&\\ \end{bmatrix} \\ \end{equation*}$$