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

别动我齐刘海

21人浏览 · 2026-08-16 14:03:19

别动我齐刘海 · 2026-08-16 14:03:19 发布

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

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

Axis没有生成帧。
Axis生成了帧，但UDP没有送达。
MocapApi收到数据，但事件队列发生覆盖或合并。
程序轮询不及时。
深拷贝时SDK内部对象已经更新。
FIFO覆盖。
GMR消费不足。
机器人通信或机器人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["实时穿戴专项测试"]
```

昨日工作的最终结论

原来的FIFO覆盖问题已经排除：程序不是只保留最新帧。
深拷贝问题已经排除：每个Avatar事件均形成独立NoitomFrame，复制期间姿态没有变化。
GMR消费丢帧已经排除：完整测试中唯一源帧数、GMR处理数、动作生成数全部相等。
MocapApi仍存在随机少交付Avatar事件的现象：优化能够降低概率，但不能从根本上保证完整。
双MocapApi冗余能改善，但共同缺失仍可能存在。
Solution C通过读取Axis原始二进制BVH，消除了MocapApi事件层的不确定性。
自研解析结果与SDK数值基本等价，位置与旋转误差远低于实际控制敏感范围。
三次完整take008测试均实现3151个唯一帧零缺失、零倒序、全部进入GMR。
当前尚未证明的是“动捕服通过Wi‑Fi实时采集时上游永不缺帧”。离线回放验证的是Axis到程序和GMR的链路；实时穿戴Wi‑Fi链路仍需要30～60分钟专项测试。