467 lines
34 KiB
Plaintext
467 lines
34 KiB
Plaintext
|
{
|
||
|
"cells": [
|
||
|
{
|
||
|
"cell_type": "markdown",
|
||
|
"metadata": {},
|
||
|
"source": [
|
||
|
"# 1 System Modelling\n",
|
||
|
"\n",
|
||
|
"This notebook contains the theory on using the vehicle Kinematics Equations to derive the linearized state space model"
|
||
|
]
|
||
|
},
|
||
|
{
|
||
|
"cell_type": "code",
|
||
|
"execution_count": 1,
|
||
|
"metadata": {},
|
||
|
"outputs": [],
|
||
|
"source": [
|
||
|
"import numpy as np\n",
|
||
|
"from scipy.integrate import odeint\n",
|
||
|
"from scipy.interpolate import interp1d\n",
|
||
|
"import cvxpy as cp\n",
|
||
|
"\n",
|
||
|
"import matplotlib.pyplot as plt\n",
|
||
|
"plt.style.use(\"ggplot\")\n",
|
||
|
"\n",
|
||
|
"import time"
|
||
|
]
|
||
|
},
|
||
|
{
|
||
|
"cell_type": "markdown",
|
||
|
"metadata": {},
|
||
|
"source": [
|
||
|
"## kinematics model equations\n",
|
||
|
"\n",
|
||
|
"The variables of the model are:\n",
|
||
|
"\n",
|
||
|
"* $x$ coordinate of the robot\n",
|
||
|
"* $y$ coordinate of the robot\n",
|
||
|
"* $v$ velocity of the robot\n",
|
||
|
"* $\\theta$ heading of the robot\n",
|
||
|
"\n",
|
||
|
"The inputs of the model are:\n",
|
||
|
"\n",
|
||
|
"* $a$ acceleration of the robot\n",
|
||
|
"* $\\delta$ steering of the robot\n",
|
||
|
"\n",
|
||
|
"These are the differential equations f(x,u) of the model:\n",
|
||
|
"\n",
|
||
|
"$\\dot{x} = f(x,u)$\n",
|
||
|
"\n",
|
||
|
"* $\\dot{x} = v\\cos{\\theta}$ \n",
|
||
|
"* $\\dot{y} = v\\sin{\\theta}$\n",
|
||
|
"* $\\dot{v} = a$\n",
|
||
|
"* $\\dot{\\theta} = \\frac{v\\tan{\\delta}}{L}$\n",
|
||
|
"\n",
|
||
|
"Discretize with forward Euler Integration for time step dt:\n",
|
||
|
"\n",
|
||
|
"${x_{t+1}} = x_{t} + f(x,u)dt$\n",
|
||
|
"\n",
|
||
|
"* ${x_{t+1}} = x_{t} + v_t\\cos{\\theta}dt$\n",
|
||
|
"* ${y_{t+1}} = y_{t} + v_t\\sin{\\theta}dt$\n",
|
||
|
"* ${v_{t+1}} = v_{t} + a_tdt$\n",
|
||
|
"* ${\\theta_{t+1}} = \\theta_{t} + \\frac{v\\tan{\\delta}}{L} dt$\n",
|
||
|
"\n",
|
||
|
"----------------------\n",
|
||
|
"\n",
|
||
|
"The Model is **non-linear** and **time variant**, but the Numerical Optimizer requires a Linear sets of equations. To approximate the equivalent **LTI** State space model, the **Taylor's series expansion** is used around $\\bar{x}$ and $\\bar{u}$ (at each time step):\n",
|
||
|
"\n",
|
||
|
"$ f(x,u) \\approx f(\\bar{x},\\bar{u}) + \\frac{\\partial f(x,u)}{\\partial x}|_{x=\\bar{x},u=\\bar{u}}(x-\\bar{x}) + \\frac{\\partial f(x,u)}{\\partial u}|_{x=\\bar{x},u=\\bar{u}}(u-\\bar{u})$\n",
|
||
|
"\n",
|
||
|
"This can be rewritten usibg the State Space model form Ax+Bu :\n",
|
||
|
"\n",
|
||
|
"$ f(\\bar{x},\\bar{u}) + A|_{x=\\bar{x},u=\\bar{u}}(x-\\bar{x}) + B|_{x=\\bar{x},u=\\bar{u}}(u-\\bar{u})$\n",
|
||
|
"\n",
|
||
|
"Where:\n",
|
||
|
"\n",
|
||
|
"$\n",
|
||
|
"A =\n",
|
||
|
"\\quad\n",
|
||
|
"\\begin{bmatrix}\n",
|
||
|
"\\frac{\\partial f(x,u)}{\\partial x} & \\frac{\\partial f(x,u)}{\\partial y} & \\frac{\\partial f(x,u)}{\\partial v} & \\frac{\\partial f(x,u)}{\\partial \\theta} \\\\\n",
|
||
|
"\\end{bmatrix}\n",
|
||
|
"\\quad\n",
|
||
|
"=\n",
|
||
|
"\\displaystyle \\left[\\begin{matrix}0 & 0 & \\cos{\\left(\\theta \\right)} & - v \\sin{\\left(\\theta \\right)}\\\\0 & 0 & \\sin{\\left(\\theta \\right)} & v \\cos{\\left(\\theta \\right)}\\\\0 & 0 & 0 & 0\\\\0 & 0 & \\frac{\\tan{\\left(\\delta \\right)}}{L} & 0\\end{matrix}\\right]\n",
|
||
|
"$\n",
|
||
|
"\n",
|
||
|
"and\n",
|
||
|
"\n",
|
||
|
"$\n",
|
||
|
"B = \n",
|
||
|
"\\quad\n",
|
||
|
"\\begin{bmatrix}\n",
|
||
|
"\\frac{\\partial f(x,u)}{\\partial a} & \\frac{\\partial f(x,u)}{\\partial \\delta} \\\\\n",
|
||
|
"\\end{bmatrix}\n",
|
||
|
"\\quad\n",
|
||
|
"= \n",
|
||
|
"\\displaystyle \\left[\\begin{matrix}0 & 0\\\\0 & 0\\\\1 & 0\\\\0 & \\frac{v \\left(\\tan^{2}{\\left(\\delta \\right)} + 1\\right)}{L}\\end{matrix}\\right]\n",
|
||
|
"$\n",
|
||
|
"\n",
|
||
|
"are the *Jacobians*.\n",
|
||
|
"\n",
|
||
|
"\n",
|
||
|
"\n",
|
||
|
"So the discretized model is given by:\n",
|
||
|
"\n",
|
||
|
"$ x_{t+1} = x_t + (f(\\bar{x},\\bar{u}) + A|_{x=\\bar{x}}(x_t-\\bar{x}) + B|_{u=\\bar{u}}(u_t-\\bar{u}) )dt $\n",
|
||
|
"\n",
|
||
|
"$ x_{t+1} = (I+dtA)x_t + dtBu_t +dt(f(\\bar{x},\\bar{u}) - A\\bar{x} - B\\bar{u}))$\n",
|
||
|
"\n",
|
||
|
"The LTI-equivalent kinematics model is:\n",
|
||
|
"\n",
|
||
|
"$ x_{t+1} = A'x_t + B' u_t + C' $\n",
|
||
|
"\n",
|
||
|
"with:\n",
|
||
|
"\n",
|
||
|
"$ A' = I+dtA|_{x=\\bar{x},u=\\bar{u}} $\n",
|
||
|
"\n",
|
||
|
"$ B' = dtB|_{x=\\bar{x},u=\\bar{u}} $\n",
|
||
|
"\n",
|
||
|
"$ C' = dt(f(\\bar{x},\\bar{u}) - A|_{x=\\bar{x},u=\\bar{u}}\\bar{x} - B|_{x=\\bar{x},u=\\bar{u}}\\bar{u}) $"
|
||
|
]
|
||
|
},
|
||
|
{
|
||
|
"cell_type": "markdown",
|
||
|
"metadata": {},
|
||
|
"source": [
|
||
|
"-----------------\n",
|
||
|
"[About Taylor Series Expansion](https://courses.engr.illinois.edu/ece486/fa2017/documents/lecture_notes/state_space_p2.pdf):\n",
|
||
|
"\n",
|
||
|
"In order to linearize general nonlinear systems, we will use the Taylor Series expansion of functions.\n",
|
||
|
"\n",
|
||
|
"Typically it is possible to assume that the system is operating about some nominal\n",
|
||
|
"state solution $\\bar{x}$ (possibly requires a nominal input $\\bar{u}$) called **equilibrium point**.\n",
|
||
|
"\n",
|
||
|
"Recall that the Taylor Series expansion of f(x) around the\n",
|
||
|
"point $\\bar{x}$ is given by:\n",
|
||
|
"\n",
|
||
|
"$f(x)=f(\\bar{x}) + \\frac{df(x)}{dx}|_{x=\\bar{x}}(x-\\bar{x})$ + higher order terms...\n",
|
||
|
"\n",
|
||
|
"For x sufficiently close to $\\bar{x}$, these higher order terms will be very close to zero, and so we can drop them.\n",
|
||
|
"\n",
|
||
|
"The extension to functions of multiple states and inputs is very similar to the above procedure.Suppose the evolution of state x\n",
|
||
|
"is given by:\n",
|
||
|
"\n",
|
||
|
"$\\dot{x} = f(x1, x2, . . . , xn, u1, u2, . . . , um) = Ax+Bu$\n",
|
||
|
"\n",
|
||
|
"Where:\n",
|
||
|
"\n",
|
||
|
"$ A =\n",
|
||
|
"\\quad\n",
|
||
|
"\\begin{bmatrix}\n",
|
||
|
"\\frac{\\partial f(x,u)}{\\partial x1} & ... & \\frac{\\partial f(x,u)}{\\partial xn} \\\\\n",
|
||
|
"\\end{bmatrix}\n",
|
||
|
"\\quad\n",
|
||
|
"$ and $ B = \\quad\n",
|
||
|
"\\begin{bmatrix}\n",
|
||
|
"\\frac{\\partial f(x,u)}{\\partial u1} & ... & \\frac{\\partial f(x,u)}{\\partial um} \\\\\n",
|
||
|
"\\end{bmatrix}\n",
|
||
|
"\\quad $\n",
|
||
|
"\n",
|
||
|
"Then:\n",
|
||
|
"\n",
|
||
|
"$f(x,u)=f(\\bar{x},\\bar{u}) + \\frac{df(x,u)}{dx}|_{x=\\bar{x}}(x-\\bar{x}) + \\frac{df(x,u)}{du}|_{u=\\bar{u}}(u-\\bar{u}) = f(\\bar{x},\\bar{u}) + A_{x=\\bar{x}}(x-\\bar{x}) + B_{u=\\bar{u}}(u-\\bar{u})$\n",
|
||
|
"\n",
|
||
|
"-----------------"
|
||
|
]
|
||
|
},
|
||
|
{
|
||
|
"cell_type": "markdown",
|
||
|
"metadata": {},
|
||
|
"source": [
|
||
|
"## ODE Model\n",
|
||
|
"Motion Prediction: using scipy intergration"
|
||
|
]
|
||
|
},
|
||
|
{
|
||
|
"cell_type": "code",
|
||
|
"execution_count": 4,
|
||
|
"metadata": {},
|
||
|
"outputs": [],
|
||
|
"source": [
|
||
|
"# Define process model\n",
|
||
|
"# This uses the continuous model \n",
|
||
|
"def kinematics_model(x,t,u):\n",
|
||
|
" \"\"\"\n",
|
||
|
" Returns the set of ODE of the vehicle model.\n",
|
||
|
" \"\"\"\n",
|
||
|
" \n",
|
||
|
" L=0.3 #vehicle wheelbase\n",
|
||
|
" dxdt = x[2]*np.cos(x[3])\n",
|
||
|
" dydt = x[2]*np.sin(x[3])\n",
|
||
|
" dvdt = u[0]\n",
|
||
|
" dthetadt = x[2]*np.tan(u[1])/L\n",
|
||
|
"\n",
|
||
|
" dqdt = [dxdt,\n",
|
||
|
" dydt,\n",
|
||
|
" dvdt,\n",
|
||
|
" dthetadt]\n",
|
||
|
"\n",
|
||
|
" return dqdt\n",
|
||
|
"\n",
|
||
|
"def predict(x0,u):\n",
|
||
|
" \"\"\"\n",
|
||
|
" \"\"\"\n",
|
||
|
" \n",
|
||
|
" x_ = np.zeros((N,T+1))\n",
|
||
|
" \n",
|
||
|
" x_[:,0] = x0\n",
|
||
|
" \n",
|
||
|
" # solve ODE\n",
|
||
|
" for t in range(1,T+1):\n",
|
||
|
"\n",
|
||
|
" tspan = [0,DT]\n",
|
||
|
" x_next = odeint(kinematics_model,\n",
|
||
|
" x0,\n",
|
||
|
" tspan,\n",
|
||
|
" args=(u[:,t-1],))\n",
|
||
|
"\n",
|
||
|
" x0 = x_next[1]\n",
|
||
|
" x_[:,t]=x_next[1]\n",
|
||
|
" \n",
|
||
|
" return x_"
|
||
|
]
|
||
|
},
|
||
|
{
|
||
|
"cell_type": "code",
|
||
|
"execution_count": 5,
|
||
|
"metadata": {},
|
||
|
"outputs": [
|
||
|
{
|
||
|
"name": "stdout",
|
||
|
"output_type": "stream",
|
||
|
"text": [
|
||
|
"CPU times: user 3.39 ms, sys: 0 ns, total: 3.39 ms\n",
|
||
|
"Wall time: 2.79 ms\n"
|
||
|
]
|
||
|
}
|
||
|
],
|
||
|
"source": [
|
||
|
"%%time\n",
|
||
|
"\n",
|
||
|
"u_bar = np.zeros((M,T))\n",
|
||
|
"u_bar[0,:] = 0.2 #m/ss\n",
|
||
|
"u_bar[1,:] = np.radians(-np.pi/4) #rad\n",
|
||
|
"\n",
|
||
|
"x0 = np.zeros(N)\n",
|
||
|
"x0[0] = 0\n",
|
||
|
"x0[1] = 1\n",
|
||
|
"x0[2] = 0\n",
|
||
|
"x0[3] = np.radians(0)\n",
|
||
|
"\n",
|
||
|
"x_bar=predict(x0,u_bar)"
|
||
|
]
|
||
|
},
|
||
|
{
|
||
|
"cell_type": "code",
|
||
|
"execution_count": 6,
|
||
|
"metadata": {},
|
||
|
"outputs": [
|
||
|
{
|
||
|
"data": {
|
||
|
"image/png": "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
|
||
|
"text/plain": [
|
||
|
"<Figure size 432x288 with 2 Axes>"
|
||
|
]
|
||
|
},
|
||
|
"metadata": {},
|
||
|
"output_type": "display_data"
|
||
|
}
|
||
|
],
|
||
|
"source": [
|
||
|
"#plot trajectory\n",
|
||
|
"plt.subplot(2, 2, 1)\n",
|
||
|
"plt.plot(x_bar[0,:],x_bar[1,:])\n",
|
||
|
"plt.plot(np.linspace(0,10,T+1),np.zeros(T+1),\"b-\")\n",
|
||
|
"plt.axis('equal')\n",
|
||
|
"plt.ylabel('y')\n",
|
||
|
"plt.xlabel('x')\n",
|
||
|
"\n",
|
||
|
"plt.subplot(2, 2, 2)\n",
|
||
|
"plt.plot(np.degrees(x_bar[2,:]))\n",
|
||
|
"plt.ylabel('theta(t) [deg]')\n",
|
||
|
"#plt.xlabel('time')\n",
|
||
|
"\n",
|
||
|
"\n",
|
||
|
"plt.tight_layout()\n",
|
||
|
"plt.show()"
|
||
|
]
|
||
|
},
|
||
|
{
|
||
|
"cell_type": "markdown",
|
||
|
"metadata": {},
|
||
|
"source": [
|
||
|
"## State Space Linearized Model"
|
||
|
]
|
||
|
},
|
||
|
{
|
||
|
"cell_type": "code",
|
||
|
"execution_count": 2,
|
||
|
"metadata": {},
|
||
|
"outputs": [],
|
||
|
"source": [
|
||
|
"\"\"\"\n",
|
||
|
"Control problem statement.\n",
|
||
|
"\"\"\"\n",
|
||
|
"\n",
|
||
|
"N = 4 #number of state variables\n",
|
||
|
"M = 2 #number of control variables\n",
|
||
|
"T = 20 #Prediction Horizon\n",
|
||
|
"DT = 0.2 #discretization step"
|
||
|
]
|
||
|
},
|
||
|
{
|
||
|
"cell_type": "code",
|
||
|
"execution_count": 3,
|
||
|
"metadata": {},
|
||
|
"outputs": [],
|
||
|
"source": [
|
||
|
"def get_linear_model(x_bar,u_bar):\n",
|
||
|
" \"\"\"\n",
|
||
|
" Computes the LTI approximated state space model x' = Ax + Bu + C\n",
|
||
|
" \"\"\"\n",
|
||
|
" \n",
|
||
|
" L=0.3 #vehicle wheelbase\n",
|
||
|
" \n",
|
||
|
" x = x_bar[0]\n",
|
||
|
" y = x_bar[1]\n",
|
||
|
" v = x_bar[2]\n",
|
||
|
" theta = x_bar[3]\n",
|
||
|
" \n",
|
||
|
" a = u_bar[0]\n",
|
||
|
" delta = u_bar[1]\n",
|
||
|
" \n",
|
||
|
" A = np.zeros((N,N))\n",
|
||
|
" A[0,2]=np.cos(theta)\n",
|
||
|
" A[0,3]=-v*np.sin(theta)\n",
|
||
|
" A[1,2]=np.sin(theta)\n",
|
||
|
" A[1,3]=v*np.cos(theta)\n",
|
||
|
" A[3,2]=v*np.tan(delta)/L\n",
|
||
|
" A_lin=np.eye(N)+DT*A\n",
|
||
|
" \n",
|
||
|
" B = np.zeros((N,M))\n",
|
||
|
" B[2,0]=1\n",
|
||
|
" B[3,1]=v/(L*np.cos(delta)**2)\n",
|
||
|
" B_lin=DT*B\n",
|
||
|
" \n",
|
||
|
" f_xu=np.array([v*np.cos(theta), v*np.sin(theta), a,v*np.tan(delta)/L]).reshape(N,1)\n",
|
||
|
" C_lin = DT*(f_xu - np.dot(A,x_bar.reshape(N,1)) - np.dot(B,u_bar.reshape(M,1)))\n",
|
||
|
" \n",
|
||
|
" return np.round(A_lin,4), np.round(B_lin,4), np.round(C_lin,4)"
|
||
|
]
|
||
|
},
|
||
|
{
|
||
|
"cell_type": "code",
|
||
|
"execution_count": 7,
|
||
|
"metadata": {},
|
||
|
"outputs": [
|
||
|
{
|
||
|
"name": "stdout",
|
||
|
"output_type": "stream",
|
||
|
"text": [
|
||
|
"CPU times: user 2.71 ms, sys: 0 ns, total: 2.71 ms\n",
|
||
|
"Wall time: 1.82 ms\n"
|
||
|
]
|
||
|
}
|
||
|
],
|
||
|
"source": [
|
||
|
"%%time\n",
|
||
|
"\n",
|
||
|
"u_bar = np.zeros((M,T))\n",
|
||
|
"u_bar[0,:] = 0.2 #m/s\n",
|
||
|
"u_bar[1,:] = np.radians(-np.pi/4) #rad\n",
|
||
|
"\n",
|
||
|
"x0 = np.zeros(N)\n",
|
||
|
"x0[0] = 0\n",
|
||
|
"x0[1] = 1\n",
|
||
|
"x0[2] = 0\n",
|
||
|
"x0[3] = np.radians(0)\n",
|
||
|
"\n",
|
||
|
"x_bar=np.zeros((N,T+1))\n",
|
||
|
"x_bar[:,0]=x0\n",
|
||
|
"\n",
|
||
|
"for t in range (1,T+1):\n",
|
||
|
" xt=x_bar[:,t-1].reshape(N,1)\n",
|
||
|
" ut=u_bar[:,t-1].reshape(M,1)\n",
|
||
|
" \n",
|
||
|
" A,B,C=get_linear_model(xt,ut)\n",
|
||
|
" \n",
|
||
|
" xt_plus_one = np.dot(A,xt)+np.dot(B,ut)+C\n",
|
||
|
" \n",
|
||
|
" x_bar[:,t]= np.squeeze(xt_plus_one)"
|
||
|
]
|
||
|
},
|
||
|
{
|
||
|
"cell_type": "code",
|
||
|
"execution_count": 8,
|
||
|
"metadata": {},
|
||
|
"outputs": [
|
||
|
{
|
||
|
"data": {
|
||
|
"image/png": "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
|
||
|
"text/plain": [
|
||
|
"<Figure size 432x288 with 2 Axes>"
|
||
|
]
|
||
|
},
|
||
|
"metadata": {},
|
||
|
"output_type": "display_data"
|
||
|
}
|
||
|
],
|
||
|
"source": [
|
||
|
"#plot trajectory\n",
|
||
|
"plt.subplot(2, 2, 1)\n",
|
||
|
"plt.plot(x_bar[0,:],x_bar[1,:])\n",
|
||
|
"plt.plot(np.linspace(0,10,T+1),np.zeros(T+1),\"b-\")\n",
|
||
|
"plt.axis('equal')\n",
|
||
|
"plt.ylabel('y')\n",
|
||
|
"plt.xlabel('x')\n",
|
||
|
"\n",
|
||
|
"plt.subplot(2, 2, 2)\n",
|
||
|
"plt.plot(np.degrees(x_bar[2,:]))\n",
|
||
|
"plt.ylabel('theta(t)')\n",
|
||
|
"#plt.xlabel('time')\n",
|
||
|
"\n",
|
||
|
"\n",
|
||
|
"plt.tight_layout()\n",
|
||
|
"plt.show()"
|
||
|
]
|
||
|
},
|
||
|
{
|
||
|
"cell_type": "markdown",
|
||
|
"metadata": {},
|
||
|
"source": [
|
||
|
"The results are the same as expected, so the linearized model is equivalent as expected."
|
||
|
]
|
||
|
},
|
||
|
{
|
||
|
"cell_type": "code",
|
||
|
"execution_count": null,
|
||
|
"metadata": {},
|
||
|
"outputs": [],
|
||
|
"source": []
|
||
|
}
|
||
|
],
|
||
|
"metadata": {
|
||
|
"kernelspec": {
|
||
|
"display_name": "Python [conda env:.conda-jupyter] *",
|
||
|
"language": "python",
|
||
|
"name": "conda-env-.conda-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.5"
|
||
|
}
|
||
|
},
|
||
|
"nbformat": 4,
|
||
|
"nbformat_minor": 4
|
||
|
}
|