Day14 unitree_G1人形机器人BVH/MocapApi实际输出少于Axis排查

明确目标：解决“Axis内部有帧，但BVH/MocapApi交给程序的真实帧更少”，不插值、不复制上一帧、不修改GMR和机器人控制。每一个送入FIFO的帧都必须来自真实的Axis输出。

先说结论：最值得优先验证的不是继续优化FIFO，而是下面三个位置：

Axis姿态解算 ↓ BVH广播是否真的发出每帧 ↓ MocapApi是否缓存每个事件 ↓ 采集程序是否来得及深拷贝

目前证据只能证明posture_index出现缺口，尚不能单凭它断定缺口产生在Axis广播、MocapApi内部缓存还是我们的读取程序。

一、方案优先级

优先级	方案	对取全真实帧的作用	可行性
P0	固定历史动作回放，排除传感器/WiFi	精确复现并定位	很高
P0	Axis→MocapApi改为TCP	消除UDP传输层丢包	很高
P0	严格使用MocapApi Cache事件模式	防止只读到最新状态	很高
P0	建立Axis广播包、SDK事件、posture三层计数	找出每次丢帧位置	很高
P1	采集线程只做Poll和深拷贝，移走日志与统计输出	降低本地覆盖概率	很高
P1	缓存稳定的Joint Handle，减少每帧SDK调用	缩短深拷贝窗口	高
P1	预分配NoitomFrame和关节数组	消除热路径内存分配抖动	高
P1	专用SPSC FIFO或分段无界队列	降低锁竞争和生产者阻塞	中高
P1	调整线程优先级和CPU亲和性	减少调度间隙	中高
P2	UDP减小数据包或增加双路冗余	UDP必须保留时降低丢包	中
P2	MocapApi回调模式A/B测试	可能降低事件响应延迟	中低
P3	绕过MocapApi直接解析BVH	完全控制socket和缓存	低，最后手段

二、第一步：先精确定位，不修改正式程序

方案1：使用Axis历史动作回放

这是最重要的实验。

使用Axis已经录制好的固定动作文件，循环播放并开启“BVH-编辑”广播。官方手册明确支持历史数据广播。Axis Studio数据广播说明

这样可以排除：

• 动捕服到Axis的WiFi；
• 传感器临时断连；
• 人体动作差异；
• 每次测试源数据不同。

对同一段例如10,000帧的数据重复广播10次。如果每次缺失位置不同，倾向于广播、SDK缓存或程序调度问题；如果每次在完全相同的位置缺失，倾向于源文件、Axis播放或协议解析问题。

可行性：非常高。
风险：无。
建议：必须最先做。

方案2：建立三层帧计数

需要分别得到：

A = Axis准备广播的帧数 B = 操作系统实际收到的BVH帧/UDP报文数 C = MocapApi AvatarUpdated事件数 D = 唯一posture_index数 E = 深拷贝进入FIFO的NoitomFrame数

判断方法：

• A > B：Axis广播或传输层丢失。
• B > C：MocapApi解析或内部事件缓存丢失。
• C > D且duplicate_indices > 0：多个事件最终读取了同一个最新姿态，存在SDK状态覆盖。
• D > E：我们的深拷贝或FIFO入口丢失。
• E连续但机器人少帧：问题已经不在MocapApi采集层。

官方说明中，MocapApi本质上是接收Axis Studio外发socket数据的中间层，不直接连接传感器。Noitom MocapApi说明

可行性：很高。
意义：这是最终判断“到底哪里少”的唯一可靠方法。

三、首选解决方案：Axis→MocapApi使用TCP

如果目标是“一帧真实数据都不能少”，TCP比UDP更符合需求。

UDP规范明确说明不保证送达、不保证顺序，也不提供重复保护；需要可靠有序传输时应使用TCP。RFC 768

TCP提供：

• 有序传输；
• 丢失检测；
• 重传；
• 重复数据处理；
• 字节流完整性。

这是TCP标准明确提供的能力。RFC 9293

Axis本身支持TCP和UDP BVH广播，因此不需要自己实现可靠协议。

优点：

• 不插值；
• 丢包后能够重传真实数据；
• 保证收到的数据顺序；
• 同一台PC使用127.0.0.1时延迟代价很小；
• 最符合机器人严格逐帧消费的场景。

代价：

• 丢包时后续数据会暂时等待重传，产生短暂延迟；
• 如果接收端长期处理不过来，TCP会形成反压；
• 需要监测FIFO延迟，不能只看是否掉帧。

可行性：很高。
理论有效性：最高。
建议：作为正式运行首选；UDP保留为对照组。

四、严格使用MocapApi Cache事件模式

官方调用说明显示，非Cache模式会先查询事件总数，再分配数组并一次取回事件；每个MCPEvent_t必须设置自己的size。MocapApi官方调用说明

对于我们的需求，Cache模式更合适：

Poll一个事件 → 若AvatarUpdated，立即读取该事件对应姿态 → 深拷贝完成 → 再Poll下一个事件

应该确认：

EnableApplicationCacheEvents(...) ApplicationCacheEventsIsEnabled(...) == true

并保持：

MCPEvent_t events[1]; uint32_t event_count = 1;

不能把sizeof(MCPEvent_t)作为事件数量传入。

理论依据：

• 事件对象只携带Avatar Handle；
• 关节数据需要再通过Handle访问SDK内部状态；
• 如果一次取出多个Avatar事件后才读取关节，前面的事件可能已经无法对应独立的历史姿态；
• 因此必须让“取事件”和“深拷贝姿态”紧密相邻。

可行性：很高。
注意：需要用固定历史回放对比Cache开/关，不能仅根据函数返回成功就认定Cache没有覆盖。

五、缩短Avatar事件到深拷贝完成的时间

即使Cache正确，深拷贝时间越长，SDK内部新姿态到达并覆盖状态的概率越大。

方案1：采集热路径禁止控制台输出

当前周期统计和NoitomJump输出发生在采集线程中。Windows控制台输出可能阻塞，尤其使用：

Tee-Object

时更明显。建议架构：

采集线程： Poll → 深拷贝 → FIFO → 更新原子计数 日志线程： 每秒读取统计快照 → 打印

缺帧详细信息也应先写入诊断队列，由日志线程打印。

可行性：很高。
预期收益：降低偶发max_poll_gap_ms。
风险：低。

方案2：缓存Joint Handle

官方说明Handle是SDK管理对象的身份索引，接口通过Handle读取数据。MocapApi对象模型

目前程序每帧重新：

GetAvatarJoints 遍历所有Joint GetJointTag 建立std::map 读取位置和旋转

可以考虑在Avatar创建或变化时缓存：

Avatar Handle Joint Handle Joint Tag → NoitomFrame位置

每帧仅执行必要的姿态数值读取。

但必须满足：

• Avatar Handle改变时立即重建；
• 连接断开/重连后全部失效；
• 先通过长时间A/B测试确认SDK的Joint Handle跨帧稳定；
• 保留复制前后posture_index检查。

可行性：高。
预期收益：明显缩短avg_avatar_read_ms和max_avatar_read_ms。
风险：中低，需要验证句柄生命周期。

方案3：预分配Frame对象

避免每帧反复：

• 创建std::vector；
• 分配BodyPose；
• 复制关节名称字符串；
• 创建std::map；
• 触发堆分配器锁。

可以使用Frame对象池，每个Frame预先包含固定数量关节槽位。采集线程只覆盖数值，不重新分配内存。

可行性：高。
理论依据：减少不可预测的堆分配延迟和锁竞争。
风险：低，但必须保证Frame归还前不会被GMR继续引用。

六、FIFO方案

当前std::deque + mutex在50 Hz下理论上已经足够，但如果追求严格实时，可改为单生产者、单消费者队列：

Mocap采集线程：唯一生产者 GMR线程：唯一消费者

SPSC队列与当前拓扑完全匹配。相关研究表明，SPSC可采用wait-free/unbounded结构减少同步延迟。Single-Producer/Single-Consumer Queues研究

推荐：

• 预分配较大的环形池，例如2048或4096帧；
• 正常运行只移动Frame槽位所有权；
• 绝不采用“满了覆盖最旧帧”；
• 队列接近满时报警并停止机器人，而不是静默丢帧；
• 如果要求无限运行，则采用分段增长式SPSC，而不是固定容量覆盖。

可行性：中高。
预期收益：降低锁抖动，但它不是当前posture_index缺口的首要嫌疑。

七、线程调度方案需要修正认识

目前采集线程设置为THREAD_PRIORITY_HIGHEST并持续yield()。这不一定总是最优。

微软明确警告：高优先级线程如果持续可运行，可能让低优先级线程得不到CPU；等待低优先级线程提供数据时，高优先级线程应阻塞，而不是持续循环。Windows调度优先级说明

MocapApi.dll内部很可能还有socket接收/解析线程。如果我们的Poll线程一直以最高优先级空转，它理论上可能反过来挤压SDK接收线程。

建议对照测试：

1. THREAD_PRIORITY_NORMAL
2. THREAD_PRIORITY_ABOVE_NORMAL
3. THREAD_PRIORITY_HIGHEST
4. ABOVE_NORMAL + CPU亲和性
5. MMCSS多媒体调度

MMCSS适合时间敏感处理，并尽量避免完全饿死普通线程。Windows MMCSS

我更推荐的理论方案是：

采集线程固定到一个逻辑核心 SDK内部线程和Axis保留其他核心 采集线程ABOVE_NORMAL或MMCSS GMR放到其他核心

而不是直接把整个进程设为RealTime。微软也明确不建议普通应用使用实时优先级。

可行性：中高。
实施前提：必须通过max_poll_gap_ms和缺帧率A/B选出结果，不能凭感觉设置。

八、UDP必须保留时的方案

1. 使用127.0.0.1单播

同机通信应使用：

目标 127.0.0.1:7012

不要使用：

255.255.255.255

广播可能经过网卡协议栈并受到防火墙、接口选择和缓冲影响。

2. 减少BVH数据包大小

开启每个关节位移后，单帧数据明显增大，可能接近或超过常见MTU。UDP数据报一旦发生IP分片，只要其中一个分片丢失，整帧数据报都会作废。

可进行A/B：

• 二进制+位移；
• 二进制+仅根节点位移/关闭额外位移；
• 新帧头；
• 旧帧头。

前提是确认关闭额外位移后，MocapApi仍能通过骨架层级计算出GMR所需的全局关节位置。

可行性：中。
风险：需要验证姿态语义，不能直接用于机器人。

3. 双路真实数据冗余

Axis界面支持多个目标地址时，可以把同一真实BVH流发送到两个本地端口：

127.0.0.1:7012 127.0.0.1:7013

两个MocapApi Application分别接收，再按posture_index合并去重。任意一路收到的真实帧都可以保留。这不是插值，所有帧都来自Axis。

局限：

• 如果Axis根本没发某一帧，两路都会缺；
• 如果两个目标共享同一内部发送队列，冗余收益可能有限；
• 程序复杂度较高。

可行性：中。
适合作为UDP无法改TCP时的增强方案。

九、不建议优先采用的方案

MocapApi回调模式

RegisterEventHandler可能降低轮询响应延迟，但存在两个风险：

• 在SDK回调线程中深拷贝所有关节，会阻塞SDK自身接收；
• 如果回调只保存Avatar Handle，之后再读取，仍可能只读到最新姿态。

因此只能作为独立A/B实验，不应直接替换Cache Poll。

直接解析Axis BVH

可以完全控制：

• TCP/UDP socket；
• SO_RCVBUF；
• 收包时间戳；
• 数据包计数；
• 自己的Frame队列。

但是需要正确实现BVH二进制帧头、版本兼容、骨骼层级和坐标系。维护风险明显高于MocapApi。

只建议在证明确实是MocapApi.dll内部丢帧、且官方无法修复时采用。

十、先理解什么是Cache

Cache中文一般叫“缓存”。比如摄像头每秒拍50张照片：

100、101、102、103、104……

有历史缓存

如果系统把每张照片都保存下来：

缓存队列： [100][101][102][103][104]

程序稍微晚一点处理，也可以依次取得：

先取100 再取101 再取102

只有最新状态

如果系统只保存最新画面：

最开始：100 新帧来了：用101覆盖100 新帧来了：用102覆盖101

程序晚一点读取，看到的可能只有：

102

100和101已经没有了，这就是我们一直担心的“旧姿态被最新姿态覆盖”。

---

十一、Noitom的Cache模式对应什么函数

r70头文件中存在三个接口：

EnableApplicationCacheEvents(...) DisableApplicationCacheEvents(...) ApplicationCacheEventsIsEnabled(...)

它们大致表达的是：

EnableApplicationCacheEvents = 请求SDK把收到的事件暂存在Application事件缓存中 ApplicationCacheEventsIsEnabled = 查询这个功能是否真的启用

当前程序连接MocapApi时会尝试：

EnableApplicationCacheEvents(application_handle_);

如果r70 DLL不支持，SDK会返回：

Error_NotSupported

也就是：

我认识你调用的这个函数，但当前版本没有实现这个功能。

---

因此r70下更需要：

尽快Poll → 得到Avatar事件 → 立即深拷贝 → 存入我们自己的FIFO

---

十二、Cache模式和“最新姿态缓存”不是一回事

这里有两个容易混淆的缓存概念。

1. Application Cache Events

对应：

EnableApplicationCacheEvents()

它想缓存的是事件通知，例如：

AvatarUpdated 100 AvatarUpdated 101 AvatarUpdated 102

r70如果返回Error_NotSupported，说明不能依赖这个显式功能保存完整事件历史。

2. SDK当前Avatar姿态缓存

即使Cache Events不支持，SDK仍然需要在内部保存当前Avatar状态，否则这些函数没有数据可读：

GetAvatarPostureIndex() GetJointGlobalPosition() GetJointGlobalRotation()

但这个内部状态很可能是：

当前最新姿态

新数据到来后，旧姿态可能被覆盖。

所以：

不支持Cache Events ≠ SDK内部什么都不保存 而是： SDK会保存当前可读状态 但不保证保存完整历史状态

---

七、这和我们自己的FIFO有什么区别

我们的FIFO是程序自己建立的：

std::deque<NoitomFrame> input_queue_;

它不属于MocapApi，也不依赖r70的Cache模式。

正确的数据路径是：

SDK当前姿态 ↓ 立即读取全部关节 独立NoitomFrame ↓ 程序自己的FIFO ↓ GMR逐帧处理

一旦深拷贝完成并进入FIFO：

Frame 100 Frame 101 Frame 102

这些帧就是程序自己的数据。SDK以后更新到103，也不会修改我们已经保存的100、101和102。

所以即使r70不支持Cache模式，我们仍然可以在应用层实现可靠FIFO。

但有一个前提：

必须在SDK旧姿态被覆盖之前Poll到事件并完成深拷贝。

---

八、为什么采集线程必须非常快

假设50Hz，每20ms来一帧：

0ms：第100帧 20ms：第101帧 40ms：第102帧

程序及时读取

第100帧到达 → 立即Poll → 立即深拷贝100 第101帧到达 → 立即Poll → 立即深拷贝101

FIFO最终是：

[100][101][102]

程序读取太慢

100到达 程序还在忙 101到达 SDK当前姿态更新成101 102到达 SDK当前姿态更新成102 程序这时才读取

程序可能只能看到：

102

100和101已经无法从SDK当前状态恢复。

这就是为什么我们做了：

• MocapApi采集线程与GMR线程分离；
• 采集线程提高优先级；
• 每次只Poll一个事件；
• 非Avatar事件立即继续Poll；
• 无事件时使用yield()；
• Avatar事件立即深拷贝；
• 深拷贝完成后进入程序自己的FIFO。

---

九、当前程序遇到“不支持Cache”会怎么样

当前连接代码已经专门处理：

if (cache_error == Error_NotSupported) { cache_events_enabled_ = false; std::cout << "[Noitom] cache_events=unsupported\n"; }

所以如果r70返回不支持，程序会显示：

[Noitom] cache_events=unsupported

然后继续运行，不会因为这个原因直接退出。

它的含义只是：

不能依赖SDK的Application Cache Events功能

程序会继续使用：

快速Poll + 立即深拷贝 + 应用层FIFO

---

十、它对“零掉帧”意味着什么

r70不支持Cache模式以后，我们能从程序上尽量做到：

不主动等待 不让GMR阻塞采集 不覆盖已经深拷贝的帧 不把SDK Handle延迟到GMR线程读取 正确逐个Poll事件

但我们无法做到：

向SDK重新索要已经被覆盖的历史帧

例如已经观察到：

posture_index从100跳到102

如果SDK里已经没有101的姿态数据，那么程序不能恢复真实的101，只能：

1. 记录missing_posture_indices=[101]；
2. 确认是不是本机Poll太慢；
3. 确认是否出现重复事件；
4. 确认深拷贝期间姿态是否变化；
5. 最后才考虑是否用插值补齐时间轴。

而你当前要求“先不要插值”，所以我们现在优先证明能否把所有真实帧及时取出。

---

十一、一张图看懂三个层次

```mermaid flowchart LR A["Axis/网络<br/>产生姿态100、101、102"] --> B["MocapApi r70<br/>当前Avatar状态"] B -. "Application Cache Events<br/>r70可能不支持" .-> C["SDK事件历史缓存"] B --> D["高优先级采集线程<br/>快速Poll"] D --> E["立即深拷贝完整姿态"] E --> F["程序自己的FIFO<br/>100、101、102"] F --> G["GMR线程逐帧处理"] C:::unsupported classDef unsupported fill:#555,color:#fff,stroke:#999,stroke-dasharray:5 5 ```

最简单的总结：

r70不支持Cache模式，意思是不能指望MocapApi替我们长期保存所有历史事件和历史姿态；因此程序必须快速Poll，在每个Avatar事件到来时立即把全部关节数据深拷贝出来，并保存在我们自己的FIFO中。已经深拷贝进FIFO的帧不会再被SDK覆盖，但在深拷贝之前已经被SDK覆盖的旧帧无法找回。

r70头文件可以直白理解为：

Noitom给C++程序的一本“SDK使用说明书和接口目录”。

我们现在用的主要头文件是：

MocapApi.h

其中r70表示这套MocapApi接口的构建版本是70。

1. 头文件不是SDK本体

需要区分两个文件。

头文件：MocapApi.h

它告诉编译器：

• 有哪些函数可以调用；
• 每个函数叫什么；
• 参数是什么类型；
• 返回什么错误码；
• 有哪些事件类型；
• Avatar和Joint Handle是什么；
• 各种数据结构长什么样。

它更像一本说明书。

动态库：MocapApi.dll

它是真正执行工作的程序，包括：

• 建立TCP/UDP连接；
• 接收Axis数据；
• 解析数据包；
• 管理Avatar；
• 产生事件；
• 返回姿态和关节数据。

它更像真正工作的机器。

关系是：

MocapApi.h = 告诉我们的代码怎么操作机器 MocapApi.dll = 真正执行这些操作

---

2. 用餐厅菜单理解

可以把头文件理解成餐厅菜单：

菜单上写着： 获取Avatar 获取姿态编号 获取关节位置 获取关节旋转 获取下一个事件

程序根据菜单下单：

GetAvatarPostureIndex(...); PollApplicationNextEvent(...);

MocapApi.dll相当于厨房，真正完成这些操作。

只有菜单，没有厨房：

程序可以编译 但运行时无法真正取得数据

只有厨房，没有菜单：

厨房具备功能 但C++编译器不知道函数怎么调用

所以编译和运行分别需要：

编译时：MocapApi.h和导入库 运行时：MocapApi.dll

---

3. r70头文件包含什么

SDK版本号

头文件里写着：

#define MOCAP_API_VERSION_MAJOR 0 #define MOCAP_API_VERSION_MINOR 0 #define MOCAP_API_VERSION_BUILD 70

所以称为r70。

错误码

例如：

Error_None Error_MoreEvent Error_InsufficientBuffer Error_NotSupported Error_NoneMessage

程序通过这些错误码判断SDK的回答。

例如：

Error_None = 调用成功 Error_MoreEvent = 当前事件有效，后面还有事件 Error_NotSupported = 当前SDK版本不支持这个功能 Error_NoneMessage = 当前没有事件

事件类型

例如：

MCPEvent_None MCPEvent_AvatarUpdated MCPEvent_SensorModulesUpdated MCPEvent_Notify MCPEvent_Error

程序需要根据事件类型决定怎样处理。

数据结构

例如：

MCPEvent_t MCPEvent_MotionData_t

MCPEvent_t大致描述：

事件结构体有多大 事件是什么类型 事件发生时间 事件携带的Avatar Handle或其他信息

接口函数

例如：

PollApplicationNextEvent(...) GetAvatarPostureIndex(...) GetJointGlobalPosition(...) GetJointGlobalRotation(...)

头文件只声明这些函数存在，真正实现位于DLL。

---

4. 为什么头文件对我们修改程序很重要

我们调用SDK函数时，必须严格按照头文件定义传参数。

例如：

PollApplicationNextEvent( MCPEvent_t* pEvent, uint32_t* punSizeOfEvent, MCPApplicationHandle_t application);

头文件告诉我们：

• 第一个参数是事件对象或事件数组的地址；
• 第二个参数是用于描述事件数量的整数地址；
• 第三个参数是Application Handle。

如果参数含义理解错误，例如把事件数量写成：

sizeof(MCPEvent_t)

程序虽然可能编译成功，但运行行为可能错误。

原因是：

编译器只能检查参数类型是不是uint32_t*，无法判断这个数字的业务含义是否正确。

以下两种写法类型上都合法：

uint32_t value = 1; PollApplicationNextEvent(&event, &value, app);

uint32_t value = sizeof(MCPEvent_t); PollApplicationNextEvent(&event, &value, app);

编译器只知道它们都是uint32_t，但SDK会按照自己的协议解释这个数值。

---

5. 头文件里有函数，不等于DLL一定支持

r70头文件中声明了：

EnableApplicationCacheEvents(...)

但是调用后，DLL可能返回：

Error_NotSupported

这类似菜单上统一印着某道菜，但当前分店回答：

本店暂不供应

因此程序不能只看头文件中有没有函数，还需要检查实际返回值。

当前程序就是这样处理：

auto error = application_api_->EnableApplicationCacheEvents(...); if (error == Error_NotSupported) { // r70运行库不支持Cache模式，继续使用快速Poll }

---

6. 头文件和DLL版本必须尽量匹配

理想情况是：

r70 MocapApi.h + r70导入库 + r70 MocapApi.dll

如果混用：

r70头文件 + 其他版本DLL

可能出现：

• 接口版本不一致；
• 结构体大小不一致；
• 函数行为不同；
• 某些接口返回Error_NotSupported；
• 程序启动失败；
• 极端情况下发生内存问题。

因此8_3干净构建时，需要确认编译和运行都使用同一套Noitom SDK。

---

7. 在当前工程中它位于哪里

r70 SDK目录：

C:\Users\RX01296\Desktop\全身控制8_3\ Noitom-MocapApi-r70(49068d72) (1)

头文件位于类似：

include\mocapapi\MocapApi.h

编译时，CMake会把这个目录加入头文件搜索路径。

编译完成后，还会把对应的：

MocapApi.dll

复制到：

build_8_3_clean_mocapapi_fifo_confirmed\Release

这样xsens_core.exe运行时才能加载它。

---

一句话总结：

r70头文件是Noitom MocapApi第70版的C++接口说明，它定义了事件、错误码、数据结构和函数调用方式；真正接收、解析和返回动作数据的是MocapApi.dll，头文件本身不会采集数据。