Day15 unitree_G1人形机器人“身外化身”通信丢帧排查

一、首先明确“丢帧”发生在哪一层

最开始不能直接假设问题一定在MocapApi，也可能发生在：

1. Axis没有生成帧。
2. Axis生成了帧，但UDP没有送达。
3. MocapApi收到数据，但事件队列发生覆盖或合并。
4. 程序轮询不及时。
5. 深拷贝时SDK内部对象已经更新。
6. FIFO覆盖。
7. GMR消费不足。
8. 机器人通信或机器人buffer丢帧。

因此首先增加三层计数：

SDK Avatar事件数 ↓ 深拷贝NoitomFrame数 ↓ 程序/FIFO输出帧数

判断规则是：

• 第一层已经少：问题发生在MocapApi之前或MocapApi内部。
• 第一层正常、第二层少：深拷贝或SDK对象生命周期有问题。
• 第二层正常、第三层少：程序FIFO或消费逻辑有问题。
• 三层相等但帧号缺失：程序没有主动丢帧，缺口进入程序前就已经存在。

实验结果长期表现为：

sdk_avatar_events = frames_deep_copied = client_frames_returned = FIFO pushed = FIFO popped

但是posture_index仍偶尔跳跃。

这说明原程序内部的FIFO和深拷贝没有继续丢帧，缺帧已经发生在程序看到Avatar事件之前。

二、先修复MocapApi调用热路径

1. 无事件时从sleep改为yield

原逻辑：

if (!frame) { std::this_thread::sleep_for(std::chrono::milliseconds(1)); }

修改为：

if (!frame) { std::this_thread::yield(); }

依据：

• 采集线程已经与GMR分离。
• Windows的sleep_for(1ms)可能实际暂停超过1毫秒。
• 50 Hz虽然每帧约20 ms，但SDK事件可能集中进入队列。
• 固定等待会扩大连续事件的轮询间隔。
• yield()允许其他可运行线程执行，但采集线程能够更快重新获得CPU。

这项修改降低了程序主动制造轮询空窗的概率。

2. 非Avatar事件立即继续poll

原来的隐患是：

读到Notify或Sensor事件 → 返回外层 → 等待 → 再次poll

改为：

for (;;) { poll SDK事件; if (没有事件) 返回空; if (系统错误) 报错; if (不是AvatarUpdated) continue; 立即深拷贝Avatar姿态; return frame; }

依据：

• GMR只需要Avatar姿态。
• Notify、SensorUpdated等事件不能阻塞后续Avatar事件。
• SDK返回Error_MoreEvent说明队列里可能还有事件，应尽快排空。
• 非Avatar事件不能成为人为的采集节流点。

3. 使用预分配事件轮询

加入：

event_delivery=preallocated_poll

目的：

• 避免热路径频繁分配和释放事件对象。
• 减少堆分配、锁竞争和不可预测停顿。
• 让采集线程尽可能只做poll、检查、深拷贝和入队。

4. 每个Avatar事件立即深拷贝

每次取得AvatarUpdated后，立即将SDK内部姿态复制成独立的NoitomFrame。

增加了：

posture_changes_during_copy

用于检测深拷贝期间SDK对象是否已经被下一帧覆盖。

测试结果：

posture_changes_during_copy=0

说明已复制帧是独立稳定的，不存在多个FIFO元素共同引用SDK最新姿态的问题。

5. 采集与GMR彻底分离

结构保持为：

MocapApi专用采集线程 ↓ 独立NoitomFrame ↓ FIFO ↓ GMR线程

后来进一步把MocapApi采集放入独立进程，通过管道传给主程序：

MocapApi采集进程 ↓ 进程内采集FIFO ↓ 命名管道 ↓ 主程序输入FIFO ↓ GMR

这样MocapApi轮询不会被GMR计算、输出线程或其他主程序工作抢占。

6. 提升采集调度优先级

加入：

process_priority_high=1 acquisition_priority_high=1

目的：

• 减少Windows调度延迟。
• 尤其在Axis、GMR和其他程序同时运行时，降低采集线程长时间得不到CPU的概率。

三、增加完整诊断统计

MocapApi路径增加或完善了：

poll_calls avatar_events empty_polls other_events more_event_returns duplicate_indices backward_indices max_poll_gap_ms max_poll_block_ms missing_posture_indices posture_changes_during_copy

三层/管道/FIFO统计包括：

sdk_avatar_events frames_deep_copied client_frames_returned pipe_frames_written frames_fifo_pushed frames_fifo_popped max_fifo_depth three_layers_equal

这些指标的作用不是单纯显示“掉了几帧”，而是确定缺帧发生的边界。

最终发现：

SDK事件数 = 深拷贝数 = 管道数 = 主程序收到数

同时仍存在随机posture_index缺口。

因此可以排除：

• 深拷贝覆盖
• FIFO覆盖
• 管道丢帧
• GMR消费导致源帧缺失
• 主程序主动只取最新帧

剩下最可疑的边界是：

Axis BVH输出 → MocapApi内部 → AvatarUpdated事件

四、方案A：尽可能优化MocapApi路径

方案A主要包括：

• 预分配poll事件。
• 采集线程独立。
• 采集进程隔离。
• 高进程/线程优先级。
• 非Avatar事件立即排空。
• 无事件使用yield()。
• 每个Avatar事件立即深拷贝。
• FIFO逐帧传递。
• 三层计数和缺帧明细。

效果确实比早期版本好，多轮测试出现：

有些轮次零缺帧 有些轮次缺1～数帧

但仍然不能稳定保证每轮完整。

关键证据是：

MocapApi只返回3148～3151个Avatar事件 程序就只能深拷贝3148～3151帧

程序无法复制一个SDK没有交付的事件。

因此继续优化FIFO或GMR已经没有理论意义。

五、方案D：双MocapApi接收路径冗余

Axis同时向两个端口广播：

127.0.0.1:7012 127.0.0.1:7013

两个独立MocapApi进程同时接收，然后按posture_index合并。

实验中出现：

7012缺失：[10971,12096,12162] 7013缺失：[10870,10904] 共同缺失：[] 合并后缺失：0

这证明两个路径的随机缺口有时互不重合，冗余合并能够恢复完整序列。

但也出现过：

两个路径共同缺失：[11825] 合并后仍缺失：1

因此方案D只能降低随机丢帧概率，不能消除共同上游缺帧。

它还有这些代价：

• 两套MocapApi实例。
• 双倍SDK资源。
• 需要按帧号合并、去重、等待。
• 实时延迟和逻辑复杂度增加。
• 两路仍然经过相同的MocapApi实现，共因故障没有被消除。

所以D适合作为诊断和备选冗余方案，不是最终首选。

六、方案C：绕过MocapApi，直接读取Axis二进制BVH UDP

这是昨天最关键的改造。

Axis配置两个目标：

127.0.0.1:7012 → MocapApi对照路径 127.0.0.1:7014 → 原始BVH UDP路径

通过同一次take008回放，同时比较：

Axis原始UDP数据包 MocapApi Avatar事件

实验出现：

原始UDP：3152个数据包，内部缺失0 MocapApi：3150个Avatar事件，缺失2

而且缺失帧号会随机变化。

这形成了直接证据：

Axis已经发出了对应帧，本机UDP也收到了，但MocapApi没有把所有帧作为Avatar事件交付给程序。

因此方案C选择完全绕开这个不稳定边界。

七、原始BVH协议解析

新增了Axis原始BVH数据源：

• [axis_bvh_raw_source.hpp](/C:/Users/RX01296/Desktop/全身控制8_3/gmr-master_PN - 副本/cpp_xsens_g1/include/axis_bvh_raw_source.hpp)
• [axis_bvh_raw_source.cpp](/C:/Users/RX01296/Desktop/全身控制8_3/gmr-master_PN - 副本/cpp_xsens_g1/src/axis_bvh_raw_source.cpp)

解析依据是Axis二进制BVH格式：

64字节帧头 59个骨骼 每个骨骼6个float 354个数据项 位置XYZ 欧拉角XYZ

解析流程：

检查帧头标志 → 读取FrameIndex → 读取59骨骼局部位移和欧拉角 → XYZ旋转转四元数 → 按BVH父子层级做前向运动学 → 厘米转换为米 → 转换到现有GMR坐标系 → 提取现有23个身体节点 → 构造独立NoitomFrame

为保证不会默默解析错误，还检查：

帧头token 协议版本/格式 DataCount=354 WithDisp=1 WithReference=0 数据包长度

配置不匹配时应拒绝数据，而不是产生一个看似正常但姿态错误的机器人动作。

八、为什么可以直接接入现有GMR

方案C没有修改GMR算法。

它只是将输入源从：

MocapApi → NoitomFrame

替换为：

Axis二进制BVH → NoitomFrame

下游看到的数据类型没有变化：

NoitomFrame posture_index timestamp 23个body position quaternion

因此以下部分保持不变：

• GMR映射算法
• IK配置
• Unitree G1模型
• MuJoCo模型
• ZMQ
• gRPC
• 控制算法
• 机器人输出数据结构

九、数值正确性验证

为了避免“虽然不丢帧，但姿态解析错了”，新增了离线逐帧对比工具：

• [axis_bvh_offline_compare_main.cpp](/C:/Users/RX01296/Desktop/全身控制8_3/gmr-master_PN - 副本/cpp_xsens_g1/src/axis_bvh_offline_compare_main.cpp)
• [noitom_capture_audit_main.cpp](/C:/Users/RX01296/Desktop/全身控制8_3/gmr-master_PN - 副本/cpp_xsens_g1/src/noitom_capture_audit_main.cpp)增加--dump-wire

比较同一帧经过两条路径得到的姿态：

路径1：Axis原始BVH → 自研解析 路径2：Axis → MocapApi → SDK姿态

3149个共同帧的对比结果：

最大位置误差约：1.32×10⁻⁶ m 最大旋转误差约：0.0685°

这个误差量级说明：

• 骨骼顺序正确。
• 父子层级正确。
• XYZ旋转顺序正确。
• 坐标轴变换正确。
• 厘米到米转换正确。
• 与原有GMR输入基本等价。

十、Solution C正式接入主程序

在[xsens_core_main.cpp](/C:/Users/RX01296/Desktop/全身控制8_3/gmr-master_PN - 副本/cpp_xsens_g1/src/xsens_core_main.cpp)中增加：

--motion-input axis-raw

运行方式：

.\xsens_core.exe ` --motion-input axis-raw ` --noitom-ip 127.0.0.1 ` --noitom-port 7014 ` --input-fps 50 ` --output-fps 50 ` --transport none ` --disable-ws ` --disable-console-metrics ` --auto-start

此时MocapApi完全不参与动作帧获取：

Axis Studio → 原始BVH UDP → AxisRawBvhSource → NoitomFrame → FIFO → GMR

十一、UDP接收稳定性改造

原始UDP接收路径设置了约16 MB接收缓冲区：

rcvbuf_requested=16777216

目的：

• 吸收短时间的线程调度抖动。
• 避免用户态暂时未读取时内核UDP队列过小。
• 不覆盖程序FIFO，不只保留最新帧。

无数据时继续使用：

std::this_thread::yield();

并将：

WSAETIMEDOUT WSAEWOULDBLOCK WSA_IO_PENDING（997）

都视为暂时没有数据，而不是致命错误。

这是因为测试中曾出现：

[Receiver] raw BVH recv failed WSA=997

导致采集提前停止。修复后，997不会再终止数据源。

十二、完善唯一帧守恒统计

增加：

source_received source_unique_frames source_missing_indices source_duplicate_indices source_backward_indices gmr_processed motion_generated unique_frames_conserved

其中：

source_unique_frames = source_received - 精确重复帧数

Axis回放结束时会重复发送一次末帧12518，因此标准take008结果为：

收到UDP包：3152 重复末帧：1 唯一姿态帧：3151 帧号范围：9368～12518

数学上：

12518 - 9368 + 1 = 3151

重复末帧不是新的动作帧，不能再次推入机器人buffer，所以按相同posture_index精确去重。

最终三次完整测试全部得到：

source_received=3152 source_unique_frames=3151 source_missing_indices=0 source_duplicate_indices=1 source_backward_indices=0 first_posture_index=9368 last_posture_index=12518 gmr_processed=3151 motion_generated=3151 unique_frames_conserved=1 input_pending=0 robot_pending=0

这证明在三轮离线回放中：

Axis的3151个唯一帧 = 程序收到的3151个唯一帧 = GMR处理的3151帧 = 生成的3151帧动作

当前程序框架

```mermaid flowchart LR Suit["动捕服传感器"] -->|Wi-Fi| Axis["Axis Studio"] Axis -->|"二进制 BVH UDP<br/>127.0.0.1:7014"| Raw["AxisRawBvhSource<br/>专用采集线程"] Axis -.->|"可选对照<br/>127.0.0.1:7012"| SDK["MocapApi路径<br/>保留作诊断/回退"] Raw --> Validate["协议与长度校验"] Validate --> Parse["解析59骨骼<br/>局部位置 + XYZ欧拉角"] Parse --> FK["前向运动学<br/>局部姿态转全局姿态"] FK --> Convert["坐标系和单位转换"] Convert --> Frame["独立NoitomFrame<br/>含posture_index"] Frame --> Check["缺失/重复/倒序统计"] Check --> FIFO["输入FIFO"] FIFO --> GMR["GMR线程"] GMR --> Motion["50 Hz机器人动作帧"] Motion --> Output["MuJoCo或机器人通信"] ```

采集和消费线程逻辑

```mermaid flowchart TD Start["启动axis-raw数据源"] --> Socket["绑定127.0.0.1:7014<br/>申请16 MB接收缓冲"] Socket --> Receive["接收一个UDP数据包"] Receive --> Error{"接收结果"} Error -->|"暂时无数据<br/>timeout/would-block/997"| Yield["yield()"] Yield --> Receive Error -->|"真实错误"| Stop["记录错误并停止"] Error -->|"收到数据"| Header{"协议头、长度、格式正确？"} Header -->|否| Reject["拒绝异常包并统计"] Reject --> Receive Header -->|是| Index["读取posture_index"] Index --> Sequence{"与上一帧比较"} Sequence -->|"index = previous"| Duplicate["重复帧计数<br/>不再推给机器人"] Duplicate --> Receive Sequence -->|"index > previous + 1"| Missing["记录所有缺失帧号"] Sequence -->|"index < previous"| Backward["记录倒序/重启"] Sequence -->|"连续"| Decode["解析姿态"] Missing --> Decode Backward --> Decode Decode --> DeepCopy["生成独立NoitomFrame"] DeepCopy --> Push["FIFO尾部入队"] Push --> GMRThread["GMR从FIFO头部逐帧取出"] GMRThread --> Count["gmr_processed++"] Count --> Motion["生成动作帧"] Motion --> Receive ```

缺帧定位逻辑

```mermaid flowchart TD Gap["发现posture_index缺口"] --> RawCheck{"Axis原始UDP是否也缺？"} RawCheck -->|"原始UDP不缺<br/>MocapApi缺"| SDKProblem["MocapApi事件交付问题"] RawCheck -->|"原始UDP也缺"| Upstream{"Axis记录/广播中是否存在该帧？"} Upstream -->|"Axis没有生成"| Wifi["动捕服Wi-Fi、传感器<br/>或Axis实时解算问题"] Upstream -->|"Axis已生成但UDP未收到"| UDP["UDP交付、套接字缓冲<br/>或本机调度问题"] SDKProblem --> SolutionC["使用axis-raw绕过MocapApi"] UDP --> Tune["增大接收缓冲<br/>提高采集优先级<br/>检查网络"] Wifi --> SuitTest["实时穿戴专项测试"] ```

昨日工作的最终结论

1. 原来的FIFO覆盖问题已经排除：程序不是只保留最新帧。

2. 深拷贝问题已经排除：每个Avatar事件均形成独立NoitomFrame，复制期间姿态没有变化。

3. GMR消费丢帧已经排除：完整测试中唯一源帧数、GMR处理数、动作生成数全部相等。

4. MocapApi仍存在随机少交付Avatar事件的现象：优化能够降低概率，但不能从根本上保证完整。

5. 双MocapApi冗余能改善，但共同缺失仍可能存在。

6. Solution C通过读取Axis原始二进制BVH，消除了MocapApi事件层的不确定性。

7. 自研解析结果与SDK数值基本等价，位置与旋转误差远低于实际控制敏感范围。

8. 三次完整take008测试均实现3151个唯一帧零缺失、零倒序、全部进入GMR。

9. 当前尚未证明的是“动捕服通过Wi‑Fi实时采集时上游永不缺帧”。离线回放验证的是Axis到程序和GMR的链路；实时穿戴Wi‑Fi链路仍需要30～60分钟专项测试。

所以，昨天真正完成的不是简单地“换了一个UDP输入”，而是通过分层计数和双路径对照，把缺帧边界定位到了MocapApi事件交付层，然后用协议等价、数值验证过的原始BVH输入替换了这个不稳定边界，同时保持GMR和机器人控制逻辑不变。