A Terrain-Adaptive epsilon-Constraint MPC for Uneven Terrain Kinodynamic Planning

Technische Samenvatting: Een terreinadaptieve $\epsilon$-constraint MPC voor kinodynamische planning op ongelijk terrein

Probleemstelling
Autonome navigatie voor auto-achtige voertuigen in ongestructureerde, oneffen omgevingen (bijvoorbeeld bouwplaatsen, rampgebieden, planetaire exploratie) vormt een dubbele uitdaging: het genereren van botsingsvrije paden terwijl complexe kinodynamische beperkingen in acht worden genomen. Traditionele op mesh gebaseerde planners, zoals die gebruikmaken van de Fast Marching Method (FMM), excelleren in het produceren van terreinbewuste geometrische paden, maar slagen er vaak niet in om dynamische haalbaarheid, zoals snelheidslimieten en voertuigstabiliteit, te integreren. Omgekeerd worstelen kinodynamische planners vaak met het in evenwicht brengen van concurrerende doelen, specifiek pad-efficiëntie versus pose-stabiliteit (het voorkomen van omvallen), zonder in lokale optima te belanden of te vertrouwen op sterk vereenvoudigde 2D-aannames. Bestaande datagedreven benaderingen (bijvoorbeeld Deep Reinforcement Learning) missen vaak generalisatie over diverse start-doel-configuraties en slagen er niet in om continue globale leidingsstructuren te integreren. Bovendien vertrouwen standaard methoden voor multi-objectieve optimalisatie vaak op gewogen scalarisatie, wat kan leiden tot het feit dat padlengte de oplossing domineert, waardoor kritieke stabiliteitsbeperkingen op ruw terrein worden verwaarloosd.

Methodologie
De auteurs stellen een regulerend horizonbesturingskader voor dat een semi-parametrisch voertuig-terreinenmodel integreert met een terreinadaptieve $\epsilon$-constraint Model Predictive Control (MPC).

1. Voertuig-Terrein Modeling:

   • Semi-Parametrische Aanpak: Het systeem combineert een analytisch kinematisch model voor auto-achtige voertuigen met een datagedreven residucorrectie. Het nominale model voorspelt de statische evolutie op basis van lineaire en hoeksnelheden.
   • Sparse Gaussian Process (SGP): Om niet-gemodelleerde terreineffecten (bijvoorbeeld slip, complexe contactdynamica) te compenseren, wordt een SGP getraind op lokale terreinbeschrijvers om de residufout tussen het nominale model en het werkelijke systeemgedrag te voorspellen.
   • Terreinbeschrijvers: Het model maakt gebruik van een rijke set beschrijvers, waaronder oppervlakte-gewogen normaalvectoren, oppervlakteruwheid ($\sigma_z$), Gaussische en gemiddelde krommingen, en het totale buurtegebied.
   • Hoogtefunctie: Een projectiegebaseerde methode bepaalt de terreinhoogte $h(x,y)$ door query-punten te mappen naar het dichtstbijzijnde vlak op een driehoekig mesh, waardoor nauwkeurige oriëntatieberekeningen worden gewaarborgd.

2. Optimalisatiekader:

   • Multi-Objectieve Formulering: De planner optimaliseert twee concurrerende doelen:
     • Pad-efficiëntie ($f_1$): Minimaliseert de directionele misalignement met een globaal FMM-vectorveld, gewogen door een doel-nabijheidsschaalfactor.
     • Pose-stabiliteit ($f_2$): Minimaliseert cumulatieve oriëntatieafwijkingen (roll en pitch) ten opzichte van de lokale terreinnormaal, met een boete voor afwijkingen boven een veilige drempel ($\alpha_{safe}$) en het afdwingen van een harde limiet ($\alpha_{max}$) om omvallen te voorkomen.
   • Terreinadaptieve $\epsilon$-constraint: In plaats van gewogen scalarisatie, wordt het probleem omgezet in een geconstrueerde single-objectieve optimalisatie. De stabiliteitsbeperking ($f_2 \leq \epsilon_{stab}$) wordt dynamisch aangepast op basis van lokale terreinruwheid en helling. In uitdagend terrein wordt het stabiliteitsbudget verscherpt; in onschuldig terrein wordt het versoepeld, waardoor meer agressieve exploratie mogelijk is.
   • Samplingstrategie: Kandidaat-besturingssequenties worden gegenereerd via een hybride aanpak:
     • Warm-starting: Het verschuiven van vorige succesvolle besturingssequenties met toegevoegde ruis om temporale continuïteit te waarborgen.
     • FMM-biased Sampling: Het mengen van besturingen die worden voorgesteld door het globale FMM-vectorveld met willekeurige exploratie om lokale minima te ontvluchten.

Belangrijkste Bijdragen
Het artikel claimt vier primaire bijdragen:

1. Een semi-parametrisch model voor de beweging van auto-achtige voertuigen op ongelijk driehoekig mesh, dat analytische dynamica combineert met SGP-gebaseerde residuleer.
2. Een kostfunctie voor mesh-navigatie, afgeleid van een continu FMM-vectorveld.
3. Een oplossingsaanpak die terreinadaptieve $\epsilon$-constraint optimalisatie gebruikt binnen een regulerend horizonkader om niet-gedomineerde trajecten te genereren die efficiëntie en stabiliteit in evenwicht brengen.
4. Een ROS-compatibele planner (Move Base Flex) ontworpen voor realtime implementatie.

Experimentele Resultaten
De voorgestelde methode ($\epsilon$-MPC) werd geëvalueerd in simulatie (Gazebo) en realiteitsexperimenten met een AgileX Scout Mini-robot op twee distincte 3D mesh-terreinen. Het werd vergeleken met Model Predictive Path Integral (MPPI) en Genetic Algorithmic Kinodynamic (GAKD) baselines.

• Prestatiemetrics: De voorgestelde methode behaalde een navigatiesuccespercentage van 94% in simulatie, beter dan MPPI (87%) en GAKD (89%).
• Stabiliteit: Het verminderde de maximale oriëntatieafwijking met 24% ten opzichte van MPPI en verbeterde de multi-objectieve trade-off-metric ($T$) met 23%.
• Ablatiestudies:
  • FMM Bias: Een FMM-biassterkte van $\alpha_{FMM}=0.7$ leverde optimale resultaten op, met een verbetering van de succespercentages met 11–27% ten opzichte van pure willekeurige sampling.
  • Adaptiviteit: De terreinadaptieve constraint-aanpak elimineerde alle omvalincidenten die werden waargenomen bij vaste epsilon-constraints en verminderde de maximale oriëntatieafwijking met 32%.
  • Realiteit-implementatie: In fysieke experimenten behield de methode een succespercentage van 80% (tegenover 67% voor MPPI), en navigeerde succesvol steile secties waar baselines faalden vanwege excessive rollhoeken. Plannertijden bleven binnen realtime-beperkingen (~35–48 ms per iteratie).

Betekenis en Claims
Het artikel stelt dat de primaire betekenis ligt in het bieden van een principieel kader dat expliciet de multi-objectieve aard van kinodynamische planning op ongelijk terrein adresseert. Door stabiliteitsbeperkingen dynamisch aan te passen op basis van lokale terreinbeschrijvers, bereikt de aanpak een superieur evenwicht tussen padoptimaliteit en fysieke veiligheid zonder te vertrouwen op complexe, computatie-intensieve evolutionaire zoekopdrachten (zoals NSGA-II, wat te traag bleek voor realtime gebruik). De auteurs concluderen dat, hoewel gewogen scalarisatiemethoden individuele doelen kunnen optimaliseren, de terreinadaptieve $\epsilon$-constraint-methode effectiever is voor het garanderen van haalbare, veilige navigatie in ongestructureerde buitenomgevingen, en een praktische oplossing biedt die schaalt van simulatie tot realiteit-implementatie.