Damit der Rover selbstständig fahren kann, muss er seine Umgebung sehen. In Beitrag #1 hatte ich noch ein 2D-Lidar im Kopf, später habe ich mit einer einfachen USB-Kamera (ORB-SLAM2) und einer alten Kinect experimentiert. Gelandet bin ich bei einer Intel RealSense D435i. In diesem Beitrag geht es um Montage, Treiber auf dem Jetson Nano und Kalibrierung. Gerade die Kalibrierung hat sich als wichtiger herausgestellt, als ich dachte.
Warum die D435i?
Die D435i ist eine Stereo-Tiefenkamera mit Farbkamera und eingebautem Inertialsensor (Beschleunigung und Drehrate). Für den Rover bringt das drei Dinge auf einmal:
- Tiefenbild: Für jeden Bildpunkt die Entfernung, damit erkennt der Rover Hindernisse und baut eine 3D-Karte.
- Farbbild: zum Mitschauen über WLAN und für die Wiedererkennung von Orten beim Kartieren.
- Kreisel (Gyro): misst die Drehrate des Rovers deutlich genauer als die Räder. Mehr dazu unten.
Im Gegensatz zum 2D-Lidar sieht die Kamera auch Tischplatten, Stuhlbeine und Absätze. Und anders als die Kinect funktioniert sie auch draußen.
Montage
Die Kamera sitzt bei mir auf einer Platte am Mast, mittig über dem Rover und mit Blick nach vorn. Ein paar Punkte, die ich beim nächsten Mal gleich beachten würde:
- Höhe: Meine Linsenmitte liegt knapp 47 cm über dem Boden. Bei ±21 Grad vertikalem Blickfeld sieht die Kamera den Boden erst ab etwa 1,2 m vor dem Rover. Was direkt davor liegt, kennt der Rover nur aus der Karte. Wer flache Hindernisse direkt vor dem Rover erkennen will, montiert die Kamera tiefer oder leicht nach unten geneigt.
- Genau vermessen: Die Position der Kamera relativ zur Rover-Mitte steht im 3D-Modell (URDF). Jeder Fehler dort verschiebt die ganze Karte. Bei mir saß die Kamera in Wirklichkeit 5,7 cm weiter links als im Modell. Ergebnis: Alle Hindernisse lagen 5,7 cm daneben, und der Rover fuhr knapp an der Türmitte vorbei. Also mit dem Maßband nachmessen: seitlicher Versatz, Höhe der Linsenmitte über dem Boden, Abstand zur Rover-Mitte.
- USB 3: Die Kamera braucht einen USB-3-Port und ein ordentliches USB-3-Kabel. Über USB 2 laufen nur reduzierte Auflösungen.
[Bild: Kamerahalterung am Mast]
Treiber auf dem Jetson Nano
Das war der aufwendigste Teil. Zwei Bausteine sind nötig:
- librealsense, die Bibliothek von Intel. Für den Jetson baut man sie am besten aus dem Quellcode mit dem sogenannten RSUSB-Backend. Dann braucht es keine Kernel-Patches. Die fertigen Skripte von JetsonHacks nehmen einem die Arbeit ab: https://github.com/JetsonHacks/installRealSenseSDK (Skript
buildLibrealsense.sh). Bei mir läuft librealsense 2.56.5. Der Bau dauert auf dem Nano gut eine Stunde. - realsense-ros, der ROS-Treiber. Für ROS 1 gibt es ihn nur noch im Zweig
ros1-legacy, letzte Version 2.3.2: https://github.com/IntelRealSense/realsense-ros/tree/ros1-legacy. Den Ordnerrealsense2_camerain den Workspace klonen und mitcatkin buildbauen. Er braucht zusätzlichddynamic_reconfigure.
Die OpenCV-Falle
Hier habe ich viel Zeit verloren. JetPack bringt OpenCV 4.1 mit, die ROS-Melodic-Pakete (z. B. cv_bridge und die Bildkompression) sind aber gegen OpenCV 3.2 gebaut. Baut man den RealSense-Treiber gegen OpenCV 4, laufen im selben Prozess zwei OpenCV-Versionen. Das geht so lange gut, bis jemand das komprimierte Kamerabild abonniert, also genau das, was man fürs WLAN braucht.
Meine Lösung war, auf dem Jetson nur OpenCV 3.2 zu verwenden:
sudo apt install libopencv-dev=3.2.0+dfsg-4ubuntu0.1
apt entfernt dabei NVIDIAs OpenCV 4.1. Danach den RealSense-Treiber neu bauen. Prüfen lässt sich das mit ldd auf die gebaute Bibliothek: Es darf nur noch libopencv_*.so.3.2 auftauchen. Die NVIDIA-Beispiele für VisionWorks und VPI fallen dabei weg, für den Rover braucht man sie nicht.
Startparameter
Der Nano ist kein Kraftpaket. Ich starte die Kamera mit reduzierten Einstellungen:
<include file="$(find realsense2_camera)/launch/rs_camera.launch">
<arg name="align_depth" value="true"/> <!-- Tiefe passend zum Farbbild -->
<arg name="enable_gyro" value="true"/>
<arg name="enable_accel" value="true"/>
<arg name="unite_imu_method" value="copy"/>
<arg name="depth_width" value="640"/> <arg name="depth_height" value="480"/>
<arg name="color_width" value="640"/> <arg name="color_height" value="480"/>
<arg name="depth_fps" value="15"/> <arg name="color_fps" value="15"/>
<arg name="enable_pointcloud" value="false"/> <!-- kostet viel CPU -->
<arg name="enable_infra1" value="false"/> <arg name="enable_infra2" value="false"/>
<arg name="tf_publish_rate" value="0.0"/> <!-- interne TFs statisch -->
</include>
Die Punktwolke rechne ich nicht im Kameratreiber, sondern später gezielt und ausgedünnt nur für die Hinderniserkennung.
Bilder übers WLAN
Das rohe Farbbild hat bei 15 Bildern pro Sekunde knapp 15 MB/s, das ist zu viel fürs WLAN. Als JPEG komprimiert sind es nur 0,8 MB/s. Das Tiefenbild lässt sich dagegen kaum sinnvoll übertragen: Die verlustfreie PNG-Kompression schafft der Nano nur mit 0,8 Bildern pro Sekunde. Deshalb verarbeitet der Jetson die Tiefe selbst (Hinderniserkennung). Zur Karte auf dem Server gehen nur gedrosselte Bildpaare mit 2 Hz. Dazu mehr in Beitrag #7.
Kalibrierung
1. Tiefenmaßstab (Tare)
Meine Kamera hat alle Entfernungen um 4 % zu kurz gemessen. Das klingt wenig, sorgt aber dafür, dass der Boden nach vorn scheinbar ansteigt und weiter weg plötzlich als Hindernis gilt. Aufgefallen ist es mir mit einem Schachbrett-Ausdruck: Aus dem Farbbild lässt sich die Entfernung zum Brett sehr genau berechnen (bei mir auf 1 mm genau wie mit dem Maßband), und die Tiefenkamera lag daneben.
Intel hat dafür die Tare-Kalibrierung: Man stellt ein ebenes Ziel in bekanntem Abstand senkrecht vor die Kamera, gibt den Abstand ein, und die Kamera korrigiert sich selbst. Das geht mit dem Depth Quality Tool von Intel oder, wie bei mir auf dem Jetson, mit einem kleinen Programm über die librealsense-Schnittstelle. Vorher die alte Kalibrierung sichern! Danach lag der Fehler unter 1 %.
2. Einbaulage
Auch kleine Neigungen der Kamera machen sich bemerkbar: 3 Grad seitlich, und der Boden gilt auf einer Seite als Hindernis. Ich bestimme die Neigung aus dem Tiefenbild: Die Kamera schaut auf einen freien, ebenen Boden, eine Ebene wird durch die Bodenpunkte gelegt, und aus deren Lage ergeben sich Höhe und Neigung der Kamera. Die Werte trage ich ins URDF ein (Gelenk camera_joint). Danach lag die gemessene Linsenhöhe nur 2 mm neben dem Maßband.
3. Kreisel
Die Drehrate liefert bei mir nur noch der Kreisel der Kamera, aus der Rad-Odometrie kommt nur die Vorwärtsgeschwindigkeit. Die Räder rutschen beim Lenken und Drehen, der Kreisel nicht. Zusammengeführt wird beides in einem Kalman-Filter (robot_localization).
Zwei Stolpersteine:
- Der Kreisel hat eine kleine Nullpunktabweichung (bei mir etwa 3,4 Grad pro Minute). Ein kleiner Knoten in meiner Version schätzt sie, solange der Rover steht, und zieht sie ab.
robot_localizationerwartet die Auswahl der Messwerte (imu0_config) im Koordinatensystem des Sensors. Bei der Kamera zeigt dort die z-Achse nach vorn und die y-Achse nach unten. Die Drehung um die Hochachse ist also die Drehung um y, nicht um z. Mit der falschen Auswahl ignoriert der Filter den Kreisel einfach.
Wie geht es weiter?
Der Rover sieht jetzt seine Umgebung und weiß recht genau, wie er sich bewegt. Im nächsten Beitrag wird daraus eine Karte, und der Rover fährt selbstständig zu einem Ziel, das man im Browser anklickt.
[Bild: Kamerabild und Tiefenbild nebeneinander]