基于视觉和动态运动基元的机械臂抓取轨迹规划

叶子涵 陈俊洪 杨振国 刘文印

（广东工业大学计算机学院，广州，510006）

0 引言

近年来，在中国制造2025的政策背景下，机器人技术已成为智能制造的重要标志之一，并且随着人工智能的快速发展以及机器人的灵巧性的提高，机器人在医疗、服务等领域的应用也愈加广泛，这让人们对机器人智能化程度有了更高的需求[1-2]。

示教学习是机器人快速学习技能的一种重要方式，也被称作模仿学习，机器人通过模仿人类动作生成相应的运动轨迹，并对轨迹进行泛化学习，进而得到相应的运动技能[3-4]。

根据示教者是否与机器人直接发生物理上的接触可分为动觉示教和远程示教。动觉示教是示教者人工操纵机器人完成相应工作动作并记录运动轨迹，再利用学习算法对运动轨迹进行学习，最终将学习到的轨迹传输给机器人执行[5]；远程示教是通过视觉、穿戴式传感器或其他远程控制工具来控制机器人运动[6]。以示教学习为代表的轨迹规划方法，着重通过学习和模仿人类演示动作，使得机器人可以与操作者协同作业。

作为机器人学的重要分支，示教学习吸引了大量的科学家对此进行研究。Calinon等人[7-8]将动力学系统参数估计问题表示为在不同坐标系中具有投影的高斯混合回归（GMR）问题，使用高斯混合模型（GMM）对运动轨迹进行建模，能有效处理大规模数据，但是存在生成轨迹不一定连续等缺点。为了有效地解决轨迹不连续的问题， Schaal等人[9]于2002年提出了基于动态系统示教学习方法的动态运动基元(Dynamical Movement Primitives, DMP)算法。该算法源于二阶弹簧阻尼模型，它可以用一组线性微分方程表示简单的线性动态系统，通过向该系统添加一个可自主学习的非线性项，将其转换为具有特定吸引子的弱非线性系统。由于弹簧阻尼模型的特性是朝着目标位置收敛，因此由非线性项调制的动态运动基元最终也会朝着特定目标位置收敛，并在收敛到目标位置的过程中利用策略参数来生成任意形状的运动轨迹，其中，策略参数可以通过局部加权回归或强化学习等方法来学习，模型通过学习示教轨迹的运动信息使新生成的轨迹具有原轨迹的运动趋势。2013年，Ijspeert等人[10]对此算法进行了改进，他们重新构造了非线性项的方程，用起始位置、目标位置和时间三者共同调制非线性函数，这意味着当系统达到目标位置时，非线性项可以有效地消失，从而保证系统最后趋于稳定。他们还使用相位变量对时间进行替换，避免了明确的时间依赖性。

近些年机器视觉在机器人领域的应用越来越广泛，其中基于深度学习的目标检测是近年来发展最为迅速的科研热点之一，它有效地解决了机器人执行操作中所需要的物体识别以及分类的问题[11-12]。

2014年Ross Girshick等人[13]提出了R-CNN（Regionbased Convolutional Neural Network，区域卷积神经网络），使用基于区域的卷积神经网络来进行目标检测，将PASCAL VOC数据集上的检测率从35.1%提升到53.7%，但训练时间以及测试时间过长。为了解决该问题，Ross Girshick团队在2015年基于R-CNN提出了新的算法Fast R-CNN，它避免了对每一个候选区域都进行特征提取的做法，取而代之的是先对一张图像进行特征提取，再根据候选区域在相应的特征上进行划分，得到对应区域的特征，从而大幅提升了目标检测的速度。此外，为了节省找出候选框所需要的时间，Ren等人进一步改进了Fast R-CNN，提出了利用神经网络提取物体边缘特征作为候选框，大大节省了寻找候选框所需要的时间，该算法也被命名为Faster R-CNN[14]。为了有效地识别出物体的轮廓信息，He等人在Faster R-CNN的基础上提出了Mask R-CNN[15]，通过在Faster R-CNN结构上添加mask预测分支，完成对物体轮廓的预测，并且改良了ROI Pooling，在实例分割上拥有不错的效果。

在上述基础上，本文提出一种基于视觉和动态运动基元的机械臂抓取轨迹规划模型，该模型用来自主识别待抓取物体、自主学习示教轨迹并生成新的抓取轨迹，最后在Baxter七自由度机械臂上进行实验，实验结果证明了该方法能有效地识别出待抓取物体，并根据原有的示教轨迹泛化出新的抓取轨迹。

动态运动基元分为非周期运动和周期运动两种场景，本文只讨论非周期运动的情况。

1 动态运动基元算法

1.1 动态运动基元算法原理

动态运动基元（DMP）算法的模型最开始是来自于二阶线性弹簧系统，该系统的特性是以特定的运动朝着目标位置收敛。DMP的基本思想是使用非线性项调制具有良好稳定特性的动力学系统，由非线性函数控制系统的运动过程，以实现期望的吸引子行为。DMP将弹簧阻尼模型抽象为二阶微分方程，并加入强迫项，如公式（1）所示:

其中， 表示系统的运动状态，在单自由度的运动下即为位移，、对应的一阶导数和二阶导数，分别表示速度和加速度。 表示系统期望的运动状态，即目标点或者称为吸引子，在轨迹规划过程中为笛卡尔空间中点的位置或是机械臂的关节角度。是时间常数，和为正常数，通过设置适当的模型参数和，如，系统可以被临界阻尼，使得单调收敛于 ，最终保证系统趋于稳定公式。

公式（1）称为转换系统。

公式（2）称为规范系统，用来表示对时间的抽象。可以被看做是一个相位变量，从一个初始状态如开始，单调收敛为0，当接近0就意味着目标基本上已经实现。为用来调节规范系统衰减速度的时间缩放因子，为常系数。

1.2 多自由度动态运动基元耦合

为了将动态运动基元应用到多自由度的轨迹学习，本研究的模型需要对系统的多个自由度进行耦合。如图1所示，本文所采用的方法是在所有的自由度之间共享一个规范系统，并且对每个自由度保留一组独立的转换系统。规范系统提供各自由度之间的时间耦合，转换系统实现各自由度的期望吸引子，即目标状态。本文所使用的的机械臂在三维笛卡尔空间中进行轨迹规划，因此将动态运动基元扩展到3自由度。

图1 3自由度系统动态运动基元

图2为三维空间中动态运动基元对示教轨迹的模仿和泛化的示例。其中，蓝色实线表示原始轨迹，黑色虚线表示模型根据多个不同的起始点和目标点生成的新的轨迹。

图2 三维动态运动基元模仿和泛化示教轨迹示例

2 Mask R-CNN算法

本文所采用的的目标检测算法是Mask R-CNN，这是一个集成了物体检测和实例分割两大功能的高性能神经网络。Mask R-CNN在目标检测算法Faster R-CNN上进行了改进，在原有的分类识别和回归定位的基础上增加了一个mask预测分支，即预测物体的掩膜，并使用RoI Align层代替RoI Pooling层来获取固定大小的特征图。Mask R-CNN的网络结构包含两个部分，第一个部分是使用RPN（Region Proposal Networks，区域推荐网络）查找候选区域，另一部分则用来对RPN生成的候选区域中的每一个RoI（Region of Interest，兴趣区域）进行分类、框回归和mask预测。整体网络框架如图3所示。

图3 Mask R-CNN网络框架图

在图3中，网络的第一部分（conv）一共包含13个卷积层、13个ReLU激励层和4个池化层，它们主要用来提取输入图像的特征图，然后将得到的特征图送入RPN网络进行候选框的提取。

RPN在输入的特征图上进行滑动，并将输入的特征图映射为一个512维的特征向量，在这个特征向量后面连接两个并联的全连接层，这两个全连接层分别用来进行边界框回归和物体分类，最后生成若干个候选区域；RPN的主要功能是实现目标定位，输出以[x,y,w,h]表示的多个矩形目标候选框。

由于Faster R-CNN使用的RoI Pooling对特征图进行了两次量化操作，丢失了图像像素的精度导致实例分割的效果大幅度降低，所以Mask R-CNN采用RoI Align代替RoI Pooling层来生成固定大小为7×7的特征图，并使用双线性插值来精确地找出每个候选区域对应的特征图。

最后，将固定大小的特征图送入全连接层，利用Softmax进行分类识别，利用L1 Loss回归进行边框的定位，并增加一个mask分支进行实例分割。

Mask R-CNN进行目标检测的效果如图4所示。

图4 目标检测效果

3 抓取任务实验设计

实验平台为Ubuntu16.04操作系统和ROS（Robot Operating System，机器人操作系统）的Kinetic版本，使用的机械臂为Baxter双臂协同机器人自带机械臂，并使用Kinect v2深度相机进行视觉感知。

3.1 Kinect深度相机标定

为了获取环境的图像信息和三维信息，本文使用Kinect 2.0版本的深度相机进行视觉感知，而在感知之前，需要事先对Kinect进行内部参数和外部参数的标定。

图5 Kinect参数标定

由于Kinect的镜头是一种透镜，因此存在径向和切向畸变，需要内部参数标定得到矫正后的相机参数，减小或消除捕获图像的畸变程度。本文参考张正友[16]标定算法，采用5×7×0.03的棋盘格进行内部参数标定，如图5（a）所示，标定程序自动识别黑白格邻接点，即角点的位置，并与已知的标定板角点位置配对，最后分别完成对Kinect相机彩色和红外镜头的参数标定。

为了实现摄像机坐标系与机器人坐标系之间的坐标转换，本文将Kinect坐标系下采集到的物体的三维坐标信息映射到机器人坐标系下的场景，具体的外部参数标定过程如图5（b）所示。

对Kinect采集的深度图进行采样，分别得到Kinect和Baxter基坐标系下的点的坐标，最后计算相应的转换矩阵。

3.2 演示轨迹记录

为了机器人从演示中学习动作，示教人员首先需要记录演示的动作轨迹并获取非线性系统的基本参数，图6展示了人工示教过程，人为抓取机械臂的末端执行器并拖动产生轨迹，轨迹由时间和笛卡尔空间中的3维位置组成。

图6 人工示教过程

3.3 示教轨迹特征提取

3.4 示教轨迹重现和泛化

在获得新的起始点或目标点后，通过代入动态运动基元算法可以得到泛化后的新轨迹，由于新的环境下强迫函数权重与示教轨迹权重相同，所以新生成的运动轨迹在形状上与示教轨迹相似，从而达到了轨迹学习的目的。本文将视觉和动态运动基元相结合，使用视觉感知捕获待抓取物体位置，即新的目标点，3.4.1和3.4.2节分别展示了通过视觉获取目标点并泛化轨迹的方法。

3.4.1 获取目标位置

通过鼠标点击Kinect捕捉到的RGB彩色图像坐标，根据Kinect内置算法将2维坐标映射到Kinect坐标系下笛卡尔空间3维点云坐标，再通过外部参数标定得到的矩阵转换为Baxter基坐标系下的笛卡尔坐标，最后将目标点发布到ROS中的话题（topic）中；DMP通过订阅该topic获取该目标点，并作为示教轨迹新的目标点，进而生成新的轨迹。图7（a）显示了鼠标点击获取目标点，图7（b）显示了为该目标点生成的新的轨迹，其中蓝色实线为示教轨迹，蓝色虚线为新生成的轨迹。

图7 鼠标点击获取目标点抓取示例

表1给出了在规范系统的不同时刻，原始示教轨迹和学习后的轨迹在笛卡尔空间中的三维位置。在起始点和目标点不变的情况下，新生成的轨迹具有与原始轨迹相同的运动趋势，达到了轨迹学习的目的。

表1 轨迹学习过程

3.4.2 目标位置实时检测

本文的重点在于将深度学习和动态运动基元相结合，基于视觉实现机械臂抓取轨迹规划。因此，笔者对待检测的物体进行数据标注，建立了自己的数据集；将数据集送入Mask R-CNN网络进行训练，得到训练好的网络模型。

通过Kinect v2深度相机的彩色摄像头读取彩色图像并作为Mask R-CNN的输入图像，可以得到待抓取物体以及目标位置的定位（方框），并用一个四元组 (x,y,w,h) 来表征方框的位置，这4个参数分别表示方框左上角的2维坐标以及方框的宽度和高度。图8展示了对待抓取物体的目标检测效果，其中类别为bottle的物体为待抓取物体。

图8 目标检测效果

通过目标检测得到的待抓取物体在图像中的位置以及三维点云信息，笔者可以获得其在Kinect坐标系下的3维坐标；和3.4.1节类似，将该坐标点转换为Baxter基坐标系下的坐标，并发布到ROS中，DMP订阅该目标点并生成新的抓取轨迹。

更改起始点（机械臂末端执行器的位置）和目标点（瓶子位置），泛化出的新的轨迹如图9所示，其中，红色圆圈表示起始点，蓝色圆圈表示目标点。机械臂抓取过程如图10所示，对于不同的目标点和起始点，新生成的轨迹仍具有与原始示教轨迹相同的运动趋势，证明模型的泛化效果有效。

图9 目标检测获取的目标点泛化轨迹

图10 机械臂抓取过程

3.5 性能分析

与本文提出的两阶段的轨迹规划方法相比，传统的机械臂轨迹规划方法得到的目标位置信息和实际位置存在较大偏差，并且由于要进行逆运动学求解等运算，存在耗时较久的问题。本文将传统轨迹规划方法与上述两阶段模型做对比实验，实验数据如表2所示，数据显示，动态运动基元和深度视觉相结合的方法生成的轨迹没有误差且耗时较短。

表2 轨迹规划方案对比分析

此外，轨迹规划分为轨迹学习和泛化两个阶段，大多数的轨迹规划方法在第二阶段并未与视觉相结合，或者仅仅使用颜色识别或贴标记（marker）识别等简单方式，因此泛化性能大大降低，本文所提出的两阶段模型生成了一套完整的拥有自主识别能力的轨迹规划系统，较好地提升了轨迹的泛化性能和自主学习能力。

4 总结

本文提出了一种基于深度视觉和动态运动基元的机械臂抓取轨迹规划方法，通过对示教轨迹的学习，机械臂可以根据不同的环境自主调整运动策略，实现对原始轨迹的重现和泛化；同时，本文集成的基于深度学习的目标检测技术，可以有效地对外界环境进行视觉感知，保障实现机械臂对物体抓取的轨迹规划，更好地适应服务机器人的多变环境。