Da cmd_vel alle ruote: trazione differenziale ROS 2

Introduzione

Tutto ciò che si trova sopra le ruote in un robot ROS 2 — teleoperazione, navigazione, il tuo controllore di aggancio — parla la stessa lingua: un geometry_msgs/msg/Twist su /cmd_vel, che significa «avanzare a questa velocità e ruotare a questa velocità». Tutto ciò che si trova sotto le ruote parla in termini di cicli di lavoro PWM.

Questo tutorial è il pezzo intermedio ed è abbastanza breve da sembrare banale. Due righe di algebra convertono una cosa nell’altra. Gli altri 90% di un nodo di trazione funzionante dipendono da ciò che quelle due righe fanno male quando al robot viene richiesto qualcosa che non può fornire.

Che cosa indica realmente un Twist

Per un robot a trazione differenziale, dei suoi sei numeri contano soltanto due:

• linear.x — velocità in avanti in metri al secondo, lungo la direzione frontale del robot.
• angular.z — velocità di rotazione in radianti al secondo, positiva in senso antiorario visto dall’alto.

Gli altri quattro descrivono movimenti che un robot a due ruote non può eseguire. linear.y è laterale e può essere eseguito soltanto da una base olonomica; ignoralo anziché approssimarlo. Sostituisci le due righe qui sotto con la matrice mecanum a quattro righe e linear.y inizierà ad avere un significato. Entrambi i valori sono espressi nel sistema di riferimento base_link proprio del robot e sono entrambi in unità SI: un robot che interpreta linear.x come una percentuale sembrerà funzionare finché non incontra un vero stack di navigazione.

Le due righe di cinematica

Indica con b la distanza tra le ruote e con v, ω rispettivamente la velocità lineare e angolare comandata. La velocità di ciascuna ruota sul terreno è la velocità in avanti del robot più o meno il contributo della rotazione:

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

Converti ciascuna velocità in velocità angolare della ruota dividendola per il raggio r:

ω_wheel = v_wheel / r        [rad/s]

Questa è tutta la conversione. La rotazione sul posto corrisponde a v = 0, che dà due velocità delle ruote uguali e opposte; l’avanzamento rettilineo corrisponde a ω = 0, che dà due velocità identiche. Il rapporto tra loro determina il raggio di curvatura, R = v / ω, e preservare quel rapporto è l’obiettivo del resto di questo tutorial.

Misura b guidando, non con un metro

La distanza tra le ruote è il parametro da cui ereditano tutti gli altri, ma non coincide con la distanza tra i loro centri.

Gli pneumatici sono deformabili, il contatto con il terreno avviene su un’area e non su una linea e le ruote scorrono lateralmente a ogni curva. Il valore che rende vere le equazioni, ossia la larghezza effettiva tra le ruote, si discosta in genere di qualche percentuale da quella fisica e si trova sempre nella direzione che fa ruotare il robot meno del dovuto.

Calibralo direttamente:

1. Comanda una rotazione pura, v = 0, ω = 1.0 rad/s, per esattamente 10 secondi.
2. Il robot dovrebbe aver ruotato di 10 radianti, cioè un giro completo più circa 213°. Segna l’orientamento iniziale e misura dove si è effettivamente fermato.
3. Scala: b_new = b_old × (turned / commanded).

Ripeti in entrambe le direzioni e fai la media, perché un gruppo di trasmissione asimmetrico fornisce risultati diversi a seconda della direzione. Di solito bastano due iterazioni. Questo stesso valore viene usato dall’odometria per integrare la posizione, quindi calibrarlo qui risolve due problemi alla volta.

Gestisci la saturazione senza modificare la curva

Un motore ha una velocità massima. Se chiedi v = 0.5 m/s con una curva brusca su un robot le cui ruote raggiungono al massimo 0.55 m/s, l’obiettivo di una ruota supera ciò che può fornire.

La risposta ovvia è limitare quella ruota. Non farlo. Limitare una ruota modifica la differenza tra le ruote e la differenza è la curva, quindi il robot percorre silenziosamente un arco più ampio di quello comandato. Un robot seguilinea che richiede una curva specifica ne ottiene una diversa, corregge, satura di nuovo e ondeggia.

Ridimensiona invece entrambe le ruote dello stesso fattore. Il rapporto si conserva e quindi si conserva anche l’arco; il robot percorre semplicemente l’arco più lentamente:

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

Tre righe e trasformano una «forma sbagliata» in una «forma giusta, più lenta», che è quasi sempre il compromesso desiderato su un robot che segue qualcosa.

Conosci l’inviluppo realmente disponibile

Il ridimensionamento salva l’arco, ma vale la pena sapere in anticipo quali comandi lo attiveranno, perché quel limite è una proprietà del tuo robot e puoi comunicarlo al pianificatore.

Imposta v_left o v_right uguale alla velocità massima della ruota e risolvi. La velocità angolare massima disponibile a una data velocità in avanti è:

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

Per il robot nel codice qui sotto, con b = 0.152 m e ruote che raggiungono al massimo 0.55 m/s:

La forma è la parte utile: velocità e velocità angolare si scambiano linearmente, e gli ultimi 10% della velocità in avanti costano quasi tutta la tua capacità di sterzo. A un robot comandato a 0.5 m/s resta il 12% dell’autorità angolare che aveva da fermo: è esattamente il motivo per cui un robot seguilinea che satura tende a farlo in una curva.

Vale la pena agire su due conseguenze. Inserisci i limiti reali nel pianificatore invece di lasciarglieli scoprire tramite la saturazione: in Nav2 sono max_vel_x e max_vel_theta, e impostare max_vel_x al valore massimo di 0.55 m/s garantisce che il controllore richieda archi che il robot non può percorrere. Quando regoli pure pursuit, ricorda che un’orizzonte di anticipo abbastanza breve da richiedere un raggio stretto limiterà silenziosamente anche la velocità.

Rampa il comando, oppure le ruote slittano

Un Twist è una variazione discontinua. La navigazione pubblica 0.0 e poi 0.35 m/s in messaggi consecutivi e un comando discontinuo chiede ai motori tutta l’accelerazione che riescono a produrre, cosa che su un robot leggero supera ciò che gli pneumatici possono trasmettere.

Non si rompe il movimento, si rompe l’odometria. Una ruota che slitta gira senza che il robot si sposti, gli encoder la contano comunque e la stima della posizione acquisisce un errore che nessun filtro a valle può distinguere da un movimento reale. La pagina sulla trazione stabilisce un limite misurato: un robot con il 55% del peso sull’asse di trazione gestisce circa 1.6 m/s2 su piastrelle polverose prima che le ruote inizino a slittare.

Limita l’accelerazione sotto quel limite, nel timer in cui dt è noto e costante:

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)

A 1.0 m/s2 una partenza da fermo raggiunge la velocità massima in 0.55 s, aggiungendo 20 mm/s a ogni tick da 50 Hz. È abbastanza lento da mantenere gli pneumatici aderenti e abbastanza rapido da non far definire lento il robot da alcuna persona.

Applica lo stesso limite a entrambe le ruote, non la stessa frazione. Limitare indipendentemente l’accelerazione di ciascuna ruota distorce il rapporto durante la rampa per lo stesso motivo per cui lo distorce il limite applicato singolarmente: la curva risulta sbagliata nella prima metà di secondo di ogni movimento, cioè proprio quando un controllore di aggancio è più sensibile.

Un nodo di trazione completo

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

Vale la pena nominare due scelte strutturali.

Emetti l’uscita da un timer, non dal callback. I motori vengono comandati a una frequenza fissa di 50 Hz, indipendentemente da quanto spesso arrivi cmd_vel. Un nodo di pubblicazione che si interrompe, perché il Wi-Fi ha avuto un problema o il pianificatore ha impiegato un ciclo lungo, non provoca più interruzioni alle ruote.

Il watchdog non è facoltativo. Senza di esso, l’ultimo comando precedente a un collegamento caduto è quello che il robot continua a eseguire, e l’ultimo comando è quasi sempre «avanti». La differenza tra un bug e un buco nel muro sta nel fatto che un robot si ferma quando non riceve più istruzioni. I tutorial sul controllo wireless sostengono lo stesso principio per lo stesso motivo.

Dalla velocità della ruota al ciclo di lavoro

Il nodo precedente fornisce rad/s per ciascuna ruota. Qualcosa deve trasformarli nel movimento effettivo e ci sono due soluzioni oneste.

Anello aperto. Associa linearmente la velocità al ciclo di lavoro e accetta l’errore. I motori hanno una zona morta: sotto circa il 15–20% del ciclo di lavoro ronzano senza girare, quindi l’associazione deve iniziare sopra questa soglia:

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

Questo va bene per la teleoperazione ed è sbagliato per qualsiasi cosa che misuri dove sia finito il robot. Il ciclo di lavoro è una richiesta di coppia, non di velocità: lo stesso ciclo di lavoro produce velocità diverse in salita, sul tappeto o con la batteria scarica.

Anello chiuso. Monta encoder in quadratura sulle ruote ed esegui un anello PID per ciascuna ruota, usando rad/s come valore di riferimento. Ora lo strato di trazione eroga la velocità richiesta e la zona morta, il carico e la batteria diventano un problema del controllore anziché tuo.

Su un robot basato su Pi, la suddivisione consueta consiste nell’eseguire il nodo ROS sul Pi e il PID per ruota su un Arduino via seriale, a qualche centinaio di hertz. Linux non è il luogo giusto per un anello che non deve oscillare.

Provalo prima che il robot possa farsi del male

L’intero nodo è collaudabile da riga di comando con le ruote sollevate da terra. Fallo prima; un errore di segno nella cinematica scoperto su un banco è una curiosità, mentre lo stesso errore scoperto sul bordo di un tavolo è una riparazione.

# 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

Esegui quattro controlli, in quest’ordine:

1. Comando rettilineo, con entrambe le ruote nella stessa direzione e alla stessa velocità. Se differiscono, il cablaggio o la convenzione sul segno sono sbagliati prima ancora di entrare nel calcolo matematico.
2. angular.z positivo, ruota sinistra all’indietro. REP 103 stabilisce che la rotazione positiva è antioraria vista dall’alto, quindi la ruota sinistra deve girare in senso inverso. Invertire questo comportamento fa sterzare ogni stack di navigazione in direzione opposta all’obiettivo.
3. Interrompi il nodo di pubblicazione con Ctrl-C. Le ruote devono fermarsi entro CMD_TIMEOUT. Misura il tempo: un watchdog mai provato è un watchdog che devi considerare difettoso.
4. Comanda oltre l’inviluppo, ad esempio linear.x: 0.5 con angular.z: 3.0, e verifica che entrambe le ruote vengano ridimensionate anziché che una sola venga limitata. Il rapporto tra le due velocità delle ruote deve rimanere invariato rispetto al comando non saturo.

Solo allora mettilo sul pavimento e usa la calibrazione della rotazione descritta sopra come primo vero collaudo.

Twist o TwistStamped?

Vale la pena saperlo prima di perderci una serata: parti più recenti dell’ecosistema ROS 2, tra cui diff_drive_controller, sono passate a geometry_msgs/msg/TwistStamped, che avvolge lo stesso Twist in un header contenente un timestamp e un frame id.

Il guasto è silenzioso. Un nodo di teleoperazione che pubblica un Twist semplice e un controllore che sottoscrive TwistStamped, dal punto di vista di DDS, usano due argomenti diversi che per caso hanno lo stesso nome. Nessun errore; il robot semplicemente non si muove. Verifica con ros2 topic info /cmd_vel --verbose, che stampa il tipo a entrambe le estremità.

Quando qualcosa va storto

Una volta che lo strato di trazione è onesto, tutto ciò che si trova sopra diventa più semplice: la stessa interfaccia /cmd_vel serve teleoperazione, un controllore di aggancio e un vero stack di navigazione senza che nessuno di essi debba conoscere i tuoi motori. Un robot seguilinea è esattamente uno di questi generatori di comandi: trasforma una rotta in una velocità lineare e una angolare e le consegna alla stessa interfaccia.