このブログは全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 パッケージは、拡張カルマンフィルタ（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方向で-0.35m、y方向で-0.033m、姿勢角で-5.9度の誤差にとどまりました。原点に対するユークリッド距離の全体的な改善率は約78.9%であり、EKFを導入した結果、1.699mから0.35mへと短縮されました。
状態推定は、ナビゲーションやマッピングなど、当AMRの他のモジュールに信頼性の高いオドメトリ情報を提供するための重要なステップです。実験を通じて、センサーフュージョン技術を用いることで、シミュレーション環境および実環境の双方において、当AMRの状態をより正確に推定できることを実証しました。 我々は、非線形システムにおけるセンサーフュージョンで広く用いられている手法である拡張カルマンフィルタ（EKF）を採用した。本シリーズの第2部では、センサーフュージョンがAMRのナビゲーションおよびマッピング機能をどのように向上させることができるかについて論じる。
当社の自動車ソリューションの専門家による30分間の相談を予約する
当社のバッテリー管理ソリューションの専門家による30分間の相談を予約する
当社の産業・エネルギーソリューションの専門家による30分間の相談会をご予約ください
当社の専門家による30分間の相談を予約する
