4. 机器人运动控制接口：ROS2 Twist消息、差速轮运动学模型、发布速度指令控制机器人移动

好，咱们进入正题。这一章要聊的，是让机器人真正“动起来”的核心。你想想看，一个机器人如果连动都不会动，那还谈什么避障？所以，运动控制接口就是我们给机器人下达“走路”指令的通道。

我个人习惯把这一章看作是“机器人的腿脚”。你写代码，就是给它发号施令。而ROS2里最常用的“号令”，就是Twist消息。

4.1 认识Twist消息：机器人的“行动指令”

Twist消息，说白了就是一个数据结构。它告诉机器人：“你该往哪个方向走，走多快，或者怎么转。” 它包含两个部分：线速度（linear）和角速度（angular）。

我刚开始接触ROS时，总觉得这玩意儿有点抽象。后来在项目里调试一台差速轮小车，才真正搞明白。你发给它一个Twist消息，它就按你的要求动。发错了，它就原地打转或者撞墙。

来看一下Twist消息的结构：

geometry_msgs/msg/Twist
    geometry_msgs/msg/Vector3 linear
        float64 x
        float64 y
        float64 z
    geometry_msgs/msg/Vector3 angular
        float64 x
        float64 y
        float64 z

这里要注意，对于大多数差速轮机器人，我们只关心linear.x（前进后退速度）和angular.z（旋转角速度）。其他分量通常设为0。为什么？因为差速轮的结构决定了它只能前后走和原地转圈，不能像全向轮那样横着飘。

4.2 差速轮运动学模型：为什么它能转弯？

好，现在你知道发什么指令了。但机器人收到指令后，内部是怎么处理的？这就涉及到差速轮运动学模型。

差速轮，顾名思义，靠两个轮子的速度差来实现转向。两个轮子同速同向，它就直走；一个快一个慢，它就转弯；两个轮子速度相反，它就原地掉头。

我记得有一次在实验室里，学生问我：“老师，为什么我发了角速度指令，它不走直线？” 我让他去检查轮子编码器数据。结果发现，左右轮子的实际速度跟指令对不上。这就是运动学模型没校准好。

差速轮的运动学公式其实很简单：

v = (v_right + v_left) / 2      # 机器人线速度
ω = (v_right - v_left) / d      # 机器人角速度，d是轮距

反过来，如果你知道目标线速度和角速度，也能算出左右轮的目标速度：

v_right = v + (ω * d) / 2
v_left  = v - (ω * d) / 2

嗯，这里要注意，d（轮距）这个参数非常关键。我曾经因为量错了轮距，导致机器人转弯半径完全不对，在狭窄走廊里差点撞到墙。所以，做仿真或者实物调试时，一定要确认这个参数。

4.3 发布速度指令：让机器人动起来

理论讲完了，咱们来点实际的。怎么在ROS2里发布Twist消息，让Gazebo里的机器人动起来？

我个人习惯写一个简单的发布节点。先创建一个包，然后写个Python脚本。来看代码：

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist

class CmdPublisher(Node):
    def __init__(self):
        super().__init__('cmd_publisher')
        self.publisher_ = self.create_publisher(Twist, '/cmd_vel', 10)
        self.timer = self.create_timer(0.1, self.timer_callback)

    def timer_callback(self):
        msg = Twist()
        msg.linear.x = 0.2   # 前进速度 0.2 m/s
        msg.angular.z = 0.1  # 左转速度 0.1 rad/s
        self.publisher_.publish(msg)
        self.get_logger().info('发布速度指令: linear.x=%.2f, angular.z=%.2f' % (msg.linear.x, msg.angular.z))

def main(args=None):
    rclpy.init(args=args)
    node = CmdPublisher()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

这段代码干了三件事：

• 创建了一个发布者，话题是/cmd_vel。这是ROS里约定俗成的速度指令话题。
• 每0.1秒发布一次Twist消息。
• 消息里设置了前进0.2米/秒，同时左转0.1弧度/秒。所以机器人会走一个弧线。

你可能会问：“为什么是/cmd_vel？能不能改？” 可以改，但几乎所有机器人驱动节点都默认监听这个话题。改了就得同步修改驱动节点，麻烦。所以，我建议你遵循惯例。

4.4 在Gazebo中验证

代码写好了，怎么验证？启动你的Gazebo仿真环境，然后运行这个节点。如果一切正常，你会看到机器人开始移动。

我个人习惯先让机器人直走，再让它转圈。这样可以快速判断运动学模型是否正确。比如：

• 只设置linear.x = 0.2，angular.z = 0.0 → 机器人应该直线前进。
• 只设置linear.x = 0.0，angular.z = 0.5 → 机器人应该原地旋转。
• 两个都设置 → 机器人走弧线。

如果机器人不走直线，或者转弯半径跟预期不符，那就要回头检查轮距参数或者编码器数据了。

4.5 本章知识体系

为了让你更直观地理解这一章的内容，我画了一张图。它展示了从你发布Twist消息，到机器人实际运动，整个链条是怎么串起来的。

这张图把整个流程串起来了。你写代码发布Twist消息，ROS2负责传递，运动学模型把线速度和角速度换算成左右轮速度，电机驱动执行，最后机器人动起来。每一步都环环相扣。

好了，这一章的内容就到这里。你掌握了Twist消息的结构，理解了差速轮的运动学模型，也学会了怎么发布速度指令。接下来，你可以试着修改代码里的速度值，看看机器人会怎么运动。实践出真知。

可可精品资料库