Die folgende Abbildung zeigt das Ergebnis der Umwandlung einer Landmarke in ein Koordinatensystem basierend auf der Landmarke basierend auf dem Beobachtungsergebnis.

Aus dieser Figur wird die relative Haltung des Roboters in Bezug auf die Landmarke $ \ boldsymbol x ^ L_ {c} $ durch die folgende Formel ausgedrückt.

\begin{align}
    \boldsymbol{x}^L_{c}
    = 
    \begin{bmatrix}
        x^L_{c} \\
        y^L_{c} \\
        \theta^L_{c} \\
    \end{bmatrix}
    = 
    \begin{bmatrix}
        d_{c} \, cos \left( \pi + \varphi_{c} - \psi_{c} \right) \\
        d_{c} \, sin \left( \pi + \varphi_{c} - \psi_{c} \right) \\
        \frac{\pi}{2} - \psi_{c} \\
    \end{bmatrix}

    \tag{5.2}\\
\end{align}

Dies wird für die Zeit $ t $ und $ t '$ berechnet und die Differenz wird berechnet.

\begin{align}
    \Delta \boldsymbol{x}^{L}_{c,t,t'} = \boldsymbol{x}^L_{c,t'} - \boldsymbol{x}^L_{c,t}
    \tag{5.3}\\
\end{align}

Betrachten Sie nun den Unterschied zwischen $ \ Delta \ boldsymbol x_ {t, t '} $ und $ \ Delta \ boldsymbol x ^ L_ {c, t, t'} $.

\begin{align}
    \boldsymbol{e}_{c,t,t'} = \Delta \boldsymbol{x}_{t,t'} - \Delta \boldsymbol{x}^L_{c,t,t'}   \tag{5.4}\\
\end{align}

$ \ Delta \ boldsymbol x_ {t, t '} $ wenn dieser Fehler $ \ boldsymbol e_ {c, t, t'} $ Null ist, dh $ \ boldsymbol x_ {t} $ und $ \ boldsymbol x_ { Es kann gesagt werden, dass t '} $ die optimale geschätzte Haltung ist. Da jedoch jede Kante einen Fehler enthält und jeder Knoten auch anderen Kanten zugeordnet ist, ist es unwahrscheinlich, dass alle Fehler $ \ boldsymbol e_ {c, t, t '} $ Null sind. Es ist unmöglich. Daher ist es notwendig, eine Berechnungsmethode für die geschätzte Haltung in Betracht zu ziehen, bei der alle Fehler $ \ boldsymbol e_ {c, t, t '} $ als optimal bezeichnet werden können.

5.2 Berechnung der Informationsmatrix

Das Berechnungsmodell der Informationsmatrix finden Sie unter "3.1.2 $ \ Sigma_ {c, t, t '}, \ Omega_ {c, t, t'} $ Berechnung" in "Erläuterung des graphbasierten SLAM [^ 6]". Es wird angewendet, bitte beziehen Sie sich darauf. Daher wird hier eine kurze Beschreibung gegeben.

\begin{align}
    \boldsymbol{\Sigma}_{c,t}
    = 
    \begin{bmatrix}
        \left(\beta_1 d^\ast_{c,t}\right)^2 & 0 & 0 \\
        0 & \left(d^\ast_{c,t} \ sin \beta_2\right)^2 & 0 \\
        0 & 0 & {\beta_2}^2 + {\beta_3}^2 \\
    \end{bmatrix}

    \tag{5.5}\\
\end{align}

Als nächstes konvertieren Sie $ \ boldsymbol \ Sigma_ {c, t} $ und $ \ boldsymbol \ Sigma_ {c, t '} $, die im Messkoordinatensystem berechnet wurden, in das Weltkoordinatensystem. Die folgende Rotationsmatrix wird für die Konvertierung verwendet.

\begin{align}
    \boldsymbol{R}_{c,t}
    = 
    \begin{bmatrix}
        c & -s & 0 \\
        s & c & 0 \\
        0 & 0 & 1 \\
    \end{bmatrix}

    \tag{5.6}\\
\end{align}

Jedoch,

\begin{align}
    c = cos(\theta_t + \varphi_t)       \tag{5.7}\\
    s = sin(\theta_t + \varphi_t)       \tag{5.8}\\
\end{align}

Ist.

• Das Vorzeichen der Formel $ (10) $ und "s" in "Erläuterung des graphbasierten SLAM [^ 6]" in der Referenz ist unterschiedlich. Ich habe eine positive Richtung gegen den Uhrzeigersinn, aber hatten die Referenzen eine positive Richtung im Uhrzeigersinn?

Schließlich sind die Kovarianzmatrix $ \ boldsymbol \ Sigma_ {c, t, t '} $ und die Informationsmatrix $ \ boldsymbol \ Omega_ {c, t, t'} $ im Weltkoordinatensystem wie folgt.

\begin{align}
    \boldsymbol{\Sigma}_{c,t,t'} &= \boldsymbol{R}_{c,t}\,\boldsymbol{\Sigma}_{c,t}\,\boldsymbol{R}_{c,t}^T
                     + \boldsymbol{R}_{c,t'}\,\boldsymbol{\Sigma}_{c,t'}\,\boldsymbol{R}_{c,t'}^T   \tag{5.9}\\
    \boldsymbol{\Omega}_{c,t,t'} &= \boldsymbol{\Sigma}_{c,t,t'}^{-1}    \tag{5.10}\\
\end{align}

5.3 Optimierungsmethode

Es ist schließlich das Optimierungsproblem, das den Kern des Hauptthemas bildet. Zunächst erstellt der Roboter alle Paare von zwei Kanten der jeweils erhaltenen Beobachtungsergebnisse. Die Menge aller Paare sei $ \ mathcal {C} $ und definiere eine Bewertungsfunktion, um die Summe der quadratischen Fehler zu ermitteln.

\begin{align}
    \boldsymbol{F}(\boldsymbol{x}) = \displaystyle \sum_{ \boldsymbol{e}_{c,t,t'} 
       \in \mathcal{C} }^{  } \ \underbrace{\boldsymbol{e}_{c,t,t'}^T \, \boldsymbol{\Omega}_{c,t,t'} \, \boldsymbol{e}_{c,t,t'}}_{\boldsymbol{F}_{t,t'}}   \tag{5.11}\\
\end{align}

Das ultimative Ziel ist es, $ \ hat {\ boldsymbol {x}} $ zu finden, das $ \ boldsymbol {F} (\ boldsymbol {x}) $ in dieser obigen Gleichung minimiert.

Die Gauß-Newton-Methode und die Levenberg-Marquardt-Methode werden im Allgemeinen als Annäherungsmethode zum Ermitteln des Mindestwerts $ \ hat {\ boldsymbol {x}} $ verwendet. Für die Gauß-Newton-Methode und die Levenberg-Marquardt-Methode können die folgenden Websites hilfreich sein. Schauen Sie also bitte vorbei, wenn Sie interessiert sind.

• CVIM # 11 3. Numerische Berechnung zur Minimierung

Dieses Mal werden wir es mit der Gauß-Newton-Methode auf die gleiche Weise lösen wie "Ein Tutorial zu graphbasiertem SLAM [^ 5]".

Zuallererst $ \ boldsymbol F_ {c, t, t '} $ und $ \ boldsymbol x_ {t'} $, $ \ Delta \ boldsymbol x_ {t} $, $ \ Delta \ boldsymbol x_ {t ' } $ Betrachten Sie den Fall der Änderung. Als Ausdruck ausgedrückt

\begin{align}
    \boldsymbol{e}_{c}(\boldsymbol{x}_{t} + \Delta\boldsymbol{x}_{t},\,\boldsymbol{x}_{t'} + \Delta\boldsymbol{x}_{t'}) 
      \ &=\ \boldsymbol{e}_{c}(\boldsymbol{x}_{t,t'}+\Delta\boldsymbol{x}) \tag{5.12}\\
      \ &\simeq\ \boldsymbol{e}_{c,t,t'} + 
         \left.\frac{\partial \boldsymbol{e}_{c,t,t'}}
                    {\partial \boldsymbol{x}}
         \right|_{\boldsymbol{x}=\boldsymbol{x}_{t,t'}}\Delta\boldsymbol{x}
   \tag{5.13}
\end{align}

Es wird ausgedrückt als. Die Jacobi-Matrix verwendet jedoch $ \ boldsymbol {J} _c $

\boldsymbol{J}_{c,t,t'} = \left.\frac{\partial \boldsymbol{e}_{c,t,t'}}{\partial\boldsymbol{x}}\right|_{\boldsymbol{x}=\boldsymbol{x}_{t,t'}}   \tag{5.14}

Es wird ausgedrückt als. Deshalb,

\begin{align}
    \boldsymbol{F}(\boldsymbol{x}+\Delta\boldsymbol{x}) &= \sum_{ \boldsymbol{e}_{c,t,t'} \in \mathcal{C} } \boldsymbol{F}(\boldsymbol{x}_{t,t'}+\Delta\boldsymbol{x})   \tag{5.15}\\
   &\simeq \sum_{ \boldsymbol{e}_{c,t,t'} \in \mathcal{C} } \ 
      (\boldsymbol{e}_{c,t,t'} + \boldsymbol{J}_{c,t,t'}\Delta{\boldsymbol{x}} )^T
      \, \boldsymbol{\Omega}_{c,t,t'} \,
      (\boldsymbol{e}_{c,t,t'} + \boldsymbol{J}_{c,t,t'}\Delta{\boldsymbol{x}} )   \tag{5.16}\\
   &= \sum_{ \boldsymbol{e}_{c,t,t'} \in \mathcal{C} } \ 
      \underbrace{\boldsymbol{e}_{c,t,t'}^T \boldsymbol{\Omega}_{c,t,t'} \boldsymbol{e}_{c,t,t'}}_{c_{c,t,t'}}
     + 2 \, \underbrace{\boldsymbol{e}_{c,t,t'}^T \boldsymbol{\Omega}_{c,t,t'} \boldsymbol{J}_{c,t,t'}}_{\boldsymbol{b}_{c,t,t'}} \Delta{\boldsymbol{x}} + \Delta{\boldsymbol{x}}^T \underbrace{\boldsymbol{J}_{c,t,t'}^T \boldsymbol{\Omega}_{c,t,t'} \boldsymbol{J}_{c,t,t'}}_{\boldsymbol{H}_{c,t,t'}}\Delta{\boldsymbol{x}}   \tag{5.17}\\

   &= \sum_{ \boldsymbol{e}_{c,t,t'} \in \mathcal{C} } \ 
      c_{c,t,t'} + 2 \, \boldsymbol{b}_{c,t,t'} \Delta{\boldsymbol{x}}
     + \Delta{\boldsymbol{x}}^T \boldsymbol{H}_{c,t,t'}\Delta{\boldsymbol{x}}   \tag{5.18}\\

   &= \underbrace{\sum_{ \boldsymbol{e}_{c,t,t'} \in \mathcal{C} } c_{c,t,t'}}_{\boldsymbol{c}}
     + 2 \underbrace{\left(\sum_{ \boldsymbol{e}_{c,t,t'} \in \mathcal{C} } \, \boldsymbol{b}_{c,t,t'} \right)}_{\boldsymbol{b}} \Delta{\boldsymbol{x}} 
     + \Delta{\boldsymbol{x}}^T \underbrace{\left( \sum_{ \boldsymbol{e}_{c,t,t'} \in \mathcal{C} }  \boldsymbol{H}_{c,t,t'} \right)}_{\boldsymbol{H}} \Delta{\boldsymbol{x}}   \tag{5.19}\\

   &= \boldsymbol{c}
    + 2 \boldsymbol{b} \Delta{\boldsymbol{x}}
    + \Delta{\boldsymbol{x}}^T \boldsymbol{H} \Delta{\boldsymbol{x}}   \tag{5.20}\\
\end{align}

$ \ Boldsymbol {c} $ im Ausdruck $ (5.20) $ ist eine Konstante. Wenn der Variationspunkt der Formel $ (5.20) $ berechnet wird,

\begin{align}
   2 \boldsymbol{b} + 2 \boldsymbol{H} \Delta{\boldsymbol{x}} = 0   \tag{5.21}\\
\end{align}

Daher ist $ \ Delta \ boldsymbol {x} $

\begin{align}
   \Delta{\boldsymbol{x}} = -\boldsymbol{H}^{-1} \boldsymbol{b}   \tag{5.22}\\
\end{align}

Wird sein. Aktualisieren Sie $ \ boldsymbol {x} $ mit dem berechneten $ \ Delta \ boldsymbol {x} $.

\begin{align}
   \boldsymbol{x} \leftarrow \boldsymbol{x}＋\Delta \boldsymbol{x}   \tag{5.23}\\
\end{align}

Berechnen Sie $ \ boldsymbol {F} (\ boldsymbol {x}) $ der Formel $ (5.11) $ mit dem aktualisierten $ \ boldsymbol {x} $ und der Formel $ (5.12) $, bis das Berechnungsergebnis ausreichend klein ist. Wiederholen Sie die Berechnung von. Sei $ \ boldsymbol {x} $, wenn das Berechnungsergebnis ausreichend klein ist, die optimale Lösung $ \ hat {\ boldsymbol {x}} $.

6 Implementierung

Ich habe Graph-Based SLAM in Python implementiert. Das Skript wird auf GitHub veröffentlicht. ・ GitHub --graph_based_slam.py

Das Ergebnis der Ausführung des Skripts ist wie folgt. (Klicken Sie auf das Bild, um zu YouTube zu springen.)

• Detaillierte Erklärungen werden später hinzugefügt.

7 Endlich

Ich habe versucht, Graph-Based SLAM zu implementieren, was in letzter Zeit sehr beliebt ist, aber ehrlich gesagt war es schwierig, diesen Artikel zu implementieren und zu schreiben. Manchmal hat es nicht funktioniert, aber ich bin froh, dass ich es geschafft habe, es so zum Laufen zu bringen, wie ich es beabsichtigt hatte. Es ist immer noch ein dünner Artikel, aber ich werde ihn hier veröffentlichen. Ich denke, es gibt viele Stellen, an denen die Erklärung unzureichend ist, aber wenn Sie darauf hinweisen können, werde ich sie hinzufügen. Darüber hinaus war der Kommentar von Herrn Ueda bei der Umsetzung sehr hilfreich.

• "Erklärung des graphbasierten SLAM [^ 6]"

Als nächstes möchte ich SLAM auf dem eigentlichen Computer ausführen ♪

Verweise

[^ 1]: "Probability Robotics (Premium Books Version)", My Navi Publishing, 2005. [^ 2]: Visualisierung der Selbstpositionsschätzungsoperation durch erweiterten Kalman-Filter [^ 3]: Visualisierung der Selbstpositionsschätzungsoperation durch Partikelfilter [^ 4]: Masahiro Tomono: Umwelterkennung mobiler Roboter - Kartenkonstruktion und Selbstpositionsschätzung