State Estimation

Ein Roboter misst seinen Zustand nie direkt; er leitet Orientierung, Geschwindigkeit und Position aus verrauschten, mit Bias behafteten Sensoren ab. Diese Seite behandelt IMU-Modelle, Komplementär- und Mahony-Filter, das Kalman-Filter, Bein-Odometrie und LiDAR-SLAM sowie das, was Strido tatsächlich veröffentlicht.

In Strido

Der XIAO ESP32-S3 liest eine LSM6DSOX-IMU aus und führt in seiner Regelschleife mit $200\mathrm{Hz}$ ein Mahony-Filter aus (firmware/strido_esp32s3/src/main.cpp, Periode kControlPeriodUs = 5000 in board_config.h). ros2/strido_control/strido_control/mcu_bridge.py veröffentlicht es erneut als /imu/data; gait_node.py veröffentlicht /odom per Koppelnavigation; ein RPLIDAR C1 veröffentlicht /scan (siehe docs/software.md und Sensoren).

IMU-Messmodelle

Eine inertiale Messeinheit enthält ein 3-Achsen-Gyroskop (Winkelgeschwindigkeit) und einen 3-Achsen-Beschleunigungssensor (spezifische Kraft). Beide werden im körperfesten Koordinatensystem des Sensors $\{b\}$ ausgedrückt.

Gyroskop.

$$\tilde{\mathit{\omega }}=\mathit{\omega }+{\mathbf{b}}_g+{\mathbf{n}}_g$$

${\mathbf{b}}_g$ ist ein langsam driftender Bias (temperaturabhängig), ${\mathbf{n}}_g$ weißes Rauschen. Die Integration des Gyroskops liefert die Orientierung, die Integration des Bias jedoch einen Winkelfehler, der linear mit der Zeit wächst: Ein Bias von nur ${0.5}^{\circ }/\mathrm{s}$ wird nach einer Minute zu ${30}^{\circ }$.

Beschleunigungssensor. Er misst die Beschleunigung minus der Schwerkraft, in den Körper gedreht:

$$\tilde{\mathbf{a}}={\mathbf{R}}^{\mathrm{\top }}(\ddot{\mathbf{p}}-\mathbf{g})+{\mathbf{b}}_a+{\mathbf{n}}_a,\mathbf{g}=(0,0,-9.81{)}^{\mathrm{\top }}\mathrm{m}/{\mathrm{s}}^2$$

wobei $\mathbf{R}$ vom Körper- ins Weltkoordinatensystem dreht. Im Stillstand ($\ddot{\mathbf{p}}=0$) liest er ${\mathbf{R}}^{\mathrm{\top }}(0,0,9.81{)}^{\mathrm{\top }}$: den „Oben“-Vektor in Körperkoordinaten. Das legt Rollen und Nicken ohne Drift fest, ist aber verrauscht und verfälscht, sobald der Körper beschleunigt (bei jedem Schritt). Über das Gieren sagt er nichts aus, da eine Drehung um die Schwerkraftachse den Schwerkraftvektor nicht verändert.

Die beiden Sensoren ergänzen sich: Das Gyroskop ist glatt, driftet aber; der Beschleunigungssensor ist verrauscht, aber driftfrei.

Das Komplementärfilter

Für einen Neigungswinkel $\theta$ wird das integrierte Gyroskop (vertrauenswürdig bei hohen Frequenzen) mit dem Winkel des Beschleunigungssensors (vertrauenswürdig bei niedrigen Frequenzen) gemischt:

$${\hat{\theta }}_k=\alpha ({\hat{\theta }}_{k-1}+{\tilde{\omega }}_k\mathrm{\Delta }t)+(1-\alpha ){\theta }_{\mathrm{acc},k},{\theta }_{\mathrm{acc}}=\mathrm{atan}2({\tilde{a}}_y,{\tilde{a}}_z)$$

Im Frequenzbereich ist das ein Hochpass auf dem Gyroskop plus ein Tiefpass auf dem Beschleunigungssensor mit derselben Grenzfrequenz; die Übergangszeitkonstante ist

$$\tau =\frac{\alpha \mathrm{\Delta }t}{1-\alpha }$$

Rechenbeispiel. Bei $\mathrm{\Delta }t=5\mathrm{ms}$ von Strido ergibt die Wahl $\alpha =0.98$ den Wert $\tau =0.98\times 0.005/0.02\approx 0.25\mathrm{s}$: länger als die Standphase des Trots von $0.2\mathrm{s}$, sodass Schrittstöße größtenteils herausgefiltert werden, während die Gyroskopdrift innerhalb von etwa einer Sekunde korrigiert wird.

Das Mahony-Filter (das, was die Firmware ausführt)

Mahonys Filter ist ein Komplementärfilter auf der Rotationsgruppe und verwendet statt getrennter Winkel ein Quaternion $\hat{\mathbf{q}}$. Jeder Schritt:

1. Die Schwerkraftrichtung („oben“) aus der aktuellen Schätzung vorhersagen: $\hat{\mathbf{v}}={\hat{\mathbf{R}}}^{\mathrm{\top }}{\mathbf{e}}_z$.
2. Den Beschleunigungssensor normieren: $\overline{\mathbf{a}}=\tilde{\mathbf{a}}/\Vert \tilde{\mathbf{a}}\Vert$.
3. Der Fehler ist die Drehung, die nötig ist, um beide auszurichten:

$$\mathbf{e}=\overline{\mathbf{a}}\times \hat{\mathbf{v}}$$

4. Das Gyroskop mit proportionaler und integraler Rückführung korrigieren (der Integralterm konvergiert gegen den negativen Gyroskop-Bias und hebt ihn auf):

$${\mathbf{b}}_k={\mathbf{b}}_{k-1}+K_i\mathbf{e}\mathrm{\Delta }t,{\mathit{\omega }}_{\mathrm{c}}=\tilde{\mathit{\omega }}+K_p\mathbf{e}+{\mathbf{b}}_k$$

5. Die Quaternion-Kinematik $\dot{\mathbf{q}}=\frac{1}{2}\mathbf{q}\otimes (0,{\mathit{\omega }}_{\mathrm{c}})$ integrieren und erneut normieren:

$${\hat{\mathbf{q}}}_k=\frac{{\hat{\mathbf{q}}}_{k-1}+\frac{1}{2}{\hat{\mathbf{q}}}_{k-1}\otimes (0,{\mathit{\omega }}_{\mathrm{c}})\mathrm{\Delta }t}{\Vert \cdot \Vert }$$

$K_p$ legt den Übergang fest (ungefähr $\tau \approx 1/K_p$); $K_i$ entfernt einen konstanten Gyroskop-Bias. Das Gieren ist ohne Magnetometer nicht beobachtbar und driftet daher.

In der Firmware wird Adafruit_Mahony mit $1/\mathrm{\Delta }t=200\mathrm{Hz}$ gestartet und über updateIMU gespeist (nur Gyroskop und Beschleunigungssensor, kein Magnetometer). Vor dem Filtern werden die Platinenachsen auf das Basiskoordinatensystem umgelegt, da die IMU mit ihrer $x$-Achse nach links und ihrer $y$-Achse nach hinten montiert ist:

$$({\omega }_x,{\omega }_y,{\omega }_z)=(-{\tilde{\omega }}_y,{\tilde{\omega }}_x,{\tilde{\omega }}_z),(a_x,a_y,a_z)=(-{\tilde{a}}_y,{\tilde{a}}_x,{\tilde{a}}_z)$$

Der Sensor ist auf $\pm 8g$, $\pm {1000}^{\circ }/\mathrm{s}$ bei einer Ausgabedatenrate von $416\mathrm{Hz}$ konfiguriert. Das Repository behält die Standardverstärkungen der Bibliothek bei (Adafruit AHRS: $K_p=0.5$, $K_i=0$, d. h. keine Bias-Integration).

Das Kalman-Filter

Das Kalman-Filter ist der optimale Schätzer für lineare Systeme mit gaußschem Rauschen. Zustand $\mathbf{x}$ mit Kovarianz $\mathbf{P}$, Modell

$${\mathbf{x}}_k=\mathbf{F}{\mathbf{x}}_{k-1}+\mathbf{B}{\mathbf{u}}_k+{\mathbf{w}}_k,\mathbf{w}\sim \mathcal{N}(0,\mathbf{Q});{\mathbf{z}}_k=\mathbf{H}{\mathbf{x}}_k+{\mathbf{v}}_k,\mathbf{v}\sim \mathcal{N}(0,{\mathbf{R}}_{\mathrm{n}})$$

Prädiktion (Modell fortschreiben; die Unsicherheit wächst):

$${\hat{\mathbf{x}}}_{k\mid k-1}=\mathbf{F}{\hat{\mathbf{x}}}_{k-1}+\mathbf{B}{\mathbf{u}}_k,{\mathbf{P}}_{k\mid k-1}=\mathbf{F}{\mathbf{P}}_{k-1}{\mathbf{F}}^{\mathrm{\top }}+\mathbf{Q}$$

Korrektur (eine Messung fusionieren; die Unsicherheit schrumpft):

$${\mathbf{K}}_k={\mathbf{P}}_{k\mid k-1}{\mathbf{H}}^{\mathrm{\top }}{(\mathbf{H}{\mathbf{P}}_{k\mid k-1}{\mathbf{H}}^{\mathrm{\top }}+{\mathbf{R}}_{\mathrm{n}})}^{-1}$$$${\hat{\mathbf{x}}}_k={\hat{\mathbf{x}}}_{k\mid k-1}+{\mathbf{K}}_k({\mathbf{z}}_k-\mathbf{H}{\hat{\mathbf{x}}}_{k\mid k-1}),{\mathbf{P}}_k=(\mathbf{I}-{\mathbf{K}}_k\mathbf{H}){\mathbf{P}}_{k\mid k-1}$$

Die Verstärkung $\mathbf{K}$ gewichtet die Innovation $\mathbf{z}-\mathbf{H}\hat{\mathbf{x}}$ danach, wie sehr wir der Messung (${\mathbf{R}}_{\mathrm{n}}$) gegenüber der Prädiktion ($\mathbf{P}$) vertrauen. Ein Neigungsfilter mit Zustand $(\theta ,b_g)$, Gyroskop als Eingang und Beschleunigungswinkel als Messung konvergiert zu einem Komplementärfilter mit automatisch abgestimmtem $\alpha$ und Bias-Schätzung. Nichtlineare Systeme (Rotationen, Laufroboter) verwenden das erweiterte Kalman-Filter, das $\mathbf{F}$ und $\mathbf{H}$ um die aktuelle Schätzung linearisiert.

Bein-Odometrie

Rutscht ein Standfuß nicht, ist seine Weltposition fest, sodass sich die Körpergeschwindigkeit aus den Gelenkgeschwindigkeiten des Beins über die Vorwärtskinematik ergibt (siehe Kinematik):

$${\mathbf{v}}_{\mathrm{body}}=-(\mathbf{J}(\mathbf{q})\dot{\mathbf{q}}+\mathit{\omega }\times {\mathbf{p}}_{\mathrm{foot}})$$

gemittelt über die Standfüße und für die Position integriert. Fehler entstehen durch Rutschen, Kontakterkennung und Kalibrierung, daher driftet sie; moderne Vierbeiner fusionieren sie in einem EKF mit der IMU.

Die Hobby-Servos von Strido haben keine Positionsrückmeldung: /joint_states meldet die kommandierten Winkel nach der Begrenzung, keine gemessenen. Echte Bein-Odometrie ist daher nicht verfügbar, und gait_node.py macht etwas Einfacheres: Es integriert die kommandierte Körpergeschwindigkeit,

$$x_k=x_{k-1}+(v_x\cos \psi -v_y\sin \psi )\mathrm{\Delta }t,y_k=y_{k-1}+(v_x\sin \psi +v_y\cos \psi )\mathrm{\Delta }t,{\psi }_k={\psi }_{k-1}+{\omega }_z\mathrm{\Delta }t$$

ohne Rutschmodell. Erreicht der Roboter tatsächlich 80 % eines Kommandos von $0.30\mathrm{m}/\mathrm{s}$, überschätzt /odom die Distanz um $0.06\mathrm{m}$ pro Sekunde.

LiDAR-Scan-Matching und SLAM

Ein 2D-LiDAR liefert Entfernungen $r_j$ unter Winkeln ${\theta }_j$. Scan-Matching findet die starre Transformation $(\mathbf{R},\mathbf{t})$, die einen neuen Scan am besten auf einen vorherigen Scan oder eine Karte ausrichtet, z. B. mit ICP (Iterative Closest Point):

$$({\mathbf{R}}^{\star },{\mathbf{t}}^{\star })=\arg \underset{\mathbf{R},\mathbf{t}}{min}\sum _j{\Vert \mathbf{R}{\mathbf{p}}_j+\mathbf{t}-{\mathbf{m}}_{c(j)}\Vert }^2$$

wobei ${\mathbf{m}}_{c(j)}$ der nächstgelegene Kartenpunkt zu ${\mathbf{p}}_j$ ist. Das Verketten von Matches ergibt LiDAR-Odometrie, die ebenfalls driftet. SLAM (simultane Lokalisierung und Kartierung) beseitigt die Drift, indem es bereits gesehene Orte wiedererkennt (Loop Closure) und eine Posengraph-Optimierung über alle Posen gleichzeitig löst. Rad-/Bein-Odometrie ist ein nützlicher Startwert für den Matcher – genau die Rolle, die der Code-Kommentar dem /odom von Strido zuweist („ein Startwert für SLAM“).

Was Strido veröffentlicht

Topic	Inhalt	Quelle
/imu/data (sensor_msgs/Imu, Frame imu_link, Achsen an base_link ausgerichtet)	Orientierungsquaternion aus dem Mahony-Filter, Winkelgeschwindigkeit in $\mathrm{rad}/\mathrm{s}$, lineare Beschleunigung in $\mathrm{m}/{\mathrm{s}}^2$ (inklusive Schwerkraft); wird nicht veröffentlicht, solange die IMU einen Fehler meldet	mcu_bridge.py, Zustandsstrom mit $100\mathrm{Hz}$ (kStatePeriodMs = 10)
/odom + TF odom $\to$ base_link (nav_msgs/Odometry)	Koppelnavigation der kommandierten Geschwindigkeit	nur gait_node.py
/scan (sensor_msgs/LaserScan, lidar_link)	RPLIDAR-C1-Entfernungen mit $10\mathrm{Hz}$	sllidar_ros2

Die gelernte Policy verwendet nur /imu/data: das Gyroskop und die aus dem Quaternion abgeleitete Schwerkraftrichtung (siehe Reinforcement Learning).

Selbsttest

1. Warum kann ein Beschleunigungssensor Rollen und Nicken liefern, aber nicht Gieren?

   Antwort

   Er misst die Schwerkraftrichtung im Körperkoordinatensystem; eine Drehung um die Hochachse ändert diese Richtung nicht.

2. Welches $\alpha$ ergibt bei $\mathrm{\Delta }t=5\mathrm{ms}$ eine Zeitkonstante des Komplementärfilters von $0.5\mathrm{s}$?

   Antwort

   $\alpha /(1-\alpha )=100$, so $\alpha =100/101\approx 0.990$.

3. Was schätzt die Integralverstärkung $K_i$ im Mahony-Filter, und was passiert bei $K_i=0$?

   Antwort

   Den Gyroskop-Bias. Bei $K_i=0$ hinterlässt ein konstanter Bias einen kleinen bleibenden Roll-/Nickversatz von etwa $b_g/K_p$ und unkorrigierte Gierdrift.

4. Was passiert in der Kalman-Korrektur mit $\mathbf{K}$, wenn ${\mathbf{R}}_{\mathrm{n}}\to \mathrm{\infty }$?

   Antwort

   $\mathbf{K}\to 0$: Die Messung wird ignoriert und die Schätzung folgt der Prädiktion.

5. Warum kann Strido keine klassische Bein-Odometrie durchführen?

   Antwort

   Seine Servos liefern keine Rückmeldung der Gelenkposition, daher sind die tatsächlichen $\mathbf{q}$ und $\dot{\mathbf{q}}$ unbekannt; nur Kommandos sind verfügbar.

Weiterführende Literatur

• R. Mahony, T. Hamel und J.-M. Pflimlin, „Nonlinear Complementary Filters on the Special Orthogonal Group“, IEEE Transactions on Automatic Control, 2008.
• S. Thrun, W. Burgard und D. Fox, Probabilistic Robotics, MIT Press, 2005.
• J. Solà, „Quaternion kinematics for the error-state Kalman filter“, arXiv:1711.02508, 2017.
• M. Bloesch et al., „State Estimation for Legged Robots — Consistent Fusion of Leg Kinematics and IMU“, Robotics: Science and Systems, 2012.
• W. Hess, D. Kohler, H. Rapp und D. Andor, „Real-Time Loop Closure in 2D LIDAR SLAM“ (Cartographer), IEEE ICRA, 2016.