基于视觉的四旋翼室内悬停与定位控制

傅君 刘子龙

摘 要：为使四旋翼飞行器具有更好的室内定点悬停效果及定位精度，提出一种基于视觉辅助与四旋翼飞行器惯性传感器数据融合的控制算法。利用机体下视摄像机获得光流航速信息，与惯性传感器姿态信息数据融合实现良好的室内悬停效果。机体前视摄像机通过ORB算法将当前帧与关键帧进行特征点匹配，以提高四旋翼飞行器的室内定位精度。将PARROT公司的ARDrone 2.0四旋翼飞行器作为实验平台， 采用OpenCV软件对图像进行处理，对控制算法进行验证，结果表明：基于光流和惯性传感器姿态数据的融合确保了四旋翼飞行器控制的安全性，提高了飞行器悬停效果和定位精度。

关键词：视觉；四旋翼飞行器；室内悬停与定位；光流；ORB特征提取；OpenCV

DOIDOI：10.11907/rjdk.181731

中图分类号：TP319

文献标识码：A 文章编号：1672-7800（2018）010-0144-04

英文摘要Abstract：In order to achieve a better indoor fixed-point hover effect and positioning accuracy for quadrotorcrafts，a control algorithm based on visual aided data fusion with quadrotorcraft inertial sensors was proposed.We utilize the body down camera to obtain the optical flow speed information，and the inertial sensor pose information data fusion achieves a good indoor hover effect.The ORB algorithm is employed so that the body front looking camera can match the current frame with the key frame to achieve the feature point matching of the four-rotor aircraft and indoor positioning accuracy.In this paper，the ARDrone 2.0 quadrotor from PARROT was used as the experimental platform.The above control algorithm was verified by using OpenCV to process the acquired images.The results show that the data fusion based on optical flow and inertial sensor attitude data ensures the control safety of of the quadrotor and improves the aircraft′s hover effect and positioning accuracy.

英文關键词Key Words：vision；quadrotor crafts；indoor hover and position；optical flow；ORB；OpenCV

0 引言

四旋翼无人机飞行器具有可垂直起降、机动性能好的优点，定位是其发展的关键因素。无人机定位指利用自身传感器确定无人机在飞行环境中相对于惯性坐标系的位置和姿态信息准确的位姿估计，是实现无人机避障、轨迹规划及目标跟踪等复杂飞行任务的前提和基础[1]。传统的四旋翼飞行器定位主要借助机身自带的惯性传感器单元（IMU）和全球定位系统（GPS）的数据融合，但在一些特殊环境如室内、楼群之间，由于GPS信号弱，很难实现传统的飞行器定位[2]。而基于视觉的定位方法仅使用机载摄像头作为外部传感器，具有体积小、重量轻、价格低、精度高等优势，视觉导航逐渐成为四旋翼无人机自主控制研究的主要方向[3-5]。

麻省理工学院和宾夕法尼亚大学是基于视觉图像的自主定位导航四旋翼无人飞行器研究的典型代表。麻省理工学院的实时室内自主飞行器试验环境主要用于多无人飞行器的协同控制与定位。宾夕法尼亚大学的HMX4通过安装在地面的摄像头测量四旋翼无人飞行器底部标记的位置和，得到其姿态角与位置信息，进而对其进行控制与定位[6-8]。清华大学通过光流传感器获取无人机的水平速度信息，实现无人机的定点悬停功能。而光流法测量相对运动的积分不是绝对运动的积分，不可避免地具有位置漂移现象，不适合长时间导航。除了基于视觉的飞行器控制，还包括视觉同步定位与建图（SLAM）[9]。SLAM主要依靠并行跟踪与建图，其基本原理是通过跟踪地图中的关键帧及关键帧中的特征点进行位置和姿态估计，建立的地图可随定位过程逐步扩充。SLAM算法对四旋翼飞行器系统要求较高，需同时测量飞行过程中的图像数据与距离信息，这对机载系统与激光测距仪要求较高，会提高飞行器成本与算法难度[10]。

结合以上两种方法的优缺点，本文提出基于光流传感器与惯性测量传感器数据融合，结合飞行器动力学模型实现四旋翼飞行器的悬停控制系统。机载前视摄像头对关键帧的特征进行室内定位，实现对四旋翼飞行器的室内控制。

1 四旋翼飞行器平台

本文采用法国PARROT公司的ARDrone 2.0四旋翼飞行器为实验平台，该飞行器原本设计用于增强现实（VR）游戏，但设计人员并不想将该飞行器局限于此。PARROT公司开源了ARDrone的SDK，希望更多的人对ARDrone进行开发。ARDrone 2.0机身如图1所示，该飞行器前部有带90°角镜头的摄像机，可拍摄最高720P（1 280×720）视频，采样频率为15帧/s；底部有一个微型摄像头，可捕捉地面360P视频，采样频率为30帧/s，底部的超声波测距仪可获得高度信息。飞行器本身为一个LINUX嵌入式系统，通过连接WiFi，将图像传至iPhone、iPad或电脑等上位机进行处理[11]。

虽然该飞行器是一款商业级无人机，但PARROT公司提供了一套成熟的开发包（SDK）供二次开发。本文根

据其SDK中提供的API接口及控制飞行器的各种上位机AT COMMAND，通过C++在Visual Studio 2017构建项目，并加载图像处理OpenCV库，对四旋翼飞行器采集的图像信息进行处理，从而对相关算法进行可行性验证。

2 悬停及定位算法

2.1 光流法航速估计

人眼观察物体时，相对运动物体在视网膜上会形成一系列连续变化的影像，即变化的信息不断“流过”视网膜，这样一种光的“流”被称作光流。光流法就是利用视频中像素点的灰度相对变化确定各个位置的运动情况，即研究图像灰度的时域变化与实际物体运动的关系。光流反映了在时间间隔dt内由于运动所造成的图像变化[12]。光流场是通过二维图像表示物体点的三维运动速度场，即表示图像中各位置的运动情况。由Lucas和Kanade[13]提出的光流法应用较广，在OpenCV中也有相关函数，逐渐应用在无人机悬停控制中。

3 算法实现

3.1 光流航速估计与姿态信息数据融合

ARDrone 2.0机体下方的摄像头和超声波传感器组成的光流计，可通过光流法求得四旋翼飞行器的水平速度，但当水平地面上地标物较少或光线较暗时，由于图像处理存在延迟，可能在姿态角发生突变时光流法求得的速度不准确。机体自身搭载的惯性传感器可以很好地估算姿态角，也可粗略估计四旋翼飞行器悬停过程中的速度，但该速度实际存在较大的误差和积分漂移[18]。

本文将光流航速与惯性传感器测量的姿态角及速度信息进行融合，算法思路如下：若姿态角没有发生突变，则机体速度信息以光流航速估计值为准。若惯性传感器测得的机体姿态角发生突变，则飞行器速度信息以惯性传感器测量估计的速度平滑滤波值为准，在姿态角变化稳定时则恢复光流航速估计。图3表示光流航速估计和惯性测量单元数据融合的结果。

3.2 无人机定位控制算法

ARDrone机底摄像头可以测得航速与惯性单元数据，机体内部搭载了内环控制环节。本文设计了串级PID控制器，结合光流计航速及机体底部超声波传感器，实现对四旋翼飞行器悬停时的稳定控制，控制系统如图4所示。

从图4可以看出，在机载内置的内环姿态控制基础上，本文采取的串级PID控制系统加入了基于光流计和惯性测量单元的速度环以及超声波测量仪、前视摄像头。基于ORB特征检测算法的高度控制环，通过串级PID控制器实现更加稳定及准确的四旋翼飞行器悬停控制，提高了四旋翼飞行器室内定位精度[19-21]。前部摄像头提取关键帧程序流程见图5。

通过视觉图像处理辅助分析，悬停状态下四旋翼飞行器高度数据如图6所示。

从图6可以看出，四旋翼飞行器悬停到相应高度时，在起飞过程中图像的突变会导致高度的判断不准确。但当飞行器接近悬停位置时，借助ORB算法的特征点提取和匹配算法，四旋翼飞行器的悬停高度逐渐趋于平稳，该实验结果表明，基于视觉的四旋翼悬停控制算法可行。

4 结语

基于视觉的四旋翼飞行器控制方案解决了四旋翼飞行器在室内的悬停及定位控制问题，可应用于室内自主巡航、跟踪等实验，但还存在以下问题：如图像处理虽然实现了一些基础的位置检测，垂直方向上有超声波测距仪辅助更好地实现z轴的位置控制，但在水平方向上的位置控制存在较大误差。由于水平距离测量的缺失导致无法精确控制水平位置。ORB特征点提取与匹配在飞行器控制算法中存在一定延时，今后在实验中要降低特征点提取算法的图像处理时间，更好地满足四旋翼飞行器实时性要求。

参考文献：

[1] 肖支才.室内四旋翼无人飞行器定位导航的研究现状与关键技术[J].飞航导弹，2014（8）：56-75.

[2] 李力，王耀南.旋翼飞行机器人视觉定位方法及系统[J].机器人，2016（38）：6-11.

[3] PLINVAL H，MORIN P.Visual servoing for underactuated VTOL UAVs：a linear ，homography-based framework[C].Proceedings of IEEE International conference on Robotics and Automation.Shanghai：IEEE，2011：3004-3010.

[4] ABEYWARDENA D，WANG Z.Visual-inertial fusion for quadrotor micro air vechicles with improved scale obsercability[C].Proceedings of IEEE International conference on Robotics and Automation.Karlsruhe：IEEE，2013：3133-3138.

[5] EHASAN A，MOHAMMAD A.Attitude Control using an extended calssifier system algorithm for offline autotuning of a PID controller[C].IEEE RSI/ISM International Conference on Robotics and Mechatronics，2014：925-930.

[6] 鄭伟.基于视觉的微小型四旋翼飞行机器人位姿估计与导航研究[D].合肥：中国科学技术大学，2014.

[7] 吕强，马建业.基于视觉伺服的小型四旋翼无人机自主飞行控制研究进展[J].科技导报，2016（34）：19-26.

[8] HUANG R，TAN P，CHEN B M.Monocular vision-based autonomous navigation system on a toy quadcopter in unknown environments[C].2015 International Conference on Unmanned Aircraft Systems ，2015：1260-1269.

[9] 曹美会，鲜斌.基于视觉的无人机自主定位与控制系统[J].信息与控制，2015（33）：11-30.

[10] 姚西，亢岩.图像透视特征提取方法及其无人机视觉导航中的应用[J].现代电子技术，2014，37（2）：16-20.

[11] PISKORSKI S.Drone Developer Guide[EB/OL].2010-10-05.http：//ardrone.parrot.com.

[12] 郑驰，项志宇，刘济林.融合光流和特征点匹配的单目视觉里程计[J].浙江大学学报，2014，48（2）：25-50.

[13] 魏伟波，潘振宽，李媛媛，等.小位移光流计算的中值公式[J].仪器仪表学报，2011，32（10）：2256-2260.

[14] 刘冠延，郭明，岳晓奎.特征點提取算法比较及四旋翼平台验证[J].哈尔滨商业大学学报，2015，31（1）：155-170.

[15] 吕科，施泽南，李一鹏.微型无人机视觉定位与环境建模研究[J].电子科技大学学报，2017，46（3）： 544-550.

[16] 林志林，张国良，姚二亮，等.动态场景下基于运动物体检测的立体视觉里程计[J].光学学报，2017，37（11）： 111-115

[17] 毛星云，冷雪飞.OpenCV3编程入门[M].北京：电子工业出版社，2015.

[18] （澳）科克.机器人学、机器视觉与控制：MATLAB算法基础[M].刘荣，译.北京：电子工业出版社，2015.

[19] 刘刚，王彪，曹云峰.PID/ADRC控制器在四旋翼无人飞行控制中的应用[J].云南民族大学学报，2014，23（2）：152-154.

[20] 时梦瑶，李健奇.基于视觉引导的室内四旋翼飞行器控制系统研究与设计[J].自动化与仪表，2018，33（2）：50-62.

[21] 季元扬，陈跃东.基于 PID 改进型互补滤波[J].控制工程，2017，24（5）：61-75.

（责任编辑：杜能钢）