Sensoren

Über Sensoren weiß der Roboter überhaupt etwas über sich und seine Umgebung. Hobby-Servos melden nichts zurück, daher setzt Strido auf vier Sensoren: eine IMU für die Körperlage, ein 2D-LiDAR für Abstände, eine Weitwinkelkamera für Bilder und einen Power-Monitor für den Akku. Diese Seite erklärt, wie jeder davon funktioniert, wo er sitzt und welche Software seine Daten nutzt.

In Strido

Sensor	Stücklistenzeile	Angeschlossen an	ROS-2-Topic	Frame
Adafruit LSM6DSOX 6-DoF IMU (4438)	lsm6dsox	XIAO, I²C 0x6A	/imu/data	imu_link
Slamtec RPLIDAR C1 + USB-Adapter	rplidar_c1	Pi USB, /dev/rplidar	/scan	lidar_link
Raspberry Pi Camera Module 3 Wide	camera_module_3_wide	Pi CSI, FFC 22→15 Pin	/camera/image_raw	camera_optical_frame
Adafruit INA260 Power-Monitor (4226)	ina260	XIAO, I²C 0x41	/battery_state	—

Sichtfeldprüfungen: docs/verification/sensors.md. Firmware-Einrichtung: firmware/strido_esp32s3/src/main.cpp.

IMU: LSM6DSOX Beschleunigungssensor und Gyroskop

Eine IMU (inertiale Messeinheit) misst Bewegung von innen, ohne externe Referenz. Der LSM6DSOX ist eine 6-DoF-IMU: zwei 3-Achsen-MEMS-Sensoren in einem Chip.

Sensor	Misst	Einheit	Einstellung in Strido
Beschleunigungssensor	spezifische Kraft: Schwerkraft plus jede Beschleunigung	m/s² (g)	Bereich ±8 g, Ausgabedatenrate 416 Hz
Gyroskop	Winkelgeschwindigkeit um jede Achse	rad/s (°/s)	Bereich ±1000 °/s, Ausgabedatenrate 416 Hz

Die Firmware liest beide alle 5 ms (200 Hz). Da der Chip mit 416 Hz läuft, liefert jeder Lesevorgang einen frischen Messwert.

Montage und Achsen

Die IMU sitzt in einer Tasche im electronics_deck, Chipseite oben, die STEMMA-QT-Buchsen nach hinten, gehalten von zwei M2.5 × 6 Schrauben. So montiert zeigt ihre x-Achse zur linken Seite des Roboters und ihre y-Achse nach hinten. Die Firmware dreht jeden Messwert in die base_link-Konvention des Roboters (x nach vorne, y nach links, z nach oben, REP-103):

$${\left(\begin{array}{c}x\\ y\\ z\end{array}\right)}_{\mathrm{base}}=\underset{{\mathbf{R}}_{\mathrm{base},\mathrm{imu}}}{\underbrace{\left(\begin{array}{ccc}0& -1& 0\\ 1& 0& 0\\ 0& 0& 1\end{array}\right)}}{\left(\begin{array}{c}x\\ y\\ z\end{array}\right)}_{\mathrm{imu}}$$

also $x_{\mathrm{base}}=-y_{\mathrm{imu}}$, $y_{\mathrm{base}}=x_{\mathrm{imu}}$, $z_{\mathrm{base}}=z_{\mathrm{imu}}$. Die Matrix ${\mathbf{R}}_{\mathrm{base},\mathrm{imu}}$ ist eine Drehung um $+{90}^{\circ }$ um $z$; dieselbe Matrix wird auf die Vektoren von Beschleunigungssensor und Gyroskop angewendet. Rotationsmatrizen werden unter Koordinatensysteme und Rotationen erklärt.

Ein falsches Vorzeichen ließe den Roboter glauben, er kippe in die andere Richtung – der klassische erste Fehler in jedem Balance-Controller.

Von zwei Sensoren zu einer Orientierung

Keiner der beiden Sensoren allein liefert eine gute Orientierung:

• Der Beschleunigungssensor misst in Ruhe nur die Schwerkraft, daher folgen Roll- und Nickwinkel direkt daraus, wohin „unten“ zeigt. Beim Gehen fügt jeder Schritt Beschleunigung hinzu, daher ist diese Schätzung verrauscht.
• Das Gyroskop ist glatt und schnell. Die Integration der Winkelgeschwindigkeit über die Zeit ergibt die Winkeländerung, doch jeder kleine Offset integriert sich zu einer stetig wachsenden Drift.

Sensorfusion kombiniert beide: Über kurze Zeiten dem Gyroskop vertrauen und den Beschleunigungssensor die Schätzung langsam zurück zum wahren „unten“ ziehen lassen. Die einfachste Form ist der Komplementärfilter für einen Winkel $\theta$ (etwa den Nickwinkel), aktualisiert in jedem Zeitschritt $\mathrm{\Delta }t$:

$${\hat{\theta }}_k=\alpha ({\hat{\theta }}_{k-1}+{\omega }_k\mathrm{\Delta }t)+(1-\alpha ){\theta }_{\mathrm{acc},k},{\theta }_{\mathrm{acc}}=\mathrm{atan}2(-a_x,\sqrt{a_y^2+a_z^2})$$

Der erste Term integriert die Gyro-Rate $\omega$; der zweite ist die Neigung, berechnet aus der Schwerkraftrichtung im Messwert $\mathbf{a}$ des Beschleunigungssensors. Mit $\alpha$ nahe 1 ist der Filter ein Hochpass für das Gyroskop und ein Tiefpass für den Beschleunigungssensor, daher der Name: Die beiden Filter ergänzen sich zu eins. Die Übergangszeitkonstante ist $\tau =\alpha \mathrm{\Delta }t/(1-\alpha )$; zum Beispiel ergibt $\alpha =0.98$ bei $\mathrm{\Delta }t=5\mathrm{ms}$ den Wert $\tau \approx 0.25\mathrm{s}$, sodass Gyro-Drift innerhalb etwa einer Viertelsekunde korrigiert wird, während kürzere Erschütterungen durch Schritte geglättet werden.

Strido betreibt auf dem XIAO einen Mahony-Filter (Adafruit_Mahony) mit 200 Hz, die 3D-Quaternionen-Form derselben Idee. Er integriert das Gyroskop und führt den Fehler $\mathbf{e}={\mathbf{g}}_{\mathrm{meas}}\times \hat{\mathbf{g}}$ zwischen vorhergesagter und gemessener Schwerkraftrichtung als Korrektur auf die Rate zurück:

$${\mathit{\omega }}_{\mathrm{corr}}={\mathit{\omega }}_{\mathrm{gyro}}+K_{\mathrm{p}}\mathbf{e}+K_{\mathrm{i}}\int \mathbf{e}\mathrm{d}t$$

Die Proportionalverstärkung $K_{\mathrm{p}}$ legt fest, wie stark der Beschleunigungssensor zieht, und die optionale Integralverstärkung $K_{\mathrm{i}}$ kann einen konstanten Gyro-Offset lernen. Die Firmware nutzt die Standardwerte der Bibliothek: $K_{\mathrm{p}}=0.5$ und $K_{\mathrm{i}}=0$, also kein Lernen des Offsets. State Estimation leitet beide Filter her und vergleicht sie mit dem Kalman-Filter.

Das Ergebnis ist ein Quaternion, eine Darstellung der 3D-Orientierung mit vier Zahlen ohne Gimbal-Lock-Probleme. Es wird im STATE-Frame mit 100 Hz übertragen und zusammen mit den Roh-Gyro- und Beschleunigungswerten als /imu/data veröffentlicht.

Der Gierwinkel ist nicht beobachtbar

Die Schwerkraft liefert Roll- und Nickwinkel, sagt aber nichts über die Ausrichtung (Gierwinkel). Ohne Magnetometer driftet die Gierwinkel-Schätzung der IMU. Für die Ausrichtung über längere Zeit verwenden Sie das LiDAR (Scan-Matching / SLAM).

Wer sie nutzt

• policy_node (gelernter Controller) wandelt das Quaternion in die Schwerkraftrichtung im Körperkoordinatensystem um und speist sie zusammen mit den Gyro-Raten in jedem 50-Hz-Schritt in die Observation der Policy ein. Er startet erst, wenn er IMU-Daten gesehen hat.
• strido-cli monitor zeigt Roll- und Nickwinkel am Prüfstand.
• mcu_bridge veröffentlicht nie eine vorgetäuschte aufrechte Orientierung: Meldet die IMU einen Fehler (imu_ok false), wird /imu/data einfach nicht veröffentlicht.

LiDAR: RPLIDAR C1

Ein 2D-LiDAR dreht einen Laser-Entfernungsmesser und misst unter jedem Winkel in einer horizontalen Ebene den Abstand zum nächsten Objekt. Der C1 misst die Laufzeit (DTOF): Er misst, wie lange ein 905-nm-Infrarot-Laserpuls für den Rückweg braucht.

Spezifikation (Slamtec-Datenblatt)	Wert
Reichweite	0,05–12 m (weißes Ziel), 0,05–6 m (schwarzes Ziel)
Abtastrate	5000 Messpunkte/s
Scanfrequenz	typisch 10 Hz (8–12 Hz)
Winkelauflösung	0,72° bei 10 Hz
Genauigkeit	±30 mm
Schnittstelle	3,3 V TTL-UART, 460 800 Baud (hier über einen USB-Adapter)
Versorgung	5 V, etwa 230 mA im Betrieb, etwa 800 mA beim Anlauf
Lasersicherheit	IEC 60825 Klasse 1

Die Zahlen passen zusammen: ${\displaystyle \frac{5000\mathrm{samples}/\mathrm{s}}{10\mathrm{scans}/\mathrm{s}}}=500$ Punkte pro Scan und ${360}^{\circ }/500={0.72}^{\circ }$ zwischen den Punkten. Die Laufzeit ergibt den Abstand als $d=c\mathrm{\Delta }t/2$ (das Licht läuft hin und zurück); bei der maximalen Reichweite von 12 m dauert der Hin- und Rückweg nur $\mathrm{\Delta }t=2d/c\approx 80\mathrm{ns}$.

Platzierung

Ein LiDAR sieht nichts unterhalb oder oberhalb seiner Scanebene, und alles am Roboter selbst, was die Ebene kreuzt, blendet einen Teil des Scans aus. Strido hebt den C1 auf einem 14 mm hohen lidar_pedestal auf top_shell_sensor an. Seine Scanebene liegt 71,6 mm über den Hüftachsen, im Stand bei 170 mm etwa 254 mm über dem Boden. Die Raycast-Prüfung in docs/verification/sensors.md blockiert in jeder Pose 0 von 720 Teststrahlen: Stand, Hocke, Scheitel der Trot-Schwungphase, maximale Schrittweite und abgespreizte Hüften.

Wer sie nutzt

sllidar_ros2 veröffentlicht /scan (sensor_msgs/LaserScan) mit 10 Hz in lidar_link. Das Topic /scan folgt den Go2-Konventionen, daher laufen Standard-ROS-2-Werkzeuge unverändert: SLAM zum Kartenaufbau, Nav2 für Hindernisvermeidung und Pfadplanung. Das ist wichtig, weil /odom von gait_node nur die befohlene Geschwindigkeit integriert (Koppelnavigation, kein Schlupfmodell). Für echte Navigation sollte eine LiDAR-basierte Odometrie- oder SLAM-Quelle sie ersetzen (Docstring von gait_node.py).

Kamera: Camera Module 3 Wide

Spezifikation (Raspberry-Pi-Produktbeschreibung)	Wert
Sensor	Sony IMX708, 11,9 MP (4608 × 2592), rückseitig belichtet, gestapelt
Objektiv	2,75 mm, F2.2
Sichtfeld	120° diagonal, 102° horizontal, 67° vertikal
Fokus	Phasenerkennungs-Autofokus, 5 cm bis unendlich
Videomodi	1080p50, 720p100, 480p120
Schnittstelle	MIPI CSI-2, 15-Pin-FPC 1 mm

Die Wide-Variante wurde gewählt, weil ein kleiner, bodennaher Roboter mit 102° horizontalem Sichtfeld mehr von seiner Umgebung sieht als mit den 66° des Standardmoduls.

Platzierung

Die Kamera sitzt auf einer neigbaren Halterung (camera_mount_base und camera_tilt_plate) auf der flachen Oberseite der Frontschale. Ein M3 × 40 Drehbolzen mit Sicherungsmutter ergibt eine Reibungsneigung. Im ersten Entwurf saß die Kamera auf der Oberschale, wo die Raycast-Prüfung ergab, dass die eigene Nase des Roboters 42 % des Bilds füllte. Die neue Position zeigt in jeder geprüften Pose 0 % (docs/design.md, docs/verification/sensors.md).

Wer sie nutzt

camera_ros veröffentlicht /camera/image_raw in camera_optical_frame. Auf dem Pi 5 benötigt die Kamera den Raspberry-Pi-Fork von libcamera, daher wird der Node auf dem Host gebaut und mit camera:=true aktiviert (Standard false; das Docker-Image hat keinen Kamera-Node). Die Kamera ist für Ihre eigenen Wahrnehmungs-Nodes da: Video für die Fernsteuerung, Objekterkennung, visuelle Navigation.

Power-Monitor: INA260

Der INA260 misst Spannung und Strom mit einem eingebauten 2-mΩ-Shunt-Widerstand, 1,25 mA Stromauflösung, bis 15 A und 36 V. In Strido sitzt er High-Side im Logikzweig (siehe Stromversorgung):

• Busspannung = Akkuspannung (abzüglich winziger Verluste). Die Firmware liest sie alle 100 ms. Sie warnt bei 6,8 V und entspannt die Servos nach 5 aufeinanderfolgenden Messwerten unter 6,4 V.
• Strom = nur Strom des Logikzweigs. Der Servostrom fließt an ihm vorbei.

mcu_bridge veröffentlicht beides als /battery_state (sensor_msgs/BatteryState, Technologie LiPo, negativer Strom = Entladen). Die Firmware verweigert ohne ihn die Ansteuerung der Servos, weil ein Roboter ohne Spannungssensor seinen LiPo bis zur Schädigung entladen kann.

Zusammenfassung: vom Sensor zur Software

Sensor	Gelesen von	Rate	Software, die ihn nutzt
LSM6DSOX	XIAO, I²C	200 Hz, veröffentlicht mit 100 Hz	Mahony-Filter → /imu/data → policy_node, Überwachung
INA260	XIAO, I²C	10 Hz	Unterspannungswarnung und -abschaltung, /battery_state
RPLIDAR C1	Pi, USB seriell	10 Hz Scans	/scan → SLAM, Nav2, Hindernisvermeidung
Camera Module 3 Wide	Pi, CSI	Videorate	/camera/image_raw → Ihre Wahrnehmungs-Nodes

Selbsttest

1. Warum liefert der Beschleunigungssensor allein keine zuverlässige Orientierung, während der Roboter trabt?
2. Was bestimmt im Mahony-Filter die Proportionalverstärkung, und was würde eine Integralverstärkung ungleich null hinzufügen?
3. Wie viele Punkte enthält ein C1-Scan bei 10 Hz, und wie groß ist ihr Winkelabstand?
4. Warum wurde die Kamera von der Oberschale auf die Frontschale verlegt?
5. Welche Ausrichtungsinformation kann die IMU nicht liefern, und welcher Sensor schließt die Lücke?

Antworten

1. In Bewegung misst er zusätzlich zur Schwerkraft die Beschleunigungen der Schritte, daher ist die gemeldete Richtung „unten“ verrauscht.
2. Die Proportionalverstärkung bestimmt, wie stark die Schwerkraftrichtung des Beschleunigungssensors die vom Gyroskop integrierte Schätzung zurückzieht. Eine Integralverstärkung ungleich null würde zusätzlich einen konstanten Gyro-Offset lernen; Strido belässt sie bei 0.
3. $5000/10=500$ Punkte, ${0.72}^{\circ }$ voneinander entfernt.
4. Auf der Oberschale füllte die Nase des Roboters 42 % des Bilds; auf der Frontschale füllt sie 0 %.
5. Der Gierwinkel (Ausrichtung) ist aus der Schwerkraft nicht beobachtbar und driftet; das LiDAR (Scan-Matching oder SLAM) liefert ihn.

Weiterführende Literatur

• LSM6DSOX-Produktseite (STMicroelectronics)
• Adafruit-LSM6DSOX-Anleitung
• RPLIDAR-C1-Datenblatt (Slamtec)
• Produktbeschreibung Camera Module 3 (Raspberry Pi)
• Open-Source-Algorithmen für IMU und AHRS (x-io, inkl. Mahony)
• Verwandte Seiten: Busse, Stromversorgung, ROS 2, Kinematik und Gaits
• Theorie: Koordinatensysteme und Rotationen, State Estimation