0 概述

目前，基于深度视觉的机器人在人类生活与生产活动中逐渐占据重要地位，并广泛应用于工农业生产、电子电器与公共安全等领域^[1-2]。近年来，随着恐怖爆炸活动的日趋频繁，机器人在公共安全领域方面的作用逐渐受到重视，基于深度相机的排爆机器人系统^[3]可代替人类在危险环境中作业，保证工作人员的安全。因为这类机器人需要在危险环境中进行高准确率的排爆抓取工作，所以对机器人的可操作性及灵活性提出较高要求，同时这也是人工智能^[4]的一个重要研究方向。文献[5]提出基于激光测距^[6]的环境建模与爆炸物定位方法，利用智能搜索A*算法实现半自主抓取爆炸物。文献[7]通过分析排爆机器人的作业工况及测量要求，利用双目立体相机进行静态及动态场景的定位实验，实现排爆机器人的现场定位与测距要求。文献[8]开发出用于工业应用的基于视觉物体分选机器人机械手。文献[9]使用立体视觉摄像机获得机器人周围工作空间的RGB及深度数据，并将这些数据输入至一个深度学习的快速R⁃CNN中，以实现对50个不同种类中当前对象的识别与定位，该项工作证实了将现有深度学习算法与工业机器人相结合构建智能系统的可能性。

目前，排爆机器人抓取系统主要分为普通排爆机器人抓取系统与基于视觉的排爆机器人抓取系统。普通排爆机器人抓取系统通常利用单片机控制机身，通过人眼判断、人工示教等方式对爆炸目标进行抓取，存在误差大、效率低与不确定性高等缺点。然而，基于视觉的排爆机器人抓取系统通常将相机固定于某个位置，将固定相机视觉^[10-11]获得的三维位置信息转换至机械臂底座，虽然该操作可根据视觉信息使得机械手作出判断，但是目标物体或机械手在移动过程中，固定相机的视觉容易被机械臂挡住，且对于焦距、像素一定的相机，远距离拍摄时清晰度不能得到保证，存在一定的局限性。

为解决传统排爆机器人自主抓取系统存在固定视觉被挡与难以保证相机拍摄清晰度的问题，本文提出一种基于深度相机的排爆机器人自主抓取系统。通过将相机放置在排爆机器人的第四关节处，利用深度相机计算目标物体的三维坐标，当相机随关节运动而运动时，通过正逆运动学关系计算相机相对于机械臂底座的位置坐标与姿态。采用本文提出的动态坐标转换方法将三维坐标实时转换至机械臂底座，并与传统手眼标定视觉系统^[12]进行对比。

1 排爆机器人抓取系统

本文所提排爆机器人自主抓取系统如图 1所示，该系统主要由RGB-D相机、五自由度^[13]排爆机器人、目标物体以及PC机组成。PC机上的系统主控软件通过控制相机系统来采集目标物体的颜色及深度信息，并计算目标物体的x、y、z坐标。机械臂系统通过正运动学将当前各关节角度及放置相机处的姿态反馈给系统主控软件，运用坐标系之间的复合变换算法将三维位置信息实时转换至机器人全局坐标系，并采用逆运动学实现指定位置的抓取^[14-15]。

1.1 机械臂系统

如图 2所示，本文机械臂系统由五自由度机械臂和车体构成，机械臂的5个关节由PC机通过蓝牙进行控制，车体由遥控器控制，通过末端执行器手爪来抓取目标物体。在初始状态下，机械臂的各关节角度均为0°。

1.2 RGB-D深度相机

RGB-D深度相机选用Intel^® RealSense^TM D435^[16-17]，该相机具有小巧的外形与较高的图像分辨率和采样帧率，适用于近距离深度图像的采集，D435相机通过USB接口与系统主控软件相连接，其内部结构如图 3所示。

在图 3中，Vision Processor D4是D400系列视觉处理模块，用于控制红外相机Left Imager、Right Imager和RGB颜色传感器Color Sensor，得到RGB和立体红外图像。IR Projector是红外激光发射器，用于计算非结构化表面的深度。

2 自主抓取系统运动算法 2.1 目标物体的三维坐标获取

利用深度相机得到RGB-D深度图像，图像中每一点的二维像素坐标(pixel[0]，pixel[1])可输出为该点的深度，即z坐标，从而获得该点到相机中心点的距离S。在深度相机拍摄图像中，每一点处的深度与距离如图 4所示。其中，z₁=z₂，z₃ > z₄ > z₅。

本文以相机中心点为原点，深度为z坐标，以相机拍摄方向为视角，横坐标为y轴，向右为正方向，纵坐标为x轴，向下为正方向。为准确得到目标物体上某点的三维坐标（x，y，z）可采取如下算法：该点的投影点到相机中心点的距离$\sqrt {{S^2} - {z^2}} $即为该点到该点所在平面的距离，根据像素点与坐标系比例关系$\frac{{{\rm{pixel}}\left[ 0 \right]}}{x} = \frac{{{\rm{pixel}}\left[ 1 \right]}}{y}$可得x、y坐标，进而得到目标点的三维坐标（x，y，z）。

2.2 机械臂的正逆运动学

机械臂的正运动学^[18]通过在已知当前各连杆长度和各关节类型及参数情况下，确定末端执行器在固定坐标系中的位置和姿态。反之，给定机械臂末端执行器的位置和姿态求解各关节的变量，则为机械臂的逆运动学。对于一个n轴的机械臂，第n个关节的角度为q_n，第n-1轴所在坐标系到第n轴所在坐标系的齐次变换矩阵为^n-1T_n，则有运动方程：

$ \begin{array}{l} \mathit{\boldsymbol{P}} = f\left( {q, n} \right){ = ^0}{\mathit{\boldsymbol{T}}_1}{\left( {{q_1}} \right)^1}{\mathit{\boldsymbol{T}}_2}\left( {{q_2}} \right){ \cdots ^{n - 1}}{\mathit{\boldsymbol{T}}_n}\left( {{q_n}} \right) = \\ \;\;\;\;\;\;\left[ \begin{array}{l} n\;\;\;o\;\;\;a\;\;\;\;\mathit{\boldsymbol{p}}\\ 0\;\;\;0\;\;\;0\;\;\;\;\;1 \end{array} \right] = \left[ \begin{array}{l} ^0{\mathit{\boldsymbol{R}}_n}\;\;\;\;\;\;{\;^0}{\mathit{\boldsymbol{P}}_n}\\ \;\;0\;\;\;\;\;\;\;\;\;\;\;1 \end{array} \right] \end{array} $

(1)

2.3 运动坐标系与全局坐标系的映射关系

当得到目标物体在运动坐标系中的位置信息后，为实现准确抓取，需将运动坐标系转换到基于机械臂底座的全局坐标系。本文用x、y和z轴表示固定参考坐标系（全局坐标系）F_x，y，z，用n、o和a轴表示相对于参考坐标系的另一个运动坐标系F_n，o，a^[19]。假设将运动坐标系放置于参考坐标系中，则运动坐标系的姿态可用相对参考坐标系的3个方向余弦来表示，即为$\mathit{\boldsymbol{F}} = \left[ \begin{array}{l} {n_x}\;\;\;\;{o_x}\;\;\;\;\;{a_x}\\ {n_y}\;\;\;\;{o_y}\;\;\;\;\;{a_y}\\ {n_z}\;\;\;\;{o_z}\;\;\;\;\;{a_z} \end{array} \right]$，给定运动坐标系原点在参考坐标系中的具体位置$\mathit{\boldsymbol{L}} = \left[ \begin{array}{l} {l_x}\\ {l_y}\\ {l_z} \end{array} \right]$，则运动坐标系在参考坐标系中的位置可表示为$\mathit{\boldsymbol{P}} = \left[ \begin{array}{l} {n_x}\;\;\;\;\;{o_x}\;\;\;\;\;{a_x}\;\;\;\;\;\;{l_x}\\ {n_y}\;\;\;\;\;{o_y}\;\;\;\;\;{a_y}\;\;\;\;\;\;{l_y}\\ {n_z}\;\;\;\;\;{o_z}\;\;\;\;\;{a_z}\;\;\;\;\;\;{l_z}\\ 0\;\;\;\;\;\;\;0\;\;\;\;\;\;0\;\;\;\;\;\;\;1 \end{array} \right]$。

当得到运动坐标系在参考坐标系中的位置和姿态后，再研究运动中的运动坐标系和参考坐标系之间的关系。

如图 5所示，假设初始状态下运动坐标系与参考坐标系完全重合，当点m位于运动坐标系中时，在运动坐标系中的坐标为（m_n，m_o，m_a），参考坐标系的坐标为（m_x，m_y，m_z），则此时m_x=m_n、m_y=m_o、m_z=m_a。运动坐标系绕参考坐标系的x轴旋转θ角度，由旋转示意图可得：

$ \left[ \begin{array}{l} {m_x}\\ {m_y}\\ {m_z} \end{array} \right] = \left[ \begin{array}{l} 1\;\;\;\;\;\;0\;\;\;\;\;\;\;0\\ 0\;\;\;\;C\theta \;\;\; - S\theta \\ 0\;\;\;\;S\theta \;\;\;\;\;\;C\theta \end{array} \right]\left[ \begin{array}{l} {m_n}\\ {m_o}\\ {m_a} \end{array} \right] $

(2)

$ {\mathit{\boldsymbol{m}}_{x, y, z}} = {\rm{Rev}}\left( {x, \theta } \right) \times {\mathit{\boldsymbol{m}}_{n, o, a}} $

(3)

同理，运动坐标系绕参考坐标系的y轴和z轴旋转的矩阵形式为：

$ {\rm{Rev}}\left( {y, \theta } \right) = \left[ \begin{array}{l} C\theta \;\;\;\;\;\;\;\;0\;\;\;\;\;\;\;\;S\theta \\ 0\;\;\;\;\;\;\;\;\;\;\;1\;\;\;\;\;\;\;\;\;0\\ - S\theta \;\;\;\;\;\;0\;\;\;\;\;\;\;\;C\theta \end{array} \right] $

(4)

$ {\rm{Rev}}\left( {z, \theta } \right) = \left[ \begin{array}{l} C\theta \;\;\;\; - S\theta \;\;\;\;\;\;\;\;0\\ S\theta \;\;\;\;\;\;\;C\theta \;\;\;\;\;\;\;\;0\\ \;0\;\;\;\;\;\;\;\;\;\;0\;\;\;\;\;\;\;\;\;1 \end{array} \right] $

(5)

坐标系间的复合变换是由当前运动坐标系或固定参考坐标系的一系列绕轴旋转变换和沿轴平移变换所组成的，任何变换都可以分解为按一定顺序的一组旋转变换和平移变换^[20]。假设点m_n，o.a固定在运动坐标系上，开始时运动坐标系的原点与参考坐标系的原点重合，运动坐标系中的点m会随着运动坐标系相对于参考坐标系旋转或者平移而相应改变。其中，存在有转换关系T，满足M′_{x, y, z}=T × M_{x, y, z}。假设旋转坐标系按一定顺序先绕x轴旋转α度，再接着分别沿x、y、z轴平移[s₁, s₂, s₃]，然后绕y轴旋转β度，最后绕o轴旋转θ度，则变换关系为：

$ \mathit{\boldsymbol{T}} = {\rm{Rev}}\left( {y, \beta } \right) \times {\rm{Mov}}\left( {{s_1}, {s_2}, {s_3}} \right) \times {\rm{Rev}}\left( {x, \alpha } \right) \times {\rm{Rev}}\left( {o, \theta } \right) $

(6)

坐标系在空间中的两种运动形式旋转（Rev）和平移（Mov）运算方法由上述推导过程已知。

假设机械臂手爪要到达M点抓取物体，则必须向M处移动。机械臂底座相对于世界坐标系U的位置用坐标系R表示，机械臂某个关节A处用坐标系A表示，机械臂相机所在处用坐标系H表示，机械手用坐标系E表示，则坐标系之间的转换关系可用式（7）表示：

$ ^U{\mathit{\boldsymbol{T}}_E}{ = ^U}{\mathit{\boldsymbol{T}}_R}^R{\mathit{\boldsymbol{T}}_A}^A{\mathit{\boldsymbol{T}}_H}^H{\mathit{\boldsymbol{T}}_E}{ = ^U}{\mathit{\boldsymbol{T}}_P}^P{\mathit{\boldsymbol{T}}_E} $

(7)

因为相机放置在机械臂的第四关节上，相对于手爪的位置也是确定的，所以变换^HT_E是已知的，则有变换式：

$ \begin{array}{l} {\left( {^U{\mathit{\boldsymbol{T}}_R}} \right)^{ - 1}}\left( {^U{\mathit{\boldsymbol{T}}_R}^R{\mathit{\boldsymbol{T}}_H}^H{\mathit{\boldsymbol{T}}_E}} \right){\left( {^H{\mathit{\boldsymbol{T}}_E}} \right)^{ - 1}} = \\ {\left( {^U{\mathit{\boldsymbol{T}}_R}} \right)^{ - 1}}\left( {^U{\mathit{\boldsymbol{T}}_P}^P{\mathit{\boldsymbol{T}}_E}} \right){\left( {^H{\mathit{\boldsymbol{T}}_E}} \right)^{ - 1}} \end{array} $

(8)

$ ^R{\mathit{\boldsymbol{T}}_H} = {\left( {^U{\mathit{\boldsymbol{T}}_R}} \right)^{ - 1}}^U{\mathit{\boldsymbol{T}}_P}^P{\mathit{\boldsymbol{T}}_E}{\left( {^H{\mathit{\boldsymbol{T}}_E}} \right)^{ - 1}} $

(9)

本文采用以下算法计算矩阵的逆：

$ {\rm{Det}}\left[ {{\rm{Rev}}\left( {x, \theta } \right)} \right] = 1\left( {{C^2}\theta + {S^2}\theta } \right) + 0 = 1 $

(10)

$ {\rm{Rev}}{\left( {x, \theta } \right)^{\rm{T}}} = \left[ \begin{array}{l} 1\;\;\;\;\;0\;\;\;\;\;0\\ 0\;\;\;\;C\theta \;\;S\theta \\ 0\;\; - S\theta \;C\theta \end{array} \right] $

(11)

$ {\rm{Adj}}\left[ {{\rm{Rev}}\left( {x, \theta } \right)} \right] = {\rm{Rev}}\left( {x, \theta } \right)_{{\rm{minor}}}^{\rm{T}} = {\mathop{\rm Rev}\nolimits} {\left( {x, \theta } \right)^{\rm{T}}} $

(12)

$ {\mathop{\rm Rev}\nolimits} \left( {x, \theta } \right){\rm{Adj}}\left[ {{\rm{Rev}}\left( {x, \theta } \right)} \right] = {\rm{Det}}\left[ {{\rm{Rev}}\left( {x, \theta } \right)} \right]\mathit{\boldsymbol{E}} $

(13)

由式（10）~式（13）可得：

$ {\rm{Rev}}{\left( {x, \theta } \right)^{ - 1}} = {\mathop{\rm Rev}\nolimits} {\left( {x, \theta } \right)^{\rm{T}}} $

(14)

2.4 运动坐标系与全局坐标系的变换

基于以上讨论，本文所提排爆机器人自主抓取系统可实现相机坐标系H、机器人全局坐标系（机械臂底座坐标系）R和末端执行器手爪的工具坐标系E三者的动态映射关系，即^RT_E=^RT_H^HT_E，具体如图 6所示。

假设在初始状态时，相机坐标系与机械臂底座坐标系的各轴方向一致。其中，运动坐标系为相机坐标系n、o、a，参考坐标系为机械臂底座坐标系x、y、z。根据机械臂当前角度，通过正运动学可求得相机中心点相对于底座的坐标p_x、p_y、p_z和姿态q_y、q_z，由于第五关节角度的变动不影响q_x，因此q_x恒等于0。

假设目标物体上的某一点在相机坐标系n、o、a中的坐标为P_n，o，a，在机械臂底座坐标系x、y、z中的坐标为P_x，y，z，则从与机械臂底座坐标系x、y、z重合的位置开始，相机坐标系n、o、a经过一系列绕轴旋转和沿轴平移变换可得到当前相机坐标系n、o、a，且变换过程如下：

步骤1  n、o、a先绕z轴旋转q_y，可得Rev(z, q_y)。

步骤2  n、o、a再绕o轴旋转q_z，可得Rev(o, q_z)。

步骤3  Rev(o, q_z)再平移到(p_x,p_y,p_z)点，可得Mov(q_x,q_y,q_z)，则得到转换关系为T=Rev(z, q_y) Rev(o, q_z)Mov(q_x, q_y, q_z)，且该物体在机械臂底座坐标系x、y、z的坐标P_x，y，z为P_{x, y, z}=T×P_{n, o, a}。

在得到底座与相机的转换关系后，分析机械臂抓取物体的运动过程。已知相机所在关节用坐标系A描述，目标物体用坐标系o、b、j描述，则有：^RT_A×^AT_E×^ET_{o, b, j}=^RT_A×_AT_H×^HT_{o, b, j}，化简得到^ET_{o, b, j}=^MT_E^-1MT_H^HT_{o, b, j}，由于^MT_E，^MT_H，已知，^HT_{o, b, j}为目标物体在相机中的坐标且已求得，因此可得手爪坐标系和物体坐标系的关系^ET_{o, b, j}。设置运动轨迹从E到o，b，j沿直线运动，手爪中心点可到达目标物体处，此时配合手爪由张开状态到闭合状态并抓取目标物体。

3 实验结果及分析 3.1 目标物体定位实验

本文目标物体选择不透明的柱状水杯，将水杯放置在实验场景中，用Realsense D435相机获取目标物体的深度图，分别选取分辨率1 080×720和640×480进行实验。通过图 7对比看出，在机械臂可抓取范围内抓取时，选取分辨率640×480的图像不容易失真，且定位更准确。

本文调用深度相机采集颜色图，将深度图与颜色图对齐，选取图中柱状水杯上的某一点，得到该点在相机坐标系中的三维坐标（-18，37，58），与实际坐标相比误差小于1 cm，这说明目标物体的定位更加准确。

3.2 坐标转换实验

在机械臂各关节固定角度下，相机在机械臂底座坐标系中的坐标和姿态如表 1所示。其中，（p_x，p_y，p_z）为相机在机械臂底座坐标系中的三维坐标，q₀、q₁、q₂、q₃、q₄为机械臂各关节当前角度，q_x为第五关节旋转角度，因为相机放置于第四关节，所以第五关节的旋转与相机姿态无关，即q_x恒为0。q_y为第一关节旋转角度，q_z为第二至第四关节旋转角度之和，即q_y=q₀，q_z=q₁+q₂+q₃。

在得到水杯在相机坐标系中的坐标和相机在机械臂底座坐标系中的坐标与姿态后，利用本文提出的坐标系转换方法进行实验。实验依次记录10个不同位置的数据并进行误差分析，结果如表 2所示。物体坐标、相机坐标和转换后坐标的空间关系如图 8所示。

实验所得转换到底座的坐标与实际坐标的关系如图 9所示。在机械臂可抓取的范围内，三维方向误差绝对值的和与目标物体相对底座的距离呈正相关，距离越远说明误差越大，但误差能控制在2 cm以内。误差的主要来源有以下3个方面：相机放置位置的误差；机械臂自身运动学和机械尺寸的误差；相机本身的测量误差。

3.3 自主抓取实验

在完成目标物体的定位和坐标系的转换后，将手爪中心点设置为抓取点，并设定手爪和水杯间的运动轨迹，在不同距离下进行多次抓取实验，实验结果如表 3所示。

由表 3可知，在误差2 cm范围内，手爪中心点到达抓取点的成功率几乎为100%，而抓取成功率受机械尺寸和结构的影响，当水杯离底座较近或较远时抓取效果一般，距离在100 cm~150 cm时可以达到最大抓取成功率85%，验证了本文所提系统的可行性和可靠性。

为进一步验证本文系统的有效性，在相同的条件下，采用传统固定视觉方法进行抓取实验，该方法将Realsense D435相机固定，使得相机坐标系与机械臂底座坐标系的相对关系保持不变，结果如表 4所示。因为固定视觉的方法可能使相机被机械臂遮挡，即相机、机械臂某部位和水杯三点一线，所以对抓取结果的影响较大。

实验对随动视觉方法与传统的固定视觉方法进行比较，结果如图 10所示。从图 10可以看出，随动视觉方法的手爪到达抓取点成功率和抓取成功率都有所提高，且在距离130 cm处达到最大抓取成功率85%。

4 结束语

本文提出一种基于随动视觉的排爆机器人自主抓取系统，采用正逆运动学关系计算深度相机相对于机械臂底座的位置坐标和姿态，运用动态坐标转换方法将深度相机获取的三维坐标实时转换至机械臂底座坐标系，并对目标物体进行抓取实验。实验结果表明，本文所提坐标转换方法的抓取成功率较高，且在误差2 cm内，手爪中心点到达抓取点的成功率接近100%，满足排爆机器人抓取爆炸物的要求。为进一步验证本文系统的有效性，在相同条件下，采用传统的固定视觉方法进行抓取实验，在误差可接受范围内，本文系统与固定视觉的机械臂抓取系统相比，不仅解决了固定相机视觉被机械臂遮挡以及拍摄清晰度不够的问题，而且可有效提高抓取成功率。后续将采用轮廓提取方法对目标物体抓取方式进行改进，以进一步提高抓取成功率。