12345qiupeng
/
mpc_python_learn
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
mpc_python_learn/notebook/MPC_cte_cvxpy.ipynb

1028 lines
148 KiB
Plaintext
Raw Normal View History

WIP: fixed equations for cte
2020-04-22 22:32:10 +08:00
{
"cells": [
{
"cell_type": "code",
Fixing notebook theory
2020-05-04 19:07:01 +08:00
"execution_count": 29,
WIP: fixed equations for cte
2020-04-22 22:32:10 +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",
"\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",
"\n",
Fixing notebook theory
2020-05-04 19:07:01 +08:00
"Discretize with forward Euler Integration for time step dt:\n",
WIP: fixed equations for cte
2020-04-22 22:32:10 +08:00
"\n",
Fixing notebook theory
2020-05-04 19:07:01 +08:00
"${x_{t+1}} = x_{t} + f(x,u)*dt$\n",
WIP: fixed equations for cte
2020-04-22 22:32:10 +08:00
"\n",
Fixing notebook theory
2020-05-04 19:07:01 +08:00
"* ${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",
WIP: fixed equations for cte
2020-04-22 22:32:10 +08:00
"\n",
Fixing notebook theory
2020-05-04 19:07:01 +08:00
"This Model is **non-linear** and **time variant**, to approximate its 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}) + \\frac{\\partial f(x,u)}{\\partial u}(u-\\bar{u})$\n",
"\n",
"For the State Space model this can be rewritten as:\n",
"\n",
"$ f(\\bar{x},\\bar{u}) + A(x-\\bar{x}) + B(u-\\bar{u})$\n",
WIP: fixed equations for cte
2020-04-22 22:32:10 +08:00
"\n",
Fixing notebook theory
2020-05-04 19:07:01 +08:00
"Where:\n",
WIP: fixed equations for cte
2020-04-22 22:32:10 +08:00
"\n",
Fixing notebook theory
2020-05-04 19:07:01 +08:00
"$\n",
"A =\n",
"\\quad\n",
WIP: fixed equations for cte
2020-04-22 22:32:10 +08:00
"\\begin{bmatrix}\n",
Fixing notebook theory
2020-05-04 19:07:01 +08:00
"\\frac{\\partial f(x,u)}{\\partial x} & \\frac{\\partial f(x,u)}{\\partial y} & \\frac{\\partial f(x,u)}{\\partial \\theta} \\\\\n",
WIP: fixed equations for cte
2020-04-22 22:32:10 +08:00
"\\end{bmatrix}\n",
"\\quad\n",
"=\n",
"\\quad\n",
"\\begin{bmatrix}\n",
Fixing notebook theory
2020-05-04 19:07:01 +08:00
"0 & 0 & -v\\sin{\\theta} \\\\\n",
"0 & 0 & v\\cos{\\theta} \\\\\n",
"0 & 0 & 0\\\\\n",
WIP: fixed equations for cte
2020-04-22 22:32:10 +08:00
"\\end{bmatrix}\n",
Fixing notebook theory
2020-05-04 19:07:01 +08:00
"\\quad\n",
"$\n",
WIP: fixed equations for cte
2020-04-22 22:32:10 +08:00
"\n",
Fixing notebook theory
2020-05-04 19:07:01 +08:00
"and\n",
WIP: fixed equations for cte
2020-04-22 22:32:10 +08:00
"\n",
"$\n",
Fixing notebook theory
2020-05-04 19:07:01 +08:00
"B = \n",
"\\quad\n",
WIP: fixed equations for cte
2020-04-22 22:32:10 +08:00
"\\begin{bmatrix}\n",
Fixing notebook theory
2020-05-04 19:07:01 +08:00
"\\frac{\\partial f(x,u)}{\\partial v} & \\frac{\\partial f(x,u)}{\\partial w} \\\\\n",
WIP: fixed equations for cte
2020-04-22 22:32:10 +08:00
"\\end{bmatrix}\n",
"\\quad\n",
Fixing notebook theory
2020-05-04 19:07:01 +08:00
"= \n",
"\\quad\n",
WIP: fixed equations for cte
2020-04-22 22:32:10 +08:00
"\\begin{bmatrix}\n",
"\\cos{\\theta_t}dt & 0 \\\\\n",
"\\sin{\\theta_t}dt & 0 \\\\\n",
Fixing notebook theory
2020-05-04 19:07:01 +08:00
"0 & dt\n",
WIP: fixed equations for cte
2020-04-22 22:32:10 +08:00
"\\end{bmatrix}\n",
"\\quad\n",
"$\n",
"\n",
Fixing notebook theory
2020-05-04 19:07:01 +08:00
"are the *Jacobians* of the function \n",
WIP: fixed equations for cte
2020-04-22 22:32:10 +08:00
"\n",
Fixing notebook theory
2020-05-04 19:07:01 +08:00
"So the discretized model is given by:\n",
WIP: fixed equations for cte
2020-04-22 22:32:10 +08:00
"\n",
"$ x_{t+1} = x_t + (f(\\bar{x},\\bar{u}) + A(x_t-\\bar{x}) + B(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",
Fixing notebook theory
2020-05-04 19:07:01 +08:00
"The LTI-equivalent kinematics model is:\n",
WIP: fixed equations for cte
2020-04-22 22:32:10 +08:00
"\n",
"$ x_{t+1} = A'x_t + B' u_t + C' $\n",
"\n",
"with:\n",
"\n",
"$ A' = I+dtA $\n",
"\n",
"$ B' = dtB $\n",
"\n",
Fixing notebook theory
2020-05-04 19:07:01 +08:00
"$ C' = dt(f(\\bar{x},\\bar{u}) - A\\bar{x} - B\\bar{u}) $\n",
"\n",
"**Errors** are:\n",
"* $\\psi$ heading error = $\\psi = \\theta_{ref} - \\theta$\n",
"* $cte$ crosstrack error = lateral distance of the robot from the track w.r.t robot frame, cte is a function of the track and x\n",
"\n",
"described by:"
WIP: fixed equations for cte
2020-04-22 22:32:10 +08:00
]
},
{
"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",
Fixing notebook theory
2020-05-04 19:07:01 +08:00
"execution_count": 30,
WIP: fixed equations for cte
2020-04-22 22:32:10 +08:00
"metadata": {},
"outputs": [],
"source": [
"# Control problem statement.\n",
"\n",
Simplified kinematics equations, param tuning
2020-05-02 00:05:24 +08:00
"N = 3 #number of state variables\n",
WIP: fixed equations for cte
2020-04-22 22:32:10 +08:00
"M = 2 #number of control variables\n",
"T = 20 #Prediction Horizon\n",
"dt = 0.25 #discretization step"
]
},
{
"cell_type": "code",
Fixing notebook theory
2020-05-04 19:07:01 +08:00
"execution_count": 31,
WIP: fixed equations for cte
2020-04-22 22:32:10 +08:00
"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",
Simplified kinematics equations, param tuning
2020-05-02 00:05:24 +08:00
" f_xu=np.array([v*np.cos(theta),v*np.sin(theta),w]).reshape(N,1)\n",
WIP: fixed equations for cte
2020-04-22 22:32:10 +08:00
" 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",
Fixing notebook theory
2020-05-04 19:07:01 +08:00
"execution_count": 32,
WIP: fixed equations for cte
2020-04-22 22:32:10 +08:00
"metadata": {},
"outputs": [],
"source": [
"# Define process model\n",
Fixing notebook theory
2020-05-04 19:07:01 +08:00
"# This uses the continuous model \n",
WIP: fixed equations for cte
2020-04-22 22:32:10 +08:00
"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",
Simplified kinematics equations, param tuning
2020-05-02 00:05:24 +08:00
" dthetadt]\n",
WIP: fixed equations for cte
2020-04-22 22:32:10 +08:00
"\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",
Fixing notebook theory
2020-05-04 19:07:01 +08:00
"execution_count": 33,
WIP: fixed equations for cte
2020-04-22 22:32:10 +08:00
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
Fixing notebook theory
2020-05-04 19:07:01 +08:00
"CPU times: user 4.29 ms, sys: 15 µs, total: 4.3 ms\n",
"Wall time: 3.97 ms\n"
WIP: fixed equations for cte
2020-04-22 22:32:10 +08:00
]
}
],
"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",
Fixing notebook theory
2020-05-04 19:07:01 +08:00
"execution_count": 34,
WIP: fixed equations for cte
2020-04-22 22:32:10 +08:00
"metadata": {},
"outputs": [
{
"data": {
Simplified kinematics equations, param tuning
2020-05-02 00:05:24 +08:00
"image/png": "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
WIP: fixed equations for cte
2020-04-22 22:32:10 +08:00
"text/plain": [
Simplified kinematics equations, param tuning
2020-05-02 00:05:24 +08:00
"<Figure size 432x288 with 2 Axes>"
WIP: fixed equations for cte
2020-04-22 22:32:10 +08:00
]
},
"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": [
"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",
Fixing notebook theory
2020-05-04 19:07:01 +08:00
"execution_count": 35,
WIP: fixed equations for cte
2020-04-22 22:32:10 +08:00
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
Fixing notebook theory
2020-05-04 19:07:01 +08:00
"CPU times: user 2.32 ms, sys: 131 µs, total: 2.45 ms\n",
"Wall time: 2.03 ms\n"
WIP: fixed equations for cte
2020-04-22 22:32:10 +08:00
]
}
],
"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",
Simplified kinematics equations, param tuning
2020-05-02 00:05:24 +08:00
" xt=x_bar[:,t-1].reshape(N,1)\n",
" ut=u_bar[:,t-1].reshape(M,1)\n",
WIP: fixed equations for cte
2020-04-22 22:32:10 +08:00
" \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",
Fixing notebook theory
2020-05-04 19:07:01 +08:00
"execution_count": 36,
WIP: fixed equations for cte
2020-04-22 22:32:10 +08:00
"metadata": {},
"outputs": [
{
"data": {
Simplified kinematics equations, param tuning
2020-05-02 00:05:24 +08:00
"image/png": "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
WIP: fixed equations for cte
2020-04-22 22:32:10 +08:00
"text/plain": [
Simplified kinematics equations, param tuning
2020-05-02 00:05:24 +08:00
"<Figure size 432x288 with 2 Axes>"
WIP: fixed equations for cte
2020-04-22 22:32:10 +08:00
]
},
"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(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"
]
},
{
"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",
Fixing notebook theory
2020-05-04 19:07:01 +08:00
"execution_count": 37,
WIP: fixed equations for cte
2020-04-22 22:32:10 +08:00
"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)\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 df(x,coeff):\n",
" return round(3*coeff[0]*x**2 + 2*coeff[1]*x**1 + coeff[2]*x**0,6)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"test it"
]
},
{
"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": [
"![mpc](img/mpc_block_diagram.png)\n",
"\n",
"![mpc](img/mpc_t.png)"
]
},
{
"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} $\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",
"$\n",
"\\begin{equation}\n",
"\\begin{aligned}\n",
"\\min_{} \\quad & \\sum^{t+T}_{j=t} C(x_{j|t},{j|t})\\\\\n",
"\\textrm{s.t.} \\quad & x(0) = x0\\\\\n",
" & x_{j+1|t} = f(x_{j|t},u_{j|t}) \\quad \\textrm{for} \\quad t<j<t+T-1 \\\\\n",
"\\end{aligned}\n",
"\\end{equation}\n",
"$\n",
"\n",
"Other nonlinear constrains may be applied:\n",
"\n",
"$ g(x_{j|t},{j|t})<0 \\quad \\textrm{for} \\quad t<j<t+T-1 $"
]
},
{
"cell_type": "code",
Fixing notebook theory
2020-05-04 19:07:01 +08:00
"execution_count": 38,
WIP: fixed equations for cte
2020-04-22 22:32:10 +08:00
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
Simplified kinematics equations, param tuning
2020-05-02 00:05:24 +08:00
"[0. 0. 0.]\n"
WIP: fixed equations for cte
2020-04-22 22:32:10 +08:00
]
}
],
"source": [
"track = compute_path_from_wp([0,5,10],[0,0,2])\n",
"\n",
"# Constrains\n",
"MAX_SPEED = 2.5\n",
"MIN_SPEED = 0.5\n",
"MAX_STEER_SPEED = 1.57/2\n",
"\n",
"# Starting Condition\n",
"x0 = np.zeros(N)\n",
"x0[0] = 0\n",
"x0[1] = 1\n",
"x0[2] = np.radians(-10)\n",
"\n",
"#X0 is needed ONLY to initialize road curve\n",
"K=road_curve(x0,track)\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.0\n",
"\n",
"# Linearized Model for Prediction##############\n",
"x_bar=np.zeros((N,T+1))\n",
"\n",
"#starting state is 0,0,0,psi,cte\n",
"x_bar[0,0]=0\n",
"x_bar[1,0]=0\n",
"x_bar[2,0]=0\n",
Simplified kinematics equations, param tuning
2020-05-02 00:05:24 +08:00
"\n",
WIP: fixed equations for cte
2020-04-22 22:32:10 +08:00
"\n",
"print(x_bar[:,0])\n",
"\n",
"for t in range (1,T+1):\n",
Simplified kinematics equations, param tuning
2020-05-02 00:05:24 +08:00
" xt=x_bar[:,t-1].reshape(N,1)\n",
" ut=u_bar[:,t-1].reshape(M,1)\n",
WIP: fixed equations for cte
2020-04-22 22:32:10 +08:00
" \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",
" x_bar[:,t]= xt_plus_one\n",
"#################################################\n"
]
},
{
"cell_type": "code",
Fixing notebook theory
2020-05-04 19:07:01 +08:00
"execution_count": 39,
WIP: fixed equations for cte
2020-04-22 22:32:10 +08:00
"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",
Simplified kinematics equations, param tuning
2020-05-02 00:05:24 +08:00
"problem: variables n = 241, constraints m = 281\n",
" nnz(P) + nnz(A) = 715\n",
WIP: fixed equations for cte
2020-04-22 22:32:10 +08:00
"settings: linear system solver = qdldl,\n",
" eps_abs = 1.0e-05, eps_rel = 1.0e-05,\n",
" eps_prim_inf = 1.0e-04, eps_dual_inf = 1.0e-04,\n",
" rho = 1.00e-01 (adaptive),\n",
" sigma = 1.00e-06, alpha = 1.60, max_iter = 10000\n",
" check_termination: on (interval 25),\n",
" scaling: on, scaled_termination: off\n",
" warm start: on, polish: on, time_limit: off\n",
"\n",
"iter objective pri res dua res rho time\n",
Fixing notebook theory
2020-05-04 19:07:01 +08:00
" 1 0.0000e+00 1.02e+00 1.02e+02 1.00e-01 2.42e-04s\n",
" 125 1.6631e+03 2.07e-05 3.42e-04 7.12e+01 1.02e-03s\n",
WIP: fixed equations for cte
2020-04-22 22:32:10 +08:00
"\n",
"status: solved\n",
"solution polish: unsuccessful\n",
"number of iterations: 125\n",
Improving road-curve MPC
2020-05-01 23:40:00 +08:00
"optimal objective: 1663.1222\n",
Fixing notebook theory
2020-05-04 19:07:01 +08:00
"run time: 2.10e-03s\n",
Improving road-curve MPC
2020-05-01 23:40:00 +08:00
"optimal rho estimate: 7.91e+01\n",
WIP: fixed equations for cte
2020-04-22 22:32:10 +08:00
"\n",
Fixing notebook theory
2020-05-04 19:07:01 +08:00
"CPU times: user 259 ms, sys: 4.14 ms, total: 263 ms\n",
"Wall time: 258 ms\n"
WIP: fixed equations for cte
2020-04-22 22:32:10 +08:00
]
}
],
"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",
Improving road-curve MPC
2020-05-01 23:40:00 +08:00
" #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",
WIP: fixed equations for cte
2020-04-22 22:32:10 +08:00
" # 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",
Fixing notebook theory
2020-05-04 19:07:01 +08:00
"execution_count": 40,
WIP: fixed equations for cte
2020-04-22 22:32:10 +08:00
"metadata": {},
"outputs": [
{
"data": {
Simplified kinematics equations, param tuning
2020-05-02 00:05:24 +08:00
"image/png": "iVBORw0KGgoAAAANSUhEUgAAAagAAACkCAYAAADCB7oPAAAABHNCSVQICAgIfAhkiAAAAAlwSFlzAAALEgAACxIB0t1+/AAAADh0RVh0U29mdHdhcmUAbWF0cGxvdGxpYiB2ZXJzaW9uMy4xLjMsIGh0dHA6Ly9tYXRwbG90bGliLm9yZy+AADFEAAAekklEQVR4nO3de1SUdf4H8PfMcJOLwJB4wajAS4oaIYSLEhLq6WSniPW44S3LzTVJc7U2W9NqkXJXEddbqXmM3H6t1k/x5/72V2kcL2luIxcL2ExItxJz5OoQjjLM8/uDZpphZmAEZp7vyPt1js7Mc5s3z3mGD9/vPM/3UUiSJIGIiEgwSrkDEBER2cMCRUREQmKBIiIiIbFAERGRkFigiIhISCxQREQkJC+5AxCRuLZu3Yri4mIEBwcjNze329vLycnBuXPncPfdd2P58uXm6Rs3bkRVVRW8vLwQHR2N+fPnw8vLuV9PWVlZ8PPzg1KphEqlwpo1a1yWk9yLBYqIHJo4cSIefPBBbNmypUe298gjj+D69es4fPiw1fQJEyZg0aJFAIC//vWvKCwsxJQpU6yW2bJlCyZOnIiYmBib7b7yyivo27dvj2TsKCe5FwsUETk0cuRIaLVaq2k//vgjdu7ciatXr8LX1xe/+93vEBER4dT2Ro8ejfLycpvpcXFx5udDhgxBbW1t94K7KCe5FwsUEd2U7du34+mnn8bAgQNx7tw5vP3223jllVd6ZNsGgwHHjx/H3Llzb2q9nJwcAMDkyZMxadIkl+ck92CBIiKn6fV6nD17FuvXrzdPMxgMAIB//etf2Lt3r806arUaK1ascGr7b7/9NkaMGIERI0YAAEpLS/Hee+8BAGpqavD111/Dz88P3t7eeP311wEA2dnZUKvVaGxsxOrVqzFo0CBERUW5NCe5BwsUETnNaDQiICAAa9eutZmXmJiIxMTELm/7gw8+wNWrVzF//nzztNjYWMTGxgJw/B2UWq0GAAQHByMhIQGVlZW48847XZaT3IenmROR0/z9/REeHo7PP/8cACBJEi5cuNDt7X766ac4c+YMlixZAqXS+V9Ler0e165dMz//8ssvERkZ6bKc5F4KjmZORI5s2LABFRUV0Ol0CA4OxvTp0zFq1Cjs2LEDDQ0NMBgMGD9+PKZNm+bU9latWoWLFy9Cr9cjKCgICxYsQGxsLB5//HH069cPfn5+ANpaOe23aa8FdfnyZaxbtw4A0NraigkTJiAjIwMAoNVqezwnuRcLFBERCYldfEREJCQWKCIiEhILFBERCcmjTzOvrq62O/22225DTU2Nm9PYxyz2eUqWQYMGuTmNWDzhM2bCTM4RMZOjzxlbUEREJCQWKCIiEhILFBERCYkFioiIhMQCRUREQhKqQBmNRvzhD3/okTtiEhGRZxOqQP3zn/90+oZiRER0axOmQNXW1qK4uBhpaWlyRyEiIgEIc6HuO++8g1mzZpmHzrfn8OHDOHz4MABgzZo1uO222+wu5+Xl5XCeuzGLfcxCRJ0RokAVFRUhODgYUVFRKC8vd7jcpEmTzLdzBuDwamiRrpRmFvs8JUtvH0mCSE5CFKizZ8/i9OnTKCkpwY0bN3Dt2jVs3LgRixcvljsaERHJRIgCNWPGDMyYMQMAUF5ejoMHD7I4ERH1csKcJEFERGRJiBaUpZiYGKtbOhMRUe/EFhQREQmJBYqIiITEAkVEREJigSIiIiEJd5IEETlWWlqKXbt2wWg0Ii0tDenp6VbzW1pasHnzZnz77bcICgrCkiVLEB4eDgDYv38/CgsLoVQq8eSTTyI2NlaOH4HIaWxBEXkIo9GInTt34o9//CPy8vJw4sQJ/PDDD1bLFBYWIiAgAJs2bcLUqVPx3nvvAQB++OEHnDx5EuvXr8eKFSuwc+dOGI1GOX4MIqexQBF5iMrKSgwYMAD9+/eHl5cXkpKSoNForJY5ffo0Jk6cCAAYN24cysrKIEkSNBoNkpKS4O3tjfDwcAwYMACVlZUy/BREzmOBIvIQdXV1CAsLM78OCwtDXV2dw2VUKhX8/f2h0+ls1lWr1TbrEomG30ERkZkn3jHAhJmcI2ImR1igiDyEWq1GbW2t+XVtbS3UarXdZcLCwtDa2orm5mYEBQXZrFtXV2ezLuCZdwwwYSbniJjJ0V0D2MVH5CGio6Nx6dIlaLVaGAwGnDx5EvHx8VbLjB07FkeOHAEAnDp1CjExMVAoFIiPj8fJkyfR0tICrVaLS5cuYciQITL8FETOYwuKyEOoVCo89dRTyMnJgdFoRGpqKm6//Xbs2bMH0dHRiI+PxwMPPIDNmzdj0aJFCAwMxJIlSwAAt99+O371q19h6dKlUCqVmDdvHpRK/n1KYlNIkiTJHaKrqqur7U4XqQnLLPZ5SpbefsNCT/iMmTCTc0TMxC4+IiLyKCxQREQkJBYoIiISEgsUEREJiQWKiIiExAJFRERCEuI6qJqaGmzZsgUNDQ1QKBSYNGkSHnroIbljUS+Rna3CM8/InYKI2hOiBaVSqTB79mzk5eUhJycHH3/8sc1tBIh6Um5ukPlx9WqVzGmIyB4hClRoaCiioqIAAH369EFERARHWiaXyM0NQm5uENavbytQpkciEo8QXXyWtFotzp8/b3ecME8caZlZ7HNnluzsthbSypWtWL/exzw9ImKQzfOXX27FypWtbslFRB0TqkDp9Xrk5uZi7ty58Pf3t5nviSMtM4t9rs6SmxuEZct0Vq2l5uZmAD4O11m6VIdnntHBMlZvH+qISE5CdPEBgMFgQG5uLpKTk5GYmCh3HPJgjrrwOurOu3ixGsuW6VyejYicJ0SBkiQJb731FiIiIvDwww/LHYc8kOVJD6ZCZNmFZ8/Fi20Dob78Mrv0iEQkRBff2bNncezYMURGRuKFF14AAGRmZiIuLk7mZCQ6U2Fy5qSHpUt15vlLl+rMjytX+kKQnk8isiBEgbr77ruxd+9euWOQhzAVpWXLdE534ZmWb/+87dG350MSUbcJUaCIOmPvpIfOmFpMptYSAJd/z6TX6/HTTz8hICAAfn5+Ln0volsdCxQJzbIL72ZaTEuX6tq1klznu+++w+HDh1FcXIwrV66Yp4eHhyM2NhaTJ09GZGSkSzMQ3YpYoEg4jrrwnDnpISJikFuL04YNG/DDDz8gKSkJixYtQkREBPr06YNr167h4sWLqKiowMaNGzF48GDz7deJyDksUCSUrnThmZ6bHt15unhycjLGjh1rMz0wMBDDhw/H8OHD8dhjj6GoqKhb79PU1IS8vDxcuXIF/fr1w+9//3sEBgZaLXPhwgXs2LED165dg1KpREZGBpKSkgAAW7ZsQUVFhfn6wqysLNx5553dykTkaixQJLueP+nBfSyL07lz5zB06FCbZSorK+0WsZtRUFCA0aNHIz09HQUFBSgoKMCsWbOslvHx8cGzzz6LgQMHoq6uDsuXL8c999yDgIAAAMDs2bMxbty4buUgcichroOi3ic7W2X1/dL69UFOX7e0dKnO3GJatsy9LaaOrF692u70nJycbm9bo9EgJSUFAJCSkgKNRmOzzKBBgzBw4EAAgFqtRnBwMK5evdrt9yaSi9MtqHfeeQcTJ05ktwB1mfWZeCoAQR0WFxG68JxhNBoBtF1wbvpncvnyZahU3R8tvbGxEaGhoQCAkJAQNDY2drh8ZWUlDAYD+vfvb572/vvv48MPP8SoUaMwc+ZMeHt726znieNdmjCTc0TM5IjTBcpoNCInJwd9+/ZFcnIykpOTERYW5spsXWL8371o8veHFHU3EBkNhUIhd6Rer6Mz8TpqNYnQheeMzMxM8/PHH3/cap5SqcRjjz3m1Hays7PR0NBgM739NhUKRYfHdX19PTZt2oSsrCwolW2dJDNmzEBISAgMBgO2bduGAwcOYNq0aTbrOjPepVRZAR/NMVy/pjdNsXqwyyauxYT2P4vi5/9M09s/Su3e6OfXvr6+uK6/1pbDvIxkm0vR7ol5+/amde/3h5+vL/R6/c95LPZT+5/BRvuf0c4ilvup/T+rda1X9vP
WIP: fixed equations for cte
2020-04-22 22:32:10 +08:00
"text/plain": [
Simplified kinematics equations, param tuning
2020-05-02 00:05:24 +08:00
"<Figure size 432x288 with 2 Axes>"
WIP: fixed equations for cte
2020-04-22 22:32:10 +08:00
]
},
"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",
Simplified kinematics equations, param tuning
2020-05-02 00:05:24 +08:00
"# plt.subplot(2, 2, 3)\n",
"# plt.plot(psi_mpc)\n",
"# plt.ylabel('psi(t)')\n",
WIP: fixed equations for cte
2020-04-22 22:32:10 +08:00
"\n",
Simplified kinematics equations, param tuning
2020-05-02 00:05:24 +08:00
"# plt.subplot(2, 2, 4)\n",
"# plt.plot(cte_mpc)\n",
"# plt.ylabel('cte(t)')\n",
WIP: fixed equations for cte
2020-04-22 22:32:10 +08:00
"\n",
"plt.tight_layout()\n",
"plt.show()"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"full track demo"
]
},
{
"cell_type": "code",
Fixing notebook theory
2020-05-04 19:07:01 +08:00
"execution_count": 45,
WIP: fixed equations for cte
2020-04-22 22:32:10 +08:00
"metadata": {},
"outputs": [
Improving road-curve MPC
2020-05-01 23:40:00 +08:00
{
"name": "stderr",
"output_type": "stream",
"text": [
"/home/marcello/.local/lib/python3.6/site-packages/ipykernel_launcher.py:29: RuntimeWarning: invalid value encountered in true_divide\n"
]
},
WIP: fixed equations for cte
2020-04-22 22:32:10 +08:00
{
"name": "stdout",
"output_type": "stream",
"text": [
Fixing notebook theory
2020-05-04 19:07:01 +08:00
"CVXPY Optimization Time: Avrg: 0.1713s Max: 0.2654s Min: 0.1564s\n"
WIP: fixed equations for cte
2020-04-22 22:32:10 +08:00
]
}
],
"source": [
Fixing notebook theory
2020-05-04 19:07:01 +08:00
"# track = compute_path_from_wp([0,3,4,6,10],\n",
"# [0,0,2,4,3])\n",
WIP: fixed equations for cte
2020-04-22 22:32:10 +08:00
"\n",
Fixing notebook theory
2020-05-04 19:07:01 +08:00
"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])\n",
"sim_duration = 100 #time steps\n",
WIP: fixed equations for cte
2020-04-22 22:32:10 +08:00
"opt_time=[]\n",
"\n",
"x_sim = np.zeros((N,sim_duration))\n",
"u_sim = np.zeros((M,sim_duration-1))\n",
"\n",
Fixing notebook theory
2020-05-04 19:07:01 +08:00
"MAX_SPEED = 1.25\n",
"MIN_SPEED = 0.75\n",
Simplified kinematics equations, param tuning
2020-05-02 00:05:24 +08:00
"MAX_STEER_SPEED = 1.57\n",
WIP: fixed equations for cte
2020-04-22 22:32:10 +08:00
"\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",
Fixing notebook theory
2020-05-04 19:07:01 +08:00
" # dynamics starting state w.r.t vehicle frame\n",
WIP: fixed equations for cte
2020-04-22 22:32:10 +08:00
" x_bar=np.zeros((N,T+1))\n",
" \n",
Fixing notebook theory
2020-05-04 19:07:01 +08:00
" #prediction for linearization of costrains\n",
WIP: fixed equations for cte
2020-04-22 22:32:10 +08:00
" for t in range (1,T+1):\n",
Simplified kinematics equations, param tuning
2020-05-02 00:05:24 +08:00
" xt=x_bar[:,t-1].reshape(N,1)\n",
" ut=u_bar[:,t-1].reshape(M,1)\n",
WIP: fixed equations for cte
2020-04-22 22:32:10 +08:00
" 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",
" for t in range(T):\n",
"\n",
Fixing notebook theory
2020-05-04 19:07:01 +08:00
" 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",
Simplified kinematics equations, param tuning
2020-05-02 00:05:24 +08:00
" cost += 20*cp.sum_squares(f(x_bar[0,t],K)-x[1,t]) # cte\n",
WIP: fixed equations for cte
2020-04-22 22:32:10 +08:00
"\n",
" # Actuation rate of change\n",
" if t < (T - 1):\n",
Simplified kinematics equations, param tuning
2020-05-02 00:05:24 +08:00
" cost += cp.quad_form(u[:, t + 1] - u[:, t], 1*np.eye(M))\n",
WIP: fixed equations for cte
2020-04-22 22:32:10 +08:00
" \n",
" # Actuation effort\n",
Simplified kinematics equations, param tuning
2020-05-02 00:05:24 +08:00
" cost += cp.quad_form( u[:, t],1*np.eye(M))\n",
WIP: fixed equations for cte
2020-04-22 22:32:10 +08:00
" \n",
Fixing notebook theory
2020-05-04 19:07:01 +08:00
" # Kinrmatics Constrains (Linearized model)\n",
WIP: fixed equations for cte
2020-04-22 22:32:10 +08:00
" 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",
Fixing notebook theory
2020-05-04 19:07:01 +08:00
"execution_count": 46,
WIP: fixed equations for cte
2020-04-22 22:32:10 +08:00
"metadata": {},
"outputs": [
{
"data": {
Fixing notebook theory
2020-05-04 19:07:01 +08:00
"image/png": "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
WIP: fixed equations for cte
2020-04-22 22:32:10 +08:00
"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": "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.6.9"
}
},
"nbformat": 4,
"nbformat_minor": 4
}