协作机械臂末端执行器的快速定位算法

喻 靖，江文松，罗 哉，杨 力，周根明

(1.中国计量大学 计量测试工程学院，浙江 杭州 310018；2.浙江科力车辆控制系统有限公司，浙江 江山 324100)

1 引 言

近年来，协作机械臂在测量领域发展迅速，被广泛应用于室内地图构建[1,2]、工业材料尺寸测量等方面[3]。协作机械臂末端执行器工作时，当前的位姿决定其运动方式，进行快速准确的位姿估计是问题的关键[4]。

协作机械臂末端定位方法，包括非合作目标与合作目标两类[5,6]。非合作目标有帧差法、视觉里程计方法和视觉惯性融合里程计方法。帧差法计算量小，速度快，但是定位误差大[7]。基于视觉里程计方法对运动物体进行定位，随着运动时间的累加，误差会越来越大[8]。视觉惯性融合里程计方法引入惯性测量单元进行信息融合，解决了误差累加问题，但难以达到实时性要求[9,10]。合作目标有自定义圆形标签与方形标签。雷金周[11]采用提取靶标参考板圆孔特征中心坐标求解位姿信息，通常应用于手眼标定场合。方形标签AprilTag识别算法可以精确计算目标相对相机的位姿信息，但算法对高分辨率图片处理速度有限[12]。

综上所述，当前机械臂末端定位主要存在处理速度慢，难以达到实时性等问题。刘虹等[13]设计了一种基于交比不变性的编码靶标，以降低处理时间，前提是处在黑暗环境的条件下才能快速定位。贾配洋等[14]提出基于AprilTag识别的改进算法，对移动目标进行局部搜索，并结合Kalman滤波器实时估计目标下一时刻位置，提升了算法处理速度，仅限于目标加速度较小的情况。张坚等[15]通过添加圆环标记物，确定包围AprilTag标签的最小矩形区域，提高了实时性。Duan H Y等[16]通过AprilTag标签与视图分辨率的面积比实现自动数字变焦，简化识别目标提高了检测效率，但对于相机设备要求更高。Ren R N等[17]提出了动态下采样算法，最后基于AprilTag识别原理设计二次检测算法，提高了识别速度。

针对上述快速定位方法应用情况。基于密西根大学April实验室设计和改进AprilTag检测算法，被应用于高精度机器人辅助定位方面，具备快速和低误报率的优点[18]。AprilTag是一种简化二维码图，易获取，该标签在普通工业相机下易于识别与探测，可以计算标签相对于相机的位姿信息。

本文提出基于图像处理方法的AprilTag标签快速定位算法。标签张贴于机械臂末端，提出下采样算法，确定目标局部搜索范围。在局部定位图中，通过几何约束保留标签边缘像素点，大幅提升算法处理速度，成功解决了机械臂末端实时性定位位姿问题。

2 算法原理

2.1 传统AprilTag标签识别算法

AprilTag标签的具体识别步骤如下：

(1)灰度图像的二值化处理[19]。原始测量图像的分辨率为2 048×2 048像素，令目标图像周围区域的像素值为P。为了防止在极值差异较大的图块边界之间出现伪影，将图像划分为4×4像素阵列，计算各图块的灰度极值，令

Pmax-Pmin=δ

(1)

式中：Pmax为4×4像素阵列的灰度极大值；Pmin为 4×4 像素阵列的灰度极小值。

当δ≥5时，则判定像素对比度明显，利用像素阈值p′=(Pmax-Pmin)/2来分配该区域的黑白效果；当δ<5时，则判定该区域像素对比度不足，并删除该区域以节省计算时间。图1(a)为灰度图，二值化后为图1(b)自适应阈值图像。

图1 AprilTag检测器的步骤Fig.1 Steps of the AprilTag detector

(2)自适应阈值图的图像边界分割。使用联合查找算法[20]对亮像素和暗像素的连通部分分割。遍历阈值图上的黑白点，按图2方式查找，相邻像素值相等则分配唯一父节点，得出每个分量唯一的ID。

图2 连通查找方式Fig.2 Connected search

使用Hash表完成聚类。遍历阈值图上黑白点，针对相邻黑白区域，将该区域上的点标识为不同聚类。按图2方法查找相邻点x0与x1，记录对应像素值和为255的父节点ID，P[x0]与P[x1]。利用ID值为每个聚类建立索引，按如下约束

(2)

式中：“”为取余符号；常数s=32；T=8 388 607；m=2 654 435 761 是2～232的近似黄金分割数，该数的取值方式让不同聚类建立的索引值不易重复。

图1(c)是聚类后线段分割图，各聚类用不同颜色表示。

(3)同一聚类点预处理。将点按其质心以角度大小排序。每个聚类包含N个点的有序坐标，令质心坐标为(XC,YC)，定义

(3)

式中：Xmax、Xmin、Ymax、Ymin分别为N个像素点横、纵坐标的最大、最小值；常数用于调整像素中心偏差。

根据公式(4)，依次计算各像素点(Xi,Yi)与质心连接的角度θ。

(4)

式中i=1,2,…,N。

聚类点采用相邻像素值加权处理,其中一像素点坐标为(Xi,Yi)，令该点权重值Wi为

(5)

式中：P1、P2为相邻上、下点的像素值；P3、P4为相邻左、右点的像素值。

(4)主成分分析(PCA)[21]计算线拟合。聚类点中依次取n(n·N)个数据，如(X1,Y1),(X2,Y2),…,(Xn,Yn)构成一个数据集，设拟合后的直线方程为

ax+by+c=0

(6)

式中：a、b、c为待求解未知常数。

(7)

则该数据集的协方差矩阵为

(8)

通过公式(9)可以求解公式(8)协方差矩阵的最大特征值对应的特征向量，也就是直线方程的系数a、b，β为拟合直线的法线相对横坐标轴正方向角度。

(9)

拟合直线方程为

(10)

拟合直线误差的计算，将该误差函数中的峰值标识为候选角。令

(11)

式中e为拟合直线误差大小。

遍历4个候选角所有排列，四边形各侧进行线拟合，计算每侧拟合直线误差。设

E=e1+e2+e3+e4

(12)

式中：E为四边形拟合总误差；e1、e2、e3、e4为四边形四边拟合直线误差。

当E为最小拟合误差，取对应4个候选角划分的四组数据点，进行四边形拟合。根据两条直线相交求解4个角点像素坐标，图1(d)中红色框表示拟合候选的四边形。丢弃不良的四边形拟合和不可解码的标签，并输出有效的标签，如图1(e)所示。

2.2 识别速度的优化

直线拟合算法处理速度慢。针对分辨率为 2 048×2 048 像素的图片，只保留标签边缘像素信息，达到了加快传统算法检测速度的目的。

图片下采样处理降低分辨率。原始图片像素点(u,v)的像素值为F(u,v)，用最近邻插值缩小图片，确定缩放比例因子K

(13)

式中：sw×sh为原图分辨率；dw×dh为目标图片分辨率。下采样后图片像素点(u,v)的像素值为f(u,v)，计算公式为

f(u,v)=F[u·(sw/dw), v·(sh/dh)]

(14)

下采样图片标签局部定位。本文K取值64，利用OpenCV函数findCountours，检测下采样图片中标签的大致轮廓，得到4个角点的像素级坐标。与K相乘，计算出原图中标签的4个角点像素坐标，最终确定标签所在区域。图3为相机定位标签局部图，图3(a)小倾角下的图像，记为0°，图3(b)为标签绕相机Z轴旋转约45°后的图像。

图3 相机定位标签局部图Fig.3 Partial view of camera positioning label

(1)改进1：局部定位图中，标签非边缘内部像素点的去除。根据几何关系确定标签内部像素点位置，设内部点I，则满足如下关系

SABCD=SABI+SBCI+SCDI+SADI

(15)

式中：A、B、C、D为下采样局部定位确定的4个角点；SABI、SBCI、SCDI、SADI分别为点I与四边组成的三角形像素面积；SABCD为四边形像素面积。

内部点I的位置需要进一步约束。设定阈值h，点I与标签四边的垂直距离应大于该阈值，具体约束条件为

(16)

式中LAB、LBC、LCD、LAD分别为标签四边的像素长度。

标签非边缘内部像素区域为点I的有效范围。将该区域设置像素值为255，二值化时该区域为低对比度，删除该区域。改进1中将标签非边缘内部像素点去除，如图4。

图4 标签内部像素点去除Fig.4 Removal of pixels inside the label

(2)改进2：局部定位图中，标签非边缘外部像素点的去除。确定标签外部像素点位置，设外部一点O，则满足如下关系

SABO+SBCO+SCDO+SADO>SABCD

(17)

式中SABO、SBCO、SCDO、SADO分别为点O与四边组成的三角形像素面积。

标签边缘像素信息的保留，需对外部点O进一步约束。令

ΔS=SABO+SBCO+SCDO+SADO-SABCD

(18)

图5中外部点O与标签的两种位置关系影响ΔS的值。由几何关系可知，图5(a)情形1中面积差ΔS为△ADO面积的两倍，图5(b)情形2中ΔS为△ADO与△CDO面积和的两倍。

图5 点O与四边形ABCD的位置关系Fig.5 Positional relationship between point O and quadrilateral ABCD

通过限制ΔS的大小，合理保留标签边缘像素信息。令

ΔS>4·0.5·max(LAB,LBC,LCD,LAD)·h

(19)

式中h为设定阈值。

保证标签的正常检测，h大小适当可调。确定了点O的有效范围后，后续图像处理方法同第2.2节中的(1)所述方法。

改进2中将标签非边缘外部像素点去除,见图6。对于不同角度标签提速效果差别大。为了进一步减少搜索空间，结合两种方法，将标签非边缘内、外像素点同时去除，基于几何学提取感兴趣区域(region of interest,ROI)，图7为提取的ROI标签内外像素点去除效果图。

图6 标签外部像素点去除Fig.6 Removal of pixels outside the label

图7 标签内外像素点去除Fig.7 Removal of pixels inside and outside the label

3 实验与结果分析

3.1 角点检测实验

本实验以机械臂为平台，在机械臂末端激光扫描仪上张贴标签，标签规格大小为7 cm×7 cm，每小格1 cm×1 cm的误差在1 μm之内，满足实验所需要求。使用相机型号为TS4MCL-180M/C，分辨率为2 048×2 048像素，帧率大小为149帧/s。

在标签距离相机55～65 cm、75～85 cm、95～105 cm、115～125 cm处，标签任意摆放，角度范围为0°～25°，所指角度为相机光轴与垂直标签方向上的夹角。从不同的角度位置各拍摄了12组，1组包含10帧标签位置相同的图片，图8为部分实验图片。

图8 相机拍摄各角度实验图Fig.8 Experimental image of camera shooting at various angles

阈值h的确定将影响本文算法角点检测相对传统算法的误差。设

R=r1+r2+r3+r4

(20)

式中：R为一帧图片的相对像素误差；r1、r2、r3、r4分别为两种算法检测后每个角点的像素距离。

最大像素误差值为

Rmax=max(R1,R2,…,Rk)

(21)

式中R1,R2,…,Rk为同一距离上k帧图片相对像素误差。

图9为角点检测相对误差与阈值h之间的关系。

图9 h与Rmax的关系Fig.9 The relationship between h and Rmax

从图9中看出，h值越大，误差最大值越小，h≥14时，误差最大值趋近平稳。

取h=14，实验距离相机55～125 cm处拍摄大量的图片，计算各帧图片角点检测相对误差的值，实验结果如图10所示。结果显示相对传统算法角点检测误差保持在0.1 pixel以内，而本文算法角点检测相对误差平均值低于0.1 pixel。当阈值h=14时，本文算法保证了角点检测精度。

图10 角点检测相对误差Fig.10 Relative error of corner detection

3.2 重复性检测实验

通过c++的库函数getTickCount()计算AprilTag标签检测角点总耗时。检测标签相同位置图片，计算各种算法检测耗时，实验结果如图11所示，纵坐标轴为以10为底的对数坐标轴，图12、图13、图14采用相同规则设定纵坐标轴。

图11 标签相同位置重复性检测实验Fig.11 Repeatability test experiment at same distance

由图11数据可知，所有算法检测耗时的偏差率都在6.8%以内，本文算法耗时偏差在2 ms以内，算法检测耗时具有稳定性。

3.3 不同环境下标签的检测实验

选用距离相机55～65 cm、75～85 cm、95～105 cm、115～125 cm处12个拍摄不同位置标签的图片。取相同位置图片检测耗时平均值，图12中数据为各检测耗时平均值。结果显示文献[18]中检测耗时在2 000 ms以上，实时性差。文献[14,15]方法在标签处于不同角度下，其检测耗时波动大，最长耗时达300 ms/帧。文献[17]中检测耗时稳定，实时性较好。针对改进1与改进2进行耗时检测，随着标签角度的不同，降低耗时的效果不同，但是都优于文献[14,15]中的检测方法。本文算法结合的改进1与改进2，检测耗时都优于对比文献中的算法检测，检测效果可观，标签角度影响较小，其耗时在25 ms/帧以下。

图12 各距离下不同位置检测实验Fig.12 Different position detection experiments at various distances

用相同距离下不同位置标签的检测耗时取平均值，分析标签距离的不同，其检测时间的关系。由图13可以看出，文献[18]中算法耗时维持稳定，耗时较长。文献[17]中算法检测速度快，但是随着距离的增加检测耗时增加。文献[14,15]中算法随距离增加，耗时降低。本文算法随距离增加，耗时降低。

图13 平均检测耗时与距离的关系Fig.13 Relationship between average detection time and distance

图14 动态验证实验Fig.14 Dynamic verification experiment

3.4 动态验证实验

末端执行器在距离相机55～125 cm处做无规则运动，保存时间序列图片。由图14中本文算法耗时曲线可知，每帧耗时均小于20 ms，因此可以实现40帧/s的检测速度。

4 结 论

传统AprilTag标签检测算法速度低下，本文在保证标签角点检测精度的同时提出了一系列改进算法检测速度的方法。通过局部定位，去除标签内外非边缘像素，进行精确快速的检测AprilTag标签角点，然后采用EPnP+LM算法对3D-2D点进行位姿解算优化[22,23]，通过最小化重投影误差得出解决方案。实验证明机械臂末端到相机的工作距离，即55～125 cm范围内，本文算法1帧耗时在25 ms之内，检测速率大于40帧/s，且随工作距离的增加，其检测速率越快，可实现对机械臂末端的实时定位。