📚 课程目录第4章 EGM接口协议解析

4. EGM接口协议解析

EGM（Externally Guided Motion）是ABB机器人提供的一种实时外部引导运动接口，它允许上位机（PC、视觉控制器、力控计算机等）以固定周期向机器人控制器发送运动修正量，同时接收机器人当前状态。要真正用好EGM，不能只停留在“调用API”的层面，必须理解它在UDP之上定义的那套二进制协议：一个EGM数据包由哪些字段组成、消息类型如何区分、序列号和时间戳各自解决什么问题、位置/速度/姿态字段采用什么坐标约定、机器人又如何通过状态反馈把执行情况回传给上位机。本节围绕这些内容展开。

一、EGM通信的基本模型

EGM采用UDP单播通信，上位机作为客户端，机器人控制器作为服务端。双方约定一个端口（默认6510），上位机以固定周期（典型为4ms或8ms）发送EGM传感器消息（EGMSensor），机器人控制器则回送EGM反馈消息（EGMFeedback）。整个链路是“请求—响应”式的：上位机每发一包，控制器回一包，二者通过序列号和时间戳对齐。EGM本身不保证可靠传输，丢包、乱序、延迟都需要上位机自行处理，这也是理解协议字段的前提。

二、消息类型：Sensor 与 Feedback 的分工

EGM协议中最核心的两类消息是EGMSensor和EGMFeedback。EGMSensor由上位机发出，携带的是“我希望机器人如何运动”的信息，可以包含位置、姿态、速度、加速度以及外部轴数据；EGMFeedback由控制器回送，携带的是“机器人当前实际处于什么状态”的信息，包括实际关节角、实际TCP位姿、当前速度、状态码以及可能的错误信息。二者字段并非完全对称：Sensor消息中位置和姿态是可选组合，Feedback消息则通常同时给出关节空间和笛卡尔空间的实际值，便于上位机做闭环校正。

除这两类主消息外，协议还定义了若干辅助消息类型，例如用于握手和参数协商的EGM_Start、EGM_Stop，以及用于传递外部轴或力传感器数据的扩展消息。实际开发中，上位机通常先发送Start消息建立会话，随后进入周期性的Sensor/Feedback交换，结束时发送Stop消息释放资源。理解消息类型的关键在于：Sensor是“命令”，Feedback是“观测”，二者通过同一序列号空间关联。

三、序列号与时间戳：实时性的两条生命线

序列号（Sequence Number）是一个单调递增的整数，上位机每发一包Sensor消息就加一，控制器在Feedback中原样返回或返回当前已处理的序号。它的作用有三个：一是检测丢包，如果上位机发现连续两个Feedback的序号跳变，说明中间有包丢失；二是检测乱序，UDP不保证顺序，序号可以帮助上位机判断哪一包是旧数据；三是确认控制器是否跟上，如果Feedback序号长期落后于Sensor序号，说明控制器处理不过来，需要降低发送频率或减小运动修正量。

时间戳（Time Stamp）记录的是该数据包对应的采样时刻，通常以微秒或毫秒为单位。它的核心用途是时间对齐：上位机在计算目标位姿时，需要知道控制器反馈的实际位姿对应的是哪个时刻，才能正确估计延迟并做补偿。例如，若上位机在t时刻发送目标位姿，控制器在t+Δ时刻才实际到达，那么上位机在下一周期计算修正量时，就应基于t+Δ时刻的反馈值，而不是t时刻的旧值。时间戳还用于计算通信抖动，帮助判断网络是否稳定。

在调试EGM时，建议先只观察Feedback中的序列号和时间戳变化，确认通信周期稳定、无大量丢包后，再逐步加入位置和姿态修正量。这样可以把“通信问题”和“控制问题”分开排查。

四、位置、速度与姿态字段的坐标约定

EGM中位置字段通常以毫米为单位，姿态字段以四元数（q1, q2, q3, q4）表示。这里必须明确坐标约定：位置和姿态默认相对于机器人基坐标系（Base Frame），而不是工具坐标系或工件坐标系。如果上位机使用的是相机坐标系或工件坐标系下的目标点，必须先通过手眼标定或坐标变换矩阵转换到基坐标系，再填入Sensor消息。速度字段同样相对于基坐标系，单位为mm/s；加速度字段单位为mm/s²。四元数需要单位化，否则控制器可能拒绝该包或产生异常姿态。

一个常见的教学示例是：假设机器人当前TCP在基坐标系下的位置为(500, 0, 400) mm，姿态为单位四元数(1, 0, 0, 0)。上位机希望机器人沿基坐标系X方向以50 mm/s的速度移动，周期为4ms，则每个周期位置增量为50 × 0.004 = 0.2 mm。于是下一周期的目标位置为(500.2, 0, 400) mm，姿态保持不变。这个计算过程虽然简单，但它体现了EGM位置字段的“绝对目标”语义：Sensor消息中填的是目标位姿，而不是增量。若误填增量，机器人会以错误的方式运动。

五、状态反馈机制与异常判断

EGMFeedback中的状态字段是上位机判断机器人是否正常执行的关键。常见状态包括：EGM_RUNNING表示正在接收并执行外部引导；EGM_STOPPED表示已停止；EGM_ERROR表示发生错误，通常伴随错误码。上位机应每个周期检查状态，一旦发现非RUNNING状态，立即停止发送Sensor消息并进入安全处理流程。此外，Feedback中还会给出实际关节角与目标关节角的偏差，如果偏差持续增大，说明机器人可能遇到奇异点、关节限位或碰撞，需要及时干预。

下面给出一个简化的状态判断逻辑示例，用于说明上位机在每个周期应如何处理Feedback：

recv = udp_receive(timeout=2ms)
if recv is None:
    lost_count += 1
    if lost_count > 3:
        stop_egm()          # 连续丢包，停止发送
    continue

fb = parse_feedback(recv)
if fb.status != EGM_RUNNING:
    log_error(fb.status, fb.error_code)
    stop_egm()
    return

if fb.sequence != expected_seq:
    log_warning("sequence jump", fb.sequence, expected_seq)

# 基于反馈的实际位姿计算下一周期目标
target = compute_next_target(fb.actual_pose, fb.timestamp)
send_sensor(target, seq=expected_seq + 1)
expected_seq += 1

这段逻辑体现了EGM实时控制的基本节奏：收Feedback → 检查状态与序号 → 计算下一目标 → 发Sensor。其中状态检查必须放在最前面，任何异常都应优先保证安全停止，而不是继续发送运动指令。

六、协议解析中的常见问题

在实际项目中，EGM协议解析最容易出问题的地方有三处。第一是字节序：EGM使用小端序（Little-Endian），如果上位机用大端序解析，位置和姿态会变成完全错误的值。第二是字段偏移：不同RobotWare版本的EGM协议在字段顺序和保留字节上可能有差异，必须对照对应版本的协议文档，不能凭记忆硬编码。第三是时间戳单位：有的版本用微秒，有的用毫秒，混用会导致延迟补偿计算错误。建议在解析层封装一个版本适配模块，把字节偏移、单位换算和字段映射集中管理，避免散落在业务代码中。

此外，EGM对实时性要求较高，上位机的发送周期抖动应尽量小。如果使用普通Windows PC，建议将EGM线程优先级调高，并避免在该线程中做内存分配、日志写盘等阻塞操作。若条件允许，可使用实时Linux或带实时扩展的工控机，把周期抖动控制在百微秒级以内，这样Feedback中的时间戳才会稳定，闭环控制效果也更好。

EGM是实时运动接口，任何协议解析错误都可能直接导致机器人异常运动。在首次联调时，务必在机器人处于安全位置、速度倍率为低值、急停可随时触发的条件下进行，并先以只读方式观察Feedback，确认字段解析正确后再发送Sensor消息。

总结来说，EGM协议解析的核心是抓住“Sensor命令、Feedback观测、序列号对齐、时间戳补偿、状态码保护”这五条主线。只要把数据包结构、消息类型、坐标约定和状态反馈机制理解清楚，后续的位置控制、力控和视觉引导才能建立在可靠的基础之上。