One-Step Model Predictive Path Integral for Manipulator Motion Planning Using Configuration Space Distance Fields

Technische Samenvatting: One-Step Model Predictive Path Integral voor Manipulator Bewegingsplanning met behulp van Configuration Space Distance Fields

Probleemstelling
De bewegingsplanning voor robotmanipulatoren kampt met een dichotomie tussen optimalisatiegebaseerde en bemonsteringsgebaseerde methoden. Klassieke optimalisatiebenaderingen (bijv. CHOMP, TrajOpt) vertrouwen op Signed Distance Fields (SDFs) in de werkruimte om botsingsvermijdingsrestricties op te leggen. Deze methoden lijden echter onder lokale minima en falen wanneer de SDF-gradiënten verdwijnen nabij obstakels. Bovendien is de verdeling van SDF-waarden in de configuratieruimte vaak niet uniform. Daartegenover staan gradiëntvrije methoden zoals Model Predictive Path Integral (MPPI) controle, die het probleem van verdwijnende gradiënten vermijden door lange-horizon traject-rollouts te gebruiken om obstakels impliciet te verkennen. Hoewel effectief, is standaard MPPI computationeel duur vanwege de noodzaak van duizenden traject-monsters, herhaalde botsingscontroles en de moeilijkheid van het ontwerpen van kostenfuncties met heterogene fysieke eenheden (bijv. het mengen van afstand, snelheid en koppel).

Methodologie
De auteurs stellen een raamwerk voor dat Configuration Space Distance Fields (CDFs) integreert met Model Predictive Path Integral (MPPI) controle om directe navigatie in de configuratieruimte van de robot mogelijk te maken.

1. CDF Representatie: In tegenstelling tot werkruimte-SDFs, modelleren CDFs ($f_c$) afstanden direct in de configuratieruimte. Ze voldoen aan de Eikonal-vergelijking ($\|\nabla_q f_c\| = 1$) bijna overal, wat betrouwbare, niet-verdwijnende gradiëntinformatie levert die wijst naar de optimale ontsnappingsrichting uit obstakels.
2. Hoekgebaseerde Kostenfunctie: Om de kostencomponenten te verenigen en heterogene eenheden te elimineren, introduceren de auteurs een kostenfunctie gebaseerd op hoeken. De kosten $c(\theta_1, \theta_2)$ worden gedefinieerd als:
   • $\theta_1$: De hoek tussen de bewegingsvector en de CDF-gradiënt (obstakelnormaal). Dit straft beweging richting obstakels af.
   • $\theta_2$: De hoek tussen de bewegingsvector en de doelvector. Dit straft beweging weg van het doel af.
   • Conditionele Logica: De obstakelkost ($\theta_1$) wordt op nul gezet als de robot van het obstakel weg beweegt, als de afstand tot het obstakel een drempelwaarde ($d_{act}$) overschrijdt, of als de afstand tot het obstakel groter is dan de afstand tot het doel. Dit voorkomt dat de robot onnodig rond obstakels cirkelt.
3. One-Step Horizon MPPI: Door gebruik te maken van de lokale geometrische veiligheidscues geleverd door de CDF-gradiënt, reduceert de methode de MPPI-voorspellingshorizon tot een enkele stap ($H=1$). In plaats van lange-horizon trajecten te simuleren om toekomstige kosten te accumuleren, bemonsteren de algoritmen single-step controle-inputs, evalueren de directe hoekgebaseerde kosten en updaten de sampling-distributiepolicy.
4. Afhandeling van Gewrichtslimieten: Controle-inputs worden geprojecteerd op de haalbare verzameling gedefinieerd door gewrichtslimieten met behulp van een elementgewijze clipping-strategie voordat ze worden uitgevoerd.

Belangrijkste Bijdragen

• Integratie van CDF en MPPI: Het artikel combineert de gradiëntbetrouwbaarheid van CDFs met de bemonsteringsrobuustheid van MPPI, waardoor de noodzaak voor lange-horizon rollouts wordt geëlimineerd.
• Verenigde Kostenfunctie: Door alle kosten als hoeken uit te drukken, vereenvoudigt de methode de afstemming van gewichten en wordt het probleem van het vergelijken van heterogene fysieke eenheden opgeheven.
• One-Step Horizon: Het gebruik van CDF-gradiënten maakt een drastische reductie van de voorspellingshorizon van $H \gg 1$ naar $H=1$ mogelijk, wat de computationele belasting aanzienlijk verlaagt terwijl botsingsvermijding behouden blijft.
• Vermijden van Lokale Minima: Door de iteratieve optimalisatie-solver (die gevoelig is voor lokale minima in hoog-dimensionale ruimtes) te verwijderen en te vertrouwen op de distributie-updates van MPPI, verbetert de methode de robuustheid in drukke omgevingen.

Experimentele Resultaten
De methode werd geëvalueerd op een 2-DOF twee-schakel robot en een 7-DOF Franka Emika manipulator in simulatie.

• 2-DOF Robot: De voorgestelde methode behaalde een succespercentage van 99,6%, waarmee het de originele MPPI (71,2%) en een QP-gebaseerde optimalisatiemethode (64,1%) aanzienlijk versloeg. Hoewel de padlengte iets langer was dan die van de baselines, slaagde de methode erin de lokale optima te vermijden waarin de QP-solver vastliep.
• 7-DOF Franka Robot:
  • Scenario 1 (60 obstakels): 100% succespercentage met een gemiddelde bestuursfrequentie van 776 Hz.
  • Scenario 2 (123 obstakels): 86% succespercentage met een gemiddelde bestuursfrequentie van 718 Hz.
  • Vergelijking: De QP-baseline bereikte slechts ~13-14% succespercentages en opereerde op ~220 Hz. Een vergelijking met een standaard MPPI-implementatie (cuRobo) toonde aan dat de voorgestelde methode op ~700+ Hz draaide versus ~28-61 Hz voor de standaard MPPI, wat wordt toegeschreven aan de vereenvoudigde one-step horizon en het efficiënte gebruik van CDF.

Betekenis en Claims
Het artikel claimt dat het voorgestelde CDF-MPPI raamwerk een praktische oplossing biedt voor hoog-dimensionale bewegingsplanning in drukke omgevingen. Door gebruik te maken van CDF-gradiënten om directe lokale veiligheidscues te bieden, bereikt de methode:

1. Hoge Efficiëntie: Bestuursfrequenties die 750 Hz overschrijden, waarmee zowel optimalisatiegebaseerde solvers als standaard MPPI aanzienlijk worden overtroffen.
2. Hoge Betrouwbaarheid: Bijna perfecte succespercentages in 2D en consequent hoge succespercentages in complexe 7-DOF scenario's.
3. Eenvoud: Een verenigde kostenfunctie die de afstemming vereenvoudigt en de computationele last van lange-horizon simulaties wegneemt.

De auteurs merken op dat fouten primair optreden wanneer de robot extreem dicht bij obstakels begint (waardoor er onvoldoende tijd overblijft voor distributie-adaptatie) of door CDF-schattingsfouten in configuraties nabij obstakels. Zij identificeren het verbeteren van de CDF-getrouwheid nabij obstakels en het uitbreiden van de representatie naar sferische regio's als richtingen voor toekomstig werk.