从 cmd_vel 到轮速：ROS 2 差速驱动

引言

ROS 2 机器人里轮子之上的一切——遥操作、导航、你自己写的对接控制器——都说同一种语言：在 /cmd_vel 上发一个 geometry_msgs/msg/Twist，意思是“以这个速度前进、以这个速度转”。轮子之下的一切说的是 PWM 占空比。

本文就是夹在中间的那一层，短到看起来不值一提。两行代数就把前者变成后者。而一个能用的驱动节点里，剩下 90% 的内容都在讲：当机器人被要求做它交付不了的事情时，那两行会怎么出错。

Twist 到底说了什么

六个数字里，只有两个对差速机器人有意义：

• linear.x——前进速度，米每秒，沿车头方向。
• angular.z——偏航率，弧度每秒，从上方看逆时针为正。

另外四个描述的是两轮机器人做不了的动作。linear.y 是横向移动，只有全向底盘做得到；忽略它，而不是去近似它。把下面那两行换成四行的麦克纳姆矩阵，linear.y 就开始有意义了。两个数值都在机器人自己的 base_link 坐标系下表达，单位都是 SI——把 linear.x 当百分比用的机器人，在遇到真正的导航栈之前看起来一直是对的。

两行运动学

设 b 是轮距，v、ω 是下发的线速度和角速度。每个轮子的地面速度，等于机器人的前进速度加上或减去转弯的贡献：

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

再除以轮子半径 r，把每个轮子的地面速度换算成轮子的角速度：

ω_wheel = v_wheel / r        [rad/s]

整个换算就这些。原地旋转是 v = 0，给出两个大小相等、方向相反的轮速；直行是 ω = 0，给出两个完全相同的轮速。两者之间的比例定下了转弯半径 R = v / ω，而保持这个比例正是本文余下部分的主题。

b 要开出来量，不要拿尺子量

轮距是其他一切参数继承的源头，而你两个轮心之间的距离并不是它。

轮胎是柔性的，它以一个面而不是一条线接触地面，而且每次转弯时两个轮子都在横向擦滑。让方程成立的那个值——有效轮距——通常与物理值差几个百分点，而且方向永远偏向让机器人转不够。

直接标定：

1. 下发纯旋转指令，v = 0、ω = 1.0 rad/s，精确持续 10 秒。
2. 机器人应该转过 10 弧度——整整一圈再多约 213°。标出起始航向，量它实际停在哪里。
3. 缩放：b_new = b_old ×（实际转过 / 指令转过）。

两个方向都做一遍并取平均，因为不对称的传动每个方向给出的答案不同。一般两轮迭代就够了。同一个数也是你的里程计用来积分位置的，所以在这里标定它等于一次修好两件事。

饱和了也不改变转弯

电机是有极速的。在轮子最高只能到 0.55 m/s 的机器人上，下发 v = 0.5 m/s 并带一个急转弯，其中一个轮子的目标就落到了它做不到的数值之上。

显而易见的做法是把那个轮子钳住。**不要。**钳住一个轮子改变的是两轮之间的差值，而这个差值就是转弯——于是机器人会悄悄走一条比指令更宽的圆弧。一个请求特定曲线的路径跟踪器会拿回一条不同的曲线，然后修正、再次饱和，左右摇摆。

改成把两个轮子按同一个系数缩放。比例保住了，圆弧就保住了；机器人只是走得慢一些：

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

三行代码，把“形状错了”变成“形状对、速度慢”——在一台要跟随某个东西的机器人上，这几乎总是你想要的取舍。

搞清楚你实际拥有的包络

缩放保住了圆弧，但提前知道哪些指令会触发它仍然值得，因为那条边界是你这台机器人的属性，而规划器可以被告知它。

令 v_left 或 v_right 等于轮子的极速，解出来。给定前进速度时能拿到的最大偏航率是：

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

对下面代码里那台机器人——b = 0.152 m，轮子极速 0.55 m/s：

形状才是有用的部分：**速度和转弯率是线性取舍，而前进速度的最后 10% 几乎花掉了你全部的转向能力。**一台被下发 0.5 m/s 的机器人，只剩下静止时 12% 的偏航权限，这正是会饱和的路径跟踪器偏偏在弯道里饱和的原因。

有两个值得照做的推论。把你的真实极限写进规划器，而不是让它靠饱和去发现——在 Nav2 里就是 max_vel_x 和 max_vel_theta，而把 max_vel_x 设成完整的 0.55 m/s，保证控制器会去要求机器人开不出来的圆弧。还有，在整定纯跟踪时记住：一个短到会要求很紧半径的前视距离，会悄悄给你的速度设上上限。

给指令加斜坡，否则轮子打滑

一个 Twist 就是个阶跃。导航在连续两条消息里发 0.0 和 0.35 m/s，而一个阶跃指令等于向电机索要它能产生的最大加速度——在一台轻机器人上，这超过了轮胎能传递的量。

坏掉的不是运动本身，而是里程计。打滑的轮子在机器人没动的情况下转了半圈，编码器照样在数，于是位姿估计里多出一份误差，而下游任何滤波器都无法把它和真实运动区分开。附着力那一页给这个现象量了一个上限：驱动轴上承载 55% 重量、地面是蒙灰瓷砖的机器人，在轮子打滑之前大约能做到 1.6 m/s2。

把加速度限制在这个上限之下，放在 dt 已知且恒定的定时器里：

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/s2 下，从静止起步到全速需要 0.55 s，50 Hz 的每一次定时中断只加 20 mm/s。这既慢到足以让轮胎保持附着，又快到不会有人嫌这车没劲。

两个轮子要用同一个限值加斜坡，而不是同一个比例。各自独立地钳加速度，会因为和单轮裁剪同样的原因在斜坡期间扭曲比例——每次动作的前半秒转弯都是错的，而那恰恰是对接控制器最敏感的时候。

一个完整的驱动节点

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

有两个结构选择值得点名。

**输出放在定时器上，而不是回调里。**电机被固定以 50 Hz 下发，与 cmd_vel 多久来一次无关。一个会打嗝的发布者——因为 Wi-Fi 卡了一下，或者规划器周期太长——不再让轮子打嗝。

**看门狗不是可选项。**没有它，链路掉之前最后一条指令就是机器人会一直执行的指令，而最后那条指令通常是“前进”。一台“没人再告诉它该做什么就停下来”的机器人，区别在于你遇到的是一个缺陷还是墙上一个洞。无线控制那些教程出于同样的理由给出同样的论证。

把轮速变成占空比

上面的节点交给下游的是每轮的 rad/s。总得有人把它变成实际动作，而有两种诚实的做法。

开环。把速度线性映射到占空比，接受其中的误差。电机有死区——占空比低于大约 15–20% 时只会嗡嗡响而不转——所以映射必须从死区之上开始：

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

对遥操作来说这样够用，对任何需要知道自己走到哪里的东西来说则是错的。占空比是对转矩的请求，不是对速度的请求：同样的占空比，在上坡时、在地毯上、或在电池变平的时候，都会给出不同的速度。

**闭环。**在轮子上装正交编码器，每轮跑一个以 rad/s 为设定点的 PID 环。这样驱动层交付的就是被要求的那个速度，而死区、负载和电池全都变成控制器的问题，不再是你的问题。

在基于树莓派的机器人上，常见的分工是 ROS 节点跑在树莓派上，每轮的 PID 通过串口跑在 Arduino 上，频率几百赫兹。Linux 不是那个不允许抖动的环该待的地方。

在机器人能伤到你自己之前先测它

整个节点都可以在轮子悬空的情况下从命令行测试。先做这个；在台架上发现的运动学符号错误是个小新闻，在桌边发现同一个错误就是一次维修。

# 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

四项检查，按顺序：

1. **直行指令下两个轮子方向和速度相同。**如果不同，还没轮到任何数学，接线或符号约定就已经错了。
2. **正的 angular.z 让左轮倒转。**REP 103 规定从上方看逆时针为正偏航，所以左轮必须反转。反过来的话，每一个导航栈都会朝远离目标的方向转。
3. **用 Ctrl-C 杀掉发布者。**轮子必须在 CMD_TIMEOUT 之内停下。掐一下表——一个从没被演练过的看门狗，是一个你应该假定已经坏掉的看门狗。
4. 下发超出包络的指令，比如 linear.x: 0.5 配 angular.z: 3.0，确认两个轮子是同比例缩放而不是其中一个被裁剪。两个轮速之间的比例应当与未饱和指令时相同。

之后才把它放到地面上，并把上面那一节的旋转标定当作第一个真正的测试来做。

用 Twist 还是 TwistStamped？

值得在它花掉你一个晚上之前就知道：ROS 2 生态里较新的部分——diff_drive_controller 就在其中——已经转向 geometry_msgs/msg/TwistStamped，它把同样的 Twist 包进一个带时间戳和坐标系 id 的头里。

这个故障是安静的。一个发普通 Twist 的遥操作节点，和一个订阅 TwistStamped 的控制器，在 DDS 看来就是两个恰好同名的不同话题。什么都不会报错；机器人只是永远不动。用 ros2 topic info /cmd_vel --verbose 检查，它会把两端的类型都打印出来。

出问题时

驱动层诚实之后，它上面的一切都变简单了：同一个 /cmd_vel 接口同时服务遥操作、对接控制器和完整的导航栈，而它们谁都不需要知道你的电机是什么。路径跟踪器正是这样一个生产者：它把一条路线变成线速度和角速度，然后从同一个接口交下去。