Skip to main content

SLAM

SLAM は Simultaneous Localization and Mapping の略で、未知環境の map を作りながら、同時に sensor 自身の pose を推定する問題です。Robotics、AR、autonomous driving、mobile mapping で中心的な技術です。

基本構成​

典型的な SLAM system は、次の component から構成されます。

Tracking​

Tracking は、現在 frame の pose を既存 map に対して推定する処理です。Visual SLAM では、feature matching、PnP、direct alignment などが使われます。RGB-D SLAM や LiDAR SLAM では、ICP や scan matching が使われることもあります。

Mapping​

Mapping は、landmark、point cloud、mesh、voxel、TSDF、semantic map などを更新する処理です。Map representation は、sensor と用途によって異なります。

Loop closure​

SLAM では、長時間移動すると pose estimation の誤差が蓄積して drift が発生します。Loop closure は、過去に訪れた場所へ戻ってきたことを検出し、全体の pose graph を修正する処理です。

Loop closure によって、map の歪みや trajectory の drift を大きく減らせます。

Visual SLAM の代表例​

ORB-SLAM は、feature-based visual SLAM の代表的な system です。ORB feature を使い、tracking、local mapping、loop closing の thread を分けて動作します。Monocular、stereo、RGB-D、visual-inertial などへ発展しています。

Sensor 別の SLAM​

種類説明
Monocular SLAM単眼 camera を使います。Scale ambiguity が課題です。
Stereo SLAMStereo camera により metric scale を得やすいです。
RGB-D SLAMDepth sensor を使い、dense map を作りやすいです。
LiDAR SLAM3D point cloud を使います。屋外や大規模環境で強いです。
Visual-Inertial SLAMCamera と IMU を組み合わせ、motion に頑健です。
Semantic SLAMSemantic label や object を map に統合します。

SLAM の難所​

  • Dynamic object
  • Textureless environment
  • Illumination change
  • Loop closure の誤検出
  • Long-term map maintenance
  • Scale drift
  • Real-time constraint

数式で見る SLAM の状態推定​

SLAM は、trajectory と map を同時に推定する確率的な状態推定問題として書けます。時刻 1:T1:T の pose を x1:T\mathbf{x}_{1:T}、map を m\mathbf{m}、control または odometry を u1:T\mathbf{u}_{1:T}、観測を z1:T\mathbf{z}_{1:T} とすると、目的は次の posterior を求めることです。

p(x1:T,m∣z1:T,u1:T)p(\mathbf{x}_{1:T},\mathbf{m}\mid \mathbf{z}_{1:T},\mathbf{u}_{1:T})

この式の気持ちは、「これまでの motion と sensor 観測をすべて見たとき、もっともありそうな trajectory と map は何か」を求めることです。Graph-based SLAM では、maximum a posteriori estimation として次のような最小二乗問題に変換します。

min⁡{xi},m∑k∥ek(xik,xjk,m)∥Ωk2\min_{\{\mathbf{x}_i\},\mathbf{m}} \sum_k \left\|\mathbf{e}_k(\mathbf{x}_{i_k},\mathbf{x}_{j_k},\mathbf{m})\right\|_{\mathbf{\Omega}_k}^2

ここで、ek\mathbf{e}_k は odometry、loop closure、landmark observation などの residual、Ωk\mathbf{\Omega}_k は information matrix です。Information matrix は、その constraint をどれくらい信頼するかを表します。

Pose graph optimization だけを見る場合、edge (i,j)(i,j) が相対 pose 観測 Zij\mathbf{Z}_{ij} を持つとして、residual は Lie group 上で次のように書けます。

eij=Log(Zij−1Xi−1Xj)\mathbf{e}_{ij}=\mathrm{Log}\left(\mathbf{Z}_{ij}^{-1}\mathbf{X}_i^{-1}\mathbf{X}_j\right)

この式は、「推定された pose 差 Xi−1Xj\mathbf{X}_i^{-1}\mathbf{X}_j が、観測された pose 差 Zij\mathbf{Z}_{ij} と一致してほしい」という意味です。Loop closure は、遠く離れた時刻の pose 間にこのような edge を追加し、蓄積した drift を全体に分散させて修正します。

関連ページ​

主なソース​