抓取状态估计中的数据融合方法

# 抓取状态估计中的数据融合方法 在机器人抓取和操作任务中，准确估计物体的状态至关重要。本文将介绍几种不同的数据融合方法，包括使用基准标记、轮廓特征和视觉目标跟踪等技术，以提高抓取状态估计的准确性。 ## 1. 基准标记的数据融合 ### 1.1 3D 点投影到 2D 图像平面 使用针孔相机模型，可以将 3D 空间中的点投影到 2D 图像平面上。对于 AprilTag 角点，其图像坐标 $\overline{p}[l]_t$ 可以通过将其在相机固定坐标系 $\{C\}$ 中的位置 $cx[l]_{p,t}$ 进行投影得到： \[s \begin{pmatrix} \overline{p}[l]_t \\ 1 \end{pmatrix} = C \begin{pmatrix} cx[l]_{p,t} \\ 1 \end{pmatrix} \] 其中，$s$ 是缩放因子，$C \in \mathbb{R}^{3\times4}$ 是相机矩阵： \[C = \begin{pmatrix} f_u & 0 & c_u & 0 \\ 0 & f_v & c_v & 0 \\ 0 & 0 & 1 & 0 \end{pmatrix} \] 这里，$f_u$ 和 $f_v$ 描述相机的焦距，$c_u$ 和 $c_v$ 是图像光心的坐标。 当描述角点相对于物体坐标系 $\{O\}$ 的位置时，投影可以表示为： \[s \begin{pmatrix} \overline{p}[l]_t \\ 1 \end{pmatrix} = CT^{-1}_c T_{o,t} \begin{pmatrix} ox[l]_p \\ 1 \end{pmatrix} \] 其中，$T_c$ 是惯性坐标系和相机固定坐标系 $\{C\}$ 之间的变换，物体变换 $T_{o,t}$ 由当前的姿态估计 $x_t$ 计算得到。 矩阵 $H_{p,t} \in \mathbb{R}^{8\times6+m}$ 可以通过对 $\overline{p}[l]_t$ 关于 $y_t$ 进行偏导数得到： \[H_{p,t} = \frac{\partial\overline{p}[l]_t}{\partial y_t}\big|_{y_t} \] 测量干扰 $Q_{p,t} \in \mathbb{R}^{8\times8}$ 表示角点坐标在像素数量上的不准确性。 ### 1.2 相机定位 在之前的视觉特征测量模型中，通常假设相机的变换 $T_c$ 是已知的。然而，对于某些机器人系统，这并不一定成立。例如，David 机器人的头戴式相机，其头部的方向由弹性连续体机制控制，无法通过运动学准确确定相机的精确位置。 为了解决这个问题，可以通过额外的视觉信息来估计相机的姿态。将 AprilTag 安装在机器人手上，可以通过测量其角点的图像坐标来确定相机相对于手掌的位置。相机定位被完全集成到抓取状态估计中，以确保手 - 物体状态的一致性。 估计的相机姿态 $x_{c,t} \in \mathbb{R}^6$ 扩展了状态向量 $y_t \in \mathbb{R}^{12+m}$： \[y_t = \begin{pmatrix} x_t \\ \tilde{q}_t \\ x_{c,t} \end{pmatrix} \] 对于扩展卡尔曼滤波器（EKF）的初始化，需要一个初始相机姿态 $x_{c,0}$，其初始协方差根据其来源的质量进行设置。 手掌安装的 AprilTag 角点图像坐标向量 $z_{palm,t} \in \mathbb{R}^8$ 可以表示为： \[z_{palm,t} = \begin{bmatrix} p[1]^T_{palm,t} & p[2]^T_{palm,t} & p[3]^T_{palm,t} & p[4]^T_{palm,t} \end{bmatrix}^T \] 通过测量手掌标签相对于手掌固定坐标系 $\{P\}$ 的恒定变换 $pT_a$，可以计算出角点在手掌坐标中的位置 $px_p$，并使用针孔相机模型将其投影到相应的图像坐标： \[s \begin{pmatrix} \overline{p}[l]_{palm,t} \\ 1 \end{pmatrix} = CT^{-1}_{c,t} T_p \begin{pmatrix} px[l]_p \\ 1 \end{pmatrix} \] 其中，$T_p$ 是手掌坐标系 $\{P\}$ 相对于惯性坐标系的变换，通过手臂的正向运动学计算得到。向量 $\overline{p}_{palm,t}$ 表示该输入的测量模型 $h_{palm,t}(y_t) \in \mathbb{R}^8$： \[h_{palm}(y_t) = \overline{p}_{palm,t} \] ### 1.3 目标跟踪 将相机定位集成到抓取状态估计中，可以准确确定物体相对于手的相对姿态。进一步扩展这种能力，可以包括对目标物体的定位。 在许多操作场景中，需要将抓取的物体放置在机器人环境中的某个元素上。例如，解决堆叠游戏需要将游戏块精确地放置在木板上方。通过将目标姿态纳入抓取状态估计，可以确定完整的手 - 物体 - 目标系统的一致描述。 状态向量扩展为包括估计的目标姿态 $x_{target,t} \in \mathbb{R}^6$： \[y_t = \begin{pmatrix} x_t \\ \tilde{q}_t \\ x_{c,t} \\ x_{target,t} \end{pmatrix} \] 确定目标姿态的一种方法是融合附加在目标上的 AprilTag 的人工特征。与物体安装的 AprilTag 类似，使用标签四个角点的图像坐标 $p[l]_{target} \in \mathbb{R}^8$，并在 EKF 中进行融合。 ## 2. 轮廓特征的数据融合 ### 2.1 特征提取 使用基准标记进行视觉信息融合虽然可靠，但需要对物体进行物理修改，这在某些应用中是不可行的。因此，提出了一种无标记的解决方案，通过提取自然发生的图像特征（如角点和边缘）来进行抓取状态估计。 该方法的目标是从图像中提取自然发