論文紹介 Real-Time Loop Closure in 2D LIDAR SLAM ササキ(@saitosasaki)

本論文 • Real-Time Loop Closure in 2D LIDAR SLAM - Google Cartographer の論文 - ICLA2016採択 - 論文だと二次元のみだが実装には三次元もあり SLAMシステムの構成 （移動ロボットの環境認識より引用） •概要 - フロントエンド グリッドベースのスキャンマッチング による逐次SLAM - バックエンド リアルタイム性のあるグラフベースSLAM 逐次SLAM グラフベースSLAM

逐次SLAM • グリッドベースのスキャンマッチング 確率格子(grid)M(rZ×rZ→[pmin,pmax])を 確率phit とpmiss で更新 • Scan-to-Map(S2M)の非線形最小二乗問題 値を範囲(low, high) 内に収める関数だが 論文だとhighしかない スキャン S2MのTransformation 網掛けと塗りつぶし:phit 網掛けのみ:pmiss S2Sに比べ、 累積誤差の軽減 スキャン点数 姿勢 バイキュービック補間でsmoothにしたM (R 2 → R) LIDAR r は論文の例では5cm

グラフベースSLAM • Sparse Pose Adjustment(SPA)[1] - Levenberg-Marquardt（LM）法で更新が終わったサブマップ・ スキャンの姿勢間の制約からなるPose-Graphの最適化 - 制約の数はポーズ数に比例するだけなので、行列はスパース →スパース線形システム用に最適化されたコレスキー分解 ソルバーを用いる ρ：損失関数、E：残差、Σiｊ：共分散、ξij：相対姿勢 ξ：サブマップの姿勢、 ξ ：スキャンの姿勢 （例えば、損失関数ρはHuber損失を用いる） [1]Sparse Pose Adjustment for 2D Mapping,IROS 2010 制約

グラフベースSLAM • Branch-and-bound scan matching(BBS) バックグラウンドで、ループクロージャ制約生成のためにピクセル精度のマッチングを行いたい W：探索範囲 Mnearest ：最も近い格子点の引数を丸めることで実数に拡張されたM 探索範囲における角度のステップサイズδθ 探索範囲の例( Wx = Wy = 7 m and Wθ = 30◦ )

グラフベースSLAM • 単純な探索アルゴリズム x,y,θの探索範囲でbest_scoreを全探索 探索範囲の例( Wx = Wy = 7 m and Wθ = 30◦ ) 単純な探索は遅い→Generic branch and bound(分岐限定法)を使用

グラフベースSLAM • Generic branch and bound(分岐限定法) ・元の問題を解くのではなく緩和問題を繰り返し解き、 厳密解を求める 緩和問題の例)xの解が0か1になる問題を、0<=x<=1として 解く ・分岐(branch)：ノードの分割 ・限定(bound)：分割された子ノードに対し、上限を求め、 求めた上限が暫定解の値以下なら、そのノードを分岐 させない(下限は今回考えない) ・ポイント 1)ノード選択 2)分岐(branching)規則 3)上限(upper bounds)の計算

グラフベースSLAM • Generic branch and bound(分岐限定法) 1)ノード選択：深さ優先探索(Depth First Search)を使用 深さ優先探索(wikipediaより引用) 葉ノード(末端) 高さch ＝0 根ノード 高さch ＝h0

グラフベースSLAM • Generic branch and bound(分岐限定法) 2)分岐(branching)規則 ch> 1であるノードCは高さch－1の最大4個の子に分岐 ツリーの各ノード c =（cx、cy、cθ、ch）∈ 4 高さchのノードは 最大2ℎ× 2ℎ個の並進移動の組み合わせと 特定の回転を表す 葉ノードは高さch ＝0を有し、以下の解に対応 根ノードは固定の高さh0で初期ノードC0に分岐

グラフベースSLAM • Generic branch and bound(分岐限定法) 3)上限(upper bounds)の計算 限定のために、計算量と範囲の質の 両方の観点から以下を使用 効率的な計算のために、事前計算済みグリッド ℎ を使用 高さchごとにグリッドを事前計算することで、 スキャン数に線形比例した効率でスコアを計算

まとめ • Real-Time Loop Closurer in 2D LIDAR SLAM - グリッドベースの逐次SLAMとリアルタイム性のあるグラフベース SLAM - Scan-to-Mapマッチングによって逐次SLAMの誤差累積低減 - Sparse Pose AdjustmentによるPose-Graphの最適化 - branch and boundによる高精度なマッチングでループクロージャ制約 を生成

参考にした資料 • 移動ロボットの環境認識 —地図構築と自己位置推定 - J-Stage • Sparse Pose Adjustment for 2D Mapping,IROS 2010 • B.2.3 分枝限定法 - 株式会社NTTデータ数理システム