Technische Samenvatting: ContactIPM

Probleemstelling

Contact-impliciete trajectoptimalisatie (CITO) streeft naar het gelijktijdig optimaliseren van robottoestanden, inputs, contactkrachten en contactmodi zonder een contactsequentie voor te schrijven. Deze flexibiliteit introduceert Mathematical Programs with Complementarity Constraints (MPCCs), waarbij unilateraal contact en wrijving worden gemodelleerd via complementariteitsvoorwaarden (bijv. $0 \le a \perp b \ge 0$).

Deze problemen presenteren een fundamentele numerieke uitdaging: bij haalbare MPCC-punten zijn de gradiënten van actieve niet-negativiteits- en productrestricties structureel afhankelijk, wat ervoor zorgt dat standaard constraint-kwalificaties (zoals LICQ) falen. Deze degeneratie maakt conventionele primal–dual nonlinear programming solvers onbetrouwbaar, wat vaak resulteert in singuliere of slecht geconditioneerde KKT-systemen.

Bestaande benaderingen pakken dit robuustheidsprobleem aan door de volledig gekoppelde primal–dual Newton-stap te verlaten. Methoden zoals CRISP optimaliseren enkel het primale traject via convexe subproblemen, terwijl IMPACT een augmented-Lagrangian outer loop met block-coordinate descent gebruikt. Daarentegen maken structure-exploiterende optimal control solvers (bijv. acados) gebruik van Riccati-recursies voor efficiëntie, maar falen ze vaak in het genereren van nuttig contact wanneer exacte complementariteit wordt opgelegd zonder speciale behandeling. Er blijft een kloof bestaan tussen de MPCC-specifieke robuustheid en de efficiëntie van structure-exploiterende optimal control (OCP) solvers.

Methodologie: ContactIPM

ContactIPM stelt een verenigde primal–dual interior-point methode voor die de block-banded OCP-structuur behoudt die vereist is voor Riccati-recursie, terwijl complementariteitsrestricties robuust worden afgehandeld.

1. Barrier-Gekoppelde Elastische Interior Relaxatie

In plaats van penalty-gewichten of augmented Lagrangians te gebruiken, introduceert ContactIPM een stage-lokale elastische interior relaxatie voor elk gemarkeerd complementariteitspaar $(a, b)$:

• Slack Variabelen: Positieve zijde slacks ($s_a, s_b$) en een product slack ($s_c$) worden geïntroduceerd.
• Elastic Lift: De complementariteitsvoorwaarde wordt getransformeerd naar een stelsel van vergelijkingen:
  $$g_a + s_a = 0, \quad g_b + s_b = 0, \quad g_a g_b - \theta(\mu) + s_c = 0$$
  waarbij $\theta(\mu) = \gamma_{mpcc}\mu$ de complementariteit-verstrakking direct koppelt aan de barrier parameter $\mu$.
• Regulariteit: Deze constructie zorgt ervoor dat de lifted constraint Jacobian een identiteitsblok bevat ten opzichte van de slacks. Bijgevolg voldoet het gerelaxeerde probleem aan de Linear Independence Constraint Qualification (LICQ) bij elk interior punt, wat unieke multipliers en een regulier primal–dual systeem garandeert.
• Geen Onafhankelijke Penalty: In tegen tegenstelling tot penalty-methoden wordt de verstrakking van de complementariteit gecontroleerd door dezelfde barrier parameter $\mu$ die de central path definieert, waardoor de noodzaak voor onafhankelijk afgestemde penalty-coëfficiënten wordt geëlimineerd.

2. Stagewise Primal–Dual Eliminatie en Riccati-Recursie

De solver handhaaft primal–dual consistentie door de trajecten, dynamica-multipliers, ongelijkheids-multipliers en slacks bij te werken als componenten van een enkel verstoord KKT-systeem.

• Lokale Eliminatie: Omdat complementariteitsparen en hun elastische variabelen lokaal zijn aan individuele stages, elimineert de solver de lokale slack ($\Delta s_k$) en dual ($\Delta \lambda_k$) variabelen per stage.
• Gereduceerd Systeem: Deze eliminatie wijzigt enkel de lokale stage Hessian ($\tilde{H}_k$) en gradient ($\tilde{q}_k$). De dynamica blijven de enige koppeling tussen opeenvolgende stages, waardoor de block-banded structuur behouden blijft.
• Efficiënte Solve: Het gereduceerde systeem wordt opgelost met een backward Riccati-recursie gevolgd door forward substitutie, wat een lineaire complexiteit $O(N)$ bereikt met betrekking tot de horizonlengte.

3. Multi-Phase MPCC Recovery

Om de meerdere stationaire takken die geassocieerd zijn met verschillende contactmodi te hanteren, gebruikt ContactIPM een vastgesteld recovery-schema bestaande uit:

• Continuation: Het geleidelijk verstrakken van de barrier parameter terwijl geprobeerd wordt in de huidige contact-mode basin te blijven.
• Restart: Als continuation faalt (bijv. door verlies van conditionering), herstelt de solver de oorspronkelijke primale gok en start opnieuw met een grotere barrier parameter en Gauss–Newton kromming om verschillende basins te verkennen.
• Terminatie: De solver termineert pas wanneer de ongerelaxeerde fysieke complementariteits-residual ($r_{phys} = \max |a_k b_k|$) onder een gespecificeerde tolerantie valt, wat garandeert dat het uiteindelijke traject voldoet aan het fysieke contactmodel, en niet slechts aan de gerelaxeerde formulering.

4. Schaling en Globalisatie

De methode past horizon-consistente diagonale coördinaat-schaling toe, afgeleid van de magnitudes van de stage-derivaten. Globalisatie wordt bereikt via een filter line search die een balans zoekt tussen objectief-reductie en constraint-schending, met een fraction-to-boundary regel om positieve slacks te behouden.

Belangrijkste Bijdragen

1. Verenigde Formulering: Een stagewise formulering die gemarkeerde complementariteitsparen omzet in barrier-gekoppelde elastische productrijen binnen een nietlineair dynamica-, kosten- en ongelijkheidsframework.
2. MPCC-Bewuste Primal–Dual Newton Methode: Een methode die de gekoppelde verstoorde KKT-structuur behoudt terwijl gebruik wordt gemaakt van lokale slack/dual eliminatie en Riccati-recursie. Het bevat traject-schaal preconditioning en een multi-phase recovery beleid.
3. Reproduceerbare Evaluatie: Een uitgebreide experimentele suite die ContactIPM vergelijkt met CRISP, IMPACT en een met OCP-structuur uitgeruste acados baseline (met exacte complementariteit), inclusclusief 50 closed-loop rollouts onder diverse verstoringen.

Experimentele Resultaten

Prestaties vs. CRISP

• Snelheid: Op vier vaste benchmark-gevallen (Cartpole, Push Box, Transport, Push T) is ContactIPM 2.17× tot 8.87× sneller dan CRISP over 20 gepaarde timing-repetities.
• Robuustheid: ContactIPM komt overeen met CRISP op Cartpole en Transport, maar slaagt in 5 extra Push Box gevallen (24/25 vs. 19/25). Op de Push T suite slaagt ContactIPM in alle 50 gevallen, terwijl CRISP slechts in 27/50 slaagt wanneer strikte fysieke complementariteit wordt afgedwongen.

Prestaties vs. IMPACT

• Snelheid: ContactIPM is 2.96× sneller op Push T en 4.91× sneller op Cart Transport. Echter, IMPACT is 4.46× sneller op Push Box.
• Robuustheid: ContactIPM slaagt in 150/150 gevallen over drie 50-gevallen suites (Push Box, Push T, Cart Transport), vergeleken met IMPACT's 147/150.

Vergelijking met OCP-gestructureerde Baseline (acados)

• Wanneer acados is geconfigureerd met exacte complementariteit (geen elastische relaxatie), faalt het vaak in het genereren van nuttig contact. Bijvoorbeeld, in de Push Box benchmark slaagt acados in alle 25 gevallen voor fysieke haalbaarheid, maar bereikt 0/25 het einddoel, waarbij het vaak eindigt op een traject met nul-contact.
• Over 165 contact-rijke gevallen (CRISP Transport, CRISP Push T, IMPACT Push T, en IMPACT Cart Transport) produceerde acados slechts 1 geaccepteerd traject, terwijl ContactIPM er 158 slaagde. Dit demonstreert dat het exploiteren van de OCP-structuur alleen onvoldoende is om de MPCC-degeneratie te overwinnen.

Closed-Loop Validatie

• In 50 closed-loop Push Box rollouts met modelmismatch, ruis en state resets, slaagde ContactIPM in alle 50 gevallen.
• Latency: De mediane solve-tijd was 2.08 ms, waarbij 99.37% van de solves voldeed aan een 100 ms controle-deadline.
• Haalbaarheid: De closed-loop feedback handhaafde de primale haalbaarheid en fysieke complementariteit, waarbij de maximale fysieke complementariteitsproducten onder $1.01 \times 10^{-6}$ bleven.

Betekenis en Claims

Het artikel claimt dat MPCC-robuustheid, primal–dual consistentie en optimal-control structuur kunnen samenbestaan in één enkele solver. ContactIPM demonstreert dat de scheiding van complementariteitsafhandeling en de primal–dual Newton-stap (zoals gezien bij CRISP en IMPACT) geen onvermijdelijke prijs is voor contact-complementariteit. Door een elastische interior relaxatie te combineren met stagewise eliminatie en Riccati-recursie, bereikt de solver zowel de numerieke robuustheid die vereist is voor contact-impliciete problemen als de computationele efficiëntie van structure-exploiterende optimal control. De resultaten suggereren dat gespecialiseerde MPCC-behandeling noodzakelijk is, zelfs wanneer geavanceerde OCP lineaire algebra technieken worden gebruikt.