技術要約: ContactIPM

問題提起

接触包含型軌道最適化（Contact-implicit trajectory optimization; CITO）は、接触シーケンスを事前に規定することなく、ロボットの状態、入力、接触力、および接触モードを同時に最適化しようとするものである。この柔軟性は、**数学的補完制約付きプログラム（Mathematical Programs with Complementarity Constraints; MPCC）**を導入する。ここでは、片側接触や摩擦が補完条件（例：$0 \le a \perp b \ge 0$）を通じてモデル化される。

これらの問題は、根本的な数値計算上の課題を提示している。実行可能MPCC点において、活性な非負制約と積制約の勾配は構造的に依存しており、標準的な制約充足条件（LICQなど）が失敗する原因となる。この退化により、従来の主変数–双対変数（primal–dual）非線形計画法ソルバーは信頼性が低くなり、しばしば特異または悪条件のKKT系を生成する。

既存のアプローチは、この堅牢性の問題に対処するために、完全な結合型主変数–双双対ニュートンステップを放棄している。CRISPは、凸部分問題を通じて主変数軌道のみを最適化する一方、IMPACTはブロック座標降下法を用いた拡張ラグランジュの外側ループを使用する。対照的に、構造活用型最適制御（OCP）ソルバー（例：acados）は、効率性のためにリッカート再帰を利用するが、特別な処理なしに厳密な補完性を課すと、有用な接触を生成できないことが多い。MPCC特有の堅牢性と、構造活用型OCPソルバーの効率性の間には、依然としてギャップが存在する。

手法: ContactIPM

ContactIPMは、リッカート再帰に必要なブロック帯行列構造を維持しつつ、補完制約を堅牢に扱う、統一された主変数–双対変数内点法を提案する。

1. バリア結合型弾性内部緩和

ペナルティ重みや拡張ラグランジアンを使用する代わりに、ContactIPMは、マークされたすべての補完ペアに対して**ステージ局所的な弾性内部緩和（stage-local elastic interior relaxation）**を導入する。

• スラック変数: 正の側のスラック（$s_a, s_b$）と、積のスラック（$s_c$）を導入する。
• 弾性的リフト: 補完条件は、以下の等式系へと変換される：
  $$g_a + s_a = 0, \quad g_b + s_b = 0, \quad g_a g_b - \theta(\mu) + s_c = 0$$
  ここで、$\theta(\mu) = \gamma_{mpcc}\mu$ は、補完性の厳密化をバリアパラメータ $\mu$ に直接結びつける。
• 正則性: この構成により、リフトされた制約ヤコビアンはスラックに関する単位ブロックを含むことが保証される。その結果、緩和された問題はあらゆる内部点において線形独立制約充足条件（LICQ）を満たし、一意なマルチプライヤーと正則な主変数–双対変数系を保証する。
• 独立したペナルティの排除: ペナルティ法とは異なり、補完性の厳密化は、中心パスを定義するバリアパラメータ $\mu$ と同じものによって制御されるため、独立して調整が必要なペナルティ係数の必要性を排除している。

2. ステージごとの主変数–双対変数消去とリッカート再帰

ソルバーは**主変数–双対変数の一貫性（primal–dual consistency）**を維持し、軌道、動力学マルチプライヤー、不等式マルチプライヤー、およびスラックを、単一の摂動KKT系の構成要素として更新する。

• 局所的消去: 補完ペアとその弾性変数は個々のステージに局所的であるため、ソルバーは局所的なスラック（$\Delta s_k$）および双対変数（$\Delta \lambda_k$）をステージごとに消去する。
• 縮小系: この消去は、局所的なステージヘッシアン（$\tilde{H}_k$）と勾配（$\tilde{q}_k$）のみを修正する。動力学は隣接するステージ間の唯一の結合であり、ブロック帯行列構造を保持する。
• 効率的な解法: 縮小された系は、**後退リッカート再帰（backward Riccati recursion）**に続いて前進代入を用いて解かれ、ホライゾン長 $N$ に対して線形計算量 $O(N)$ を達成する。

3. マルチフェーズMPCCリカバリ

異なる接触モードに関連する複数の定常枝を扱うために、ContactIPMは以下の固定リカバリ・スケジュールを採用する：

• 継続（Continuation）: 現在の接触モードの盆地（basin）に留まるよう努めながら、バリアパラメータを段階的に厳密化する。
• 再起動（Restart）: 継続が失敗した場合（例：条件数の悪化による）、ソルバーは元の主変数推測値を復元し、異なる盆地を探索するために、より大きなバリアパラメータとガウス–ニュートン曲率を用いて再起動する。
• 終了（Termination）: ソルバーは、緩和されていない物理的補完残差（$r_{phys} = \max |a_k b_k|$）が指定された許容誤差を下回ったときにのみ終了し、最終的な軌道が緩和された定式化だけでなく、物理的な接触モデルを満たすことを保証する。

4. スケーリングとグローバリゼーション

本手法は、ステージの微分値の大きさから導出された、ホライゾン間で一貫した対角座標スケーリングを適用する。グローバリゼーションは、目的関数の減少と制約違反のバランスをとるフィルタ・ラインサーチを通じて達成され、正のスラックを維持するためのfraction-to-boundaryルールが適用される。

主な貢献

1. 統一された定式化: マークされた補完ペアを、非線形動力学、コスト、および不等式の枠組み内で、バリア結合型の弾性的積行へと変換するステージごとの定式化。
2. MPCCを考慮した主変数–双対変数ニュートン法: 結合された摂動KKT構造を保持しながら、局所的なスラック/双対変数の消去とリッカート再帰を利用する手法。これには、軌道スケールの前処理とマルチフェーズ・リカバリ・ポリシーが含まれる。
3. 再現可能な評価: ContactIPMを、CRISP、IMPACT、および（厳密な補完性を伴う）OCP構造を持つacadosのベースラインと比較する包括的な実験スイート。これには、様々な摂動下での50回のクローズドループ・ロールアウトが含まれる。

実験結果

CRISPに対する性能

• 速度: 4つの固定ベンチマークケース（Cartpole, Push Box, Transport, Push T）において、ContactIPMは20回のペア・タイミング反復にわたってCRISPよりも2.17倍から8.87倍高速である。
• 堅牢性: ContactIMBはCartpoleとTransportにおいてCRISPと同等の性能を示すが、Push Boxのケースでは追加で5つのケースに成功した（25ケース中25に対し19/25）。Push Tスイートにおいて、CRISPが厳密な物理的補完性を強制した際に50ケース中27ケースしか成功しなかったのに対し、ContactIPMは50ケースすべてに成功した。

IMPACTに対する性能

• 速度: ContactIPMはPush Tにおいて2.96倍高速であり、Cart Transportにおいては4.91倍高速である。しかし、Push BoxにおいてはIMPACTの方が4.46倍高速である。
• 堅牢性: 3つの50ケース・スイート（Push Box, Push T, Cart Transport）全体で、IMPACTが150ケース中147ケースであったのに対し、ContactIPMは150/150ケースすべてで成功した。

OCP構造化ベースライン（acados）との比較

• acadosを厳密な補完性（弾性的緩和なし）を用いて構成した場合、有用な接触を生成できないことが多い。例えば、Push Boxベンチマークにおいて、acadosは25ケースすべてで物理的実現可能性チェックを通過したが、0/25が終端目標に到達し、多くの場合、ゼロ接触の軌道で終了した。
• 165の接触豊富なケース（CRISP Transport, CRISP Push T, IMPACT Push T, および IMPACT Cart Transport）において、acadosはわずか1つの受理された軌道しか生成できなかったが、ContactIPMは158に成功した。これは、OCP構造を活用するだけではMPCCの退化を克服するには不十分であることを示している。

クローズドループ検証

• モデルの不一致、ノイズ、および状態のリセットを含む50回の閉ループPush Boxロールアウトにおいて、ContactIPMは50ケースすべてで成功した。
• レイテンシ: 中央値の解法時間は2.08 msであり、99.37%の解が100 msの制御デッドラインを満たした。
• 実現可能性: 閉ループフィードバックは主変数の実現可能性と物理的補完性を維持し、最大物理的補完積は$1.01 \times 10^{-6}$未満であった。

意義と主張

本論文は、MPCCの堅牢性、主変数–双対変数の一貫性、および最適制御構造が、単一のソルバーの中で共存できることを主張している。ContactIPMは、補完性の処理を主変数–双対変数ニュートンステップから分離すること（CRISPやIMPACTに見られる手法）が、接触補完性を扱う上での避けられないコストではないことを示している。弾性的内部緩和とステージごとの消去およびリッカート再帰を組み合わせることで、ソルバーは接触問題に必要な数値的堅牢性と、構造活用型最適制御の計算効率の両方を達成している。これらの結果は、高度なOCP線形代数技術を使用する場合でも、専門的なMPCC処理が必要であることを示唆している。