电动VTOL飞行器双目立体视觉导航方法

张博翰 蔡志浩 王英勋

(北京航空航天大学 无人驾驶飞行器设计研究所，北京100191)

电动VTOL(Vertical Take-Off and Landing)飞行器在建筑物内部、城区、丛林等复杂环境中执行侦查、监视等任务时，其飞行控制面临着一些新的问题.首先，这类无人机的导航要更加智能化，无人机需要对周围环境一定程度的认知，当障碍物不存在时应能够预先形成比较理想的飞行轨迹控制飞行.其次，这类无人机必须具备检测和躲避未知障碍物的能力［1］.传统的由惯性测量单元和GPS(Global Position System)组成的导航系统显然无法满足要求.

美国麻省理工学院的Abraham Bachrach等［2］采用激光测距仪和双目视觉相结合，选取常规布局的四旋翼飞行器为验证平台，在简单的室内环境下进行了无人机自主飞行的验证.此外，美国爱荷华州立大学航空机器人实验室［3］采用单个摄像头，以普通旋翼直升机为飞行平台，实现了室内的视觉导航、测距以及飞行姿态估计.

本文在双目立体视觉的基础上研究了无人机在走廊中的横向位置坐标定位、检测未知环境中的障碍物并生成避障导航点的方法.

1 系统硬件构成

1.1 飞行器平台

研究基于图1所示的飞行器平台，其宽度为99 cm，纵向桨距为 46.9 cm，全机长度 65.7 cm.它将飞翼机身与4个可倾转的旋翼相结合，是一种靠电力驱动的、能垂直起降和高速前飞的、多旋翼式飞行器.与常规布局的旋翼式飞行器相比，其结构更为紧凑，且可以产生更大的升力，特别适合在建筑物内部、城区以及丛林中执行监视、侦查、损伤评估、态势感知、通信中继等任务，具有广阔的应用前景.

图1 四旋翼飞行器

1.2 双目立体视觉系统

建立了无线的双目立体视觉系统，见图2.左右两个摄像头通过图3所示的控制云台，固定在飞机上，获取的两路视频由无线链路传输到地面站电脑进行处理，图像经过视觉导航系统处理后生成一系列航迹点传给飞行控制系统的外回路.

选用的摄像头为480线的微型摄像头，单个重20 g，工作电压12 V，分辨率640×480.两个摄像头镜头中心横向距离11 cm，前向平行放置，光轴与地面平行.

图2 双目视觉系统硬件

图3 控制云台

2 双目立体视觉图像处理

双目立体视觉系统算法流程如图4所示，左右图像经过图像预处理转换为灰度图，由摄像机标定、校正、立体匹配输出视差图，通过提取特征点的视差值算出其相对与摄像头位置的三维坐标，再经过信息融合生成无人机的航迹点.

图4 双目立体视觉算法流程

2.1 摄像机数学模型和立体标定

摄像机标定的目的是建立有效的摄像机成像模型，确定摄像机的内外属性参数，以便正确建立空间坐标系中物体的空间点与像点之间的对应关系.双目立体视觉系统中的单个摄像机的成像采用针孔摄像机数学模型来描述，即任何点Q在图像中的投影位置q，为光心与Q点的连线与图像平面的交点.如图5所示.

图5 针孔摄像机模型

物理世界中的点Q，其坐标为(X，Y，Z)，投影为点(x，y，f)，如下式所示:

式中，cx和cy为成像芯片的中心与光轴的偏移;fx和fy为透镜的物理焦距长度与成像仪每个单元尺寸sx和sy的乘积.式(1)写成矩阵形式为

矩阵M称为摄像机的内参数矩阵.

在摄像机标定过程中可以同时求出镜头畸变向量，对镜头畸变进行校正.而立体标定是计算空间上两台摄像机几何关系的过程，即寻找两台摄像机之间的旋转矩阵R和平移矩阵T.标定图像选取9行6列的黑白棋盘图，标定过程中在摄像头前平移和旋转棋盘图，在不同角度获取棋盘图上的角点位置.假设定标板放在世界坐标系中z=0的平面上.使用zhang的方法［4］求解摄像头的焦距和偏移，使用另外的基于Brown的方法［4］求解畸变参数.图6为由OpenCV(Open Source Computer Vision Library)绘制棋盘图角点的图像［4］.

图6 绘制棋盘图角点

2.2 立体校正

给定立体图像间的旋转和平移矩阵(R，T)，使用立体校正的Bouguet算法进行立体校正［4］.立体校正的目的是使两摄像机拍摄的图像中的对应匹配点分别在两图像的同名像素行中，从而将匹配搜索范围限制在一个像素行内.Bouguet算法将右摄像机图像平面旋转到左摄像机图像平面的旋转矩阵R分离成两部分，即两个合成旋转矩阵rl和rr.每个摄像机都旋转一半，这样的旋转可以让摄像机共面但是行不对准.为了计算将左摄像机极点变换到无穷远，并使极线水平对准的矩阵Rrect创建一个由极点 e1方向开始的旋转矩阵

这个矩阵将左图像绕着投影中心旋转，使得极线变成水平，并且极点在无穷远处.这样，两台摄像机的行对准即可以通过以下两个旋转矩阵实现:

由图7可以看出经过立体校正后左右两幅图像中的对应匹配点已经处于在同名像素行中了.

2.3 立体匹配

选择区域灰度相关法进行立体匹配.其计算简单、匹配速度快.对于无人机视觉导航来说，如果计算能够以很快的速度完成，则系统就可以提早探测到障碍物而及时采取有利的行动.这点对于时刻处于不断变换的环境中的无人机导航系统的稳定性来说是十分重要的.

图7 立体校正示意图

选用的相似性测度因子为:像素灰度差的绝对值和［5］，简称 SAD.

其中Il(x，y)和Ir(x，y)分别为左图和右图的像素灰度值，假设匹配以左图为参考图，则wp为左图中以Pl点为中心的邻域窗口.如图8所示，对于左图像中的点Pl在右图像中沿着其对应的极线搜索匹配像素，当区域中的像素使相似性准则最小时，则认为是匹配的.

图8 立体匹配示意图

对于室内走廊环境这类弱纹理场景使用灰度相关匹配算法非常容易出现误匹配的情况.所以，如何最大程度提高图像的纹理同时减少左右摄像头获取的图像对之间的亮度差异是立体匹配的关键.为此，引入了高斯拉普拉斯滤波对图像进行预处理［6］.

由图9可以看出经过高斯拉普拉斯滤波后其纹理明显增强.视差图上每一个值代表位于摄像头前的某一距离值.视差越大表示距离越近.其中灰度值越大的区域亮度越高，表示与摄像头的相对距离越近.

2.4 三维坐标恢复

图10为经过校正后的理想的双目立体视觉坐标系，其坐标原点为左摄像头投影中心，X轴由原点指向右摄像头投影中心，Z轴垂直于摄像机成像平面指向前方，Y轴垂直于X-Z平面箭头向下.

图9 立体匹配示意图

图10 双目立体视觉系统坐标系

由立体成像得到视差图后，如果给定屏幕坐标和摄像机内参数矩阵，二维点即可以重投影到三维中，重投影矩阵Q如下式:

其中，除c'x外的所有参数都来自于左图像，c'x是主点在右图像上的x坐标.如果主光线在无穷远处相交，那么cx=c'x，并且右下角的项为0.给定一个二维齐次点和其关联的视差d，可以将此点投影到三维:

三维坐标是(X/W，Y/W，Z/W).

3 室内走廊中飞行器横向坐标定位

电动VTOL飞行器在走廊中横向位置定位实际上是判断走廊中左右两侧墙壁与飞行器的相对距离.实现方法为通过角点提取获得位于墙壁上的角点，再对左右摄像机获取的图像进行角点匹配.记录下匹配角点的横坐标值分别为xl，xr，则其视差值d=xl－xr.再由立体视觉算出其相对飞行器的三维位置坐标，这样就可以得到飞行器在走廊中的横向位置.

角点是像素点在其邻域内的各个方向上灰度变换值足够高的点.对于Harris角点［7］，使用每点周围小窗口的二阶导数图像的自相关矩阵.其定义如下式:

角点位于自相关矩阵有两个最大特征值的地方.采用基于奇异值分解的匹配算法对检测到的角点进行匹配［8］.图11为对图像左右两侧的走廊墙壁区域进行角点匹配的示意图，匹配角点之间用白色直线相连，从图上可以看出该方法匹配的精度较高.

图11 角点匹配

表1为使用立体视觉方法恢复图中匹配角点三维坐标的结果.

表1 飞行器横向坐标定位

4 双目立体视觉障碍物检测

在障碍物存在的前提下无人飞行器的实时导航需要一个复杂的障碍物检测系统，能够及时更新障碍物的相对位置信息，供相应的路径规划算法使用.这不仅关系到预定任务的顺利完成还直接关系到飞行器生存能力的提高.本文利用双目视觉系统构造观测场景的视差图，然后从视差图上分离出障碍物(见图12).

图12 走廊中障碍物视差图

从视差图中分离出障碍物的方法有很多种.对于位于地面上的障碍物，文献［9－10］通过像素点的高度判断从视差图中去除地面像素点，剩下的点被认为是障碍物.文献［11］使用区域生长算法分离视差图上相近视差区域，并且判断该区域中像素的多少决定是否是障碍物.文献［12］通过引入一系列滤波器对视差图进行处理，检测细长型的障碍物效果比较理想，依次包括:地面匹配删除、取出虚匹配和距离过滤、列像素过滤、像素密度过滤等.

本文的方法是:首先将视差图上所有像素点的值归一化到0～255范围内，然后遍历整个视差图，统计每个视差值出现的次数，如视差在某一范围内的像素点总个数超过某一阈值时则认为可能是障碍物.再由立体视觉系统通过恢复特征点三维坐标去除地面和墙壁像素点.图13为使用该方法提取出的障碍物.

图13 提取出的障碍物

通过双目立体视觉系统计算得到障碍物的位置信息为:中心坐标(－49，－3，110)，单位为cm，最大宽度50 cm，高度72 cm.

5 室内走廊中飞行器躲避障碍物

电动VTOL飞行器障碍物躲避的实现主要通过在存在碰撞危险的障碍物的周围生成一系列处于安全区域的轨迹控制点，控制飞行器通过这些航迹点.

躲避障碍物的方法分如下几个步骤:

1)因为研究对象是三维避障问题，所以首先判断是否可以直接越过障碍物.

2)对于不能越过的障碍物通过双目立体视觉系统获取其相对距离以及大小，判断其危险等级，危险等级应与可通过区域的大小成反比，相对距离成反比.

3)针对危险等级最高的障碍物，考虑加入立体视觉系统的位置不确定性和轮廓不确定性，生成危险区域.

4)结合危险区域的范围、无人机的尺寸以及走廊环境内的空间，在障碍物附近生成一系列控制点，引导无人机安全飞过障碍物区域.

轨迹控制点的生成原则是:

1)检测到存在碰撞危险的障碍物时，首先判断其左右两侧存在的安全空间.如果该安全空间小于飞行器的宽度，则判定此障碍物无法躲避.

2)如果左右两侧的安全空间相同且大于飞行器宽度，则判断飞行器与障碍物左右边缘的角度，选择角度小的方向飞行.其第一个导航点高度与飞行器当前高度相同.

3)在穿越障碍物的过程中，实时判断飞行器前方的横向安全空间是否明显变大以决定是否已经成功避障.

4)避障成功后，飞行器航向偏转回到新的安全空间的横向中心位置.至此，完成躲避障碍物的任务.

生成的躲避障碍物导航点如图14所示，图中黑色粗线为障碍物，其右侧线段分别为加入的轮廓和位置不确定，构成碰撞危险区域.以危险区域最右侧点为圆心飞行器宽度的一半为半径画圆，如图所示，则虚线为生成的避障轨迹.可以看出，当电动VTOL飞行器的飞行轨迹位于圆内时存在碰撞危险.

图14 躲避障碍物导航点示意图

6 结论

1)采用双目立体视觉导航技术，结合电动VTOL飞行器的特点，可以在未知的复杂环境中实现飞行器的自主飞行.

2)针对室内走廊的环境采用角点匹配的方法实现飞行器横向坐标定位，为飞行器的安全飞行提供保障.

3)提出的从视差图上提取障碍物的算法简单，提取的障碍物区域准确.再进一步由双目立体视觉恢复障碍物的三维坐标，可以实现躲避障碍物的目的.

References)

［1］Yoko Watanabe，Anthony J Calise，Eric N Johnson.Vision-based obstacle avoidance for UAVs［C］//AIAA Guidance，Navigation and Control Conference and Exhibit.South Carolina:American Institute of Aeronautics and Astronautics Press，2007:20 －23

［2］Bachrach A G.Autonomous flight in unstructured and unknown indoor enviornments［D］.Dept of Electrical Engineering and Computer Science，Massachusetts Institute of Technology.2009:119－126

［3］Koray C，Chung S J，Matthew C，et al.Monocular vision SLAM for indoor aerial vehicles［C］//The 2009 IEEE/RSJ International Conference on Intelligent Robots and Systems.USA St Louis:IEEE Computer Society Press，2009:1566 －1573

［4］Gary Bradski，Adrian Kaebler.学习 OpenCV(中文版)［M］.于仕琪，刘瑞祯，译.北京:清华大学出版社，2009:178－254 Gary Bradski，Adrian Kaebler.Learning OpenCV(Chinese version)［M］.Translated by Yu Shiqi，Liu Ruizhen.Beijng:Tsinghua University Perss，2009:178 －254(in Chinese)

［5］Kanade T，Okutomi M.A stereo matching algorithm with an adaptive window:theory and experiment［J］.IEEE Transactions on Pattern Analysis and Machine Intelligence，1994，6(9):920－932

［6］杜歆.用于导航的立体视觉系统［D］.杭州:浙江大学信息学院，2003 Du Xin.Stereovision used for navigation［D］.Hangzhou:School of Information，Zhejiang University ，2003(in Chinese)

［7］Harris C，Stephens M.A combined corner and edge detector［C］//Proceedings of the 4th Alvey Vision Conference.Manchester:Manchester University Press，1988:147 －151

［8］谢凤英，赵丹培.Visual C++数字图像处理［M］.北京:电子工业出版社，2008:278－367 Xie Fengying，Zhao Danpei.Visual C++digital image processing［M］.Beijing:Electronic Industry Press，2008:278 － 367(in Chinese)

［9］Nedevschi S，Danescu R，Frentiu D，et al.High accuracy stereovision approach for obstacle detection on non-planar roads［C］//IEEE INES.Romania:IEEE Computer Society Press，2004:211－216

［10］Lemonde T，Devy N.Obstacle detection with stereovision［C］//Proc MECHROB.Germany:IEEE Computer Society Press，2004(3):919－924

［11］Dang T，Hoffmann C.Fast object hypotheses generation using 3D position and 3D motion［C］//Int IEEE Workshop Mach Vis Intell Vehicles.San Diego:IEEE Computer Society Press，2005:56

［12］Claudio Caraffi，Stefano Cattani，Paolo Grisleri.Off-road path and obstacle detection using decision networks and stereo vision［J］.IEEE Transactions on Intelligent Transportion Systems，2007(8):607－618