1188 lines
85 KiB
Plaintext
1188 lines
85 KiB
Plaintext
{
|
|
"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": [
|
|
"<Figure size 432x288 with 2 Axes>"
|
|
]
|
|
},
|
|
"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": [
|
|
"<Figure size 432x288 with 2 Axes>"
|
|
]
|
|
},
|
|
"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<j<t+T-1 \\\\\n",
|
|
"\\end{aligned}\n",
|
|
"\\end{equation}\n",
|
|
"$\n",
|
|
"\n",
|
|
"Other linear constrains may be applied,for instance on the control variable:\n",
|
|
"\n",
|
|
"$ U_{MIN} < u_{j|t} < U_{MAX} \\quad \\textrm{for} \\quad t<j<t+T-1 $\n",
|
|
"\n",
|
|
"The objective fuction accounts for quadratic error on deviation from 0 of the state and the control inputs sequences. Q and R are the **weight matrices** and are used to tune the response.\n",
|
|
"\n",
|
|
"Because the goal is tracking a **reference signal** such as a trajectory, the objective function is rewritten as:\n",
|
|
"\n",
|
|
"$\n",
|
|
"\\begin{equation}\n",
|
|
"\\begin{aligned}\n",
|
|
"\\min_{} \\quad & \\sum^{t+T-1}_{j=t} \\delta x^T_{j|t}Q\\delta x_{j|t} + u^T_{j|t}Ru_{j|t}\n",
|
|
"\\end{aligned}\n",
|
|
"\\end{equation}\n",
|
|
"$\n",
|
|
"\n",
|
|
"where the error w.r.t desired state is accounted for:\n",
|
|
"\n",
|
|
"$ \\delta x = x_{j,t,ref} - x_{j,t} $"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"# Problem Formulation: Study case\n",
|
|
"\n",
|
|
"In this case, the objective function to minimize is given by:\n",
|
|
"\n",
|
|
"https://www.notion.so/Academic-Papers-65e231322112442d80e32c617826b69e#feb1ff08484d43c0b20797c855356e5b\n",
|
|
"\n",
|
|
"$\n",
|
|
"\\begin{equation}\n",
|
|
"\\begin{aligned}\n",
|
|
"\\min_{} \\quad & \\sum^{t+T-1}_{j=t} cte_{j|t,ref}^TQcte_{j,t,ref} + \\psi_{j|t,ref}^TP\\psi_{j|t,ref} + (v_{j|t} - v_{j|t,ref})^TK(v_{j|t} - v_{j|t,ref}) + 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<j<t+T-1 \\\\\n",
|
|
" & a_{MIN} < a_{j|t} < a_{MAX} \\quad \\textrm{for} \\quad t<j<t+T-1 \\\\\n",
|
|
" & \\delta_{MIN} < \\delta_{j|t} < \\delta_{MAX} \\quad \\textrm{for} \\quad t<j<t+T-1 \\\\\n",
|
|
"\\end{aligned}\n",
|
|
"\\end{equation}\n",
|
|
"$\n",
|
|
"\n",
|
|
"\n",
|
|
"Where R,P,K,Q are the cost matrices used to tune the response.\n"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 19,
|
|
"metadata": {},
|
|
"outputs": [
|
|
{
|
|
"name": "stdout",
|
|
"output_type": "stream",
|
|
"text": [
|
|
"-----------------------------------------------------------------\n",
|
|
" OSQP v0.6.0 - Operator Splitting QP Solver\n",
|
|
" (c) Bartolomeo Stellato, Goran Banjac\n",
|
|
" University of Oxford - Stanford University 2019\n",
|
|
"-----------------------------------------------------------------\n",
|
|
"problem: variables n = 322, constraints m = 404\n",
|
|
" nnz(P) + nnz(A) = 1051\n",
|
|
"settings: linear system solver = qdldl,\n",
|
|
" eps_abs = 1.0e-05, eps_rel = 1.0e-05,\n",
|
|
" eps_prim_inf = 1.0e-04, eps_dual_inf = 1.0e-04,\n",
|
|
" rho = 1.00e-01 (adaptive),\n",
|
|
" sigma = 1.00e-06, alpha = 1.60, max_iter = 10000\n",
|
|
" check_termination: on (interval 25),\n",
|
|
" scaling: on, scaled_termination: off\n",
|
|
" warm start: on, polish: on, time_limit: off\n",
|
|
"\n",
|
|
"iter objective pri res dua res rho time\n",
|
|
" 1 0.0000e+00 1.04e+00 1.05e+02 1.00e-01 3.63e-04s\n",
|
|
" 75 1.7529e+03 4.70e-07 1.49e-06 1.55e+00 1.01e-03s\n",
|
|
"\n",
|
|
"status: solved\n",
|
|
"solution polish: unsuccessful\n",
|
|
"number of iterations: 75\n",
|
|
"optimal objective: 1752.9093\n",
|
|
"run time: 1.26e-03s\n",
|
|
"optimal rho estimate: 3.18e+00\n",
|
|
"\n",
|
|
"CPU times: user 118 ms, sys: 0 ns, total: 118 ms\n",
|
|
"Wall time: 115 ms\n"
|
|
]
|
|
}
|
|
],
|
|
"source": [
|
|
"%%time\n",
|
|
"\n",
|
|
"MAX_SPEED = 1.5 #m/s\n",
|
|
"MAX_STEER = np.radians(30) #rad\n",
|
|
"MAX_ACC = 1.0\n",
|
|
"REF_VEL=1.0\n",
|
|
"\n",
|
|
"track = compute_path_from_wp([0,3,6],\n",
|
|
" [0,0,0],0.05)\n",
|
|
"\n",
|
|
"# Starting Condition\n",
|
|
"x0 = np.zeros(N)\n",
|
|
"x0[0] = 0 #x\n",
|
|
"x0[1] = -0.25 #y\n",
|
|
"x0[2] = 0.0 #v\n",
|
|
"x0[3] = np.radians(-0) #yaw\n",
|
|
" \n",
|
|
"#starting guess\n",
|
|
"u_bar = np.zeros((M,T))\n",
|
|
"u_bar[0,:] = MAX_ACC/2 #a\n",
|
|
"u_bar[1,:] = 0.1 #delta\n",
|
|
"\n",
|
|
"# dynamics starting state w.r.t world frame\n",
|
|
"x_bar = np.zeros((N,T+1))\n",
|
|
"x_bar[:,0] = x0\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",
|
|
"x = cp.Variable((N, T+1))\n",
|
|
"u = cp.Variable((M, T))\n",
|
|
"cost = 0\n",
|
|
"constr = []\n",
|
|
"\n",
|
|
"# Cost Matrices\n",
|
|
"Q = np.diag([200,200,100,10]) #state error cost\n",
|
|
"Qf = np.diag([10,10,10,10]) #state final error cost\n",
|
|
"R = np.diag([10,10]) #input cost\n",
|
|
"R_ = np.diag([10,10]) #input rate of change cost\n",
|
|
"\n",
|
|
"#Get Reference_traj\n",
|
|
"x_ref, d_ref = get_ref_trajectory(x_bar[:,0] ,track, REF_VEL)\n",
|
|
"\n",
|
|
"#Prediction Horizon\n",
|
|
"for t in range(T):\n",
|
|
" \n",
|
|
" # Tracking Error\n",
|
|
" cost += cp.quad_form(x[:,t]-x_ref[:,t], Q)\n",
|
|
"\n",
|
|
" # Actuation effort\n",
|
|
" cost += cp.quad_form( u[:, t], R)\n",
|
|
" \n",
|
|
" # Actuation rate of change\n",
|
|
" if t < (T - 1):\n",
|
|
" cost += cp.quad_form(u[:, t + 1] - u[:, t], R_)\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]] #starting condition\n",
|
|
"constr += [x[2, :] <= MAX_SPEED] #max speed\n",
|
|
"constr += [x[2, :] >= 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": [
|
|
"<Figure size 432x288 with 4 Axes>"
|
|
]
|
|
},
|
|
"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<ipython-input-12-8fcc0dfd7424>\u001b[0m in \u001b[0;36m<module>\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
|
|
}
|