Technische Samenvatting: Een Proprioceptieve-Only Benchmark voor Staatschatting bij Vierpotige Robots

Probleemstelling

Betrouwbare proprioceptieve staatschatting is cruciaal voor wendbare voortbeweging van vierpotige robots, vooral in omgevingen waar exteroceptieve sensing (bijv. visueel, LiDAR, GPS) onbetrouwbaar of niet beschikbaar is door stof, mist, kenmerkloze gangen of signaalonderbrekingen. In dergelijke scenario's moeten robots uitsluitend vertrouwen op Inertial Measurement Units (IMU's), joint-encoders en contactinformatie om de beweging van het lichaam te schatten. De kernuitdaging bestaat uit het vinden van een balans tussen twee concurrerende eisen:

1. Lange-termijn consistentie: Het behouden van nauwkeurigheid over uitgebreide trajecten ondanks drift en modelfouten.
2. Korte-horizon fideliteit: Het behouden van lokale nauwkeurigheid die essentieel is voor feedbackregeling en het plannen van steunpunten, terwijl strikte latentie- en rekenbegrotingen op ingebouwde platformen worden gerespecteerd.

Hoewel er diverse benaderingen bestaan, ontbreekt het aan directe, "appels-tot-appels"-vergelijkingen tussen verschillende state-of-the-art paradigma's (filteren versus gladmaken, invariant versus niet-invariant) op een gemeenschappelijke real-world dataset om practitioners te begeleiden bij het selecteren van de juiste schatter voor hun specifieke toepassingsbeperkingen.

Methodologie

De auteurs presenteren een vergelijkende benchmark van drie representatieve, open-source staatschatters voor vierpotige robots, die allemaal opereren in een proprioceptieve-only modus op de CYN-1-sequentie van de GrandTour Dataset. Alle methoden schatten de zwevende-basis pose $(R, p) \in SO(3) \times \mathbb{R}^3$ en de lichaamssnelheid $v \in \mathbb{R}^3$.

1. MUSE (Multi-sensor Unified State Estimator)

• Paradigma: Modulaire, real-time fusiestack (Filteren).
• Mechanisme: In deze studie werkt MUSE puur proprioceptief, waarbij exteroceptieve blokken zijn uitgeschakeld. Het maakt gebruik van een gekaskede Niet-lineaire Observer (NLO) en een eXogene Kalman-filter (XKF) voor houdingsschatting, wat wereldwijde stabiliteit garandeert. Benen-odometrie wordt afgeleid uit voorwaartse kinematica van benen in stabiel contact. Deze metingen worden gefuseerd via een Kalman-filter om pose en twist te outputten.
• Kernkenmerk: Ontworpen voor robuustheid en real-time prestaties, en in staat om grote initiële oriëntatiefouten te hanteren dankzij zijn wereldwijd stabiele houdingsobserver.

2. Invariant Extended Kalman Filter (IEKF)

• Paradigma: Invariant Filteren.
• Mechanisme: Geformuleerd op een groep-affien systeem waarbij de rechts-invariante foutdynamica onafhankelijk is van de staat. Deze structuur verbetert convergentie en consistentie, met name tijdens contactgebeurtenissen.
• Mechanisme Details: Propagatie integreert IMU-data, terwijl updates stabiel contact veronderstellen (nul voetsnelheid). Het maakt gebruik van Lie-groepstructuren om foutdynamica te lineariseren, en integreert robuuste meetkosten en slipbewustzijn.

3. Invariant Smoother (IS)

• Paradigma: Fixed-lag Gladmaken (Maximum A Posteriori).
• Mechanisme: In tegenstelling tot de IEKF, die alleen de meest recente staat update, optimaliseert de IS over een vast venster van staten ($n$) en metingen. Het formuleert het probleem als een niet-lineair kleinste-kwadratenprobleem met structuurbewuste residuen die zijn aangepast aan de groep-affiene eigenschappen van de IEKF.
• Kernkenmerk: Door te optimaliseren over een venster bereikt het een verbeterde tolerantie voor dynamisch contact (bijv. slippen) en hogere nauwkeurigheid, maar ten koste van een verhoogde rekenlast en algoritmische latentie die evenredig is met de venstergrootte.

Evaluatiemetrieken

De studie maakt gebruik van standaard metrieken berekend via het evo Python-pakket:

• Absolute Trajectfout (ATE): Meet globale pose-nauwkeurigheid en geaccumuleerde drift door het geschatte traject uit te lijnen met de ground truth via SE(3) Umeyama-uitlijning.
• Relatieve Posefout (RPE): Meet korte-horizon nauwkeurigheid. Geëvalueerd onder twee schema's:
  • Ruimtelijk venster ($\Delta = 1$ m): Onderzoekt drift op meterschaal.
  • Tijdsvenster ($\Delta = 1$ frame): Onderzoekt frame-tot-frame consistentie en gevoeligheid voor meetjitter.
• Snelheid RMSE: Beoordeelt de nauwkeurigheid van geschatte snelheidsprofielen.
• Runtime: Berekeningstijd per update gemeten op een vaste hardware/software-stack.

Belangrijkste Resultaten

Nauwkeurigheid

• Lange-termijn (ATE): Zowel de IEKF als de Invariant Smoother (IS) behaalden een lagere ATE dan MUSE. Het verschil tussen de beste (IS) en de slechtste (MUSE) was ongeveer 0,91 m over een 300 m traject (ongeveer 0,3%). Dit suggereert dat groep-affiene modellering en gladmakende regularisatie helpen bij het verminderen van geaccumuleerde drift.
• Korte-termijn (RPE):
  • Bij $\Delta = 1$ m: IEKF en IS presteerden aanzienlijk beter dan MUSE (ongeveer 40% reductie in translationele RPE), wat wijst op een betere hantering van systematische fouten die zich over meerdere updates opstapelen.
  • Bij $\Delta = 1$ frame: IEKF en MUSE vertoonden een lagere RPE dan IS (ongeveer respectievelijk 72% en 66% reductie). Dit suggereert dat de hogere-rate fusie in filters in deze specifieke metriek beter hoogfrequente ruis onderdrukt dan de gladmakende aanpak.
  • Rotatie RPE: De resultaten waren over het algemeen vergelijkbaar voor alle methoden.
• Snelheid: Alle drie de methoden leverden nauwkeurige snelheidsprofielen met kleine RMSE-verschillen.

Runtime en Latentie

• MUSE: Snelste, met een gemiddelde runtime van $\approx 0,012$ ms per iteratie.
• IEKF: Iets langzamer, $\approx 0,02$ ms per iteratie.
• Invariant Smoother: Toont een duidelijke nauwkeurigheids-latentie trade-off afhankelijk van de venstergrootte (WS).
  • WS = 1: $\approx 0,110$ ms.
  • WS = 5: $\approx 0,583$ ms.
  • Opmerking: Fixed-lag gladmaken introduceert algoritmische latentie die evenredig is met de vertraging, wat de staatschattingen kan vertragen afhankelijk van de implementatie van de pijplijn.

Convergentiegedrag

Onder grote initiële oriëntatiefouten (180-graden roll-misalignement) convergeerden alle drie de schatters naar de juiste oriëntatie. Echter, MUSE convergeerde sneller dan IEKF en IS. Dit wordt toegeschreven aan het ontwerp van de wereldwijd stabiele houdingsobserver van MUSE, terwijl IEKF en IS vertrouwen op lokale linearisatie die kan worden geschonden door grote initiële fouten, wat leidt tot langzamere convergentie.

Betekenis en Bijdragen

Het paper biedt een beknopte, reproduceerbare snapshot van de trade-offs tussen nauwkeurigheid, latentie en rekenkosten voor inzetbare vierpotige systemen. De primaire bijdragen zijn:

1. Directe Vergelijking: Een "appels-tot-appels"-evaluatie van drie verschillende, hoogpresterende schattingsparadigma's (MUSE, IEKF, IS) op een gedeelde real-world dataset.
2. Practitioner Gids: Duidelijke data over hoe verschillende metrieken (ATE versus RPE op verschillende schalen) en rekenbeperkingen (latentie versus nauwkeurigheid) de selectie van de schatter beïnvloeden.
3. Open-source Reproduceerbaarheid: De release van een complete benchmarking-pijplijn, inclusief code, documentatie en dataverwerkingstools, om toekomstige gestandaardiseerde benchmarks te faciliteren.

De auteurs concluderen dat terwijl gladmaken (IS) superieure lange-termijn nauwkeurigheid biedt, filters (MUSE, IEKF) lagere latentie en potentieel betere korte-termijn ruisonderdrukking bieden. De keuze van de schatter moet worden bepaald door de specifieke eisen van de toepassing met betrekking tot wereldwijde consistentie, regellusfrequentie en beschikbare rekenresources. De studie claimt niet alle uitdagingen op het gebied van staatschatting op te lossen, maar beoogt het huidige landschap te verduidelijken voor practitioners.