最近在机器人开发圈子里优必选UBTECH机器人在世界机器人大会WRC上的精彩舞蹈表演无疑是一个技术展示的高光时刻。这背后不仅仅是预先编排的动作播放更涉及机器人运动控制、实时通信、多机协同等一系列复杂技术的集成。对于开发者而言无论是想了解人形机器人的开发流程还是希望为自己的机器人项目无论是实体机器人还是仿真应用添加复杂的动作序列掌握一套从动作设计到程序执行的完整方法论都至关重要。本文将从一个开发者的实战视角出发深度拆解如何为机器人以人形机器人为例编排并执行一套类似“跳舞”的复杂动作。我们将从最基础的单关节运动控制讲起逐步深入到动作序列编排、多机协同通信并最终探讨在资源受限的真实硬件上如何优化性能。文中将提供基于ROS 2机器人操作系统的完整代码示例、配置文件和避坑指南无论你是机器人专业的学生还是正在从事工业机器人、服务机器人开发的工程师都能从中获得可直接复用的经验。1. 背景与核心概念机器人舞蹈背后的技术栈在深入代码之前我们有必要厘清几个核心概念。机器人“跳舞”本质上是一个运动规划与控制问题它要求机器人在特定时间内其各个关节舵机/电机能够平滑、准确地运动到一系列预设的位置。运动控制这是最底层的能力。每个关节如膝关节、肘关节由一个伺服电机舵机驱动。控制的核心是给每个舵机发送目标角度或位置指令。高级的控制还会涉及速度、加速度甚至扭矩的规划。动作序列一个舞蹈动作是由一连串的“姿势”Pose按时间顺序组成的。每个“姿势”定义了在某个时刻机器人所有关节的目标角度。将这些姿势串联起来就形成了动作。插值算法机器人不能从一个姿势“瞬移”到下一个姿势。我们需要通过插值算法如线性插值、三次样条插值来计算在两个关键姿势之间每一时刻每个关节应该处于的中间角度从而产生平滑的运动。通信与同步对于多台机器人协同舞蹈还需要解决机器人与控制主机之间以及机器人彼此之间的通信与时间同步问题。ROS 2的Topic、Service和Action机制在这里扮演了关键角色。资源管理在真实的机器人硬件上计算资源CPU、内存、电源和网络带宽都是有限的。动作文件的大小、控制指令的频率都需要精心设计以避免卡顿或失控。本次实战我们将以ROS 2 Humble作为主要的软件开发框架因为它提供了标准的通信中间件和丰富的工具链非常适合机器人开发。虽然优必选有其自研的系统和SDK但ROS 2的抽象和理念是相通的理解ROS 2下的开发流程能帮助你更快地适配其他平台。2. 环境准备与版本说明在开始编写跳舞程序前请确保你的开发环境已就绪。操作系统推荐 Ubuntu 22.04 LTS。这是ROS 2 Humble的官方支持系统。ROS 2版本Humble Hawksbill。这是当前的LTS版本拥有较好的稳定性和社区支持。编程语言Python 3.10 或 C 20。本文示例将主要使用Python因其开发迭代更快。额外工具rviz2ROS 2的可视化工具用于查看机器人模型和运动状态。ros2 bag用于录制和回放ROS 2话题数据可用于记录和复现舞蹈动作。安装ROS 2 Humble 如果你尚未安装请执行以下命令以Ubuntu为例# 设置语言环境 sudo apt update sudo apt install locales sudo locale-gen en_US en_US.UTF-8 sudo update-locale LC_ALLen_US.UTF-8 LANGen_US.UTF-8 export LANGen_US.UTF-8 # 添加ROS 2软件源 sudo apt install software-properties-common sudo add-apt-repository universe sudo apt update sudo apt install curl -y sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg echo deb [arch$(dpkg --print-architecture) signed-by/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release echo $UBUNTU_CODENAME) main | sudo tee /etc/apt/sources.list.d/ros2.list /dev/null # 安装ROS 2核心包 sudo apt update sudo apt install ros-humble-desktop python3-rosdep2 sudo rosdep init rosdep update # 配置环境变量 source /opt/ros/humble/setup.bash echo source /opt/ros/humble/setup.bash ~/.bashrc创建工作空间和功能包mkdir -p ~/robot_dance_ws/src cd ~/robot_dance_ws/src ros2 pkg create robot_dance_py --build-type ament_python --dependencies rclpy std_msgs sensor_msgs geometry_msgs cd ~/robot_dance_ws colcon build --symlink-install source install/setup.bash3. 核心原理与模块拆解一个完整的机器人舞蹈系统通常包含以下模块3.1 机器人模型与URDF我们需要一个描述机器人物理结构的文件即URDF。它定义了机器人的连杆、关节、外观以及关节的运动限位。虽然动作编排不直接依赖URDF但可视化rviz2和仿真需要它。一个简化的人形机器人躯干URDF片段如下!-- 文件~/robot_dance_ws/src/robot_dance_py/urdf/simple_humanoid.urdf -- ?xml version1.0? robot namesimple_humanoid link namebase_link visual geometry box size0.2 0.1 0.4/ /geometry /visual /link joint namewaist_joint typerevolute parent linkbase_link/ child linkupper_body/ origin xyz0 0 0.2 rpy0 0 0/ axis xyz0 0 1/ limit lower-1.57 upper1.57 effort100 velocity2.0/ /joint link nameupper_body visual geometry box size0.15 0.25 0.3/ /geometry /visual /link joint nameleft_shoulder_joint typerevolute parent linkupper_body/ child linkleft_upper_arm/ origin xyz0.1 0.15 0.1 rpy0 0 0/ axis xyz0 1 0/ limit lower-2.0 upper2.0 effort100 velocity3.0/ /joint link nameleft_upper_arm.../link !-- 更多关节和连杆定义 -- /robot3.2 动作文件格式设计我们需要一种格式来存储舞蹈动作序列。一个简单实用的格式是JSON或YAML它易于人类阅读和编辑。示例JSON格式{ dance_name: wave_and_step, fps: 30, joint_names: [waist_joint, left_shoulder_joint, right_shoulder_joint, left_knee_joint, right_knee_joint], keyframes: [ { time: 0.0, positions: [0.0, 0.0, 0.0, 0.0, 0.0] }, { time: 1.0, positions: [0.3, -0.5, 0.5, 0.2, 0.2] }, { time: 2.0, positions: [-0.3, 0.5, -0.5, -0.2, -0.2] }, { time: 3.0, positions: [0.0, 0.0, 0.0, 0.0, 0.0] } ] }fps控制回放帧率影响动作平滑度。joint_names需要控制的关节名称列表必须与URDF和控制器中的名称一致。keyframes关键帧列表。每个关键帧包含时间戳秒和对应时刻所有关节的位置弧度。3.3 运动插值器这是舞蹈流畅与否的核心。我们使用线性插值作为入门。对于第i个关节在两个关键帧(t1, pos1)和(t2, pos2)之间在时刻t的位置计算公式为pos(t) pos1 (pos2 - pos1) * (t - t1) / (t2 - t1)对于更平滑的运动如要求速度连续可以使用三次样条插值但这需要更复杂的数学计算。3.4 ROS 2 控制器接口在ROS 2中我们通常通过发布话题消息来控制关节。常见的消息类型是sensor_msgs/msg/JointState用于状态反馈或std_msgs/msg/Float64MultiArray用于直接位置控制。更规范的做法是使用controller_manager和joint_trajectory_controller但对于理解原理我们直接从话题开始。4. 完整实战案例编写一个单机器人舞蹈节点让我们一步步实现一个能读取动作文件并控制机器人仿真模型跳舞的ROS 2节点。4.1 项目结构~/robot_dance_ws/src/robot_dance_py/ ├── package.xml ├── setup.py ├── setup.cfg ├── resource/robot_dance_py ├── test ├── launch/ # 启动文件目录 │ └── dance.launch.py ├── urdf/ # 机器人模型 │ └── simple_humanoid.urdf ├── config/ # 配置文件 │ └── wave_dance.json # 舞蹈动作文件 └── robot_dance_py/ ├── __init__.py └── dance_player_node.py # 主程序4.2 创建舞蹈动作文件将前面设计的JSON动作文件保存为config/wave_dance.json。4.3 编写舞蹈播放节点创建主程序文件robot_dance_py/dance_player_node.py。#!/usr/bin/env python3 # 文件robot_dance_py/dance_player_node.py import rclpy from rclpy.node import Node from sensor_msgs.msg import JointState import json import time import os from ament_index_python.packages import get_package_share_directory class DancePlayerNode(Node): def __init__(self): super().__init__(dance_player) self.get_logger().info(舞蹈播放节点启动) # 1. 加载舞蹈动作文件 package_dir get_package_share_directory(robot_dance_py) dance_file_path os.path.join(package_dir, config, wave_dance.json) self.get_logger().info(f加载动作文件: {dance_file_path}) try: with open(dance_file_path, r) as f: self.dance_data json.load(f) except FileNotFoundError: self.get_logger().error(f动作文件未找到: {dance_file_path}) raise except json.JSONDecodeError as e: self.get_logger().error(fJSON解析错误: {e}) raise self.joint_names self.dance_data[joint_names] self.keyframes self.dance_data[keyframes] self.fps self.dance_data.get(fps, 30) self.cycle_time 1.0 / self.fps # 2. 创建关节状态发布者 # 假设我们控制一个名为joint_states的话题rviz2和许多控制器会订阅它 self.joint_state_pub self.create_publisher( JointState, joint_states, 10 ) # 3. 创建定时器以固定频率发布关节状态 self.timer self.create_timer(self.cycle_time, self.timer_callback) self.start_time self.get_clock().now() self.current_keyframe_index 0 self.get_logger().info(f准备播放舞蹈: {self.dance_data.get(dance_name, 未命名)}) self.get_logger().info(f关节数: {len(self.joint_names)}, 关键帧数: {len(self.keyframes)}) def linear_interpolate(self, t, t1, pos1, t2, pos2): 线性插值 if t2 t1: return pos1 ratio (t - t1) / (t2 - t1) # 确保比例在[0, 1]之间防止计算误差 ratio max(0.0, min(1.0, ratio)) return pos1 ratio * (pos2 - pos1) def timer_callback(self): 定时器回调函数计算并发布当前时刻的关节状态 # 计算从开始到现在的时间 elapsed (self.get_clock().now() - self.start_time).nanoseconds / 1e9 # 寻找当前时间所在的关键帧区间 while (self.current_keyframe_index 1 len(self.keyframes) and elapsed self.keyframes[self.current_keyframe_index 1][time]): self.current_keyframe_index 1 # 如果舞蹈已播放完则停止定时器或循环播放 if self.current_keyframe_index len(self.keyframes) - 1: self.get_logger().info(舞蹈播放完毕) self.timer.cancel() # 如果想循环播放可以重置self.current_keyframe_index 0; self.start_time self.get_clock().now() return # 获取当前和下一个关键帧 kf_current self.keyframes[self.current_keyframe_index] kf_next self.keyframes[self.current_keyframe_index 1] t1, t2 kf_current[time], kf_next[time] pos1_array, pos2_array kf_current[positions], kf_next[positions] # 对每个关节进行插值 current_positions [] for i, joint_name in enumerate(self.joint_names): interp_pos self.linear_interpolate(elapsed, t1, pos1_array[i], t2, pos2_array[i]) current_positions.append(interp_pos) # 构建并发布JointState消息 msg JointState() msg.header.stamp self.get_clock().now().to_msg() msg.name self.joint_names msg.position current_positions # 速度和力可以为空由底层控制器处理 msg.velocity [] msg.effort [] self.joint_state_pub.publish(msg) # 可选打印日志调试用 # self.get_logger().debug(f发布关节状态: {current_positions}) def main(argsNone): rclpy.init(argsargs) node DancePlayerNode() try: rclpy.spin(node) except KeyboardInterrupt: node.get_logger().info(节点被用户中断) finally: node.destroy_node() rclpy.shutdown() if __name__ __main__: main()4.4 创建启动文件为了便于启动机器人模型和我们的舞蹈节点创建一个launch文件launch/dance.launch.py。# 文件launch/dance.launch.py from launch import LaunchDescription from launch_ros.actions import Node from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch_ros.substitutions import FindPackageShare def generate_launch_description(): # 参数是否使用GUI use_gui LaunchConfiguration(use_gui, defaulttrue) # 参数URDF文件路径 urdf_file PathJoinSubstitution([ FindPackageShare(robot_dance_py), urdf, simple_humanoid.urdf ]) return LaunchDescription([ DeclareLaunchArgument( use_gui, default_valuetrue, description是否启动joint_state_publisher的GUI ), # 启动 robot_state_publisher发布机器人TF Node( packagerobot_state_publisher, executablerobot_state_publisher, namerobot_state_publisher, outputscreen, arguments[urdf_file] ), # 启动 joint_state_publisher可选用于手动调节关节 Node( packagejoint_state_publisher, executablejoint_state_publisher, namejoint_state_publisher, outputscreen, parameters[{use_gui: use_gui}] ), # 启动我们的舞蹈播放节点 Node( packagerobot_dance_py, executabledance_player_node, namedance_player, outputscreen ), # 启动rviz2进行可视化 Node( packagerviz2, executablerviz2, namerviz2, arguments[-d, PathJoinSubstitution([ FindPackageShare(robot_dance_py), config, view_robot.rviz # 需要预先保存一个rviz配置 ])], outputscreen ), ])4.5 修改package.xml和setup.py确保package.xml包含了所有依赖!-- 在package.xml的export标签前添加 -- exec_dependrclpy/exec_depend exec_dependstd_msgs/exec_depend exec_dependsensor_msgs/exec_depend exec_dependgeometry_msgs/exec_depend exec_dependjoint_state_publisher/exec_depend exec_dependrobot_state_publisher/exec_depend exec_dependrviz2/exec_depend exec_dependlaunch_ros/exec_depend在setup.py中注册节点入口点# 在setup.py的setup函数中找到entry_points entry_points{ console_scripts: [ dance_player_node robot_dance_py.dance_player_node:main, ], },4.6 运行与验证编译工作空间cd ~/robot_dance_ws colcon build --symlink-install source install/setup.bash启动舞蹈ros2 launch robot_dance_py dance.launch.py观察结果如果一切正常rviz2窗口将打开并显示你的机器人模型。随后机器人将开始按照wave_dance.json中定义的动作进行运动挥手、踏步等。5. 进阶多机器人协同与性能优化单个机器人跳舞只是第一步。世界机器人大会上的表演往往是多机协同。这引入了新的挑战同步。5.1 基于ROS 2 Time的同步我们可以利用ROS 2的/clock话题在仿真中或网络时间协议NTP来实现软同步。一个简单的主从模式设计如下主节点负责播放舞蹈动作并发布一个“开始信号”话题消息中包含主节点的开始时间戳。从节点订阅“开始信号”话题。收到信号后根据主节点的时间戳校准自己的本地时钟偏移然后开始播放相同的动作文件。主节点发布信号# 在主节点的__init__中 self.start_signal_pub self.create_publisher(String, /dance_start_signal, 10) # 在准备开始跳舞时 start_msg String() start_msg.data str(self.start_time.nanoseconds) # 发布纳秒时间戳 self.start_signal_pub.publish(start_msg)从节点同步# 在从节点的订阅回调中 def start_signal_callback(self, msg): master_start_ns int(msg.data) self.start_time self.get_clock().now() # 从节点当前时间 self.time_offset self.start_time.nanoseconds - master_start_ns # 之后在插值计算时使用 (elapsed - self.time_offset) 来对齐主节点时间线5.2 资源受限优化在真实的机器人硬件如基于ESP32、树莓派等上需要优化动作数据压缩不使用JSON改用二进制格式如struct.pack存储动作数据大幅减少内存占用和加载时间。降低控制频率并非所有关节都需要高频率控制。对于慢速运动可以降低发布频率如从30Hz降到10Hz。预计算插值如果动作是固定的可以在上位机预计算好每一帧的所有关节位置机器人端只需按顺序读取和执行减少实时计算开销。通信优化使用ROS 2的Fast DDS或Cyclone DDS并合理设置QoS策略如BestEffort而非ReliableVolatile而非TransientLocal以减少网络延迟和带宽占用。6. 常见问题与排查思路问题现象可能原因排查步骤与解决方案rviz2中看不到机器人模型1. URDF文件路径错误或语法错误。2.robot_state_publisher节点未启动或崩溃。3. TF树未正确发布。1. 检查终端是否有URDF解析错误。使用ros2 run urdfdom check_urdf your.urdf验证URDF。2. 使用ros2 node list和ros2 topic list确认节点和话题是否存在。3. 在rviz2中添加TF显示并检查是否有坐标系。机器人关节不动1. 舞蹈节点发布的话题名与joint_state_publisher或控制器订阅的话题名不匹配。2. 关节名称不匹配。3. 插值计算错误所有位置为0。1. 使用ros2 topic echo /joint_states查看是否有数据发布。使用ros2 topic info /joint_states查看发布者和订阅者。2. 核对URDF、动作文件、代码中的关节名称是否完全一致大小写敏感。3. 在代码中添加调试打印输出计算出的关节位置。动作卡顿、不流畅1. 定时器周期 (cycle_time) 不稳定或过长。2. 主循环计算耗时过长。3. 网络延迟在多机协同时。1. 使用ROS 2的rqt_graph和rqt_console检查系统负载和消息延迟。2. 优化代码避免在回调函数中进行复杂计算或阻塞IO。3. 对于多机检查网络状况考虑使用更高效的序列化方式或降低数据频率。多机器人不同步1. 网络时钟不同步。2. 开始信号传输延迟不一致。3. 各机器人计算性能差异。1. 在所有机器上配置NTP服务同步系统时钟。2. 使用硬件时间同步或PTP协议对于高精度要求。3. 采用“准备-就绪-执行”三步握手协议确保所有机器人真正同时开始。7. 最佳实践与工程建议模块化设计将动作加载器、插值器、通信模块分离。这样便于单独测试和替换例如未来可以轻松更换为更高级的样条插值器。参数化配置将关节名称、控制话题、帧率等所有可配置项通过ROS 2参数服务器或YAML文件进行管理避免硬编码。异常处理与日志如示例代码所示对文件读取、JSON解析、数组越界等操作进行完善的异常捕获并记录不同级别的日志INFO, WARN, ERROR便于线上排查。仿真先行在将舞蹈程序部署到昂贵的实体机器人如优必选Walker之前务必在Gazebo、Webots或MuJoCo等仿真环境中进行充分测试验证动作的合理性和安全性防止自碰撞或失去平衡。动作设计工具链考虑开发或使用图形化动作设计工具如Blender插件、ROS的moveit助手通过拖拽或关键帧录制来生成动作文件这比手动编写JSON高效且直观得多。版本控制动作库将不同的舞蹈动作文件JSON/YAML纳入Git等版本控制系统进行管理方便团队协作和回滚。安全第一为每个关节设置软件限位在代码中约束插值结果使其不超过URDF中定义的硬件限位。在动作设计中预留足够的稳定裕量避免机器人做极端的、可能导致损坏的动作。从单关节控制到复杂的多机协同舞蹈我们走完了一个完整的机器人动作开发流程。关键在于理解“动作即数据控制即播放”的核心思想并利用好ROS 2这样的工具链来处理通信、调度和可视化等繁琐问题。下一步你可以尝试集成IMU传感器数据实现带平衡补偿的舞蹈。为动作添加音乐节奏点让舞蹈更具表现力。探索使用moveit进行更复杂的、带有避障的路径规划式“舞蹈”。将整个系统部署到真实的机器人平台上感受从虚拟到现实的挑战与乐趣。