80、基于扩展卡尔曼滤波器的标记点姿态估计

基于扩展卡尔曼滤波器的标记点姿态估计

1. 引言

在估计缺失标记点数据时，若不考虑人体运动学，可能会影响关节角度的准确性。此前的一些方法存在计算成本高、无法保证协方差矩阵为正对称定等问题。本文提出了一种基于扩展卡尔曼滤波器（EKF）的新方法，用于实时全身标记点姿态估计，能够处理标记点缺失和交换的情况。

2. 技术方法

• 人体建模 ：将人体建模为铰接刚体骨架，相邻骨骼通过旋转关节连接，世界坐标系与运动学模型基础之间的转换由3个棱柱关节和3个旋转关节表示，每个标记点假定刚性附着在模型上。
• EKF公式
  • 状态更新 ：EKF状态由关节位置和速度组成，假设状态更新函数为常速度模型：
    [
    x_{k + 1} =
    \begin{bmatrix}
    I & \Delta t \
    0 & I
    \end{bmatrix}
    \underbrace{
    \begin{bmatrix}
    I & \Delta t \
    0 & I
    \end{bmatrix}
    } {A}
    x {k} + w_{k}
    ]
    其中，(x_{k} = [q_{k} \dot{q} {k}]^{T})是时间(k)时的关节角度和速度向量，(\Delta t)是采样间隔，(w {k})是零均值、协方差为(Q_{k})的白高斯噪声（WGN）。
  • 测量向量 ：测量向量由(m)个标记点的观测3D位置组成，通过非线性正向运动学与状态相关：
    [
    z_{k} = [x_{1} y_{1} z_{1} \cdots x_{m} y_{m} z_{m}]^{T} + v_{k} = h(x_{k})
    ]
    假设运动捕捉系统的测量噪声(v_{k})是协方差为(R_{k})的WGN。
  • EKF迭代算法 ：

算法1. EKF迭代：
Hk = ∂h/∂x 是测量雅可比矩阵，Pk 是状态误差协方差，Pz 是投影测量误差协方差，RKk 是受限最优卡尔曼增益。
1: 预测步骤:
    ˆx−k = Aˆxt−1
    P−k = APk−1AT + Qk−1
2: 更新步骤:
    ˜zk = zk − h(ˆx−k)
    计算受限卡尔曼增益:
        Pz = HkP−kHTk + Rk
        Kk = P−kHTkP−1z − (ˆx−k − b)(˜zTP−1z˜z)−1˜zTP−1z
    重新计算非受限关节的卡尔曼增益:
        ¯Pz = ¯Hk ¯P−k ¯HTk + Rk
        ¯Kk = ¯P−k ¯HTk ¯P−1z
    结合受限和非受限增益更新状态和协方差:
        Kk = [¯Kk; Kk]
        ˆxk = ˆx−k + Kk˜z
        Pk = (I − KkHk)P−k (I − KkHk)T + KkRKTk

为满足人体关节约束，限制最优卡尔曼增益。为确保非受限关节的卡尔曼增益接近最优，假设处于约束状态的关节固定，并重新计算其余关节的增益。使用约瑟夫形式的EKF协方差更新来保持误差协方差矩阵为正对称定。
- 标记点匹配
- 在实际中，基于反射标记点的运动捕捉系统常无法正确标记连续帧之间的标记点，且并非所有标记点在每帧都可见，因此EKF需能正确改变测量向量的大小。
- 为识别对应关系，使用投影到测量空间的误差协方差的逆(P_{z}^{-1})来构建每个观测标记点和建模标记点之间的对应度量。计算归一化观测概率的对数：
[
m_{ij} = \log(P(z_{i}|\hat{z} {j})) = (z {i} - \hat{z} {j})^{T} \Sigma {j}^{-1} (z_{i} - \hat{z} {j})
]
其中，(z {i} = [x_{i} y_{i} z_{i}])是测量标记点位置，(\hat{z} {j} = [\hat{x} {j} \hat{y} {j} \hat{z} {j}])是使用正向运动学预测的标记点位置。
- 构建匹配矩阵(M)，跟踪最小值索引。当最小值低于阈值时，检测到缺失或新匹配的标记点；两个建模标记点的最小索引交换表明运动捕捉标记点标签交换。最后，仅使用与匹配标记点对应的(\tilde{z} {k})、(H {k})和(K_{k})的值、行和列进行EKF更新。

3. 实验结果

• 跳远练习数据集

  • 数据集情况 ：包含21名参与者进行3种不同距离的立定跳远运动，使用修改后的Helen Hayes全身标记点集，共30个标记点，有原始未清理的标记点数据（原始数据）和手动清理后的数据（清理数据），最终分析了680条轨迹。
  • 参数设置 ：EKF初始协方差设为单位矩阵，过程噪声假设为协方差为(10^{6})的WGN，测量噪声设为10 mm，对应度量阈值设为(100 m^{2})，启用膝关节、踝关节和肘关节的关节约束。
  • 对比配置 ：
    • 标准原始配置：将原始标记点数据输入标准EKF，不进行对应调整，缺失标记点数据用最后已知值填充。
    • 标准清理配置：将手动清理后的数据输入标准EKF。
    • 提出的原始配置：将原始标记点数据输入提出的算法。
  • 评估指标 ：
    • 标记点位置均方根误差（RMSE，ME）：使用正向运动学和从三种测试中获得的关节角度估计标记点位置，并与清理数据进行比较。
    • 关节角度误差（JE）：将提出方法估计的关节角度与使用清理配置获得的关节角度进行比较，对于球形关节，使用基于旋转矩阵距离（RD）的关节误差度量。
    • 标记点对应结果：标记点可被识别为正确、交换或缺失。
    • 绝对最大关节速度（MJV）：若MJV高，表明标记点对应估计不准确，关节估计变化迅速以匹配错误分配的标记点。
  • 结果分析 ：使用原始标记点数据且无EKF改进时，RMSE最高，因为运动捕捉系统有时会误标记身体左右两侧。提出的使用原始数据的方法与使用手动清理数据的性能相似，表明该算法能成功检测标记点缺失和交换事件。
    | | Hip | R Leg | L Leg | Torso | R Arm | L Arm | Avg |
    | — | — | — | — | — | — | — | — |
    | ME StR | 51 ± 41 | 41 ± 65 | 45 ± 67 | 73 ± 70 | 165 ± 227 | 170 ± 225 | 81 ± 78 |
    | ME StC | 33 ± 4 | 22 ± 8 | 21 ± 3 | 29 ± 6 | 12 ± 3 | 11 ± 3 | 21 ± 3 |
    | ME PpR | 35 ± 3 | 22 ± 8 | 20 ± 3 | 30 ± 9 | 13 ± 5 | 13 ± 6 | 22 ± 3 |
    | JE Rev | 1 ± 12 | 1 ± 1 | 2 ± 3 | 5 ± 14 | 3 ± 13 | 2 ± 5 | |
    | RD Sph | 99 ± 1 | 99 ± 1 | 99 ± 1 | 99 ± 3 | 99 ± 4 | 99 ± 2 | |
    | ID Cor | 86 ± 29 | 90 ± 28 | 91 ± 28 | 83 ± 23 | 58 ± 42 | 52 ± 40 | 79 ± 21 |
    | ID Swa | 9 ± 27 | 10 ± 28 | 9 ± 28 | 14 ± 22 | 40 ± 42 | 47 ± 40 | 19 ± 21 |
    | ID Mis | 6 ± 10 | 1 ± 2 | 1 ± 2 | 3 ± 6 | 1 ± 3 | 1 ± 3 | 2 ± 2 |
    | VC Rev | 421 ± 189 | 413 ± 62 | 187 ± 220 | 351 ± 137 | 356 ± 155 | 344 ± 128 | |
    | VR Rev | 684 ± 1515 | 575 ± 101 | 443 ± 265 | 1231 ± 1991 | 980 ± 1284 | 702 ± 525 | |

• 全身运动数据集

  • 数据集情况 ：包含15名参与者进行各种全身运动，共80个标记点，由于标记点数量和接近程度，遮挡频繁，无法进行手动清理和与第一个数据集相同的比较。
  • 评估指标 ：报告标记点位置RMSE、标记点对应结果和绝对最大关节速度。
  • 结果分析 ：尽管存在多个标记点缺失和交换事件，提出的方法实现了低于30 mm的RMSE精度。简单运动（如拾取和放置、手臂蝴蝶摆动）表现良好，MJV标准差低，表明标记点遮挡处理平稳，关节约束使关节保持在可行范围内；其他运动（如深蹲）MJV标准差高，可能是参与者运动速度不同或重建误差导致。
    | | PICK | ABUT | HIGH | SQUF | SQUC | CORK | Avg |
    | — | — | — | — | — | — | — | — |
    | ME PpR | 25 ± 3 | 25 ± 3 | 25 ± 3 | 23 ± 2 | 24 ± 3 | 54 ± 103 | 27 ± 30 |
    | ID Cor | 75 ± 22 | 83 ± 16 | 67 ± 19 | 74 ± 23 | 71 ± 20 | 54 ± 41 | 73 ± 23 |
    | ID Swa | 18 ± 19 | 11 ± 12 | 22 ± 9 | 19 ± 23 | 21 ± 18 | 43 ± 24 | 19 ± 20 |
    | ID Mis | 7 ± 5 | 7 ± 5 | 11 ± 12 | 7 ± 5 | 8 ± 5 | 17 ± 8 | 9 ± 7 |
    | VR Rev | 206 ± 163 | 235 ± 176 | 958 ± 709 | 487 ± 239 | 549 ± 115 | 4872 ± 4524 | 1012 ± 2506 |

4. 方法流程总结

graph TD
    A[人体建模] --> B[EKF公式]
    B --> C[标记点匹配]
    C --> D[实验验证]
    D --> E[结果分析]

综上所述，提出的基于扩展卡尔曼滤波器的方法在处理标记点缺失和交换方面表现良好，能有效提高姿态估计的准确性。但该算法仍存在一些不足，如无法完全处理长时间遮挡的标记点。未来可考虑为消失的标记点添加阻尼权重，以及根据参与者当前姿势修改关节限制，使用更复杂的运动学模型以进一步降低RMSE。

基于扩展卡尔曼滤波器的标记点姿态估计

5. 讨论与分析

从实验结果来看，该算法在处理标记点姿态估计问题上有显著的优势，但也暴露出了一些问题。

• 优势体现

  • 处理标记点异常 ：算法能够成功检测标记点的缺失和交换事件。在跳远练习数据集中，使用原始数据的提出方法与使用手动清理数据的性能相近，说明其在处理标记点缺失和交换方面表现出色，减少了因标记点异常带来的误差。
  • 关节约束有效 ：通过限制卡尔曼增益来强制执行现实的关节限制，使得关节角度估计更加合理。例如在全身运动数据集中，简单运动的MJV标准差低，表明关节约束能使关节保持在可行范围内，减少了不必要的异常运动。

• 存在的问题

  • 长时间遮挡问题 ：算法无法完全处理长时间被遮挡的标记点。当缺失的标记点在更远的位置重新出现时，由于长时间遮挡或高速运动，肢体位置的估计可能与标记点位置相差过大，导致标记点无法重新关联。
  • 运动学模型简单 ：一些复杂的身体运动，如深蹲时手臂交叉在胸前和螺旋式触趾动作，算法的表现不佳。这可能是因为运动学模型过于简单，无法捕捉复杂的脊柱或肩部运动，或者在模型构建过程中关键标记点缺失导致参数不准确。

6. 未来工作展望

为了进一步提升算法的性能，可以从以下几个方面进行改进：

• 处理长时间遮挡标记点 ：为消失的标记点添加阻尼权重，在标记点重新出现之前，减少肢体估计的剧烈变化，使估计值更加平滑，避免因标记点长时间遮挡或高速运动导致的估计偏差。

• 优化关节限制 ：考虑根据参与者的当前姿势修改关节限制，即引入上下文相关的球形关节限制。这样可以使关节角度的估计更加符合人体实际运动情况，提高姿态估计的准确性。

• 改进运动学模型 ：使用更复杂的运动学模型来描述人体运动。对于复杂的身体部位，如躯干和肩部，采用更精细的建模方法，以更好地捕捉其运动特征，从而降低RMSE。

7. 总结

本文提出的基于扩展卡尔曼滤波器的标记点姿态估计方法，通过生成匹配矩阵处理标记点缺失和交换问题，并限制卡尔曼增益以满足关节限制。实验结果表明，该方法在处理标记点异常时具有良好的性能，使用原始数据的估计结果与手动清理数据的结果相近。然而，算法仍存在处理长时间遮挡标记点和运动学模型简单等问题。未来可以通过添加阻尼权重、优化关节限制和改进运动学模型等方式进一步提升算法性能。

改进方向	具体措施	预期效果
处理长时间遮挡标记点	为消失的标记点添加阻尼权重	减少肢体估计的剧烈变化，使估计更平滑
优化关节限制	根据参与者当前姿势修改关节限制	使关节角度估计更符合人体实际运动
改进运动学模型	使用更复杂的运动学模型描述人体运动	更好地捕捉复杂身体部位运动特征，降低RMSE

graph LR
    A[当前算法] --> B[处理长时间遮挡问题]
    A --> C[优化关节限制]
    A --> D[改进运动学模型]
    B --> E[添加阻尼权重]
    C --> F[引入上下文关节限制]
    D --> G[采用精细建模方法]
    E --> H[更平滑的估计]
    F --> I[更准确的关节角度]
    G --> J[更低的RMSE]

总之，该算法为标记点姿态估计提供了一种有效的解决方案，但仍有改进的空间。通过不断优化和完善，有望在人体运动捕捉和姿态估计领域取得更好的应用效果。