Collision Detection with Analytical Derivatives of Contact Kinematics

Technische Samenvatting: iDCOL – Impliciete Differentieerbare Botsingsdetectie met Analytische Afgeleiden

Probleemstelling
Gradiëntgebaseerde methoden in de robotica (planning, controle, leren) vereisen snelle en nauwkeurige berekening van afgeleiden van sturende vergelijkingen met betrekking tot robottoestanden en controle-inputs. Een kritieke voorwaarde voor differentieerbare contactfysica is een goed gedefinieerde, differentieerbare contactkinematica-kaart die robottoestanden relateert aan contactafstand, locatie en normaal.

Bestaande benaderingen kampen met twee primaire beperkingen:

1. Niet-gladheid in gedegenereerde configuraties: Standaard botsingsdetectiemethoden (bijv. GJK/EPA) vertrouwen op discrete feature-selectie. Wanneer contactfeatures wisselen (bijv. vlak-tegen-vlak, rand-tegen-rand), worden de resulterende contacthoeveelheden stuksgewijs gedefinieerd en niet-differentieerbaar.
2. Geometrische degeneratie: Differentiabiliteit wordt fundamenteel beperkt door de geometrie van de contacterende lichamen. Niet-strikt convexe geometrieën (bijv. polytopen, cilinders) bezitten nul of ongedefinieerde kromming in bepaalde richtingen, wat leidt tot niet-unieke contactlocaties of normalen. In contrast hiermee bieden strikt convexe geometrieën (bijv. sferen, ellipsoïden) unieke contactpunten en normalen, wat differentieerbare kinematica oplevert.

Methodologie
Het artikel introduceert iDCOL (implicit differentiable collision detection), een raamwerk dat deze beperkingen aanpakt door niet-strikt convexe geometrieën te regulariseren naar strikt convexe impliciete representaties.

1. Geometrische Regularisatie via Impliciete Oppervlakken:
   In plaats van exacte polyedrische of conische representaties te gebruiken, modelleert iDCOL botsende lichamen met behulp van strikt convexe impliciete oppervlakken gedefinieerd door scalaire functies $\phi: \mathbb{R}^3 \to \mathbb{R}$.

   • Gladde Polytopen: Benaderd met de LogSumExp (smooth-max) operator om halfruimte-restricties te mengen.
   • Superquadrics en Superelliptische Cilinders: Gebruikt om dozen, kegels en cilinders te benaderen met instelbare vormexponenten ($n$) en scherpteparameters ($\beta$).
   • Deze representaties garanderen een positieve eindige kromming overal, wat unieke contactpunten en normalen garandeert.

2. Schaalgebaseerde Optimalisatieformulering:
   Bouwend op het DCOL-raamwerk, formuleert iDCOL botsingsdetectie als het vinden van de minimale uniforme schaalfactor $\alpha$ waarvoor twee geschaalde lichamen een contactpunt delen. Het probleem wordt geformuleerd als:
   $$\min_{x, \alpha} \alpha \quad \text{s.t.} \quad \phi_1(x/\alpha) \le 0, \quad \phi_2(R^T(x-r)/\alpha) \le 0$$
   De Karush–Kuhn–Tucker (KKT) condities van dit probleem reduceren tot een systeem van zes niet-lineaire vergelijkingen met zes onbekenden ($x \in \mathbb{R}^3, \alpha, \lambda_1, \lambda_2$).

3. Numerieke Oplossingsstrategie:

   • Reparametrisatie: De schaalfactor wordt hergeparametreerd als $\alpha = e^s$ om positiviteit af te dwingen en de conditionering te verbeteren.
   • Surrogaat Formulering: Om extreme configuraties te behandelen (bijna samenvallende of ver van elkaar verwijderde lichamen), herschaalt een surrogaatprobleem de relatieve translatie om niet-penetrerende initiële condities te waarborgen.
   • Solver: Er wordt een aangepaste, beveiligde Newton-type solver met backtracking line search gebruikt. Deze maakt gebruik van analytisch berekende Jacobiaan en ondersteunt warm starten vanaf voorgaande tijdstappen.

4. Analytische Afgeleiden:
   Door de Impliciete Functiestelling (IFT) toe te passen op het KKT-residustsysteem, leidt het raamwerk analytische afgeleiden van de contactoplossing ($z^*$) met betrekking tot de configuratie $q$. Dit maakt de exacte berekening van Jacobiaan voor contactkinematica-hoeveelheden (gap, normaal, getuigenpunten) mogelijk zonder numerieke differentiatie of de overhead van automatische differentiatie.

Belangrijkste Bijdragen

• iDCOL Raamwerk: Een impliciet differentieerbaar botsingsdetectie en contactkinematica-raamwerk dat de differentiabiliteit in gedegenereerde contactconfiguraties herstelt door geometrische regularisatie.
• Vaste Grootte Niet-lineair Systeem: In tegenstelling tot eerdere optimalisatiegebaseerde benaderingen die mogelijk variërende dimensies hebben, reduceert iDCOL botsingsdetectie tot een deterministisch $6 \times 6$ systeem, wat efficiënte oplossing en differentiatie mogelijk maakt.
• Analytische Afgeleiden: De afleiding van exacte analytische afgeleiden voor contactkinematica via de IFT, waardoor de computationele kosten en benaderingsfouten van numerieke of automatische differentiatie worden vermeden.
• Open-Source Implementatie: Een snelle, microseconde-schaal C++ solver wordt geleverd, met ondersteuning voor een bibliotheek van gladde impliciete primitieven (polytopen, afgeplatte kegels, superellipsoïden, superelliptische cilinders).

Resultaten en Evaluatie
De aanpak werd geëvalueerd via uitgebreide simulaties en benchmarking:

• Robuustheid en Runtime: De solver convergeert betrouwbaar voor diverse vormparen. Zonder warm starten liggen de gemiddelde looptijden tussen de ~2 µs (eenvoudige vormen) en ~19 µs (complexe superellipsoïden). Met warm starten dalen de looptijden consistent naar ~2 µs.
• Vergelijking met DCOL:
  • Voor polytopen-polytopen contacten is iDCOL aanvankelijk trager dan DCOL (dat exacte polyedrische geometrie gebruikt), maar wordt het vergelijkbaar met warm starten.
  • Voor gladde vormparen (bijv. ellipsoïde-ellipsoïde) is iDCOL sneller dan DCOL.
  • Cruciaal is dat iDCOL een gladdere configuratieafhankelijkheid biedt, wat voordelig is voor gradiëntgebaseerde optimalisatie, terwijl DCOL een stuksgewijs-gladde gedraging vertoont door wijzigingen in de actieve verzameling (active sets).
• Toepassingen:
  • Quadrotor Trajectplanning: Gradiëntgebaseerde optimalisatie van een botsingsvrij traject verminderde de rekentijd met bijna twee ordes van grootte vergeleken met methoden zonder analytische gradiënten.
  • Multibody Rigid Collisions: Een simulatie van 10 interagerende starre lichamen werd voltooid in ~1,94s; zonder analytische afgeleiden duurde de simulatie ongeveer 6,5 keer langer en vertoonde het minder robuustheid.
  • Soft-Rigid Interactie: Een zachte manipulator die interageert met starre obstakels werd succesvol gesimuleerd. Zonder analytische afgeleiden liep de solver vast, wat leidde tot looptijden die tot twee ordes van grootte hoger lagen.

Betekenis en Claims
Het artikel stelt dat iDCOL een fundamentele spanning adresseert tussen geometrische exactheid en analytische differentieerbaarheid in contactrijke robotica. Door expliciet geometrische exactheid in te ruilen voor strikte convexiteit (via instelbare smoothing parameters), mitigeert het raamwerk geometrische degeneraties die de differentiabiliteit ondermijnen.

De auteurs beweren dat iDCOL efficiënte, microseconde-schaal botsingsqueries met volledige analytische afgeleiden mogelijk maakt, waardoor het geschikt is voor integratie in gradiëntgebaseerde simulatie-, controle- en trajectoptimalisatie-frameworks. Het raamwerk is ontworpen om modulair te zijn, zodat nieuwe convexe primitieven eenvoudig kunnen worden geïntroduceerd door enkel hun impliciete functies en afgeleiden te definiëren, terwijl de kernalgoritme vormonafhankelijk blijft. Het werk beoogt de ontwikkeling van schaalbare, contactbewuste algoritmen te faciliteren die geometrie, dynamica en optimalisatie nauw met elkaar koppelen.