mirror of
https://github.com/AtsushiSakai/PythonRobotics.git
synced 2026-04-22 03:00:22 -04:00
Fix clean up other docs (#580)
* update docs * update docs * update docs * update docs
This commit is contained in:
Atsushi Sakai
Binary file not shown.
|
Before Width: | Height: | Size: 33 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 30 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 25 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 19 KiB |
File diff suppressed because one or more lines are too long
@@ -1,333 +0,0 @@
|
||||
{
|
||||
"cells": [
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"source": [
|
||||
"## Model predictive speed and steering control\n",
|
||||
"\n",
|
||||
"\n"
|
||||
],
|
||||
"metadata": {}
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"source": [
|
||||
"\n",
|
||||
"\n",
|
||||
"code:\n",
|
||||
"\n",
|
||||
"[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)\n",
|
||||
"\n",
|
||||
"This is a path tracking simulation using model predictive control (MPC).\n",
|
||||
"\n",
|
||||
"The MPC controller controls vehicle speed and steering base on linealized model.\n",
|
||||
"\n",
|
||||
"This code uses cvxpy as an optimization modeling tool.\n",
|
||||
"\n",
|
||||
"- [Welcome to CVXPY 1\\.0 — CVXPY 1\\.0\\.6 documentation](http://www.cvxpy.org/)"
|
||||
],
|
||||
"metadata": {}
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"source": [
|
||||
"### MPC modeling\n",
|
||||
"\n",
|
||||
"State vector is:\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",
|
||||
"\n",
|
||||
"$$ u = [a, \\delta]$$\n",
|
||||
"\n",
|
||||
"a: accellation, δ: steering angle\n",
|
||||
"\n"
|
||||
],
|
||||
"metadata": {}
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"source": [
|
||||
"The MPC cotroller minimize this cost function for path tracking:\n",
|
||||
"\n",
|
||||
"$$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$$\n",
|
||||
"\n",
|
||||
"z_ref come from target path and speed."
|
||||
],
|
||||
"metadata": {}
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"source": [
|
||||
"subject to:\n",
|
||||
"\n",
|
||||
"- Linearlied vehicle model\n",
|
||||
"\n",
|
||||
"$$z_{t+1}=Az_t+Bu+C$$\n",
|
||||
"\n",
|
||||
"- Maximum steering speed\n",
|
||||
"\n",
|
||||
"$$|u_{t+1}-u_{t}|<du_{max}$$\n",
|
||||
"\n",
|
||||
"- Maximum steering angle\n",
|
||||
"\n",
|
||||
"$$|u_{t}|<u_{max}$$\n",
|
||||
"\n",
|
||||
"- Initial state\n",
|
||||
"\n",
|
||||
"$$z_0 = z_{0,ob}$$\n",
|
||||
"\n",
|
||||
"- Maximum and minimum speed\n",
|
||||
"\n",
|
||||
"$$v_{min} < v_t < v_{max}$$\n",
|
||||
"\n",
|
||||
"- Maximum and minimum input\n",
|
||||
"\n",
|
||||
"$$u_{min} < u_t < u_{max}$$\n",
|
||||
"\n"
|
||||
],
|
||||
"metadata": {}
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"source": [
|
||||
"This is implemented at \n",
|
||||
"\n",
|
||||
"[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)"
|
||||
],
|
||||
"metadata": {}
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"source": [
|
||||
"### Vehicle model linearization\n",
|
||||
"\n",
|
||||
"\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",
|
||||
"\n"
|
||||
],
|
||||
"metadata": {}
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"source": [
|
||||
"ODE is\n",
|
||||
"\n",
|
||||
"$$ \\dot{z} =\\frac{\\partial }{\\partial z} z = f(z, u) = A'z+B'u$$\n",
|
||||
"\n"
|
||||
],
|
||||
"metadata": {}
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"source": [
|
||||
"where\n",
|
||||
"\n",
|
||||
"$\\begin{equation*}\n",
|
||||
"A' =\n",
|
||||
"\\begin{bmatrix}\n",
|
||||
"\\frac{\\partial }{\\partial x}vcos(\\phi) & \n",
|
||||
"\\frac{\\partial }{\\partial y}vcos(\\phi) & \n",
|
||||
"\\frac{\\partial }{\\partial v}vcos(\\phi) &\n",
|
||||
"\\frac{\\partial }{\\partial \\phi}vcos(\\phi)\\\\\n",
|
||||
"\\frac{\\partial }{\\partial x}vsin(\\phi) & \n",
|
||||
"\\frac{\\partial }{\\partial y}vsin(\\phi) & \n",
|
||||
"\\frac{\\partial }{\\partial v}vsin(\\phi) &\n",
|
||||
"\\frac{\\partial }{\\partial \\phi}vsin(\\phi)\\\\\n",
|
||||
"\\frac{\\partial }{\\partial x}a& \n",
|
||||
"\\frac{\\partial }{\\partial y}a& \n",
|
||||
"\\frac{\\partial }{\\partial v}a&\n",
|
||||
"\\frac{\\partial }{\\partial \\phi}a\\\\\n",
|
||||
"\\frac{\\partial }{\\partial x}\\frac{vtan(\\delta)}{L}& \n",
|
||||
"\\frac{\\partial }{\\partial y}\\frac{vtan(\\delta)}{L}& \n",
|
||||
"\\frac{\\partial }{\\partial v}\\frac{vtan(\\delta)}{L}&\n",
|
||||
"\\frac{\\partial }{\\partial \\phi}\\frac{vtan(\\delta)}{L}\\\\\n",
|
||||
"\\end{bmatrix}\n",
|
||||
"\\\\\n",
|
||||
" =\n",
|
||||
"\\begin{bmatrix}\n",
|
||||
"0 & 0 & cos(\\bar{\\phi}) & -\\bar{v}sin(\\bar{\\phi})\\\\\n",
|
||||
"0 & 0 & sin(\\bar{\\phi}) & \\bar{v}cos(\\bar{\\phi}) \\\\\n",
|
||||
"0 & 0 & 0 & 0 \\\\\n",
|
||||
"0 & 0 &\\frac{tan(\\bar{\\delta})}{L} & 0 \\\\\n",
|
||||
"\\end{bmatrix}\n",
|
||||
"\\end{equation*}$\n",
|
||||
"\n"
|
||||
],
|
||||
"metadata": {}
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"source": [
|
||||
"$\\begin{equation*}\n",
|
||||
"B' =\n",
|
||||
"\\begin{bmatrix}\n",
|
||||
"\\frac{\\partial }{\\partial a}vcos(\\phi) &\n",
|
||||
"\\frac{\\partial }{\\partial \\delta}vcos(\\phi)\\\\\n",
|
||||
"\\frac{\\partial }{\\partial a}vsin(\\phi) &\n",
|
||||
"\\frac{\\partial }{\\partial \\delta}vsin(\\phi)\\\\\n",
|
||||
"\\frac{\\partial }{\\partial a}a &\n",
|
||||
"\\frac{\\partial }{\\partial \\delta}a\\\\\n",
|
||||
"\\frac{\\partial }{\\partial a}\\frac{vtan(\\delta)}{L} &\n",
|
||||
"\\frac{\\partial }{\\partial \\delta}\\frac{vtan(\\delta)}{L}\\\\\n",
|
||||
"\\end{bmatrix}\n",
|
||||
"\\\\\n",
|
||||
" =\n",
|
||||
"\\begin{bmatrix}\n",
|
||||
"0 & 0 \\\\\n",
|
||||
"0 & 0 \\\\\n",
|
||||
"1 & 0 \\\\\n",
|
||||
"0 & \\frac{\\bar{v}}{Lcos^2(\\bar{\\delta})} \\\\\n",
|
||||
"\\end{bmatrix}\n",
|
||||
"\\end{equation*}$\n",
|
||||
"\n"
|
||||
],
|
||||
"metadata": {}
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"source": [
|
||||
"You can get a discrete-time mode with Forward Euler Discretization with sampling time dt.\n",
|
||||
"\n",
|
||||
"$$z_{k+1}=z_k+f(z_k,u_k)dt$$\n",
|
||||
"\n",
|
||||
"Using first degree Tayer expantion around zbar and ubar\n",
|
||||
"$$z_{k+1}=z_k+(f(\\bar{z},\\bar{u})+A'z_k+B'u_k-A'\\bar{z}-B'\\bar{u})dt$$\n",
|
||||
"\n",
|
||||
"$$z_{k+1}=(I + dtA')z_k+(dtB')u_k + (f(\\bar{z},\\bar{u})-A'\\bar{z}-B'\\bar{u})dt$$\n"
|
||||
],
|
||||
"metadata": {}
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"source": [
|
||||
"So, \n",
|
||||
"\n",
|
||||
"$$z_{k+1}=Az_k+Bu_k +C$$\n",
|
||||
"\n",
|
||||
"where,\n",
|
||||
"\n",
|
||||
"$$\\begin{equation*}\n",
|
||||
"A = (I + dtA')\\\\\n",
|
||||
"=\n",
|
||||
"\\begin{bmatrix} \n",
|
||||
"1 & 0 & cos(\\bar{\\phi})dt & -\\bar{v}sin(\\bar{\\phi})dt\\\\\n",
|
||||
"0 & 1 & sin(\\bar{\\phi})dt & \\bar{v}cos(\\bar{\\phi})dt \\\\\n",
|
||||
"0 & 0 & 1 & 0 \\\\\n",
|
||||
"0 & 0 &\\frac{tan(\\bar{\\delta})}{L}dt & 1 \\\\\n",
|
||||
"\\end{bmatrix}\n",
|
||||
"\\end{equation*}$$"
|
||||
],
|
||||
"metadata": {}
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"source": [
|
||||
"$$\\begin{equation*}\n",
|
||||
"B = dtB'\\\\\n",
|
||||
"=\n",
|
||||
"\\begin{bmatrix} \n",
|
||||
"0 & 0 \\\\\n",
|
||||
"0 & 0 \\\\\n",
|
||||
"dt & 0 \\\\\n",
|
||||
"0 & \\frac{\\bar{v}}{Lcos^2(\\bar{\\delta})}dt \\\\\n",
|
||||
"\\end{bmatrix}\n",
|
||||
"\\end{equation*}$$"
|
||||
],
|
||||
"metadata": {}
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"source": [
|
||||
"$$\\begin{equation*}\n",
|
||||
"C = (f(\\bar{z},\\bar{u})-A'\\bar{z}-B'\\bar{u})dt\\\\\n",
|
||||
"= dt(\n",
|
||||
"\\begin{bmatrix} \n",
|
||||
"\\bar{v}cos(\\bar{\\phi})\\\\\n",
|
||||
"\\bar{v}sin(\\bar{\\phi}) \\\\\n",
|
||||
"\\bar{a}\\\\\n",
|
||||
"\\frac{\\bar{v}tan(\\bar{\\delta})}{L}\\\\\n",
|
||||
"\\end{bmatrix}\n",
|
||||
"-\n",
|
||||
"\\begin{bmatrix} \n",
|
||||
"\\bar{v}cos(\\bar{\\phi})-\\bar{v}sin(\\bar{\\phi})\\bar{\\phi}\\\\\n",
|
||||
"\\bar{v}sin(\\bar{\\phi})+\\bar{v}cos(\\bar{\\phi})\\bar{\\phi}\\\\\n",
|
||||
"0\\\\\n",
|
||||
"\\frac{\\bar{v}tan(\\bar{\\delta})}{L}\\\\\n",
|
||||
"\\end{bmatrix}\n",
|
||||
"-\n",
|
||||
"\\begin{bmatrix} \n",
|
||||
"0\\\\\n",
|
||||
"0 \\\\\n",
|
||||
"\\bar{a}\\\\\n",
|
||||
"\\frac{\\bar{v}\\bar{\\delta}}{Lcos^2(\\bar{\\delta})}\\\\\n",
|
||||
"\\end{bmatrix}\n",
|
||||
")\\\\\n",
|
||||
"=\n",
|
||||
"\\begin{bmatrix} \n",
|
||||
"\\bar{v}sin(\\bar{\\phi})\\bar{\\phi}dt\\\\\n",
|
||||
"-\\bar{v}cos(\\bar{\\phi})\\bar{\\phi}dt\\\\\n",
|
||||
"0\\\\\n",
|
||||
"-\\frac{\\bar{v}\\bar{\\delta}}{Lcos^2(\\bar{\\delta})}dt\\\\\n",
|
||||
"\\end{bmatrix}\n",
|
||||
"\\end{equation*}$$"
|
||||
],
|
||||
"metadata": {}
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"source": [
|
||||
"This equation is implemented at \n",
|
||||
"\n",
|
||||
"[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)\n"
|
||||
],
|
||||
"metadata": {}
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"source": [
|
||||
"### Reference\n",
|
||||
"\n",
|
||||
"- [Vehicle Dynamics and Control \\| Rajesh Rajamani \\| Springer](http://www.springer.com/us/book/9781461414322)\n",
|
||||
"\n",
|
||||
"- [MPC Course Material \\- MPC Lab @ UC\\-Berkeley](http://www.mpc.berkeley.edu/mpc-course-material)\n"
|
||||
],
|
||||
"metadata": {}
|
||||
}
|
||||
],
|
||||
"metadata": {
|
||||
"kernelspec": {
|
||||
"display_name": "Python 3",
|
||||
"language": "python",
|
||||
"name": "python3"
|
||||
},
|
||||
"language_info": {
|
||||
"codemirror_mode": {
|
||||
"name": "ipython",
|
||||
"version": 3
|
||||
},
|
||||
"file_extension": ".py",
|
||||
"mimetype": "text/x-python",
|
||||
"name": "python",
|
||||
"nbconvert_exporter": "python",
|
||||
"pygments_lexer": "ipython3",
|
||||
"version": "3.6.8"
|
||||
}
|
||||
},
|
||||
"nbformat": 4,
|
||||
"nbformat_minor": 2
|
||||
}
|
||||
Binary file not shown.
|
Before Width: | Height: | Size: 39 KiB |
Reference in New Issue
Block a user