[PYTHON] Trajectory estimation simulation using Graph-Based SLAM
1.First of all
According to "Probability Robotics" [^ 1], SLAM is roughly divided into __ online SLAM__ </ font> and __ offline SLAM__ </ font> 2 It is classified into two methods. __ Online SLAM__ </ font> means that the posterior probabilities of the map and posture are calculated at each time.
\begin{align}
p(\ x_t,\ m\ |\ z_{1:t},\ u_{1:t}\ ) \tag{1.1}\\
\end{align}
As you can see from the formula $ (1.1) $, online SLAM </ font> has the latest attitude to past observations and controls $ x_t $ __ only __ Is being calculated. Online SLAM </ font> is a __real-time algorithm __ to calculate the latest postures sequentially. The self-position estimation algorithm by the extended Kalman filter [^ 2] and particle filter [^ 3] that I created earlier corresponds to this online SLAM </ font>.
On the other hand, in __ offline SLAM__ </ font> (sometimes called complete SLAM), not only the latest attitude $ x_t $, but the entire robot trajectory $ x_ {1 The posterior probability is calculated for: t} $.
\begin{align}
p(\ x_{1:t},\ m\ |\ z_{1:t},\ u_{1:t}\ ) \tag{1.2}\\
\end{align}
Offline SLAM </ font> is batch algorithm because the calculation is executed at a specific timing.
2 Graph-Based SLAM The mainstream of SLAM these days is offline SLAM using graphs, and offline SLAM using graphs is generally called Graph-Based SLAM [^ 4]. This time, I would like to implement this Graph-Based SLAM in Python and check its operation. The papers referred to during implementation are "A Tutorial on Graph-based SLAM [^ 5]" and "Explanation of graph-based SLAM [^ 6]".
3 Robot posture and motion model
The posture and motion model of the robot applies to "Chapter 5 Robot Motion" in the book "Probabilistic Robotics" [^ 1], so please refer to the book for details. Therefore, a brief explanation will be given here. First, the posture $ \ boldsymbol {x} $ of the robot at a discrete time $ t $ is defined as follows.
\begin{align}
\boldsymbol{x}_{t} =
\begin{bmatrix}
x_{t} \\
y_{t} \\
\theta_{t} \\
\end{bmatrix} \tag{3.1}\\
\end{align}
At time $ t $, the robot is given translation speed $ v {t} $ __ and __ rotation speed $ \ omega {t} $ _, and at time $ t'$ the attitude $ \ boldsymbol x { It is assumed that t'} $ is obtained. However, in the real environment, _translation speed $ v {t} $ __ and _rotation speed $ \ omega {t} $ __ are noisy, so they can be expressed by the following formula.
\begin{align}
\begin{bmatrix}
x_{t'} \\
y_{t'} \\
\theta_{t'} \\
\end{bmatrix}
=
\begin{bmatrix}
x_{t} \\
y_{t} \\
\theta_{t} \\
\end{bmatrix}
+
\begin{bmatrix}
-\frac{v_{t}}{\omega_{t}}sin\theta_{t}
+ \frac{v_{t}}{\omega_{t}}sin(\theta_{t}+\omega_{t}\Delta t) \\
\frac{v_{t}}{\omega_{t}}cos\theta_{t}
- \frac{v_{t}}{\omega_{t}}cos(\theta_{t}+\omega_{t}\Delta t) \\
\omega_{t}\Delta t + \gamma_{t}\Delta t \\
\end{bmatrix}
\tag{3.2}\\
\end{align}
However,
\begin{align}
\begin{bmatrix}
v_{t} \\
\omega_{t} \\
\gamma_{t} \\
\end{bmatrix}
=
\begin{bmatrix}
v^\ast_{t} \\
\omega^\ast_{t} \\
0 \\
\end{bmatrix}
+
\begin{bmatrix}
\varepsilon_{\alpha_1 {v_{t}^\ast}^2 + {\alpha_2 \omega_{t}^\ast}^2} \\
\varepsilon_{\alpha_3 {v_{t}^\ast}^2 + {\alpha_4 \omega_{t}^\ast}^2} \\
\varepsilon_{\alpha_5 {v_{t}^\ast}^2 + {\alpha_6 \omega_{t}^\ast}^2} \\
\end{bmatrix}
\tag{3.3}\\
\end{align}
Is. Here, $ v_ {t} ^ \ ast $ and $ \ omega_ {t} ^ \ ast $ represent the true values of the translation speed and the rotation speed, respectively. Also, $ \ varepsilon $ represents noise, and $ \ alpha_1 $ to $ \ alpha_6 $ are robot-specific noise parameters. Here, $ \ varepsilon_b $ means an error variable with a mean of zero and a standard deviation of $ b $.
3.1 Definition of coordinate system
In implementing Graph-Based SLAM, the following three coordinate systems are defined.
__ World coordinate system __
A coordinate system based on a certain point in the world. Resting landmarks basically do not move in the world coordinate system.
__ Robot coordinate system __
A coordinate system based on a certain point on the robot. The reference point moves with the robot. Therefore, the stationary landmark will move as the robot moves.
In addition, landmarks are basically observed in this robot coordinate system.
4 Landmark observation model
For the landmark observation model, "2.2 Observation" in "Explanation of graph-based SLAM [^ 6]" is applied, so please refer to that. Therefore, a brief description will be given here. By observing landmark $ L_c $ in the robot coordinate system at a certain discrete time $ t $, it is assumed that information on the following three landmarks can be obtained.
--Distance to landmark $ L_c $: $ d_ {c, t} $ --Direction where landmark $ L_c $ was observed: $ \ varphi_ {c, t} $ --Landmark $ L_c $ Direction to which you are facing: $ \ psi_ {c, t} $
The formula is as follows.
\begin{align}
\begin{bmatrix}
d_{c,t} \\
\varphi_{c,t} \\
\psi_{c,t} \\
\end{bmatrix}
=
\begin{bmatrix}
d^\ast_{c,t} \\
\varphi^\ast_{c,t} \\
\psi^\ast_{c,t} \\
\end{bmatrix}
+
\begin{bmatrix}
\varepsilon_{\beta_1 {d^\ast_{c,t}}^2} \\
\varepsilon_{\beta_2} \\
\varepsilon_{\beta_3} \\
\end{bmatrix}
\tag{4.1}\\
\end{align}
$ d ^ \ ast_ {c, t} $, $ \ varphi ^ \ ast_ {c, t} $, $ \ psi ^ \ ast_ {c, t} $ represent the true values, respectively, from $ \ beta_1 $ $ \ Beta_3 $ is a robot-specific noise parameter.
5 Graph-Based SLAM problem
5.1 Relative attitude calculation of robot
To solve the problem, consider a graph with nodes (vertices) and edges (edges). The nodes correspond to the posture of the robot at each time, and the edge is set between two nodes that observe the same landmark.
At this time, it can be seen that there are the following two methods for calculating the relative posture of the robot at each time. ① Difference in estimated robot attitude between nodes at each time: $ \ Delta \ boldsymbol x_ {t, t'} $
This can be done by simply calculating the attitude difference.
\begin{align}
\Delta \boldsymbol{x}_{t,t'} = \boldsymbol{x}_{t'} - \boldsymbol{x}_t
\tag{5.1}\\
\end{align}
The figure below shows the results of observing landmarks and converting them into a coordinate system based on the landmarks based on the observation results.
From this figure, the relative posture of the robot with respect to the landmark $ \ boldsymbol x ^ L_ {c} $ is expressed by the following formula.
\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}
This is calculated for the time $ t $ and $ t'$, and the difference is calculated.
\begin{align}
\Delta \boldsymbol{x}^{L}_{c,t,t'} = \boldsymbol{x}^L_{c,t'} - \boldsymbol{x}^L_{c,t}
\tag{5.3}\\
\end{align}
Now consider the difference between $ \ Delta \ boldsymbol x_ {t, t'} $ and $ \ 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'} $ when this error $ \ boldsymbol e_ {c, t, t'} $ is zero, that is, $ \ boldsymbol x_ {t} $ and $ \ boldsymbol x_ { It can be said that t'} $ is the optimum estimated posture. However, since each edge contains an error and each node is also associated with other edges, it is unlikely that all errors $ \ boldsymbol e_ {c, t, t'} $ will be zero. It's impossible. Therefore, it is necessary to consider a calculation method of the estimated posture in which all errors $ \ boldsymbol e_ {c, t, t'} $ are optimal.
5.2 Calculation of information matrix
For the calculation model of the information matrix, refer to "3.1.2 $ \ Sigma_ {c, t, t'}, \ Omega_ {c, t, t'} $ calculation" in "Explanation of graph-based SLAM [^ 6]". It will be applied, so please refer to that. Therefore, a brief description will be given here.
\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}
Next, $ \ boldsymbol \ Sigma_ {c, t} $ and $ \ boldsymbol \ Sigma_ {c, t'} $ calculated in the measured coordinate system are converted into the world coordinate system. The following rotation matrix is used for the 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}
However,
\begin{align}
c = cos(\theta_t + \varphi_t) \tag{5.7}\\
s = sin(\theta_t + \varphi_t) \tag{5.8}\\
\end{align}
Is.
- The signs of the formula $ (10) $ and "s" in the reference "Explanation of graph-based SLAM [^ 6]" are different. I have a positive counterclockwise direction, but did the references have a positive clockwise direction?
Finally, the covariance matrix $ \ boldsymbol \ Sigma_ {c, t, t'} $ and the information matrix $ \ boldsymbol \ Omega_ {c, t, t'} $ in the world coordinate system are as follows.
\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 Optimization method
It is finally the optimization problem that is the heart of the main subject. First, the robot creates all pairs of two edges of the observation results obtained at each time. Let the set of all pairs be $ \ mathcal {C} $ and define an evaluation function to find the sum of squared errors.
\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}
The ultimate goal is to find $ \ hat {\ boldsymbol {x}} $ that minimizes $ \ boldsymbol {F} (\ boldsymbol {x}) $ in this above equation.
The Gauss-Newton method and the Levenberg-Marquardt method are generally used as the approach method for finding the minimum value $ \ hat {\ boldsymbol {x}} $. For the Gauss-Newton method and Levenberg-Marquardt method, the following sites will be helpful, so please take a look if you are interested.
-CVIM # 11 3. Numerical calculation for minimization
This time, we will solve it by the Gauss-Newton method in the same way as "A Tutorial on Graph-based SLAM [^ 5]".
First of all, $ \ boldsymbol F_ {c, t, t'} $ and $ \ boldsymbol x_ {t'} $, respectively, $ \ Delta \ boldsymbol x_ {t} $, $ \ Delta \ boldsymbol x_ {t' } $ Consider the case of changing. Expressed as an 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}
It is expressed as. However, the Jacobian matrix uses $ \ 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}
It is expressed as. Therefore,
\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} $ in the expression $ (5.20) $ is a constant. Furthermore, when the inflection point of the equation $ (5.20) $ is calculated,
\begin{align}
2 \boldsymbol{b} + 2 \boldsymbol{H} \Delta{\boldsymbol{x}} = 0 \tag{5.21}\\
\end{align}
So $ \ Delta \ boldsymbol {x} $ is
\begin{align}
\Delta{\boldsymbol{x}} = -\boldsymbol{H}^{-1} \boldsymbol{b} \tag{5.22}\\
\end{align}
Will be. Update $ \ boldsymbol {x} $ with the calculated $ \ Delta \ boldsymbol {x} $.
\begin{align}
\boldsymbol{x} \leftarrow \boldsymbol{x}＋\Delta \boldsymbol{x} \tag{5.23}\\
\end{align}
Calculate $ \ boldsymbol {F} (\ boldsymbol {x}) $ of formula $ (5.11) $ using the updated $ \ boldsymbol {x} $, and formula $ (5.12) $ until the calculation result is sufficiently small. Repeat the calculation from. Let $ \ boldsymbol {x} $ when the calculation result is sufficiently small be the optimal solution $ \ hat {\ boldsymbol {x}} $.
6 Implementation
I implemented Graph-Based SLAM in Python. The script is published on GitHub. ・ GitHub --graph_based_slam.py
The result of executing the script is as follows.
(Click the image to jump to YouTube.)
- Detailed explanation will be added later.
7 Finally
I tried to implement Graph-Based SLAM, which is popular recently, but honestly, it was difficult to implement and write this article. Sometimes it didn't work, but I'm glad I managed to get it to work as I intended. It's still a thin article, but I'll publish it here. I think there are many places where the explanation is insufficient, but if you can point it out, I will add it. In addition, Mr. Ueda's commentary was very helpful in implementing it.
-"Explanation of graph-based SLAM [^ 6]"
Next, I would like to run SLAM on the actual machine ♪
References
[^ 1]: "Probability Robotics (Premium Books Edition)", Mynavi Publishing, 2005. [^ 2]: Visualization of self-position estimation operation by extended Kalman filter [^ 3]: Visualization of self-position estimation operation by particle filter [^ 4]: Masahiro Tomono: Environmental recognition of mobile robots — Map construction and self-position estimation