De cmd_vel aux vitesses de roues en ROS 2

Introduction

Tout ce qui est au-dessus des roues dans un robot ROS 2 — téléopération, navigation, votre propre contrôleur d’accostage — parle la même langue : un geometry_msgs/msg/Twist sur /cmd_vel, qui dit « avance à cette vitesse et tourne à cette vitesse ». Tout ce qui est sous les roues parle rapports cycliques de PWM.

Ce tutoriel est la pièce entre les deux, et elle est assez courte pour sembler triviale. Deux lignes d’algèbre convertissent l’un en l’autre. Les autres 90 % d’un nœud de traction qui fonctionne sont ce que ces deux lignes font de travers quand on demande au robot quelque chose qu’il ne peut pas fournir.

Ce que dit vraiment un Twist

Seuls deux de ses six nombres comptent pour un robot à entraînement différentiel :

• linear.x — vitesse avant en mètres par seconde, le long du nez du robot.
• angular.z — vitesse de lacet en radians par seconde, positive dans le sens antihoraire vu de dessus.

Les quatre autres décrivent un mouvement qu’un robot à deux roues ne peut pas exécuter. linear.y est le latéral, que seule une base holonome peut faire ; ignorez-le plutôt que de l’approximer. Échangez les deux lignes ci-dessous contre la matrice mecanum à quatre lignes et linear.y commence à signifier quelque chose. Les deux valeurs sont exprimées dans le repère propre du robot, base_link, et les deux sont en unités SI — un robot qui prend linear.x comme un pourcentage semblera fonctionner jusqu’au jour où il rencontre une vraie pile de navigation.

Les deux lignes de cinématique

Soit b l’écartement des roues et v, ω les vitesses linéaire et angulaire commandées. La vitesse au sol de chaque roue est la vitesse avant du robot plus ou moins la contribution du virage :

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

Convertissez chacune en vitesse angulaire de roue en divisant par le rayon de roue r :

ω_wheel = v_wheel / r        [rad/s]

Voilà toute la conversion. Tourner sur place est v = 0, ce qui donne deux vitesses de roues égales et opposées ; rouler droit est ω = 0, ce qui donne deux vitesses identiques. Le rapport entre elles fixe le rayon de virage, R = v / ω, et préserver ce rapport est l’objet du reste de ce tutoriel.

Mesurez b en roulant, pas au mètre

L’écartement des roues est le paramètre dont tout le reste hérite, et la distance entre les centres de vos roues ne l’est pas.

Les pneus sont souples, ils touchent le sol sur une surface plutôt qu’une ligne, et les deux ripent latéralement dans chaque virage. La valeur qui rend les équations vraies — la voie effective — est typiquement à quelques pour cent de la valeur physique, et toujours dans le sens qui fait sous-virer votre robot.

Étalonnez-la directement :

1. Commandez une rotation pure, v = 0, ω = 1,0 rad/s, pendant exactement 10 secondes.
2. Le robot aurait dû tourner de 10 radians — un tour complet plus environ 213°. Marquez le cap de départ et mesurez où il s’est vraiment arrêté.
3. Mettez à l’échelle : b_new = b_old × (tourné / commandé).

Répétez dans les deux sens et moyennez, parce qu’une transmission asymétrique donne une réponse différente de chaque côté. Deux itérations suffisent généralement. Ce même nombre est celui que votre odométrie utilise pour intégrer la position, donc l’étalonner ici corrige deux choses à la fois.

Gérez la saturation sans changer le virage

Un moteur a une vitesse maximale. Demandez v = 0,5 m/s avec un virage serré sur un robot dont les roues plafonnent à 0,55 m/s, et la cible d’une roue atterrit au-dessus de ce qu’elle peut faire.

La réponse évidente est d’écrêter cette roue. Ne le faites pas. Écrêter une roue change la différence entre les roues, et la différence est le virage — donc le robot suit discrètement un arc plus large que celui commandé. Un suiveur de trajectoire qui demande une courbe précise en reçoit une autre, corrige, sature à nouveau, et louvoie.

Mettez les deux roues à l’échelle par le même facteur à la place. Le rapport survit, donc l’arc survit ; le robot le parcourt simplement plus lentement :

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

Trois lignes, et cela convertit « mauvaise forme » en « bonne forme, plus lent » — ce qui est presque toujours le compromis voulu sur un robot qui suit quelque chose.

Connaissez l’enveloppe dont vous disposez vraiment

La mise à l’échelle sauve l’arc, mais il vaut la peine de savoir à l’avance quelles commandes la déclencheront, parce que cette frontière est une propriété de votre robot et qu’on peut en informer un planificateur.

Posez v_left ou v_right égal à la vitesse maximale de la roue et résolvez. La vitesse de lacet maximale disponible à une vitesse avant donnée est :

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

Pour le robot du code ci-dessous — b = 0,152 m, roues plafonnant à 0,55 m/s :

La forme est la partie utile : la vitesse et la vitesse de virage s’échangent linéairement, et les derniers 10 % de vitesse avant coûtent presque tout votre braquage. Un robot commandé à 0,5 m/s dispose de 12 % de l’autorité en lacet qu’il avait à l’arrêt, et c’est exactement pourquoi un suiveur de trajectoire qui sature le fait en général dans un virage.

Deux conséquences valent d’agir. Mettez vos vraies limites dans le planificateur plutôt que de le laisser les découvrir en saturant — dans Nav2 ce sont max_vel_x et max_vel_theta, et régler max_vel_x au plein 0,55 m/s garantit que le contrôleur demandera des arcs que le robot ne peut pas suivre. Et en réglant la poursuite pure, souvenez-vous qu’une anticipation assez courte pour exiger un rayon serré plafonnera silencieusement votre vitesse.

Rampez la commande, ou les roues patinent

Un Twist est un échelon. La navigation publie 0,0 puis 0,35 m/s sur des messages consécutifs, et une commande en échelon demande aux moteurs autant d’accélération qu’ils peuvent produire — ce qui sur un robot léger dépasse ce que les pneus peuvent transmettre.

Ce qui casse n’est pas le mouvement, c’est l’odométrie. Une roue qui patine tourne sans que le robot avance, les encodeurs la comptent quand même, et l’estimée de pose gagne une erreur qu’aucun filtre aval ne peut distinguer d’un vrai mouvement. La page sur la traction en donne un plafond mesuré : un robot avec 55 % de son poids sur l’essieu moteur tient environ 1,6 m/s² sur carrelage poussiéreux avant que les roues décrochent.

Limitez l’accélération sous ce plafond, dans l’horloge où dt est connu et constant :

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)

À 1,0 m/s² un départ arrêté atteint la pleine vitesse en 0,55 s, ajoutant 20 mm/s par tic à 50 Hz. C’est assez lent pour garder les pneus accrochés et assez rapide pour qu’aucun humain ne traite le robot de poussif.

Rampez les deux roues avec la même limite, pas la même fraction. Borner l’accélération de chaque roue indépendamment déforme le rapport pendant la rampe pour la même raison que l’écrêtage par roue — le virage sort faux pendant la première demi-seconde de chaque déplacement, précisément quand un contrôleur d’accostage est le plus sensible.

Un nœud de traction complet

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

Deux choix de structure valent d’être nommés.

Émettre sur une horloge, pas dans le rappel. Les moteurs sont commandés à 50 Hz fixes quelle que soit la fréquence d’arrivée de cmd_vel. Un publieur qui hoquette — parce que le Wi-Fi a hésité, ou qu’un planificateur a pris un long cycle — ne fait plus hoqueter les roues.

Le chien de garde n’est pas optionnel. Sans lui, la dernière commande avant une liaison coupée est la commande que le robot continue d’exécuter, et la dernière commande est généralement « avant ». Un robot qui s’arrête quand on cesse de lui dire quoi faire est la différence entre un bogue et un trou dans le mur. Les tutoriels de commande sans fil tiennent le même raisonnement pour la même raison.

Transformer la vitesse des roues en rapport cyclique

Le nœud ci-dessus transmet des rad/s par roue. Quelque chose doit le réaliser, et il y a deux façons honnêtes.

Boucle ouverte. Projetez la vitesse en rapport linéairement et acceptez l’erreur. Les moteurs ont une bande morte — sous environ 15 à 20 % de rapport ils bourdonnent et ne tournent pas — donc la projection doit partir au-dessus :

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

C’est bien pour la téléopération et faux pour tout ce qui mesure où il est allé. Le rapport cyclique est une demande de couple, pas de vitesse : le même rapport donne une vitesse différente en montée, sur moquette, ou avec une batterie plate.

Boucle fermée. Mettez des encodeurs en quadrature sur les roues et faites tourner une boucle PID par roue avec les rad/s en consigne. Maintenant la couche de traction délivre la vitesse demandée, et la bande morte, la charge et la batterie deviennent le problème du contrôleur plutôt que le vôtre.

Sur un robot à base de Pi, la répartition habituelle est de faire tourner le nœud ROS sur le Pi et le PID par roue sur un Arduino via liaison série, à quelques centaines de hertz. Linux n’est pas l’endroit pour une boucle qui ne doit pas giguer.

Testez avant que le robot puisse faire mal

Tout le nœud se teste depuis la ligne de commande avec les roues en l’air. Faites d’abord cela ; une erreur de signe en cinématique découverte sur l’établi est une curiosité, et la même erreur découverte au bord d’une table est une réparation.

# 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

Quatre contrôles, dans l’ordre :

1. Commande tout droit, les deux roues dans le même sens et à la même vitesse. Si elles diffèrent, le câblage ou la convention de signe est faux avant toute mathématique.
2. angular.z positif, roue gauche en arrière. REP 103 dit que le lacet positif est antihoraire vu de dessus, donc la roue gauche doit tourner en marche arrière. Se tromper ici fait braquer chaque pile de navigation à l’opposé de son but.
3. Tuez le publieur avec Ctrl-C. Les roues doivent s’arrêter dans CMD_TIMEOUT. Chronométrez — un chien de garde jamais exercé est un chien de garde à supposer cassé.
4. Commandez au-delà de l’enveloppe, par exemple linear.x: 0.5 avec angular.z: 3.0, et confirmez que les deux roues sont mises à l’échelle plutôt qu’une écrêtée. Le rapport entre les deux vitesses de roues doit être inchangé par rapport à la commande non saturée.

Ensuite seulement posez-le au sol, et faites de l’étalonnage de rotation de la section ci-dessus le premier vrai test.

Twist ou TwistStamped ?

Bon à savoir avant que cela coûte une soirée : les parties récentes de l’écosystème ROS 2 — dont diff_drive_controller — sont passées à geometry_msgs/msg/TwistStamped, qui enveloppe le même twist dans un en-tête portant horodatage et identifiant de repère.

La panne est silencieuse. Un nœud de téléopération publiant du Twist simple et un contrôleur abonné à TwistStamped utilisent, pour DDS, deux sujets différents qui partagent un nom par hasard. Rien ne produit d’erreur ; le robot ne bouge simplement jamais. Vérifiez avec ros2 topic info /cmd_vel --verbose, qui affiche le type de chaque côté.

Quand ça tourne mal

Une fois la couche de traction honnête, tout ce qui est au-dessus devient plus facile : la même interface /cmd_vel sert la téléopération, un contrôleur d’accostage, et une pile de navigation complète sans qu’aucun connaisse vos moteurs. Un suiveur de trajectoire est exactement un tel producteur : il transforme un itinéraire en vitesse linéaire et angulaire et les transmet par cette même interface.