mirror of
https://github.com/AtsushiSakai/PythonRobotics.git
synced 2026-01-14 08:17:59 -05:00
clean up doc
This commit is contained in:
Atsushi Sakai
@@ -35,10 +35,16 @@
|
||||
"### MPC modeling\n",
|
||||
"\n",
|
||||
"State vector is:\n",
|
||||
"$$ z = [x, y, v,\\phi]$$ x: x-position, y:y-position, v:velocity, φ: yaw angle\n",
|
||||
"\n",
|
||||
"$$ z = [x, y, v,\\phi]$$\n",
|
||||
"\n",
|
||||
"x: x-position, y:y-position, v:velocity, φ: yaw angle\n",
|
||||
"\n",
|
||||
"Input vector is:\n",
|
||||
"$$ u = [a, \\delta]$$ a: accellation, δ: steering angle\n",
|
||||
"\n",
|
||||
"$$ u = [a, \\delta]$$\n",
|
||||
"\n",
|
||||
"a: accellation, δ: steering angle\n",
|
||||
"\n"
|
||||
]
|
||||
},
|
||||
@@ -103,9 +109,13 @@
|
||||
"\n",
|
||||
"\n",
|
||||
"Vehicle model is \n",
|
||||
"\n",
|
||||
"$$ \\dot{x} = vcos(\\phi)$$\n",
|
||||
"\n",
|
||||
"$$ \\dot{y} = vsin((\\phi)$$\n",
|
||||
"\n",
|
||||
"$$ \\dot{v} = a$$\n",
|
||||
"\n",
|
||||
"$$ \\dot{\\phi} = \\frac{vtan(\\delta)}{L}$$\n",
|
||||
"\n",
|
||||
"\n",
|
||||
@@ -128,7 +138,7 @@
|
||||
"source": [
|
||||
"where\n",
|
||||
"\n",
|
||||
"\\begin{equation*}\n",
|
||||
"$\\begin{equation*}\n",
|
||||
"A' =\n",
|
||||
"\\begin{bmatrix}\n",
|
||||
"\\frac{\\partial }{\\partial x}vcos(\\phi) & \n",
|
||||
@@ -156,7 +166,7 @@
|
||||
"0 & 0 & 0 & 0 \\\\\n",
|
||||
"0 & 0 &\\frac{tan(\\bar{\\delta})}{L} & 0 \\\\\n",
|
||||
"\\end{bmatrix}\n",
|
||||
"\\end{equation*}\n",
|
||||
"\\end{equation*}$\n",
|
||||
"\n"
|
||||
]
|
||||
},
|
||||
@@ -164,7 +174,7 @@
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"\\begin{equation*}\n",
|
||||
"$\\begin{equation*}\n",
|
||||
"B' =\n",
|
||||
"\\begin{bmatrix}\n",
|
||||
"\\frac{\\partial }{\\partial a}vcos(\\phi) &\n",
|
||||
@@ -184,7 +194,7 @@
|
||||
"1 & 0 \\\\\n",
|
||||
"0 & \\frac{\\bar{v}}{Lcos^2(\\bar{\\delta})} \\\\\n",
|
||||
"\\end{bmatrix}\n",
|
||||
"\\end{equation*}\n",
|
||||
"\\end{equation*}$\n",
|
||||
"\n"
|
||||
]
|
||||
},
|
||||
@@ -228,7 +238,7 @@
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"\\begin{equation*}\n",
|
||||
"$\\begin{equation*}\n",
|
||||
"B = dtB'\\\\\n",
|
||||
"=\n",
|
||||
"\\begin{bmatrix} \n",
|
||||
@@ -237,14 +247,14 @@
|
||||
"dt & 0 \\\\\n",
|
||||
"0 & \\frac{\\bar{v}}{Lcos^2(\\bar{\\delta})}dt \\\\\n",
|
||||
"\\end{bmatrix}\n",
|
||||
"\\end{equation*}"
|
||||
"\\end{equation*}$"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"\\begin{equation*}\n",
|
||||
"$\\begin{equation*}\n",
|
||||
"C = (f(\\bar{z},\\bar{u})-A'\\bar{z}-B'\\bar{u})dt\\\\\n",
|
||||
"= dt(\n",
|
||||
"\\begin{bmatrix} \n",
|
||||
@@ -275,7 +285,7 @@
|
||||
"0\\\\\n",
|
||||
"-\\frac{\\bar{v}\\bar{\\delta}}{Lcos^2(\\bar{\\delta})}dt\\\\\n",
|
||||
"\\end{bmatrix}\n",
|
||||
"\\end{equation*}"
|
||||
"\\end{equation*}$"
|
||||
]
|
||||
},
|
||||
{
|
||||
|
||||
@@ -29,13 +29,13 @@ State vector is:
|
||||
|
||||
.. math:: z = [x, y, v,\phi]
|
||||
|
||||
\ x: x-position, y:y-position, v:velocity, φ: yaw angle
|
||||
x: x-position, y:y-position, v:velocity, φ: yaw angle
|
||||
|
||||
Input vector is:
|
||||
|
||||
.. math:: u = [a, \delta]
|
||||
|
||||
\ a: accellation, δ: steering angle
|
||||
a: accellation, δ: steering angle
|
||||
|
||||
The MPC cotroller minimize this cost function for path tracking:
|
||||
|
||||
@@ -94,57 +94,9 @@ ODE is
|
||||
|
||||
where
|
||||
|
||||
:raw-latex:`\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*} 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*}`
|
||||
|
||||
:raw-latex:`\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*}`
|
||||
: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.
|
||||
@@ -165,49 +117,9 @@ 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*}`
|
||||
|
||||
:raw-latex:`\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*} B = dtB'\\ = \begin{bmatrix} 0 & 0 \\ 0 & 0 \\ dt & 0 \\ 0 & \frac{\bar{v}}{Lcos^2(\bar{\delta})}dt \\ \end{bmatrix} \end{equation*}`
|
||||
|
||||
:raw-latex:`\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*}`
|
||||
: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
|
||||
|
||||
|
||||
Reference in New Issue
Block a user