466 lines
34 KiB
Plaintext
466 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",
|
|
"\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, dydt, dvdt, dthetadt]\n",
|
|
"\n",
|
|
" return dqdt\n",
|
|
"\n",
|
|
"\n",
|
|
"def predict(x0, u):\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, x0, tspan, 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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\n",
|
|
"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(\n",
|
|
" [v * np.cos(theta), v * np.sin(theta), a, v * np.tan(delta) / L]\n",
|
|
" ).reshape(N, 1)\n",
|
|
" C_lin = DT * (\n",
|
|
" f_xu - np.dot(A, x_bar.reshape(N, 1)) - np.dot(B, u_bar.reshape(M, 1))\n",
|
|
" )\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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\n",
|
|
"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
|
|
}
|