Improvement of Robot's Simultaneous Localization and Mapping Using an Effective Transformation to Achieve Linear Model

技術要約：線形モデルを実現するための効果的な変換を用いたロボットの同時自己位置推定および地図作成（SLAM）の改善

問題提起
同時自己位置推定および地図作成（SLAM）は移動ロボットにとって基本的なタスクであるが、拡張カルマンフィルタ（EKF）に基づく標準的な実装は、発散や不整合性に悩まされる。これらの失敗の主な原因は、ロボットの運動および観測モデルに内在する非線形性であり、それが線形化を必要とする点にある。この線形化は誤差を導入し、それが時間の経過とともに蓄積されることで、状態ベクトルの誤った更新、推定位置の跳躍、および真の不確実性を反映しない推定共分散行列の減少を引き起こす。これらの問題を軽減するために、Fast SLAM、Unscented Fast SLAM (UnFS)、およびCubature Kalman Filter (CKF) の派生手法などの様々な手法が提案されてきたが、それらはしばしば複雑な近似に依存しているか、あるいは大規模な環境や高いセンサ不確実性の下での一貫性に依然として課題を抱えている。

手法
本論文では、LMKF SLAM（Linear Model Kalman Filter SLAM）と呼ばれる新しいアプローチを提案する。核心となる手法は、標準的な非線形状態空間モデルを厳密な線形モデルへと変換することであり、これにより、線形化近似やヤコビアンの計算を必要とせずに、標準的なカルマンフィルタ（KF）を直接適用することを可能にする。

1. 状態空間の変換:

   • 運動モデル: 著者らは、ロボットが各ステップでロボットの絶対角（$\theta_k$）を測定するために単純な電子コンパスを装備していると仮定する。この測定値を組み込むことで、状態ベクトルから方位角を除外することができる。もともと$\theta_k$の三角関数によって非線形であった運動方程式は、変換される。ノイズ項はゼロの周りで線形化され、その結果、状態更新が前状態および制御入力に線形に依存する線形運動モデルが得られる。
   • 観測モデル: 同様に、距離（$r_k$）と方位角（$\phi_k$）をランドマークの位置に関連付ける観測モデルも変換される。測定されたロボットの角度$\theta_k$を利用することで、ランドマークの極座標をグローバルフレームに対するデカルト座標へと変換する。この変換（$\zeta_k$）により、観測データと状態ベクトル（ロボットおよびランドマークの位置）との関係が線形化される。

2. フィルタリングアルゴリズム:

   • 運動モデルと観測モデルの両方が線形化されているため、標準的なカルマンフィルタを直接適用する。
   • アルゴリズムは、異なるランドマークからの観測を独立したものとして扱うことで、計算速度を高めるための逐次的なランドマーク更新を採用している。
   • 状態ベクトルは、ロボットの$(x, y)$座標と、すべての観測されたランドマークの座標で構成される。

主な貢献

• 変換による線形化: 本論文は、単純な電子コンパスのみを用いて非線形SLAMモデルを線形モデルへと変換する単純な変換を導入しており、EKFのヤコビアンやUnscented Transformationのような複雑な線形化技術を排除している。
• 理論的収束性: 非線形性ゆえに収束の証明が困難なEKFベースの手法とは異なり、提案された線形モデルは厳密な収束の証明を可能にし、ランドマークの絶対位置誤差が下限値に向かうことを保証する。
• 堅牢性: この手法は、センサの不確実性（速度、操舵角、およびロボットの角度）やシステムパラメータの変化に対して非常に高い安定性を持つように設計されている。

実験結果
著者らは、人工的なシミュレーションおよび実世界のVictoria Parkデータセットの両方を用いて、LMKF SLAMを標準的なEKF、UnFS、およびImproved CKF (ICKF) と比較評価した。

• シミュレーション（人工環境）:

  • テストは、クローズドループおよびオープンループの経路の両方で行われた。
  • 精度: LMKFは、ロボットおよびランドマークの配置の両方において、EKF、UnFS、およびICKFと比較して大幅に低い平方根平均二乗誤差（RMSE）を示した。
  • 堅牢性: ロボットの速度（$v$）およびセンサノイズ（$\sigma_G, \sigma_\theta$）が増加した場合でも、LMKFの性能は安定していた。対照的に、EKFおよびICKFのRMSEは、ノイズと速度の増加に伴って急速に増大した。
  • 経路依存性: 他の手法は、ループ閉鎖による補正（ループクロージャ）によって誤差が大幅に減少したが、LMKFはオープンループ経路においても高い精度を維持しており、その性能がループの存在に依存しにくいことを示している。
  • 安定性: RMSEの標準偏差（$\sigma_{RMSE}$）はLMKFが最も低く、これは異なるノイズシーケンスに対して高い信頼性があることを示している。

• 実世界データ（Victoria Park データセット）:

  • 2DレーザースキャナとGPSを用いたデータセットを使用し、GPSと経路情報から欠落しているロボットの角度データを推定した。
  • 提案手法にとって不利な要素であるロボット角度の不正確な推定にもかかわらず、LMKFは、EKF（2.58 m）、UnFS（2.1 m）、およびICKF（2.33 m）と比較して、最も低いRMSE（1.2 m）および最大絶対誤差（3.46 m）を達成した。
  • また、LMKFは他の手法と比較して、実行時間も最短（45.43秒）であった。

意義と主張
本論文は、提案されたLMKF SLAM手法が、最先端のフィルタリング手法に対して重要な改善を表すと主張している。単純な変換を通じて線形モデルを実現することで、この手法は収束を保証し、優れた精度と安定性を提供する。著者らは、LMKFがセンサの不確実性やシステムパラメータの変化に対して特に堅牢であり、競合する手法が理論的に有利な条件下（例：非線形モデルに有利になるようシミュレーションパラメータを調整した場合）であっても、EKF、UnFS、およびICKFを凌駕することを強調している。本研究は、このアプローチが、複雑な計算オーバーヘッドを必要とせずに、EKFベースのSLAMに共通する発散の問題を効果的に解決することを結論付けている。