邓文尧，段学超，靖明阳

（西安电子科技大学电子装备结构设计教育部重点实验室，陕西西安710071）

引 言

基于视觉的动态目标检测与跟踪已逐渐渗透到现代生产、生活的各个领域，例如视频监控、虚拟现实、人机交互、行星探测等场景。在目标检测算法研究领域，文献[1]将积分图用于图像的特征表达，采用级联分类器实现了目标的鲁棒性实时检测。文献[2]通过获取图像的关键点来描述物体的特征，提出了尺度不变特征变换（Scale Invariant Feature Transform, SIFT），但是该方法对场景中非目标物体的运动干扰较为敏感。文献[3]提出了梯度直方图（Histogram of Oriented Gradient, HOG）特征，将其应用于静态图像中的行人检测。文献[4]将HOG 与支持向量机（Support Vector Machine, SVM）相结合提出了可变形部件模型（Deformable Part Model,DPM）。近年来，随着深度学习的发展，文献[5]于2015 年提出的Fast-R-CNN 被广泛应用在目标识别方面。以上这些目标跟踪方法都是在无任何靶标的情况下根据运动物体的自身特征进行检测定位，具有较强的普遍性，但是难以满足高精度定位的要求，尤其在多目标运动的情况下更难实现精准定位和跟踪。

ArUco marker是文献[6]于2014年提出的一种用于增强现实和机器人定位等领域的相机姿态估计基准标记系统。该类marker 具有较高的鲁棒性，将其标记在物体的表面，可以实现物体的精确、快速定位。文献[7]提出的稀疏光流法主要根据两帧图像对应的像素点的信息进行计算，从而得到光流矢量，实现对运动物体的跟踪。相比之下，传统的光流则是利用SIFT 或SURF（Speeded-Up Robust Feature）[8]特征进行跟踪，但是由于两帧图像的特征点有时存在误匹配，因此存在显著误差。本文采用ArUco marker特征点代替运动物体的SIFT 特征，以期实现更高精度的定位。

另一方面，多旋翼无人机的应用近年来得到了大范围普及，但是在各类应用中无人机的安全回收环节仍是一个众所周知的问题。为了保证多旋翼无人机的安全降落，需要其在降落的过程中与收纳平面保持水平。本文将ArUco marker 与光流法相结合应用于无人机的实时位姿计算与动态跟踪，从而为无人机抓捕收纳提供支持。

1 稀疏光流跟踪目标的检测原理

1.1 稀疏光流原理

光流是一种运动模式，它是空间运动物体在成像平面上对应像素的瞬时速度。光流法是利用图像序列中像素在时间域上的变化以及相邻帧之间的相关性来确定上一帧与当前帧之间存在的对应关系，从而计算出相邻帧之间物体运动信息的一种方法。光流法的基本假设是图像像素和亮度的守恒，并且同一物体在前后两帧之间没有明显的形状差别。

LK（Lucas-Kanade）算法[7]基于以下3 个基本假设：

1）亮度恒定。图像中目标像素点在前后帧的外观保持不变，即目标像素点在前后帧的亮度恒定，如式(1)所示：

式中，I(t)为t 时刻的图像。

2）时间持续性或微小波动。即图像的运动相对于时间的变化比较缓慢，如式(2)所示：

式中：Ix,Iy表示图像坐标系中x,y 方向的差分；u,v为x,y 方向的速度；It为对应点在两幅图像上的差分。

3）空间一致性。同一场景中同一表面上的点具有相似的运动，其在图像表面的成像也是在临近的区域内。在LK 光流法中选取5×5 的区域作为临近点，每个点建立1 个方程，可以组成25 个方程的方程组，其矩阵形式如式(3)所示：

上式可以进一步简写成：

利用最小二乘法求解该方程：

解得：

由此便可以得到每一个特征点的速度。LK 光流法一般适用于低速运动的物体，它的改进算法是金字塔LK 光流法[9]。金字塔LK 光流法的一般流程是首先在两幅图像的最高层计算LK 光流，金字塔下一层以上一层估计的结果作为起始点，从金字塔的顶层到底层不断地重复该过程，实现由粗到细的光流跟踪，因而对更快目标的跟踪具有更强的鲁棒性。

1.2 稀疏光流和ArUco marker 的结合

ArUco marker 是Garrido-Jurado，R. Muñoz-Salinas 等人在2014 年提出的一种生成可配置标记字典（大小和比特数）的算法。该算法遵循同一个准则来最大化标记间的距离和比特转换数，从而提高标记点识别的鲁棒性和准确性。

使用ArUco marker 的主要目的是获取准确的标记点，然后对标记点采用稀疏光流法进行检测，避免直接使用SIFT，SURF，ORB（Oriented FAST and Rotated BRIEF）等特征检测方法可能带来的错误匹配的问题。就算法效率而言，ArUco marker 的检测方法更快，但ArUco marker 一般需要人为附加标记点，不能用于没有标记的运动目标。

ArUco marker 算法的主要步骤为：

1）将图像转成灰度图，采用自适应阈值的方法进行图像分割；

2）使用Suzuki 算法[10]进行轮廓提取，并进行四边形拟合，对于距离相近的轮廓选取最外围的四边形；

3）计算单应性矩阵，进行阈值分割得到二值图像，通过对黑色边界的检测剔除无关轮廓；

4）提取轮廓内的marker 信息，得到库中匹配的marker。

每个marker 都有唯一的ID，因此可以通过ID 匹配的方式寻找上一帧和下一帧图像对应的匹配点并使用稀疏光流，从而实现对运动物体的跟踪，并且计算出相应的速度。靶标的布置如图1 所示。

图1 ArUco marker 在无人机上的布局

2 基于光流法的无人机定位和跟踪

2.1 相机的双目标定和手眼标定

相机的成像模型可以看成是小孔成像模型。图2描述了从空间三维坐标到平面像素二维坐标的转换关系。

图2 小孔成像模型

图中：xw,yw,zw表示世界坐标系坐标；xc,yc,zc表示相机坐标系坐标；x,y 为图像坐标系坐标；u,v为像素坐标系坐标；f 为焦距。

如图2 所示，从世界坐标系到像素坐标系的转换关系为：

式中：s 为比例系数；K 为相机的内参矩阵；[r1r2r3t]为从世界坐标系到相机坐标系的齐次变换矩阵；H 为相机单应性矩阵；fx,fy是在u,v 方向的放大系数；u0,v0表示图像中心点的坐标；µ表示相机在轴向的畸变系数，通过张正友相机标定算法[11] 获取。

本文使用直接线性变换（Direct Linear Transform, DLT）方法获得目标点在左相机坐标系下的坐标。如图3 所示，假设世界坐标系下有1 点P，坐标为[X Y Z 1]T，P 在左、右相机下的投影点分别为pL,pR。

图3 双目视觉系统模型

在实际的投影中，通过标定完毕的相机内参进行计算得到的结果很难实现两个点完全重合，这种情况下可以取交线的一个中点，如图3 所示。根据式(7)，对左、右相机分别建立从相机坐标系到世界坐标系的转换关系如下：

式中：u1,v1表示P 点在左相机像素坐标系下的坐标；KL为左相机的内参矩阵；P1是内参矩阵和外参矩阵相乘的结果；P11,P12,P13为P1的3 个行向量；X 表示世界坐标系坐标。对等式两边同时叉乘x1有：

从而有：

通过观察式(11)可以得到两个线性方程。同理，右相机下具有相同的结果。将最终结果合并可以得到式(12)：

假设世界坐标系和左相机坐标系重合，则左相机的旋转矩阵为单位矩阵，平移向量为0 向量。右相机到左相机的位姿转换关系可以通过标定的方法获取。因此，式(12)仅有3 个未知数而具有4 个等式，可以采用最小二乘法进行求解。在求解之前，先对向量X 做归一化处理，即：

构造最小二乘的目标函数如下：

该优化问题的解可以通过奇异值分解（Singular Value Decomposition, SVD）得到A 的对角化矩阵，其中最小特征值对应的特征向量即为所求的结果。由此得到点P 在左相机坐标系下的坐标为：

2.2 基于光流法的无人机定位和跟踪

采用已进行标定的双目相机对无人机进行跟踪。首先在无人机的底面贴上5 个ArUco marker 标记，分别位于无人机的底面中心和4 个边角处。利用相机寻找无人机，定位到无人机后采用单个相机获取ArUco marker 的光流，从而获得无人机的运动状态。如果满足抓取要求，则分析双目相机获得的深度图，找到标记点的位置坐标，然后通过手眼标定得到的转换矩阵获取无人机位置。算法流程如图4 所示。

图4 无人机定位抓取流程图

当无人机出现在相机视野中时，通过并联机器人调整上平台位姿或者控制无人机使其落到图像的中心点。同时分析深度图，得到5 个靶标的坐标，每个靶标4 个角点，一共20 个点。5 个靶标都位于无人机平面，可以采用最小二乘法拟合该平面：

式中：z =ax+by+c 为需要拟合的平面方程；Zi为实际测量的坐标；靶标点数n=20。

通过双目相机获取的靶标坐标是左相机坐标系坐标，从相机坐标系到机器人坐标系的转换还需要手眼标定。假设从相机坐标系到世界坐标系的旋转矩阵为R，空间坐标t ∈R3。现有世界坐标系下的3D 空间点Xi∈R3和图像中对应的点xi，其中i 为匹配点集合的索引，最小化3D 空间点投影误差公式为：

式中，F(·)表示投影函数，可以写成：

对于式(16)表示的凸函数，其局部最优解即为全局最优解，因此初始化矩阵R,t 可以取任意值。可以计算出靶标点在机器人坐标系中的位置向量为：

式中，TCtoR为标定的齐次变换矩阵。进一步拟合所有点，即可得到拟合的平面方程。

3 实验和结果

本文采用小觅双目深度D1000-50 相机，分辨率为1 280×720，像素尺寸为3.75 µm×3.75 µm，基线长度为120 mm，焦距为3.9 mm，深度工作距离为0.49 ～10 m。采用张正友相机标定法，最终标定结果如表1 所示。

表1 相机内参表

从标定的结果来看，相机的基线距离为120.1 mm，左相机焦距为3.87 mm，右相机焦距为3.87 mm，这和实际相机的标称参数吻合。利用标定好的相机对目标进行检测，如图5 所示。

图5 双目实际测量结果

最终得到4 个角点的左相机坐标系坐标。为了衡量相对精度，计算了角点两两之间的距离，并和实际的距离进行对比，如表2 所示。

表2 测量结果

从表2 可以看出，任意两点之间的距离约为90 mm，和实际距离一致，而角点的坐标Z 值变化较大，这是因为实际拍摄过程中marker 和相机之间存在一个倾角。因此，在距离相机30 ～90 mm 的范围内，相机的相对精度能控制在1 ～2 mm 以内，满足无人机跟踪要求。

最后，为了验证本文提出的算法，采用ArUco marker 进行了一次初步检测实验。图6(a)中每个靶标角点处粗线标识线代表marker 点的运动方向，线的长度表示速度的大小。图6(b)是动态跟踪的目标在白色背景下绘制出来的轨迹图。将图6(b)和实际情况对比，动态跟踪的结果和实际运动相符。最后，通过相机的双目标定、手眼标定衡量检测的误差，并通过一定的误差补偿对最终结果进行校正。为了验证本文提供的方案，设计并加工了原理样机，展开了无人机抓捕实验。

图6 动态目标识别与跟踪

实验过程中，首先控制粘贴有靶标图案的无人机飞抵抓捕与收纳机器人的工作空间，并利用无人机的定点悬停功能悬停在机器人的工作空间内。机器人视觉系统获取无人机靶标的图像信息，并计算得到无人机的位姿信息。接下来控制系统根据相机反馈的位姿信息发出控制指令，驱动抓捕平台快速向目标移动。在满足抓捕条件时，控制系统向抓手发出抓取指令，完成对无人机的抓取。最后，机器人系统收回电动缸并锁紧电动缸与抓手，完成对无人机的收纳，如图7所示。

图7 无人机抓捕实验

4 结束语

本文将ArUco marker 和稀疏光流相结合，实现了对无人机的精准动态跟踪，避免了特征点误匹配带来的误差。通过双目视觉系统，测量目标点的相机坐标系坐标，在0.3 ～2 m 的距离范围内，测量相对精度能够控制在毫米量级，在不采用任何图形处理器（Graphics Processing Unit, GPU）加速的情况下能够控制在15 ～25 帧的速率；然后通过手眼标定将相机坐标系转换到并联机器人的上平台坐标系；最后，对所有的标记点进行平面拟合，计算出动态目标的实时位姿。本文通过实验验证了采用ArUco marker 和稀疏光流实现动态跟踪的有效性，并衡量了双目测量技术和设备的精度。