机器人 / 系统课时
将 Twist 指令换算成左右轮速
将 Twist 指令换算成左右轮速
差速机器人收到前进速度和转向角速度后,电机控制器仍需要知道左右车轮各自应该转多快。本课从轮距和轮半径推导逆运动学公式,将 ROS 2 的 Twist 指令换算为车轮角速度,并通过直行、原地旋转和弧线行驶三个算例验证方向与数值。
学习目标
- 从
Twist.linear.x和Twist.angular.z计算左右车轮的目标角速度。 - 用具体数值验证直线行驶、原地旋转和转弯时的轮速关系。
- 编写可运行的 Python 换算函数,识别坐标符号、轮速限制和单位错误。
确认坐标方向与底盘参数
采用 ROS 2 常用的 base_link 车体坐标约定:X 轴指向机器人前方,Y 轴指向左侧,Z 轴指向上方。绕 Z 轴的正角速度表示从上方观察时逆时针旋转,也就是机器人向左转。
差速底盘由左右两个独立驱动轮构成,假设车轮纯滚动、无侧向滑动,并且左右轮半径相同。逆运动学需要以下四个量:
| 符号 | 含义 | 示例值 |
|---|---|---|
| 车体前进线速度 | 0.20 m/s | |
| 车体绕 Z 轴的角速度 | 0.50 rad/s | |
| 左右驱动轮中心间距 | 0.30 m | |
| 驱动轮有效滚动半径 | 0.05 m |
在 geometry_msgs/msg/Twist 中, 对应 linear.x, 对应 angular.z。普通双轮差速底盘不能独立实现 linear.y 指定的横向平移,也不能直接执行绕 X、Y 轴的旋转指令。

推导从车体速度到车轮速度的公式
机器人直线前进时,两轮的接地点沿前进方向具有相同速度 。当机器人同时以角速度 左转时,右轮需要比车体中心走得更快,左轮则更慢。
由于两个车轮中心分别位于车体中心左右 处,转向带来的切向速度增量为 。左右轮的线速度因此为:
车轮在无滑动条件下满足线速度等于角速度乘半径。记左右轮绕自身驱动轴的目标角速度为 和 ,即可得到差速底盘逆运动学公式:
输出单位为 rad/s,不是 m/s。这里约定两个车轮的正角速度都表示机器人向前运动;实际电机轴或 URDF 关节轴的正方向如果不同,应在驱动接口处进行符号转换。
公式还可以通过反向代入进行核验:
如果换算后的两轮速度代入上述公式不能恢复原始 和 ,应优先检查轮距是否使用了半轮距、左右轮是否交换以及角速度的符号。
用三种运动状态核对数值
固定轮距 m、轮半径 m,分别输入以下三组指令。表中正轮速表示车轮向前滚动,负轮速表示向后滚动。
| 运动状态 | (m/s) | (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 |
直线行驶时角速度为零,两轮角速度都等于 rad/s。原地左转时车体中心不发生平移,两轮必须等速反向旋转,才能产生正的偏航角速度。
对于第三组指令,转向引起的轮侧速度差值为 m/s。左轮线速度为 m/s,右轮为 m/s,再分别除以半径 m,就得到 和 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 节点。
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()预期输出:
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,后续再交给轮速闭环控制器执行。
接入电机前检查速度限制与符号
运动学公式假设左右轮可以达到任何计算出的目标速度,真实电机却存在最高转速。假设两轮允许的最大绝对角速度均为 rad/s,第三组算例的右轮目标为 rad/s,已经超过限制。
不能只把右轮裁剪到 rad/s 而保持左轮 rad/s,否则实际曲率会改变。可采用同比例缩放,将两轮速度一起乘以下面的系数:
此时 ,缩放后的左轮目标约为 rad/s,右轮为 rad/s。这样会同时降低车体线速度和角速度,但在理想无滑动模型下保持原有转弯曲率。实际控制中还应检查电机加速度和驱动器限流约束。
接入实体底盘时还需要逐项确认:
- 正方向:分别发送较小的正轮速,例如 rad/s,确认左右轮都推动机器人前进。若单侧反转,检查电机接线、编码器方向和关节轴符号。
- 速度单位:运动学输出为车轮 rad/s。若驱动器接收电机轴 RPM,必须先乘减速比,再乘 。例如车轮角速度为 rad/s、减速比为 时,电机轴目标约为 RPM。
- 参数测量:轮距使用左右驱动轮接地点对应轮中心的横向间距,不是外壳宽度;轮半径应采用校准后的有效滚动半径。
- 失联保护:实际机器人需要速度命令超时停车机制,例如在控制器中设置 s 的命令超时,避免上游停止发布后仍保持旧轮速。
在仿真或实体底盘上验收时,先发送 m/s、,确认机器人直行;再发送 、 rad/s,确认原地逆时针旋转;最后发送 m/s、 rad/s,确认左转且右轮速度更高。三组测试均符合预期,才说明速度映射、左右轮顺序和正方向约定基本一致。