mirror of
https://github.com/AtsushiSakai/PythonRobotics.git
synced 2026-04-22 03:00:22 -04:00
302 lines
8.5 KiB
Plaintext
302 lines
8.5 KiB
Plaintext
{
|
||
"cells": [
|
||
{
|
||
"cell_type": "markdown",
|
||
"metadata": {},
|
||
"source": [
|
||
"# Model predictive speed and steering control\n",
|
||
"\n",
|
||
"code:\n",
|
||
"\n",
|
||
"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/)"
|
||
]
|
||
},
|
||
{
|
||
"cell_type": "markdown",
|
||
"metadata": {},
|
||
"source": [
|
||
"# 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",
|
||
"Input vector is:\n",
|
||
"$$ u = [a, \\delta]$$ a: accellation, δ: steering angle\n",
|
||
"\n"
|
||
]
|
||
},
|
||
{
|
||
"cell_type": "markdown",
|
||
"metadata": {},
|
||
"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."
|
||
]
|
||
},
|
||
{
|
||
"cell_type": "markdown",
|
||
"metadata": {},
|
||
"source": [
|
||
"subject to:\n",
|
||
"- Linearlied vehicle model\n",
|
||
"$$z_{t+1}=Az_t+Bu+C$$\n",
|
||
"- Maximum steering speed\n",
|
||
"$$|u_{t+1}-u_{t}|<du_{max}$$\n",
|
||
"- Maximum steering angle\n",
|
||
"$$|u_{t}|<u_{max}$$\n",
|
||
"- Initial state\n",
|
||
"$$z_0 = z_{0,ob}$$\n",
|
||
"- Maximum and minimum speed\n",
|
||
"$$v_{min} < v_t < v_{max}$$\n",
|
||
"- Maximum and minimum input\n",
|
||
"$$u_{min} < u_t < u_{max}$$\n"
|
||
]
|
||
},
|
||
{
|
||
"cell_type": "markdown",
|
||
"metadata": {},
|
||
"source": [
|
||
"This is implemented at \n",
|
||
"\n",
|
||
"https://github.com/AtsushiSakai/PythonRobotics/blob/f51a73f47cb922a12659f8ce2d544c347a2a8156/PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py#L247-L301"
|
||
]
|
||
},
|
||
{
|
||
"cell_type": "markdown",
|
||
"metadata": {},
|
||
"source": [
|
||
"# Vehicle model linearization\n",
|
||
"\n",
|
||
"\n",
|
||
"\n",
|
||
"Vehicle model is \n",
|
||
"$$ \\dot{x} = vcos(\\phi)$$\n",
|
||
"$$ \\dot{y} = vsin((\\phi)$$\n",
|
||
"$$ \\dot{v} = a$$\n",
|
||
"$$ \\dot{\\phi} = \\frac{vtan(\\delta)}{L}$$\n",
|
||
"\n",
|
||
"\n",
|
||
"\n"
|
||
]
|
||
},
|
||
{
|
||
"cell_type": "markdown",
|
||
"metadata": {},
|
||
"source": [
|
||
"ODE is\n",
|
||
"\n",
|
||
"$$ \\dot{z} =\\frac{\\partial }{\\partial z} z = f(z, u) = A'z+B'u$$\n",
|
||
"\n"
|
||
]
|
||
},
|
||
{
|
||
"cell_type": "markdown",
|
||
"metadata": {},
|
||
"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"
|
||
]
|
||
},
|
||
{
|
||
"cell_type": "markdown",
|
||
"metadata": {},
|
||
"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"
|
||
]
|
||
},
|
||
{
|
||
"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$$\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"
|
||
]
|
||
},
|
||
{
|
||
"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 \n",
|
||
"\n",
|
||
"https://github.com/AtsushiSakai/PythonRobotics/blob/eb6d1cbe6fc90c7be9210bf153b3a04f177cc138/PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py#L80-L102"
|
||
]
|
||
},
|
||
{
|
||
"cell_type": "markdown",
|
||
"metadata": {},
|
||
"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": {
|
||
"kernelspec": {
|
||
"display_name": "Python [default]",
|
||
"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.4"
|
||
}
|
||
},
|
||
"nbformat": 4,
|
||
"nbformat_minor": 2
|
||
}
|