mirror of
https://github.com/AtsushiSakai/PythonRobotics.git
synced 2026-01-13 03:17:59 -05:00
add rst
This commit is contained in:
Atsushi Sakai
@@ -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 <https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py>`__
|
||||
|
||||
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 <http://www.cvxpy.org/>`__
|
||||
|
||||
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}|<du_{max}
|
||||
|
||||
- Maximum steering angle
|
||||
|
||||
.. math:: |u_{t}|<u_{max}
|
||||
|
||||
- Initial state
|
||||
|
||||
.. math:: z_0 = z_{0,ob}
|
||||
|
||||
- Maximum and minimum speed
|
||||
|
||||
.. math:: v_{min} < v_t < v_{max}
|
||||
|
||||
- Maximum and minimum input
|
||||
|
||||
.. math:: u_{min} < u_t < u_{max}
|
||||
|
||||
This is implemented at
|
||||
|
||||
`PythonRobotics/model_predictive_speed_and_steer_control.py at
|
||||
f51a73f47cb922a12659f8ce2d544c347a2a8156 ·
|
||||
AtsushiSakai/PythonRobotics <https://github.com/AtsushiSakai/PythonRobotics/blob/f51a73f47cb922a12659f8ce2d544c347a2a8156/PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py#L247-L301>`__
|
||||
|
||||
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 <https://github.com/AtsushiSakai/PythonRobotics/blob/eb6d1cbe6fc90c7be9210bf153b3a04f177cc138/PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py#L80-L102>`__
|
||||
|
||||
Reference
|
||||
~~~~~~~~~
|
||||
|
||||
- `Vehicle Dynamics and Control \| Rajesh Rajamani \|
|
||||
Springer <http://www.springer.com/us/book/9781461414322>`__
|
||||
|
||||
- `MPC Course Material - MPC Lab @
|
||||
UC-Berkeley <http://www.mpc.berkeley.edu/mpc-course-material>`__
|
||||
Reference in New Issue
Block a user