Von cmd_vel zu Raddrehzahlen: ROS 2 Differenzialantrieb

Einführung

Alles oberhalb der Räder in einem ROS-2-Roboter – Teleop, Navigation, dein eigener Dockingregler – spricht dieselbe Sprache: eine geometry_msgs/msg/Twist auf /cmd_vel, die sagt „fahre so schnell vorwärts und drehe so schnell“. Alles unterhalb der Räder spricht PWM-Tastverhältnisse.

Diese Anleitung ist das Stück dazwischen, und es ist kurz genug, um trivial zu wirken. Zwei Zeilen Algebra wandeln eines ins andere um. Die anderen 90 % eines arbeitenden Antriebsknotens sind das, was diese zwei Zeilen falsch machen, wenn der Roboter etwas verlangt bekommt, das er nicht liefern kann.

Was ein Twist wirklich sagt

Nur zwei seiner sechs Zahlen zählen für einen Differenzialroboter:

• linear.x – Vorwärtsgeschwindigkeit in Metern pro Sekunde, entlang der Roboternase.
• angular.z – Gierrate in Radiant pro Sekunde, positiv gegen den Uhrzeigersinn von oben gesehen.

Die anderen vier beschreiben Bewegung, die ein Zweiradroboter nicht kann. linear.y ist seitwärts, was nur eine holonome Basis kann; ignoriere es statt es zu approximieren. Tausche die zwei Zeilen unten gegen die vierzeilige Mecanum-Matrix aus, bekommt linear.y Bedeutung. Beide Werte stehen im robotereigenen base_link-Rahmen und beide in SI-Einheiten – ein Roboter, der linear.x als Prozent nimmt, wirkt richtig, bis er auf einen echten Navigationsstack trifft.

Die zwei Zeilen Kinematik

Sei b der Radabstand und v, ω die kommandierte Linear- und Winkelgeschwindigkeit. Jede Rad-Bodengeschwindigkeit ist die Vorwärtsgeschwindigkeit des Roboters plus oder minus Drehanteil:

v_left  = v − ω · b / 2
v_right = v + ω · b / 2

Wandle jede per Division durch den Radradius r in eine Radwinkelgeschwindigkeit um:

ω_wheel = v_wheel / r        [rad/s]

Das ist die ganze Umwandlung. Drehen auf der Stelle ist v = 0, ergibt zwei gleiche, entgegengesetzte Raddrehzahlen; Geradeausfahrt ist ω = 0, ergibt zwei identische. Das Verhältnis dazwischen fixiert den Kurvenradius R = v / ω, und dieses Verhältnis zu erhalten ist der Rest dieser Anleitung.

Miss b durch Fahren, nicht mit dem Lineal

Der Radabstand ist der Parameter, von dem alles andere erbt, und der Abstand zwischen deinen Radmitten ist es nicht.

Die Reifen sind nachgiebig, sie berühren den Boden über eine Fläche statt einer Linie, und beide schrubben seitwärts durch jede Kurve. Der Wert, der die Gleichungen wahr macht – die effektive Spurweite –, liegt typisch wenige Prozent neben dem physischen und immer in die Richtung, die deinen Roboter unterdrehen lässt.

Kalibriere ihn direkt:

1. Kommandiere reine Rotation, v = 0, ω = 1,0 rad/s, für exakt 10 Sekunden.
2. Der Roboter sollte 10 Radiant gedreht haben – eine volle Umdrehung plus etwa 213°. Markiere die Startausrichtung und miss, wo er wirklich stoppte.
3. Skaliere: b_neu = b_alt × (gedreht / kommandiert).

Wiederhole in beiden Richtungen und mittle, weil ein asymmetrischer Antrieb je Richtung andere Antworten gibt. Zwei Iterationen reichen meist. Dieselbe Zahl nutzt deine Odometrie zur Positionsintegration, sie hier zu kalibrieren fixt also zwei Dinge zugleich.

Sättigung behandeln, ohne die Kurve zu ändern

Ein Motor hat eine Höchstdrehzahl. Fordere v = 0,5 m/s mit harter Kurve auf einem Roboter, dessen Räder bei 0,55 m/s enden, landet ein Radziel über dem Machbaren.

Die naheliegende Antwort ist, dieses Rad zu klemmen. Tu es nicht. Ein Rad zu klemmen ändert die Differenz zwischen den Rädern, und die Differenz ist die Kurve – der Roboter fährt also leise einen weiteren Bogen als kommandiert. Ein Pfadfolger, der eine bestimmte Kurve verlangt, bekommt eine andere zurück, korrigiert, sättigt wieder und pendelt.

Skaliere stattdessen beide Räder um denselben Faktor. Das Verhältnis überlebt, also überlebt der Bogen; der Roboter durchfährt ihn nur langsamer:

peak = max(abs(left), abs(right))
if peak > MAX_WHEEL_SPEED:
    scale = MAX_WHEEL_SPEED / peak
    left *= scale
    right *= scale

Drei Zeilen, und sie wandeln „falsche Form“ in „richtige Form, langsamer“ – fast immer der Tausch, den du auf einem Roboter willst, der etwas folgt.

Kenne die Hüllkurve, die du wirklich hast

Skalieren rettet den Bogen, aber es lohnt sich, vorab zu wissen, welche Kommandos es auslösen, weil diese Grenze eine Eigenschaft deines Roboters ist und man sie einem Planer sagen kann.

Setze v_left oder v_right gleich der Radhöchstgeschwindigkeit und löse auf. Die größte verfügbare Gierrate bei gegebener Vorwärtsgeschwindigkeit ist:

w_max(v) = 2 * (V_max - |v|) / b

Für den Roboter im Code unten – b = 0,152 m, Räder mit Spitze 0,55 m/s:

Die Form ist das Nützliche: Geschwindigkeit und Drehrate tauschen linear, und die letzten 10 % Vorwärtsgeschwindigkeit kosten fast deine gesamte Lenkung. Ein mit 0,5 m/s kommandierter Roboter hat 12 % der Gierautorität wie im Stand – genau darum sättigt ein Pfadfolger, der sättigt, gern in einer Kurve.

Zwei Konsequenzen zum Handeln. Lege deine echten Grenzen in den Planer statt ihn durch Sättigung entdecken zu lassen – in Nav2 sind das max_vel_x und max_vel_theta, und max_vel_x auf volle 0,55 m/s zu setzen garantiert, dass der Regler Bögen verlangt, die der Roboter nicht fahren kann. Und beim Abstimmen von Pure Pursuit denke daran: Eine Vorausschau kurz genug für einen engen Radius deckelt leise deine Geschwindigkeit.

Rampe das Kommando, sonst rutschen die Räder

Ein Twist ist ein Sprung. Navigation publiziert 0,0 dann 0,35 m/s in aufeinanderfolgenden Nachrichten, und ein Sprungkommando verlangt von den Motoren so viel Beschleunigung wie sie liefern können – was auf einem leichten Roboter mehr ist, als die Reifen übertragen können.

Was bricht, ist nicht die Bewegung, sondern die Odometrie. Ein rutschendes Rad dreht ohne Roboterbewegung, die Encoder zählen trotzdem mit, und die Posenschätzung gewinnt Fehler, den kein nachgelagerter Filter von echter Bewegung unterscheiden kann. Die Traktionsseite nennt eine gemessene Obergrenze: Ein Roboter mit 55 % Gewicht auf der Antriebsachse schafft etwa 1,6 m/s² auf staubiger Fliese, bevor die Räder losbrechen.

Begrenze die Beschleunigung unter diese Decke, im Timer, wo dt bekannt und konstant ist:

MAX_ACCEL = 1.0        # m/s^2 at the wheel, comfortably under the traction limit

def slew(target, current, dt):
    step = MAX_ACCEL * dt
    return max(current - step, min(current + step, target))

# in tick(), before sending:
self.out_left = slew(self.left, self.out_left, 0.02)
self.out_right = slew(self.right, self.out_right, 0.02)

Bei 1,0 m/s² erreicht ein stehender Start in 0,55 s volle Drehzahl, addiert 20 mm/s pro 50-Hz-Tick. Langsam genug, dass die Reifen haften, und schnell genug, dass kein Mensch den Roboter träge nennt.

Rampe beide Räder mit derselben Grenze, nicht demselben Bruchteil. Jedes Rads Beschleunigung einzeln zu klemmen verzerrt das Verhältnis während der Rampe aus demselben Grund wie Einzelrad-Clipping – die Kurve kommt die erste halbe Sekunde jeder Fahrt falsch heraus, genau wenn ein Dockingregler am empfindlichsten ist.

Ein kompletter Antriebsknoten

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist

WHEEL_RADIUS = 0.0325        # m
WHEEL_SEPARATION = 0.152     # m — calibrated by driving, see above
MAX_WHEEL_SPEED = 0.55       # m/s at full duty, measured
CMD_TIMEOUT = 0.4            # s


class DiffDrive(Node):
    def __init__(self):
        super().__init__('diff_drive')
        self.create_subscription(Twist, 'cmd_vel', self.on_cmd, 10)
        self.create_timer(0.02, self.tick)          # 50 Hz output
        self.left = self.right = 0.0
        self.last_cmd = self.get_clock().now()

    def on_cmd(self, msg):
        v, w = msg.linear.x, msg.angular.z
        left = v - w * WHEEL_SEPARATION / 2.0
        right = v + w * WHEEL_SEPARATION / 2.0

        peak = max(abs(left), abs(right))
        if peak > MAX_WHEEL_SPEED:                  # keep the arc, lose the speed
            left *= MAX_WHEEL_SPEED / peak
            right *= MAX_WHEEL_SPEED / peak

        self.left, self.right = left, right
        self.last_cmd = self.get_clock().now()

    def tick(self):
        age = (self.get_clock().now() - self.last_cmd).nanoseconds * 1e-9
        if age > CMD_TIMEOUT:                       # nobody is driving — stop
            self.left = self.right = 0.0
        send_wheel_targets(self.left / WHEEL_RADIUS,
                           self.right / WHEEL_RADIUS)   # rad/s

Zwei Strukturentscheidungen verdienen Namen.

Ausgabe per Timer, nicht im Callback. Die Motoren werden mit festen 50 Hz kommandiert, egal wie oft cmd_vel kommt. Ein stotternder Publisher – weil WLAN hustete oder ein Planer einen langen Zyklus nahm – stottert die Räder nicht mehr.

Der Watchdog ist nicht optional. Ohne ihn ist das letzte Kommando vor einem Streckenabriss das Kommando, das der Roboter weiter ausführt, und das letzte Kommando ist meist „vorwärts“. Ein Roboter, der stoppt, wenn man ihm nichts mehr sagt, ist der Unterschied zwischen Bug und Loch in der Wand. Die Funksteuerungs-Anleitungen argumentieren aus demselben Grund genauso.

Raddrehzahl in Tastverhältnis verwandeln

Der Knoten oben übergibt rad/s pro Rad. Etwas muss das herstellen, und es gibt zwei ehrliche Wege.

Offene Schleife. Bilde Drehzahl linear auf Tastverhältnis ab und akzeptiere den Fehler. Motoren haben ein Totband – unter grob 15–20 % Tastverhältnis brummen sie und drehen nicht –, die Abbildung muss also darüber starten:

int dutyFor(float radPerSec) {
  if (fabsf(radPerSec) < 0.05f) return 0;                 // genuinely stopped
  float u = fabsf(radPerSec) / MAX_RAD_PER_SEC;           // 0..1
  int duty = DEADBAND + (int)((255 - DEADBAND) * u);
  return (radPerSec > 0) ? duty : -duty;
}

Das reicht für Teleop und ist falsch für alles, was misst, wo es hinkam. Tastverhältnis ist eine Drehmoment-Anfrage, keine Drehzahl: Dasselbe Tastverhältnis gibt bergauf, auf Teppich oder mit leerem Akku verschiedene Drehzahlen.

Geschlossene Schleife. Setze Quadratur-Encoder auf die Räder und fahre eine PID-Schleife pro Rad mit rad/s als Sollwert. Nun liefert die Antriebsschicht die gefragte Drehzahl, und Totband, Last und Akku werden zum Problem des Reglers statt deines.

Auf einem Pi-Roboter ist die übliche Teilung, den ROS-Knoten auf dem Pi und den Rad-PID auf einem Arduino über Seriell mit einigen Hundert Hertz zu fahren. Linux ist nicht der Ort für eine Schleife, die nicht jittern darf.

Teste, bevor der Roboter sich wehtun kann

Der ganze Knoten ist von der Kommandozeile testbar, mit Rädern in der Luft. Tue das zuerst; ein Kinematik-Vorzeichenfehler auf der Werkbank ist eine Kuriosität, derselbe Fehler an einer Tischkante eine Reparatur.

# Straight ahead at 0.2 m/s, republished at 10 Hz
ros2 topic pub /cmd_vel geometry_msgs/msg/Twist \
  '{linear: {x: 0.2}, angular: {z: 0.0}}' -r 10

# Pure rotation, counter-clockwise at 1 rad/s
ros2 topic pub /cmd_vel geometry_msgs/msg/Twist \
  '{linear: {x: 0.0}, angular: {z: 1.0}}' -r 10

# Who is on the topic, and with which message type
ros2 topic info /cmd_vel --verbose

Vier Prüfungen, der Reihe nach:

1. Geradeauskommando, beide Räder gleich schnell gleichsinnig. Unterscheiden sie sich, sind Verdrahtung oder Vorzeichenkonvention falsch, bevor Mathematik ins Spiel kommt.
2. Positives angular.z, linkes Rad rückwärts. REP 103 sagt, positive Gierung ist gegen den Uhrzeigersinn von oben, das linke Rad muss also rückwärts laufen. Verkehrt herum lässt jeden Navigationsstack von seinem Ziel weglenken.
3. Kille den Publisher mit Strg-C. Die Räder müssen innerhalb CMD_TIMEOUT stoppen. Miss die Zeit – ein nie geübter Watchdog ist ein als defekt anzunehmender Watchdog.
4. Kommandiere über die Hüllkurve hinaus, etwa linear.x: 0,5 mit angular.z: 3,0, und bestätige, dass beide Räder skalieren statt eines zu clippen. Das Verhältnis beider Raddrehzahlen sollte unverändert zum ungesättigten Kommando sein.

Erst dann auf den Boden, und fahre die Rotationskalibrierung von oben als ersten echten Test.

Twist oder TwistStamped?

Wissenswert, bevor es dich einen Abend kostet: Neuere Teile des ROS-2-Ökosystems – darunter diff_drive_controller – sind zu geometry_msgs/msg/TwistStamped gewechselt, das denselben Twist in einen Header mit Zeitstempel und Rahmen-ID hüllt.

Der Ausfall ist lautlos. Ein Teleop-Knoten mit plain Twist und ein Regler mit TwistStamped nutzen, soweit DDS betrifft, zwei verschiedene Topics mit zufällig gleichem Namen. Nichts meldet Fehler; der Roboter bewegt sich schlicht nie. Prüfe mit ros2 topic info /cmd_vel --verbose, das den Typ an jedem Ende druckt.

Wenn es schiefgeht

Ist die Antriebsschicht ehrlich, wird alles darüber leichter: Dieselbe /cmd_vel-Schnittstelle bedient Teleop, einen Dockingregler und einen vollen Navigationsstack, ohne dass einer deine Motoren kennt. Ein Pfadfolger ist genau so ein Erzeuger: Er macht aus einer Route eine Linear- und eine Winkelgeschwindigkeit und reicht sie über dieselbe Schnittstelle nach unten.