379 lines
30 KiB
Plaintext
379 lines
30 KiB
Plaintext
{
|
|
"cells": [
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"# STATE SPACE MODEL MATRICES"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 9,
|
|
"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": 9,
|
|
"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": 10,
|
|
"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": 10,
|
|
"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": 11,
|
|
"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": 12,
|
|
"metadata": {},
|
|
"outputs": [
|
|
{
|
|
"name": "stderr",
|
|
"output_type": "stream",
|
|
"text": [
|
|
"/home/marcello/.local/lib/python3.6/site-packages/IPython/core/interactiveshell.py:3331: RankWarning: Polyfit may be poorly conditioned\n",
|
|
" exec(code_obj, self.user_global_ns, self.user_ns)\n"
|
|
]
|
|
}
|
|
],
|
|
"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)-1 #index ox closest wp, take the previous to have a straighter line\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,:], 5, 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\n",
|
|
"def f(x,coeff):\n",
|
|
" return coeff[0]*x**5+coeff[1]*x**4+coeff[2]*x**3+coeff[3]*x**2+coeff[4]*x**1+coeff[5]*x**0"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 13,
|
|
"metadata": {},
|
|
"outputs": [
|
|
{
|
|
"data": {
|
|
"text/plain": [
|
|
"array([ 0.10275887, 0.03660033, -0.21750601, 0.03551043, -0.53861442,\n",
|
|
" -0.58083993])"
|
|
]
|
|
},
|
|
"execution_count": 13,
|
|
"metadata": {},
|
|
"output_type": "execute_result"
|
|
}
|
|
],
|
|
"source": [
|
|
"coeff"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 20,
|
|
"metadata": {},
|
|
"outputs": [
|
|
{
|
|
"data": {
|
|
"text/plain": [
|
|
"(-0.7501943063424155,\n",
|
|
" 15.750194306342419,\n",
|
|
" -0.3404752506209626,\n",
|
|
" 4.2088127135203335)"
|
|
]
|
|
},
|
|
"execution_count": 20,
|
|
"metadata": {},
|
|
"output_type": "execute_result"
|
|
},
|
|
{
|
|
"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,2,0.001) #interp range of curve \n",
|
|
"\n",
|
|
"# VEHICLE REF FRAME\n",
|
|
"ax1=plt.subplot(2,1,1)\n",
|
|
"ax1.set_title('parametrized curve, vehicle ref frame')\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",
|
|
"ax2=plt.subplot(2,1,2)\n",
|
|
"ax2.set_title('waypoints, map ref frame')\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 value 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": 15,
|
|
"metadata": {},
|
|
"outputs": [
|
|
{
|
|
"name": "stdout",
|
|
"output_type": "stream",
|
|
"text": [
|
|
"0.03551042807053904\n",
|
|
"12.271049217491628\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 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.6.9"
|
|
}
|
|
},
|
|
"nbformat": 4,
|
|
"nbformat_minor": 4
|
|
}
|