[AMR] Finish first rework

2026-07-27 21:29:09 +02:00 · 2026-06-17 15:27:18 +02:00
parent 5bf9d9a022
commit b5c5d2ccb1
6 changed files with 8 additions and 7 deletions
@@ -7,7 +7,7 @@ We try to minimise cost functional (for $\vec{u}(t) = -\mat{K}\vec{x}(t)$)
 \]
 Solution: $\mat{K} = \mat{R}^{-1} (\mat{G}_c^\top \mat{P})$, with $\mat{P}$ found from
 \[
-    \mat{F}_c^\top \mat{P} + \mat{PF}_c - (\mat{PG}_c) \mat{R}^{-1} + \mat{Q} = \mat{0}
+    \mat{F}_c^\top \mat{P} + \mat{PF}_c - (\mat{PG}_c) \mat{R}^{-1} (\mat{G}_c^\top \mat{P}) + \mat{Q} = \mat{0}
 \]
 Finding $\mat{K}$ is expensive, but \textit{offline}, at runtime only $\vec{u}(t)$.

@@ -1,7 +1,7 @@
 \subsection{MPC}
 \bi{Cost function} ($p(\vec{x}_N)$ \textit{terminal cost}, sum the \textit{stage cost})
 \[
-    J_{0 \rightarrow N}(\vec{x}_0, \vec{u}_0, \ldots, \vec{u_{N - 1}}) = p(\vec{x}_N) + \sum_{k = 0}^{N - 1} q(\vec{x}_k, \vec{u}_k)
+    J_{0 \rightarrow N}(\vec{x}_0, \vec{u}_0, \ldots, \vec{u}_{N - 1}) = p(\vec{x}_N) + \sum_{k = 0}^{N - 1} q(\vec{x}_k, \vec{u}_k)
 \]
 We minimize the above s.t. for $k \in \{ 0, \ldots, N - 1 \}$ we have $\vec{x}_{k + 1} = \vec{f}(\vec{x}_k, \vec{u}_k)$, $\vec{g}(\vec{x}_k, \vec{u}_k) \leq \vec{0}$
 and $\vec{x}_N \in \cX_f$ and $\vec{x}_0 = \vec{x}(0)$
@@ -13,4 +13,4 @@ Quad. Cost:
    \vec{x}_N^\top \vec{P} \vec{x}_N + \sum_{k = 0}^{N - 1} \vec{x}_k^\top \mat{Q} \vec{x}_k + \vec{u}_k^\top \mat{R} \vec{u}_k
 \]
 without constraints, \bi{State Feedback Law} $\vec{u}^*_0 = -\mat{K}\vec{x}(0)$.
-With constraints, minimize as above.
+With constraints, minimize as above, $\vec{x}_{k + 1} = \mat{F} \vec{x}_k + \mat{G} \vec{u}_k$
@@ -27,7 +27,7 @@
    \end{algorithmic}
 \end{algorithm}
 Returns a collision-free path as graph. Need nearest neighbour search.
-Extension to RRT* to make path better (because this is quite bad)
+Extension to RRT* to make path better:
 \begin{algorithm}
    \begin{algorithmic}[1]
        \State $X_{\text{near}} \gets$ \Call{neighbours}{Graph, $x_f$, $R$}
@@ -10,4 +10,5 @@ With e.g. ${\color{purple} c_\text{att}} = \frac{1}{2} k_\text{att} ||\vec{x} -
        \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}
+
+{\color{ForestGreen} Simple control laws}, {\color{red} may trap in loc. min., no diff. const, no guar. to avoid coll}
@@ -5,11 +5,11 @@ at the end large reward (good or bad).
 Def. by states $\vec{x} \in \cX$ (RL: $s$), actions $\vec{u} \in \cU$ (RL: $a$), prob. state trans. $\cT(\vec{x}, \vec{u}, \vec{x}_+) = \P(\vec{x}_+ \divider \vec{x}, \vec{u})$,
 reward func $\cR(\vec{x}, \vec{u}, \vec{x}_+)$, start state $\vec{x}_0$, optional terminal state $\vec{x}_N$.

-\bi{Utility Func} Expected reward: $V = \sum_{k = 0}^{N} r_k$ or \textit{discounted} reward  $V = \sum_{k = 0}^{\8} \gamma^k r_k$ with $\gamma < 1$
+\bi{Utility Func} Expected reward: $V = \sum_{k = 0}^{N} r_k$ or \textit{discounted} reward  $V = \sum_{k = 0}^{\8} \gamma^k r_k$ with $\gamma < 1$ (if inf. runtime)

 \bi{Solving} (Val iter) $V_0(\vec{x}) = 0$ and $V_{i + 1}(\vec{x}) = \max_{\vec{u}} Q(\vec{x}, \vec{u})$ with
 \[
-    Q(\vec{x}, \vec{u}) = \sum_{\vec{x}_+} \P(\vec{x}_+ \divider \vec{x}, \vec{u}) [\cR(\vec{x}, \vec{u}, \vec{x}^+) + \gamma V_i(\vec{x}_+)]
+    Q(\vec{x}, \vec{u}) = \sum_{\vec{x}_+} \P(\vec{x}_+ \divider \vec{x}, \vec{u}) [\cR(\vec{x}, \vec{u}, \vec{x}_+) + \gamma V_i(\vec{x}_+)]
 \]
 Repeat until conv. to $V^*$ ($\tco{|\cU||\cX|^2}$ per iter). Optimal policy:
 \[