12345qiupeng
/
mpc_python_learn
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

added trajectory generation method

master
mcarfagno 2020-10-23 10:43:10 +01:00
parent 385daab272
commit 3601c09206
2 changed files with 51 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

0
notebooks/MPC_racecar_crosstrack-Copy1.ipynb → notebooks/MPC_racecar_crosstrack.ipynb
View File

62
notebooks/MPC_racecar_tracking.ipynb
View File

@ -478,7 +478,12 @@
" final_xp=np.append(final_xp,fx(interp_range))\n", " final_xp=np.append(final_xp,fx(interp_range))\n",
" final_yp=np.append(final_yp,fy(interp_range))\n", " final_yp=np.append(final_yp,fy(interp_range))\n",
" \n", " \n",
" return np.vstack((final_xp,final_yp))\n", " dx = np.append(0, np.diff(final_xp))\n",
" dy = np.append(0, np.diff(final_yp))\n",
" theta = np.arctan2(dy, dx)\n",
"\n",
" return np.vstack((final_xp,final_yp,theta))\n",
"\n",
"\n", "\n",
"def get_nn_idx(state,path):\n", "def get_nn_idx(state,path):\n",
" \"\"\"\n", " \"\"\"\n",
@ -487,7 +492,7 @@
"\n", "\n",
" dx = state[0]-path[0,:]\n", " dx = state[0]-path[0,:]\n",
" dy = state[1]-path[1,:]\n", " dy = state[1]-path[1,:]\n",
" dist = np.sqrt(dx**2 + dy**2)\n", " dist = np.hypot(dx,dy)\n",
" nn_idx = np.argmin(dist)\n", " nn_idx = np.argmin(dist)\n",
"\n", "\n",
" try:\n", " try:\n",
@ -508,10 +513,45 @@
"\n", "\n",
" return target_idx\n", " return target_idx\n",
"\n", "\n",
"def get_course(state, path):\n", "def get_ref_trajectory(state, path, target_v):\n",
" \"\"\"\n", " \"\"\"\n",
" \"\"\"\n", " \"\"\"\n",
" " " xref = np.zeros((N, T + 1))\n",
" dref = np.zeros((1, T + 1))\n",
" \n",
" #sp = np.ones((1,T +1))*target_v #speed profile\n",
" \n",
" ncourse = path.shape[1]\n",
"\n",
" ind = get_nn_index(state, path)\n",
"\n",
" xref[0, 0] = path[0,ind] #X\n",
" xref[1, 0] = path[1,ind] #Y\n",
" xref[2, 0] = target_v #sp[ind] #V\n",
" xref[3, 0] = path[2,ind] #Theta\n",
" dref[0, 0] = 0.0 # steer operational point should be 0\n",
" \n",
" dl = 0.05 # Waypoints spacing\n",
" travel = 0.0\n",
"\n",
" for i in range(T + 1):\n",
" travel += abs(state[2]) * DT #current V or target V?\n",
" dind = int(round(travel / dl))\n",
"\n",
" if (ind + dind) < ncourse:\n",
" xref[0, i] = path[0,ind + dind]\n",
" xref[1, i] = path[1,ind + dind]\n",
" xref[2, i] = target_v #sp[ind + dind]\n",
" xref[3, i] = path[2,ind + dind]\n",
" dref[0, i] = 0.0\n",
" else:\n",
" xref[0, i] = path[0,ncourse - 1]\n",
" xref[1, i] = path[1,ncourse - 1]\n",
" xref[2, i] = target_v #sp[ncourse - 1]\n",
" xref[3, i] = path[2,ncourse - 1]\n",
" dref[0, i] = 0.0\n",
"\n",
" return xref, dref"
] ]
}, },
{ {
@ -529,9 +569,9 @@
"cell_type": "markdown", "cell_type": "markdown",
"metadata": {}, "metadata": {},
"source": [ "source": [
"![mpc](img/mpc_block_diagram.png)\n", "<!-- ![mpc](img/mpc_block_diagram.png) -->\n",
"\n", "\n",
"![mpc](img/mpc_t.png)" "<!-- ![mpc](img/mpc_t.png) -->"
] ]
}, },
{ {
@ -662,10 +702,10 @@
"\n", "\n",
"# Starting Condition\n", "# Starting Condition\n",
"x0 = np.zeros(N)\n", "x0 = np.zeros(N)\n",
"x0[0] = 0\n", "x0[0] = 0 #x\n",
"x0[1] = -0.25\n", "x0[1] = -0.25 #y\n",
"x0[2] = 0.0\n", "x0[2] = 0.0 #v\n",
"x0[3] = np.radians(-0)\n", "x0[3] = np.radians(-0) #yaw\n",
" \n", " \n",
"#starting guess\n", "#starting guess\n",
"u_bar = np.zeros((M,T))\n", "u_bar = np.zeros((M,T))\n",
@ -1150,7 +1190,7 @@
"name": "python", "name": "python",
"nbconvert_exporter": "python", "nbconvert_exporter": "python",
"pygments_lexer": "ipython3", "pygments_lexer": "ipython3",
"version": "3.8.4" "version": "3.8.5"
} }
}, },
"nbformat": 4, "nbformat": 4,