背景#

将动捕（mocap/BVH）骨骼数据 retarget 到轮式底盘人形机器人时，机器人比人多出底盘平移与转向（4WS 轮式底盘）这组额外自由度。常见做法是把底盘位姿（x, y, yaw）和上身关节一起丢进联合 IK 求解，但这会带来两个结构性问题。

问题#

1. 底盘被当配重：底盘自由度参与联合 IK 后，手臂前伸等大幅度上身动作会让 IK 把底盘往反方向挪来平衡末端误差，产生不符合物理直觉的底盘漂移。
2. yaw 参考失真：直接从躯干（Hips）朝向四元数提取 yaw，但骨骼末端的四元数往往携带了缩放/关节修正，不是干净的朝向角；侧向行走、弯腰等动作下失准明显。

方案：三步解耦流水线#

核心思路是把底盘轨迹从联合 IK 中剥离出来，单独用几何方法求解，再把上身 IK 限制在一个”冻住的底盘”上求解：

1
mocap 骨骼序列
2
    ├─ Step 1  底盘轨迹提取
3
    │   从骨盆位置直接算底盘 XY 轨迹（不进 IK）
4
    │   yaw = 行进速度方向（静止时 hold 上一帧值）
5
    │   低通滤波 + rate limit 平滑
6
    │
7
    ├─ Step 2  冻结底盘的上身 IK
8
    │   每帧把 Step-1 算出的底盘位姿写入 IK 的 seed
9
    │   底盘自由度施加极高权重，等效锁死
10
    │   IK 只求解上身关节
11
    │
12
    └─ Step 3  强制写回
13
        用 Step-1 的底盘轨迹覆盖 IK 输出中的底盘分量

底盘轨迹来源从”联合 IK 的副产品”变成”直接几何提取”，上身 IK 不再需要为平衡误差挪动底盘。

Yaw 估计设计#

用滑窗均速（而非逐帧瞬时速度）判断处于哪个区间，防止速度在阈值附近抖动导致状态来回切换。

一个容易踩的坑：hold 状态的初始值不能从第 0 帧速度求出——起始动作往往是静止的，第 0 帧速度本身就是噪声，一旦被当作初始 yaw hold 住，会污染整段序列，进而让下游 IK 在错误的坐标系里求解，上身姿态出现系统性偏差。通用结论：任何”状态保持型”滤波器，初始值都应该给一个安全的默认值（如 0 或标定值），而不是从可能带噪的第一帧数据里现算。

滑窗滤波的边界处理#

底盘轨迹做低通滤波时，序列首尾的滑窗天然缺少足够的邻域样本。若不特殊处理，边缘会被均值滤波”拉平”而失真。做法是：

• 起始边缘：用第一个有效值向前后向填充，保证窗口凑满时不引入虚假的向零衰减
• 结尾边缘：直接保持最后一段有效值，不做额外延伸

这是滑窗类算法的通用处理方式：只要输出会被下游消费（这里是底盘控制器），边界失真比中间段的噪声更容易在实际运行时被放大。