1221 lines
217 KiB
Plaintext
1221 lines
217 KiB
Plaintext
|
{
|
||
|
|
"cells": [
|
||
|
|
{
|
||
|
|
"cell_type": "code",
|
||
|
|
"execution_count": 17,
|
||
|
|
"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",
|
||
|
|
"* $\\theta$ heading of the robot\n",
|
||
|
|
"\n",
|
||
|
|
"The inputs of the model are:\n",
|
||
|
|
"\n",
|
||
|
|
"* $v$ linear velocity of the robot\n",
|
||
|
|
"* $w$ 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{\\theta} = w$\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",
|
||
|
|
"* ${\\theta_{t+1}} = \\theta_{t} + w_t 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 w} \\\\\n",
|
||
|
|
"\\end{bmatrix}\n",
|
||
|
|
"\\quad\n",
|
||
|
|
"= \n",
|
||
|
|
"\\quad\n",
|
||
|
|
"\\begin{bmatrix}\n",
|
||
|
|
"\\cos{\\theta} & 0 \\\\\n",
|
||
|
|
"\\sin{\\theta} & 0 \\\\\n",
|
||
|
|
"0 & 1\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": 18,
|
||
|
|
"metadata": {},
|
||
|
|
"outputs": [],
|
||
|
|
"source": [
|
||
|
|
"# Control problem statement.\n",
|
||
|
|
"\n",
|
||
|
|
"N = 3 #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": 19,
|
||
|
|
"metadata": {},
|
||
|
|
"outputs": [],
|
||
|
|
"source": [
|
||
|
|
"def get_linear_model(x_bar,u_bar):\n",
|
||
|
|
" \"\"\"\n",
|
||
|
|
" \"\"\"\n",
|
||
|
|
" \n",
|
||
|
|
" x = x_bar[0]\n",
|
||
|
|
" y = x_bar[1]\n",
|
||
|
|
" theta = x_bar[2]\n",
|
||
|
|
" \n",
|
||
|
|
" v = u_bar[0]\n",
|
||
|
|
" w = u_bar[1]\n",
|
||
|
|
" \n",
|
||
|
|
" A = np.zeros((N,N))\n",
|
||
|
|
" A[0,2]=-v*np.sin(theta)\n",
|
||
|
|
" A[1,2]=v*np.cos(theta)\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,1]=1\n",
|
||
|
|
" B_lin=dt*B\n",
|
||
|
|
" \n",
|
||
|
|
" f_xu=np.array([v*np.cos(theta),v*np.sin(theta),w]).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 A_lin,B_lin,C_lin"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"cell_type": "markdown",
|
||
|
|
"metadata": {},
|
||
|
|
"source": [
|
||
|
|
"Motion Prediction: using scipy intergration"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"cell_type": "code",
|
||
|
|
"execution_count": 20,
|
||
|
|
"metadata": {},
|
||
|
|
"outputs": [],
|
||
|
|
"source": [
|
||
|
|
"# Define process model\n",
|
||
|
|
"# This uses the continuous model \n",
|
||
|
|
"def kinematics_model(x,t,u):\n",
|
||
|
|
" \"\"\"\n",
|
||
|
|
" \"\"\"\n",
|
||
|
|
"\n",
|
||
|
|
" dxdt = u[0]*np.cos(x[2])\n",
|
||
|
|
" dydt = u[0]*np.sin(x[2])\n",
|
||
|
|
" dthetadt = u[1]\n",
|
||
|
|
"\n",
|
||
|
|
" dqdt = [dxdt,\n",
|
||
|
|
" dydt,\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": 21,
|
||
|
|
"metadata": {},
|
||
|
|
"outputs": [
|
||
|
|
{
|
||
|
|
"name": "stdout",
|
||
|
|
"output_type": "stream",
|
||
|
|
"text": [
|
||
|
|
"CPU times: user 8.17 ms, sys: 10 µs, total: 8.18 ms\n",
|
||
|
|
"Wall time: 7.26 ms\n"
|
||
|
|
]
|
||
|
|
}
|
||
|
|
],
|
||
|
|
"source": [
|
||
|
|
"%%time\n",
|
||
|
|
"\n",
|
||
|
|
"u_bar = np.zeros((M,T))\n",
|
||
|
|
"u_bar[0,:] = 1 #m/s\n",
|
||
|
|
"u_bar[1,:] = np.radians(-10) #rad/s\n",
|
||
|
|
"\n",
|
||
|
|
"x0 = np.zeros(N)\n",
|
||
|
|
"x0[0] = 0\n",
|
||
|
|
"x0[1] = 1\n",
|
||
|
|
"x0[2] = 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": 22,
|
||
|
|
"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": 23,
|
||
|
|
"metadata": {},
|
||
|
|
"outputs": [
|
||
|
|
{
|
||
|
|
"name": "stdout",
|
||
|
|
"output_type": "stream",
|
||
|
|
"text": [
|
||
|
|
"CPU times: user 1.72 ms, sys: 0 ns, total: 1.72 ms\n",
|
||
|
|
"Wall time: 1.2 ms\n"
|
||
|
|
]
|
||
|
|
}
|
||
|
|
],
|
||
|
|
"source": [
|
||
|
|
"%%time\n",
|
||
|
|
"\n",
|
||
|
|
"u_bar = np.zeros((M,T))\n",
|
||
|
|
"u_bar[0,:] = 1 #m/s\n",
|
||
|
|
"u_bar[1,:] = np.radians(-10) #rad/s\n",
|
||
|
|
"\n",
|
||
|
|
"x0 = np.zeros(N)\n",
|
||
|
|
"x0[0] = 0\n",
|
||
|
|
"x0[1] = 1\n",
|
||
|
|
"x0[2] = 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": 24,
|
||
|
|
"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": [
|
||
|
|
"------------------\n",
|
||
|
|
"\n",
|
||
|
|
"the kinematics model predictits psi and cte for constant heading references. So, for non-constant paths appropriate functions have to be developed.\n",
|
||
|
|
"\n",
|
||
|
|
"-----------------"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"cell_type": "markdown",
|
||
|
|
"metadata": {},
|
||
|
|
"source": [
|
||
|
|
"## PRELIMINARIES"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"cell_type": "code",
|
||
|
|
"execution_count": 25,
|
||
|
|
"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[2]) - dy * np.sin(-state[2]),\n",
|
||
|
|
" dy * np.cos(-state[2]) + dx * np.sin(-state[2]) ))\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": 26,
|
||
|
|
"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 = 241, constraints m = 281\n",
|
||
|
|
" nnz(P) + nnz(A) = 753\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.06e+02 1.00e-01 2.96e-04s\n",
|
||
|
|
" 125 1.8119e+03 6.13e-05 5.69e-04 4.76e+01 1.06e-03s\n",
|
||
|
|
"\n",
|
||
|
|
"status: solved\n",
|
||
|
|
"solution polish: unsuccessful\n",
|
||
|
|
"number of iterations: 125\n",
|
||
|
|
"optimal objective: 1811.9387\n",
|
||
|
|
"run time: 1.27e-03s\n",
|
||
|
|
"optimal rho estimate: 4.81e+01\n",
|
||
|
|
"\n",
|
||
|
|
"CPU times: user 237 ms, sys: 4 ms, total: 241 ms\n",
|
||
|
|
"Wall time: 236 ms\n"
|
||
|
|
]
|
||
|
|
}
|
||
|
|
],
|
||
|
|
"source": [
|
||
|
|
"%%time\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 function\n",
|
||
|
|
" #cost += 10*cp.sum_squares( x[3, t]) # psi\n",
|
||
|
|
" #cost += 500*cp.sum_squares( x[4, t]) # cte\n",
|
||
|
|
" \n",
|
||
|
|
" cost += 10*cp.sum_squares(x[2,t]-np.arctan(df(x_bar[0,t],K))) # psi\n",
|
||
|
|
" cost += 500*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], 100*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 += [u[0, :] <= MAX_SPEED]\n",
|
||
|
|
"constr += [u[0, :] >= MIN_SPEED]\n",
|
||
|
|
"constr += [cp.abs(u[1, :]) <= MAX_STEER_SPEED]\n",
|
||
|
|
"\n",
|
||
|
|
"prob = cp.Problem(cp.Minimize(cost), constr)\n",
|
||
|
|
"solution = prob.solve(solver=cp.OSQP, verbose=True)"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"cell_type": "code",
|
||
|
|
"execution_count": 27,
|
||
|
|
"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": 28,
|
||
|
|
"metadata": {},
|
||
|
|
"outputs": [
|
||
|
|
{
|
||
|
|
"name": "stderr",
|
||
|
|
"output_type": "stream",
|
||
|
|
"text": [
|
||
|
|
"<ipython-input-25-0756f6b412cb>:29: RuntimeWarning: invalid value encountered in true_divide\n",
|
||
|
|
" v /= np.linalg.norm(v)\n",
|
||
|
|
"<ipython-input-28-79710852001d>:33: RankWarning: Polyfit may be poorly conditioned\n",
|
||
|
|
" K=road_curve(x_sim[:,sim_time],track)\n",
|
||
|
|
"<ipython-input-28-79710852001d>:33: RankWarning: Polyfit may be poorly conditioned\n",
|
||
|
|
" K=road_curve(x_sim[:,sim_time],track)\n"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"name": "stdout",
|
||
|
|
"output_type": "stream",
|
||
|
|
"text": [
|
||
|
|
"CVXPY Optimization Time: Avrg: 0.1897s Max: 0.2940s Min: 0.1666s\n"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"name": "stderr",
|
||
|
|
"output_type": "stream",
|
||
|
|
"text": [
|
||
|
|
"<ipython-input-28-79710852001d>:33: RankWarning: Polyfit may be poorly conditioned\n",
|
||
|
|
" K=road_curve(x_sim[:,sim_time],track)\n"
|
||
|
|
]
|
||
|
|
}
|
||
|
|
],
|
||
|
|
"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_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",
|
||
|
|
" 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": 29,
|
||
|
|
"metadata": {},
|
||
|
|
"outputs": [
|
||
|
|
{
|
||
|
|
"data": {
|
||
|
|
"image/png": "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
|
||
|
|
"text/plain": [
|
||
|
|
"<Figure size 1080x720 with 3 Axes>"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
"metadata": {},
|
||
|
|
"output_type": "display_data"
|
||
|
|
}
|
||
|
|
],
|
||
|
|
"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": 51,
|
||
|
|
"metadata": {},
|
||
|
|
"outputs": [
|
||
|
|
{
|
||
|
|
"name": "stderr",
|
||
|
|
"output_type": "stream",
|
||
|
|
"text": [
|
||
|
|
"<ipython-input-25-0756f6b412cb>:29: RuntimeWarning: invalid value encountered in true_divide\n",
|
||
|
|
" v /= np.linalg.norm(v)\n",
|
||
|
|
"<ipython-input-51-a1c8be923656>:44: RankWarning: Polyfit may be poorly conditioned\n",
|
||
|
|
" K=road_curve(x_sim[:,sim_time],track)\n",
|
||
|
|
"<ipython-input-51-a1c8be923656>:44: RankWarning: Polyfit may be poorly conditioned\n",
|
||
|
|
" K=road_curve(x_sim[:,sim_time],track)\n",
|
||
|
|
"<ipython-input-51-a1c8be923656>:44: RankWarning: Polyfit may be poorly conditioned\n",
|
||
|
|
" K=road_curve(x_sim[:,sim_time],track)\n",
|
||
|
|
"<ipython-input-51-a1c8be923656>:44: RankWarning: Polyfit may be poorly conditioned\n",
|
||
|
|
" K=road_curve(x_sim[:,sim_time],track)\n",
|
||
|
|
"<ipython-input-51-a1c8be923656>:44: RankWarning: Polyfit may be poorly conditioned\n",
|
||
|
|
" K=road_curve(x_sim[:,sim_time],track)\n",
|
||
|
|
"<ipython-input-51-a1c8be923656>:44: RankWarning: Polyfit may be poorly conditioned\n",
|
||
|
|
" K=road_curve(x_sim[:,sim_time],track)\n",
|
||
|
|
"<ipython-input-51-a1c8be923656>:44: RankWarning: Polyfit may be poorly conditioned\n",
|
||
|
|
" K=road_curve(x_sim[:,sim_time],track)\n"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"name": "stdout",
|
||
|
|
"output_type": "stream",
|
||
|
|
"text": [
|
||
|
|
"CVXPY Optimization Time: Avrg: 21.4061s Max: 49.1719s Min: 1.2366s\n"
|
||
|
|
]
|
||
|
|
}
|
||
|
|
],
|
||
|
|
"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": 52,
|
||
|
|
"metadata": {},
|
||
|
|
"outputs": [
|
||
|
|
{
|
||
|
|
"data": {
|
||
|
|
"image/png": "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
|
||
|
|
"text/plain": [
|
||
|
|
"<Figure size 1080x720 with 3 Axes>"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
"metadata": {},
|
||
|
|
"output_type": "display_data"
|
||
|
|
}
|
||
|
|
],
|
||
|
|
"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
|
||
|
|
}
|