La figure ci-dessous montre le résultat de la conversion d'un repère en un système de coordonnées basé sur le repère basé sur le résultat de l'observation.

A partir de cette figure, la posture relative du robot par rapport au repère $ \ boldsymbol x ^ L_ {c} $ est exprimée par la formule suivante.

\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}

Ceci est calculé pour les temps $ t $ et $ t '$, et la différence est calculée.

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

Considérons maintenant la différence entre $ \ Delta \ boldsymbol x_ {t, t '} $ et $ \ 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 '} $ lorsque cette erreur $ \ boldsymbol e_ {c, t, t'} $ vaut zéro, c'est-à-dire $ \ boldsymbol x_ {t} $ et $ \ boldsymbol x_ { On peut dire que t '} $ est la posture estimée optimale. Cependant, puisque chaque arête contient une erreur et que chaque nœud est également associé à d'autres arêtes, il est peu probable que toutes les erreurs $ \ boldsymbol e_ {c, t, t '} $ soient nulles. C'est impossible. Par conséquent, il faut considérer une méthode de calcul de la posture estimée dans laquelle toutes les erreurs $ \ boldsymbol e_ {c, t, t '} $ peuvent être dites optimales.

5.2 Calcul de la matrice d'information

Pour le modèle de calcul de la matrice d'information, reportez-vous à "3.1.2 $ \ Sigma_ {c, t, t '}, \ Omega_ {c, t, t'} $ calcul" dans "Explication du SLAM basé sur un graphe [^ 6]". Il sera appliqué, veuillez donc vous y référer. Par conséquent, une brève description sera donnée ici.

\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}

Ensuite, convertissez $ \ boldsymbol \ Sigma_ {c, t} $ et $ \ boldsymbol \ Sigma_ {c, t '} $ calculés dans le système de coordonnées de mesure en système de coordonnées universel. La matrice de rotation suivante est utilisée pour la conversion.

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

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

cependant,

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

Est.

• Le signe de la formule $ (10) $ et "s" dans "Explication of graph-based SLAM [^ 6]" dans la référence est différent. J'ai une direction positive dans le sens antihoraire, mais les références ont-elles une direction positive dans le sens horaire?

Enfin, la matrice de covariance $ \ boldsymbol \ Sigma_ {c, t, t '} $ et la matrice d'information $ \ boldsymbol \ Omega_ {c, t, t'} $ dans le système de coordonnées mondial sont les suivantes.

\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 Méthode d'optimisation

C'est finalement le problème d'optimisation qui est au cœur du sujet principal. Tout d'abord, le robot crée toutes les paires de deux arêtes des résultats d'observation obtenus à chaque instant. Soit l'ensemble de toutes les paires $ \ mathcal {C} $ et définissons une fonction d'évaluation pour trouver la somme des carrés des erreurs.

\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}

Le but ultime est de trouver $ \ hat {\ boldsymbol {x}} $ qui minimise $ \ boldsymbol {F} (\ boldsymbol {x}) $ dans cette équation ci-dessus.

La méthode Gauss-Newton et la méthode Levenberg-Marquardt sont généralement utilisées comme méthode d'approche pour trouver la valeur minimale $ \ hat {\ boldsymbol {x}} $. Pour la méthode Gauss-Newton et la méthode Levenberg-Marquardt, les sites suivants peuvent être utiles, veuillez donc jeter un œil si vous êtes intéressé.

• CVIM # 11 3. Calcul numérique pour la minimisation

Cette fois, nous allons le résoudre par la méthode de Gauss-Newton de la même manière que "Un tutoriel sur le SLAM basé sur des graphes [^ 5]".

Tout d'abord, $ \ boldsymbol F_ {c, t, t '} $ et $ \ boldsymbol x_ {t'} $, respectivement, $ \ Delta \ boldsymbol x_ {t} $, $ \ Delta \ boldsymbol x_ {t ' } $ Prenons le cas du changement. Exprimé comme une expression

\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}

Il est exprimé comme. Cependant, la matrice Jacobi utilise $ \ 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}

Il est exprimé comme. Donc,

\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} $ dans l'expression $ (5.20) $ est une constante. De plus, lorsque le point de variation de la formule $ (5.20) $ est calculé,

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

Par conséquent, $ \ Delta \ boldsymbol {x} $ est

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

Sera. Mettez à jour $ \ boldsymbol {x} $ avec le $ \ Delta \ boldsymbol {x} $ calculé.

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

Calculez $ \ boldsymbol {F} (\ boldsymbol {x}) $ de la formule $ (5.11) $ en utilisant la mise à jour $ \ boldsymbol {x} $, et la formule $ (5.12) $ jusqu'à ce que le résultat du calcul soit suffisamment petit. Répétez le calcul à partir de. Soit $ \ boldsymbol {x} $ lorsque le résultat du calcul est suffisamment petit, la solution optimale $ \ hat {\ boldsymbol {x}} $.

6 Mise en œuvre

J'ai implémenté SLAM basé sur des graphes en Python. Le script est publié sur GitHub. ・ GitHub --graph_based_slam.py

Le résultat de l'exécution du script est le suivant. (Cliquez sur l'image pour accéder à YouTube.)

• Une explication détaillée sera ajoutée plus tard.

7 Enfin

J'ai essayé d'implémenter le SLAM basé sur des graphiques, qui est populaire récemment, mais honnêtement, il était difficile de mettre en œuvre et d'écrire cet article. Parfois, cela ne fonctionnait pas, mais je suis heureux d'avoir réussi à le faire fonctionner comme prévu. C'est encore un article mince, mais je le publierai ici. Je pense qu'il y a de nombreux endroits où l'explication est insuffisante, mais si vous pouvez le souligner, je vais l'ajouter. De plus, le commentaire de M. Ueda a été très utile pour sa mise en œuvre.

• "Explication du SLAM basé sur un graphique [^ 6]"

Ensuite, je voudrais exécuter SLAM sur la machine réelle ♪

Les références

[^ 1]: "Probability Robotics (Premium Books Version)", My Navi Publishing, 2005. [^ 2]: Visualisation de l'opération d'estimation de l'auto-position par filtre de Kalman étendu [^ 3]: Visualisation de l'opération d'estimation d'auto-position par filtre à particules [^ 4]: Masahiro Tomono: Reconnaissance environnementale des robots mobiles - Construction de cartes et estimation de l'auto-position