github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-01-13 11:57:56 -05:00
Code Issues Packages Projects Releases Wiki Activity

finish vehicle model linealization documentation

Browse Source
This commit is contained in:
Atsushi Sakai
2018-07-17 15:00:48 +09:00
parent eb6d1cbe6f
commit 754aec51a8
1 changed files with 98 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

102
PathTracking/model_predictive_speed_and_steer_control/notebook.ipynb
View File

@@ -42,7 +42,7 @@
"source": [
"ODE is\n",
"\n",
"$$ \\dot{z} =\\frac{\\partial }{\\partial z} z = A'z+B'u$$\n",
"$$ \\dot{z} =\\frac{\\partial }{\\partial z} z = f(z, u) = A'z+B'u$$\n",
"\n"
]
},
@@ -76,7 +76,7 @@
"　=\n",
"\\begin{bmatrix}\n",
"0 & 0 & cos(\\bar{\\phi}) & -\\bar{v}sin(\\bar{\\phi})\\\\\n",
"0 & 1 & sin(\\bar{\\phi}) & \\bar{v}cos(\\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",
@@ -109,10 +109,104 @@
"0 & \\frac{\\bar{v}}{Lcos^2(\\bar{\\delta})} \\\\\n",
"\\end{bmatrix}\n",
"\\end{equation*}\n",
"\n",
"\n"
]
},
{
"cell_type": "markdown",
"metadata": {},
"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$$"
"$$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)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"
]
},
{
"cell_type": "markdown",
"metadata": {},
"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*}"
]
},
{
"cell_type": "markdown",
"metadata": {},
"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*}"
]
},
{
"cell_type": "markdown",
"metadata": {},
"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*}"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"This equation is implemented at https://github.com/AtsushiSakai/PythonRobotics/blob/eb6d1cbe6fc90c7be9210bf153b3a04f177cc138/PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py#L80-L102"
]
},
{