diff --git a/electives/amr/autonomous-mobile-robots-cheatsheet.pdf b/electives/amr/autonomous-mobile-robots-cheatsheet.pdf index 9ad48b1..21f660a 100644 Binary files a/electives/amr/autonomous-mobile-robots-cheatsheet.pdf and b/electives/amr/autonomous-mobile-robots-cheatsheet.pdf differ diff --git a/electives/amr/autonomous-mobile-robots-cheatsheet.tex b/electives/amr/autonomous-mobile-robots-cheatsheet.tex index 0963adc..c2e6544 100644 --- a/electives/amr/autonomous-mobile-robots-cheatsheet.tex +++ b/electives/amr/autonomous-mobile-robots-cheatsheet.tex @@ -85,6 +85,10 @@ \input{parts/05_planning-control/00_feedback-control/02_lqr.tex} \input{parts/05_planning-control/00_feedback-control/03_mpc.tex} \input{parts/05_planning-control/01_motion-planning-exploration/00_intro.tex} +\input{parts/05_planning-control/01_motion-planning-exploration/01_a-star-algorithm.tex} +\input{parts/05_planning-control/01_motion-planning-exploration/02_rrt.tex} +\input{parts/05_planning-control/01_motion-planning-exploration/03_exploration.tex} +\input{parts/05_planning-control/01_motion-planning-exploration/04_collision-avoidance.tex} % \input{parts/05_planning-control/01_motion-planning-exploration/} \end{document} diff --git a/electives/amr/parts/05_planning-control/01_motion-planning-exploration/00_intro.tex b/electives/amr/parts/05_planning-control/01_motion-planning-exploration/00_intro.tex index e69de29..9d2fc14 100644 --- a/electives/amr/parts/05_planning-control/01_motion-planning-exploration/00_intro.tex +++ b/electives/amr/parts/05_planning-control/01_motion-planning-exploration/00_intro.tex @@ -0,0 +1,11 @@ +\subsection{Motion Planning} +\bi{Config. Space}: $\cC$. Subset of robot states for planning. + +\bi{Free C-Space}: $\cC_{\text{free}}$, \bi{C-Space Obstacles} $\cC_\text{obst}$ (occupied) + +\bi{Collision checker}: $c(\vec{x}) : \cC \rightarrow \{ 0, 1 \}$ + +\bi{Visibility graph}: Connect corners, goal outside obstacles + +\bi{Voronoi Diagram}: Edges at max. dist. from obst. (benefit: safer paths). +Also tends to be faster than Dijkstra. diff --git a/electives/amr/parts/05_planning-control/01_motion-planning-exploration/01_a-star-algorithm.tex b/electives/amr/parts/05_planning-control/01_motion-planning-exploration/01_a-star-algorithm.tex new file mode 100644 index 0000000..d3a38bc --- /dev/null +++ b/electives/amr/parts/05_planning-control/01_motion-planning-exploration/01_a-star-algorithm.tex @@ -0,0 +1,36 @@ +\subsubsection{A* Algorithm} +Function \texttt{h(\dots)} is a lower bound of optimal cost. +% TODO: If space constrained, move to below +% Init vert. with \texttt{dist}, \texttt{totDistEst} to $\8$, \texttt{prev} \texttt{undef}, \texttt{start} +\begin{algorithm} + \begin{algorithmic}[1] + \Procedure{Astar}{\texttt{Graph, start, goal}} + \For{each \texttt{v} in \texttt{Graph.Verticies}} + \State $\texttt{dist[v]} \gets \8$ + \State $\texttt{totDistEst[v]} \gets \8$ + \State $\texttt{prev[v]} \gets \texttt{undef}$ + \EndFor + \State \texttt{insert start into openSet} + \State $\texttt{dist[start]} \gets 0$ + \State $\texttt{totDistEst[start]} \gets \texttt{h(start, goal)}$ + \While{\texttt{openSet not empty}} + \State $\texttt{u} \gets \texttt{vert in openSet w/ min totDistEst}$ + \State \texttt{remove u from openSet} + \If{\texttt{u == goal}} + \State \Return{\texttt{dist[u], prev}} + \EndIf + \For{each neighbour \texttt{v} of \texttt{u}} + \State $\texttt{alt} \gets \texttt{dist[u] + G.Edges(u, v).dist}$ + \If{\texttt{alt < dist[v]}} + \State $\texttt{dist[v]} \gets \texttt{alt}$ + \State $\texttt{totDistEst[v]} \gets \texttt{alt + h(v, goal)}$ + \State \texttt{insert v into openSet} + \State $\texttt{prev[v]} \gets \texttt{u}$ + \EndIf + \EndFor + \EndWhile + \State \Return $\8$, \texttt{prev} + \EndProcedure + \end{algorithmic} +\end{algorithm} + diff --git a/electives/amr/parts/05_planning-control/01_motion-planning-exploration/02_rrt.tex b/electives/amr/parts/05_planning-control/01_motion-planning-exploration/02_rrt.tex new file mode 100644 index 0000000..ab4f9fe --- /dev/null +++ b/electives/amr/parts/05_planning-control/01_motion-planning-exploration/02_rrt.tex @@ -0,0 +1,46 @@ +\newpage +\subsubsection{Rapidly-Exploring Random Tree (RRT)} +\begin{algorithm} + \begin{algorithmic}[1] + \Procedure{RRT}{\texttt{start, goal}} + \State \Call{insertVertex}{\texttt{start, Graph}} + \For{\texttt{k < MAX\_ITER}} + \State $\texttt{s} \gets \texttt{scal.}\ \texttt{unif.}\ \texttt{rand.}\ \texttt{sample on}\ [0, 1]$ + \If{\texttt{s < GOAL\_CONNECT\_PROB}} + \State $x \gets \texttt{goal}$ + \Else + \State $x \gets \texttt{random coll.-free config}$ + \EndIf + \State $x_n \gets$ \Call{nearestConfiguration}{\texttt{Graph}, $x$} + \State $x_f \gets$ \Call{stoppingConfiguration}{$x_n, x$} + \If{$x_f \neq x_n$} + \State \texttt{insertVertex}{\texttt{Graph}, $x_f$} + \State \Comment{Extension of \texttt{RRT}* goes here} + \State \texttt{insertEdge}{\texttt{Graph}, $x_n, x_f$} + \EndIf + \If{$x_f \texttt{==} x_n$} + \State \Return{\texttt{SUCCESS}, \texttt{Graph}} + \EndIf + \EndFor + \State \Return \texttt{FAILURE}, \texttt{Graph} + \EndProcedure + \end{algorithmic} +\end{algorithm} +Returns a collision-free path as grapha. Need nearest neighbour search. +Extension to RRT* to make path better (because this is quite bad) +\begin{algorithm} + \begin{algorithmic}[1] + \State $X_{\text{near}} \gets$ \Call{neighbours}{Graph, $x_f$, $R$} + \State $x_{\min} \gets$ \Call{neighbours}{Graph, $x_f$, $R$} + \State $c_{\min} \gets$ \Call{cost}{$x_n$} + \Call{edgeCost}{$x_n, x_f$} + \For{each $x_{\text{near}}$ in $X_\text{near}$} + \If{\Call{edgeCollisionFree}{$x_\text{near}, x_f$} and \Call{cost}{$x_\text{near}$} + \Call{edgeCost}{$x_\text{near}, x_f$} $< c_{\min}$} + \State \Call{insertEdge}{\texttt{Graph}, $x_f, x_{\text{near}}$} + \State $x_\text{parent} \gets$ \Call{parent}{\texttt{Graph}, $x_{\text{near}}$} + \State \Call{removeEdge}{\texttt{Graph}, $x_\text{parent}, x_{\text{near}}$} + \EndIf + \EndFor + \end{algorithmic} +\end{algorithm} + +\bi{Informed RRT*} extension: Once conn. betw. start and goal found, restrict sampling to (hyper)ellipsoid. diff --git a/electives/amr/parts/05_planning-control/01_motion-planning-exploration/03_exploration.tex b/electives/amr/parts/05_planning-control/01_motion-planning-exploration/03_exploration.tex new file mode 100644 index 0000000..5496e6a --- /dev/null +++ b/electives/amr/parts/05_planning-control/01_motion-planning-exploration/03_exploration.tex @@ -0,0 +1,2 @@ +\subsubsection{Exploration} +Find action sequence by finding goals, planning collision-free paths there, choose goal/path with maximum utility (typically max map information) diff --git a/electives/amr/parts/05_planning-control/01_motion-planning-exploration/04_collision-avoidance.tex b/electives/amr/parts/05_planning-control/01_motion-planning-exploration/04_collision-avoidance.tex new file mode 100644 index 0000000..8f311f0 --- /dev/null +++ b/electives/amr/parts/05_planning-control/01_motion-planning-exploration/04_collision-avoidance.tex @@ -0,0 +1,13 @@ +\subsubsection{Collision Avoidance} +\bi{Dynamic Window Approach} Assume: Robot moves inst. on circ. arcs $(v, \omega)$. +Compute arcs with coll. {\color{ForestGreen} Accounts for Kino-Dyn}, {\color{red} Cost func prone to loc. min, assumes static obj} + +\bi{Vel. Obst.} Assume: Robot in str. line $(v_x, v_y)$. Comp pos with coll. + {\color{ForestGreen} Vel. of obj}, {\color{red} prone to local optima, no kino-dyn.} + +\bi{Potential Field Methods} Define {\color{gray} \textit{repulsive}} and {\color{purple} attractive} potential $c = {\color{purple} c_{\text{att}}} + {\color{gray} c_{\text{rep}}}$. +With e.g. ${\color{purple} c_\text{att}} = \frac{1}{2} k_\text{att} ||\vec{x} - \vec{x}_\text{goal}||^2$ and ${\color{gray} c_\text{rep}} = \begin{cases} + \frac{1}{2} k_\text{rep} \left( \frac{1}{\rho(\vec{x})} + \frac{1}{\rho_{\lim}} \right) & \rho \leq \rho_{\lim} \\ + 0 & \text{else} + \end{cases}$ + {\color{ForestGreen} Simple control laws}, {\color{red} may trap in loc. min., no diff. const, no guar. to avoid coll}