基于手指位置测量和基准标记的数据融合实现抓取状态估计

# 基于手指位置测量和基准标记的数据融合实现抓取状态估计 ## 1. 基于手指位置测量的抓取状态估计 ### 1.1 预测状态协方差计算 预测状态协方差 $P$ 按照公式 (4.24) 计算： $P_t = F_{t - 1} P_{t - 1} F_{t - 1}^T + R_t$ 其中，矩阵 $F_t \in R^{6 + m \times 6 + m}$ 是 $f$ 关于 $y$ 的偏导数，即： $F_t = \frac{\partial f}{\partial y} \big|_{y_{t - 1}, u_t}$ 虽然可以通过解析方法得到 $F_t$，但由于完整表达式较为复杂，建议使用计算机辅助符号微分技术。 在使用手雅可比矩阵和抓取矩阵预测物体运动时，隐含假设抓取配置（即手指连杆和物体表面上的接触位置）是恒定的。然而，由于手指在物体上的滚动和滑动，这种假设并不准确。此外，离散差分的计算会引入近似误差，且这些误差会随时间累积。因此，需要适当选择运动干扰 $R_t \in R^{6 + m \times 6 + m}$ 的大小，以表示预测中不断增加的不确定性。 预测步骤并未处理抓取状态中的不一致性，如手指连杆与物体之间的碰撞，这些问题将在更新步骤中得到纠正。 ### 1.2 测量模型 扩展卡尔曼滤波器（EKF）的第二步是更新，通过纳入一组测量值 $z_t$ 来获得校正后的状态估计。对于抓取状态估计，更新步骤的目标是解决手指与物体之间的不正确对齐问题。 测量模型描述了接触所固有的约束。如果手指连杆和物体都是刚性的，那么它们之间的穿透在物理上是不可能的。此外，如果手指连杆和物体之间存在感应接触，则两个物体必须对齐。 从分析角度来看，这种约束可以使用物体表面和连杆表面上的接触点 $c[i]_o$ 和 $c[i]_f$ 来表示。如果两个物体确实对齐，则这两个点之间的差值必须为零： $0_{3 \times 1} = c[i]_f - c[i]_o$ 否则，差值向量 $c[i]_f - c[i]_o$ 描述了最小可能的位移，该位移可以解决碰撞问题或使两个物体接触。这种类型的约束可以在 EKF 中表示为完美测量。 对于索引为 $i$ 的接触，测量模型描述了 $c[i]_o$ 和 $c[i]_f$ 之间的差异。如果手指连杆和物体确实接触，则可测量距离必须为 $z[i]_t = 0_{3 \times 1}$。对于所有 $n$ 个接触约束，完整的测量模型在时间 $t$ 可以表示为： $h(y_t) = c_f - c_o$ 完整的测量向量为： $z_t = 0_{3n \times 1}$ 虽然向量 $h(y_t) \in R^{3n}$ 可以根据接触点位置轻松获得，但更新步骤的计算还需要 $h(y_t)$ 的偏导数： $H_t = \frac{\partial h}{\partial y} \big|_{y_t}$ 由于 $c_f$ 和 $c_o$ 是通过迭代 GJK 算法计算得到的，因此无法通过解析方法进行公式化。然而，$h(y_t)$ 中接触位置关于状态的偏导数可以使用手雅可比矩阵和抓取矩阵来表示。 物体上接触点的运动与物体姿态的变化关系如下： $\frac{\partial c_o}{\partial x} = G^T W^{-1}$ 类似地，调整状态向量中的关节位置偏差会影响手指连杆上接触点的位置，这可以通过手雅可比矩阵描述： $\frac{\partial c_f}{\partial \tilde{q}} = J$ 完整的矩阵 $H_t \in R^{3n \times 6 + m}$ 通过堆叠这两个偏导数得到： $H_t = \begin{bmatrix} -G^T W^{-1} & J \end{bmatrix}$ 随后，可以计算卡尔曼增益 $K_t \in R^{6 + m \times 3n}$： $K_t = P_t H_t^T (H_t P_t H_t^T + Q_t)$ 原则上，测量干扰 $Q_t \in R^{3n \times 3n}$ 可以设置为零，因为完美测量 $z_t = 0_{3n \times 1}$ 不受任何噪声影响。然而，为了保证滤波器的数值稳定性，最好选择一个小的非零值。 最后，可以更新抓取状态的均值和协方差： $y_t = \hat{y}_t + K_t (z_t - h(y_t))$ $P_t = (I - K_t H_t) P_t$ ### 1.3 扩展 基于关节位置测量的抓取状态估计主要有两个组成部分：一是运动模型，可根据关节速度预测物体运动；二是测量模型，有助于使手指在物体上对齐。在此基础上，滤波器可以通过一些附加组件进行扩展，以提高实际应用中的估计质量。 #### 1.3.1 手掌预测 在实际应用中，机器人手通常不是固定在静态位置，而是手掌连接到机器人手臂，手臂可以自由定位操纵器。移动手掌会改变被抓物体的姿态，这会在估计滤波器中造成手与物体之间的不对齐，需要通过测量更新来解决。如果手臂移动速度较快，这种不对齐可能会很大，从而导致相当大的估计误差。 因此，在预测步骤中应将手掌的运动作为运动模型的扩展进行考虑。手掌在时间步 $t$ 的姿态记为 $x_{palm,t}$，通过机器人手臂的正向运动学计算得到。其速度 $\dot{x}_{palm,t}$ 可以测量或通过离散差分计算得到。它与手掌的扭转 $\nu_{palm,t}$ 之间的关系使用映射矩阵 $W_{palm}$ 表示。手掌扭转与接触点速度之间的关系可以使用类似物体的抓取矩阵 $G_{palm}$ 表示。手掌运动引起的接触位置变化 $\Delta c_{palm,t}$ 可以表示为： $\Delta c_{palm,t} = G_{palm}^T W_{palm}^{-1} \dot{x}_{palm,t} \Delta t$ 这个运动对来自手指关节的接触点位移 $\Delta c_t$ 有贡献，组合后