{ "cells": [ { "cell_type": "code", "execution_count": 1, "metadata": {}, "outputs": [], "source": [ "import numpy as np\n", "from scipy.integrate import odeint\n", "from scipy.interpolate import interp1d\n", "import cvxpy as cp\n", "\n", "import matplotlib.pyplot as plt\n", "plt.style.use(\"ggplot\")\n", "\n", "import time" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "### kinematics model equations\n", "\n", "The variables of the model are:\n", "\n", "* $x$ coordinate of the robot\n", "* $y$ coordinate of the robot\n", "* $v$ velocity of the robot\n", "* $\\theta$ heading of the robot\n", "\n", "The inputs of the model are:\n", "\n", "* $a$ acceleration of the robot\n", "* $\\delta$ steering of the robot\n", "\n", "These are the differential equations f(x,u) of the model:\n", "\n", "$\\dot{x} = f(x,u)$\n", "\n", "* $\\dot{x} = v\\cos{\\theta}$ \n", "* $\\dot{y} = v\\sin{\\theta}$\n", "* $\\dot{v} = a$\n", "* $\\dot{\\theta} = \\frac{v\\tan{\\delta}}{L}$\n", "\n", "Discretize with forward Euler Integration for time step dt:\n", "\n", "${x_{t+1}} = x_{t} + f(x,u)dt$\n", "\n", "* ${x_{t+1}} = x_{t} + v_t\\cos{\\theta}dt$\n", "* ${y_{t+1}} = y_{t} + v_t\\sin{\\theta}dt$\n", "* ${v_{t+1}} = v_{t} + a_tdt$\n", "* ${\\theta_{t+1}} = \\theta_{t} + \\frac{v\\tan{\\delta}}{L} dt$\n", "\n", "----------------------\n", "\n", "The Model is **non-linear** and **time variant**, but the Numerical Optimizer requires a Linear sets of equations. To approximate the equivalent **LTI** State space model, the **Taylor's series expansion** is used around $\\bar{x}$ and $\\bar{u}$ (at each time step):\n", "\n", "$ f(x,u) \\approx f(\\bar{x},\\bar{u}) + \\frac{\\partial f(x,u)}{\\partial x}|_{x=\\bar{x},u=\\bar{u}}(x-\\bar{x}) + \\frac{\\partial f(x,u)}{\\partial u}|_{x=\\bar{x},u=\\bar{u}}(u-\\bar{u})$\n", "\n", "This can be rewritten usibg the State Space model form Ax+Bu :\n", "\n", "$ f(\\bar{x},\\bar{u}) + A|_{x=\\bar{x},u=\\bar{u}}(x-\\bar{x}) + B|_{x=\\bar{x},u=\\bar{u}}(u-\\bar{u})$\n", "\n", "Where:\n", "\n", "$\n", "A =\n", "\\quad\n", "\\begin{bmatrix}\n", "\\frac{\\partial f(x,u)}{\\partial x} & \\frac{\\partial f(x,u)}{\\partial y} & \\frac{\\partial f(x,u)}{\\partial v} & \\frac{\\partial f(x,u)}{\\partial \\theta} \\\\\n", "\\end{bmatrix}\n", "\\quad\n", "=\n", "\\displaystyle \\left[\\begin{matrix}0 & 0 & \\cos{\\left(\\theta \\right)} & - v \\sin{\\left(\\theta \\right)}\\\\0 & 0 & \\sin{\\left(\\theta \\right)} & v \\cos{\\left(\\theta \\right)}\\\\0 & 0 & 0 & 0\\\\0 & 0 & \\frac{\\tan{\\left(\\delta \\right)}}{L} & 0\\end{matrix}\\right]\n", "$\n", "\n", "and\n", "\n", "$\n", "B = \n", "\\quad\n", "\\begin{bmatrix}\n", "\\frac{\\partial f(x,u)}{\\partial a} & \\frac{\\partial f(x,u)}{\\partial \\delta} \\\\\n", "\\end{bmatrix}\n", "\\quad\n", "= \n", "\\displaystyle \\left[\\begin{matrix}0 & 0\\\\0 & 0\\\\1 & 0\\\\0 & \\frac{v \\left(\\tan^{2}{\\left(\\delta \\right)} + 1\\right)}{L}\\end{matrix}\\right]\n", "$\n", "\n", "are the *Jacobians*.\n", "\n", "\n", "\n", "So the discretized model is given by:\n", "\n", "$ x_{t+1} = x_t + (f(\\bar{x},\\bar{u}) + A|_{x=\\bar{x}}(x_t-\\bar{x}) + B|_{u=\\bar{u}}(u_t-\\bar{u}) )dt $\n", "\n", "$ x_{t+1} = (I+dtA)x_t + dtBu_t +dt(f(\\bar{x},\\bar{u}) - A\\bar{x} - B\\bar{u}))$\n", "\n", "The LTI-equivalent kinematics model is:\n", "\n", "$ x_{t+1} = A'x_t + B' u_t + C' $\n", "\n", "with:\n", "\n", "$ A' = I+dtA|_{x=\\bar{x},u=\\bar{u}} $\n", "\n", "$ B' = dtB|_{x=\\bar{x},u=\\bar{u}} $\n", "\n", "$ C' = dt(f(\\bar{x},\\bar{u}) - A|_{x=\\bar{x},u=\\bar{u}}\\bar{x} - B|_{x=\\bar{x},u=\\bar{u}}\\bar{u}) $" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "-----------------\n", "[About Taylor Series Expansion](https://courses.engr.illinois.edu/ece486/fa2017/documents/lecture_notes/state_space_p2.pdf):\n", "\n", "In order to linearize general nonlinear systems, we will use the Taylor Series expansion of functions.\n", "\n", "Typically it is possible to assume that the system is operating about some nominal\n", "state solution $\\bar{x}$ (possibly requires a nominal input $\\bar{u}$) called **equilibrium point**.\n", "\n", "Recall that the Taylor Series expansion of f(x) around the\n", "point $\\bar{x}$ is given by:\n", "\n", "$f(x)=f(\\bar{x}) + \\frac{df(x)}{dx}|_{x=\\bar{x}}(x-\\bar{x})$ + higher order terms...\n", "\n", "For x sufficiently close to $\\bar{x}$, these higher order terms will be very close to zero, and so we can drop them.\n", "\n", "The extension to functions of multiple states and inputs is very similar to the above procedure.Suppose the evolution of state x\n", "is given by:\n", "\n", "$\\dot{x} = f(x1, x2, . . . , xn, u1, u2, . . . , um) = Ax+Bu$\n", "\n", "Where:\n", "\n", "$ A =\n", "\\quad\n", "\\begin{bmatrix}\n", "\\frac{\\partial f(x,u)}{\\partial x1} & ... & \\frac{\\partial f(x,u)}{\\partial xn} \\\\\n", "\\end{bmatrix}\n", "\\quad\n", "$ and $ B = \\quad\n", "\\begin{bmatrix}\n", "\\frac{\\partial f(x,u)}{\\partial u1} & ... & \\frac{\\partial f(x,u)}{\\partial um} \\\\\n", "\\end{bmatrix}\n", "\\quad $\n", "\n", "Then:\n", "\n", "$f(x,u)=f(\\bar{x},\\bar{u}) + \\frac{df(x,u)}{dx}|_{x=\\bar{x}}(x-\\bar{x}) + \\frac{df(x,u)}{du}|_{u=\\bar{u}}(u-\\bar{u}) = f(\\bar{x},\\bar{u}) + A_{x=\\bar{x}}(x-\\bar{x}) + B_{u=\\bar{u}}(u-\\bar{u})$\n", "\n", "-----------------" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "### Kinematics Model" ] }, { "cell_type": "code", "execution_count": 2, "metadata": {}, "outputs": [], "source": [ "\"\"\"\n", "Control problem statement.\n", "\"\"\"\n", "\n", "N = 4 #number of state variables\n", "M = 2 #number of control variables\n", "T = 20 #Prediction Horizon\n", "DT = 0.25 #discretization step" ] }, { "cell_type": "code", "execution_count": 3, "metadata": {}, "outputs": [], "source": [ "def get_linear_model(x_bar,u_bar):\n", " \"\"\"\n", " Computes the LTI approximated state space model x' = Ax + Bu + C\n", " \"\"\"\n", " \n", " L=0.3 #vehicle wheelbase\n", " \n", " x = x_bar[0]\n", " y = x_bar[1]\n", " v = x_bar[2]\n", " theta = x_bar[3]\n", " \n", " a = u_bar[0]\n", " delta = u_bar[1]\n", " \n", " A = np.zeros((N,N))\n", " A[0,2]=np.cos(theta)\n", " A[0,3]=-v*np.sin(theta)\n", " A[1,2]=np.sin(theta)\n", " A[1,3]=v*np.cos(theta)\n", " A[3,2]=v*np.tan(delta)/L\n", " A_lin=np.eye(N)+DT*A\n", " \n", " B = np.zeros((N,M))\n", " B[2,0]=1\n", " B[3,1]=v/(L*np.cos(delta)**2)\n", " B_lin=DT*B\n", " \n", " f_xu=np.array([v*np.cos(theta), v*np.sin(theta), a,v*np.tan(delta)/L]).reshape(N,1)\n", " C_lin = DT*(f_xu - np.dot(A,x_bar.reshape(N,1)) - np.dot(B,u_bar.reshape(M,1)))\n", " \n", " return np.round(A_lin,4), np.round(B_lin,4), np.round(C_lin,4)" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Motion Prediction: using scipy intergration" ] }, { "cell_type": "code", "execution_count": 4, "metadata": {}, "outputs": [], "source": [ "# Define process model\n", "# This uses the continuous model \n", "def kinematics_model(x,t,u):\n", " \"\"\"\n", " Returns the set of ODE of the vehicle model.\n", " \"\"\"\n", " \n", " L=0.3 #vehicle wheelbase\n", " dxdt = x[2]*np.cos(x[3])\n", " dydt = x[2]*np.sin(x[3])\n", " dvdt = u[0]\n", " dthetadt = x[2]*np.tan(u[1])/L\n", "\n", " dqdt = [dxdt,\n", " dydt,\n", " dvdt,\n", " dthetadt]\n", "\n", " return dqdt\n", "\n", "def predict(x0,u):\n", " \"\"\"\n", " \"\"\"\n", " \n", " x_ = np.zeros((N,T+1))\n", " \n", " x_[:,0] = x0\n", " \n", " # solve ODE\n", " for t in range(1,T+1):\n", "\n", " tspan = [0,DT]\n", " x_next = odeint(kinematics_model,\n", " x0,\n", " tspan,\n", " args=(u[:,t-1],))\n", "\n", " x0 = x_next[1]\n", " x_[:,t]=x_next[1]\n", " \n", " return x_" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Validate the model, here the status w.r.t a straight line with constant heading 0" ] }, { "cell_type": "code", "execution_count": 5, "metadata": {}, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ "CPU times: user 3.49 ms, sys: 0 ns, total: 3.49 ms\n", "Wall time: 3 ms\n" ] } ], "source": [ "%%time\n", "\n", "u_bar = np.zeros((M,T))\n", "u_bar[0,:] = 0.2 #m/ss\n", "u_bar[1,:] = np.radians(-np.pi/4) #rad\n", "\n", "x0 = np.zeros(N)\n", "x0[0] = 0\n", "x0[1] = 1\n", "x0[2] = 0\n", "x0[3] = np.radians(0)\n", "\n", "x_bar=predict(x0,u_bar)" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Check the model prediction" ] }, { "cell_type": "code", "execution_count": 6, "metadata": {}, "outputs": [ { "data": { "image/png": "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\n", "text/plain": [ "
" ] }, "metadata": {}, "output_type": "display_data" } ], "source": [ "#plot trajectory\n", "plt.subplot(2, 2, 1)\n", "plt.plot(x_bar[0,:],x_bar[1,:])\n", "plt.plot(np.linspace(0,10,T+1),np.zeros(T+1),\"b-\")\n", "plt.axis('equal')\n", "plt.ylabel('y')\n", "plt.xlabel('x')\n", "\n", "plt.subplot(2, 2, 2)\n", "plt.plot(np.degrees(x_bar[2,:]))\n", "plt.ylabel('theta(t) [deg]')\n", "#plt.xlabel('time')\n", "\n", "\n", "plt.tight_layout()\n", "plt.show()" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Motion Prediction: using the state space model" ] }, { "cell_type": "code", "execution_count": 7, "metadata": {}, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ "CPU times: user 2.17 ms, sys: 379 µs, total: 2.55 ms\n", "Wall time: 1.76 ms\n" ] } ], "source": [ "%%time\n", "\n", "u_bar = np.zeros((M,T))\n", "u_bar[0,:] = 0.2 #m/s\n", "u_bar[1,:] = np.radians(-np.pi/4) #rad\n", "\n", "x0 = np.zeros(N)\n", "x0[0] = 0\n", "x0[1] = 1\n", "x0[2] = 0\n", "x0[3] = np.radians(0)\n", "\n", "x_bar=np.zeros((N,T+1))\n", "x_bar[:,0]=x0\n", "\n", "for t in range (1,T+1):\n", " xt=x_bar[:,t-1].reshape(N,1)\n", " ut=u_bar[:,t-1].reshape(M,1)\n", " \n", " A,B,C=get_linear_model(xt,ut)\n", " \n", " xt_plus_one = np.dot(A,xt)+np.dot(B,ut)+C\n", " \n", " x_bar[:,t]= np.squeeze(xt_plus_one)" ] }, { "cell_type": "code", "execution_count": 8, "metadata": {}, "outputs": [ { "data": { "image/png": "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\n", "text/plain": [ "
" ] }, "metadata": {}, "output_type": "display_data" } ], "source": [ "#plot trajectory\n", "plt.subplot(2, 2, 1)\n", "plt.plot(x_bar[0,:],x_bar[1,:])\n", "plt.plot(np.linspace(0,10,T+1),np.zeros(T+1),\"b-\")\n", "plt.axis('equal')\n", "plt.ylabel('y')\n", "plt.xlabel('x')\n", "\n", "plt.subplot(2, 2, 2)\n", "plt.plot(np.degrees(x_bar[2,:]))\n", "plt.ylabel('theta(t)')\n", "#plt.xlabel('time')\n", "\n", "\n", "plt.tight_layout()\n", "plt.show()" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "The results are the same as expected, so the linearized model is equivalent as expected." ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## PRELIMINARIES" ] }, { "cell_type": "code", "execution_count": 9, "metadata": {}, "outputs": [], "source": [ "def compute_path_from_wp(start_xp, start_yp, step = 0.1):\n", " \"\"\"\n", " Computes a reference path given a set of waypoints\n", " \"\"\"\n", " \n", " final_xp=[]\n", " final_yp=[]\n", " delta = step #[m]\n", "\n", " for idx in range(len(start_xp)-1):\n", " section_len = np.sum(np.sqrt(np.power(np.diff(start_xp[idx:idx+2]),2)+np.power(np.diff(start_yp[idx:idx+2]),2)))\n", "\n", " interp_range = np.linspace(0,1,np.floor(section_len/delta).astype(int))\n", " \n", " fx=interp1d(np.linspace(0,1,2),start_xp[idx:idx+2],kind=1)\n", " fy=interp1d(np.linspace(0,1,2),start_yp[idx:idx+2],kind=1)\n", " \n", " final_xp=np.append(final_xp,fx(interp_range))\n", " final_yp=np.append(final_yp,fy(interp_range))\n", " \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", "def get_nn_idx(state,path):\n", " \"\"\"\n", " Computes the index of the waypoint closest to vehicle\n", " \"\"\"\n", "\n", " dx = state[0]-path[0,:]\n", " dy = state[1]-path[1,:]\n", " dist = np.hypot(dx,dy)\n", " nn_idx = np.argmin(dist)\n", "\n", " try:\n", " v = [path[0,nn_idx+1] - path[0,nn_idx],\n", " path[1,nn_idx+1] - path[1,nn_idx]] \n", " v /= np.linalg.norm(v)\n", "\n", " d = [path[0,nn_idx] - state[0],\n", " path[1,nn_idx] - state[1]]\n", "\n", " if np.dot(d,v) > 0:\n", " target_idx = nn_idx\n", " else:\n", " target_idx = nn_idx+1\n", "\n", " except IndexError as e:\n", " target_idx = nn_idx\n", "\n", " return target_idx\n", "\n", "def get_ref_trajectory(state, path, target_v):\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_idx(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" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "### MPC Problem formulation\n", "\n", "**Model Predictive Control** refers to the control approach of **numerically** solving a optimization problem at each time step. \n", "\n", "The controller generates a control signal over a fixed lenght T (Horizon) at each time step." ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "\n", "\n", "" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "#### Linear MPC Formulation\n", "\n", "Linear MPC makes use of the **LTI** (Linear time invariant) discrete state space model, wich represents a motion model used for Prediction.\n", "\n", "$x_{t+1} = Ax_t + Bu_t$\n", "\n", "The LTI formulation means that **future states** are linearly related to the current state and actuator signal. Hence, the MPC seeks to find a **control policy** U over a finite lenght horizon.\n", "\n", "$U={u_{t|t}, u_{t+1|t}, ...,u_{t+T|t}}$\n", "\n", "The objective function used minimize (drive the state to 0) is:\n", "\n", "$\n", "\\begin{equation}\n", "\\begin{aligned}\n", "\\min_{} \\quad & \\sum^{t+T-1}_{j=t} x^T_{j|t}Qx_{j|t} + u^T_{j|t}Ru_{j|t}\\\\\n", "\\textrm{s.t.} \\quad & x(0) = x0\\\\\n", " & x_{j+1|t} = Ax_{j|t}+Bu_{j|t}) \\quad \\textrm{for} \\quad t= 0.0] #min_speed (not really needed)\n", "constr += [cp.abs(u[0, :]) <= MAX_ACC] #max acc\n", "constr += [cp.abs(u[1, :]) <= MAX_STEER] #max steer\n", "\n", "prob = cp.Problem(cp.Minimize(cost), constr)\n", "solution = prob.solve(solver=cp.OSQP, verbose=True)" ] }, { "cell_type": "code", "execution_count": 20, "metadata": {}, "outputs": [ { "data": { "image/png": "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\n", "text/plain": [ "
" ] }, "metadata": {}, "output_type": "display_data" } ], "source": [ "x_mpc=np.array(x.value[0, :]).flatten()\n", "y_mpc=np.array(x.value[1, :]).flatten()\n", "v_mpc=np.array(x.value[2, :]).flatten()\n", "theta_mpc=np.array(x.value[3, :]).flatten()\n", "a_mpc=np.array(u.value[0, :]).flatten()\n", "w_mpc=np.array(u.value[1, :]).flatten()\n", "\n", "#simulate robot state trajectory for optimized U\n", "x_traj=predict(x0, np.vstack((a_mpc,w_mpc)))\n", "\n", "#plt.figure(figsize=(15,10))\n", "#plot trajectory\n", "plt.subplot(2, 2, 1)\n", "plt.plot(track[0,:],track[1,:],\"b+\")\n", "plt.plot(x_traj[0,:],x_traj[1,:])\n", "plt.axis(\"equal\")\n", "plt.ylabel('y')\n", "plt.xlabel('x')\n", "\n", "#plot v(t)\n", "plt.subplot(2, 2, 2)\n", "plt.plot(a_mpc)\n", "plt.ylabel('a(t)')\n", "#plt.xlabel('time')\n", "\n", "\n", "plt.subplot(2, 2, 4)\n", "plt.plot(theta_mpc)\n", "plt.ylabel('theta(t)')\n", "\n", "plt.subplot(2, 2, 3)\n", "plt.plot(v_mpc)\n", "plt.ylabel('v(t)')\n", "\n", "plt.tight_layout()\n", "plt.show()" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "full track demo" ] }, { "cell_type": "code", "execution_count": 12, "metadata": {}, "outputs": [ { "ename": "NameError", "evalue": "name 'road_curve' is not defined", "output_type": "error", "traceback": [ "\u001b[0;31m---------------------------------------\u001b[0m", "\u001b[0;31mNameError\u001b[0mTraceback (most recent call last)", "\u001b[0;32m\u001b[0m in \u001b[0;36m\u001b[0;34m\u001b[0m\n\u001b[1;32m 32\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m 33\u001b[0m \u001b[0miter_start\u001b[0m\u001b[0;34m=\u001b[0m\u001b[0mtime\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mtime\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m---> 34\u001b[0;31m \u001b[0mK\u001b[0m\u001b[0;34m=\u001b[0m\u001b[0mroad_curve\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mx_sim\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m,\u001b[0m\u001b[0msim_time\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m,\u001b[0m\u001b[0mtrack\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m 35\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m 36\u001b[0m \u001b[0;31m# dynamics starting state w.r.t vehicle frame\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n", "\u001b[0;31mNameError\u001b[0m: name 'road_curve' is not defined" ] } ], "source": [ "track = compute_path_from_wp([0,3,4,6,10,12,14,6,1,0],\n", " [0,0,2,4,3,3,-2,-6,-2,-2],0.5)\n", "\n", "# track = compute_path_from_wp([0,5,7.5,10,12,13,13,10],\n", "# [0,0,2.5,2.5,0,0,5,10],0.5)\n", "\n", "sim_duration = 175 #time steps\n", "opt_time=[]\n", "\n", "x_sim = np.zeros((N,sim_duration))\n", "u_sim = np.zeros((M,sim_duration-1))\n", "\n", "MAX_SPEED = 1.25\n", "MIN_SPEED = 0.75\n", "MAX_STEER = 1.57/2\n", "MAX_ACC = 1.0\n", "\n", "# Starting Condition\n", "x0 = np.zeros(N)\n", "x0[0] = 0\n", "x0[1] = -0.25\n", "x0[2] = 0\n", "x0[3] = np.radians(-0)\n", "x_sim[:,0]=x0\n", " \n", "#starting guess\n", "u_bar = np.zeros((M,T))\n", "u_bar[0,:]=0.5\n", "u_bar[1,:]=0.01\n", "\n", "for sim_time in range(sim_duration-1):\n", " \n", " iter_start=time.time()\n", " K=road_curve(x_sim[:,sim_time],track)\n", " \n", " # dynamics starting state w.r.t vehicle frame\n", " x_bar=np.zeros((N,T+1))\n", " x_bar[2,0]=x_sim[2,sim_time]\n", " \n", " #prediction for linearization of costrains\n", " for t in range (1,T+1):\n", " xt=x_bar[:,t-1].reshape(N,1)\n", " ut=u_bar[:,t-1].reshape(M,1)\n", " A,B,C=get_linear_model(xt,ut)\n", " xt_plus_one = np.squeeze(np.dot(A,xt)+np.dot(B,ut)+C)\n", " x_bar[:,t]= xt_plus_one\n", " \n", " #CVXPY Linear MPC problem statement\n", " cost = 0\n", " constr = []\n", " x = cp.Variable((N, T+1))\n", " u = cp.Variable((M, T))\n", " \n", " #Prediction Horizon\n", " for t in range(T):\n", "\n", " cost += 30*cp.sum_squares(x[3,t]-np.arctan(df(x_bar[0,t],K))) # psi\n", " cost += 20*cp.sum_squares(f(x_bar[0,t],K)-x[1,t]) # cte\n", " cost += 10*cp.sum_squares(1.-x[2,t]) # desired v\n", "\n", " # Actuation rate of change\n", " if t < (T - 1):\n", " cost += cp.quad_form(u[:, t + 1] - u[:, t], 10*np.eye(M))\n", " \n", " # Actuation effort\n", " cost += cp.quad_form( u[:, t],10*np.eye(M))\n", " \n", " # Kinrmatics Constrains (Linearized model)\n", " A,B,C=get_linear_model(x_bar[:,t],u_bar[:,t])\n", " constr += [x[:,t+1] == A@x[:,t] + B@u[:,t] + C.flatten()]\n", "\n", " # sums problem objectives and concatenates constraints.\n", " constr += [x[:,0] == x_bar[:,0]] #<--watch out the start condition\n", " constr += [x[2, :] <= MAX_SPEED]\n", " constr += [x[2, :] >= 0.0]\n", " constr += [cp.abs(u[0, :]) <= MAX_ACC]\n", " constr += [cp.abs(u[1, :]) <= MAX_STEER]\n", " \n", " # Solve\n", " prob = cp.Problem(cp.Minimize(cost), constr)\n", " solution = prob.solve(solver=cp.OSQP, verbose=False)\n", " \n", " #retrieved optimized U and assign to u_bar to linearize in next step\n", " u_bar=np.vstack((np.array(u.value[0, :]).flatten(),\n", " (np.array(u.value[1, :]).flatten())))\n", " \n", " u_sim[:,sim_time] = u_bar[:,0]\n", " \n", " # Measure elpased time to get results from cvxpy\n", " opt_time.append(time.time()-iter_start)\n", " \n", " # move simulation to t+1\n", " tspan = [0,dt]\n", " x_sim[:,sim_time+1] = odeint(kinematics_model,\n", " x_sim[:,sim_time],\n", " tspan,\n", " args=(u_bar[:,0],))[1]\n", " \n", "print(\"CVXPY Optimization Time: Avrg: {:.4f}s Max: {:.4f}s Min: {:.4f}s\".format(np.mean(opt_time),\n", " np.max(opt_time),\n", " np.min(opt_time))) " ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "#plot trajectory\n", "grid = plt.GridSpec(4, 5)\n", "\n", "plt.figure(figsize=(15,10))\n", "\n", "plt.subplot(grid[0:4, 0:4])\n", "plt.plot(track[0,:],track[1,:],\"b+\")\n", "plt.plot(x_sim[0,:],x_sim[1,:])\n", "plt.axis(\"equal\")\n", "plt.ylabel('y')\n", "plt.xlabel('x')\n", "\n", "plt.subplot(grid[0, 4])\n", "plt.plot(u_sim[0,:])\n", "plt.ylabel('a(t) [m/ss]')\n", "\n", "plt.subplot(grid[1, 4])\n", "plt.plot(x_sim[2,:])\n", "plt.ylabel('v(t) [m/s]')\n", "\n", "plt.subplot(grid[2, 4])\n", "plt.plot(np.degrees(u_sim[1,:]))\n", "plt.ylabel('delta(t) [deg/s]')\n", "\n", "plt.subplot(grid[3, 4])\n", "plt.plot(x_sim[3,:])\n", "plt.ylabel('theta(t) [m/s]')\n", "\n", "plt.tight_layout()\n", "plt.show()" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## OBSTACLE AVOIDANCE\n", "see dccp paper for reference" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "import dccp\n", "track = compute_path_from_wp([0,3,4,6,10,13],\n", " [0,0,2,4,3,3],0.25)\n", "\n", "obstacles=np.array([[4,6],[2,4]])\n", "obstacle_radius=0.5\n", "\n", "def to_vehic_frame(pt,pos_x,pos_y,theta):\n", " dx = pt[0] - pos_x\n", " dy = pt[1] - pos_y\n", "\n", " return [dx * np.cos(-theta) - dy * np.sin(-theta),\n", " dy * np.cos(-theta) + dx * np.sin(-theta)]\n", " \n", "# track = compute_path_from_wp([0,5,7.5,10,12,13,13,10],\n", "# [0,0,2.5,2.5,0,0,5,10],0.5)\n", "\n", "sim_duration = 80 #time steps\n", "opt_time=[]\n", "\n", "x_sim = np.zeros((N,sim_duration))\n", "u_sim = np.zeros((M,sim_duration-1))\n", "\n", "MAX_SPEED = 1.25\n", "MIN_SPEED = 0.75\n", "MAX_STEER_SPEED = 1.57\n", "\n", "# Starting Condition\n", "x0 = np.zeros(N)\n", "x0[0] = 0\n", "x0[1] = -0.25\n", "x0[2] = np.radians(-0)\n", "x_sim[:,0]=x0\n", " \n", "#starting guess\n", "u_bar = np.zeros((M,T))\n", "u_bar[0,:]=0.5*(MAX_SPEED+MIN_SPEED)\n", "u_bar[1,:]=0.00\n", "\n", "for sim_time in range(sim_duration-1):\n", " \n", " iter_start=time.time()\n", " \n", " #compute coefficients\n", " K=road_curve(x_sim[:,sim_time],track)\n", " \n", " #compute opstacles in ref frame\n", " o_=[]\n", " for j in range(2):\n", " o_.append(to_vehic_frame(obstacles[:,j],x_sim[0,sim_time],x_sim[1,sim_time],x_sim[2,sim_time]) )\n", " \n", " # dynamics starting state w.r.t vehicle frame\n", " x_bar=np.zeros((N,T+1))\n", " \n", " #prediction for linearization of costrains\n", " for t in range (1,T+1):\n", " xt=x_bar[:,t-1].reshape(N,1)\n", " ut=u_bar[:,t-1].reshape(M,1)\n", " A,B,C=get_linear_model(xt,ut)\n", " xt_plus_one = np.squeeze(np.dot(A,xt)+np.dot(B,ut)+C)\n", " x_bar[:,t]= xt_plus_one\n", " \n", " #CVXPY Linear MPC problem statement\n", " cost = 0\n", " constr = []\n", " x = cp.Variable((N, T+1))\n", " u = cp.Variable((M, T))\n", " \n", " #Prediction Horizon\n", " for t in range(T):\n", "\n", " #cost += 30*cp.sum_squares(x[2,t]-np.arctan(df(x_bar[0,t],K))) # psi\n", " cost += 50*cp.sum_squares(x[2,t]-np.arctan2(df(x_bar[0,t],K),x_bar[0,t])) # psi\n", " cost += 20*cp.sum_squares(f(x_bar[0,t],K)-x[1,t]) # cte\n", "\n", " # Actuation rate of change\n", " if t < (T - 1):\n", " cost += cp.quad_form(u[:, t + 1] - u[:, t], 100*np.eye(M))\n", " \n", " # Actuation effort\n", " cost += cp.quad_form( u[:, t],1*np.eye(M))\n", " \n", " # Kinrmatics Constrains (Linearized model)\n", " A,B,C=get_linear_model(x_bar[:,t],u_bar[:,t])\n", " constr += [x[:,t+1] == A@x[:,t] + B@u[:,t] + C.flatten()]\n", " \n", " # Obstacle Avoidance Contrains\n", " for j in range(2):\n", " constr += [ cp.norm(x[0:2,t]-o_[j],2) >= obstacle_radius ]\n", "\n", " # sums problem objectives and concatenates constraints.\n", " constr += [x[:,0] == x_bar[:,0]] #<--watch out the start condition\n", " constr += [u[0, :] <= MAX_SPEED]\n", " constr += [u[0, :] >= MIN_SPEED]\n", " constr += [cp.abs(u[1, :]) <= MAX_STEER_SPEED]\n", " \n", " # Solve\n", " prob = cp.Problem(cp.Minimize(cost), constr)\n", " solution = prob.solve(method=\"dccp\", verbose=False)\n", " \n", " #retrieved optimized U and assign to u_bar to linearize in next step\n", " u_bar=np.vstack((np.array(u.value[0, :]).flatten(),\n", " (np.array(u.value[1, :]).flatten())))\n", " \n", " u_sim[:,sim_time] = u_bar[:,0]\n", " \n", " # Measure elpased time to get results from cvxpy\n", " opt_time.append(time.time()-iter_start)\n", " \n", " # move simulation to t+1\n", " tspan = [0,dt]\n", " x_sim[:,sim_time+1] = odeint(kinematics_model,\n", " x_sim[:,sim_time],\n", " tspan,\n", " args=(u_bar[:,0],))[1]\n", " \n", "print(\"CVXPY Optimization Time: Avrg: {:.4f}s Max: {:.4f}s Min: {:.4f}s\".format(np.mean(opt_time),\n", " np.max(opt_time),\n", " np.min(opt_time))) " ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "#plot trajectory\n", "grid = plt.GridSpec(2, 3)\n", "\n", "plt.figure(figsize=(15,10))\n", "\n", "ax=plt.subplot(grid[0:2, 0:2])\n", "plt.plot(track[0,:],track[1,:],\"b+\")\n", "plt.plot(x_sim[0,:],x_sim[1,:])\n", "#obstacles\n", "circle1=plt.Circle((obstacles[0,0], obstacles[1,0]), obstacle_radius, color='r')\n", "circle2=plt.Circle((obstacles[0,1], obstacles[1,1]), obstacle_radius, color='r')\n", "plt.axis(\"equal\")\n", "plt.ylabel('y')\n", "plt.xlabel('x')\n", "\n", "ax.add_artist(circle1)\n", "ax.add_artist(circle2)\n", "\n", "plt.subplot(grid[0, 2])\n", "plt.plot(u_sim[0,:])\n", "plt.ylabel('v(t) [m/s]')\n", "\n", "plt.subplot(grid[1, 2])\n", "plt.plot(np.degrees(u_sim[1,:]))\n", "plt.ylabel('w(t) [deg/s]')\n", "\n", "\n", "plt.tight_layout()\n", "plt.show()" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [] } ], "metadata": { "kernelspec": { "display_name": "Python 3", "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.8.5" } }, "nbformat": 4, "nbformat_minor": 4 }