基于双目立体视觉避障的四旋翼飞行器设计

(广东海洋大学 电子与信息工程学院， 广东 湛江 524088)

0 引言

随着工业4.0的到来，各种高新科技产业蓬勃发展，高新科技产品也逐渐的平民化，四旋翼飞行器凭借自身优势已逐渐渗透到各行各业之中，为人类的工作和生活带来了极大的便利[1]。然而，在目前四旋翼飞行器研究领域，自主避障是亟待优化的重大难题，理想的避障机制是能够让人们轻松的驾驭，且减少飞行器意外事故的发生和不必要的损失。双目立体视觉的出现让人们眼前一亮[2-3]，将之有效融入四旋翼飞行器的检测与控制之中，有效规避高空电缆、树枝等普通传感器难以检测的障碍物，这一强大的技术融合无疑是四旋翼飞行器智能化的重要体现。在本设计中用STM32作为四旋翼飞行器的主控，用笔记本电脑进行障碍识别，将障碍识别的结果反馈到飞行器上，让其能够判断前方障碍物的位置。在本文中，重点讲述系统的软件部分，姿态解算使用基于四元素姿态的梯度下降法，飞行器稳定控制器使用的是串级PID控制器，在进行障碍物识别时，笔记本电脑在用Visual Studio 2015开发环境下调用Opencv机器视觉库来编写双目视觉检测障碍物的程序并运行，在电脑界面可以看到挂载在飞行器上的双目摄像头传回来的实时图像以便观察飞行状态。

1 系统总体方案设计

1.1 系统总体结构

基于双目立体视觉避障的四旋翼飞行器总体结构由USB双目摄像头识别、2.4 G遥控器、控制指令解析、飞行器位置控制器、飞行器姿态控制器、匿名上位机、空心杯电机控制等模块构成，各模块之间相互联系，相互协调，使四旋翼飞行器系统能够正常工作、快速反应。四旋翼飞行器总体结构如图1所示。

图1 系统总体结构

各模块功能说明如下：

1)USB双目摄像头识别：通过路由器运行在OpenWrt环境下，采集USB摄像头的图像，并把图像流放到8080网络端口，电脑接入该网络后，获取图像流并在OpenCV环境进行图像处理，识别飞行器前方视野障碍物的距离及坐标信息，通过网络发送到飞行器主控芯片上处理，并做出相应的飞行动作。

2)2.4 G遥控器：通过摇杆遥控手动下发控制指令到飞行器上，可以切换手动控制模式与寻物模式运行，在紧急情况下可以手动控制飞行器飞行，避免不必要的损失与伤害。

3)控制指令解析：飞行器在运行过程中需要接收控制指令才能正常运行，控制指令通过路由器的串口传输与2.4 G接收机的PWM信号传输，传送到飞控的指令解析器，将指令数据传送到控制器。

4)飞行器位置控制器：由GPS、气压计、超声波的测量数据融合出飞行器在地球表面的近似直角坐标，以此反馈信号传输到优化的PID位置控制器进行控制。在三维坐标上不断修正飞行器相对地面的位置，使其相对地面静止，保证飞行器稳定，为稳定的图像提供基础。

5)飞行器姿态控制器：通过读取飞行器上惯性测量单元IMU的数据，使用基于四元数姿态的梯度下降法进行姿态融合，由公式将四元数表达方式转换为欧拉角。反馈得到的欧拉角数据与陀螺仪数据传送到串级PID控制器，进行姿态控制，使飞行器稳定起飞并在空间上保持在一定角度。

1.2 控制系统硬件设计

四旋翼飞行器控制系统的硬件设计分为两部分，一是地面部分，另一部分是飞行器本体部分。地面部分也称非飞行器部分，用来控制和辅助四旋翼飞行器实现相关功能，结构相对简单，主要包括笔记本电脑、中继路由器和一个2.4 G遥控器。飞行器本体部分结构则比较复杂，它是以STM32为主控芯片对各种数据进行处理，并对整个机体进行控制；除与地面部分相搭配的嵌入式路由器和2.4 G遥控器接收机以外，还在主控芯片上搭载了许多外围模块，如超声波测距模块、惯性测量单元(IMU)模块和GPS模块等，此外还在路由器上挂载了一个双目摄像头。控制系统硬件结构如图2所示。

图2 控制系统硬件结构图

1.3 系统电源设计

在四旋翼飞行器系统中，性能稳定是至关重要的，这就要求系统不仅在硬件设计上要合理，选取的器件性能好，保证在电磁波复杂的空中不受干扰稳定运行，而且还要求系统有良好稳定的直流电源，使之在复杂的电源环境中能够滤掉谐波，为各用电器件提供稳定的直流电。

在进行系统电源设计时，需要考虑直流电源品质，如电源能够提供的最大输出电流、纹波抑制比大小、输入系统的电源范围等，只要其中一项不符合要求，都可能使系统供电中断不能正常工作，严重的还会导致飞行器在飞行时坠落。本系统的电源设计框图如图3所示，在电调输出的5 V直流电源基础上，分配电压到系统的各用电器件。

图3 电源设计框图

1.4 IMU模块电路

IMU模块如图4所示，模块集成了运动惯性器件MPU6050、地磁计HMC5883L、气压计MS5611三个传感器，分别采集对应的数据。IMU模块与STM32主控的接线方式如图5所示。为了减小机体震动对IMU的影响，本文将IMU放置在带减震球的板上，并用海绵盖住，减小旋翼气流对IMU的影响[4]。

图4 IMU模块图5 IMU与STM32的接线

1.5 2.4 G遥控器设计

操纵人员通过2.4 G遥控器与飞行器主控进行双向通信。在设计遥控通讯时，需要考虑通信距离、通信速率、无线干扰、操作性等，本设计中选用大功率NRF24L01无线模块，主控通过SPI通讯端口与其进行通信，数据传输速率最高可达到2 Mbps，可以做数据双向透传，而且本通信模块本身设计就是有功率芯片的，这大大地增加了通信的范围，最远可以达到1.5千米，对于飞行器的通信要求来说十分适用。

2 系统软件设计

2.1 系统软件结构

基于双目立体视觉避障的四旋翼飞行器系统软件设计框图如图6所示。整个软件系统分为两部分：飞行器稳定系统和图像处理系统。飞行器稳定系统保证四旋翼飞行器在空中能够稳定飞行；图像处理系统通过读取挂载在飞行器上的双目摄像头采集回来的图像，在笔记本电脑上处理图像信息，进行空间三维重建得出与前方障碍物的距离信息，通过路由器传送到飞行器系统判断飞行器是否需要采取避障绕飞。

图6 系统软件设计框图

2.2 飞行器姿态控制器

在整个稳定系统里，系统需要获取控制飞行器的信息，读取惯性测量单元的数据并进行姿态估算，数据准备好后通过姿态稳定控制器运算出调节飞行器运动输出的控制量，通过不断循环这样的操作使飞行器在空中稳定飞行。

飞行器的不稳定因素主要来自于在飞行过程中周边的气流的不平稳。由于在飞行中每一处的空气密度和气压大小不一致，尽管空心杯输出的转矩、转速一致也不能够保证飞行器能够平稳飞行。串级PID控制的基本理念就是将系统中的外部扰动纳入副回路的调节中。可是扰动亦影响机体角速度变化，这个变化可以由IMU中的陀螺仪检测出来，从而在副回路角速度PID控制器控制角速度从而增加飞行稳定性。在串级PID控制系统的主回路中，我们通过IMU中陀螺仪的角度反馈值去控制飞行器的期望角度保持稳定，从而可以平滑地改变飞行器的飞行姿态。

通过对四旋翼飞行器系统模型的分析可知，四旋翼飞行器是一个典型的非线性系统[5-6]，进行控制时往往需要用相应的非线性控制算法去匹配，但PID控制算法经过优化后可以在四旋翼飞行器上实现稳定控制，并具有良好的控制品质[7-8]，图7所示为运用串级PID实现的飞行器姿态控制。通过PID算法利用偏差去消除偏差的思想，飞行器主控通过遥控器发出的指令以及飞行器自身各个传感器反馈的数据互相结合最后计算出当前实际的飞行状态和我们给定的目标状态的偏差，然后作为串级PID控制器的输入，最终输出PWM控制四个空心杯电机作动力输出。

图7 姿态串级PID控制

2.3 图像处理系统

双目检测障碍物的原理：采集一对平行的单目摄像机的图像，计算同一时刻两幅图像的视差(d=xl-xr)，障碍物三维坐标中的距离Z=fT/d，在OpenCV中，视差d和焦距f的单位是像素，T的单位是根据摄像头标定时棋盘标定板的实际大小和人为输入的数值所决定的，根据检测精度，一般设为毫米级或0.1毫米级。

如若还想计算障碍物的X，Y坐标的话，还需要知道左平面和右平面的坐标系和立体坐标系中原点的偏移量Cx和Cy。通过双目视觉立体标定，可以获取焦距f，摄像头中心矩Tx,左右平面坐标系和立体坐标系的原点偏移量Cx和Cy的初始值，再通过校准优化，计算出视差d，最后求出空间点的三维坐标[X,Y,Z],物体越远视差越小，物体越近视差越大，直接对摄像头视野内的距离进行测量，无论前方的障碍物是什么类型，都可以得到动态变化的距离信息，从而可以让飞行器根据该信息做出相应的行为方案。

双目避障的图像处理流程如图8所示。流程分为双目图像采集、双目图像矫正、双目立体匹配、计算深度信息四大部分。本文通过运行在开源OpenWrt系统的嵌入式路由器采集图像信息，并通过WIFI网络实时传输到笔记本电脑上。因此需要将路由器刷入OpenWrt系统，并在系统里安装免驱动的USB软件包并配置，同时安装Ser2net软件包，配置路由器串口进行控制数据的传输。识别飞行器前方的障碍物的核心算法是基于开源计算机视觉库OPENCV编写的，通过SGBM算法获得图像的视差图从而计算出深度信息，最终获得飞行器前方的障碍物到摄像头的距离以及坐标信息，将结果显示到屏幕上并发送到四旋翼飞行器，飞行器控制器收到障碍物的位置和坐标信息后判断是否需要绕行避开障碍物。

图8 双目避障的图像处理流程

3 结果与分析

3.1 双目测距结果

挂载在飞行器上的双目摄像头采集图像信息，并流放到8080网络端口，笔记本电脑接入该网络后，获取图像流并在OpenCV环境进行处理，识别到障碍物的距离及坐标信息后，通过网络发送到飞行器主控，实现避障功能。经过处理后的图像，可在图9所示的可视化窗口中显示，由图可以看出，左右图像严格保持在同一水平线上。

图9 可视化窗口显示图像

图10显示的是双目摄像头到障碍物的距离以及坐标信息，多次测量得出351.386、351.081、349.559、349.863 mm与实际测量距离350 mm相差1 mm左右，该双目视觉测距系统成功地实现了实时测距功能，并且测距精度在1 mm左右，同时还可看到程序处理每一帧图像数据的时间0.25 s。

图10 双目摄像头到障碍物的距离以及坐标

3.2 姿态解算结果

四旋翼飞行器在飞行过程中，受到大气压、空气密度变化等因素影响，所以稳定的姿态解算是飞行器能否正常运作的重中之重。本文的姿态解算主要是利用惯性测量元件IMU中的加速度计、陀螺仪以及地磁计进行信息融合。具体的实现过程是，先计算出陀螺仪的四元数的变化速率，同时把加速度计和地磁计获取的数据进行归一化处理以增强适应性，计算中间辅助变量，再通过地磁计的参考方向和梯度修正算法进行修正，最后得到四元数的变化速率和变化的四元数范围，进行归一化处理，再根据四元数和欧拉角之间的转换关系得到欧拉角[9-10]。经过基于四元数姿态的梯度下降法解算后，四旋翼飞行器的姿态能够精确地估算出来。图11所示为陀螺仪姿态角的实时波形，解算出来的姿态角能够快速跟踪，且在四旋翼飞行器快速晃动时波形能够平滑地变化，证明用梯度下降法解算的结果能够精确地估算出四旋翼飞行器的姿态。

图11 陀螺仪姿态角的实时波形

通过使用匿名科创地面站上位机，可以方便直观地观察到飞行器飞行时各项参数的动态变化，以便进行数据分析。在应用经验整定法调节PID参数时主要观察陀螺仪姿态角的三个方面的变化，一是稳定性，给定一个期望的平衡角度，观察是否能够长时间稳定在期望角度附近；二是准确性，当通过遥控器发操纵命令时，飞行器能否准确响应，是否会出现过冲；第三是快速性，当操纵命令发出时，飞行器能否快速顺滑地跟随操纵人员的期望动作，操纵与飞行器响应是否存在滞后。

3.3 实验结果与分析

四旋翼飞行器的实物图如图12所示，控制电路板安放在机体上面，用减震板支撑，获取图像的嵌入式路由器板安放在机体中间，双目摄像头安装在机体底部。四旋翼飞行器能够实现一键起飞、定高、定点巡航、双目躲避障碍等功能；由于光流传感器的作用，从地面静止到在空中悬停，可以做到在微小范围内稳定悬飞；控制飞行方向时，飞行器响应速度快，跟随性能好；在GPS定点巡航时，定位精度在cm级，在室外开阔场地能够定位准确。在进行障碍物检测时，辅以双目视觉检测技术，能够利用返回障碍物的位置信息，在正常飞行时能够通过飞行器的方向环调节，使机体绕开障碍物，避障效果良好。

图12 四旋翼飞行器的实物图

4 结束语

本次设计，对基于双目立体视觉避障的四旋翼飞行器从理论构思到系统硬软件分析，通过一步步实践做出实物，并通过编程验证理论，证实了当初的构思是可行的。在双目避障的图像处理方面，本文提及的算法还需要进一步优化，以达到精确的识别效果。