cleaned up modelling notebooks, added notebook from autoware.ai

master
mcarfagno 2021-07-19 14:01:45 +01:00
parent 4ffbb35207
commit a2eb20e7be
6 changed files with 1388 additions and 1217 deletions

File diff suppressed because one or more lines are too long

View File

@ -0,0 +1,193 @@
{
"cells": [
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Ackermann Kinematics model\n",
"\n",
"### Jacobians"
]
},
{
"cell_type": "code",
"execution_count": 5,
"metadata": {},
"outputs": [
{
"data": {
"text/latex": [
"$\\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]$"
],
"text/plain": [
"Matrix([\n",
"[0, 0, cos(theta), -v*sin(theta)],\n",
"[0, 0, sin(theta), v*cos(theta)],\n",
"[0, 0, 0, 0],\n",
"[0, 0, tan(delta)/L, 0]])"
]
},
"metadata": {},
"output_type": "display_data"
},
{
"data": {
"text/latex": [
"$\\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]$"
],
"text/plain": [
"Matrix([\n",
"[0, 0],\n",
"[0, 0],\n",
"[1, 0],\n",
"[0, v*(tan(delta)**2 + 1)/L]])"
]
},
"metadata": {},
"output_type": "display_data"
}
],
"source": [
"x,y,theta,v,delta,L,a = sp.symbols(\"x y theta v delta L a\")\n",
"\n",
"gs = sp.Matrix([[ sp.cos(theta)*v],\n",
" [ sp.sin(theta)*v],\n",
" [a],\n",
" [ v*sp.tan(delta)/L]])\n",
"\n",
"X = sp.Matrix([x,y,v,theta])\n",
"\n",
"#A\n",
"A=gs.jacobian(X)\n",
"\n",
"U = sp.Matrix([a,delta])\n",
"\n",
"#B\n",
"B=gs.jacobian(U)\n",
"display(A)\n",
"display(B)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"### Discretized and Linearized model"
]
},
{
"cell_type": "code",
"execution_count": 9,
"metadata": {},
"outputs": [
{
"data": {
"text/latex": [
"$\\displaystyle \\left[\\begin{matrix}1 & 0 & dt \\cos{\\left(\\theta \\right)} & - dt v \\sin{\\left(\\theta \\right)}\\\\0 & 1 & dt \\sin{\\left(\\theta \\right)} & dt v \\cos{\\left(\\theta \\right)}\\\\0 & 0 & 1 & 0\\\\0 & 0 & \\frac{dt \\tan{\\left(\\delta \\right)}}{L} & 1\\end{matrix}\\right]$"
],
"text/plain": [
"Matrix([\n",
"[1, 0, dt*cos(theta), -dt*v*sin(theta)],\n",
"[0, 1, dt*sin(theta), dt*v*cos(theta)],\n",
"[0, 0, 1, 0],\n",
"[0, 0, dt*tan(delta)/L, 1]])"
]
},
"metadata": {},
"output_type": "display_data"
},
{
"data": {
"text/latex": [
"$\\displaystyle \\left[\\begin{matrix}0 & 0\\\\0 & 0\\\\dt & 0\\\\0 & \\frac{dt v \\left(\\tan^{2}{\\left(\\delta \\right)} + 1\\right)}{L}\\end{matrix}\\right]$"
],
"text/plain": [
"Matrix([\n",
"[ 0, 0],\n",
"[ 0, 0],\n",
"[dt, 0],\n",
"[ 0, dt*v*(tan(delta)**2 + 1)/L]])"
]
},
"metadata": {},
"output_type": "display_data"
},
{
"data": {
"text/latex": [
"$\\displaystyle \\left[\\begin{matrix}dt \\theta v \\sin{\\left(\\theta \\right)}\\\\- dt \\theta v \\cos{\\left(\\theta \\right)}\\\\0\\\\- \\frac{\\delta dt v \\left(\\tan^{2}{\\left(\\delta \\right)} + 1\\right)}{L}\\end{matrix}\\right]$"
],
"text/plain": [
"Matrix([\n",
"[ dt*theta*v*sin(theta)],\n",
"[ -dt*theta*v*cos(theta)],\n",
"[ 0],\n",
"[-delta*dt*v*(tan(delta)**2 + 1)/L]])"
]
},
"metadata": {},
"output_type": "display_data"
}
],
"source": [
"DT = sp.symbols(\"dt\")\n",
"\n",
"display(sp.eye(4)+A*DT)\n",
"display(B*DT)\n",
"display(DT*(gs - A*X - B*U))"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"# ADD DELAY (for real time implementation)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"It is necessary to take *actuation latency* into account: so instead of using the actual state as estimated, the delay factored in using the kinematic model\n",
"\n",
"Starting State is :\n",
"\n",
"* $x_{delay} = 0.0 + v * dt$\n",
"* $y_{delay} = 0.0$\n",
"* $psi_{delay} = 0.0 + w * dt$\n",
"* $cte_{delay} = cte + v * sin(epsi) * dt$\n",
"* $epsi_{delay} = epsi - w * dt$\n",
"\n",
"Note that the starting position and heading is always 0; this is becouse the path is parametrized to **vehicle reference frame**"
]
},
{
"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.8.6"
}
},
"nbformat": 4,
"nbformat_minor": 4
}

File diff suppressed because one or more lines are too long

View File

@ -152,7 +152,7 @@
"name": "python", "name": "python",
"nbconvert_exporter": "python", "nbconvert_exporter": "python",
"pygments_lexer": "ipython3", "pygments_lexer": "ipython3",
"version": "3.8.5" "version": "3.8.6"
} }
}, },
"nbformat": 4, "nbformat": 4,