基于双处理器的四旋翼飞行控制系统研究

吴思颖+李亚楠+王年丰+姚远

摘要摘要：为提高四旋翼飞行器工作可靠性，采用STM32F407VGT和R5F100LE处理器，设计了四旋翼飞行器控制系统。加速度陀螺仪MPU6050模块采集飞行姿态数据，超声测距模块、激光传感模块和摄像头模块提供导航参数，采用滤波算法对传感器所采集的数据进行处理，使用PID控制算法实现寻线前进。实验测试表明，飞行器能一键起飞，完成空投任务，精确降落。

关键词关键词：四旋翼飞行器；姿态控制；双处理器；滤波算法

DOIDOI：10.11907/rjdk.162226

中图分类号：TP319文献标识码：A文章编号文章编号：16727800（2017）001004403

引言

四旋翼飞行器是一个多传感器以及众多子系统（导航、通信、飞行控制等）构成的高集成控制系统。近年来，随着嵌入式处理器、传感技术、控制技术的发展，尤其是智能控制算法的应用，其性能得到了极大提高，在测控、巡检等军事和民用领域得到了广泛应用[12]，具有极高的研究价值和应用潜力。

四旋翼飞行器有4个输入量、6个自由度输出，调整飞行姿态和实现室内定位是四旋翼飞行器设计的难点之一[3]。现有的飞行控制系统一般采用ARM7、DSP等高速处理器作为控制芯片。这类单芯片飞控系统，在一个控制周期内要完成数据采集、数据处理、控制运算及指令输出，还需将数据输出到监控系统，实现室内定位和寻迹飞行，会产生过重负荷，导致系统可靠性下降。为提高系统可靠性，本文采用ST公司的STM32F407VGT负责主控，瑞萨单片机R5F100LE处理器负责数据处理，调制PWM（Pulse Width Modulation）信号输出占空比驱动无刷直流电机，姿态传感器（MPU6050集成模块）检测飞行器的加速度和角速度，摄像头寻迹模块检测地面导航线，US-100超声测距模块获取飞行器的飞行高度，运用卡尔曼滤波算法对传感器数据进行分析融合，串级PID算法对飞行器进行控制，实现四旋翼飞行器的双处理器飞行控制系统设计。

1飞行控制系统

本设计采用分工协作机制，由ST公司的STM32F407VGT负责主控，瑞萨单片机R5F100LE负责数据处理以提高控制可靠性。其中STM32F407VGT采用先进的Coretex-M4内核，最高处理速度可达210DMIPS的处理能力，存储空间大，具有浮点运算能力和增强处理指令的优点，带有多种外设接口，如照相机接口、加密处理器和带FIFO的DMA控制器等。而R5F100LE单片机内置高速振荡器时钟，最高频率可达32MHZ，其最短指令执行时间可在高速至超低速间更改，自带单电源闪存，支持自编程功能，内置上电复位、看门狗定时器、按键中断、时钟输出/蜂鸣器输出控制电路等功能，使用极其方便。

四旋翼飞行器的4个动力臂呈十字交叉状固定在四旋翼飞行器中心部件上，每个动力臂末端的电机座上固定一个电机和螺旋桨。通过改变电机转速，可使飞行器在其惯性参考坐标系中产生六自由度运动，包括3个坐标轴方向的线运动（进退、左右侧飞和升降）和3个坐标轴方向的角运动（偏航、俯仰和横滚），可实现在有限区域的垂直起降、穩定盘旋以及精确目标移动等飞行姿态控制[1]。

姿态控制是整个飞行控制的基础。姿态控制系统通过检测飞行器在机体坐标系下3个轴向的角速度、角度和相对地面的高度等相应姿态信息，采用相应的控制算法解算出4个电机的转速，发送给电机调速器调整电机转速，实现对飞行器飞行姿态的调整。

2相关控制方法

四旋翼飞行器姿态调整的控制算法很多，如DI（digital input）控制、PID（比例微分积分）控制以及LQ控制等。其中，PID控制算法具有适用面广、控制参数相互独立、参数选定相对简单等优点[45]，本系统采用PID控制算法实现飞行姿态控制。该算法可以分别计算出飞行器的自转调节量、左右倾斜调节量、油门控制调节量、前后俯仰调节量等，使系统稳定偏差最小，保证飞行器能按照动作指令有效完成指定动作，如寻线和定高飞行等。PID控制系统原理：①比例环节可以准实时成比例地反映控制系统的偏差信号变化；②积分环节主要用于消除静差，提高系统的无差度；③微分环节反应偏差信号的变化趋势（变化速率），加快系统的动作速度，减小调节时间，如图1所示。

理[67]，计算出当前的飞行姿态和地面控制需要的目标姿态，比较实际姿态和目标姿态的差异，估计过程状态，并使估计均方差最小；利用PID算法计算出每个电机的调整量，调整电机转速，从而实现飞行姿态的调整。为了得到稳定、准实时的飞行姿态，以定高飞行为例，利用超声波传回的高度值作为外环，将加速度计及加速度计积分得出的Z轴方向速度值作为内环，外环的输出量解算后成为内环的期望值，内环通过PID控制器将计算得出的量叠加在油门上控制高度，通过调节PID参数值，用先整定内环后整定外环的方法确定PID参数，完成稳定的定高控制。在实际飞行中需调节KP、TI、TD和t这4个常数，这是采用PID算法进行控制的重点和难点。本系统借助实验对这4个常数进行调节，得到一组较可靠的飞行姿态控制数据。借助视频传感器（摄像头）采集图像，通过对图像的二值化处理，统计每一行和每一列的灰度值，算出灰度直方图，根据行（列）的灰度总值变化趋势，分析灰度出现突变的行列位置，得出导航线横向的纵坐标，竖线的横坐标，然后根据坐标进行飞行器的循线导航控制。

3硬件电路设计

系统硬件分为6个部分[67]，如图2所示。超声传感器负责测量高度，和陀螺仪模块一起实现定高控制；视频传感器（摄像头）负责寻迹控制；激光传感器完成电子视高控制；电机模块负责按照主控信号驱动电机；姿态传感部分负责获取飞行姿态。为了提高处理速度，激光传感器和视频传感器传回的数据先传入瑞萨单片机（R5F100LE）计算处理，处理结果作为引导量输出到主控，再由主控使用PID算法完成稳定的寻迹前进，并实现物体的拾取和定点空投。

考虑到超声波传感器具有方向性好、能定向传播的优点，超声测距模块US100被用来测量四旋翼飞行器的飞行高度。该模块使用简单、测量准确，而且能使用串口指令读取数据[810]，与陀螺仪模块结合可校正高度数据，实现定高飞行控制。为实现姿态获取和控制，本系统集成了三轴加速度计和三轴陀螺仪MPU6050模块实现飞行姿态感知，使用卡尔曼滤波处理数据后输出比较稳定的姿态角，输出加速度和陀螺仪的值。数字运动处理（DMP：Digital Motion Processing）引擎可减少复杂的融合演算数据，较容易地获取和矫正飞行姿态；MPU6050模块集成的陀螺仪测量范围为±500dps，加速度计测量范围为±2g。对陀螺仪和加速度计分别使用3个16位的ADC进行模数变化，采集速度快，能以數字输出6轴或9轴融合演算数据并可程式控制，消除加速器与陀螺仪轴间敏感度，降低设定的影响度及感测器飘移。

由于直流电机工作电流大，硬件电路设计时需注意MCU与直流电机的隔离，若隔离措施不到位，容易烧坏MCU的端口。由于主控要连接较多的外围电路，为了减少反接意外情况，将所有供电系统都通过保险丝控制，具体电路如图3所示。

4软件工作流程

四旋翼飞行器软件主要分为3部分：①电机驱动部分。利用处理器的多路PWM输出功能驱动电机；②数据接收和处理部分。获取并处理飞行姿态数据；③姿态控制部分。根据接收到的传感器数据，运用相应算法，使飞行器姿态平衡，实现特定状态飞行。主程序根据要求设定不同的飞行控制模式，具体工作模式流程如图4所示。系统软件工作流程如图5所示。

为验证系统的可靠性和稳定性，分3种情况进行测试：①测试寻迹往返飞行。在飞行高度70cm时，测试降落地与设定中心的最大偏差为38cm；②测试寻迹定高载物定点投放往返飞行。在飞行高度70-79cm时，测试物体落地与定点投放中心的最大偏差为50cm，降落地与设定中心的偏差为38cm，在5次测试中碰触定高线引发报警1次；③测试寻迹定点拾取物体往返飞行。测试降落地与设定中心的最大偏差为40cm。测试结果表明本文设计的四旋翼飞行器飞行高度稳定、姿态解算正确，能实现寻迹、定高飞行、定点投放、拾取重物等多种需求。

6结语

本文采用ST公司的STM32F407VGT处理器和瑞萨R5F100LE单片机，设计实现了四旋翼飞行器控制系统，可实现定向飞行和定高飞行，在规定线路内的自主飞行，并且能够通过小型电磁铁吸取薄铁片飞至指定区域完成空投。测试表明飞行器能一键起飞，完成空投任务，精确降落。

参考文献：

[1]杨云高，鲜斌，殷强，等.四旋翼无人飞行器架构及飞行控制的研究现状[C].第三十届中国控制会议，中国烟台， IEEE Press，2010：448453.

[2]黄智伟.全国大学生电子设计竞赛常用电路模块制作[M].北京：北京航空航天大学出版社，2011.

[3]聂博文.微小型四旋翼飞行器的研究现状与关键技术[J].电光与控制，2007，14（6）：113117.

[4]杨明志，王敏.四旋翼微型飞行器控制系统设计[J].计算机测量与控制，2008，16（4）485490.

[5]白立群，李成铁，周剑峰.基于 STM32 的飞行控制器系统设计[J].计算机应用，2013，32（2）：1533.

[6]HOFFMANN F，GODDEMEIER N，BERTRAM T.Attitude estimation and control of a quadrocopter [C].The 2010 IEEE/RSJ International conference on Intelligent Robots and Systems.Taipei：IEEE，2010：10721077 .

[7]王学斌，徐建宏，张章.卡尔曼滤波器参数分析与应用方法研究[J].计算机应用与软件，2012，29（6）：212215.

[8]李强，林明耀.无传感器无刷直流电机控制系统及其起动分析[J].微电机，2003，36（3）：3335.

[9]周权，黄向华，朱理化.四旋翼微型飞行平台姿态稳定控制试验研究[J].传感器与微系统，2009，28（5）：7279.

[10]黄敦华，朱青松.基于微小型四旋翼飞行器的目标监测与识别综述[J].机电产品开发与创新，2011，24（6）：1618.