In diesem Artikel wird eine Methode vorgestellt, ein aus zwei Robotern bestehendes Gesamtsystem mit Simulink, Gazebo und dem Robot Operating System (ROS) zu entwerfen und zu simulieren. Für die Low-Level-Bewegungssteuerung und Sensorik werden die ROS-Pakete Husky und Kinova verwendet, was einen nahtlosen Übergang von der Simulation zur physischen Hardware gestattet, da die ROS-Befehle unverändert bleiben. Die Gazebo-Welt enthält zudem ein Co-Simulations-Plugin, das die direkte Kommunikation zwischen den Matlab- und Simulink-Produktfamilien und Gazebo ermöglicht, wodurch man beispielsweise die Gazebo-Welt abfragen oder Zustände von Links innerhalb der Welt definieren kann. In Gazebo stellen Links die physischen Körper in einer simulierten Umgebung dar. Die Zustände eines Links sind dessen Position und Orientierung sowie Linear- und Winkelgeschwindigkeit.

Der Workflow

Die simulierte Recyclinganlage mit dem mobilen Manipulator.

© Mathworks

In unserem Beispiel werden Simulink und Stateflow dazu genutzt, verschiedene Komponenten eines Pick-and-Place-Workflows mit einem mobilen Manipulator in einer simulierten Lagerhalle eines Recyclingbetriebs zu implementieren. Der Manipulator nimmt recyclebare Objekte von einem zentralen Förderband auf und transportiert sie zu den jeweiligen Recyclingstationen. Zur Untersuchung der Pick-and-Place-Komponenten lässt sich das Simulink-Modell über das Matlab- Befehlsfenster öffnen. Das Modell besteht aus folgenden Komponenten: Main Task Scheduler, Roboter-Manipulator, Mobiler Roboter.

Main Task Scheduler

Ein Stateflow-Diagramm steuert den Ablauf der Tasks, die der mobile Manipulator zur Ausführung der Pick-and-Place-Aufgabe abarbeiten muss. Es aktiviert die Tasks für den Robotermanipulator oder den mobilen Roboter in Abhängigkeit vom erforderlichen Arbeitsablauf. Der Arbeitsablauf besteht aus den folgenden Schritten:

• Der mobile Manipulator fährt zum Förderband, um ein Objekt für das Recycling aufzunehmen.
• Die RGB-D-Kamera am Roboterarm erkennt die Orientierungen und Arten der Objekte mit Deep Learning.
• Der Roboterarm hebt das erkannte Objekt auf.
• Der mobile Manipulator steuert den für den erkannten Objekttyp (Flasche oder Dose) geeigneten Recyclingbehälter an und legt das Objekt hinein.
• Der Roboter kehrt zum Förderband zurück und wiederholt diesen Arbeitsablauf, bis keine Gegenstände mehr erkannt werden.

Roboter-Manipulator

Der Main Task Schedule zeigt den Hauptarbeitsablauf für den mobilen Manipulator.

© Mathworks

Diese Komponente implementiert die Tasks, die der Main Task Scheduler dem Roboter-Manipulator zuweist. Sie umfasst den Roboterarm-Scheduler, das Wahrnehmungs-Subsystem und das Bewegungsplanungs-Subsystem.

Der Roboterarm-Scheduler innerhalb der Komponente für den Roboter-Manipulator enthält fünf Zustände: Idle, MoveToHome, Detect, PickObject und PlaceObject. Der Roboterarm befindet sich je nach Zuweisung eines Tasks durch den Main Task Scheduler stets in einem dieser Zustände. Um Konflikte auszuschließen, muss sich immer dann, wenn sich entweder der Roboterarm oder der mobile Roboter nicht im Idle-Zustand befindet, der jeweils andere im Idle befinden.

Das Wahrnehmungs-Subsystem ist ein ausgelöstes (triggered) Subsystem, welches das simulierte Bild der am Ende des Endeffektors angebrachten Kamera mithilfe eines vortrainierten Deep-Learning-Modells analysiert und so recycelbare Objekte erkennt. Das Deep-Learning-Modell verwendet das Kamerabild als Eingabe und gibt die 2D-Position des Objekts in Form von Pixelpositionen und den passenden Recyclingbehälter (blau oder grün) aus, in den es gelegt werden sollte. Die 2D-Position im Bild wird mithilfe von Informationen über Kameraeigenschaften, zum Beispiel Brennweite und Sichtfeld, Daten des Tiefensensors sowie der Vorwärtskinematik des Roboters auf die Roboterbasis als Referenz abgebildet. Der Main Task Scheduler des Robotermanipulators sendet ein Signal, das dieses Subsystem auslöst, sobald der Roboter während des Pick-and-Place-Workflows das nächste Objekt erkennen soll.

Das Subsystem für die Bewegungsplanung ist ein aktiviertes (enabled) Subsystem, das vom Robot Arm Scheduler das Signal erhält, eine Trajektorie zu erstellen, der der Robotermanipulator folgen soll. Das Subsystem sendet dem Joint Trajectory Controller in ROS den Befehl zur Verfolgung dieser Trajektorie mithilfe eines ROS/Publish-Blocks. Der Roboterarm-Scheduler aktiviert dieses Subsystem immer dann, wenn sich der Roboter-Manipulator während des Pick-and-Place-Workflows bewegen muss, und es bleibt aktiviert, bis der Roboter die gewünschte Zielposition erreicht hat.

Mobiler Roboter

Das Deep Learning-Modell kennzeichnet den Standort und die Recyclingklasse eines Objekts.

© Mathworks

Die Komponente ‚Mobiler Roboter‘ implementiert die Tasks, die der Main Task Scheduler dem mobilen Roboter zuweist. Sie besteht aus dem Mobile Robot-Scheduler, dem Pfadplanungs-Subsystem und dem Pfadverfolgungs-Subsystem.

Kartierung von Lagerhäusern

Ein Diagramm des Mobile Robot-Scheduler.

© Mathworks

Zur Erstellung einer Karte des Arbeitsbereichs muss die Umgebung mithilfe eines am Husky-Roboter montierten Lidar-Sensors gescannt werden. In diesem Fall wurde die Funktion buildmap zur Erzeugung einer Karte mit einem Occupancy Grid verwendet, indem der Roboter durch den Raum navigiert wurde. Der Mobile Robot Scheduler enthält drei Zustände: Idle, PlanPath und FollowPath. Standardmäßig befindet sich der mobile Roboter im Idle-Zustand, was bedeutet, dass die Systeme zur Pfadplanung und -verfolgung inaktiv sind. Der Mobile Robot Scheduler erhält Taskbefehle vom Main Task Scheduler. Empfängt er einen Tasks.Robot_Navigate-Task, wechselt er vom Idle-Zustand in den PlanPath-Zustand. Der Scheduler durchläuft dann mithilfe der Rückmeldungen der Pfadplanungs- und Pfadverfolgungs-Subsysteme die erforderlichen Schritte und kehrt schließlich in den Idle-Zustand zurück, sobald der Task absolviert wurde. Der Task wird daraufhin als inaktiv markiert und ein Signal an den Main Task Scheduler gesendet, das anzeigt, dass dieser mit Aufgaben auf höherer Ebene fortfahren kann. Der Prozess wiederholt sich mit jedem neuen Befehl durch den Main Task Scheduler.

Im Zustand PlanPath setzt der Mobile Robot-Scheduler taskActive auf true und aktiviert so das Pfadplanungs-Subsystem. Das Subsystem gibt dann eine Reihe von Wegpunkten sowie ein logisches Flag (isPath) zurück, das anzeigt, ob die Wegsuche erfolgreich war. Sobald der Mobile Robot-Scheduler den Wert true für isPath erhält, geht er in den Zustand FollowPath über.

Im Zustand FollowPath setzt der Mobile Robot Scheduler das Flag requestFollowPath auf true und aktiviert damit das Pfadverfolgungs-Subsystem. Das Pfadverfolgungs-Subsystem besteht aus drei Hauptkomponenten, die in der folgenden Reihenfolge ausgeführt werden:

• Bewegung mit Pure Pursuit steuern: Der Pure Pursuit-Block stellt die primäre Bewegungssteuerung dar. Er berechnet die Geschwindigkeit, die der Roboter benötigt, um den nächsten Wegpunkt zu erreichen, basierend auf der aktuellen Position des Roboters und dem bevorstehenden Wegpunkt.
• Verhalten je nach Abstand zum Ziel anpassen: Die Subsysteme Check Distance to Goal und Zero-Velocity at Goal sorgen dafür, dass der Roboter anhält, sobald er das Ziel erreicht hat. Die Ausgabe des Distance to Goal-Subsystems wird außerdem vom Modell zur Verifizierung verwendet, dass der Task abgeschlossen wurde.
• Senden von Befehlen an Gazebo über ROS: Die Blöcke Blank Message und Publish senden die vom Subsystem Zero-Velocity at Goal gesendeten Befehle bezüglich der Linear- und Winkelgeschwindigkeit an die jeweiligen Geschwindigkeitsregler des Roboters in Gazebo. Gemeinsam leiten diese Komponenten den Roboter durch die Gazebo-Welt. Sobald der Roboter das Ziel erreicht hat, gibt das Subsystem für atGoal den Wert true zurück und signalisiert dem Mobile Robot Scheduler, dass dieser Schritt abgeschlossen wurde.

Zuweisung der Variablen

Ein aus einem Matlab-File geladenes Manipulatormodell.

© Mathworks

Zur Ausführung des Simulink-Modells müssen zunächst einige Parameter für die Ausgangskonfigurationen und Target-Posen des Roboters eingestellt werden. In der vorliegenden Simulation wird ein an einem mobilen Husky-Roboter befestigter Kinova Gen3-Manipulator verwendet. Das Modell des Manipulators, das auch einen Greifer enthält, ist in einem MAT-File gespeichert.

Nachdem sowohl der Manipulatorarm als auch der mobile Roboter modelliert und die Initialisierungsvariablen gesetzt worden sind, kann das Gesamtsystem simuliert werden. Bevor das Simulink-Modell ausgeführt wird, müssen die Gazebo-Simulation, der ROS-Master und die Gazebo-Schnittstellenverbindungen gestartet werden.

Simulation zur Leistungsvalidierung

Der simulierte mobile Manipulator besteht aus einem Kinova Gen3-Manipulator auf einem mobilen Husky-Roboter.

© Mathworks

Simuliert man nun das Gesamtsystem in dieser realitätsgetreuen Umgebung, lässt sich dieses unter verschiedensten Bedingungen validieren. Dazu gehören Testszenarien für verschiedene Recyclinganlagen sowie Kombinationen aus unterschiedlichen Algorithmen, Sensoren und mechanischen Komponenten. Bei der Arbeit an dem vorgestellten Robotersystem zeigten erste Simulationen beispielsweise, dass der ursprünglich für die Objektidentifizierung verwendete Wahrnehmungs-Algorithmus nicht exakt genug war. Daraufhin wurde das ursprüngliche neuronale Netz mit einem größeren Bilddatensatz trainiert, der mithilfe von Methoden zur synthetischen Erweiterung (Data Augmentation) gewonnen wurde, bis der gewünschte Genauigkeitsgrad erreicht war. Daneben wurde die Simulation für What-if-Studien verwendet, bei denen verschiedene Pfadplanungs-Algorithmen simuliert und deren Ergebnisse im Hinblick auf die Ausführungszeit und die Fähigkeit der einzelnen Algorithmen zur Berechnung optimaler Pfade verglichen wurden. Dieser Ansatz führte zur Auswahl des RRTStar-Pfadplanungs-Algorithmus, der bessere Ergebnisse lieferte als AStar, PRM, Dstar und andere getestete Algorithmen. Sobald die Simulationsergebnisse bestätigten, dass die aktuelle Version des Robotersystems die gewünschte Zuverlässigkeit erreichte, konnte mit der Codegenerierung fortgefahren werden. Dadurch wurde der Algorithmus zunächst in Code zur Ausführung auf Prototypen für die Echtzeit-Validierung und schließlich für das abschließend ausgelieferte Produkt umgewandelt.

Die Autoren:

Marco Roggero ist Senior Application Engineer bei Mathworks in Aachen.
 
Anastasia Mavrommati ist Senior Robotics Research Scientist bei Mathworks in Natick, MA, USA.
 
Roberto Valenti ist Senior Robotics Research Scientist bei Mathworks in Natick, MA, USA.
 
Hannes Daepp ist Senior Team Lead, Robotics & UAV Applications bei Mathworks in Natick, MA, USA.