Rrtstar

Note on implementation of informed RRT*

Preliminary knowledge on informed-RRT*

Let us define $f(x)$ as minimum cost of the path when path is constrained to pass through $x$ (so path will be $x_{\mathrm{start}}\to \mathrm{x}\to {\mathrm{x}}_{\mathrm{goal}}$). Also, let us define $c_{\mathrm{best}}$ as the current minimum cost of the feasible paths. Let us define a set $ X(f) = \left{ x \in X | f(x) < c*{\mathrm{best}} \right} $. If we could sample a new point from $X_f$ instead of $X$ as in vanilla RRT*, chance that $c\ast \mathrm{best}$ is updated is increased, thus the convergence rate is improved.

In most case, $f(x)$ is unknown, thus it is straightforward to approximate the function $f$ by a heuristic function $\hat{f}$. A heuristic function is admissible if $\mathrm{\forall }x\in X,\hat{f}(x)<f(x)$, which is sufficient condition of conversion to optimal path. The good heuristic function $\hat{f}$ has two properties: 1) it is an admissible tight lower bound of $f$ and 2) sampling from $X(\hat{f})$ is easy.

According to Gammell et al [1], a good heuristic function when path is always straight is $\hat{f}(x)=||x_{\mathrm{start}}-x||+||x-x_{\mathrm{goal}}||$. If we don't assume any obstacle information the heuristic is tightest. Also, $X(\hat{f})$ is hyper-ellipsoid, and hence sampling from it can be done analytically.

Modification to fit reeds-sheep path case

In the vehicle case, state is $x=(x_1,x_2,\theta )$. Unlike normal informed-RRT* where we can connect path by a straight line, here we connect the vehicle path by a reeds-sheep path. So, we need some modification of the original algorithm a bit. To this end, one might first consider a heuristic function ${\hat{f}}_{\mathrm{RS}}(x)=\mathrm{RS}(x_{\mathrm{start}},x)+\mathrm{RS}(x,x_{\mathrm{goal}})<f(x)$ where $\mathrm{RS}$ computes reeds-sheep distance. Though it is good in the sense of tightness, however, sampling from $X({\hat{f}}_{RS})$ is really difficult. Therefore, we use ${\hat{f}}_{euc}=||\mathrm{pos}(x_{\mathrm{start}})-\mathrm{pos}(x)||+||\mathrm{pos}(x)-\mathrm{pos}(x_{\mathrm{goal}})||$, which is admissible because $\mathrm{\forall }x\in X,{\hat{f}}_{euc}(x)<{\hat{f}}_{\mathrm{RS}}(x)<f(x)$. Here, $\mathrm{pos}$ function returns position $(x_1,x_2)$ of the vehicle.

Sampling from $X({\hat{f}}_{\mathrm{euc}})$ is easy because $X({\hat{f}}_{\mathrm{euc}})=\mathrm{Ellipse}\times (-\pi ,\pi ]$. Here $\mathrm{Ellipse}$'s focal points are $x_{\mathrm{start}}$ and $x_{\mathrm{goal}}$ and conjugate diameters is $\sqrt{c^{2}{\mathrm{best}} - ||\mathrm{pos}(x}) - \mathrm{pos}(x_{\mathrm{goal}}))|| } $ (similar to normal informed-rrtstar's ellipsoid). Please notice that $\theta$ can be arbitrary because ${\hat{f}}_{\mathrm{euc}}$ is independent of $\theta$.

[1] Gammell et al., "Informed RRT*: Optimal sampling-based path planning focused via direct sampling of an admissible ellipsoidal heuristic." IROS (2014)