500 lines
23 KiB
Plaintext
500 lines
23 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": {
|
|
"ExecuteTime": {
|
|
"end_time": "2024-10-24T02:21:22.012892Z",
|
|
"start_time": "2024-10-24T02:21:21.029487Z"
|
|
}
|
|
},
|
|
"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": 2,
|
|
"metadata": {
|
|
"ExecuteTime": {
|
|
"end_time": "2024-10-24T02:21:22.017780Z",
|
|
"start_time": "2024-10-24T02:21:22.013940Z"
|
|
}
|
|
},
|
|
"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": 3,
|
|
"metadata": {
|
|
"ExecuteTime": {
|
|
"end_time": "2024-10-24T02:21:22.177869Z",
|
|
"start_time": "2024-10-24T02:21:22.016227Z"
|
|
}
|
|
},
|
|
"outputs": [
|
|
{
|
|
"ename": "NameError",
|
|
"evalue": "name 'M' is not defined",
|
|
"output_type": "error",
|
|
"traceback": [
|
|
"\u001B[0;31m---------------------------------------------------------------------------\u001B[0m",
|
|
"\u001B[0;31mNameError\u001B[0m Traceback (most recent call last)",
|
|
"File \u001B[0;32m<timed exec>:1\u001B[0m\n",
|
|
"\u001B[0;31mNameError\u001B[0m: name 'M' is not defined"
|
|
]
|
|
}
|
|
],
|
|
"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": 4,
|
|
"metadata": {
|
|
"ExecuteTime": {
|
|
"end_time": "2024-10-24T02:21:22.400345Z",
|
|
"start_time": "2024-10-24T02:21:22.178469Z"
|
|
}
|
|
},
|
|
"outputs": [
|
|
{
|
|
"ename": "NameError",
|
|
"evalue": "name 'x_bar' is not defined",
|
|
"output_type": "error",
|
|
"traceback": [
|
|
"\u001B[0;31m---------------------------------------------------------------------------\u001B[0m",
|
|
"\u001B[0;31mNameError\u001B[0m Traceback (most recent call last)",
|
|
"Cell \u001B[0;32mIn[4], line 3\u001B[0m\n\u001B[1;32m 1\u001B[0m \u001B[38;5;66;03m# plot trajectory\u001B[39;00m\n\u001B[1;32m 2\u001B[0m plt\u001B[38;5;241m.\u001B[39msubplot(\u001B[38;5;241m2\u001B[39m, \u001B[38;5;241m2\u001B[39m, \u001B[38;5;241m1\u001B[39m)\n\u001B[0;32m----> 3\u001B[0m plt\u001B[38;5;241m.\u001B[39mplot(x_bar[\u001B[38;5;241m0\u001B[39m, :], x_bar[\u001B[38;5;241m1\u001B[39m, :])\n\u001B[1;32m 4\u001B[0m plt\u001B[38;5;241m.\u001B[39mplot(np\u001B[38;5;241m.\u001B[39mlinspace(\u001B[38;5;241m0\u001B[39m, \u001B[38;5;241m10\u001B[39m, T \u001B[38;5;241m+\u001B[39m \u001B[38;5;241m1\u001B[39m), np\u001B[38;5;241m.\u001B[39mzeros(T \u001B[38;5;241m+\u001B[39m \u001B[38;5;241m1\u001B[39m), \u001B[38;5;124m\"\u001B[39m\u001B[38;5;124mb-\u001B[39m\u001B[38;5;124m\"\u001B[39m)\n\u001B[1;32m 5\u001B[0m plt\u001B[38;5;241m.\u001B[39maxis(\u001B[38;5;124m\"\u001B[39m\u001B[38;5;124mequal\u001B[39m\u001B[38;5;124m\"\u001B[39m)\n",
|
|
"\u001B[0;31mNameError\u001B[0m: name 'x_bar' is not defined"
|
|
]
|
|
},
|
|
{
|
|
"data": {
|
|
"text/plain": "<Figure size 640x480 with 1 Axes>",
|
|
"image/png": "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"
|
|
},
|
|
"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": null,
|
|
"metadata": {
|
|
"ExecuteTime": {
|
|
"end_time": "2024-10-24T02:21:22.405520Z",
|
|
"start_time": "2024-10-24T02:21:22.401778Z"
|
|
}
|
|
},
|
|
"outputs": [],
|
|
"source": [
|
|
"\"\"\"\n",
|
|
"Control problem statement.\n",
|
|
"\"\"\"\n",
|
|
"\n",
|
|
"N = 4 # number of state variables\n",
|
|
"M = 2 # number of control variables\n",
|
|
"T = 40 # Prediction Horizon\n",
|
|
"DT = 0.2 # discretization step"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {
|
|
"ExecuteTime": {
|
|
"start_time": "2024-10-24T02:21:22.403693Z"
|
|
}
|
|
},
|
|
"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": null,
|
|
"metadata": {
|
|
"ExecuteTime": {
|
|
"start_time": "2024-10-24T02:21:22.404761Z"
|
|
}
|
|
},
|
|
"outputs": [],
|
|
"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": null,
|
|
"metadata": {
|
|
"ExecuteTime": {
|
|
"start_time": "2024-10-24T02:21:22.406154Z"
|
|
}
|
|
},
|
|
"outputs": [],
|
|
"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": {
|
|
"ExecuteTime": {
|
|
"end_time": "2024-10-24T02:21:22.407303Z",
|
|
"start_time": "2024-10-24T02:21:22.406998Z"
|
|
}
|
|
},
|
|
"outputs": [],
|
|
"source": []
|
|
}
|
|
],
|
|
"metadata": {
|
|
"kernelspec": {
|
|
"name": "python3",
|
|
"language": "python",
|
|
"display_name": "Python 3 (ipykernel)"
|
|
},
|
|
"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
|
|
}
|