一、整体四层通信分层

1. VR 眼镜（人机输入层）
2. 上位机（PC / 工控机，ROS2 算法层）
3. 机器人主控（嵌入式 Linux，UDP 桥接层）
4. 机械臂关节驱动器（底层运动执行层）

二、分段通信协议、实现原理、对应代码逐一详解

1. VR 眼镜 ↔ 上位机（PC/ROS2）通信协议

使用协议：WiFi 无线 + UDP 套接字 / ROS2 DDS（两种主流方案）

• VR 端（Meta/Pico 等）：Unity/Unreal 引擎，通过 WiFi UDP 实时发送手部追踪 IMU、关节姿态数据；
• 上位机接收后转为 ROS2 话题消息 RemoteMsgEnd，供运动插值节点使用。

实现流程

1. VR 手柄 / 手部传感器采集 6DoF 位姿、手指关节角度；
2. VR 引擎打包二进制 UDP 数据包，WiFi 广播到局域网上位机 IP；
3. 上位机 UDP 服务接收 VR 原始追踪数据，做坐标转换、运动学映射；
4. 转换为双臂 7 关节目标角度数组，发布 ROS2 话题 /remote_cmd（对应代码go.py）；
5. 插值节点（send_bezier_q_udp_1.py）读取话题，三次样条平滑，输出平滑关节指令。

对应代码支撑

go.py：ROS2 节点订阅 / 发布RemoteMsgEnd，做缓启动插值； send_bezier_q_udp_1.py：接收目标关节，平滑插值生成连续指令。

2. 上位机（ROS2 工控机） ↔ 机器人主控（嵌入式板）通信协议

两层协议嵌套（ROS2 DDS + 自定义 UDP 二进制协议）

上层：ROS2 DDS（进程内 / 跨机器进程通信）

• 协议：ROS2 DDS（Data Distribution Service），分布式实时中间件；
• 交互话题：
  • /remote_cmd：上位机下发期望关节指令 remotemsgend/msg/RemoteMsgEnd；
  • /driver_pvt：主控回传机器人实际关节状态 driver_pvt/msg/DriverPVT；
• 作用：多算法节点解耦（插值、轨迹回放、VR 映射、状态可视化），支持时间戳、QoS 实时调度。

下层：自定义 UDP 二进制协议（跨设备以太网 / WiFi）

物理层：以太网 / WiFi；传输层：UDP；应用层：自定义带 CRC 二进制结构体

核心协议规范

1. 数据包结构： 固定结构体载荷(无CRC) + CRC16-CCITT校验和(2字节)
   • 结构体格式 REMOTE_MSG_FMT_NO_CRC：小端序<，包含头标记 0xAA、模式、双臂 7 关节期望角度arm_q_exp_l/r、腰部 / 头部 / 夹爪、底盘控制、使能开关等全部控制字段；
   • CRC 校验：binascii.crc_hqx(data,0xFFFF)，防止 WiFi / 以太网传输丢包、错包；
2. 通信端口：上位机 UDP 发送端口 3336，机器人主控监听 3334；
3. 频率：50Hz/100Hz 固定周期发送，精确 sleep 控帧率，保障运动平滑；

转发实现代码：udp_remote_sender_new.py

• 订阅 ROS2 话题/remote_cmd；
• 将 ROS2 消息RemoteMsgEnd逐字段打包为二进制 struct；
• 计算 CRC16 追加包尾；
• UDP Socket 发送至机器人主控 IP；

反向回传代码：udp_bridge.py（机器人主控侧）

• 主控开启 UDP 线程，双向收发：
  1. 接收上位机 UDP 控制包，解析期望关节q_exp；
  2. 读取底层机械臂实际关节数据，打包 102float+int UDP 包回传上位机；
  3. 解析回传数据封装为 ROS2 消息DriverPVT，发布/driver_pvt供上位机读取实际关节位置（go.py订阅用于缓启动）。

上位机→主控完整数据流

VR 追踪 → ROS2 插值平滑节点 (send_bezier) → /remote_cmd话题 → udp_remote_sender_new.py UDP 打包（带 CRC） → WiFi / 以太网 → 机器人主控嵌入式板。

3. 机器人主控（嵌入式） ↔ 机械臂关节驱动器通信协议

底层工业实时总线：EtherCAT（主流）/ CAN FD/CANopen（备选）

源码侧佐证：udp_bridge.py内部存储全身关节实际q、期望q_exp、力矩tau，这些数据由 EtherCAT 总线从电机驱动器读取。

1）EtherCAT

• 类型：工业硬实时以太网总线，主从架构，主控为主站，每个关节伺服为从站；
• 周期：1ms~2ms，多轴同步误差 < 1us，满足双臂协同运动；
• 数据流向：
  1. 主控解析 UDP 收到的双臂 7 关节期望角度arm_q_exp_l/r；
  2. EtherCAT 主站下发各关节位置指令给伺服驱动器；
  3. 伺服闭环 PID 控制电机，编码器实时采集实际角度q、速度qd、力矩tau；
  4. 所有关节状态通过 EtherCAT 回传给主控，存入robot_data；
  5. 主控打包 UDP 回传给上位机，生成/driver_pvt状态话题。

2）CAN FD/CANopen

关节数量少、实时性要求低场景使用，总线传输位置、力矩反馈，带宽低于 EtherCAT，延迟更高。

底层闭环控制逻辑

主控只做运动指令转发，真正伺服闭环在关节驱动器内部；EtherCAT 保证双臂多关节同步，避免动作抖动，对应上层代码里的加速度限制、三次样条平滑就是为适配底层总线实时性设计。

三、代码功能分层封装

1. 轨迹生成 / 平滑层（上位机算法）

文件：send_bezier_q_udp_1.py 功能：三次样条 C2 连续插值、腕关节加速度限幅、一阶低通滤波、速度缩放；输出平滑连续关节角度，直接填充 UDP 控制字典。

2. ROS2 指令发布层（VR / 手动控制）

文件：go.py 功能：订阅机械臂实际状态/driver_pvt，带缓启动平滑插值，发布 ROS2 标准控制消息RemoteMsgEnd，解耦算法与底层 UDP 通信。

3. ROS2 转 UDP 桥接层（上位机→主控转发）

文件：udp_remote_sender_new.py 功能：ROS2 话题转自定义 CRC 校验 UDP 二进制包，统一协议封装，所有上位机控制逻辑共用一套 UDP 发送规则。

4. 机器人主控双向 UDP 桥（主控底层）

文件：udp_bridge.py 功能：双向通信：

• 下行：接收上位机 UDP 指令，解析期望关节；
• 上行：读取 EtherCAT 关节实际状态，打包 UDP 回传上位机，并封装为 ROS2 标准DriverPVT状态话题。

5. 数据回放 / 分析工具（辅助调试）

• export_driver_pvt_csv.py：读取 ROS2 录制 db3 数据库，导出关节 q/q_exp 轨迹 CSV；
• plot_q_vs_qexp.py：绘制期望关节 vs 实际关节曲线，调试跟踪误差；
• send_csv_q_full_mapping_udp.py：回放 CSV 轨迹，UDP 复现动作；
• chakandb3.py/test_db3.py：直接解析 ros2 bag 数据库原始二进制数据，离线调试。

四、全链路闭环完整数据流

1. VR 采集：VR 眼镜手部追踪 IMU 输出双臂目标位姿 → WiFi UDP 发送上位机；
2. 上位机 ROS2 处理
   • VR 数据运动学逆解，算出双臂 7 关节目标角度；
   • go.py发布/remote_cmd ROS2 话题；
   • send_bezier_q_udp_1.py三次样条平滑、限加速度滤波；
   • udp_remote_sender_new.py将 ROS2 消息打包带 CRC 自定义 UDP 二进制包；
3. 网络传输：WiFi / 以太网 UDP 发送至机器人嵌入式主控；
4. 机器人主控解析：udp_bridge.py UDP 线程解析期望关节q_exp；
5. 底层实时总线下发：EtherCAT 总线同步下发各关节位置指令；
6. 伺服电机闭环执行：驱动器 PID 控制电机转动，编码器采集实际角度、力矩；
7. 状态回传闭环：
   • 关节真实状态通过 EtherCAT 回主控；
   • 主控打包 UDP 包回传上位机；
   • 上位机解析生成/driver_pvt话题；
   • 插值节点读取实际关节，实现缓启动、误差补偿，形成完整闭环。

五、各通信协议优缺点总结

通信链路	协议	优势	劣势
VR ↔ 上位机	WiFi-UDP	无线低延迟、轻量化、适配 VR 引擎	Wi4 Fi 易受干扰，无可靠重传
上位机内部节点	ROS2 DDS	模块化解耦、标准消息、时间戳、QoS	进程间开销高于原生 UDP
上位机 ↔ 机器人主控	以太网 / WiFi + 自定义 UDP (CRC)	轻量、100Hz 高频、跨设备简单	需手动实现 CRC 校验、分包
主控 ↔ 机械臂关节	EtherCAT	微秒级同步、硬实时、多轴协同	需要专用硬件从站，成本高
主控 ↔ 机械臂关节	CAN FD	低成本、抗干扰、布线简单	周期最低 5ms，多轴同步差