Van cmd_vel naar wielersnelheden: ROS 2 differentiaalaandrijving

Inleiding

Alles boven de wielen in een ROS 2-robot — teleop, navigatie, je eigen dockingsregelaar — spreekt dezelfde taal: een geometry_msgs/msg/Twist op /cmd_vel, met de betekenis “ga zo snel vooruit en draai zo snel”. Alles onder de wielen spreekt PWM-dutycycles.

Deze tutorial is het stuk ertussen, en het is kort genoeg om triviaal te lijken. Twee regels algebra zetten het een om in het ander. De andere 90% van een werkende aandrijfnode is wat die twee regels fout doen wanneer de robot om iets wordt gevraagd dat hij niet kan leveren.

Wat een Twist werkelijk zegt

Slechts twee van de zes getallen zijn van belang voor een robot met differentiaalaandrijving:

• linear.x — vooruitsnelheid in meter per seconde, langs de neus van de robot.
• angular.z — giersnelheid in radialen per seconde, positief tegen de klok in gezien van boven.

De andere vier beschrijven bewegingen die een robot met twee wielen niet kan uitvoeren. linear.y is zijwaarts, en dat kan alleen een holonome basis; negeer het in plaats van het te benaderen. Vervang de twee regels hieronder door de vierregelige mecanum-matrix en begint linear.y betekenis te krijgen. Beide waarden staan in het eigen referentiestelsel base_link van de robot, en beide zijn in SI-eenheden — een robot die linear.x als percentage neemt lijkt het goed te doen tot hij een echte navigatiestack tegenkomt.

De twee regels kinematica

Laat b de wielafstand zijn en v, ω de aangestuurde lineaire en hoeksnelheid. De grondsnelheid van elk wiel is de vooruitsnelheid van de robot plus of min de bijdrage van de draaiing:

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

Reken elke om naar een hoeksnelheid van het wiel door te delen door de wieldraaide r:

ω_wheel = v_wheel / r        [rad/s]

Dat is de hele omrekening. Ter plaatse draaien is v = 0, wat twee gelijke en tegengestelde wielsnelheden geeft; rechtdoor rijden is ω = 0, wat twee identieke geeft. De verhouding tussen beide bepaalt de draaistraal, R = v / ω, en het behoud van die verhouding is waar de rest van deze tutorial over gaat.

Meet b door te rijden, niet met een liniaal

De wielafstand is de parameter die al het andere erft, en de afstand tussen je wielmiddens is dat niet.

De banden zijn soepel, ze raken de grond over een vlak in plaats van een lijn, en ze schuiven bij elke bocht zijdelings door. De waarde die de vergelijkingen waar maakt — de effectieve spoorbreedte — wijkt doorgaans enkele procenten af van de fysieke, en altijd in de richting die je robot te weinig laat draaien.

Kalibreer hem rechtstreeks:

1. Stuur een zuivere draaiing aan, v = 0, ω = 1.0 rad/s, gedurende precies 10 seconden.
2. De robot hoort 10 radialen te hebben gedraaid — één volle draai plus ongeveer 213°. Markeer de beginrichting en meet waar hij werkelijk stopte.
3. Schaal: b_new = b_old × (turned / commanded).

Herhaal dit in beide richtingen en neem het gemiddelde, omdat een asymmetrische aandrijflijn elke kant een ander antwoord geeft. Twee iteraties zijn meestal genoeg. Datzelfde getal gebruikt je odometrie om de positie te integreren, dus het hier kalibreren repareert twee dingen tegelijk.

Verzadiging opvangen zonder de draaiing te veranderen

Een motor heeft een maximumsnelheid. Vraag om v = 0.5 m/s met een scherpe bocht op een robot waarvan de wielen op 0.55 m/s eindigen, en het doel van één wiel komt boven wat het aankan.

Het voor de hand liggende antwoord is dat wiel begrenzen. Doe dat niet. Eén wiel begrenzen verandert het verschil tussen de wielen, en dat verschil is de draaiing — dus de robot rijdt stilletjes een ruimere boog dan aangestuurd. Een padvolger die om een specifieke kromme vraagt krijgt een andere kromme terug, corrigeert, verzadigt opnieuw en slingert.

Schaal in plaats daarvan beide wielen met dezelfde factor. De verhouding overleeft, dus de boog overleeft; de robot legt hem alleen langzamer af:

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

Drie regels, en het zet “verkeerde vorm” om in “juiste vorm, langzamer” — en dat is vrijwel altijd de afweging die je wilt op een robot die iets volgt.

Ken de begrenzing die je werkelijk hebt

Schalen redt de boog, maar het is de moeite waard vooraf te weten welke commando’s het zullen uitlokken, want die grens is een eigenschap van je robot en een planner kan erover worden geïnformeerd.

Stel v_left of v_right gelijk aan de maximumsnelheid van het wiel en los het op. De grootste beschikbare giersnelheid bij een gegeven vooruitsnelheid is:

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

Voor de robot in de code hieronder — b = 0.152 m, wielen die eindigen op 0.55 m/s:

De vorm is het nuttige deel: snelheid en draaisnelheid ruilen lineair, en de laatste 10% van de vooruitsnelheid kost je bijna al je stuurvermogen. Een robot die op 0,5 m/s wordt aangestuurd heeft 12% van het giervermogen dat hij had vanuit stilstand, en juist daarom verzadigt een padvolger dat doorgaans in een bocht.

Twee gevolgen waar je iets mee kunt doen. Zet je werkelijke grenzen in de planner in plaats van ze te laten ontdekken door te verzadigen — in Nav2 zijn dat max_vel_x en max_vel_theta, en max_vel_x op de volle 0,55 m/s zetten garandeert dat de regelaar bogen vraagt die de robot niet kan rijden. En onthoud bij het afstellen van pure pursuit dat een vooruitkijkeradius die kort genoeg is om een krappe straal te eisen stilletjes je snelheid begrenst.

Rampeer het commando, anders slippen de wielen

Een Twist is een sprong. Navigatie publiceert 0,0 en daarna 0,35 m/s op opeenvolgende berichten, en een springcommando vraagt de motoren om zoveel acceleratie als ze kunnen leveren — wat op een lichte robot meer is dan de banden kunnen overbrengen.

Wat er kapotgaat is niet de beweging, maar de odometrie. Een slippend wiel draait zonder dat de robot beweegt, de encoders tellen het toch, en de poseschatting krijgt fout erbij die geen enkele filter downstream van echte beweging kan onderscheiden. De pagina over tractie plaatst een gemeten bovengrens: een robot met 55% van zijn gewicht op de aandrijfas haalt ongeveer 1,6 m/s2 op stoffige tegels voordat de wielen doorslippen.

Begrens de acceleratie onder die bovengrens, in de timer waar dt bekend en constant is:

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)

Bij 1,0 m/s2 bereikt een start vanaf stilstand de volle snelheid in 0,55 s, met 20 mm/s per 50 Hz-tik erbij. Dat is langzaam genoeg om de banden te laten grijpen en snel genoeg dat geen mens de robot traag noemt.

Ramp beide wielen met dezelfde grens, niet met dezelfde fractie. De acceleratie van elk wiel onafhankelijk begrenzen vervormt de verhouding tijdens de helling om dezelfde reden als het apart afknippen per wiel — de draaiing komt fout uit in de eerste halve seconde van elke beweging, en dat is precies wanneer een dockingsregelaar het gevoeligst is.

Een complete aandrijfnode

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

Twee structurele keuzes zijn het noemen waard.

Uitvoer op een timer, niet in de callback. De motoren worden met een vaste 50 Hz aangestuurd, ongeacht hoe vaak cmd_vel binnenkomt. Een uitgever die hapert — omdat wifi struikelde, of een planner een lange cyclus deed — laat de wielen niet langer haperen.

De watchdog is niet optioneel. Zonder hem is het laatste commando voor een weggevallen verbinding het commando dat de robot blijft uitvoeren, en het laatste commando is meestal “vooruit”. Het verschil tussen een bug en een gat in de muur is een robot die stopt zodra hij niets meer te horen krijgt. De tutorials over draadloze besturing maken om dezelfde reden hetzelfde argument.

Van wielsnelheid naar dutycycle

De node hierboven geeft rad/s per wiel door. Iets moet dat omzetten, en er zijn twee eerlijke manieren.

Open loop. Zet snelheid lineair om naar duty en accepteer de fout. Motoren hebben een dode zone — onder ongeveer 15–20% duty zoemen ze en draaien ze niet — dus de afbeelding moet daarboven beginnen:

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;
}

Dit is prima voor teleop en fout voor alles waar meet waar hij is gebleven. Duty is een verzoek om koppel, niet om snelheid: dezelfde duty levert een andere snelheid bergop, op tapijt, of met een lege accu.

Gesloten lus. Zet kwadratuurencoders op de wielen en draai een PID-lus per wiel met rad/s als setpoint. Nu levert de aandrijflaag de snelheid die je vroeg, en de dode zone, de belasting en de accu worden allemaal het probleem van de regelaar in plaats van van jou.

Op een robot op basis van een Pi is de gebruikelijke opsplitsing: de ROS-node draait op de Pi en de PID per wiel op een Arduino via serieel, op een paar honderd hertz. Linux is de plek niet voor een lus die niet mag rammelen.

Test het voordat de robot zichzelf pijn kan doen

De hele node is vanaf de opdrachtregel te testen met de wielen van de grond. Doe dat eerst; een tekenfout in de kinematica die je op een werkbank ontdekt is een curiositeit, en dezelfde fout die je op een tafelrand ontdekt is een reparatie.

# 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 controles, in volgorde:

1. Rechtuit-commando, beide wielen dezelfde richting en snelheid. Verschillen ze, dan is de bedrading of de tekenconventie fout, vóórdat er enige wiskunde bij komt kijken.
2. Positieve angular.z, linkerwiel achteruit. REP 103 zegt dat positieve gier tegen de klok in is gezien van boven, dus het linkerwiel moet achteruit draaien. Draai je dit om, dan stuurt elke navigatiestack weg van zijn doel.
3. Dood de uitgever met Ctrl-C. De wielen moeten binnen CMD_TIMEOUT stoppen. Meet het — een watchdog die nooit wordt gebruikt is een watchdog die je als defect moet beschouwen.
4. Commando voorbij de begrenzing, zeg linear.x: 0.5 met angular.z: 3.0, en bevestig dat beide wielen schalen in plaats van dat er één wordt afgeknipt. De verhouding tussen de twee wielsnelheden moet ongewijzigd zijn ten opzichte van het onverzadigde commando.

Zet hem pas dan op de vloer, en doe de rotatiekalibratie uit de sectie hierboven als eerste echte test.

Twist of TwistStamped?

Goed om te weten voordat het je een avond kost: nieuwere delen van het ROS 2-ecosysteem — diff_drive_controller onder meer — zijn overgegaan op geometry_msgs/msg/TwistStamped, dat dezelfde twist in een header verpakt met een tijdstempel en een frame-id.

De fout is stil. Een teleop-node die gewone Twist publiceert en een regelaar die op TwistStamped abonneert gebruiken, wat DDS betreft, twee verschillende topics die toevallig dezelfde naam delen. Er gaat niets mis; de robot beweegt alleen simpelweg nooit. Controleer het met ros2 topic info /cmd_vel --verbose, dat aan elk uiteinde het type afdrukt.

Het gaat mis

Zodra de aandrijflaag eerlijk is, wordt alles erboven gemakkelijker: dezelfde /cmd_vel-interface bedient teleop, een dockingsregelaar en een volledige navigatiestack, zonder dat een van hen weet wat jouw motoren zijn. Een padvolger is precies zo’n producent: hij maakt van een route een lineaire en een hoeksnelheid en geeft ze door dezelfde interface.