[AMR] Fix errors pointed out to me

2026-09-10 19:15:25 +02:00 · 2026-08-09 08:30:17 +02:00
parent cc5f111813
commit a8b3bd7b20
24 changed files with 50 additions and 47 deletions
@@ -8,7 +8,6 @@
 \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.

 \bi{Discrete} {\scriptsize via graph/grid}: {\color{ForestGreen} complete solution}, {\color{red} Curse of dimensions}

@@ -1,5 +1,6 @@
 \subsubsection{A* Algorithm}
-Function \texttt{h(\dots)} is a lower bound of optimal cost. 
+Function \texttt{h(\dots)} is a lower bound of optimal cost.
+Tends to be faster than Dijkstra.
 % TODO: If space constrained, move to below
 % Init vert. with \texttt{dist}, \texttt{totDistEst} to $\8$, \texttt{prev} \texttt{undef}, \texttt{start}
 \begin{algorithm}
@@ -19,7 +19,7 @@
                    \State \Comment{Extension of \texttt{RRT}* goes here}
                    \State \texttt{insertEdge}{\texttt{Graph}, $x_n, x_f$}
                \EndIf
-                \If{$x_f \texttt{==} x_n$}
+                \If{$x_f \texttt{==} \text{goal}$}
                    \State \Return{\texttt{SUCCESS}, \texttt{Graph}}
                \EndIf
            \EndFor
@@ -33,11 +33,19 @@ Extension to RRT* to make path better:
    \small
    \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 $x_{\min} \gets x_n$
        \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_{\min} \gets x_\text{near}$
+                \State $c_{\min} \gets$ \Call{cost}{$x_\text{near}$} + \Call{edgeCost}{$x_\text{near}$, $x_f$}
+            \EndIf
+        \EndFor
+        \State \Call{insertEdge}{\texttt{Graph}, $x_{\min}, x_f$}
+        \State \Call{cost}{$x_f$} $\gets c_{\min}$
+        \For{each $x_{\text{near}}$ in $X_\text{near}$}
+            \If{\Call{edgeCollisionFree}{$x_f, x_\text{near}$} and\\ \Call{cost}{$x_f$} + \Call{edgeCost}{$x_f, x_\text{near}$} $<$ \Call{cost}{$x_\text{near}$}}
+                \State \Call{insertEdge}{\texttt{Graph}, $x_f, x_{\text{near}}$} \Comment{new connection}
                \State $x_\text{parent} \gets$ \Call{parent}{\texttt{Graph}, $x_{\text{near}}$}
                \State \Call{removeEdge}{\texttt{Graph}, $x_\text{parent}, x_{\text{near}}$}
            \EndIf
@@ -46,4 +54,4 @@ Extension to RRT* to make path better:
 \end{algorithm}

 \bi{Informed RRT*} extension: Once conn. betw. start and goal found, restrict sampling to (hyper)ellipsoid.
-$b = 0.5 \sqr{d^2 - ||x_s - x_g||^2}$.
+$b = 0.5 \sqrt{d^2 - ||x_s - x_g||^2}$.
@@ -7,7 +7,7 @@ Compute arcs with coll. {\color{ForestGreen} Accounts for Kino-Dyn}, {\color{red

 \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}
+        \frac{1}{2} k_\text{rep} \left( \frac{1}{\rho(\vec{x})} + \frac{1}{\rho_{\lim}} \right)^2 & \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}