Path tracking

move to a pose control

This is a simulation of moving to a pose control

2

Ref:

Pure pursuit tracking

Path tracking simulation with pure pursuit steering control and PID speed control.

3

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.

4

Ref:

Rear wheel feedback control

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

5

Ref:

Linear–quadratic regulator (LQR) steering control

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

6

Ref:

Linear–quadratic regulator (LQR) speed and steering control

Path tracking simulation with LQR speed and steering control.

7

Ref:

Model predictive speed and steering control

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

Nonlinear Model Predictive Control with C-GMRES

from IPython.display import Image
Image(filename="Figure_4.png",width=600)
../_images/cgmres_nmpc_1_0.png
Image(filename="Figure_1.png",width=600)
../_images/cgmres_nmpc_2_0.png
Image(filename="Figure_2.png",width=600)
../_images/cgmres_nmpc_3_0.png
Image(filename="Figure_3.png",width=600)
../_images/cgmres_nmpc_4_0.png
gif

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*}\)