论文笔记（六十七）Application of Visual Servo in Tracking and Grasping Moving Target

文章概括

引用：

@inproceedings{nguyen2022application,
  title={Application of Visual Servo in Tracking and Grasping Moving Target},
  author={Nguyen, Duc Tam-Hong and Nguyen, Vinh-Hao},
  booktitle={2022 International Conference on Control, Robotics and Informatics (ICCRI)},
  pages={83--87},
  year={2022},
  organization={IEEE}
}

Nguyen, D.T.H. and Nguyen, V.H., 2022, April. Application of Visual Servo in Tracking and Grasping Moving Target. In 2022 International Conference on Control, Robotics and Informatics (ICCRI) (pp. 83-87). IEEE.

原文： https://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=9870710
代码、数据和视频：

系列文章：
请在 $《$文章$》$ 专栏中查找

摘要

本文提出了一种用于跟踪和抓取三维空间中运动目标的视觉伺服系统。该系统采用结合已知物体3D CAD模型的多线索特征视觉跟踪模块，用于提取RGB-D图像中的目标姿态。为进一步补偿系统延迟并估计物体运动，开发了扩展卡尔曼滤波器（EKF）。提出了一种两阶段控制器用于视觉伺服，第一部分能够控制夹爪跟踪目标，而第二部分是基于姿态的视觉伺服（PBVS）方案，用于抓取动作。实验结果表明，该系统能够执行实时跟踪和抓取运动目标的任务。

I. 引言

服务机器人在社会和自动化的发展中变得越来越受欢迎和重要$[1]$, $[2]$。服务机器人的主要功能之一是增强人机交互，以促进自动化。例如，人与机器之间的物体转移可以视为一项典型任务，该任务可以分解为跟踪和捕获物体。因此，使用相机进行机器人控制（视觉伺服）和物体识别变得必要。

视觉伺服（VS）研究多年来稳步发展。VS方法有两种主要分类：相机配置和输入信号。根据相机的排列，VS可以分为两种类型。眼在手（EIH）配置将相机直接放置在机器人上，而眼对手（ETH）配置将相机固定在空间中的位置$[3]$。另一个基于误差信号分析的VS分类方法包括：基于图像的视觉伺服（IBVS）、基于姿态的视觉伺服（PBVS）和混合视觉伺服（$2\frac{1}{2}$D VS）$[4]$, $[5]$。IBVS直接使用图像像素来为机器人提供控制信号。同时，PBVS方法通过图像估计物体姿态来生成控制信号。$2\frac{1}{2}$D VS方法结合了前两种方法来计算控制信号的不同部分。

为了在运动中执行视觉伺服的跟踪，研究通常使用预测器来找到物体的轨迹。根据$[6]$ –$[9]$，将PBVS与3D运动估计器相结合的系统给出了最满意的结果。$[6]$利用递归最小二乘估计器与PBVS相结合来捕捉飞行物体。$[7]$将PBVS与卡尔曼滤波器（KF）结合使用，以处理图像数据中的噪声，并使用扩展卡尔曼滤波器（EKF）来估计目标的运动。同时，$[8]$通过增强运动跟踪，使用自适应扩展卡尔曼滤波器（AEKF）扩展了$[7]$的工作。上述两个系统都应用于具有EIH配置和单目相机的5自由度（DOF）机器人。他们的工作集中在目标具有独特视觉特征和独特的目标抓取姿态的情况下。

随着RGB-D相机的出现，物体检测和视觉跟踪也提升到了一个新的水平。$[9]$应用了PBVS并使用特殊的仿射几何技术，通过实时计算增强了RGB-D图像中物体的跟踪。同时，$[10]$使用了RGB-D多相机系统，用于同时观察整个工作区域，并利用立体视觉实现更精确的控制。本研究研究了一种基于ETH配置的PBVS系统在6自由度（DOF）臂上抓取运动物体的任务中的表现。目标可以从机器人工作空间外的任意位置开始。此外，基于给定3D CAD模型的多特征框架用于RGB-D图像的视觉跟踪，同时使用EKF来估计物体的轨迹。设计了一种基于两个特定区域的两阶段机器人控制器，以确保机器人能够捕捉到运动目标。

本文其余部分组织如下：第二部分介绍了视觉模块，包括RGB-D视觉跟踪系统和EKF。第三部分介绍了为跟踪和抓取任务提出的两阶段控制器。接下来，第四部分展示了系统有效性的实验结果。最后，第五部分以结论结束本文。

II. 运动目标的视觉跟踪

A. 视觉伺服系统概述

该视觉伺服系统的主要目标是捕捉一个在自由空间中运动且轨迹未知的物体。所提出系统的工作原理可以解释如下：

• 首先，视觉模块将在RGB-D图像中执行物体的识别和跟踪。该视觉跟踪框架利用多线索特征，如边缘、纹理和深度信息。通过结合已知的CAD模型，提取目标的姿态，并进一步输入到扩展卡尔曼滤波器（EKF）中进行运动估计。

• 其次，根据目标姿态和夹爪姿态选择物体的抓取姿态。期望的抓取姿态用于所提出的基于两个不同区域的两阶段机器人控制器。在该控制器中，第二个区域被定义为机器人的工作空间，而第一个区域是工作空间外的体积。

• 当期望的抓取姿态位于第一个区域时，执行控制器的第一阶段，称为瞄准跟踪控制。在这一阶段，机器人最后三个旋转关节被驱动，使得夹爪跟踪物体。当抓取姿态进入第二个区域时，控制器切换到第二阶段，该阶段用于根据PBVS方案将夹爪移向抓取姿态。

• 最后，当夹爪接近物体并且满足抓取条件时，夹爪闭合以抓取目标。图1展示了所提系统的整体算法。

图1：视觉伺服系统的算法

B. 多特征视觉跟踪模块

如上所述，视觉模块利用多个特征根据已知的CAD模型在RGB-D图像中跟踪目标物体，基于$[11]$。在本研究中，使用Realsense D435相机捕获颜色和深度信息，并使用三个特征进行视觉跟踪。前两个特征是来自颜色通道的目标边缘和纹理。边缘特征通过使用运动边缘跟踪器$[11]$检测，而纹理特征则通过应用SIFT跟踪器$[12]$进行跟踪。此外，密集深度特征在我们的框架中被使用，以提供对图像噪声的鲁棒性。视觉系统通过ViSP库（C++）$[13]$实现，以提供高效的计算。

C. 扩展卡尔曼滤波器

视觉跟踪模块的输出是目标在当前图像中的姿态。为了补偿视觉伺服系统中的延迟，实施了扩展卡尔曼滤波器（EKF）。运动物体的动态模型基于$[14]$，将运动模型分为两部分：平移模型和姿态模型。首先，目标运动的平移模型是一个恒定加速度模型：

$$\begin{array}{c}\{\begin{array}{l}{\mathbf{p}}_k={\mathbf{p}}_{k-1}+{\mathbf{v}}_{k-1}\Delta t+\frac{1}{2}{\mathbf{a}}_{k-1}(\Delta t{)}^2+{\mathbf{w}}_p\\ {\mathbf{v}}_k={\mathbf{v}}_{k-1}+{\mathbf{a}}_{k-1}\Delta t+{\mathbf{w}}_v\\ {\mathbf{a}}_k={\mathbf{a}}_{k-1}+{\mathbf{w}}_a\end{array}\end{array}\text{\hspace{1em}(1)}$$

其中，$\Delta t$是采样时间。而$p_k$、$v_k$和$a_k$分别是平移模型的位移、速度和加速度向量。此外，$w_p$、$w_v$和$w_a$是模型中的相应噪声向量。接下来，姿态模型选择为一个恒定角速度模型，并使用四元数表示：

$$\begin{array}{c}\{\begin{array}{l}{\mathbf{q}}_k=\left[\cos (A)I_{4x4}+\frac{2}{|{\omega }_{k-1}|}\sin (A)\Omega ({\omega }_{k-1})\right]{\mathbf{q}}_{k-1}+{\mathbf{w}}_q\\ {\omega }_k={\omega }_{k-1}+{\mathbf{w}}_{\omega }\end{array}\end{array}\text{\hspace{1em}(2)}$$

其中$A=\frac{|{\omega }_{k-1}|\Delta t}{2}$，$q_k$是四元数向量，${\omega }_k$是角速度，$w_q$和$w_{\omega }$是过程噪声向量。函数$\Omega ({\omega }_k)$进一步定义为：

$$\begin{array}{c}\Omega ({\omega }_k)=\left[\begin{array}{cccc}0& {\omega }_z& -{\omega }_y& {\omega }_x\\ -{\omega }_z& 0& {\omega }_x& {\omega }_y\\ {\omega }_y& -{\omega }_x& 0& {\omega }_z\\ -{\omega }_x& -{\omega }_y& -{\omega }_z& 0\end{array}\right]\end{array}\text{\hspace{1em}(3)}$$

因此，EKF的平移动态和测量模型为：

$$\begin{array}{c}\{\begin{array}{l}{\mathbf{x}}_{1,k}=\left[\begin{array}{c}{\mathbf{p}}_k\\ {\mathbf{v}}_k\\ {\mathbf{a}}_k\end{array}\right]=F_{1,k-1}{\mathbf{x}}_{1,k-1}+{\mathbf{w}}_{1,k-1}\\ {\mathbf{z}}_{1,k}=[{\mathbf{p}}_k]=G_{1,k}{\mathbf{x}}_{1,k}+{\mathbf{v}}_{1,k}\end{array}\end{array}\text{\hspace{1em}(4)}$$

其中$G_{1k}=\left[\begin{array}{ccc}{I}_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\end{array}\right]$，$F_1$基于(1)，$z_{1k}$是状态向量$x_{1,k}$的测量向量。此外，$w_{1k-1}$和$u_{1,k}$是模型和测量噪声向量。接下来，EKF中的姿态模型为：

$$\begin{array}{c}\{\begin{array}{l}{\mathbf{x}}_{2,k}=\left[\begin{array}{c}{\mathbf{q}}_k\\ {\mathbf{\omega }}_k\end{array}\right]=f_2({\mathbf{x}}_{2,k-1})+{\mathbf{w}}_{2,k-1}\\ {\mathbf{z}}_{2,k}=[{\mathbf{q}}_k]=G_{2,k}{\mathbf{x}}_{2,k}+{\mathbf{v}}_{2,k}\end{array}\end{array}\text{\hspace{1em}(5)}$$

其中$f_2(x_{2,k-1})$是基于(2)–(3)的非线性方程，$G_{2,k}=I_{4\times 4}{0}_{4\times 3}{x}_{2,k}{z}_{2,k}$是状态和测量向量，$w_{2k-1}$和$u_{2,k}$是它们对应的噪声向量。

III. 两阶段机器人控制器

如第II.A节所述，两个阶段的控制器是对应于两个不同区域实现的。如果期望的抓取姿态在机器人工作空间外，执行第一阶段（瞄准跟踪控制）来控制夹爪跟踪目标。相反，执行第二阶段（PBVS）来控制机器人抓取物体。

实施该控制器有两个主要原因。首先，当抓取姿态位于第一个区域时，目标超出了机械臂的可达范围。其次，通过使用瞄准跟踪控制，目标是为夹爪与目标之间创建一个最优的接近角度。因此，第二阶段的轨迹应该是一条笛卡尔线。

在本工作中，$T_i^j=\left[\begin{array}{cc}{R}_i^j& t_i^j\\ 0_{1\times 3}& 1\end{array}\right]$ 表示变换矩阵，而$R_l^J$和$t_l^J$分别是从框架$i$到框架$j$的旋转矩阵和平移向量。同时，设$6$为关节位置向量，当前夹爪姿态是通过正向运动学（FK）函数计算的，如下所示：

$$\begin{array}{c}{T}_{\text{gripper}}^{\text{robot}}=T_6^0(\mathbf{\theta })=T_1^0{T}_2^1{T}_3^2{T}_4^3{T}_5^4{T}_6^5\end{array}\text{\hspace{1em}(6)}$$

图2：a) 机器人MotoMini，b) 机器人示意图与DH参数

图2a展示了本文使用的实际MotoMini机器人。同时，图2b中分配给机器人每个关节的DH参数列举如下（单位为毫米）：$a_1=20$，$d_1=103$，$a_2=165$，$d_4=165$和$d_6=135$。

A. 第一阶段：瞄准跟踪控制

由于在这一阶段机器人无法到达期望的抓取姿态，因此计算一个新的期望姿态。控制最后三个关节，使得夹爪绕腕部中心旋转，以接近这个新的姿态。

参数$d_6$表示机器人腕部中心到夹爪的距离。因此，可以通过设定$d_6=0$并计算$(6)$来确定腕部中心$t_{center}^{robot}$。然后，可以通过连接$t_{center}^{robot}$和$t_{object}^{robot}$的直线$l_{co}^{robot}$计算期望位置$t_{pose}^{robot}$：

$$\begin{array}{c}{\mathbf{t}}_{\text{pose }}^{\text{robot }}=l_{co}^{\text{robot }}\left(d_6\right)={\mathbf{t}}_{\text{center }}^{\text{robot }}+{\mathbf{t}}_{\text{center }}^{\text{robot }}-{\mathbf{t}}_{\text{object }}^{\text{robot }}{d}_6\end{array}\text{\hspace{1em}(7)}$$

期望姿态和物体姿态的旋转矩阵分别为$R_{pose}^{robot}=[x_{pose}^{robot}{y}_{pose}^{robot}{z}_{pose}^{robot}]$和$R_{object}^{robot}=[x_{object}^{robot}{y}_{object}^{robot}{z}_{object}^{robot}]$。为了使夹爪与目标对准，$x_{pose}^{robot}$和$x_{object}^{robot}$应该彼此平行。因此，期望姿态的方向部分计算为：

$$\begin{array}{r}{\mathbf{Z}}_{\text{pose}}^{\text{robot}}=({\mathbf{t}}_{\text{center}}^{\text{robot}}-{\mathbf{t}}_{\text{object}}^{\text{robot}})/|{\mathbf{t}}_{\text{center}}^{\text{robot}}-{\mathbf{t}}_{\text{object}}^{\text{robot}}|\\ {\mathbf{y}}_{\text{pose}}^{\text{robot}}={\mathbf{Z}}_{\text{pose}}^{\text{robot}}\times {\mathbf{X}}_{\text{object}}^{\text{robot}}\\ {\mathbf{X}}_{\text{pose}}^{\text{robot}}={\mathbf{y}}_{\text{pose}}^{\text{robot}}\times {\mathbf{Z}}_{\text{object}}^{\text{robot}}\end{array}\text{\hspace{1em}(8)}$$

接下来，$R_3^0$是通过关节位置反馈和$(6)$获得的。最后三个关节的期望值$({\theta }_{4d},{\theta }_{5d},{\theta }_{6d})$被确定为：

$$\begin{array}{c}{R}_{d6}^3=R_4^3({\theta }_{4d})R_5^4({\theta }_{5d})R_6^5({\theta }_{6d})=(R_3^0{)}^{-1}{R}_{\text{pose}}^{\text{robot}}\end{array}\text{\hspace{1em}(9)}$$

图3展示了第一阶段新期望姿态的设置。该姿态位于直线$l_{robotco}$上，距离腕部中心$d_6$。其方向基于$(8)$进行了正确定位，蓝色和红色箭头分别表示$z$和$x$向量。此外，当机器人接近这个期望姿态时，夹爪将与目标形成一个最优的接近角度（即一条直线）。

图 3: 目标跟踪控制的理想姿势

B. 第二阶段：基于位置的视觉伺服用于抓取

变换$T_{object}^{cam}$（从物体坐标系到相机坐标系）通过EKF和视觉系统进行预测。同时，$T_{cam}^{robot}$表示从相机到机器人坐标系的变换。$T_{gripper}^{robot}$通过$(6)$计算得到。期望的抓取姿态在物体坐标系中的变换$T_{grasp}^{object}$将在下一部分计算。PBVS方案利用这些矩阵计算期望的平移和方向速度$V_{robot}$和${\omega }_{robot}$。首先，误差矩阵$T_{grasp}^{gripper}$通过以下方式获得：

$$\begin{array}{rl}{T}_{\text{grasp}}^{\text{gripper}}& =\left[\begin{array}{cc}{R}_{\text{grasp}}^{\text{gripper}}(\mathbf{k},\theta )& {\mathbf{t}}_{\text{grasp}}^{\text{gripper}}\\ 0_{1\times 3}& 1\end{array}\right]\\ & =T_{\text{gripper}}^{{\text{robot}}^{-1}}{T}_{\text{cam}}^{\text{robot}}{T}_{\text{object}}^{\text{cam}}{T}_{\text{grasp}}^{\text{object}}\end{array}\text{\hspace{1em}(10)}$$

其中$k,\theta$是$R_{grasp}^{gripper}$的轴和角度分量。然后，期望速度的计算为：

$$\begin{array}{c}\left[\begin{array}{c}{\omega }_{\text{robot}}=R_{\text{gripper}}^{\text{robot}}{k}_1\theta \mathbf{k}\\ {\mathbf{V}}_{\text{robot}}=R_{\text{gripper}}^{\text{robot}}[k_2{\mathbf{t}}_{\text{grasp}}^{\text{gripper}}-k_1\theta \mathbf{k}\times {\mathbf{t}}_{\text{grasp}}^{\text{gripper}}+{\mathbf{t}}_{\text{gripper}}^{\text{robot}}]\end{array}\right].\end{array}\text{\hspace{1em}(11)}$$

其中$k_1,k_2$是PBVS方案的控制参数。它们是根据响应结果（稳态时间、超调百分比和稳态误差）手动调整的，当将PBVS法应用于机器人系统时，控制输入是预定义的静态目标姿态。最后，通过机器人的雅可比矩阵$J$，可以计算期望的关节速度${\dot{\theta }}_d$：

$$\begin{array}{c}{\dot{\mathbf{\theta }}}_d=J^T(J{J}^T+{\lambda }^{2}I{)}^{-1}\left[\begin{array}{c}{\mathbf{V}}_{\text{robot}}\\ {\omega }_{\text{robot}}\end{array}\right]\end{array}\text{\hspace{1em}(12)}$$

其中$\lambda$是一个预定义的参数。

C. 抓取姿态选择

由于目标是一个矩形盒子且夹爪是平行型的，机器人应该在盒子的六个面之一上抓取目标。因此，抓取姿态应该提前选择。此外，期望的抓取姿态的方向应该使夹爪的法向量（x轴）大致平行于选择的面对应的盒子轴。

定义集合 { p l } , i ∈ { 1 , … , 6 } \{p_l\}, i \in \{1, \dots, 6\} {pl​},i∈{1,…,6}为对应于盒子六个面的抓取点坐标。选择与直线$l_{co}^{robot}$和夹爪位置的距离最小的点作为期望的抓取点。抓取姿态的方向计算方式与$(8)$类似。

图 4:选择抓取姿势

图4展示了抓取姿态选择方法的示例。为了说明，给盒子附加了一个坐标系。最初在图4a中，抓取姿态位于盒子的白色长面上。然后，当实验者旋转物体时，选择了另一个面作为期望的面，并在图4b中设置了新的抓取姿态。

IV. 实验结果

在本研究中，PC工作站为戴尔Inspiron 14笔记本，搭载Intel Core i5-7200 @ 2.50GHz，运行Ubuntu 18.04。本研究使用机器人操作系统（ROS）$[15]$来处理机器人与PC之间的多线程和通信。D435相机通过USB端口连接到PC，并通过ViSP库集成。图5展示了所提视觉伺服系统的架构。

图 5:视觉伺服系统

在实验过程中，目标（矩形盒子）从机器人工作空间外的一个任意姿态开始。实验者随后将目标沿着一个未知的3D轨迹移动，向机器人移动，以模拟人类与机器人之间物体转移的动作。当夹爪完成抓取目标时，实验停止。

图 6: a) 抓手和目标在三维空间中的三维轨迹 b) 机器人关节角度的时间历程

图6a展示了抓取运动目标实验中的结果轨迹。点1和点1’表示夹爪和目标的起始姿态。当目标不可达时（点2），执行控制器的第一阶段来跟踪物体（点2’），从而获得夹爪的期望接近角度。当期望的抓取姿态进入工作空间时（点3），PBVS方案启动抓取动作（点3’）。在点4时，抓取条件满足，夹爪成功抓取目标。此外，夹爪在此时正确地与盒子对准，因为夹爪和目标的x轴（红色轴）平行。

图6b展示了实验中关节值的时间历史。从0.3秒到8.4秒，抓取姿态不可达，最后三个关节通过瞄准跟踪控制被驱动。当机器人在剩余的测试时间内能够到达抓取姿态时，机器人臂通过PBVS方案进行控制。因此，夹爪的轨迹是到目标的笛卡尔线，如图6a所示。

图 7: 从另一个角度看实验

图7从不同角度展示了实验。在图7a中，视觉模块检测到工作空间外的物体。夹爪被控制来跟踪目标，当目标在自由空间中移动时，如图7b所示。图7c显示了机器人能够在工作空间内捕捉到物体。在实际系统中，所提系统能够以20 Hz的采样频率为机器人控制系统提供实时解决方案。

V. 结论

所提的视觉伺服系统能够在跟踪和抓取期望目标的任务中执行实时操作。多特征系统与已知CAD模型的结合提高了RGB-D图像中视觉跟踪的效率。此外，EKF能够补偿系统的时间延迟。控制器的第一阶段使夹爪能够在三维空间中跟踪物体，而第二阶段则通过PBVS控制机器人以最优的笛卡尔方式抓取目标。实验结果证明了所提的两阶段机器人控制器的有效性，以及EKF在所选任务中的运动估计性能提升。