diff --git a/docs/modules/PathTracking/model_predictive_speed_and_steer_control/Model_predictive_speed_and_steering_control.rst b/docs/modules/PathTracking/model_predictive_speed_and_steer_control/Model_predictive_speed_and_steering_control.rst new file mode 100644 index 00000000..9cf57433 --- /dev/null +++ b/docs/modules/PathTracking/model_predictive_speed_and_steer_control/Model_predictive_speed_and_steering_control.rst @@ -0,0 +1,244 @@ + +Model predictive speed and steering control +------------------------------------------- + +.. figure:: https://github.com/AtsushiSakai/PythonRobotics/raw/master/PathTracking/model_predictive_speed_and_steer_control/animation.gif?raw=true + :alt: 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. + +- `Welcome to CVXPY 1.0 — CVXPY 1.0.6 + documentation `__ + +MPC modeling +~~~~~~~~~~~~ + +State vector is: + +.. math:: z = [x, y, v,\phi] + +\ x: x-position, y:y-position, v:velocity, φ: yaw angle + +Input vector is: + +.. math:: u = [a, \delta] + +\ a: accellation, δ: steering angle + +The MPC cotroller minimize this cost function for path tracking: + +.. math:: 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 + +.. math:: z_{t+1}=Az_t+Bu+C + +- Maximum steering speed + +.. math:: |u_{t+1}-u_{t}|`__ + +Vehicle model linearization +~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +Vehicle model is + +.. math:: \dot{x} = vcos(\phi) + +.. math:: \dot{y} = vsin((\phi) + +.. math:: \dot{v} = a + +.. math:: \dot{\phi} = \frac{vtan(\delta)}{L} + +ODE is + +.. math:: \dot{z} =\frac{\partial }{\partial z} z = f(z, u) = A'z+B'u + +where + +.. math:: + + \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*} + +.. math:: + + \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. + +.. math:: z_{k+1}=z_k+f(z_k,u_k)dt + +Using first degree Tayer expantion around zbar and ubar + +.. math:: z_{k+1}=z_k+(f(\bar{z},\bar{u})+A'z_k+B'u_k-A'\bar{z}-B'\bar{u})dt + +.. math:: z_{k+1}=(I + dtA')z_k+(dtB')u_k + (f(\bar{z},\bar{u})-A'\bar{z}-B'\bar{u})dt + +So, + +.. math:: z_{k+1}=Az_k+Bu_k +C + +where, + +.. math:: + + \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*} + +.. math:: + + \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*} + +.. math:: + + \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 `__ + +Reference +~~~~~~~~~ + +- `Vehicle Dynamics and Control \| Rajesh Rajamani \| + Springer `__ + +- `MPC Course Material - MPC Lab @ + UC-Berkeley `__