352 lines
23 KiB
Plaintext
352 lines
23 KiB
Plaintext
{
|
|
"cells": [
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"# STATE SPACE MODEL MATRICES"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 1,
|
|
"metadata": {},
|
|
"outputs": [
|
|
{
|
|
"data": {
|
|
"text/latex": [
|
|
"$\\displaystyle \\left[\\begin{matrix}0 & 0 & - v \\sin{\\left(\\theta \\right)} & 0 & 0\\\\0 & 0 & v \\cos{\\left(\\theta \\right)} & 0 & 0\\\\0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & v \\cos{\\left(\\psi \\right)} & 0\\end{matrix}\\right]$"
|
|
],
|
|
"text/plain": [
|
|
"Matrix([\n",
|
|
"[0, 0, -v*sin(theta), 0, 0],\n",
|
|
"[0, 0, v*cos(theta), 0, 0],\n",
|
|
"[0, 0, 0, 0, 0],\n",
|
|
"[0, 0, 0, 0, 0],\n",
|
|
"[0, 0, 0, v*cos(psi), 0]])"
|
|
]
|
|
},
|
|
"execution_count": 1,
|
|
"metadata": {},
|
|
"output_type": "execute_result"
|
|
}
|
|
],
|
|
"source": [
|
|
"import sympy as sp\n",
|
|
"\n",
|
|
"x,y,theta,psi,cte,v,w = sp.symbols(\"x y theta psi cte v w\")\n",
|
|
"\n",
|
|
"gs = sp.Matrix([[ sp.cos(theta)*v],\n",
|
|
" [ sp.sin(theta)*v],\n",
|
|
" [w],\n",
|
|
" [-w],\n",
|
|
" [ v*sp.sin(psi)]])\n",
|
|
"\n",
|
|
"state = sp.Matrix([x,y,theta,psi,cte])\n",
|
|
"\n",
|
|
"#A\n",
|
|
"gs.jacobian(state)"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 121,
|
|
"metadata": {},
|
|
"outputs": [
|
|
{
|
|
"data": {
|
|
"text/latex": [
|
|
"$\\displaystyle \\left[\\begin{matrix}\\cos{\\left(\\theta \\right)} & 0\\\\\\sin{\\left(\\theta \\right)} & 0\\\\0 & 1\\\\0 & -1\\\\\\sin{\\left(\\psi \\right)} & 0\\end{matrix}\\right]$"
|
|
],
|
|
"text/plain": [
|
|
"Matrix([\n",
|
|
"[cos(theta), 0],\n",
|
|
"[sin(theta), 0],\n",
|
|
"[ 0, 1],\n",
|
|
"[ 0, -1],\n",
|
|
"[ sin(psi), 0]])"
|
|
]
|
|
},
|
|
"execution_count": 121,
|
|
"metadata": {},
|
|
"output_type": "execute_result"
|
|
}
|
|
],
|
|
"source": [
|
|
"state = sp.Matrix([v,w])\n",
|
|
"\n",
|
|
"#B\n",
|
|
"gs.jacobian(state)"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": []
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"# PATH WAYPOINTS AS PARAMETRIZED CURVE"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 98,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"import numpy as np\n",
|
|
"from scipy.interpolate import interp1d\n",
|
|
"\n",
|
|
"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"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 110,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"#define track\n",
|
|
"wp=np.array([0,5,6,10,11,15, 0,0,2,2,0,4]).reshape(2,-1)\n",
|
|
"track = compute_path_from_wp(wp[0,:],wp[1,:],step=0.5)\n",
|
|
"\n",
|
|
"#vehicle state\n",
|
|
"state=[3.5,0.5,np.radians(30)]\n",
|
|
"\n",
|
|
"#given vehicle pos find lookahead waypoints\n",
|
|
"nn_idx=get_nn_idx(state,track) #index ox closest wp\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",
|
|
"coeff=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 coeff[0]*x**3+coeff[1]*x**2+coeff[2]*x**1+coeff[3]*x**0"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 111,
|
|
"metadata": {},
|
|
"outputs": [
|
|
{
|
|
"data": {
|
|
"text/plain": [
|
|
"array([-0.12829547, 0.96144073, -1.47313104, -0.47433648])"
|
|
]
|
|
},
|
|
"execution_count": 111,
|
|
"metadata": {},
|
|
"output_type": "execute_result"
|
|
}
|
|
],
|
|
"source": [
|
|
"coeff"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 113,
|
|
"metadata": {},
|
|
"outputs": [
|
|
{
|
|
"data": {
|
|
"image/png": "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\n",
|
|
"text/plain": [
|
|
"<Figure size 432x288 with 2 Axes>"
|
|
]
|
|
},
|
|
"metadata": {},
|
|
"output_type": "display_data"
|
|
}
|
|
],
|
|
"source": [
|
|
"import matplotlib.pyplot as plt\n",
|
|
"plt.style.use(\"ggplot\")\n",
|
|
"\n",
|
|
"x=np.arange(0,3,0.001) #interp range of curve \n",
|
|
"\n",
|
|
"# VEHICLE REF FRAME\n",
|
|
"plt.subplot(2,1,1)\n",
|
|
"plt.scatter(0,0)\n",
|
|
"plt.scatter(wp_vehicle_frame[0,:],wp_vehicle_frame[1,:])\n",
|
|
"plt.plot(x,[f(xs,coeff) for xs in x])\n",
|
|
"plt.axis('equal')\n",
|
|
"\n",
|
|
"# MAP REF FRAME\n",
|
|
"plt.subplot(2,1,2)\n",
|
|
"plt.scatter(state[0],state[1])\n",
|
|
"plt.scatter(track[0,:],track[1,:])\n",
|
|
"plt.scatter(track[0,nn_idx:nn_idx+LOOKAHED],track[1,nn_idx:nn_idx+LOOKAHED])\n",
|
|
"plt.axis('equal');"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"# COMPUTE ERRORS"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"* **crosstrack error** cte -> desired y-position - y-position of vehicle: this is the bias term of the fitted polynomial (road curve)\n",
|
|
" \n",
|
|
"$\n",
|
|
"f = K_0 * x^3 + K_1 * x^2 + K_2 * x + K_3\n",
|
|
"$\n",
|
|
"\n",
|
|
"Then for the origin cte = K_3\n",
|
|
" \n",
|
|
"* **heading error** epsi -> desired heading - heading of vehicle : is the inclination of tangent to the fitted polynomial (road curve)\n",
|
|
"\n",
|
|
"The derivative of the fitted poly has the form\n",
|
|
"\n",
|
|
"$\n",
|
|
"f' = 3.0 * K_0 * x^2 + 2.0 * K_1 * x + K_2\n",
|
|
"$\n",
|
|
"\n",
|
|
"Then for the origin the equation of the tangent in the origin is $y=k2$ \n",
|
|
"\n",
|
|
"epsi = -atan(K_2)\n",
|
|
"\n",
|
|
"in general:\n",
|
|
"\n",
|
|
"$\n",
|
|
"y_{desired} = f(px) \\\\\n",
|
|
"heading_{desired} = -atan(f`(px))\n",
|
|
"$"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 119,
|
|
"metadata": {},
|
|
"outputs": [
|
|
{
|
|
"name": "stdout",
|
|
"output_type": "stream",
|
|
"text": [
|
|
"-0.47433647872956336\n",
|
|
"55.83031014720696\n"
|
|
]
|
|
}
|
|
],
|
|
"source": [
|
|
"#for 0\n",
|
|
"\n",
|
|
"cte=coeff[3]\n",
|
|
"epsi=-np.arctan(coeff[2])\n",
|
|
"print(cte)\n",
|
|
"print(np.degrees(epsi))"
|
|
]
|
|
},
|
|
{
|
|
"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",
|
|
"* $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 current 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.6.9"
|
|
}
|
|
},
|
|
"nbformat": 4,
|
|
"nbformat_minor": 4
|
|
}
|