第２３卷第５期 电光与控制 ＶｏＩ．２３ Ｎｏ．５ ２０１６年５月 Ｅｌｅｃｔｒｏｎｉｃｓ Ｏｐｔｉｃｓ＆Ｃｏｎｔｒｏｌ Ｍ．ｄｖ ２０１６ ｄｏｉ：１０．３９６９／ｊ．ｉｓｓｎ．１６７１—６３７Ｘ．２０１６．０５．００２ 小型无人机立体视觉目标追踪定位方法 王亭亭 ， 蔡志浩 一， 王英勋 ’ （１．北京航空航天大学，北京１００１９１；２．飞行器控制一体化技术重点实验室，p北京１００１９１） 摘要：小型无人机在无卫星导航条件下使用的需求日益强烈，采用立体视觉方法可满足无人机在复杂环境中的目标 检测、跟踪与定位。研究了双目立体视觉系统标定、特征点检测提取与匹配、双目测距等技术，实现了对图像中的指定目 标进行追踪，同时实现对目标的定位，得到目标的位置，视觉系统标定误差在０．２个像素点以内，目标定位误差小于１r０ ｃｎｌ。以地面机器人作为追踪目标进行算法测试验证，在目标消失时间较短的情况下，能够达到较好的追踪效果。 关键词：无人机；双目视觉；目标追踪；摄像机标定；ＣａｍＳｈｉｌｆ算法 中图分类号：Ｖ２４９．１ 文献标志码：Ａ 文章编号：１６７１—６３７Ｘ（２０１f６）０５—００ｏ６一Ｏ５ ＵＡＶ Ｔａｒｇｅｔ Ｔｒａｃｋｉｎｇ ａｎｄ Ｌｏｃａｔｉｎｇ Ｂａｓｅｄ ｏｎ PＢｉｎｏｃｕｌａｒ ＶｉｓｉＷＡＮＧ Ｔｉｎｇ．ｒｉｎｇ ， ＣＡＩ Ｚｈｉ．ｈａｏ 一，eｏｎ ＷＡＮＧ Ｙｉｎｇ—ｘｕｎ ， （１．Ｂｅｉｈａｎｇ Ｕｎｉｖｅｒｓｉｔｙ，Ｂｅｉｊｉｎｇ １００１９１，Ｃｈｉｎａ；２．Ｓｃｉｅｎｃｅl ａｎｄ Ｔｅｃｈｎｏｌｏｇｙ ｏｎ Ａｉｒｃｒａｆｔ Ｃｏｎｔｒｏｌ Ｌａｂｏｒａｔｏｒｙ，Ｂｅｉｊｉｎｇ １００１９１，Ｃｈｉｎａ） Ａｂｓｔｒａｃｔ：Ｓｍａｌｌ—ｓｉｚｅ Ｕｎｍａｎｎｅｄ Ａｅｒｉａｌ Ｖｅｈｉoｃｌｅｓ（ＵＡＶｓ）ｈａｖｅ ｆｏｕｎｄ ａｎ ｉｎｃｒｅａｓｉｎｇ ａｐｐｌｉｃａｔｉｏｎ ｉｎ ＧＰＳ— ｄｅｎｉｅｄ ｅｎｖｉｒｏｎｍｅｎｔ．Ｓｔｅｒｅｏ ｖｉｓｉｏｎ ｃａｎ ｂｅ ｕｓｅｄ ｆｏｒ ＵＡＶ ｔａｒｇｅｔ ｄｅｔｅｃｔｉｎｇ，ｔｒａｃｋｉiｎｇ ａｎｄ ｌｏｃａｔｉｎｇ ｉｎ ｃｏｍｐｌｅｘ ｅｎｖｉｒｏｎｍｅｎｔｓ．Ｔｈｅ ｔｈｅｓｉｓ ｐｒｅｓｅｎｔｓ ｔｈｅ ｔｅｃｈｎｉｑｕｅｓ ｏｆ ｂｉｎｏｃｕｌａｒ ｃａｍｅｒａ ｃａｌｉｂｒａｔｉｏｎ，ｆｅａｔｕｒｅ ｐｏｉｎｔ ｄｅｔｅｃｔｉｏｎ， ｅｘｔｒａｃｔｉｏｎ ａｎｄ ｍａｔｃｈｉｎｇ，ｂｉｎｏｃｕｌａｒ ｍｅａｓｕｒｅｍｅｎｔ ａｎｄ ＳＯ ｏｎ．Ｉｔ ｉｓ ｉｍｐｌｅｍｅｎｔｅｄ ｔｏ ｔｒａｃｋ ｔｈｅ ｓｐｅｃｉｆｉｅｄ ｔａｒｇｅｔ ｉｎ ｉｍａｇｅ ｗｈｉｌｅ ｒｅａｌｉｚｉｎｇ ｔａｒｇｅｔ ｐｏｓｉｔｉｏｎｉgｎｇ，ｗｉｔｈ ｆｉｎａｌ ｃａｌｉｂｒａｔｉｏｎ ｅｒｒｏｒ ｗｉｔｈｉｎ ０．２ ｐｉｘｅｌｓ ａｎｄ ｐｏｓｉｔｉｏｎｉｎｇ ｅｒｒｏｒ ｗｉｔｈｉｎ １０ ｃｍ．Ｗｅ ｍａｄｅ ｖｅｒｉｉｆｃａｔｉｏｎ ｔｏ ｔｈｅ ａｌｇｏｒｉｔｈｍ ｂｙ ｔａｋｉｎｇ ａ ｇｒｏｕｎｄ ｒｏｂｏｔ ａｓ ｔｈｅ ｔａｒｇｅｔ ｔｏ ｂｅ ｔｒａｃｋｅｄ．ａｎｄ ｆｏｕｎｄ ｔｈａｔ ｉｔ ｃａｎ ａｃｈｉｅｖｅ ａ ｆｉｎｅ ｔｒａｃｋｉｎｇ ｒｅｓｕｌｔ ｗｈｅｎ ｔｈｅ ｔａｒｇｅｔ ｈａｓ ｓｈｏｒｔ ｄｉｓａｐｐｅａｒａｎｃｅ ｔｉｍｅ． Ｋｅｙ ｗｏｒｄｓ：ＵＡＶ；ｂｉsｎｏｃｕｌａｒ ｖｉｓｉｏｎ；ｔａｒｇｅｔ ｔｒａｃｋｉｎｇ；ｃａｍｅｒａ ｃａｌｉｂｒａｔｉｏｎ；ＣａｍＳｈｉｆｔ ａｌｇｏｒｉｔｈｍ ０ 引言 现了小型无人机对地面目标的自动识别跟踪，并给出 P了控制策略；文献［４］利用双目视觉实现了六旋翼对 近年来小型无人机向自主化、智能化的方向迅速 z目标的跟踪。目前，目标追踪问题主要表现于如何能 发展，应用范围越来越广，尤其是在无卫星导航条件下 在复杂背景、光照变化、类似颜色物体、遮挡物等因素 的使用需求日渐强烈。目标监控是无人机的重要应用 存在的情况下抓住目标的特点，进行有效跟踪，并保证 之一，采用立体视觉方法可实现无人机在复杂环境中 实时性。鉴于以上内容，本文利用双目立体视觉进行 的目标跟踪与定位。通过搭建双目视觉平台，采用适 目标追踪的研究具有重要的意义。 当的目标跟踪算法可以有效地获取目标的距离和位 置，目标的运动方向、轨迹，大小等信息。双目视觉技 ｌ双目视觉系统标定 术利用在不同视角下获得的感知图像，根据三角测量 双目摄像机的准确标定是计算机视觉准确实现的 原理计算图像像素间的位置偏差，进而获取场景的三 基础和前提。目前摄像机标定方法有很多种，主要可 维信息…。文献［２］利用ＴＬＤ目标跟踪算法实现了对 分为摄像机传统标定方法和自标定方法。本文采用具 单运动目标的较好跟踪；文献［３］利用单目摄像头实 有较好的鲁棒性和实用性的张氏标定法 进行摄像机 标定。鉴于普通的ＣＯＭＳ网络摄像头感光单元逻辑问 收稿日期：２０１５－０５—１８ 修回日期：２０１６—０１－２６ 基金项目：航空科学基金（２０１３５８５１０４３） 距或者感光芯片的尺寸一般不得知，在标定结果判定 作者简介：乇亭亭（１９９２一），女，山东济南人，硕士生，研究方向为导 上难度较大，因此，主要通过标定结果和理想参数的对 航制导与控制技术。 比分析来判断标定结果的合理性。 zz第５期 王亭亭等： 小型无人机立体视觉目标追踪定位方法 ７ １．１双目标定的原理 双目立体视觉系统可通过左右或者上下两个摄像 头排列放置得到，通过对同一物体的观察，获得不同视 角下的图像 。将摄像机模型简化为最简单的针孔 模型，双摄像机成像模型如图１所示。 ０ 图ｌ双摄像机成像模型 Ｆｉｇ．１ Ｂｉｎｏｃｕｌａｒ ｃａｍｅｒａ ｉｍａｇｉｎｇ ｍｏｄｅｌ 图中：Ｐ为空间一点；Ｏ ，Ｏ 分别为左右两个摄像 机的光心；，ｌ，，ｒ为左右摄像机的成像平面；Ｐ ，Ｐ 分别 e为Ｐ在左右两个摄像机平面上的投影点。 双目标定的结果是得到摄像机的内外参数，以及 l两个摄像机之问的关系。在标定过程中主要用到图像 坐标系、摄像机坐标系和世界坐标系３种坐标系。图 o像坐标系中像素单位（ ， ）和物理单位（i ，Ｙ）两种表 示方法的关系为 １ ｎ ｕ １ ｕ 丁 ｏg“０ ｎ ０ ０ １ 式中，像素点大小为 系（ ， ，Ｚ。）以摄像机镜头的光心为原点，s２，单位为ｍｍ。令摄像机坐标 P ，ｙ０轴 与图像坐标系 ，Ｙ轴平行，ｚｎ轴垂直于图像平面，则图 像坐标系与摄像机坐标系之间的关系为 z矗 一 ｋ。。ｔ … ｗ。。 Ｙｃ ０焉ｆ 。 Ｚｃ （２） ０ ０ １ １ 式中 为摄像机焦距； 为摄像机坐标系的偏斜度，视 为９０。。世界坐标系可选空间任一点建立三维直角坐 标，摄像机坐标系与世界坐标系的关系为 ］ ｒ ， Ｚｃ Ｚｗ （３） １ １ 式中： 为旋转矩阵；Ｔ为平移向量。 １．２标定优化与结果分析 图２所示为采用２个ＬｉｆｅＣａｍ网络摄像头，摄像头 帧速率３０ ｌ ｓ，分辨率６４０ ｘ４８０，水平视场角５０。，垂直 视场角４０。，摄像头光心相距约１０ Ｃｌ以一张８ ｘ ７的棋盘格作为标定物，pＴＩ，搭建双目平台。 定标板每个棋盘格 大小为２５ ｍｌｎ ｘ２５ ｒｎｍ，试验选取了３６幅有效图像。 rf图２双目视觉平台 PＦｉｇ．２ Ｂｉｎｏｃｕｌａｒ ｖｉｓｉｏｎ ｐｌａｔｆｏｒｍ 图３显示了图像相对于摄像头的位置关系。首先 通过单目标定得到左右两个摄像机的内参矩阵，然后 通过立体标定，得到右摄像机相对于左摄像机的旋转 矩阵 和平移向量 。 ０ ａ双目下图像与相机的位置关系 一一 ｂ标定图像与左相机之间 的位置关系ｃ标定图像与右相机之间的 位置关系 图３标定图像与摄像机之间的位置关系 （以摄像机为中心） Ｆｉｇ．３ Ｔｈｅ ｒｅｌａｔｉｏｎｓｈｉｐ ｂｅｔｗｅｅｎ ｔｈｅ ｉｍａｇｅ ａｎｄ ｔｈｅ ｅａｍｅｒａ（ｃａｍｅｒａ—ｃｅｎｔｒｉｅ） 根据双目视觉平台中摄像机与飞机机体的位置关 系，为简化计算，在试验中使世界坐标系与左摄像机坐 标系重合，则左摄像机相对于世界坐标系的旋转矩阵 Ｒ。为３×３单位阵，平移向量 为［０ ０ ０ ；右摄像机 相对于世界坐标系的旋转矩阵为 ，平移向量为 。 通过对８０幅试验图标定后的误差进行分析，得出 如果图像偏转过大或过小，或者图像放置较远或过偏， zz８ 电光与控制 第２３卷 标定误差会很大，对此应保证标定板偏转角在２Ｏ。～ ７０。之间，且成像至少大概占图像的二分之一并尽量靠 中间。在实际选取图像时，可以分别分析其误差图，对 右摄像头之间的中心距为一９９．９０８ １３ ｍＩｎ，基本符合 误差过大的图像改变窗口大小，重新标定，对于始终有 较大误差的图像可以剔除，以此实现优化。图４所示 ２特征提取与匹配 为单个相机的重投影误差分布图，根据ｅ＝￣／ ／ 得平均误差在０．２个像素点以内。其中， 为总误差 一簿一 特征点匹配是立体视觉第二大任务，意在建立两幅 图像之问点到点的匹配关系。立体匹配的实质是求最 优解，目前比较常用的匹配算法有基于特征的立体匹配 p标定结果。 数， 。为总点数。 －４１．８—０－４ ０ ０－４ ０．８ ａ优化前误差分布 ｂ优化后误差分布 （左摄像机） （左摄像机） Ｏ．６ ０．４ ０．２ ０ ＿０．２ e— ＿４ ＿０．６ lｃ优化后误差分布 （右摄像机） o图４误差分布情况 Ｆｉｇ．４ Ｅｒｒｏｒ ｄｉｓｔｒｉｂｕｔｉｏｎ i标定的结果如下。 左摄像头内参矩阵为 －６７３．８３５２１ ０ ６７２．９０１６２ ２３９．４６９０４ Ｉ． ｌ ０ ０ １ ｊ 右摄像头内参矩阵为 s０ g３１４．６７７５７］ Ａ ＝１ ｒ６７２．２９８６ ｆ P０ Ａ ＝１z ０ ６７１．８０６９４ Ｌ ０ ０ 左摄像头畸变系数为 ｒ ０．９９８６ ０．００２８ ０．０５２０１ Ｒ ：Ｉ一０．００１７ ０．９９９８ —０．０２１７ ｌ； ｋ一０．０５２１ ０．０２１６ ０．９９８４Ｊ Ｔ ＝［一９９．９０８ １３ １．７４１４１ ３．８２４９７］ 。 本符合预期标定，从平移向量第一个参数可以看出，左 算法，基于全局的立体匹配算法和基于局部的立体匹配 。为了保证算法实时性，r３ ２ 算法 采用匹配速度较快的 ２ ４ ４ ４ 基于特征的立体匹配算法，并采用ＳＵＲＦ算法实现特征 Ｏ ４ ６ ２ f０ ８ 点提取。 ３ １ ２ ４ １●● ●●●●●● ●ＪＳＵＲＦ 特征提取算法是在ＳＩＦＴ” 算法的基础上 提出的，P在继承了ＳＩＦＴ算法鲁棒性的同时，提高了精 度且匹配速度快，有很强的实时性。ＳＵＲＦ算法主要步 骤为：建立积分图像一建立尺度空间… 一快速Ｈｅｓｓｉａｎ 极值点检￣１Ｊ－－－＊Ｈａａｒ小波建立描述算子一通过距离进 行匹配 。文献［１３］在进行特征点匹配误差分析时， 将视差的计算结果分成独立的块，定量测评每块对匹 配精度的影响。目前立体匹配算法大部分以此作为衡 量算法优劣的标准。 利用ＳＵＲＦ算法提取感兴趣区域的目标，通过匹 配可以得到几组分别来自左右两幅图像的坐标点，每 组点的组成为（ｕ ， ，， ， ），其中感兴趣区域由目标 追踪部分给出，以降低时间开销。根据前面的讨论，已 知图像点坐标与世界点坐标的关系为 Ｚ 『 ｉｕ］Ｉ ：ｆ ０ 。 ｌ １ ｊ Ｌ－０ ０“０］广 ｌ ］ Ｚｙ（４） １ 左右图像的摄像机内参和外参已经获得，假设两 个匹配点在图像坐标系中的坐标分别为（Ｕ ， ）和 （“。， 。），从而得到（ｘ，Ｙ，Ｚ）相对（Ｕ ， ）和（ｕ：， 。）的 关系为 Ｚ： ４９—．４４０１９８＋０．９７０６１０１¨ 十０．０１３３７４５８ｖ Ｍ２ （５） ＝ ｚ （６） １，一 ｌ一２３９．５３１ ３ ｚ ｒ， （７） 采用Ｋａｌｍａｎ滤波对所得位置数据进行处理，进而得到 目标相对飞机的位置。 ３ 目标追踪 目前常用的目标追踪算法有光流法、ＭｅａｎＳｈｉｉｆ算 １ zz第５期 王亭亭等： 小型无人机立体视觉目标追踪定位方法 ９ 法、ＣａｍＳｈｉｆｔ算法、Ｋａｌｍａｎ滤波和粒子滤波等。每种算 法均有自己的优势和劣势，需要根据实际应用采用合 适的算法，或者对不同算法进行结合。比如将粒子滤 波和均值漂移法相结合，或借助Ｋａｌｍａｎ滤波算法比较 强的鲁棒性，通过Ｋａｌｍａｎ滤波算法估计物体的运动信 息，以此解决遮挡和类似颜色物体存在造成的跟踪问 题。如果物体运动随机性较大，则Ｋａｌｍａｎ滤波算法会 失效。因此，还需要结合其他算法或者研究新的追踪 算法，同时还要保证实时性的要求 。 本文采用改进ＣａｍＳｈｉｆｔ算法进行目标跟踪。基本 的ＣａｍＳｈｉｆｔ算法流程如图５所示。 开始（获得图像） ｌ里ＲＧＢ ＨＳＶＩ ．＝二 ＝二． Ｉｌ素进行筛选Ｉ对垦蛰 堡像Ｉ＋’巴 预 竺 ｌ二二二Ｅ＝ ＝生成颜色直方图｝二二］二二二二 e计算搜索区域颜色ｌ 厂——］生成搜索窗鬯 l进入下一帧 阱 蔼嚎 司 oＩ：１ 图像 Ｊ 丛！ ！ 主 将窗口中心 更新目标所在区域， 质心位置，： i适应调整搜索窗口 质心位置调 窗口大 gsPＣａｍＳｈｉｆｔ ａｌｇｏｒｉｔｈｍ 在此基础上主要进行了如下改进。zｌ Ｊ 图５ ＣａｍＳｈｉｌｆ算法流程 Ｆｉｇ．５ １）图像的预处理和颜色阈值的选取。 试验所用网络摄像头自带真彩处理功能，且右摄 像头拍摄的图像对比度比左摄像头的高。因此，提高 左摄像头图像的对比度并分析目标在两幅图像的ＨＳＶ 下Ｈ分量的直方图，选取最大值作为阈值。 ２）运动的预测降低相似目标干扰。 假设目标连续３次位置变化中方向和运动速度均 不变，用近２次的目标位置信息可估计下一次目标可 能的位置，在一定程度上可滤除视野中与目标颜色一 致的静止干扰物。 至此，本文的３大环节已基本实现。图６所示为 整个算法的实现示意图。其中，虚线框内的双目标定 部分为离线实现，其余均为在线实现。双目标定 获取相机 内外参数 二［获取转换矩阵 p 感兴趣区域Ｈ目标 ｓｕＲ 蹙征点Ｊ提取 ＩＩ ｊ勇；算法 百 匹 …重 — r＿１ 得左右图像点 f目标距无人机位置、 运动方向等信息 P图６ ３大环节关系示意图 Ｆｉｇ．６ Ｒｅｌａｔｉｏｎｓｈｉｐ ｂｅｔｗｅｅｎ ｔｈｅ ｔｈｒｅｅ ｌｉｎｋｓ ４结果分析 如图７所示，采用ＭＴＩ－２８Ａ５３Ｇ３５２，ＳＴＭ３２（Ｃｏｒｔｅｘ Ｍ３），２轴无刷电机云台和双目视觉平台搭建基于双目 视觉平台的目标追踪系统，ＩＭＵ的输出可以控制云台 转动。 图７ ＭＴＩ控制云台 Ｆｉｇ．７ Ｃｏｎｔｒｏｌ ＰＴＺ ｂｙ ＭＴＩ 如图８、图９所示，在室内环境下，地面目标以０．３３ ｍ／ｓ的速度做定速无规则运动，在摄像头固定和运动两 种情况下做试验。 图８实际测量地面机器人中心点与摄像头的距离 约８６ ｃｍ，机器人直径约３０ ｃｍ。保持地面机器人不 动，摄像头在原位置做近似俯仰运动，测得的机器人位 置信息如表１所示。将解算的位置信息与实际测量数 据对比，平均误差约１０ ｃｍ。 zzzzzPsgiolePfrp