从EtherCAT到MoveIt2：六轴机械臂完整控制链路搭建实战

最近帮人调一台EtherCAT总线的六轴机械臂，从硬件上电到能在MoveIt2里拖拽规划，整整折腾了三天。网上资料不是零散就是版本对不上，很多教程只讲到“单独跑通EtherCAT”或者“单独演示MoveIt2”，真正把ROS2 Control、EtherCAT主站、伺服驱动器和MoveIt2串成完整链路的内容特别少。这篇文章就把我从底层启动到运动规划的完整过程写下来，包括每一步为什么要这么做、命令怎么敲、坑踩在哪里。适合已经接触过ROS2基础概念，但第一次要把真实机械臂接进MoveIt2的开发者参考；如果你用的是自己的六轴机械臂，哪怕不是同一款驱动器，思路也完全通用。

1. 先把整个系统拆清楚

1.1 三层架构，每层各干一件事

一套能跑运动规划的机械臂系统，从下往上其实是三层：EtherCAT主站负责和伺服驱动器交换数据，ROS2 Control负责把EtherCAT收上来的裸数据变成ROS里的关节状态和指令接口，MoveIt2负责算轨迹、做碰撞检测。三层各司其职，中间用话题和Action连接起来。

我见过很多刚接触的人容易犯一个错误，就是觉得MoveIt2可以直接控制伺服。实际上MoveIt2只是一个“大脑”，它算出来的是关节角度序列，也就是轨迹，真正把这些角度写进驱动器让电机转起来的，是ROS2 Control里的controller，而controller又是通过硬件接口（hardware_interface）去调用EtherCAT主站完成通讯。所以整个链路是：

MoveIt2规划轨迹 -> joint_trajectory_controller接收轨迹 -> 读取硬件接口的指令接口 -> EtherCAT主站周期性发送位置命令 -> 伺服驱动器执行 -> 编码器读数通过EtherCAT返回 -> 硬件接口更新状态接口 -> /joint_states发布 -> MoveIt2和RViz显示实际位姿。

每一层都有自己独立的问题域。EtherCAT层考虑的是实时同步和报文能不能稳定收发，ROS2 Control层考虑的是接口怎么抽象、复用，MoveIt2层考虑的是运动学求解和路径规划。调试时最大的忌讳是把这三层混在一起找问题，后面我单独会用一节讲如何分层排查。

1.2 为什么选EtherCAT，而不是CANopen或Modbus

现在工业机器人上最主流的总线方案基本就是EtherCAT，原因就两个：速度快、同步精度高。CANopen能做到1ms左右的周期已经是极限了，而EtherCAT在六轴机械臂这种场景下跑1ms非常轻松，很多配置甚至能把周期压到500us或者250us。EtherCAT的分布式时钟（DC）能保证所有轴在同一个时刻采样和输出，这对多轴联动非常关键，否则就会出现轨迹畸变甚至机构别劲。

对比Modbus TCP这种非实时的方案，EtherCAT用的是主站发一帧数据，所有从站在这一帧里同时读写下自己的数据，然后帧再传回主站的机制，时延极低。机械臂在运动过程中，每一个周期都要同步刷新六个关节的目标位置，任何一轴的抖动都会被放大到末端，所以数据同步能力是刚需。

如果你自己搭机械臂，选EtherCAT驱动器的成本现在也下来了。国产伺服厂商基本都把EtherCAT作为标配功能，价格比传统脉冲型驱动器高不了太多，省去了单独拉脉冲线和编码器线的麻烦，控制柜走线也清爽很多。

1.3 版本选型与兼容性组合

我的目标平台是Ubuntu 22.04，对应的ROS2发行版是Humble，MoveIt2和ros2_control在Humble下都有稳定的二进制包。这个组合是目前最推荐的，原因不是“新”，而是生态最完整。ROS2的版本节奏比较快，Iron以上版本确实更新，但很多第三方库和教程还停留在Humble，遇到问题能查到的资料多，踩坑成本低。

EtherCAT主站这块有两个方向：一个是内核态的IGH（EtherLab），稳定性和实时性更好，但内核版本一升级，驱动模块经常要重新编译，对新手不友好；另一个是用户态的SOEM（Simple Open EtherCAT Master），部署简单，不依赖内核模块，适合快速验证和教学。如果你做的是产线级设备，我建议用IGH；如果是为了把系统跑通、跑完再决定要不要换IGH，SOEM完全够用。

我最后用的是IGH加Humble的组合，下面就按这个组合来写。如果你用的是SOEM，整体流程类似，差别主要在EtherCAT主站的启动方式上。

2. 环境准备：Ubuntu22.04上的全套工具链

2.1 实时性处理与EtherCAT主站安装

先说明一下，很多人上来就在普通内核上跑IGH，结果发现控制周期只要到1ms，系统就各种卡顿。EtherCAT主站虽然是内核态驱动，但控制线程还是跑在用户态的，如果系统调度不给力，周期抖动会非常大。所以我强烈建议至少在工控机上安装一个实时内核，比如linux-rt或者带PREEMPT_RT补丁的内核。Ubuntu上可以通过安装linux-image-rt-amd64这类包来获得实时内核。

IGH的安装属于整个链路里最容易卡住的环节，因为不同内核版本要打不同的补丁。我直接说一个我试验过能用的流程：先拿到和你当前内核版本匹配的IGH源码，一般直接下1.5.2版本就可以，在编译之前检查一下补丁是否需要；编译时用./configure --prefix=/opt/etherlab，然后make && sudo make install。安装完成之后，需要把从站描述文件放到IGH能找到的位置，然后配置网卡的MAC地址。

在/etc/ethercat.conf里，核心配置就两个：一个是指定主站绑定的网卡设备名，另一个是设置设备驱动模块。大部分Linux自带网卡驱动都还行，Intel和Realtek的千兆网卡踩坑最少。

如果ethercat slaves扫描不到从站，先不要怀疑软件，先查网线连接是不是OK，EtherCAT对线序和连接器质量很敏感。

2.2 安装ROS2 Humble、MoveIt2、ros2_control

假设你已经装好了Ubuntu 22.04和ROS2 Humble的base环境，接下来几条命令搞定核心依赖：

sudo apt install ros-humble-ros-base sudo apt install ros-humble-moveit ros-humble-moveit-setup-assistant sudo apt install ros-humble-ros2-control ros-humble-ros2-controllers sudo apt install ros-humble-joint-state-broadcaster ros-humble-joint-trajectory-controller

MoveIt2在Humble下建议直接装二进制包，源码编译适合你想改MoveIt2内部实现的情况。对于绝大多数人，二进制的版本已经够用，而且省时间。ros2-controllers这个包里包含了我们后面要用到的joint_trajectory_controller、joint_state_broadcaster等标准控制器。

这里有一个经验：装好之后先跑一下ros2 pkg list | grep moveit确认版本，再跑ros2 control list_controllers看看能不能正常操作。先让ROS侧的工具链处于一个“确认可用”的状态，再和EtherCAT对接，可以省掉很多问题排查时间。

2.3 验证软件栈基本可用

一个非常推荐的验证方法：在还没有接真实机械臂之前，先用URDF加fake hardware把MoveIt2和ros2_control这一层跑通。fake hardware的意思是模拟一块硬件接口，读取指令接口的数据然后原样写回状态接口，相当于假装电机能响应。ROS2 Control里内置了fake_components，你只需要在URDF的ros2_control标签里把type配置成fake_components/GenericSystem即可。

跑通fake硬件有什么好处呢？你可以在没有机械臂的情况下验证MoveIt2生成的轨迹能不能被joint_trajectory_controller正确接收，RViz里的机器人能不能动起来，MoveIt2的规划结果和实际执行的状态反馈能不能对上。这一层通了，后面接真实硬件遇到问题时，就能大概率确定问题出在EtherCAT这一侧，而不是MoveIt2配置侧。

3. EtherCAT总线调试：从扫描从站到伺服使能

3.1 线缆拓扑与主站配置

EtherCAT的拓扑是菊花链式，也就是一个从站接着一个从站串下去，最后一个从站不再往下接。机械臂控制柜里，六个伺服驱动器和一个耦合器（如果需要的话）就是按这种链路串起来的。启动EtherCAT主站之后，它会在每个周期发送报文，从站收到报文后取出自己需要的数据，插入自己的数据，再往下传。最后一个从站如果不接回主站，叫开环拓扑，报文不会再回到主站，但实际调试中绝大多数系统并不会把最后一个从站再连回主站，而是依赖从站的“看门狗”和主站的心跳机制来确认链路状态。

确认主站配置正确之后，启动主站：

sudo /etc/init.d/ethercat start ethercat slaves

如果你的从站都上电且线缆正常，这里会列出所有从站的厂商ID、产品码和名字。如果只能扫到一个或者完全扫描不到，优先查网线、从站电源、最后一个从站的终端电阻设置，这三个是EtherCAT前期最常出问题的点。

扫描到从站之后，可以用ethercat slaves -v看到每个从站的详细信息和SM（SyncManager）配置。SM是EtherCAT从站里用于邮箱通信和过程数据交换的内存管理单元，我们后续要配置的PDO映射，最终就是要落到SM的通道里。

3.2 PDO映射与FMMU/SM是什么关系

这里必须把PDO、FMMU和SM这三个概念讲清楚，否则后面配置的时候很容易一头雾水。SM是每一个从站内部的同步管理器，负责管理输入输出过程数据的交换；FMMU是主站内存里面的一个映射单元，主站通过FMMU把一个逻辑地址范围映射到从站的物理地址，然后每个从站就知道这一帧报文里哪一段是自己的数据。

简单理解：主站和从站之间跑的是一辆固定路线的班车，FMMU相当于给每个从站划定了“在第几站下车、在第几站上车”，SM相当于告诉从站“这个站点的货物应该放到你心里的哪个仓库”。PDO映射就是把伺服驱动器里的目标位置、控制字、状态字、实际位置这些参数塞到这个站点仓库的指定货架上。

在IGH里，配置从站的PDO是通过ethercat命令或编写SII描述文件来完成的。如果你用的是现成的驱动器，厂商一般会提供ESI文件，IGH会从从站的EEPROM里读取这些信息，大部分情况下主站能自动识别出默认PDO映射。但需要验证映射是否满足你的需求。

我在配置中经常遇到的一个情况是：驱动器默认PDO只包含部分对象字典项，比如可能只有实际位置和控制字，但没有实际速度。这种情况需要用SDO或者PDO映射命令把缺失的对象字典项加进去。常用的PDO对象字典项包括：

• 控制字（6040h）：控制伺服使能、运行、停止等状态
• 状态字（6041h）：读取伺服当前状态机的状态
• 目标位置（607Ah）：位置模式下控制器下发的目标位置
• 实际位置（6064h）：编码器反馈的实际位置
• 目标速度（60FFh）和实际速度（606Ch）：速度模式使用
• 操作模式（6060h）和操作模式显示（6061h）：选择位置/速度/扭矩模式

3.3 用ethercat命令验证通信并完成伺服使能

在ROS2 Control接入之前，一定要先用ethercat命令直接对伺服进行操作，确认每条指令能发出去，编码器反馈能读回来。这一步不要跳过，因为如果跳过，后面ROS2 Control一旦出问题，你根本不知道是ROS这一侧的问题还是底层总线的问题。

先用这个命令看PDO映射是否完整：

ethercat pdos

然后进入周期数据预览模式，可以实时看到主站和从站交换的数据：

ethercat pdos -v

看到数据在更新之后，再用SDO读写验证伺服状态：

ethercat sdo read 0x6060 # 读取操作模式 ethercat sdo write 0x6060 0x08 > /dev/null # 切换到CSP模式 ethercat sdo read 0x6041 # 读取状态字

这里我建议使用CSP（Cyclic Synchronous Position）模式，也就是循环同步位置模式。为什么用CSP而不是老老实实走Pp（Profile Position）模式？因为CSP模式下，每个周期主站都下发一个目标位置，伺服内部会做位置环和速度环的插补，这样多个轴之间的同步由主站周期协调，非常适合六轴机械臂这种多轴联动场景。Pp模式下每个轴执行轮廓曲线时的插补逻辑是驱动器自己的，同步效果跨轴会差一些。

伺服使能的过程就是按照CiA402的标准状态机，一步一步把状态字从“Switch on disabled”推到“Operation enabled”。手动用SDO写控制字也能使能，但实际操作中我更推荐直接靠ROS2 Control的硬件接口来控制控制字，这样使能逻辑和ROS节点生命周期连在一起，更安全。当然第一次测试时用ethercat sdo write 0x6040 0x06 0x80这种命令手动使能也是可以的，确认驱动器和抱闸逻辑没有问题了再转到ROS侧。

4. ROS2 Control硬件接口：让机械臂进入ROS世界

4.1 在URDF里接上ros2_control

现在开始到了ROS2 Control的关键环节。URDF里要加<ros2_control>标签，这个标签描述的是机械臂每个关节的“接口能力”：有哪些command interface和state interface。对于机械臂关节，状态接口一般要三个：position、velocity、effort；指令接口一般只需要position或velocity。

一个标准的关节定义大致长这样：

<ros2_control name="RealArm" type="system"> <hardware> <plugin>my_ethercat_hardware/EthercatSystem</plugin> </hardware> <joint name="joint1"> <command_interface name="position"> <param name="min">-3.14</param> <param name="max">3.14</param> </command_interface> <state_interface name="position"/> <state_interface name="velocity"/> </joint> <!-- joint2 ~ joint6 同理 --> </ros2_control>

注意这里的type是system，对应我们自定义的硬件插件。ROS2 Control提供了SystemInterface（系统级硬件接口）和ActuatorInterface（致动器级硬件接口）两种基类。对机械臂这种每个关节都有独立伺服驱动器的场景，我推荐用SystemInterface，因为你可以把整条EtherCAT总线的管理逻辑放在一个地方统一处理，而不需要为每个关节单独分配一个硬件实例。

4.2 写一个SystemInterface硬件插件

写硬件插件是实现整个系统里最核心的动作。你需要在C++中继承hardware_interface::SystemInterface，并实现若干关键方法。核心要理解的是这两个方法：

• on_read：在每个控制周期从EtherCAT主站读取所有从站的反馈数据，并写到state interfaces里。
• on_write：在每个控制周期把所有command interfaces里要下发的目标值写到对应的EtherCAT报文数据区里。

为什么必须用这两个方法？因为ros2_control_node的实时控制循环会周期性调用它们，这个周期就是我们之前配置的1ms或者更小。为了满足实时性，on_read和on_write里不能有动态内存分配、不能有Mutex锁等可能阻塞的操作，要尽量保证确定性执行。

我实现时的做法是，在on_init阶段建立EtherCAT主站连接、初始化PDO映射、将各关节的编码器零点偏置都读出来；在on_activate阶段把伺服使能（写控制字走状态机），同时做好位置单位换算；在read和write里只做数值转换和报文读写。数值转换这块很容易漏：ROS2 Control内部用的单位是弧度（rad）和服务器的弧度每秒，而驱动器通常使用脉冲（counts）或用户自定义单位。假设编码器一圈是65536个脉冲，减速比为100，那么写目标位置时要把弧度值乘以（100*65536/（2π））换算成脉冲，读实际位置时反过来除以这个系数。

// 只贴关键循环 return_value EthercatSystem::on_write(const rclcpp_lifecycle::State& /*state*/) { for (size_t i = 0; i < num_joints_; ++i) { double target_rad = hw_commands_[i]; int32_t target_counts = static_cast<int32_t>(target_rad * kRadToCounts); ecat_slaves_[i].set_target_position(target_counts); } // 实际调用IGH的周期发送函数 ec_master_send(master_); return OK; }

写完插件后，别忘了在plugin.xml里声明，并在package.xml里加依赖。然后重新编译你的包，确保没有编译错误，再继续。

4.3 启动controller_manager并验证关节状态

硬件插件写好后，先别着急接MoveIt2，先把controller_manager跑起来验证最基本的关节状态能不能发布。启动机器人的launch文件一般需要做三件事：加载URDF、启动robot_state_publisher、启动controller_manager。

在launch里，可以直接用命令行参数把URDF传给robot_state_publisher，然后加载下面两个控制器：

• joint_state_broadcaster：发布所有关节状态到/joint_states话题
• joint_trajectory_controller：提供follow_joint_trajectory的Action服务，供MoveIt2使用

启动后验证一下：

ros2 controller list ros2 topic hz /joint_states ros2 topic echo /joint_states --once

如果/joint_states里的position数据随机械臂转动而变化，说明EtherCAT和ROS2 Control这一层已经通了。此时如果你想手动让某个关节动一下，可以直接用ros2 action或ros2 topic往joint_trajectory_controller发一个简单轨迹，确认伺服能按照指令转动。

这一步是整条链路中“从硬件到软件”的第一次闭环，如果走到这里，后面的MoveIt2基本上就只是配置问题了。

5. MoveIt2运动规划：让机械臂看懂目标点

5.1 用MoveIt Setup Assistant生成配置

先运行MoveIt Setup Assistant生成moveit_config包：

ros2 run moveit_setup_assistant moveit_setup_assistant

在助手界面里加载你的URDF，然后配置几个东西：

• 设置自碰撞矩阵（Self-Collision），一般让助手自动生成默认矩阵就好，后续可以在RViz里显示碰撞检测结果。
• 定义Planning Group，比如一个叫arm的规划组包含从基座到末端的六个活动关节。注意这一步决定了MoveIt2在运动学求解时把哪些关节纳入考虑。
• 设置预定义姿态（Pre-defined Poses），比如home、vertical等，方便后面快速切换。
• 配置Controllers，这一步决定了MoveIt2怎么把规划出来的轨迹发给controller_manager。在生成的controllers.yaml里，把MoveIt2的controller名称设定为joint_trajectory_controller，action_ns对应follow_joint_trajectory即可。

其中最关键也是新手容易漏掉的，是在Setup Assistant里正确关联URDF中的ros2_control关节和MoveIt2的规划组。MoveIt2只知道规划组，不知道底层用什么controller执行，它只关心有没有一个符合FollowJointTrajectory规范的Action服务存在。所以如果controller_manager里加载了joint_trajectory_controller，MoveIt2就能通过action_ns找到它。

5.2 打通MoveIt2和joint_trajectory_controller

这里有一个非常隐蔽的点：joint_trajectory_controller默认要求收到的轨迹中每个轨迹点都带时间戳，时间戳必须单调递增，而且第一个点必须和目标状态兼容。MoveIt2生成的轨迹是满足这些要求的，但如果你手动发一些不带时间戳的测试轨迹，常常会看到controller拒绝了你的指令。这一点在调试时特别容易误导人，要留意。

另一个容易踩的坑是MoveIt2默认从/joint_states话题读取当前状态，如果joint_state_broadcaster没有启动，MoveIt2就会一直等待初始状态，看起来好像“卡住了”。所以启动顺序里，一定要先保证/joint_states有高频数据发出来，然后才启动move_group节点。

检查MoveIt2和controller的连接是否正常，可以观察move_group的日志。启动之后在RViz里设置目标位姿并点击Plan，如果计划成功，日志里会显示求解用时和碰撞检测信息；点击Execute之后，joint_trajectory_controller会开始按轨迹执行，机械臂开始运动。

5.3 从开机到运动规划的完整启动顺序

到这里，整条链路已经通了。我把最终的启动顺序整理一下，方便你以后调试时直接参考：

1. 给控制柜上电，等待伺服驱动器完成初始化，EtherCAT链路建立。
2. 启动EtherCAT主站并确认能从站扫描完整。
3. 启动机器人的URDF驱动节点，这个节点内部会初始化EtherCAT主站、加载硬件接口插件、启动controller_manager并加载controller。
4. 确认/joint_states有数据，确认controller_manager里joint_trajectory_controller已经active。
5. 启动move_group节点和RViz，加载MoveIt2配置。
6. 在RViz里设定目标位姿，执行Plan，检查轨迹和机械臂动作。

这套顺序的核心思想是：先让底层稳定再让上层介入，每一层都有明确的验证标准。不要图快，跳步一次，排查时就要多花几个小时。

6. 常见问题速查与调参心得

6.1 高频坑与解决对照表

下面这张表是我在调这类系统时遇到最多的问题，按出现频率排序，基本覆盖了80%以上的场景。

6.2 调试顺序建议

如果你遇到问题且一时拿不准在哪一层，我的习惯是从底层往上排查，每层只验证一个关键点。EtherCAT层先确认ethercat slaves输出稳定，再用ethercat pdos -v看数据是否在变化，最后用ethercat sdo write控制字手动使能，验证伺服电机本身没问题。ROS2 Control层只看/joint_states的数值是否跟随机械臂运动，不关心EtherCAT的细节。MoveIt2层只看规划日志和轨迹执行状态。

每层都验证通过之后再往上一层走，不要在某一层还没稳定时就急着去调上层。我见过太多人一边调整MoveIt2的参数，一边怀疑EtherCAT掉线，结果最后发现其实是网卡驱动在多核中断绑定上出了问题，导致EtherCAT周期不稳。底层不稳，上层怎么调都是白费。

6.3 一些底层参数调优经验

EtherCAT主站周期建议先用1ms跑通，稳定后再尝试500us或者更低。周期越小对CPU实时性要求越高，如果发现周期抖动加大，优先检查IGH的中断绑定是不是被系统调度到了不同的CPU核上，然后确认实时内核是不是真的生效。

伺服侧参数里，位置环增益和速度环增益建议先从驱动器厂商给的默认值开始，不要上来就追求高速响应。响应太快在机械结构刚性不足时会激发出共振，表现出来就是关节发尖啸或者抖动。我一般会先用很慢的轨迹跑一遍全行程，确认机械和伺服没有问题，再逐步提高速度和加速度限制。

最后再提一个小技巧：在MoveIt2里做首次规划前，先把规划时间限制设大一些，比如10秒，并选择RRTConnect这类规划器。首次运行时运动学求解可能需要更长时间来构建规划场景，不要一看到几秒钟没出结果就以为是死循环。调通之后再根据实际需求把参数收紧。

我在实际操作中还发现，控制柜里的接地和屏蔽对EtherCAT稳定性影响非常大。伺服驱动器的动力线如果和EtherCAT网线绑在同一个线槽里，干扰会直接反映在主站的掉线计数上。我花了一整天才意识到是线缆走线导致的干扰，换了屏蔽网线、把动力线和通讯线分槽走之后，问题彻底消失。建议你第一次布线时就把这个问题考虑进去，后面会省很多麻烦。