技術要約: ElastiQP: 制約付きロボット制御のための常時実行可能（Always-Feasible）なQPソルバー

問題提起

二次計画法（QP）は、全身制御、安全性フィルタ、歩行制御など、モデルベースのロボット制御における基礎となる技術です。ロボットの能力向上や環境の複雑化に伴い、これらのQPにおける制約の数が増加しており、その結果、制約が瞬間的に矛盾して「実行不可能（infeasible）」となる状況が発生しやすくなっています。標準的なQPソルバーは、このようなシナリオにおいて「実行不可能」というステータスを返しますが、これはロボットコントローラに実行可能なコマンドがなくなることを意味し、極めて危険な状態を招きます。

不実行可能性（infeasibility）に対処する既存のアプローチには、以下のような重大なトレードオフが存在します：

• ハード・ソルバー（Hard Solvers）: 不実行可能性がある場合に解を返さない。
• $\ell_2$ 緩和（ソフト制約）: すべての制約に対して違反を分散させるため、競合していない制約まで影響を受け、望ましくないトレードオフやスパース性の喪失を招く。
• 拡張 $\ell_1$ 緩和: すべての不等式制約にスラック変数を導入する。これにより実行可能性とスパース性は確保されるが、決定変数が大幅に増加する。ProxQP、PIQP、DAQPなどのソルバーでは、このスラック変数が因数分解のコストを支配することになり、高頻度（1 kHz）制御に必要な厳格な1 msのタイムバジェットを満たせなくなる場合がある。
• 既存のエラスティック・ソルバー: qpaxのような手法は、凝縮されたエラスティック・モードを備えているが、硬い等式制約（hard equality constraints）へのサポートや、高自由度（DoF）システムにおける十分な速度が欠けている。

手法

著者らは、マイクロ秒レベルの性能を維持しつつ、実行可能性を保証するように設計された、修正されたデュアル・アクティブセットQPソルバーである ElastiQP を提案しています。核心となる革新は、**エラスティック・スラック変数の解析的な凝縮（analytical condensation）**にあります。

1. エラスティックQPの定式化

ElastiQPは、不等式制約を厳密な $\ell_1$ ペナルティで緩和し、等式制約（通常は動力学）は硬い制約として残すエラスティックQPを解きます：
$$\begin{aligned} \min_{x,t} \quad & \frac{1}{2}x^\top Qx + q^\top x + w^\top t \\ \text{s.t.} \quad & Ax = b \\ & Gx - t \leq h \\ & t \geq 0 \end{aligned}$$
ここで、$t$ はスラック変数を示します。決定的な点は、$t$ に関する内部の最小化問題は解析的に（$t_i = \max(0, [Gx-h]_i)$）解くことができ、これにより問題を $x$ のみの厳密な $\ell_1$ ペナルティ定式化へと還元できることです。

2. デュアル・アクティブセット法におけるエラスティック凝縮

従来のソルバーは、スラック $t$ を決定変数として扱い、問題のサイズを拡大させます。ElastiQPは、$t$ を解析的に排除します。**デュアル・アクブル・セット法（ASM）**の文脈において、この凝縮は、デュアル制約の単純な修正へと問題を変換します：

• 有界な乗数（Bounded Multipliers）: 不等式乗数 $z$ は、標準的な非負の半直線 $[0, \infty)$ ではなく、ボックス制約 $[0, w]$ に制約されます。
• 3つの状態: アルゴリズムは、不等式制約に対して以下の3つの状態を管理します：
  1. 非アクティブ（Inactive）: $z_i = 0$ （充足）。
  2. アクティブ（Active）: $0 < z_i < w_i$ （タイト）。
  3. 飽和（Saturated）: $z_i = w_i$ （違反しており、スラック $t_i > 0$ を持つ）。
• 線形システム: この凝縮により、線形システムは決定変数の数 $n$ に依存する $n \times n$ のサイズとなります。これは、不等式の数 $p$ が $n$ よりもはるかに大きい場合でも、標準的なASMの計算効率を維持することを意味します。

3. アルゴリズムの調整

ソルバーは、標準的なデュアルASMの反復処理を以下のように修正します：

• ワーキングセットの解法には、飽和した制約（$M_S w_S$）を考慮する項が含まれます。
• 更新ロジックは「飽和」状態を処理し、制約が違反された場合にはワーキングセットに追加し、厳密に充足された場合には削除します。
• ロボットの動力学において等式制約は一貫しており、構成上も整合しているため、エラスティック・デュアルASMは、等式制約が一貫している限り、常に解を返すことが保証されます。

主な貢献

1. ElastiQP ソルバー: 解析的なスラック凝縮を備えたデュアル・アクティブセット法を実装した、ヘッダーオン形式のC++ライブラリ（Python/JAXインターフェースも提供）。
2. 常時実行可能（Always-Feasible）な保証: 不等式に $\ell_1$ 緩和を用い、等式に硬い制約を用いることで、決して「実行不可能」を返さず、ロボットが常に制御コマンドを得られるようにします。
3. パフォーマンス: スラック変数の解析的な排除により、実行可能な問題に対しては主要なソルバーと同等以上の速度を実現し、不実行な問題を最大で40倍高速に処理できます。
4. スパース性の維持: $\ell_2$ 緩和とは異なり、$\ell_1$ 構造により違反がスパースになり、競合している制約のみに影響を与えます。

結果

ロボット制御ベンチマーク

著者らは、6自由度アーム、12自由度両腕システム、28自由度ヒューマノイドの3つのロボット制御シナリオでElastiQPを評価しました。

• 実行可能なループ: ElastiQPはマイクロ秒レベルの解法時間を達成し（例：28自由度ヒューマノイドのコールドスタートで52 $\mu$s）、1 msの予算を余裕を持って満たしています。実行可能な問題においては、DAQP（最も高速なハード・ソルバー）と同等の性能を発揮します。
• 不実行なループ: 不実行な問題において、ハード・ソルバー（DAQP, PIQP, ProxQP）は失敗するか、不実行性を報告するまでに過大な時間を要します。ElastiQPは、マイクロ秒単位で最小限の違反解を返します。
  • 速度: ヒューマノイドの不実行問題において、ElastiQPは次点の代替案（ProxQPのスパース・バックエンド）よりも最大40倍高速です。
  • 品質: ElastiQPは競合する制約（テストケースの1行）のみを違反させますが、$\ell_2$ ソルバー（DAQP-soft, ProxQP-closest）は28〜29行にわたって違反を分散させます。
  • 等式の一貫性: ElastiQPは等式制約（動力学）を厳密な許容誤差（$10^{-8}$）内に維持しますが、$\ell_2$ ソフトモードは等式の充足度を著しく低下させることがあります。

Maros–Mészáros ベンチマーク

より広範な汎用目的の、条件数の悪い問題群を用いて評価を行いました：

• ElastiQPは、優れた汎用ソルバー（小規模問題ではDAQP、大規模なスパース問題ではPIQP）に対して競争力があります。
• エラスティック凝縮は、一般的な問題における性能を低下させません。ElastiQPは、他のソルバーが収束に失敗するインスタンスも解決しています。
• 凝縮されたエラスティック・モードを提供する唯一の他のソルバーであるqpaxを、速度と信頼性の両面で大きく上回っています。

意義と主張

本論文は、ElastiQPがロボット制御における重要なギャップを埋めるものであると主張しています。すなわち、「高頻度制御のために十分に高速」であり、かつ「避けられない制約の衝突に対しても、ロボットを無反応な状態に陥らせることなく、堅牢に対処できる」必要があります。

• 信頼性: 「常に実行可能」な解を提供することで、コントローラが出力を出せないという危険な状態を防ぎます。
• 効率性: スラック変数を解析的に凝縮することで、エラスティック定式化に伴う計算コストを回避し、高自由度システムへの適用を可能にします。
• 実用性: $\ell_1$ ペナルティ構造により、$\ell_2$ 手法の拡散的なトレードオフとは異なり、階層的な緩和（例：タスク完了よりも安全性を優先する）が可能になります。
• 展開: 著者らは、ElastiQPが既存の安全性フィルタやコントローラへ即座に統合可能であることを強調しており、オープンソースとして公開されています。

著者らは今後の展望について、現在はCPUベースのリアルタイム制御に焦重しているものの、将来的な方向性として、微分可能性、バッチ解法のためのGPU加速、およびグローバル逆運動学のSQPパイプラインにおける内部ソルバーとしての活用を挙げています。