Can we stabilize an inverted pendulum with feedback from a time-of-flight camera?

技術要約：Time-of-Flightカメラのフィードバックによる倒立振子の安定化

問題提起
Time-of-Flight（ToF）カメラは、そのコンパクトさ、低コスト、および照明条件に対する堅牢性からロボット工学で広く採用されているが、一般的には、空間解像度の低さ、深度ノイズ、および系統的な誤差（マルチパス干渉やモーションブラーなど）のため、精密かつ高帯域なフィードバック制御には不向きであると考えられている。本研究が取り組む中心的な問いは、「低コスト・低解像度のToFセンサが、高速で不安定な動的システムを安定化させるのに十分な品質の計測を提供できるか」という点である。著者らは、テストベッドとして倒立振子（カート・ポール）を選択した。この典型的なベンチマークは、非最小位相特性、微弱な減衰、およびミリ秒単位のレイテンシやジッターに対する敏感さゆえに、制御が極めて困難であることで知られている。課題は、特定の実験幾何学的配置によってさらに深刻化している。すなわち、ToFカメラが振子の真下に設置され、上方を見上げているという構成である。この「不利な」視点は、深度バイアスやマルチパス伝播を悪化させる大きな入射角を誘発し、センシング・パイプラインへの意図的なストレス・テストとなっている。

手法
著者らは、Maxon製BLDCモータによってラック・アンド・ピニオン機構を介して駆動される3Dプリント製のカートと、その上にバランスを取る振子（金のペン）からなる、コンパクトで再現可能な実験プラットフォームを構築した。制御およびセンシング・スタックはRaspberry Pi 5上でROS2を用いて動作しており、これは非自明な処理ジッターを導入するものであり、制御ループに対する現実的なストレス・テストとして機能する。

• センシング・パイプライン: システムは、角度フィードバックとして低価格なArduCam ToFモジュール（解像度 240 × 180、30 fps）のみに依存している。カートの線形位置および速度は、モータのホール効果センサを介して計測される。
• ビジョン処理: 生の深度画像は、範囲外ピクセルの除去、平滑化、および二値閾値処理によって前処理され、振子を孤立させる。システムは、ユークリッド距離変換とサブピクセル精細化を用いて、振子の輪郭内に2つの円（ベース用とトップ用）を適合させる。角度は、これら2つの円の中心を結ぶセグメントの方向から導出される。
• 状態推定および制御: 著者らは標準的な線形二次ガウス（LQG）レギュレータを採用している。カルマンフィルタが、ノイズを含むToF由来の角度計測値とモータエンコーダのデータを融合し、完全な状態ベクトル（カートの位置、カートの速度、振子の角度、および振子の角速度）を推定する。フィルタのノイズ共分散は、グラウンドトゥルースのエンコーダデータに対する二乗平均平方根誤差（RMSE）を最小化するように、ベイズ最適化を通じてチューニングされる。
• システム同定: 主要な動的パラメータ（質量、慣性モーメント、摩擦係数）は、収集された軌跡とRunge–Kutta 4法を用いて統合された非線形モデルとの比較による、バッチ最小二乗法を用いて同定された。

主な結果
システムは、高価なエンコーダや外部モーションキャプチャ・システムを使用することなく、角度フィードバックにのみToFカメラを使用して、倒立振子の信頼性と精密な安定化を成功裏に達成した。

• 安定化性能: 10秒間の試行を30回行った結果、コントローラは振子を直立位置に維持した。制御された振子の角度と直立平衡状態との間の時間平均RMSEは $1.0 \times 10^{-3}$ rad·s$^{-1}$ であった。
• センサ精度: ToFカメラから導出された生の角度推定値は、制御された軌道におけるグラウンドトゥルースのエンコーダと比較して、$3.0 \times 10^{-3}$ rad のRMSEを示した。角度推定の静止ノイズは、標準偏差が $1.7 \times 10^{-3}$ rad のゼロ平均ガウス分布として特徴付けられた。
• 堅牢性: 不利なカメラ配置（マルチパスやモーションブラーを誘発）や、ROS2/Raspberry Piスタックに固有の処理ジッターにもかかわらず、LQGコントローラは安定性を維持し、ToFセンサが閉ループ制御のための十分な信号忠実度を提供していることを示した。

意義および主張
本論文は、安価で低解像度のToFセンサが、高速で不安定なシステムの精密なフィードバック制御をサポートできることを示す「存在証明」の研究を提示している。著者らは、自らの結果を、ToFベースのフィードバックの潜在能力に関する保守的な下限値として明確に位置づけている。困難な幾何学的配置と標準的な非最適化コントローラ（LQG）を選択することで、劣悪な条件下であってもToFセンサが実行可能であることを示している。

その意義は、ToFカメラは低〜中程度の制御要求（例：障害物回避や粗いサーボ制御）に限定されるという一般的な仮説に異を唱えることにある。著者らは、思慮深いシステム設計を行えば、ToFセンサが特定のアプリケーションにおいて高価なモーションキャプチャ・インフラや高性能エンコーダに取って代わり得ることを主張している。彼らは、さらなるベンチマークと研究を促進するためにオープンソースのプラットフォームとコードを提供しており、今後の研究では、性能の限界をさらに押し広げるために、高度な制御戦略（モデル予測制御やロバスト制御など）や代替のセンサ構成を探索できることを示唆している。