Kanade's Lab.

結合モデル制御のコンセプト

おもしろそうな論文を見つけました。

https://arxiv.org/abs/2404.14724

Tightly Joined Positioning and Control Model for Unmanned Aerial Vehicles Based on Factor Graph Optimization b https://github.com/RoboticsPolyu/IPN_MPC

この論文は、ドローンの自己位置推定と制御を別々に解くのではなく、単一のグラフ最適化 (FGO: Factor Graph Optimization) 問題として同時に解く 結合モデル (JPCM: Joint Positioning and Control Model) を提案しています。狙いは GNSS の劣化や LiDAR 外乱・ノイズなどで位置推定が不確かなときでも、制御入力が過激に振れず、滑らかに軌道追従できるようにすることです。論文では、センサ測定を「位置推定ファクタ」、MPC由来の制約を「制御ファクタ」として同じ因子グラフに入れ、GTSAMで解いています。

概要

従来のドローン制御はだいたい次の流れです。

  1. センサフュージョンで位置を推定する
  2. 推定状態を制御器に渡す
  3. 制御入力を出す

この構成では、制御器は位置推定値をほぼ「正しい現在状態」として扱いがちです。しかし、都市部のGNSSマルチパスや動的物体の多いLiDAR環境では、位置推定の不確かさが大きくなります。論文は、従来の分離型パイプラインでは「位置推定の不確かさ」と「非線形ダイナミクス」を同じ問題の中で扱いにくい、と指摘しています。

ポイントは、位置推定と制御は本来つながっているということです。例えば、制御側が持つドローンの運動モデルは、次の状態がどのあたりに来るべきかという情報を持っています。これは位置推定にも役立つはずです。逆に、位置推定の不確かさは制御入力の決め方に影響すべきです。論文の問題意識は、「この情報の流れを片方向ではなく双方向にしよう」というものです。

結合モデル

JPCMでは、未知変数を「過去から現在までの状態」と「未来の予測状態・制御入力」としてまとめます。そして、以下を同じ最適化問題に入れます。

  • 絶対ファクタ：GNSS のようなある時刻の状態そのものを観測する因子
  • 相対ファクタ：IMU や Visual Odometry のように隣接時刻間の相対変化を測る因子
  • 動力学ファクタ：運動方程式に由来する制約
  • 参照軌道ファクタ：予測状態を目標軌道に近づける制約
  • 制御入力の制約：回転数の上下限など

論文では、JPCMの統一最適化問題を、位置推定由来の絶対・相対ファクタと、動力学・参照軌道・制御制限・制御変化率ファクタの和として定式化しています。特に重要なのは、動力学ファクタが位置推定側と制御側をつなぐ橋になっている点です。

MPCとの違い

通常の MPC は、現在状態 x0 を位置推定器から受け取り、それを初期条件として未来の制御入力を最適化します。JPCMはそこが違います。 JPCMでは、現在状態 x0 も「確定値」ではなく、センサ観測や過去状態、動力学、制御目的と一緒に最適化されます。つまり、位置推定の不確かさを抱えたまま、制御入力まで同時に決める構造です。 そのため、位置観測がノイジーでも、制御入力が測定ノイズに過剰反応しにくくなります。論文のシミュレーションでも、通常MPCでは制御入力が上下限に張り付きやすい一方、JPCMでは入力がより集中して滑らかになると報告されています。

実装例：ドローン

論文はクアッドロータを例に、状態を位置・姿勢・速度・角速度で表し、推力・トルク・重力・空力ドラッグを含むモデルを使っています。ロータ回転数から推力とトルクを計算し、それを動力学ファクタに変換します。また、空力ドラッグを無視すると高速飛行や円軌道で誤差が出るため、ドラッグ項を動力学ファクタに追加した JPCM-Drag も提案しています。速度成分の残差にドラッグ係数行列を入れる形です。

実験結果の読み方 シミュレーション

高い位置不確かさを入れた条件で、MPC、JPCM、スライディングウィンドウ版JPCM-SWを比較しています。表IIでは、位置RMSEが通常MPCでは 0.068, 0.066, 0.055 m、JPCMでは 0.031, 0.019, 0.015 m、JPCM-SWでは 0.017, 0.017, 0.019 m となっています。つまり、位置ノイズが大きい条件ではJPCM系がMPCより軌道追従誤差を小さくしています。

さらに、急な外乱を入れた場合、JPCMはMPCより復帰が遅いことがある一方で、より滑らかな誤差曲線になると説明されています。論文は、これはスムーズな制御を重視するJPCMの性質であり、位置外れ値やスプーフィングのような状況では利点になると述べています。

空力ドラッグ

ドラッグを入れると通常JPCMの追従誤差は増えます。たとえばドラッグ係数を 0.3I にした場合、JPCMの位置RMSEは 0.067, 0.061, 0.039 m まで悪化します。一方、ドラッグをモデル化したJPCM-Dragでは 0.031, 0.025, 0.028 m まで改善しています。

これはかなり自然な結果で、現実の空気抵抗を制御モデルに入れないと、特に高速・曲線軌道で内側にずれるという現象を、モデルにドラッグ項を足すことで補正していると読めます。

実機実験

実機では、モーションキャプチャの真値にガウスノイズを加えてGNSS風の不確かな位置観測を作り、従来モデルとJPCMを比較しています。論文は、JPCMの方が追従誤差が低く、従来モデルは位置推定値を強く信じるため制御がジッタしやすい、と説明しています。

計算時間については、JPCMは100 Hz制御を達成し、平均計算時間は5.4 ms、最適化問題の99.8%が10 ms未満だったと報告しています。論文は、JPCMの計算複雑性は従来の分離型モデルと同程度だと主張しています。

この論文の貢献

第一に、位置推定とMPC制御を同じFGO問題として統合したことです。これにより、センサ測定、動力学、軌道追従、入力制限を同じグラフ上で扱えます。

第二に、位置推定の不確かさが大きいときに、制御が過剰反応しにくくなることを示した点です。シミュレーションと実機実験の両方で、従来MPCより滑らかで低誤差な軌道追従を報告しています。

第三に、空力ドラッグ込みのJPCM-Drag を示し、ドラッグによる軌道ずれを減らせることを示しています。

注意点

まず、実機実験はかなり管理された環境です。論文の今後の課題でも、都市部のような実環境でより多くのセンサを統合して試す必要があると述べています。

次に、FGOで位置推定と制御を同時に解くため、実装の複雑さは通常の「推定器＋MPC」より上がります。論文は計算時間が実用的であることを示していますが、センサ数、状態窓長、障害物回避、複数UAVなどを入れたときのスケーラビリティは追加検証が必要です。

また、安全保証についてはまだ弱いです。著者自身も、今後はJPCMの安全性を定量的に監視する仕組みが必要だと述べています。

コメント

特に「自己位置推定」と「制御」を単一のモデルで扱うという点において、非常にシンプルに実装できる可能性を感じました。従来のよくある制御では自己位置推定モジュールと制御モジュールは分かれていました。しかし、ファクターグラフ最適化の世界ではこれらの区別は本質的には意味がありません。変数が固定されて確定している領域が過去で、変数を変えられる領域が未来です。

この論文のコンセプトを自動車の制御を例に、実際に動かしながらやってみたいと思います。