22 "cells" : [
33 {
44 "cell_type" : " markdown" ,
5- "metadata" : {},
65 "source" : [
76 " ## Model predictive speed and steering control\n " ,
87 " \n " ,
98 " \n "
10- ]
9+ ],
10+ "metadata" : {}
1111 },
1212 {
1313 "cell_type" : " markdown" ,
14- "metadata" : {},
1514 "source" : [
1615 " \n " ,
1716 " \n " ,
2625 " This code uses cvxpy as an optimization modeling tool.\n " ,
2726 " \n " ,
2827 " - [Welcome to CVXPY 1\\ .0 — CVXPY 1\\ .0\\ .6 documentation](http://www.cvxpy.org/)"
29- ]
28+ ],
29+ "metadata" : {}
3030 },
3131 {
3232 "cell_type" : " markdown" ,
33- "metadata" : {},
3433 "source" : [
3534 " ### MPC modeling\n " ,
3635 " \n " ,
4645 " \n " ,
4746 " a: accellation, δ: steering angle\n " ,
4847 " \n "
49- ]
48+ ],
49+ "metadata" : {}
5050 },
5151 {
5252 "cell_type" : " markdown" ,
53- "metadata" : {},
5453 "source" : [
5554 " The MPC cotroller minimize this cost function for path tracking:\n " ,
5655 " \n " ,
5756 " $$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 " ,
5857 " \n " ,
5958 " z_ref come from target path and speed."
60- ]
59+ ],
60+ "metadata" : {}
6161 },
6262 {
6363 "cell_type" : " markdown" ,
64- "metadata" : {},
6564 "source" : [
6665 " subject to:\n " ,
6766 " \n " ,
8988 " \n " ,
9089 " $$u_{min} < u_t < u_{max}$$\n " ,
9190 " \n "
92- ]
91+ ],
92+ "metadata" : {}
9393 },
9494 {
9595 "cell_type" : " markdown" ,
96- "metadata" : {},
9796 "source" : [
9897 " This is implemented at \n " ,
9998 " \n " ,
10099 " [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)"
101- ]
100+ ],
101+ "metadata" : {}
102102 },
103103 {
104104 "cell_type" : " markdown" ,
105- "metadata" : {},
106105 "source" : [
107106 " ### Vehicle model linearization\n " ,
108107 " \n " ,
120119 " \n " ,
121120 " \n " ,
122121 " \n "
123- ]
122+ ],
123+ "metadata" : {}
124124 },
125125 {
126126 "cell_type" : " markdown" ,
127- "metadata" : {},
128127 "source" : [
129128 " ODE is\n " ,
130129 " \n " ,
131130 " $$ \\ dot{z} =\\ frac{\\ partial }{\\ partial z} z = f(z, u) = A'z+B'u$$\n " ,
132131 " \n "
133- ]
132+ ],
133+ "metadata" : {}
134134 },
135135 {
136136 "cell_type" : " markdown" ,
137- "metadata" : {},
138137 "source" : [
139138 " where\n " ,
140139 " \n " ,
168167 " \\ end{bmatrix}\n " ,
169168 " \\ end{equation*}$\n " ,
170169 " \n "
171- ]
170+ ],
171+ "metadata" : {}
172172 },
173173 {
174174 "cell_type" : " markdown" ,
175- "metadata" : {},
176175 "source" : [
177176 " $\\ begin{equation*}\n " ,
178177 " B' =\n " ,
196195 " \\ end{bmatrix}\n " ,
197196 " \\ end{equation*}$\n " ,
198197 " \n "
199- ]
198+ ],
199+ "metadata" : {}
200200 },
201201 {
202202 "cell_type" : " markdown" ,
203- "metadata" : {},
204203 "source" : [
205204 " You can get a discrete-time mode with Forward Euler Discretization with sampling time dt.\n " ,
206205 " \n " ,
210209 " $$z_{k+1}=z_k+(f(\\ bar{z},\\ bar{u})+A'z_k+B'u_k-A'\\ bar{z}-B'\\ bar{u})dt$$\n " ,
211210 " \n " ,
212211 " $$z_{k+1}=(I + dtA')z_k+(dtB')u_k + (f(\\ bar{z},\\ bar{u})-A'\\ bar{z}-B'\\ bar{u})dt$$\n "
213- ]
212+ ],
213+ "metadata" : {}
214214 },
215215 {
216216 "cell_type" : " markdown" ,
217- "metadata" : {},
218217 "source" : [
219218 " So, \n " ,
220219 " \n " ,
221220 " $$z_{k+1}=Az_k+Bu_k +C$$\n " ,
222221 " \n " ,
223222 " where,\n " ,
224223 " \n " ,
225- " $\\ begin{equation*}\n " ,
224+ " $$ \\ begin{equation*}\n " ,
226225 " A = (I + dtA')\\\\\n " ,
227226 " =\n " ,
228227 " \\ begin{bmatrix} \n " ,
231230 " 0 & 0 & 1 & 0 \\\\\n " ,
232231 " 0 & 0 &\\ frac{tan(\\ bar{\\ delta})}{L}dt & 1 \\\\\n " ,
233232 " \\ end{bmatrix}\n " ,
234- " \\ end{equation*}$"
235- ]
233+ " \\ end{equation*}$$"
234+ ],
235+ "metadata" : {}
236236 },
237237 {
238238 "cell_type" : " markdown" ,
239- "metadata" : {},
240239 "source" : [
241- " $\\ begin{equation*}\n " ,
240+ " $$ \\ begin{equation*}\n " ,
242241 " B = dtB'\\\\\n " ,
243242 " =\n " ,
244243 " \\ begin{bmatrix} \n " ,
247246 " dt & 0 \\\\\n " ,
248247 " 0 & \\ frac{\\ bar{v}}{Lcos^2(\\ bar{\\ delta})}dt \\\\\n " ,
249248 " \\ end{bmatrix}\n " ,
250- " \\ end{equation*}$"
251- ]
249+ " \\ end{equation*}$$"
250+ ],
251+ "metadata" : {}
252252 },
253253 {
254254 "cell_type" : " markdown" ,
255- "metadata" : {},
256255 "source" : [
257- " $\\ begin{equation*}\n " ,
256+ " $$ \\ begin{equation*}\n " ,
258257 " C = (f(\\ bar{z},\\ bar{u})-A'\\ bar{z}-B'\\ bar{u})dt\\\\\n " ,
259258 " = dt(\n " ,
260259 " \\ begin{bmatrix} \n " ,
285284 " 0\\\\\n " ,
286285 " -\\ frac{\\ bar{v}\\ bar{\\ delta}}{Lcos^2(\\ bar{\\ delta})}dt\\\\\n " ,
287286 " \\ end{bmatrix}\n " ,
288- " \\ end{equation*}$"
289- ]
287+ " \\ end{equation*}$$"
288+ ],
289+ "metadata" : {}
290290 },
291291 {
292292 "cell_type" : " markdown" ,
293- "metadata" : {},
294293 "source" : [
295294 " This equation is implemented at \n " ,
296295 " \n " ,
297296 " [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 "
298- ]
297+ ],
298+ "metadata" : {}
299299 },
300300 {
301301 "cell_type" : " markdown" ,
302- "metadata" : {},
303302 "source" : [
304303 " ### Reference\n " ,
305304 " \n " ,
306305 " - [Vehicle Dynamics and Control \\ | Rajesh Rajamani \\ | Springer](http://www.springer.com/us/book/9781461414322)\n " ,
307306 " \n " ,
308307 " - [MPC Course Material \\ - MPC Lab @ UC\\ -Berkeley](http://www.mpc.berkeley.edu/mpc-course-material)\n "
309- ]
308+ ],
309+ "metadata" : {}
310310 }
311311 ],
312312 "metadata" : {
330330 },
331331 "nbformat" : 4 ,
332332 "nbformat_minor" : 2
333- }
333+ }
0 commit comments