2019-11-27 21:15:13 +08:00
{
"cells": [
{
"cell_type": "code",
2019-11-28 00:09:38 +08:00
"execution_count": 1,
2019-11-27 21:15:13 +08:00
"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",
"* $\\psi$ heading error = $\\psi = \\theta_{ref} - \\theta$\n",
2019-11-28 00:09:38 +08:00
"* $cte$ crosstrack error = lateral distance of the robot from the path \n",
2019-11-27 21:15:13 +08:00
"\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} = v\\cos{\\theta}$ \n",
"* $\\dot{y} = v\\sin{\\theta}$\n",
"* $\\dot{\\theta} = w$\n",
"* $\\dot{\\psi} = -w$\n",
2019-11-28 00:09:38 +08:00
"* $\\dot{cte} = v\\sin{-\\psi}$\n",
2019-11-27 21:15:13 +08:00
"\n",
"The **Continuous** State Space Model is\n",
"\n",
2019-11-28 00:09:38 +08:00
"$ {\\dot{x}} = Ax + Bu $\n",
2019-11-27 21:15:13 +08:00
"\n",
"with:\n",
"\n",
2019-11-28 00:09:38 +08:00
"$ A =\n",
"\\quad\n",
2019-11-27 21:15:13 +08:00
"\\begin{bmatrix}\n",
"\\frac{\\partial f(x,u)}{\\partial x} & \\frac{\\partial f(x,u)}{\\partial y} & \\frac{\\partial f(x,u)}{\\partial \\theta} & \\frac{\\partial f(x,u)}{\\partial \\psi} & \\frac{\\partial f(x,u)}{\\partial cte} \\\\\n",
"\\end{bmatrix}\n",
"\\quad\n",
"=\n",
"\\quad\n",
"\\begin{bmatrix}\n",
"0 & 0 & -vsin(\\theta) & 0 & 0 \\\\\n",
"0 & 0 & vcos(\\theta) & 0 & 0 \\\\\n",
"0 & 0 & 0 & 0 & 0 \\\\\n",
"0 & 0 & 0 & 0 & 0 \\\\\n",
2019-11-28 00:09:38 +08:00
"0 & 0 & 0 & vcos(-\\psi) & 0 \n",
2019-11-27 21:15:13 +08:00
"\\end{bmatrix}\n",
2019-11-28 00:09:38 +08:00
"\\quad $\n",
2019-11-27 21:15:13 +08:00
"\n",
2019-11-28 00:09:38 +08:00
"\n",
"$ B = \\quad\n",
2019-11-27 21:15:13 +08:00
"\\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_t} & 0 \\\\\n",
"\\sin{\\theta_t} & 0 \\\\\n",
"0 & 1 \\\\\n",
"0 & -1 \\\\\n",
"\\sin{(-\\psi_t)} & 0 \n",
"\\end{bmatrix}\n",
2019-11-28 00:09:38 +08:00
"\\quad $\n",
2019-11-27 21:15:13 +08:00
"\n",
2019-11-28 00:09:38 +08:00
"discretize with forward Euler Integration for time step dt:\n",
2019-11-27 21:15:13 +08:00
"\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",
"* ${\\psi_{t+1}} = \\psi_{t} - w_t*dt$\n",
"* ${cte_{t+1}} = cte_{t} + v_t\\sin{-\\psi}*dt$\n",
"\n",
2019-11-28 00:09:38 +08:00
"The **Discrete** State Space Model is then:\n",
2019-11-27 21:15:13 +08:00
"\n",
"${x_{t+1}} = Ax_t + Bu_t $\n",
"\n",
"with:\n",
"\n",
"$\n",
"A = \\quad\n",
"\\begin{bmatrix}\n",
"1 & 0 & -v\\sin{\\theta}dt & 0 & 0 \\\\\n",
"0 & 1 & v\\cos{\\theta}dt & 0 & 0 \\\\\n",
"0 & 0 & 1 & 0 & 0 \\\\\n",
"0 & 0 & 0 & 1 & 0 \\\\\n",
2019-11-28 00:09:38 +08:00
"0 & 0 & 0 & vcos{(-\\psi)}dt & 1 \n",
2019-11-27 21:15:13 +08:00
"\\end{bmatrix}\n",
"\\quad\n",
"$\n",
"\n",
"$\n",
"B = \\quad\n",
"\\begin{bmatrix}\n",
"\\cos{\\theta_t}dt & 0 \\\\\n",
"\\sin{\\theta_t}dt & 0 \\\\\n",
"0 & dt \\\\\n",
"0 & -dt \\\\\n",
"\\sin{-\\psi_t}dt & 0 \n",
"\\end{bmatrix}\n",
"\\quad\n",
"$\n",
"\n",
"This State Space Model is not linear (A,B are time changing), to linearize it the **Taylor's series expansion** is used around $\\bar{x}$ and $\\bar{u}$:\n",
"\n",
"$ \\dot{x} = f(x,u) \\approx f(\\bar{x},\\bar{u}) + A(x-\\bar{x}) + B(u-\\bar{u})$\n",
"\n",
"So:\n",
"\n",
2019-11-28 00:09:38 +08:00
"$ x{t+1} = x_t + (f(\\bar{x},\\bar{u}) + A(x-\\bar{x}) + B(u-\\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 Discrete linearized kinematics model is\n",
"\n",
"$ x_{t+1} = A'x_t + B' u_t + C' $\n",
"\n",
"with:\n",
"\n",
2019-11-29 22:24:08 +08:00
"$ A' = I+dtA $\n",
2019-11-28 00:09:38 +08:00
"\n",
2019-11-29 22:24:08 +08:00
"$ B' = dtB $\n",
2019-11-27 21:15:13 +08:00
"\n",
2019-11-29 22:24:08 +08:00
"$ C' = dt(f(\\bar{x},\\bar{u}) - A\\bar{x} - B\\bar{u}) $"
2019-11-27 21:15:13 +08:00
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
2019-11-28 00:09:38 +08:00
"------------------\n",
2019-11-29 22:24:08 +08:00
"NB: psi and cte are expressed w.r.t. the track as reference frame.\n",
"In the reference frame of the veicle the equtions would be:\n",
2019-11-27 21:15:13 +08:00
"* psi_dot = w\n",
2019-11-28 00:09:38 +08:00
"* cte_dot = sin(psi)\n",
"-----------------"
2019-11-27 21:15:13 +08:00
]
},
2019-11-28 01:46:53 +08:00
{
"cell_type": "markdown",
"metadata": {},
"source": [
"### Kinematics Model"
]
},
2019-11-27 21:15:13 +08:00
{
"cell_type": "code",
2019-11-28 00:09:38 +08:00
"execution_count": 2,
2019-11-27 21:15:13 +08:00
"metadata": {},
"outputs": [],
"source": [
"# Control problem statement.\n",
"\n",
"N = 5 #number of state variables\n",
"M = 2 #number of control variables\n",
"T = 20 #Prediction Horizon\n",
"dt = 0.25 #discretization step\n",
"\n",
"x = cp.Variable((N, T+1))\n",
2019-11-28 00:09:38 +08:00
"u = cp.Variable((M, T))"
2019-11-27 21:15:13 +08:00
]
},
{
"cell_type": "code",
2019-11-28 00:09:38 +08:00
"execution_count": 3,
2019-11-27 21:15:13 +08:00
"metadata": {},
"outputs": [],
"source": [
"def get_linear_model(x_bar,u_bar):\n",
2019-11-28 01:46:53 +08:00
" \"\"\"\n",
" \"\"\"\n",
2019-11-27 21:15:13 +08:00
" \n",
" x = x_bar[0]\n",
" y = x_bar[1]\n",
" theta = x_bar[2]\n",
" psi = x_bar[3]\n",
" cte = x_bar[4]\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",
2019-11-28 00:09:38 +08:00
" A[4,3]=v*np.cos(-psi)\n",
2019-11-27 21:15:13 +08:00
" 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[3,1]=-1\n",
" B[4,0]=np.sin(-psi)\n",
" B_lin=dt*B\n",
" \n",
" f_xu=np.array([v*np.cos(theta),v*np.sin(theta),w,-w,v*np.sin(-psi)]).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",
2019-11-28 00:09:38 +08:00
"execution_count": 4,
2019-11-27 21:15:13 +08:00
"metadata": {},
"outputs": [],
"source": [
"# Define process model\n",
"def kinematics_model(x,t,u):\n",
2019-11-28 01:46:53 +08:00
" \"\"\"\n",
" \"\"\"\n",
2019-11-27 21:15:13 +08:00
"\n",
" dxdt = u[0]*np.cos(x[2])\n",
" dydt = u[0]*np.sin(x[2])\n",
" dthetadt = u[1]\n",
" dpsidt = -u[1]\n",
" dctedt = u[0]*np.sin(-x[3])\n",
"\n",
" dqdt = [dxdt,\n",
" dydt,\n",
" dthetadt,\n",
" dpsidt,\n",
" dctedt]\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",
2019-11-28 00:09:38 +08:00
"execution_count": 5,
2019-11-27 21:15:13 +08:00
"metadata": {},
2019-11-28 00:09:38 +08:00
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
2019-11-28 01:46:53 +08:00
"CPU times: user 4.18 ms, sys: 92 µs, total: 4.27 ms\n",
"Wall time: 3.79 ms\n"
2019-11-28 00:09:38 +08:00
]
}
],
2019-11-27 21:15:13 +08:00
"source": [
2019-11-28 00:09:38 +08:00
"%%time\n",
"\n",
2019-11-27 21:15:13 +08:00
"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",
"x0[3] = 0\n",
"x0[4] = 1\n",
"\n",
"x_bar=predict(x0,u_bar)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Check the model prediction"
]
},
{
"cell_type": "code",
2019-11-28 00:09:38 +08:00
"execution_count": 6,
2019-11-27 21:15:13 +08:00
"metadata": {},
"outputs": [
{
"data": {
"image/png": "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
"text/plain": [
"<Figure size 432x288 with 4 Axes>"
]
},
"metadata": {
"needs_background": "light"
},
"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",
"plt.subplot(2, 2, 3)\n",
"plt.plot(np.degrees(x_bar[3,:]))\n",
"plt.ylabel('psi(t) [deg]')\n",
"#plt.xlabel('time')\n",
"\n",
"plt.subplot(2, 2, 4)\n",
"plt.plot(x_bar[4,:])\n",
"plt.ylabel('cte(t)')\n",
"\n",
"plt.tight_layout()\n",
"plt.show()"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"the results seems valid:\n",
"* the cte is correct\n",
"* theta error is correct"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Motion Prediction: using the state space model"
]
},
{
"cell_type": "code",
2019-11-28 00:09:38 +08:00
"execution_count": 7,
2019-11-27 21:15:13 +08:00
"metadata": {},
2019-11-28 00:09:38 +08:00
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
2019-11-28 01:46:53 +08:00
"CPU times: user 2.34 ms, sys: 1.92 ms, total: 4.26 ms\n",
"Wall time: 1.24 ms\n"
2019-11-28 00:09:38 +08:00
]
}
],
2019-11-27 21:15:13 +08:00
"source": [
2019-11-28 00:09:38 +08:00
"%%time\n",
"\n",
2019-11-27 21:15:13 +08:00
"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",
"x0[3] = 0\n",
"x0[4] = 1\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(5,1)\n",
" ut=u_bar[:,t-1].reshape(2,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",
2019-11-28 00:09:38 +08:00
"execution_count": 8,
2019-11-27 21:15:13 +08:00
"metadata": {},
"outputs": [
{
"data": {
"image/png": "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
"text/plain": [
"<Figure size 432x288 with 4 Axes>"
]
},
"metadata": {
"needs_background": "light"
},
"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(x_bar[2,:])\n",
"plt.ylabel('theta(t)')\n",
"#plt.xlabel('time')\n",
"\n",
"plt.subplot(2, 2, 3)\n",
"plt.plot(x_bar[3,:])\n",
"plt.ylabel('psi(t)')\n",
"#plt.xlabel('time')\n",
"\n",
"plt.subplot(2, 2, 4)\n",
"plt.plot(x_bar[4,:])\n",
"plt.ylabel('cte(t)')\n",
"\n",
"plt.tight_layout()\n",
"plt.show()"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"The results are the same as expected"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
2019-11-28 00:09:38 +08:00
"------------------\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",
"-----------------"
2019-11-27 21:15:13 +08:00
]
},
{
"cell_type": "code",
2019-11-28 00:09:38 +08:00
"execution_count": 9,
2019-11-27 21:15:13 +08:00
"metadata": {},
"outputs": [],
"source": [
"def calc_err(state,path):\n",
" \"\"\"\n",
" Finds psi and cte w.r.t. the closest waypoint.\n",
"\n",
" :param state: array_like, state of the vehicle [x_pos, y_pos, theta]\n",
" :param path: array_like, reference path ((x1, x2, ...), (y1, y2, ...), (th1 ,th2, ...)]\n",
" :returns: (float,float)\n",
" \"\"\"\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",
"# front_axle_vect = [np.cos(state[2] - np.pi / 2),\n",
"# np.sin(state[2] - np.pi / 2)]\n",
" path_ref_vect = [np.cos(path[2,target_idx] + np.pi / 2),\n",
" np.sin(path[2,target_idx] + np.pi / 2)]\n",
" \n",
" #heading error w.r.t path frame\n",
" psi = path[2,target_idx] - state[2]\n",
" \n",
" # the cross-track error is given by the scalar projection of the car->wp vector onto the faxle versor\n",
" #cte = np.dot([dx[target_idx], dy[target_idx]],front_axle_vect)\n",
" cte = np.dot([dx[target_idx], dy[target_idx]],path_ref_vect)\n",
"\n",
" return target_idx,psi,cte\n",
"\n",
"def compute_path_from_wp(start_xp, start_yp, step = 0.1):\n",
2019-11-28 01:46:53 +08:00
" \"\"\"\n",
2019-11-27 21:15:13 +08:00
" Interpolation range is computed to assure one point every fixed distance step [m].\n",
" \n",
" :param start_xp: array_like, list of starting x coordinates\n",
" :param start_yp: array_like, list of starting y coordinates\n",
" :param step: float, interpolation distance [m] between consecutive waypoints\n",
" :returns: array_like, of shape (3,N)\n",
2019-11-28 01:46:53 +08:00
" \"\"\"\n",
2019-11-27 21:15:13 +08:00
"\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,section_len/delta)\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))"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"test it"
]
},
{
"cell_type": "code",
2019-11-28 00:09:38 +08:00
"execution_count": 10,
2019-11-27 21:15:13 +08:00
"metadata": {},
"outputs": [],
"source": [
"track = compute_path_from_wp([0,5],[0,0])\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",
"_,psi,cte = calc_err(x0,track)\n",
"x0[3]=psi\n",
"x0[4]=cte\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(5,1)\n",
" ut=u_bar[:,t-1].reshape(2,1)\n",
" \n",
" A,B,C=get_linear_model(xt,ut)\n",
" \n",
" xt_plus_one = np.squeeze(np.dot(A,xt)+np.dot(B,ut)+C)\n",
" \n",
" _,psi,cte = calc_err(xt_plus_one,track)\n",
" xt_plus_one[3]=psi\n",
" xt_plus_one[4]=cte\n",
" \n",
" x_bar[:,t]= xt_plus_one"
]
},
{
"cell_type": "code",
2019-11-28 00:09:38 +08:00
"execution_count": 11,
2019-11-27 21:15:13 +08:00
"metadata": {},
"outputs": [
{
"data": {
"image/png": "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
"text/plain": [
"<Figure size 432x288 with 4 Axes>"
]
},
"metadata": {
"needs_background": "light"
},
"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(track[0,:],track[1,:],\"b-\")\n",
"plt.axis('equal')\n",
"plt.ylabel('y')\n",
"plt.xlabel('x')\n",
"\n",
"plt.subplot(2, 2, 2)\n",
"plt.plot(x_bar[2,:])\n",
"plt.ylabel('theta(t)')\n",
"#plt.xlabel('time')\n",
"\n",
"plt.subplot(2, 2, 3)\n",
"plt.plot(x_bar[3,:])\n",
"plt.ylabel('psi(t)')\n",
"#plt.xlabel('time')\n",
"\n",
"plt.subplot(2, 2, 4)\n",
"plt.plot(x_bar[4,:])\n",
"plt.ylabel('cte(t)')\n",
"\n",
"plt.tight_layout()\n",
"plt.show()"
]
},
{
"cell_type": "code",
2019-11-28 00:09:38 +08:00
"execution_count": 12,
2019-11-27 21:15:13 +08:00
"metadata": {},
"outputs": [],
"source": [
"track = compute_path_from_wp([0,2,4,5,10],[0,0,3,1,1])\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] = 2\n",
"x0[1] = 2\n",
"x0[2] = np.radians(0)\n",
"_,psi,cte = calc_err(x0,track)\n",
"x0[3]=psi\n",
"x0[4]=cte\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(5,1)\n",
" ut=u_bar[:,t-1].reshape(2,1)\n",
" \n",
" A,B,C=get_linear_model(xt,ut)\n",
" \n",
" xt_plus_one = np.squeeze(np.dot(A,xt)+np.dot(B,ut)+C)\n",
" \n",
" _,psi,cte = calc_err(xt_plus_one,track)\n",
" xt_plus_one[3]=psi\n",
" xt_plus_one[4]=cte\n",
" \n",
" x_bar[:,t]= xt_plus_one"
]
},
{
"cell_type": "code",
2019-11-28 00:09:38 +08:00
"execution_count": 13,
2019-11-27 21:15:13 +08:00
"metadata": {},
"outputs": [
{
"data": {
"image/png": "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
"text/plain": [
"<Figure size 432x288 with 4 Axes>"
]
},
"metadata": {
"needs_background": "light"
},
"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(track[0,:],track[1,:],\"b-\")\n",
"plt.axis('equal')\n",
"plt.ylabel('y')\n",
"plt.xlabel('x')\n",
"\n",
"plt.subplot(2, 2, 2)\n",
"plt.plot(x_bar[2,:])\n",
"plt.ylabel('theta(t)')\n",
"#plt.xlabel('time')\n",
"\n",
"plt.subplot(2, 2, 3)\n",
"plt.plot(x_bar[3,:])\n",
"plt.ylabel('psi(t)')\n",
"#plt.xlabel('time')\n",
"\n",
"plt.subplot(2, 2, 4)\n",
"plt.plot(x_bar[4,:])\n",
"plt.ylabel('cte(t)')\n",
"\n",
"plt.tight_layout()\n",
"plt.show()"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
2019-11-29 22:24:08 +08:00
"### 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",
""
]
},
{
"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",
"$J(x(t),U) = \\sum^{t+T-1}_{j=t} x^T_{j|t}Qx_{j|t} + u^T_{j|t}Ru_{j|t}$\n",
"\n",
"This 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",
"Becouse the goal is tracking a **reference signal** such as a trajectory, the objective function is rewritten as:\n",
"\n",
"$J(x(t),U) = \\sum^{t+T-1}_{j=t} \\delta x^T_{j|t}Q\\delta x_{j|t} + u^T_{j|t}Ru_{j|t}$\n",
"\n",
"where the error w.r.t desired state is accounted for:\n",
"\n",
"$\\delta x = x_{j,t,ref} - x_{j,t}$\n",
"\n",
"#### Non-Linear MPC Formulation\n",
"\n",
"In general cases, the objective function is any non-differentiable non-linear function of states and inputs over a finite horizon T. In this case the constrains include nonlinear dynamics of motion.\n",
"\n",
"$J(x(t),U) = \\sum^{t+T}_{j=t} C(x_{j|t},{j|t})$\n",
"\n",
"s.t.\n",
"\n",
"$ x_{j+1|t} = f(x_{j|t},{j|t}) t<j<t+T-1 $\n",
"\n",
"Other nonlinear constrains may be applied:\n",
"\n",
"$ g(x_{j|t},{j|t})<0 t<j<t+T-1 $"
2019-11-27 21:15:13 +08:00
]
},
{
"cell_type": "code",
2019-11-29 22:24:08 +08:00
"execution_count": 18,
2019-11-27 21:15:13 +08:00
"metadata": {},
"outputs": [
2019-11-28 01:46:53 +08:00
{
"name": "stderr",
"output_type": "stream",
"text": [
"/home/marcello/.local/lib/python3.5/site-packages/ipykernel_launcher.py:18: RuntimeWarning: invalid value encountered in true_divide\n"
]
},
2019-11-27 21:15:13 +08:00
{
"name": "stdout",
"output_type": "stream",
"text": [
2019-11-29 22:24:08 +08:00
"CPU times: user 277 ms, sys: 0 ns, total: 277 ms\n",
"Wall time: 276 ms\n"
2019-11-27 21:15:13 +08:00
]
}
],
"source": [
2019-11-28 00:09:38 +08:00
"%%time\n",
"\n",
2019-11-28 01:46:53 +08:00
"track = compute_path_from_wp([0,2,2,10],[0,0,2,2])\n",
"\n",
"MAX_SPEED = 2.5\n",
"MIN_SPEED = 0.5\n",
"MAX_STEER_SPEED = 1.57/2\n",
2019-11-27 21:15:13 +08:00
"\n",
"#starting guess\n",
"u_bar = np.zeros((M,T))\n",
2019-11-28 01:46:53 +08:00
"u_bar[0,:]=0.5*(MAX_SPEED+MIN_SPEED)\n",
2019-11-27 21:15:13 +08:00
"u_bar[1,:]=0.01\n",
"\n",
"# Starting Condition\n",
"x0 = np.zeros(N)\n",
2019-11-28 01:46:53 +08:00
"x0[0] = 0\n",
"x0[1] = 0\n",
"x0[2] = np.radians(-0)\n",
2019-11-27 21:15:13 +08:00
"_,psi,cte = calc_err(x0,track)\n",
"x0[3]=psi\n",
"x0[4]=cte\n",
"\n",
"# Prediction\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(5,1)\n",
" ut=u_bar[:,t-1].reshape(2,1)\n",
" \n",
" A,B,C=get_linear_model(xt,ut)\n",
" \n",
" xt_plus_one = np.squeeze(np.dot(A,xt)+np.dot(B,ut)+C)\n",
" \n",
" _,psi,cte = calc_err(xt_plus_one,track)\n",
" xt_plus_one[3]=psi\n",
" xt_plus_one[4]=cte\n",
" \n",
" x_bar[:,t]= xt_plus_one\n",
"\n",
"\n",
2019-11-28 01:46:53 +08:00
"#CVXPY Linear MPC problem statement\n",
2019-11-27 21:15:13 +08:00
"cost = 0\n",
"constr = []\n",
"\n",
"for t in range(T):\n",
" \n",
" # Cost function\n",
2019-11-28 01:46:53 +08:00
" #cost += 5*cp.sum_squares( x[3, t]) # psi\n",
" #cost += 100*cp.sum_squares( x[4, t]) # cte\n",
2019-11-27 21:15:13 +08:00
" \n",
" # Tracking\n",
" idx,_,_ = calc_err(x_bar[:,t],track)\n",
2019-11-28 01:46:53 +08:00
" delta_x = track[:,idx]-x[0:3,t]\n",
2019-11-27 21:15:13 +08:00
" cost+= cp.quad_form(delta_x,10*np.eye(3))\n",
2019-11-28 01:46:53 +08:00
" \n",
" # Tracking 5 states\n",
" #delta_x = np.append(track[:,idx],[0,0])\n",
" #cost+= cp.quad_form(delta_x-x[:,t],500*np.eye(N))\n",
2019-11-27 21:15:13 +08:00
" \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",
2019-11-28 01:46:53 +08:00
" cost += cp.quad_form(u[:, t + 1] - u[:, t], 5*np.eye(M))\n",
2019-11-27 21:15:13 +08:00
" \n",
" #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] == x0]\n",
"constr += [u[0, :] <= MAX_SPEED]\n",
"constr += [u[0, :] >= MIN_SPEED]\n",
"constr += [cp.abs(u[1, :]) <= MAX_STEER_SPEED]\n",
"\n",
"\n",
"prob = cp.Problem(cp.Minimize(cost), constr)\n",
2019-11-28 00:09:38 +08:00
"solution = prob.solve(solver=cp.ECOS, verbose=False)"
2019-11-27 21:15:13 +08:00
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Print Results:"
]
},
{
"cell_type": "code",
2019-11-29 22:24:08 +08:00
"execution_count": 19,
2019-11-27 21:15:13 +08:00
"metadata": {},
"outputs": [
{
"data": {
2019-11-28 01:46:53 +08:00
"image/png": "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
2019-11-27 21:15:13 +08:00
"text/plain": [
"<Figure size 432x288 with 3 Axes>"
]
},
"metadata": {
"needs_background": "light"
},
"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[1, :]).flatten()\n",
"psi_mpc=np.array(x.value[1, :]).flatten()\n",
"cte_mpc=np.array(x.value[1, :]).flatten()\n",
"v_mpc=np.array(u.value[0, :]).flatten()\n",
"w_mpc=np.array(u.value[1, :]).flatten()\n",
"\n",
2019-11-28 00:09:38 +08:00
"#simulate robot state trajectory for optimized U\n",
2019-11-27 21:15:13 +08:00
"x_traj=predict(x0, np.vstack((v_mpc,w_mpc)))\n",
"\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",
"#plot theta(t)\n",
"#plt.subplot(2, 2, 4)\n",
"#plt.plot(cte_mpc)\n",
"#plt.ylabel('cte(t)')\n",
"#plt.xlabel('time')\n",
"#plt.legend(loc='best')\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.5.2"
}
},
"nbformat": 4,
"nbformat_minor": 2
}
