Sensor Query Schedule and Sensor Noise Covariances for Accuracy-constrained Trajectory Estimation

Titel: Sensor Query-schema's en Sensorruis-Covarianties voor Trajectschatting met Accuraiteitsbeperkingen

Auteurs: Abhishek Goudar en Angela P. Schoellig (Technische Universiteit München & University of Toronto)

1. Probleemstelling

Bij het schatten van het traject van een mobiele robot wordt de positie en oriëntatie in de tijd bepaald door dynamische modellen te combineren met ruisachtige sensorobservaties. De nauwkeurigheid van deze schatting wordt bepaald door twee factoren:

1. De fideliteit van het systeemmodel.
2. De kwaliteit en frequentie van de sensormetingen.

Traditionele benaderingen voor sensordata-gebruik proberen vaak de onzekerheid van de toestand te minimaliseren, zonder expliciet te kijken naar een voorgeschreven nauwkeurigheidsdoel. Dit leidt vaak tot inefficiënt gebruik van middelen (zoals energie, bandbreedte en rekkracht), omdat sensoren mogelijk vaker worden afgelezen dan strikt noodzakelijk is, of juist niet vaak genoeg om een bepaald doel te halen.

De kernvraag die dit artikel beantwoordt is: Welke sensorparameters (zoals de frequentie van metingen of de ruiscovariantie) zijn nodig om een specifieke, gewenste schattingnauwkeurigheid te bereiken? Het artikel richt zich op twee scenario's:

• Het bepalen van het query-schema (metingsfrequentie) voor een sensor met bekende ruis, om een bepaalde nauwkeurigheid te halen.
• Het bepalen van de benodigde ruiscovariantie (kwaliteit van de sensor) voor een gegeven update-frequentie om diezelfde nauwkeurigheid te halen.

2. Methodologie

De auteurs ontwikkelen een optimalisatieframework dat onafhankelijk is van de specifieke schatter die wordt gebruikt (estimator-agnostisch).

• Posterior Cramér-Rao Bound (PCRB): In plaats van te vertrouwen op de prestaties van een specifieke filter (zoals een Kalman-filter), gebruiken de auteurs de PCRB. Dit biedt een theoretische ondergrens voor de Mean Squared Error (MSE) van elke onbevooroordeelde schatter. De PCRB fungeert als een garantie voor de minimale fout die haalbaar is gegeven de systeemmodellen en sensorparameters.
• Recursieve Formulering: Om de PCRB computatie-efficiënt te maken, wordt een recursieve vorm afgeleid voor de Fisher Informatie Matrix (FIM). Dit stelt de auteurs in staat om de ondergrens van de schattingfout stap voor stap te berekenen zonder de volledige geschiedenis van de toestand opnieuw te hoeven inverteren.
• Semidefinite Programming (SDP): De beperking dat de schattingnauwkeurigheid een bepaalde drempel ($k_a$) niet mag overschrijden, wordt omgezet in een convexe beperking.
  • De auteurs formuleren het probleem als een Semidefinite Program (SDP).
  • De doelstelling is het minimaliseren van de "kosten" (bijvoorbeeld het aantal metingen of de ruis) onder de voorwaarde dat de PCRB-basis voldoet aan de gewenste nauwkeurigheidsmatrix ($P \preceq k_a^2 I$).
  • Door variabelen te transformeren (bijvoorbeeld het omgekeerde van de tijd tussen metingen of het omgekeerde van de covariantie), wordt het probleem convex en oplosbaar met standaard solvers (zoals MOSEK).

3. Belangrijkste Bijdragen

1. Optimalisatie op basis van nauwkeurigheid: Een nieuwe methode om sensoren te plannen en te karakteriseren die expliciet rekening houdt met een gewenste schattingnauwkeurigheid, in plaats van alleen onzekerheid te minimaliseren.
2. Tweevoudige toepassing:
   • Berekening van het optimale sensor query-schema (hoe vaak moet er gemeten worden?) voor een gegeven sensorkwaliteit.
   • Berekening van de benodigde sensorruis-covariantie (hoe nauwkeurig moet de sensor zijn?) voor een gegeven meetfrequentie.
3. Haalbaarheidsanalyse: Het framework kan scenario's identificeren waarin een gewenste nauwkeurigheid onmogelijk is te bereiken met de huidige systeem- en sensorparameters (infeasibility), wat cruciaal is voor systeemontwerp.
4. Validatie: Uitgebreide validatie in zowel simulaties als echte experimenten met een mobiele robot en UWB-sensoren (Ultra-Wideband).

4. Resultaten

De methode werd getest in simulaties en met een echte omnidirectionele robot in een omgeving met UWB-ankers en een motion capture-systeem voor grondwaarheid.

• Simulaties:

  • Voor zowel positie- als afstandssensoren (range sensors) bleek dat de berekende optimale query-frequentie ($m_s$) consistent resulteerde in een Root Mean Square Error (RMSE) die gelijk was aan of lager was dan de gewenste nauwkeurigheid ($k_a$).
  • Het gebruik van een lagere frequentie ($m_s/3$) leidde in de meeste gevallen tot een RMSE die de limiet overschreed.
  • Bij het schatten van covarianties bleek dat sensoren met de berekende optimale ruis de doelstelling haalden, terwijl sensoren met een hogere ruis (3x de berekende waarde) faalden.

• Echte Experimenten:

  • De methode slaagde erin om de juiste meetfrequentie te bepalen voor UWB-sensoren om een specifieke RMSE te halen.
  • Haalbaarheidsgrenzen: Het systeem kon succesvol aangeven wanneer een doel onbereikbaar was. Bijvoorbeeld, voor een zeer hoge nauwkeurigheids eis ($k_a = 0.001$ m) gaf de solver aan dat het probleem onoplosbaar was (infeasible) met de beschikbare hardware, wat een realistische limiet aangeeft.
  • In haalbare scenario's (bijv. $k_a = 0.07$ m) leverde de optimale frequentie een RMSE van 0.055 m op, terwijl een suboptimale frequentie (1/3e) een RMSE van 0.072 m opleverde, wat de doelstelling overschreed.

5. Betekenis en Toekomstperspectief

Dit werk biedt een fundamenteel nieuw perspectief op sensorbeheer in robotica. In plaats van "hoeveel data kunnen we verwerken?", stelt het de vraag "hoeveel data is er minimaal nodig om een taak succesvol uit te voeren?".

• Efficiëntie: Het voorkomt overbelasting van communicatiekanalen en rekkracht door alleen de noodzakelijke metingen te vragen.
• Systeemontwerp: Het helpt ingenieurs bij het kiezen van de juiste sensoren en het bepalen van de operationele limieten van een robot voordat deze wordt gebouwd of ingezet.
• Toekomst: De auteurs noemen als volgende stappen het uitbreiden van het framework naar diverse bewegingsmodellen, het schatten van procescovarianties en het toepassen op niet-lineaire variëteiten (zoals rotaties in SE(3)).

Kortom, de paper presenteert een wiskundig robuust en praktisch toepasbaar framework om de balans tussen sensorprestaties, meetfrequentie en schattingnauwkeurigheid optimaal te sturen.