机器人 / 系统课时

将 Twist 指令换算成左右轮速

将 Twist 指令换算成左右轮速

差速机器人收到前进速度和转向角速度后,电机控制器仍需要知道左右车轮各自应该转多快。本课从轮距和轮半径推导逆运动学公式,将 ROS 2 的 Twist 指令换算为车轮角速度,并通过直行、原地旋转和弧线行驶三个算例验证方向与数值。

学习目标

  • 从 Twist.linear.x 和 Twist.angular.z 计算左右车轮的目标角速度。
  • 用具体数值验证直线行驶、原地旋转和转弯时的轮速关系。
  • 编写可运行的 Python 换算函数,识别坐标符号、轮速限制和单位错误。

确认坐标方向与底盘参数

采用 ROS 2 常用的 base_link 车体坐标约定:X 轴指向机器人前方,Y 轴指向左侧,Z 轴指向上方。绕 Z 轴的正角速度表示从上方观察时逆时针旋转,也就是机器人向左转。

差速底盘由左右两个独立驱动轮构成,假设车轮纯滚动、无侧向滑动,并且左右轮半径相同。逆运动学需要以下四个量:

符号 含义 示例值
vv 车体前进线速度 0.20 m/s
ω\omega 车体绕 Z 轴的角速度 0.50 rad/s
LL 左右驱动轮中心间距 0.30 m
rr 驱动轮有效滚动半径 0.05 m

在 geometry_msgs/msg/Twist 中,vv 对应 linear.x,ω\omega 对应 angular.z。普通双轮差速底盘不能独立实现 linear.y 指定的横向平移,也不能直接执行绕 X、Y 轴的旋转指令。

差速底盘车体坐标、轮距及左右轮速度关系

推导从车体速度到车轮速度的公式

机器人直线前进时,两轮的接地点沿前进方向具有相同速度 vv。当机器人同时以角速度 ω\omega 左转时,右轮需要比车体中心走得更快,左轮则更慢。

由于两个车轮中心分别位于车体中心左右 L/2L/2 处,转向带来的切向速度增量为 ωL/2\omega L/2。左右轮的线速度因此为:

vL=v−ωL2,vR=v+ωL2v_L=v-\frac{\omega L}{2},\qquad v_R=v+\frac{\omega L}{2}

车轮在无滑动条件下满足线速度等于角速度乘半径。记左右轮绕自身驱动轴的目标角速度为 ΩL\Omega_L 和 ΩR\Omega_R,即可得到差速底盘逆运动学公式:

ΩL=v−ωL/2r,ΩR=v+ωL/2r\Omega_L=\frac{v-\omega L/2}{r},\qquad \Omega_R=\frac{v+\omega L/2}{r}

输出单位为 rad/s,不是 m/s。这里约定两个车轮的正角速度都表示机器人向前运动;实际电机轴或 URDF 关节轴的正方向如果不同,应在驱动接口处进行符号转换。

公式还可以通过反向代入进行核验:

v=r2(ΩR+ΩL),ω=rL(ΩR−ΩL)v=\frac{r}{2}(\Omega_R+\Omega_L),\qquad \omega=\frac{r}{L}(\Omega_R-\Omega_L)

如果换算后的两轮速度代入上述公式不能恢复原始 vv 和 ω\omega,应优先检查轮距是否使用了半轮距、左右轮是否交换以及角速度的符号。

用三种运动状态核对数值

固定轮距 L=0.30L=0.30 m、轮半径 r=0.05r=0.05 m,分别输入以下三组指令。表中正轮速表示车轮向前滚动,负轮速表示向后滚动。

运动状态 vv (m/s) ω\omega (rad/s) 左轮 (rad/s) 右轮 (rad/s)
直线前进 0.20 0.00 4.00 4.00
原地左转 0.00 1.00 -3.00 3.00
前进并左转 0.20 0.50 2.50 5.50

直线行驶时角速度为零,两轮角速度都等于 0.20/0.05=40.20/0.05=4 rad/s。原地左转时车体中心不发生平移,两轮必须等速反向旋转,才能产生正的偏航角速度。

对于第三组指令,转向引起的轮侧速度差值为 0.50×0.30/2=0.0750.50\times0.30/2=0.075 m/s。左轮线速度为 0.1250.125 m/s,右轮为 0.2750.275 m/s,再分别除以半径 0.050.05 m,就得到 2.52.5 和 5.55.5 rad/s。

使用 ROS 2 Python 完成换算

以下示例适用于 Ubuntu 24.04、ROS 2 Jazzy 和 Python 3.12。先在终端执行 source /opt/ros/jazzy/setup.bash,将代码保存为 twist_to_wheels.py,再运行 python3 twist_to_wheels.py。脚本直接构造真实的 geometry_msgs.msg.Twist 消息,不依赖运行中的 ROS 节点。

python
from geometry_msgs.msg import Twist

WHEEL_RADIUS = 0.05  # 米,车轮有效滚动半径
TRACK_WIDTH = 0.30   # 米,左右轮中心间距


def to_wheel_rates(cmd: Twist) -> tuple[float, float]:
    # 双轮差速底盘只支持前进速度和偏航角速度
    unsupported = (
        cmd.linear.y, cmd.linear.z,
        cmd.angular.x, cmd.angular.y,
    )
    if any(abs(value) > 1e-9 for value in unsupported):
        raise ValueError("不支持横移、升降或滚转俯仰速度")

    v = cmd.linear.x
    omega = cmd.angular.z

    # 先求轮侧线速度,再除以轮半径得到角速度
    left = (v - omega * TRACK_WIDTH / 2.0) / WHEEL_RADIUS
    right = (v + omega * TRACK_WIDTH / 2.0) / WHEEL_RADIUS
    return left, right


def main() -> None:
    # 覆盖直行、原地左转和前进左转三个边界场景
    cases = ((0.2, 0.0), (0.0, 1.0), (0.2, 0.5))
    for v, omega in cases:
        cmd = Twist()
        cmd.linear.x = v
        cmd.angular.z = omega
        left, right = to_wheel_rates(cmd)
        print(
            f"v={v:.1f} w={omega:.1f} -> "
            f"L={left:.1f} R={right:.1f} rad/s"
        )


if __name__ == "__main__":
    main()

预期输出:

text
v=0.2 w=0.0 -> L=4.0 R=4.0 rad/s
v=0.0 w=1.0 -> L=-3.0 R=3.0 rad/s
v=0.2 w=0.5 -> L=2.5 R=5.5 rad/s

这里的 to_wheel_rates() 是纯换算函数,可以直接放入订阅 /cmd_vel 的 ROS 2 节点,在收到 Twist 消息后调用。函数输出顺序固定为左轮、右轮,单位固定为 rad/s,后续再交给轮速闭环控制器执行。

接入电机前检查速度限制与符号

运动学公式假设左右轮可以达到任何计算出的目标速度,真实电机却存在最高转速。假设两轮允许的最大绝对角速度均为 55 rad/s,第三组算例的右轮目标为 5.55.5 rad/s,已经超过限制。

不能只把右轮裁剪到 55 rad/s 而保持左轮 2.52.5 rad/s,否则实际曲率会改变。可采用同比例缩放,将两轮速度一起乘以下面的系数:

s=min⁡(1,5max⁡(∣ΩL∣,∣ΩR∣))s=\min\left(1,\frac{5}{\max(|\Omega_L|,|\Omega_R|)}\right)

此时 s=5/5.5≈0.9091s=5/5.5\approx0.9091,缩放后的左轮目标约为 2.272.27 rad/s,右轮为 5.005.00 rad/s。这样会同时降低车体线速度和角速度,但在理想无滑动模型下保持原有转弯曲率。实际控制中还应检查电机加速度和驱动器限流约束。

接入实体底盘时还需要逐项确认:

  1. 正方向:分别发送较小的正轮速,例如 11 rad/s,确认左右轮都推动机器人前进。若单侧反转,检查电机接线、编码器方向和关节轴符号。
  2. 速度单位:运动学输出为车轮 rad/s。若驱动器接收电机轴 RPM,必须先乘减速比,再乘 60/(2π)60/(2\pi)。例如车轮角速度为 44 rad/s、减速比为 20:120:1 时,电机轴目标约为 764764 RPM。
  3. 参数测量:轮距使用左右驱动轮接地点对应轮中心的横向间距,不是外壳宽度;轮半径应采用校准后的有效滚动半径。
  4. 失联保护:实际机器人需要速度命令超时停车机制,例如在控制器中设置 0.50.5 s 的命令超时,避免上游停止发布后仍保持旧轮速。

在仿真或实体底盘上验收时,先发送 v=0.20v=0.20 m/s、ω=0\omega=0,确认机器人直行;再发送 v=0v=0、ω=1.00\omega=1.00 rad/s,确认原地逆时针旋转;最后发送 v=0.20v=0.20 m/s、ω=0.50\omega=0.50 rad/s,确认左转且右轮速度更高。三组测试均符合预期,才说明速度映射、左右轮顺序和正方向约定基本一致。

按自己的节奏,确认真正理解。

目录与阅读设置

阅读设置

17

记录一个想法

将 Twist 指令换算成左右轮速

0 / 10000 · 笔记保存到账号。未提交的修改会在关闭、离开或刷新后丢失。

登录哪都通