このブログは全2回シリーズの第1回であり、ROS 2（Robot Operating System 2）に対応した自律移動ロボット（AMR）におけるセンサーフュージョンの活用について紹介します。 本編では、センサーフュージョンについて解説し、EKF（拡張カルマンフィルタ）と呼ばれるセンサーフュージョンアルゴリズムについて簡単に触れた後、シミュレーションおよび当社のAMRで行った実験の一部について順を追って説明します。
我々が解決しようとしている問題は、動的システムの状態を正確に推定することです。動的システムとは、時間の経過とともに状態が変化するシステムと定義できます。この状態の変化は、システムの自然な変化によるものもあれば、何らかの外部入力によって引き起こされる場合もあります。動的システムは、そのシステムが時間とともにどのように変化していくかを表す一連の微分方程式を用いて状態を定義することで、数学的に表現されます。
これを、当システムの「運動・プロセスモデル」と呼びます。システムへの入力（必須ではありません）と観測値が与えられれば、次の時間ステップにおけるシステムの状態を推定することが可能です。これを状態推定と呼びます。このようなシステムの例としては、倉庫や実験室内を移動する自律移動ロボット（AMR）が挙げられます。
ここで、AMRの状態とは、ある固定された原点に対する位置、速度、加速度を指します。これらの状態変数は、直接測定することも、センサーの測定値から数学的に導き出すこともできます。しかし、個々のセンサーにはそれぞれ固有の誤差があることを念頭に置いておくことが重要です。 例えば、ホイールエンコーダは車輪の回転数を計測するのに役立ちます。この情報とAMRの動力学（運動モデル）を組み合わせることで、AMRの新しい位置を予測することができます。
しかし、車輪がスリップすると、車輪エンコーダは実際の回転数よりも多い回転数を報告することになり、その結果、AMRの状態予測が誤ったものとなってしまいます。こうした問題は、センサーノイズや物理的環境の影響により、さらに深刻化します。 単一センサーの限界を補うため、複数のセンサーからの測定値を組み合わせることで、動的システムの状態推定精度を向上させることができます。動的システムの状態推定における不確実性を低減したり、精度を向上させたりするために複数のセンサーデータを組み合わせるプロセスは、「センサーフュージョン」と呼ばれます。
拡張カルマンフィルタ（EKF）は、複数のセンサーからのデータを融合させるための一般的な手法である。これは、線形動的システムの状態推定に使用される、よく知られたカルマンフィルタ（線形）アルゴリズムの拡張版である。カルマンフィルタの基本的な仮定として、状態の各変数はガウス分布で定義され、センサーノイズとプロセスノイズは平均ゼロの独立したガウス分布に従うことが挙げられる。 図1は、カルマンフィルタにおける予測・更新サイクルを用いてセンサーフュージョンを実現する仕組みのブロック図を示しています。その仕組みを理解するために、手順を追って見ていきましょう。
AMRのような非線形システムを扱う場合、カルマンフィルタアルゴリズムにいくつかの変更を加える必要があります。予測および更新の各ステップの後も状態表現がガウス分布のままであるようにするためには、運動モデルと計測モデルを線形化する必要があります。これは、テイラー展開を用いて非線形モデルを線形モデルとして近似することで行われます。このように拡張されたカルマンフィルタは、拡張カルマンフィルタと呼ばれます。
ROS を使用してセンサーフュージョンを行うための最も人気のあるパッケージの一つが、robot_localization パッケージです。robot_localization パッケージは、拡張カルマンフィルタ（EKF）およびアンセンテッド・カルマンフィルタ（UKF）と呼ばれるカルマンフィルタの別種を実装した、汎用的な状態推定パッケージです。多くのセンサーを融合させる柔軟性を持ち、15 次元のベクトル状態推定値を出力します。 状態推定値は、x、y、z方向の位置、姿勢、直線速度、角速度、直線加速度で構成されています。このパッケージが提供する運動モデルは全方向型です。関連データが与えられれば、グローバル状態推定とローカル状態推定の両方に使用できます。パッケージの詳細については、http://docs.ros.org/en/noetic/api/robot_localization/html/index.htmlをご覧ください。
Gazeboは、ROS 2に対応したシミュレーション環境であり、ロボットシステムのテストにおいて高い柔軟性を提供します。この環境では、センサーノイズを加えたり、さまざまな摩擦係数を試したり、車輪エンコーダの精度を下げたりすることが可能であり、これにより、実世界においてAMRの状態推定に影響を与えるような現実的なシナリオを再現することができます。
センサーフュージョンの性能を定量的に評価するために、ループ閉鎖テストを実施することができます。これは、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方向、y方向、および姿勢の誤差はそれぞれ-0.35m、-0.033m、-5.9度でした。原点に対するユークリッド距離の全体的な改善率は約78.9%であり、EKFを実装した後は1.699mから0.35mへと短縮されました。
状態推定は、ナビゲーションやマッピングなど、当AMRの他のモジュールに信頼性の高いオドメトリ情報を提供するための重要なステップです。実験を通じて、センサーフュージョン技術を用いることで、シミュレーション環境および実環境の双方において、当AMRの状態をより正確に推定できることを実証しました。 我々は、非線形システムにおけるセンサーフュージョンで広く用いられている手法である拡張カルマンフィルタを採用した。本シリーズの第2部では、センサーフュージョンがどのようにしてAMRのナビゲーションおよびマッピング機能を向上させるかについて論じる。
当社の自動車ソリューションの専門家による30分間の相談を予約する
当社のバッテリー管理ソリューションの専門家による30分間の相談を予約する
当社の産業・エネルギーソリューションの専門家による30分間の相談会をご予約ください
当社の専門家による30分間の相談を予約する
