このブログは2部構成のシリーズの第1部であり、ROS 2（Robot Operating System 2）で動作する自律移動ロボット（AMR）におけるセンサーフュージョンの活用方法について紹介します。 本編では、センサーフュージョンの概要を説明し、拡張カルマンフィルタ（EKF）と呼ばれるセンサーフュージョンアルゴリズムを簡潔に解説した後、シミュレーション環境および実機AMRで実施した実験事例をいくつか紹介します。
私たちが解決したい問題は、動的システムの状態を正確に推定することです。動的システムとは、時間の経過とともに状態が変化するシステムと定義できます。状態変化はシステムの自然な進化によって生じることもあれば、外部からの入力によって引き起こされることもあります。動的システムは、与えられたシステムが時間を通じてどのように伝播するかを表す微分方程式の集合によってその状態を定義することで、数学的に表現されます。
これを当システムの動作／プロセスモデルと呼びます。システムへの入力（必須ではない）と観測された測定値が与えられれば、次の時間ステップにおけるシステム状態を推定することが可能です。これを状態推定と呼びます。この種のシステムの例としては、倉庫や研究室内を移動する自律移動ロボット（AMR）が挙げられます。
ここで、AMRの状態とは、ある固定された原点に対する位置、速度、加速度を指す。これらの状態変数は、直接測定するか、センサーの読み取り値から数学的に導出することができる。ただし、個々のセンサーには固有の欠陥があることを念頭に置くことが重要である。 例えば、ホイールエンコーダは車輪の回転数を計測するのに役立ちます。この情報とAMRの運動特性（運動モデル）を組み合わせることで、AMRの新たな位置を予測することが可能です。
しかし、車輪がスリップすると、車輪エンコーダが余分な車輪回転数を報告し、その結果、自律移動ロボット（AMR）の状態予測が誤ったものとなります。このような問題は、センサーノイズや物理的環境によってさらに悪化します。 単一センサーの限界を補うため、複数のセンサーからの測定値を組み合わせることで、動的システムの状態推定精度を向上させることが可能です。動的システムの状態推定における不確実性の低減や精度向上のために複数のセンサーデータを統合するプロセスは、センサーフュージョンと呼ばれます。
拡張カルマンフィルタ（EKF）は、複数のセンサーからのデータを融合する一般的な手法である。これは、線形動的システムの状態推定に用いられるよく知られたカルマンフィルタ（線形）アルゴリズムの拡張版である。カルマンフィルタの基本的な仮定は、状態変数がガウス分布で定義され、センサーノイズとプロセスノイズが平均ゼロの独立したガウス分布に従うことである。 図1は、カルマンフィルタにおける予測と更新のサイクルを用いてセンサー融合を実現するブロック図を示している。その動作をステップごとに確認していこう：
AMRのような非線形システムを扱う場合、カルマンフィルタアルゴリズムにいくつかの変更を加える必要がある。各予測ステップおよび更新ステップの後でも状態表現がガウス分布を維持するよう保証するため、運動モデルと計測モデルを線形化する必要がある。これはテイラー展開を用いて非線形モデルを線形モデルとして近似することで実現される。このカルマンフィルタの拡張形は拡張カルマンフィルタと呼ばれる。
ROSを用いたセンサー融合を実行する最も人気のあるROSパッケージの一つがrobot_localizationパッケージである。robot_localizationパッケージは汎用状態推定パッケージであり、拡張カルマンフィルタ（Extended Kalman Filter）およびアンサンテッドカルマンフィルタ（Unscented Kalman Filter）と呼ばれる別のカルマンフィルタのバリエーションを実装している。多くのセンサーと出力を融合する柔軟性を備え、15ベクトルの状態推定値を出力する。 状態推定値は、位置、姿勢、直線速度、角速度、およびx, y, z方向の直線加速度で構成されます。本パッケージが提供する運動モデルは全方向対応です。関連データがあれば、グローバル状態推定とローカル状態推定の両方に使用できます。パッケージの詳細は以下を参照してください：http://docs.ros.org/en/noetic/api/robot_localization/html/index.html
ガゼボはROS 2対応のシミュレーション環境であり、ロボットシステムのテストに高い柔軟性を提供します。センサーノイズの導入、様々な摩擦係数の試行、車輪エンコーダの精度低下といった操作が可能であり、これにより実環境における自律移動ロボット（AMR）の状態推定に影響を与える現実的なシナリオを再現できます。
センサー融合の性能を定量化するため、ループ閉鎖テストを実行できます。これは、自律移動ロボット（AMR）を初期位置と最終位置が同一となるようにランダムに閉じた軌跡上を走行させるテストです。 その後、IMUと車輪エンコーダを融合した後のEKFによる状態推定をプロットし、生の車輪エンコーダからの状態推定と比較します。理想的には、AMRの推定状態がx、y、ヨーをそれぞれ(0, 0, 0)と報告することを目指します。
Turtlebot3（TB3）パッケージを用いて様々な実験を実施した。その一例を図2に示す。ここでは、TB3を遠隔操作でTurtlebot3ワールド内を複数回移動させ、最終的に開始位置へ戻す。 図からわかるように、緑色で示したEKFによる予測軌道は、自律移動ロボット（AMR）が初期位置に戻ったことをより正確に推定できる。一方、赤色で示した車輪エンコーダのみを用いた推定では、約1.4メートルの誤差が生じている。 生の車輪エンコーダデータによる最終位置誤差は、x方向0.405m、y方向-1.36m、ヨー角29.72度であった。一方、EKFによる推定位置誤差はx方向0.103m、y方向-0.0165m、ヨー角0.0187度である。 初期位置に対するユークリッド距離の全体的な改善率は約92.64%であり、ホイールエンコーダデータのみ使用時の1.42mから、EKF使用後は0.10mとなった。
Gazeboで実施した実験は実際の自律移動ロボット（AMR）にも拡張可能です。当社オフィス構内でこれらの実験を実施し、AMRのIMUデータと車輪エンコーダデータを融合させることで信頼性の高い結果を得ることができました。 実行例を図3に示す。車輪エンコーダデータのみを使用した場合、推定状態はx方向で1.58m、y方向で-0.69mの誤差、およびヨー方位で55度の誤差を報告した。 一方、EKF推定値はより精度が高く、x方向-0.35m、y方向-0.033m、姿勢-5.9度の誤差を示した。EKF実装後の原点からのユークリッド距離の総合改善率は約78.9%に達し、1.699mから0.35mへと縮小した。
状態推定は、自律移動ロボット（AMR）のナビゲーションやマッピングといった他のモジュールに信頼性の高いオドメトリ情報を提供する上で重要なステップです。実験を通じて、センサーフュージョン技術を用いることで、シミュレーション環境と実世界の両方でAMRの状態をより正確に推定できることを実証しました。 非線形システムにおけるセンサー融合で広く用いられる手法である拡張カルマンフィルタを採用した。本シリーズ第2部では、センサー融合がAMRのナビゲーションおよびマッピング能力をいかに強化できるかについて論じる。
当社の自動車ソリューション専門家による30分間の相談を予約する
バッテリー管理ソリューションの専門家による30分間の相談を予約する
当社の産業・エネルギーソリューション専門家による30分間の相談を予約する
専門家との30分間の相談を予約する
