1180 lines
72 KiB
Plaintext
1180 lines
72 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",
|
||
|
|
"* $v$ linear velocity of the robot\n",
|
||
|
|
"* $\\delta$ angular velocity 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 \\theta} \\\\\n",
|
||
|
|
"\\end{bmatrix}\n",
|
||
|
|
"\\quad\n",
|
||
|
|
"=\n",
|
||
|
|
"\\quad\n",
|
||
|
|
"\\begin{bmatrix}\n",
|
||
|
|
"0 & 0 & -v\\sin{\\theta} \\\\\n",
|
||
|
|
"0 & 0 & v\\cos{\\theta} \\\\\n",
|
||
|
|
"0 & 0 & 0\\\\\n",
|
||
|
|
"\\end{bmatrix}\n",
|
||
|
|
"\\quad\n",
|
||
|
|
"$\n",
|
||
|
|
"\n",
|
||
|
|
"and\n",
|
||
|
|
"\n",
|
||
|
|
"$\n",
|
||
|
|
"B = \n",
|
||
|
|
"\\quad\n",
|
||
|
|
"\\begin{bmatrix}\n",
|
||
|
|
"\\frac{\\partial f(x,u)}{\\partial v} & \\frac{\\partial f(x,u)}{\\partial \\delta} \\\\\n",
|
||
|
|
"\\end{bmatrix}\n",
|
||
|
|
"\\quad\n",
|
||
|
|
"= \n",
|
||
|
|
"\\quad\n",
|
||
|
|
"\\begin{bmatrix}\n",
|
||
|
|
"\\cos{\\theta} & 0 \\\\\n",
|
||
|
|
"\\sin{\\theta} & 0 \\\\\n",
|
||
|
|
"\\frac{\\tan{\\delta}}{L} & \\frac{v}{L{\\cos{\\delta}}^2}\n",
|
||
|
|
"\\end{bmatrix}\n",
|
||
|
|
"\\quad\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": [
|
||
|
|
"# Control problem statement.\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",
|
||
|
|
" \"\"\"\n",
|
||
|
|
" L=0.3\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[1,2]=np.sin(theta)\n",
|
||
|
|
" A[0,3]=-v*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[0,0]=np.cos(theta)\n",
|
||
|
|
" B[1,0]=np.sin(theta)\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",
|
||
|
|
" \"\"\"\n",
|
||
|
|
" \n",
|
||
|
|
" #[x,y,v,th]\n",
|
||
|
|
" #[a,d]\n",
|
||
|
|
" \n",
|
||
|
|
" L=0.3\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_bar = np.zeros((N,T+1))\n",
|
||
|
|
" \n",
|
||
|
|
" x_bar[:,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_bar[:,t]=x_next[1]\n",
|
||
|
|
" \n",
|
||
|
|
" return x_bar"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"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.65 ms, sys: 0 ns, total: 3.65 ms\n",
|
||
|
|
"Wall time: 3.47 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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
|
||
|
|
"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.48 ms, sys: 0 ns, total: 2.48 ms\n",
|
||
|
|
"Wall time: 1.85 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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
|
||
|
|
"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",
|
||
|
|
" 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",
|
||
|
|
" return np.vstack((final_xp,final_yp))\n",
|
||
|
|
"\n",
|
||
|
|
"def get_nn_idx(state,path):\n",
|
||
|
|
"\n",
|
||
|
|
" dx = state[0]-path[0,:]\n",
|
||
|
|
" dy = state[1]-path[1,:]\n",
|
||
|
|
" dist = np.sqrt(dx**2 + dy**2)\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 road_curve(state,track):\n",
|
||
|
|
" \n",
|
||
|
|
" #given vehicle pos find lookahead waypoints\n",
|
||
|
|
" nn_idx=get_nn_idx(state,track)-1\n",
|
||
|
|
" LOOKAHED=6\n",
|
||
|
|
" lk_wp=track[:,nn_idx:nn_idx+LOOKAHED]\n",
|
||
|
|
"\n",
|
||
|
|
" #trasform lookahead waypoints to vehicle ref frame\n",
|
||
|
|
" dx = lk_wp[0,:] - state[0]\n",
|
||
|
|
" dy = lk_wp[1,:] - state[1]\n",
|
||
|
|
"\n",
|
||
|
|
" wp_vehicle_frame = np.vstack(( dx * np.cos(-state[3]) - dy * np.sin(-state[3]),\n",
|
||
|
|
" dy * np.cos(-state[3]) + dx * np.sin(-state[3]) ))\n",
|
||
|
|
"\n",
|
||
|
|
" #fit poly\n",
|
||
|
|
" return np.polyfit(wp_vehicle_frame[0,:], wp_vehicle_frame[1,:], 3, rcond=None, full=False, w=None, cov=False)\n",
|
||
|
|
"\n",
|
||
|
|
"def f(x,coeff):\n",
|
||
|
|
" return round(coeff[0]*x**3 + coeff[1]*x**2 + coeff[2]*x**1 + coeff[3]*x**0,6)\n",
|
||
|
|
"\n",
|
||
|
|
"# def f(x,coeff):\n",
|
||
|
|
"# return round(coeff[0]*x**5+coeff[1]*x**4+coeff[2]*x**3+coeff[3]*x**2+coeff[4]*x**1+coeff[5]*x**0,6)\n",
|
||
|
|
"\n",
|
||
|
|
"def df(x,coeff):\n",
|
||
|
|
" return round(3*coeff[0]*x**2 + 2*coeff[1]*x**1 + coeff[2]*x**0,6)\n",
|
||
|
|
"# def df(x,coeff):\n",
|
||
|
|
"# return round(5*coeff[0]*x**4 + 4*coeff[1]*x**3 +3*coeff[2]*x**2 + 2*coeff[3]*x**1 + coeff[4]*x**0,6)"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"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 the sum of the **cross-track error** plus **heading error**\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} + 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",
|
||
|
|
"where R,P,Q are the cost matrices used to tune the response.\n",
|
||
|
|
"\n",
|
||
|
|
"## Error Formulation\n",
|
||
|
|
"\n",
|
||
|
|
"The track can be represented by fitting a curve trough its waypoints, using the vehicle position as reference\n",
|
||
|
|
"\n",
|
||
|
|
"\n",
|
||
|
|
"\n",
|
||
|
|
"A fitted cubic poly has the form:\n",
|
||
|
|
"\n",
|
||
|
|
"$\n",
|
||
|
|
"f = K_0 * x^3 + K_1 * x^2 + K_2 * x + K_3\n",
|
||
|
|
"$\n",
|
||
|
|
"\n",
|
||
|
|
"The derivative of a fitted cubic poly has the form:\n",
|
||
|
|
"\n",
|
||
|
|
"$\n",
|
||
|
|
"f' = 3.0 * K_0 * x^2 + 2.0 * K_1 * x + K_2\n",
|
||
|
|
"$\n",
|
||
|
|
"\n",
|
||
|
|
"\n",
|
||
|
|
"* **crosstrack error** cte: desired y-position - y-position of vehicle -> this is the value of the fitted polynomial\n",
|
||
|
|
"\n",
|
||
|
|
"* **heading error** epsi: desired heading - heading of vehicle -> is the inclination of tangent to the fitted polynomial\n",
|
||
|
|
"\n",
|
||
|
|
"Then using the fitted polynomial representation in vehicle frame the errors can be easily computed as:\n",
|
||
|
|
"\n",
|
||
|
|
"$\n",
|
||
|
|
"cte = f(px) \\\\\n",
|
||
|
|
"\\psi = -atan(f`(px)) \\\\\n",
|
||
|
|
"$"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"cell_type": "code",
|
||
|
|
"execution_count": 22,
|
||
|
|
"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 = 282, constraints m = 364\n",
|
||
|
|
" nnz(P) + nnz(A) = 975\n",
|
||
|
|
"settings: linear system solver = qdldl,\n",
|
||
|
|
" eps_abs = 1.0e-04, eps_rel = 1.0e-04,\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.00e+00 1.00e+02 1.00e-01 3.06e-04s\n",
|
||
|
|
" 50 3.9564e-01 4.47e-10 6.69e-10 1.00e-01 6.77e-04s\n",
|
||
|
|
"\n",
|
||
|
|
"status: solved\n",
|
||
|
|
"solution polish: unsuccessful\n",
|
||
|
|
"number of iterations: 50\n",
|
||
|
|
"optimal objective: 0.3956\n",
|
||
|
|
"run time: 9.19e-04s\n",
|
||
|
|
"optimal rho estimate: 4.67e-02\n",
|
||
|
|
"\n",
|
||
|
|
"CPU times: user 284 ms, sys: 0 ns, total: 284 ms\n",
|
||
|
|
"Wall time: 281 ms\n"
|
||
|
|
]
|
||
|
|
}
|
||
|
|
],
|
||
|
|
"source": [
|
||
|
|
"%%time\n",
|
||
|
|
"\n",
|
||
|
|
"MAX_SPEED = 1.25\n",
|
||
|
|
"MIN_SPEED = 0.75\n",
|
||
|
|
"MAX_STEER = 1.57\n",
|
||
|
|
"MAX_ACC = 1.0\n",
|
||
|
|
"\n",
|
||
|
|
"track = compute_path_from_wp([0,3],\n",
|
||
|
|
" [0,0],0.5)\n",
|
||
|
|
"\n",
|
||
|
|
"# Starting Condition\n",
|
||
|
|
"x0 = np.zeros(N)\n",
|
||
|
|
"x0[0] = 0\n",
|
||
|
|
"x0[1] = -0.25\n",
|
||
|
|
"x0[2] = 1\n",
|
||
|
|
"x0[3] = np.radians(-0)\n",
|
||
|
|
" \n",
|
||
|
|
"#starting guess\n",
|
||
|
|
"u_bar = np.zeros((M,T))\n",
|
||
|
|
"u_bar[0,:]=0.01\n",
|
||
|
|
"u_bar[1,:]=0.01\n",
|
||
|
|
"\n",
|
||
|
|
" \n",
|
||
|
|
"K=road_curve(x0,track)\n",
|
||
|
|
"\n",
|
||
|
|
"# dynamics starting state w.r.t vehicle frame\n",
|
||
|
|
"x_bar=np.zeros((N,T+1))\n",
|
||
|
|
"x_bar[2]=x0[2]\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",
|
||
|
|
"\n",
|
||
|
|
"x = cp.Variable((N, T+1))\n",
|
||
|
|
"u = cp.Variable((M, T))\n",
|
||
|
|
"\n",
|
||
|
|
"#CVXPY Linear MPC problem statement\n",
|
||
|
|
"cost = 0\n",
|
||
|
|
"constr = []\n",
|
||
|
|
"\n",
|
||
|
|
"for t in range(T):\n",
|
||
|
|
" \n",
|
||
|
|
" cost += 1*cp.sum_squares(x[3,t]-np.arctan(df(x_bar[0,t],K))) # psi\n",
|
||
|
|
" cost += 1*cp.sum_squares(f(x_bar[0,t],K)-x[1,t]) # cte\n",
|
||
|
|
" # Actuation effort\n",
|
||
|
|
" cost += cp.quad_form( u[:, t],1*np.eye(M))\n",
|
||
|
|
" \n",
|
||
|
|
" # Actuation rate of change\n",
|
||
|
|
" if t < (T - 1):\n",
|
||
|
|
" cost += cp.quad_form(u[:, t + 1] - u[:, t], 1*np.eye(M))\n",
|
||
|
|
" \n",
|
||
|
|
" # KINEMATICS constrains\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, :] >= MIN_SPEED]\n",
|
||
|
|
"constr += [cp.abs(u[0, :]) <= MAX_ACC]\n",
|
||
|
|
"constr += [cp.abs(u[1, :]) <= 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": 24,
|
||
|
|
"metadata": {},
|
||
|
|
"outputs": [
|
||
|
|
{
|
||
|
|
"data": {
|
||
|
|
"image/png": "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
|
||
|
|
"text/plain": [
|
||
|
|
"<Figure size 432x288 with 2 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",
|
||
|
|
"theta_mpc=np.array(x.value[2, :]).flatten()\n",
|
||
|
|
"v_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((v_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(v_mpc)\n",
|
||
|
|
"plt.ylabel('v(t)')\n",
|
||
|
|
"#plt.xlabel('time')\n",
|
||
|
|
"\n",
|
||
|
|
"#plot w(t)\n",
|
||
|
|
"# plt.subplot(2, 2, 3)\n",
|
||
|
|
"# plt.plot(w_mpc)\n",
|
||
|
|
"# plt.ylabel('w(t)')\n",
|
||
|
|
"#plt.xlabel('time')\n",
|
||
|
|
"\n",
|
||
|
|
"# plt.subplot(2, 2, 3)\n",
|
||
|
|
"# plt.plot(psi_mpc)\n",
|
||
|
|
"# plt.ylabel('psi(t)')\n",
|
||
|
|
"\n",
|
||
|
|
"# plt.subplot(2, 2, 4)\n",
|
||
|
|
"# plt.plot(cte_mpc)\n",
|
||
|
|
"# plt.ylabel('cte(t)')\n",
|
||
|
|
"\n",
|
||
|
|
"plt.tight_layout()\n",
|
||
|
|
"plt.show()"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"cell_type": "markdown",
|
||
|
|
"metadata": {},
|
||
|
|
"source": [
|
||
|
|
"full track demo"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"cell_type": "code",
|
||
|
|
"execution_count": null,
|
||
|
|
"metadata": {},
|
||
|
|
"outputs": [],
|
||
|
|
"source": [
|
||
|
|
"track = compute_path_from_wp([0,3,4,6,10,13],\n",
|
||
|
|
" [0,0,2,4,3,3],0.25)\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 = 1.57\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] = 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.1\n",
|
||
|
|
"\n",
|
||
|
|
"for sim_time in range(sim_duration-1):\n",
|
||
|
|
" \n",
|
||
|
|
" iter_start=time.time()\n",
|
||
|
|
" \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",
|
||
|
|
" \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",
|
||
|
|
" # 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(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(2, 3)\n",
|
||
|
|
"\n",
|
||
|
|
"plt.figure(figsize=(15,10))\n",
|
||
|
|
"\n",
|
||
|
|
"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",
|
||
|
|
"plt.axis(\"equal\")\n",
|
||
|
|
"plt.ylabel('y')\n",
|
||
|
|
"plt.xlabel('x')\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",
|
||
|
|
"plt.tight_layout()\n",
|
||
|
|
"plt.show()"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"cell_type": "markdown",
|
||
|
|
"metadata": {},
|
||
|
|
"source": [
|
||
|
|
"## OBSTACLE AVOIDANCE\n",
|
||
|
|
"see dccp paper for reference"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"cell_type": "code",
|
||
|
|
"execution_count": null,
|
||
|
|
"metadata": {},
|
||
|
|
"outputs": [],
|
||
|
|
"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": "code",
|
||
|
|
"execution_count": null,
|
||
|
|
"metadata": {},
|
||
|
|
"outputs": [],
|
||
|
|
"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 [conda env:jupyter] *",
|
||
|
|
"language": "python",
|
||
|
|
"name": "conda-env-jupyter-py"
|
||
|
|
},
|
||
|
|
"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.3"
|
||
|
|
}
|
||
|
|
},
|
||
|
|
"nbformat": 4,
|
||
|
|
"nbformat_minor": 4
|
||
|
|
}
|