[AMR] Motion planning

2026-05-30 16:21:19 +02:00 · 2026-05-11 16:05:56 +02:00
parent fccf55704d
commit 74e3cbc64a
7 changed files with 112 additions and 0 deletions
@@ -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}
@@ -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.
@@ -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}
+
@@ -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.
@@ -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)
@@ -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}