基于YOLOv3和EPnP算法的多药盒姿态估计

2021-03-04王国顺赵扬扬

计算机测量与控制 2021年2期

仇翔，王国顺，赵扬扬，滕游，俞立

(浙江工业大学信息工程学院，杭州 310023)

0 引言

机械臂抓取和分拣在当前工业生产和生活中越来越普遍，机械臂代替人工完成复杂和重复的工作不但可以提高生产效率，也可以让人们的生活更加便利，使人们可以更专注于脑力劳动。现今在医院和药房中基本还是通过人工取药，引入机器人进行药房药品的自动抓取和药品的自动分拣能够提高医院和医药仓库的操作效率，具有重要的现实意义和商业价值。在机械臂的药盒抓取和分拣操作中，实现药盒的精确定位和姿态估计是机械臂准确抓取药盒的前提，因此需要对药盒进行实时定位和6D姿态估计。本文针对此应用场景进行药盒定位和姿态估计的相关技术研究。

药房或药品仓库是一个光照和背景环境相对复杂的场所，比如交错的背景和上层货架对于光线的影响，以及药盒紧密排列时容易被误识别为一个整体等，在进行药盒定位和姿态估计时很容易受到这些不利因素的影响。因此，传统的图像边缘检测或形状分割等方法难以胜任此类场景下的任务需求。自2012年Krizhevsky提出深度卷积网络AlexNet[1]以来，基于深度学习的图像处理技术在目标检测[2]、文字识别[3]、语义分割[4]等领域得到了迅猛的发展。深度学习算法可以很大程度地克服图像处理过程中由光照和复杂背景带来的不利影响，可以利用高维特征对目标进行检测，可以大幅提高目标定位效果。自从Redmon等人提出YOLO[5](you only look once)深度学习网络以来，因其在图像识别速度上的突出优势获得了广泛关注，并在后续改进版本YOLOv2[6]、YOLOv3[7]中改进了对小目标物体的识别精度，使其在识别速度和识别准确度上都有着很好的效果。最近几年，已有研究将深度学习算法直接用于姿态估计[8-9]，其中输入数据仅仅是RGB图像，这些方法可以实现较好的目标检测，但这类方法难以实现实时的姿态估计，而且通过深度学习算法直接实现姿态估计需要复杂而庞大的数据集，这些都是难以构建的。

为实现在复杂场景下对多药盒的精确定位和姿态估计，本文提出一种基于YOLOv3深度学习算法和EPnP[10]算法相结合的多药盒姿态估计算法。EPnP算法具有很强的抗干扰性能，只需三对共面(非共面需要四对)3D-2D匹配点就可以精确估计目标姿态。本文充分利用YOLOv3算法和EPnP算法兼具准确性和快速性的优点，将多目标药盒的定位和姿态估计分步进行。首先，利用YOLOv3对于多目标物体的定位框，依次分割出单个药盒，然后对每个药盒进行ORB(Oriented FAST and rotated BRIEF)特征提取、特征匹配和EPnP姿态估计。本算法对于多目标、姿态不固定药盒具有很好的定位和姿态估计效果，满足机械臂抓取操作过程中对目标定位和姿态估计的实时性要求。最后，本文结合OptiTrack(动作捕捉系统)系统设计了本算法的姿态精度对比实验，实验结果表明，本文所提出的多药盒姿态估计算法能够满足实际应用的需求。

图2 YOLOv3算法网络结构图

1 系统结构及原理

针对药房或药品仓库中药盒分拣任务的要求，本算法主要分为多药盒定位和姿态估计两个部分：1)药盒定位部分采用YOLOv3算法，通过对药盒训练集标注的边框信息进行k-means聚类运算以减少模型训练量。采集药盒图片制作PASCAL VOC数据集并训练药盒识别模型；2)姿态估计部分通过药盒定位框信息分割出目标药盒并结合药盒的类别信息进行药盒模板选取，然后通过模板和目标药盒之间的ORB特征点进行特征匹配和EPnP药盒姿态求取。本算法系统流程如图1所示。硬件环境如表1所示，在此基础上搭建软件环境：Ubuntu 16.04、Python、Opencv、CUDA等。

图1 系统流程图

表1 工况试验设置及结果

2 药盒定位

本文采用YOLOv3算法对药盒进行定位，YOLOv3算法由YOLO算法发展改进而来，YOLO算法将目标的侯选区选取和识别两个阶段合二为一，只看一眼图片就能知道哪些对象以及它们的位置，在目标识别定位的速度上有很大的优势，可以达到45FPS到155FPS。YOLOv3算法针对YOLO算法的不足之处进行了改进， YOLOv3算法通过采用多尺度特征进行对象检测，在保持YOLO算法检测速度的前提下，大幅提高了对小对象目标的检测精度。YOLOv3算法采用9个先验框，在3种尺度特征图上分别设置了3个不同的先验框用于预测，对于416*416的输入图片，3种尺度分别为13*13特征图、26*26特征图和52*52特征图，它们分别用于预测大的物体、中等大小物体和小的物体。YOLOv3算法网络结构如图2所示。

YOLOv3算法采用了新的网络结构Darknet-53进行图像特征提取，借鉴了残差网络Residual network的做法，大量使用3*3与1*1卷积层依次连接的形式，并且添加了Shortcut连接，以防止出现过拟合现象。除此之外，YOLOv3的对象分类中采用Logistic函数取代了YOLO算法中采用的Softmax函数，使其能够支持对多标签对象的检测。

药盒具有丰富的特征信息，包括颜色、纹理、大小以及卷积神经网络提取的高维特征等，通过这些特征对药盒识别模型进行训练。本文针对5种不同种类药盒，制作PASCAL VOC数据集时一共采集了1500张不同类别、数目、背景和姿态的药盒图片，按照3∶1的比例划分为训练集和测试集，通过训练获取药盒识别模型。模型训练参数如表2所示。

表2 模型训练参数表

YOLOv3损失函数如式(1)所示。

(1)

YOLOv3损失函数第一部分为预测边界框的中心坐标损失，第二部分为预测边界框的宽高损失，第三、四部分为预测的置信度损失，第五部分为预测的类别损失。通过反向传播进行模型训练以调整权重参数，当总体IOU(预测框和真实框的交并比)值稳定为1，总体损失稳定到0.06以下时结束模型训练。模型训练Loss(损失)函数曲线和IOU函数曲线及模型识别定位结果分别如图3～5所示。

图3 Loss曲线

图4 IOU曲线

图5 药盒识别效果图

由Loss曲线可以看出，随着训练次数的增加，YOLOv3训练损失逐渐降低，迭代次数达到5000次之后总体损失趋于稳定并保持在0.06以下。

IOU曲线显示，随着迭代次数的增加总体IOU值逐渐上升，迭代次数在1 000～4 000期间IOU值达到1左右但不稳定，迭代次数大于4 000次之后，IOU值稳定为1，表明此时模型训练精度达到了预定要求。

通过YOLOv3算法可以确定每个药盒的类别以及定位框的坐标，根据这些定位框信息对药盒进行ROI(感兴趣区域)区域分割，可以将多药盒的处理转化为对单个药盒的处理，另外根据检测的类别信息选取模板库中对应的药盒模板并在后续操作中进行特征提取和特征匹配。药盒定位是实现姿态估计的前提，本文所采用的姿态估计方法将在下一章具体阐述。

3 姿态估计

根据药盒定位框进行ROI区域分割后可以对ROI区域和模板图片进行ORB特征点提取，特征点对可以通过文献[10]方法很容易求得它们之间的单应矩阵。单应矩阵描述了两幅图像共面特征点之间的转换关系，虽然单应矩阵本身包含有相机的姿态信息，但通过单应矩阵直接分解出的旋转矩阵和平移矩阵会有多个解[11]，需要通过其他约束条件去除不合理的解，并且获取的最优解也会存在一定的误差。基于单应矩阵的位姿求取方法主要包括单应矩阵的辨识，单应矩阵的分解，以及唯一性的确定3个步骤，相对复杂。为了简化药盒姿态求取以及避免单应性矩阵分解产生的多解问题，结合EPnP算法和药盒的固有尺寸信息进行药盒姿态求取。首先对前述步骤获取的单应矩阵进行矩阵透视变换获取模板在目标图像上的映射，从而获得包围目标药盒的外包框和4个角点的像素坐标。将获取的目标药盒4个角点像素坐标作为EPnP算法求取药盒姿态所需的2D点，根据药盒的精确尺寸在相机坐标系Z=0平面上构建药盒平面并作为EPnP求解所需的3D点。上述做法可以极大地减少EPnP算法中的特征点计算数量，加快了算法运行速度。此外，利用单应矩阵通过透视矩阵变换可以在药盒不全或者部分遮挡的情况下有效地获取药盒角点坐标，此方法在复杂场景下具有更强的鲁棒性。EPnP求解所需对应点对如图6所示。

图6 EPnP求解所需对应点对

3.1 特征提取和特征匹配

ORB特征点由关键点和描述子两部分组成，添加了尺度和旋转的描述，改进了FAST检测子不具有方向性的问题，并且通过结合二进制描述子BRIEF(binary robust independent elementary feature)，极大地加快了对于图像特征的提取速度。除此之外，ORB特征还可以指定最终要提取的特征点数量。

FAST关键点通过比较一个像素与其周围像素值的差值(亮度)来确定，因为只需要比较像素值的大小，所以检测速度极快，可满足姿态检测的实时性要求。FAST关键点检测方法如图7所示。其实现步骤如下：

步骤1：定位图像中的一个像素p及其亮度Ip。

步骤2：设定FAST关键点检测阈值T。

步骤3：获取以像素p为中心，半径为3的圆上的16个像素亮度值，如图6所示。

步骤4：若圆上连续N个像素亮度值大于Ip+T或者小于Ip-T则定义为FAST角点(N通常取12，即为FAST-12)。

步骤5：对每个像素进行以上操作。

图7 FAST特征点

ORB特征点通过构建图像金字塔描述尺度不变性，通过灰度质心法来实现旋转描述。灰度质心法定义过程如下：

1)在一个图像块B中定义图像块的矩为：

(2)

2)通过矩可以找到图像块的质心：

(3)

θ=artan(m01,m10)

(4)

BRIEF描述子通过0和1来编码特征点附近像素的量级关系，BRIEF采用随机选点的比较方法，速度较快，通过FAST和BRIEF的组合可以非常高效地描述ORB特征点。ORB特征点匹配如图8所示。

图8 ORB特征点匹配

特征点描述了模板图像与目标图像之间的转换关系，采用文献[10]中所提出的方法可以获取单应性矩阵，根据单应性矩阵的透视矩阵变换可以获取药盒平面外包框和4个角点的像素坐标，并且可以有效地克服部分遮挡情况。单应性矩阵透视矩阵变换后的药盒外包框和4个角点如图9所示。

图9 单应矩阵反馈的4个点

3.2 EPnP算法原理

EPnP算法姿态估计的核心思想是利用4个虚拟控制点线性加权地表示真实物体上的各个点在相机坐标系下的坐标。

(5)

药盒参考坐标系下3D点可用虚拟控制点加权表示，aij为均一重心坐标(homogeneous barycentric coordinates)。

同理虚拟控制点的坐标和3D参考点坐标之间的关系在相机坐标系下可表示为：

(6)

假设3D参考点在像素坐标系的投影为(ui,vi)，则：

(7)

其中：zi为参考点投影深度，A为相机内参矩阵，fu、fv、uc、vc为相机内部参数。由式(7)可得每个参考点和像素坐标点的对应关系为：

(8)

通过4个参考点和像素点对可得到包含8个方程的线性方程组，记为Mx=0，M为8×12的矩阵，x为12×1的向量，包含了4个虚拟控制点在相机坐标系下的非齐次坐标。由于4个参考点在相机坐标系下坐标已知，从而将求解三维到二维的PNP问题转换成经典的三维到三维的刚体运动问题，从而可求出目标药盒的旋转矩阵R和平移矩阵t。

4 实验结果与分析

为了验证本算法在药盒姿态求取中的姿态估计精度，通过结合OptiTrack系统进行了姿态精度对比实验。OptiTrack系统通过检测感光Marker点来进行姿态测量，具有亚毫米级的重建精度。本实验以OptiTrack系统的姿态测量值作为标准值，依次对偏航角、俯仰角、滚转角进行误差检测。通过对比OptiTrack系统的姿态测量值与本算法的测量值并进行分析。图10为OptiTrack系统，图11为测量板，图12为姿态估计效果图，图13～图15分别为偏航角误差曲线、俯仰角误差曲线和滚转角误差曲线图。

图10 OptiTrack系统图11 测量板

图12 姿态估计效果

图13 偏航角误差

图14 俯仰角误差

图15 滚转角误差

由实验数据可以看到采用本文提出的算法进行实时姿态估计时，偏航角的误差在0～90°内的平均误差在0.5°以内。俯仰角的平均误差在0～25°内小于0.5°，在25～45°内的平均误差小于0.8°，滚转角误差在0～20°以内平均误差小于0.5°，在25～45°平均误差小于0.6°。实验结果表明本算法具有足够的姿态估计精度并且可以满足机械臂药盒分拣任务需求。