diff --git a/PathTracking/model_predictive_speed_and_steer_control/Model_predictive_speed_and_steering_control.ipynb b/PathTracking/model_predictive_speed_and_steer_control/Model_predictive_speed_and_steering_control.ipynb index 317318b32f..c9bbe2f7a4 100644 --- a/PathTracking/model_predictive_speed_and_steer_control/Model_predictive_speed_and_steering_control.ipynb +++ b/PathTracking/model_predictive_speed_and_steer_control/Model_predictive_speed_and_steering_control.ipynb @@ -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*}$" ] }, { diff --git a/docs/modules/Model_predictive_speed_and_steering_control.rst b/docs/modules/Model_predictive_speed_and_steering_control.rst index fa5df4a92c..4a424dcc42 100644 --- a/docs/modules/Model_predictive_speed_and_steering_control.rst +++ b/docs/modules/Model_predictive_speed_and_steering_control.rst @@ -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*}` - -: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*} 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. @@ -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*}` - -: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*} 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