基于四旋翼无人机的单目建模系统

庄晓明，李 涛，付 龙

(中科院合肥物质科学研究院先进制造技术研究所,江苏 常州 213164)

基于四旋翼无人机的单目建模系统

庄晓明，李 涛，付 龙

(中科院合肥物质科学研究院先进制造技术研究所,江苏 常州 213164)

为了实现四旋翼无人机的自主飞行、环境单目稠密重建与即时导航功能，文中设计了该单目建模导航系统。飞行控制器采用MPU6050六轴传感器采集六轴信号，通过算法拟合出飞行器姿态，然后通过STM32F103RBT6芯片分别控制4个电机驱动模块,即时通过PID算法输出PWM值进行调速来实现飞行姿态的稳定和改变。建模系统通过控制GPS模块，可以获取飞行器的实时位置；飞行器通过图像采集模块，可以获取飞行器的环境信息。设计了上位机软件，能够接收图像并且进行单目稠密重建。稠密重建主要由从运动到结构、基础表面构建、约束场景流稠密重建、稠密对齐和局部模型集成共五方面组成，然后通过无线串口与无人机通讯，实现无人机的智能自主飞行。实验结果表明，该系统能够实现单目建模的功能。

四旋翼无人机；智能导航；三维重建；姿态控制

0 引 言

四旋翼无人机具有垂直起降、自由悬停的特点，并且体积小，操控方便，携带方便，可以进入人不易进入的复杂环境。目前，四旋翼无人机在航拍、娱乐、勘测、救灾等恶劣环境中发挥着极其重要的作用[1]。

对于复杂不易进入的环境，更多时候可以使用无人机进入。无人机要具有自主飞行的功能，并且能对未知环境进行建模以方便外部人员观察。传统的四旋翼飞机需要使用遥控器进行位置和姿态控制，但无法感知环境的图像细节和深度细节。传统的三维重建方法主要有结构光方法和立体视觉方法，代表产品如Kinect体感器和Bumblebee相机等。

文中设计了一种基于从运动信息中恢复三维场景结构(structure from motion)的三维重建算法，通过使用单目相机采集图像，传输至远程电脑进行实时三维重建，四旋翼无人机根据三维环境结合IMU信息进行姿态控制，实现自主智能导航。

1 飞行器的基本工作原理

四旋翼飞行器是一种有四个旋翼并且旋翼呈十字形交叉的飞行器[2]。飞行器的机械结构简单,旋翼对称分布在机体的前、后、左、右四个方向，四个旋翼处于同一高度平面，且四个旋翼的结构和半径都相同。四个电机对称地安装在飞行器的支架端，支架中间安放飞行控制计算机和外部设备。常见结构如图1所示。

图1 四旋翼飞行器的结构

四旋翼飞行器通过调节四个电机的转速来改变旋翼转速[3]，从而实现升力的变化，控制四旋翼飞行器的位姿。四旋翼飞行器是一种六自由度的垂直升降机，但只有四个输入力，同时却有六个状态输出，所以是一种典型的欠驱动系统。其主要飞行姿态如图2所示。

(1)垂直运动：同时增加四个电机的旋转速度使总拉力增大，当拉力克服整机的重量时，四旋翼飞行器就可以垂直上升;反之，同时减小四个电机的旋转速度，四旋翼飞行器就垂直下降，直至平衡落地，实现了沿z轴的垂直运动。当外界扰动量为零时，当旋翼产生的升力等于飞行器的重力时，飞行器就能保持悬停。

(2)俯仰运动:使电机1的转速上升，电机3的转速下降，电机2和4的转速保持不变。由于旋翼1的升力增大，旋翼3的升力减小，产生的不平衡力矩使机身绕y轴旋转，实现飞行器的俯仰运动。

(3)滚转运动：改变电机2和4的转速，保持电机1和3的转速不变，就可以使机身绕x轴旋转，实现飞行器的滚转运动。

(4)偏航运动：当电机1和3的转速上升，电机2和4的转速下降时，机身就在反扭矩的作用下绕z轴转动，实现飞行器的偏航运动。

(5)前后运动：增加电机3的转速，并相应减小电机1的转速，同时保持其他两个电机转速不变，以保持反扭矩不变。飞行器就先发生一定程度的倾斜，使旋翼拉力产生水平分量，从而实现飞行器的前后运动。

(6)倾向运动：由于结构对称，因此倾向飞行的工作原理与前后运动的原理一致。

2 系统整体设计

四旋翼飞行器的系统结构如图3所示。

图3 系统结构图

GPS模块和六轴传感组成姿态测量系统，主控制器将姿态测量获得的数据进行卡尔曼滤波得到当前姿态，再与从无线模块发送过来的自主导航指令一起作为姿态控制器的输入，姿态控制器通过PID算法输出对应的PWM值[4]，驱动四个电调，进而改变4个电机的转速实现姿态控制。

同时系统搭建了基于单目建模的SLAM系统，可以实时单目稠密重建并在PC端进行渲染和导航。

3 飞行控制系统设计

四旋翼飞行器能够沿着规划的轨迹飞行，并且能够在飞行过程中同步进行SLAM，实时修正轨迹，并且能够返回初始位置[5]。导航系统主要是由GPS模块、六轴传感模块和图像采集模块组成。该系统具有精确修正系统误差的功能，从而提高导航精度[6]。

主控制芯片要进行数据的采集和滤波、图像的采集和传输等大量复杂运算，故选用了STM32F103-RBT6作为主控芯片，STM32F103XX基于高性能32位RISC的ARMCortex-M3核，工作频率为72MHz。片上集成了高速存储器，通过APB总线连接了丰富、增强的外设和I/O。所有的设备都提供标准的通信接口[7]。

3.1 GPS模块

GPS模块型号为LEA-5T，作为导航系统的主要模块，GPS对导航精度及灵敏度起着十分重要的作用，所以必须使用高性能、高可靠性的GPS定位模块。该系统采用的GPS模块具有以下特点：

(1)拥有一个-160 dBm捕获和跟踪灵敏度的Super Sense;

(2)具备Kick Start功能，信号微弱时可实现加速启动；

(3)热启动和辅助启动首次定位时间小于1 s;

(4)支持AssistNow Online和AssistNow Offline等A-GPS服务；

(5)GPS、GALILEO、SBAS(WAAS、 EGNOS、MSAS、GAGAN)混合引擎。

3.2 六轴传感模块

六轴传感模块使用的是MPU6050,MPU6050的角速度全格感测范围为±250、±500、±1 000与±2 000 °/sec(dps)，可准确追踪快速与慢速动作，并且用户可程式控制的加速器全格感测范围为±2 g、±4 g、±8 g与±16 g。产品传输可透过最高至400 kHz的IC。

3.3 图像采集模块

图像采集模块使用的是0V2640。0V2640像素输出最大支持200万像素，支持QCIF(176*144)、QVGA(320*240)、VGA(640*480)、1 027*768、1 600*1 200等像素输出。具备两种输出格式：

(1)原始数据如RGB565，RGB RAW，YUV422；

(2)JPEG压缩图像格式(可极大减少传输带宽，例如640*480分辨率的原图片大小在300 kB左右，JPEG编码输出后大小仅约为16 kB)。

在该系统中，由于使用原始数据格式，图像文件过大(以RGB RAW为例，一帧640*480的图像文件大小为640*480*3=900 Kbytes)，影响数据传输速率和更新图像的频率。TM32f103RBT6处理能力有限，无法做复杂的图像压缩算法，所以选择0V2640内部DSP压缩后的JPEGS图像格式是最好的选择。

MCU与0V2640通信采用串行与并行结合，0V2640带有SCCB(Serial Camera Control Bus)双线串行接口，MCU通过SCCB接口配置和读取0V2640的信息；MCU通过并行总线的方式接收0V2640图像数据。

4 系统软件分析

文中提出了一种可以使用单个实时相机进行场景的快速稠密重建的方法。使用基于点的实时从运动中恢复结构(SFM)作为起始点，生成精确的3D相机姿态估计和稀疏点云。主要创新在于使用了一种源于SFM的基础网格生成方法，预测在一束自动选择的跨越场景的参考帧附近的姿态中的视野，然后使用一种视野预测的光流和有约束的场景流更新的方法将基础网格变形为高度精确的深度图[8]。由于产生深度图的高质量，全局场景模型可以简单将它们并列排放,然后去除重叠区域。笔者展示了一个现实环境可以通过一个实时相机在数秒内重建，其处理使用的仅是普通桌面级别的硬件[9]。

4.1 从运动到结构

与所有稠密重建系统类似，都需要在全局帧中获得高质量的相机位姿估计。使用SFM来提供实时相机位姿的估计和作为稠密场景重建的起始点的稀疏点云。Davison介绍了一种简单的相机追踪和映射系统，其使用了连续概率滤波器来构建一致的场景图和实施相机位姿估计,但是绝对精确对受限于每帧所追踪到的数量较少的特征点。Klein和Murray的平行追踪和映射系统(PTAM)转而依赖基于每帧几百个特征点的测量的帧率位姿估计，并迭代使用最优化平行方法的重复束调整。它能够实现几千个场景点位置和历史摄像帧序列的位姿的高精度估计，因此能够满足需求。

只收集从PTAM获得的高质量的点，并将其送入稠密重建流水线，期间丢弃那些异常值大于10的点。另一种从PTAM获得的有用的值是在关键帧集中场景点的联合可见性。通过对关键帧中的每个点的光轴方向取平均的方法来进行每个点粗略的初始表面法线估计。在上述的关键帧中，这些点都是可见的。

4.2 基础表面构建

已有从SFM中得到的起始3D点云，现在想要估计起始连续场景表面来获得稠密细化的基础。从有向点采样进行稠密3D表面重建的问题已经在计算机图形学社区获得了足够的关注度[10]，并且这个问题是模型曲面补洞、优化表现模式和模型美化的基础，隐式曲面重建技术更是获得了足够的关注。通过这些方法，表面可以通过将函数拟合成点数据的方法估计得到，其中，R3→R,f(x)=0。可以通过多边形化函数的零点集合(zerolevelset)的方法来提取重建网格[11]。在获得即时的基础模型中，速度是至关重要的。而全局优化的无参数的表面拟合技术需要极大的计算代价用于解很大的稠密系统的方程组。近年来，重建的计算消耗有了大幅度的减少，主要是将全局优化算法替换为分层和由粗到精的方法[12]。尤其是有限支持径向基函数可以使有约束的稠密系统变得稀疏。

文中使用了一种最先进的多尺度紧支撑径向基函数的方法用于3D离散数据内插[13]。该方法结合了全局和局部估计方法的优点，适合于稀疏的、方向估计的从SFM过程中获得的点云，特别是保留了在IRef低稠密度的大区域插值的能力。

零点集合的多边形化，使用了Bloomenthal的方法，也由MSCSRBF支持，因为当估计隐式曲面时计算的大部分是基于上一层的函数层次。实际上，可以在每次生成一个新的关键帧时都进行基础表面重建，以便维护即时的基础模型。

4.3 约束场景流稠密重建

在该系统中，使用了简单迭代稠密深度图重建算法。该算法依赖于高质量的相机追踪与在上一章节中获得的基础模型。

假设有n个静止的摄像机观察一个可变形的表面。目标是运用稠密的跨摄像机束的图像测量来使基础表面估计变形到一个更加新而准确的形状上去。

(1)

稠密的基础网面提供了一个关于在摄像机束中每帧可见的表面元素的位置和法向量的预测。希望使用图像对应信息来获取端点更新Δxj以用来将当前基础模型的每个端点x变形到一个新的准确体现真实场景的估计中。

Δxi=riλi

(2)

(3)

最小二乘解通过下列标准方程求得：

(4)

形成稠密的逐端点的表面更新：

(5)

这个计算只包括了两个规模为2n的内积运算，以及一个标量除法，可以在GPU硬件上进行并行化运算。在实践中，通过使用之前的式(1)，获取最后的端点更新。在初始基础模型的端点已经无解的情况下，优化过程继续最多3次迭代，或者直至平均重投影误差小于e=1e-4个归一化像素。

4.4 稠密对齐

4.5 局部模型集成

已经有大量算法可以用于多个深度图的融合，这些深度图传统上是从结构光和范围扫描器中获得的[14]。这些方法可大致分为两类。第一类方法使用计算深度图、密集点样本或者体素符号距离函数来拟合全局连续函数，此函数可用于多边形化。但是很难用于大规模的重建，因为受到普通PC内存和CPU的限制。第二类方法直接在局部网格上进行重建。链式多边形网格,作为最原始的方法，通过去除网格之间的重叠区域的方法来获得连续拓扑网格。文献[9]贡献了有噪声的深度图实时融合的方法。该方法可以获取一系列的深度图，同时可以降低这些深度图的误差并且去除空间碰撞，然后将其多边形化。

因为前面的工作保证了深度图的连续高质量，只要使用一种非常简单的方法就可以满足需求。

假定已知一个从参考帧Pref获得的三角化网格，将刚集成的稠密重建深度图渲染至Pref并且在误差范围内去除邻近顶点。

5 仿真与调试

为了验证四旋翼无人机的飞行与单目导航功能建模，首先制作了样机，在地面调试PID参数，最后在室内进行实际飞行测试，并且在上位机上进行同步建模。飞行效果图如图4～6所示。

图4 四旋翼飞行器样机调试

图5 四旋翼飞行器传输图片

图6 单目稠密重建效果图

6 结束语

文中设计了一种基于四旋翼无人机的单目导航建模系统。该系统能够通过摄像头对环境进行单目稠密重建，在PC端显示并发送给四旋翼无人机，结合无人机的GPS和惯导系统，能够使无人机对极端环境进行探测。设计并制作了四旋翼无人机样机,并进行了实际飞行测试。试飞结果表明：该系统可以用于四旋翼无人机系统进入复杂环境并且进行建模。

[1] 詹 镭，贺人庆，谢 阳，等.基于微型四旋翼无人机的智能导航系统[J].电子测量技术,2011，34(6):1-3.

[2] 王晓明．电动机的单片机控制[M]．第2版．北京：北京航空航天大学出版社，2007.

[3] 杨明志,王 敏．四旋翼微型飞行器控制系统设计[J]．计算机测量与控制，2008，16(4)：485-487．

[4] 刘金琨．先进PID控制及其MATLAB仿真[M]．北京：电子工业出版社，2004．

[5] 聂博文,马宏绪,王 剑,等．微小型四旋翼飞行器的研究现状与关键技术[J]．电光与控制，2007，14(6)：113-117．

[6] 刘慧英,范宝山.基于STM32的多步进电机控制系统研究[J]．测控技术，2010，29(6)：54-57．

[7] 符 冰，方宗德，侯 宇．一种新型微旋翼飞行器的设计与控制[J]．航空制造技术，2006(5)：91-94．

[8]WangL,MordohaiP,FrahmJM,etal.Real-timevisibility-basedfusionofdepthmaps[C]//Proceedingsoftheinternationalconferenceoncomputervision.[s.l.]:[s.n.],2007.

[9]NewcombeRA,StrasdatH,KellyPHJ,etal.SLAM++:simultaneouslocalisationandmappingatthelevelofobjects[C]//Procof2013IEEEconferenceoncomputervisionandpatternrecognition.[s.l.]:IEEE,2013:1352-1359.

[10]WuHao,BuJiajun,ChenChun,etal.Locallydiscriminativetopicmodeling[J].PatternRecognition,2012,45(1):617-625.

[11]KleinG,MurrayDW.ParalleltrackingandmappingforsmallARworkspaces[C]//Proceedingsoftheinternationalsymposiumonmixedandaugmentedreality.[s.l.]:[s.n.],2007.

[12]YuF,GallupD.3dreconstructionfromaccidentalmotion[C]//Proceedingsof27thIEEEconferenceoncomputervisionandpatternrecognition.[s.l.]:IEEE,2014.

[13]WagmanRS,BarmpoutisA,BoziaE.Anovelframeworkfor3dreconstructionandanalysisofancientinscriptions[J].JournalofMachineVisionandApplications,2010,21(6):989-998.

[14]ZhangLijun,ChenChun,BuJiajun,etal.Activelearningbasedonlocallylinearreconstruction[J].IEEETransactionsonPatternAnalysisandMachineIntelligence，2011,33(10)：2026-2038.

Monocular Modeling System Based on Four Rotors UAV

ZHUANG Xiao-ming,LI Tao,FU Long

(Institute of Advanced Manufacturing Technology,Hefei Institute of Physical Science, Chinese Academy of Science,Changzhou 213164,China)

In order to realize autonomous flight,monocular dense reconstruction and auto navigation of four rotors UAV,the monocular reconstruction and modeling system is designed.The flight controller uses six axis sensors,MPU-6050,to detect the attitude of the four rotors UAV;the speed of the 4 motor drive module is controlled by the STM32F103RBT6 chip to change the attitude of the UAV with PID algorithm.The monocular reconstruction system acquires real-time position and environment information of the UAV by controlling the GPS module and image acquisition module.The software on PC can receive the images and make monocular dense reconstruction.Dense reconstruction contains five main part including structure from motion,basic surface construction,constrained scene flow dense reconstruction and local model integration.The intelligent autonomous flight of UAV is realized through communications between the PC and the UAV with wireless serial port.Experiments show that the system can realize monocular dense reconstruction.

four-rotor micro UAV;smart navigation;3D reconstruction;attitude control

2015-06-16

2015-09-23

时间：2016-06-22

国家青年科学基金项目(61401437);中国科学院合肥物质科学研究院院长基金青年火花项目(YZJJ201323)

庄晓明(1987-),男，硕士研究生，研究方向为控制工程与机器视觉。

http://www.cnki.net/kcms/detail/61.1450.TP.20160621.1701.008.html

TP302

A

1673-629X(2016)07-0104-05

10.3969/j.issn.1673-629X.2016.07.022