De cmd_vel às rodas no acionamento diferencial ROS 2

Introdução

Tudo o que fica acima das rodas em um robô ROS 2 — teleoperação, navegação, seu próprio controlador de docagem — fala a mesma língua: uma mensagem geometry_msgs/msg/Twist em /cmd_vel, dizendo “avance a esta velocidade e gire a esta velocidade”. Tudo o que fica abaixo das rodas fala em ciclos de trabalho PWM.

Este tutorial é a camada entre elas e é curto o suficiente para parecer trivial. Duas linhas de álgebra convertem uma coisa na outra. Os outros 90% de um nó de acionamento funcional estão no tratamento do que essas duas linhas fazem de errado quando o robô recebe uma solicitação que não consegue cumprir.

O que um Twist realmente diz

Apenas dois de seus seis números importam para um robô de acionamento diferencial:

• linear.x — velocidade para a frente em metros por segundo, ao longo do eixo longitudinal do robô.
• angular.z — taxa de guinada em radianos por segundo, positiva no sentido anti-horário quando vista de cima.

Os outros quatro descrevem movimentos que um robô com duas rodas não pode executar. linear.y é movimento lateral, algo que apenas uma base omnidirecional pode fazer; ignore esse valor em vez de aproximá-lo. Substitua as duas linhas abaixo pela matriz de quatro linhas do mecanum e linear.y passará a ter efeito. Os dois valores são expressos no referencial base_link do próprio robô e estão em unidades SI — um robô que interpreta linear.x como uma porcentagem parecerá funcionar até encontrar um sistema de navegação real.

As duas linhas da cinemática

Seja b a separação das rodas e sejam v e ω, respectivamente, as velocidades linear e angular comandadas. A velocidade de cada roda no solo é a velocidade para a frente do robô mais ou menos a contribuição da curva:

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

Converta cada uma para uma velocidade angular da roda dividindo-a pelo raio r:

ω_wheel = v_wheel / r        [rad/s]

Essa é toda a conversão. Girar no lugar corresponde a v = 0, o que produz duas velocidades de roda iguais e opostas; avançar em linha reta corresponde a ω = 0, o que produz duas velocidades idênticas. A proporção entre elas define o raio da curva, R = v / ω, e preservar essa proporção é o tema do restante deste tutorial.

Meça b dirigindo, não com uma régua

A separação das rodas é o parâmetro do qual todos os outros dependem, e a distância entre os centros das rodas não é esse parâmetro.

Os pneus são deformáveis, fazem contato com o solo em uma área, não em uma linha, e os dois arrastam lateralmente a cada curva. O valor que torna as equações verdadeiras — a bitola efetiva — normalmente está alguns por cento longe do valor físico e sempre no sentido que faz o robô subgirar.

Calibre-o diretamente:

1. Comande um giro puro, v = 0, ω = 1.0 rad/s, por exatamente 10 segundos.
2. O robô deve ter girado 10 radianos — uma volta completa mais cerca de 213°. Marque a orientação inicial e meça onde ele realmente parou.
3. Escala: b_new = b_old × (turned / commanded).

Repita nos dois sentidos e faça a média, porque um conjunto de acionamento assimétrico fornece resultados diferentes em cada sentido. Duas iterações normalmente bastam. Esse mesmo número é usado pela odometria para integrar a posição; portanto, calibrá-lo aqui corrige duas coisas de uma vez.

Trate a saturação sem alterar a curva

Um motor tem uma velocidade máxima. Solicite v = 0.5 m/s com uma curva acentuada em um robô cujas rodas atingem no máximo 0.55 m/s, e o valor solicitado para uma delas ficará acima do que ela consegue fazer.

A resposta óbvia é limitar essa roda. Não faça isso. Limitar uma roda altera a diferença entre elas, e essa diferença determina a curva — então o robô silenciosamente percorre um arco mais largo que o comandado. Um seguidor de caminho que solicita uma curva específica recebe outra curva, corrige, satura novamente e serpenteia.

Escale ambas as rodas pelo mesmo fator. A proporção é preservada, assim como o arco; o robô apenas percorre o arco mais lentamente:

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

Três linhas convertem “forma errada” em “forma certa, mais devagar” — quase sempre a troca desejada em um robô que está seguindo algo.

Conheça o envelope que você realmente tem

O escalonamento preserva o arco, mas vale a pena saber de antemão quais comandos vão acioná-lo, porque esse limite é uma propriedade do seu robô e pode ser informado ao planejador.

Defina v_left ou v_right igual à velocidade máxima da roda e resolva. A maior taxa de guinada disponível para uma dada velocidade linear é:

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

Para o robô do código abaixo — b = 0.152 m, rodas com velocidade máxima 0.55 m/s:

A forma da curva é a parte útil: velocidade e taxa de guinada trocam linearmente, e os últimos 10% da velocidade para a frente consumem quase toda a capacidade de guinada. Um robô comandado a 0,5 m/s conserva 12% da capacidade de guinada que tinha em repouso, e é exatamente por isso que um seguidor de caminho que satura tende a fazer isso em uma esquina.

Vale agir sobre duas consequências. Coloque seus limites reais no planejador em vez de deixá-lo descobri-los por saturação — no Nav2, eles são max_vel_x e max_vel_theta, e definir max_vel_x como os 0,55 m/s completos garante que o controlador solicitará arcos que o robô não pode percorrer. E, ao ajustar pure pursuit, lembre-se de que uma distância de antecipação curta o bastante para exigir um raio apertado limitará silenciosamente a velocidade.

Rampeie o comando, ou as rodas patinarão

Um Twist é um degrau. A navegação publica 0,0 e depois 0,35 m/s em mensagens consecutivas, e um comando em degrau pede aos motores toda a aceleração que conseguem produzir — o que, em um robô leve, é mais do que os pneus conseguem transmitir ao solo.

O que quebra não é o movimento, mas a odometria. Uma roda que patina gira sem que o robô se mova, os encoders contam mesmo assim e a estimativa de pose acumula um erro que nenhum filtro posterior consegue distinguir de movimento real. A página sobre patinação e aderência estabelece um limite medido: um robô com 55% do peso no eixo motriz consegue cerca de 1,6 m/s² em azulejo empoeirado antes de as rodas perderem aderência.

Limite a aceleração abaixo desse limite, no temporizador em que dt é conhecido e constante:

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/s², uma partida do repouso alcança a velocidade máxima em 0,55 s, acrescentando 20 mm/s a cada ciclo de 50 Hz. Isso é lento o bastante para manter os pneus aderentes e rápido o bastante para ninguém chamar o robô de lento.

Rampeie as duas rodas com o mesmo limite, não com a mesma fração. Limitar a aceleração de cada roda independentemente distorce a proporção durante a rampa pelo mesmo motivo que o recorte individual — a curva sai errada no primeiro meio segundo de cada movimento, que é precisamente quando um controlador de docagem é mais sensível.

Um nó de acionamento 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 destacar duas escolhas estruturais.

A saída em um temporizador, não na função de retorno. Os motores são comandados a uma taxa fixa de 50 Hz, independentemente da frequência com que cmd_vel chega. Um publicador que engasga — porque o Wi-Fi falhou ou um planejador demorou um ciclo — não faz mais as rodas engasgarem.

O watchdog não é opcional. Sem ele, o último comando antes de a conexão cair é o comando que o robô continua executando, e o último comando geralmente é “para frente”. Um robô que para quando deixa de receber instruções é a diferença entre um bug e um buraco na parede. Os tutoriais de controle sem fio fazem o mesmo argumento pelo mesmo motivo.

Transformando a velocidade das rodas em ciclo de trabalho

O nó acima entrega rad/s para cada roda. A camada de acionamento precisa transformar essa velocidade em um sinal de controle, e há duas abordagens legítimas.

Malha aberta. Mapeie a velocidade linearmente para o ciclo de trabalho e aceite o erro. Os motores têm uma zona morta — abaixo de aproximadamente 15–20% do ciclo, eles zumbem e não giram — então o mapeamento precisa começar acima dela:

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

Isso funciona para teleoperação e é inadequado para qualquer sistema que precise determinar a posição alcançada. O ciclo de trabalho solicita torque, não velocidade: o mesmo ciclo produz uma velocidade diferente em uma subida, sobre um carpete ou com a bateria fraca.

Malha fechada. Coloque encoders em quadratura nas rodas e execute um laço PID por roda, com rad/s como referência. Agora a camada de acionamento entrega a velocidade solicitada, e a zona morta, a carga e a bateria passam a ser problemas do controlador, não do usuário.

Em um robô baseado em Pi, a divisão usual é executar o nó ROS no Pi e o PID de cada roda em um Arduino via serial, a algumas centenas de hertz. Linux não é o ambiente adequado para um laço que não pode oscilar.

Teste antes que o robô possa se ferir

Todo o nó pode ser testado pela linha de comando com as rodas fora do solo. Faça isso primeiro; um erro de sinal na cinemática descoberto na bancada é uma curiosidade, enquanto o mesmo erro descoberto na borda de uma mesa representa um reparo.

# 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

Quatro verificações, nesta ordem:

1. Comando em linha reta, ambas as rodas na mesma direção e com a mesma velocidade. Se diferirem, a fiação ou a convenção de sinal está errada antes de qualquer cálculo cinemático.
2. angular.z positivo, roda esquerda em marcha à ré. A REP 103 diz que a guinada positiva é anti-horária quando vista de cima; portanto, a roda esquerda deve girar em sentido inverso. Inverter essa convenção faz todo o sistema de navegação se desviar do objetivo.
3. Encerre o publicador com Ctrl-C. As rodas devem parar dentro de CMD_TIMEOUT. Cronometre — um watchdog nunca exercitado deve ser considerado quebrado.
4. Comande além do envelope, por exemplo linear.x: 0.5 com angular.z: 3.0, e confirme que as duas rodas são escalonadas em vez de uma ser recortada. A proporção entre as duas velocidades de roda deve permanecer igual à do comando sem saturação.

Só então coloque o robô no chão e execute a calibração de rotação da seção anterior como o primeiro teste real.

Twist ou TwistStamped?

Vale saber antes que isso lhe custe uma noite: partes mais recentes do ecossistema ROS 2 — incluindo o diff_drive_controller — migraram para geometry_msgs/msg/TwistStamped, que encapsula o mesmo twist em um cabeçalho com carimbo de data/hora e ID do referencial.

A falha é silenciosa. Um nó de teleoperação que publica apenas Twist e um controlador que assina TwistStamped estão, do ponto de vista do DDS, usando dois tópicos diferentes que por acaso compartilham o mesmo nome. Nada dá erro; o robô simplesmente nunca se move. Verifique com ros2 topic info /cmd_vel --verbose, que imprime o tipo em cada extremidade.

Quando dá errado

Depois que a camada de acionamento é honesta, tudo acima dela fica mais fácil: a mesma interface /cmd_vel atende à teleoperação, a um controlador de docagem e a um sistema completo de navegação sem que nenhum deles precise saber quais são seus motores. Um seguidor de caminho é exatamente um produtor desses comandos: transforma uma rota em velocidade linear e angular e as entrega pela mesma interface.