Distributed 3D Leader-Follower Formation Control with Field-of-View Safety via Control Barrier Functions

技術的概要：制御バリア関数を用いた視野安全を備えた分散型 3D リーダー・フォロワー編成制御

問題定義
本論文は、特に相対状態推定が搭載カメラに依存する GPS 拒否環境において、マルチ UAV システムのための安全で視覚ベースの 3D リーダー・フォロワー編成（3D-LFF）を維持する課題に取り組んでいる。このようなシステムにおける重要な制約は、フォロワーのカメラの視野（FOV）と深度範囲である。編成追跡コマンドがリーダーをフォロワーのカメラの視錐台（frustum）の外へ駆動した場合、視認性が失われ、相対状態推定の失敗および潜在的なミッションの失敗につながる。既存の位置ベースのアプローチは、しばしばこれらの 3D カメラ視錐台制約を厳密にモデル化することに失敗しており、また、平面視認性手法を 3D UAV 編成へ拡張することは、結合した 3D 幾何学と相対的な進行方向の影響により容易ではない。核心的な問題は、正確な編成追跡を達成しつつ、常にリーダーがフォロワーのカメラ視錐台内で厳密に視認可能であることを保証する分散型制御フレームワークを設計することである。

手法
提案される解決策は、名义編成コントローラと制御バリア関数（CBF）に基づく安全フィルタを統合する、階層的で知覚を考慮した制御アーキテクチャである。

1. システムモデリング: 著者らは、相対進行方向変数（$\alpha_{ij}$）を付加した球座標（$L_{ij}, \phi_{ij}, \xi_{ij}$）を用いて、リーダー・フォロワー対の相対運動学モデルを導出した。このモデルは、フォロワーのボディ固定カメラとリーダーとの間の幾何学を、フォロワーのボディ座標系で表現し、捉える。
2. 名义コントローラ: 局所的に利用可能な相対状態と通信されたリーダーの状態のみを使用して、分散型 3D-LFF 追跡コントローラが設計される。このコントローラは、編成誤差を指数関数的にゼロへ駆動することを意図した名义速度コマンド（$u_i^{nom}$）を生成する。
3. 安全フィルタ（CBF-QP）: 視認性を強制するために、名义コマンドは制御バリア関数に基づく二次計画（CBF-QP）を介して処理される。
   • 安全集合の定義: 知覚安全集合は、カメラ視錐台制約（水平/垂直 FOV 角度および深度制限 $[x_{near}, x_{far}]$）によって定義される。これらの制約は、直交座標系において連続微分可能な関数（$h_c$）として符号化される。
   • 最適化: 各制御時間ステップにおいて、QP は CBF 制約（$\dot{h}_c + \kappa h_c \geq 0$）をsubjectに、安全コマンド（$u_i^{safe}$）と名义コマンド（$u_i^{nom}$）との間の偏差を最小化する。これにより安全集合の前方不変性が保証され、つまりリーダーが視錐台内で開始すれば、そこに留まり続けることを意味する。
   • ロバスト性: このフレームワークには、制御入力限界と未モデル化の擾乱を処理するためのスラック変数が含まれており、過激な機動下でも実行可能性を確保する。

主要な貢献
本論文は、3 つの主要な貢献を概説している。

1. 相対運動学モデル: 相対進行方向を組み込んだ球座標系における 3D 運動学モデルであり、局所的な相対測定に基づく分散制御則を可能にする。
2. リアルタイム安全フィルタ: 名义編成コマンドを最小限に変更することで 3D カメラ視錐台制約（FOV および深度）を強制する CBF-QP 安全フィルタ。これは、実行可能である限り追跡性能を維持しつつ、連続的な視認性を保証する。
3. 実験的検証: Gazebo シミュレーションおよび Crazyflie 2.1 ハードウェア実験を用いた包括的な検証。結果は、望ましい編成が視認性限界と矛盾する場合や、リーダーが急激な機動を行う場合であっても、フレームワークが望ましい編成を追跡し、FOV 制約を厳格に強制する能力を実証している。

結果
提案された手法は、2 つのシナリオ（3 段階編成タスクおよび急激なリーダー運動タスク）において、安全フィルタなしのベースラインコントローラ（NoCBF）と比較評価された。

• 3 段階タスク: 第 2 段階では、望ましい編成の拡大がリーダーをカメラ視錐台境界へ押しやったが、NoCBF ベースラインは安全制約に違反し、リーダーが FOV から外れる原因となった。対照的に、CBF 搭載フォロワーは、リーダーを安全集合内に保持するために速度を動的に調整し、編成目標と知覚安全の間の対立を成功裡に解決した。
• 急激な運動: 急激なリーダーの減速を含むシナリオにおいて、NoCBF ベースラインはフォロワーのオーバーシュートとリーダーの喪失を防げなかった。提案フレームワークはオーバーシュートを成功裡に軽減し、視認性を維持した。
• ハードウェア実験: Crazyflie ドローンを用いた物理飛行テストは、シミュレーション結果を確認した。CBF コントローラは（ヨー変調レムニスケート軌道を含む）複雑な機動中にリーダーを視錐台内に維持したが、ベースラインは編成拡大フェーズ中に安全制約を維持することに失敗した。

意義と主張
本論文は、3D 空間における知覚を考慮した編成制御に対する厳密な解決策を提供すると主張している。その意義は以下の点にある。

• 保証された視認性: 3D 幾何学を完全にモデル化していないヒューリスティクスに依存する以前の手法とは異なり、このアプローチは CBF を用いて、リーダーがカメラ視錐台内に留まることを数学的に保証する（前方不変性）。
• 最小限の介入: CBF-QP 定式化は、安全フィルタが制御入力を必要に応じてのみ変更することを保証し、可能な限り名义編成追跡性能を維持する。
• 実用的適用性: シミュレーションおよび物理ハードウェア（Crazyflie）の両方での成功した検証は、このアプローチがリソース制約のある UAV でのリアルタイム実装に対して計算的に実行可能であることを示している。

著者らは、現在の実験が理想的な相対測定を模倣するためにモーションキャプチャを利用しているが、将来の研究は生画像ベースの測定を使用してループを閉じるために onboard 知覚を統合することを目指していると指摘している。本論文は、すべてのマルチエージェント協調問題を解決すると主張するものではなく、特に 3D リーダー・フォロワー構成における編成追跡と知覚安全の重要な交差点を標的としている。