四旋翼飞行器实时航拍及GPS 定位系统设计

千承辉，杨慧婷，任同阳，李峙雄，滕 飞

(吉林大学仪器科学与电气工程学院，长春130026)

四旋翼飞行器实时航拍及GPS 定位系统设计

千承辉，杨慧婷，任同阳，李峙雄，滕 飞

(吉林大学仪器科学与电气工程学院，长春130026)

针对国内民用无人飞行器功能单一、不易操控等缺点，设计了以Cortex-M3内核为核心、实现图像实时传输与GPS(Global Positioning System)定位并基于PID(Proportion Integration Differentiation)算法控制的多功能四旋翼飞行器。结合建立的动力学模型对飞行器进行姿态分析和控制，采集飞行中的姿态、油门幅值及位置参数等数据并通过无线方式传输到上位机中。对比飞行姿态的理论数据和实测数据，根据算法调整其响应时间短、稳定性高的控制参数，并实现传感器数据的绘图和保存。测试结果表明，飞行器平稳飞行，航拍图像传输距离为110 m，并能实现多功能操控。

四旋翼飞行器;Cortex-M3;PID算法;全球定位系统;航拍

0 引 言

近年来，具有广泛用途的无人驾驶飞行器成为许多国家的研究热点，美国、日本、法国等国均有此类概念研究项目［1］。其4个螺旋桨对称分布，使四旋翼飞行器的机动能力更强，静态盘旋的稳定性更好，也更容易实现机型的微型化，因此特别适合在室内、城区和丛林等近地面环境开展监视、侦查等任务，具有良好的军事和民用前景［2］。面向市场的民用飞行器普遍存在价格昂贵、功能单一、主动控制困难以及不易操纵等缺点，在国内的研究发展一直比较缓慢［3］。为此，笔者设计了一种带有实时航拍和GPS(Global Positioning System)定位功能的四旋翼飞行器，可完成高空作业的任务。为实现四旋翼飞行器的平稳飞行控制，笔者对四旋翼飞行器进行了动力学建模，并在此基础上设计了PID (Proportion Integration Differentiation)控制器。在平稳飞行的同时，GPS模块的定位信息、航拍摄像头采集的图像信息和飞行状态信息无线传输到上位机，通过Matlab的GUI界面清晰显示飞行轨迹和状态参量。

1 原理及算法

1.1 动力学建模

飞行器采用“X型”电动式四旋翼结构，具有较大的机载能力且可灵活调整空间姿态，实现起落、悬停、俯仰、偏航和翻滚等动作。

1.1.1 等效模型的建立

一个系统的独立控制量输入维数若少于这个系统的自由度，则该系统称为欠驱动系统［4］。飞行器是一个具有6个运动自由度和4个可控输入的欠驱系统［5］，所受的动力学拉力f与角速度的平方成正比，且方向恒为正，如图1所示。

旋翼飞行器和固定翼飞行器相比，气动力是非线性的和非定常的，气动环境复杂得多，很难准确地建立其数学模型，忽略次要因素得到近似的数学模型如下［6］

图1 四旋翼飞行器的结构简图及受力分析Fig.1 Structure diagram and stress analysis of the quadrotor

其中ρ为空气密度，CT为阻力系数，Ai为第i个螺旋桨面积，ωi为第i个螺旋桨所属电机的转速，Ri为第i个旋翼浆叶片半径长度。由于螺旋桨的叶片尺寸、材质以及电机性能均相同，所以ρ、CT、Ri可近似为常量参数，式(1)可化简为

得出4个螺旋桨所产生的拉力与电机转动的角速度ωi平方成正比。

对模型进行受力分析，飞行器的总拉力u是4个单独旋翼拉力之和，而俯仰力矩τθ是电机M2与电机M3旋翼拉力之和与电机M1与电机M4旋翼拉力之和的差的函数，即

其中l为电机相邻两轴连线的中心点到飞行器重心轴的垂直距离。侧向翻滚力矩τΦ原理同俯仰力矩，是电机M1与电机M2旋翼拉力之和与电机M4与电机M3旋翼拉力之和的差的函数，即

水平偏航力矩τΨ是每个电机旋翼为克服空气阻力而产生的，是转动轴加速度和旋转叶片阻力产生的反作用力矩τMi之和，即

设电机的阻力转矩为τdrag，由转动力矩的定义

其中Irot是电机机转子绕它自身轴旋转的转动惯量，ωi为电机转子的角加速度量，τMi-τdrag为合成力矩。根据空气动力学的阻力矩定义

其中vi为第i个旋翼叶片与空气流相对的速度，可简化为

其中kdrag是依赖于空气密度的常数。对于准平稳飞行器的飞行运动，ω为常量，最终可得

1．1．2 飞行姿态简述

1)垂直飞行:同时增加4个旋翼电机的输出功率，使输出转速升高，拉力u变大，克服自身重力的能力变强，当u＞m g时，飞机向上飞行(见图2)。

图2 飞行器垂直飞行Fig.2 Vertical flight

2)前后飞行:控制电机M2和电机M3转速同时增加，合力f2+f3的值增大，控制电机M1和电机M4转速减小，合力f1+f4的值减小，即可提供向前的水平分量。

3)翻滚飞行:原理同前后俯仰运动，控制电机M1和电机M2转速同时增加，合力f1+f2的值增大，控制电机M4和电机M3转速减小，合力的值f3+f4减小，当(f1+f2)-(f3+f4)＞0，即可提供向右的水平分量。

4)偏航飞行:可以控制电机M1和电机M3，使上升拉力f1和f3同时增加，同时，控制电机M2和电机M4的转速减小，使上升拉力f2和f4同时减小，保证飞行器在空中的4个旋浆总拉力u=m g，此时可使飞机做水平逆时针运动。

1．2 四元数法

四元数是控制过程中最主要的算法，姿态解算的精度是实现稳定飞行的前提，小型四旋翼飞行器多选用微机电系统(MEMS)器件，其精度低、易发散，制约了飞行器稳定的航行特征［7］。为此使用四元数补偿算法，该算法简单有效，对惯性器件的精度要求较低，使经过处理后的姿态值在一段时间内保持稳定。使用四元数表示的旋转可通过求解四元数微分方程求得［8］，利用四元数法获取姿态欧拉角的步骤如下。

1)初始化四元数

2)从陀螺仪获取加速度、角速度:三向加速度ax、ay、az，三向角速度gx、gy、gz。

3)将三轴加速度计得出的3个加速度值转换为3维的单位向量(规范化)，结果为

ay和az的求法同上。

从四元数中获得3轴的重力量

4)向量外积再相减得到差分就是误差，作为陀螺仪的修正量，由e=a×y得

5)利用得到的误差对陀螺仪的测量值进行修正

eyint和ezint的求法同上。

gyint和gzint的求法同上。

6)利用修正后的陀螺仪数值更新四元数

7)将采集后的四元数进行归一化

q1、q2和q3的求法同上。

8)上述1)～7)步完成更新四元数，作为下一次运算时的初始四元数，将四元数转换为3个欧拉角，完成姿态的初步解算。

1.3 PID控制算法

飞行器使用PID控制器。四旋翼是非线性系统，在此可近似为线性系统［9］。PID控制器是在工业控制中常见的反馈回路部件，由比例单元P(使四旋翼飞行器能够产生对于外界干扰的抵抗力矩)、积分单元I (让飞行器产生一个与角度成正比的抵抗力)和微分单元D组成，PID控制器的时域微分方程为

其中Kp为比例系数，Ti为积分时间常数，Td微分时间常数，u(t)为控制量。通过后向差分法离散化后，得到离散化的PID控制器方程为

图3 典型PID控制系统结构框图Fig.3 A typical PID control system structure diagram

1.4 卡尔曼滤波算法

1)建立飞行器姿态模型。卡尔曼滤波是一种利用线性系统状态方程、通过系统输入输出观测数据、对系统状态进行最优估计的算法。观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。

状态方程

输出方程

其中k为时间系数，A、B、C为矩阵，x为状态变量，u为系统的已知输入，y是输出信号，q为进程噪声，r为测量噪声［11］。

2)卡尔曼滤波算法的实现。在飞行器飞行过程中采样和运算时间d t很小，所以将三轴角度的变化看作线性变化，卡尔曼滤波将从加速度传感器和陀螺仪获取的角度数据融合处理，以确保姿态估算的精度［12］。观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程［13］。

对陀螺仪数据进行滤波的过程如下。

①获得当前陀螺仪数据，根据加速度传感器获得的数值计算角度值。

②根据系统模型，通过前一刻的系统状态估算当前的系统状态

③更新协方差

⑤得到当前状态的最优估计

⑥更新系统协方差

2 硬件设计与仿真

2．1 主体构成

承载机身采用大疆DJI风火轮F450机架，它具有价格低廉、强度高、拓展接口丰富等优点;动力系统采用电子调速器和无刷直流电机配合的方式。电子调速器采用无感换向方式将直流电逆变成三相交流电输出给无刷电机［14］。GPS定位模块选用定位精度较高的UBLOX系列LEA-6H。无线收发模块采用工作于2.4～2.5 GHz ISM频段的NRF2401。航拍摄像头采用SONY 700线高清摄像头，重量12 g。无线图传模块采用微型TS5823，发射频段5.6～5.9 GHz，发射功率200 mW，重量7.3 g，理论发射距离200 m。接收部分采用RC805接收机，输出端通过AV线与图像显示器相连。

2.2 主要芯片使用

飞行器对控制器的要求比其他控制系统高，整个运算过程需要足够快的采集速度、较短的响应时间和较低的功耗，故选用STM32F103增强型系列VCT6，它是ST公司以Cortex-M3为内核且基于ARMv7体系结构的处理器［15］。检测偏角的传感器采用MPU6050。它在实际使用中精度高，采集和运算速度快，满足飞行器在飞行中对姿态灵敏感知的要求。在飞行的过程中需要检测飞行器的朝向，以确定飞行的偏航角，故选择3轴数字罗盘HMC5883L。这款电子罗盘误差可以控制在1°～2°，并具有简易的I2C系列总线接口，易于使用。

2.3 硬件总体框架

将以上各组件按照功能划分，系统一共分为五大部分，即主控部分(见图5)、GPS信号发射部分、视频采集部分、USB连接上位机部分(见图4)和图像接收部分。以上5个系统组成一个数据网络。

3 程序设计与软件编写

图4 USB连接上位机设备Fig.4 USB connectwith PC

3.1 核心程序的编写

图5 主控部分框图Fig.5 Main control block diagram

1)姿态控制程序。系统控制语言采用C语言编写，主要功能是接收遥控指令，对飞行器的飞行姿态进行控制，同时通过采集飞行器姿态信号，对飞行器进行PID闭环控制。控制程序中采用内环PD加外环PID的双闭环控制方法，将内环进行两次PD调节得到的角速度PID输出值给到外环进行一次PID调节得到角度PID值，构成一次完整的PID调节。在四旋翼飞行器通过姿态解算和滤波后，输出角与初始角之间存在偏差，通过调节4个电机转速使偏差逐渐减小，这个过程就需要PID控制。以俯仰角(pitch)为例。

设初始时静态值为pitch_offset，通过传感器获得的实时值设为PITCH，则偏差为e(k)=PITCH-pitch _offset。对偏差进行积分时，pit_increment+=e(k)，由于容易产生积分饱和，所以需要进行积分限幅|pit _increment|＜=pit_increment_max，又因为微分为e(k)-e(k-1)，则PID控制的输出为Pid_out=kpe(k) +kipit_increment+kd［e(k)-e(k-1)］。翻滚角与偏航角同俯仰角的PID控制思路一致。最终将俯仰、翻滚以及偏航3向PID的输出值再结合油门输出给电机的4路PWM波，控制4个电机的转速。由于四旋翼飞行器4个电机和机体飞行时产生的振动以及三轴角速度ω数据包含大量的噪声，而陀螺仪本身受噪声影响较小，所以必须对加速度计的数据进行卡尔曼滤波，消除噪声。再和陀螺仪的数据进行融合，以消除加速度计的累计误差。

图6 接收部分流程图Fig.6 The receiving part of flow chart

2)GPS定位模块程序。GPS模块通过卫星定位会获得7种模式的数据信息，采用推荐模式“GPRMC”。首先判断一下接收到的数据是否为推荐模式，然后再将所接收到的ASCII码数据通过串口读入MCU，同时结合航拍的图像实时地传输给上位机进行轨迹分析。

通过将NRF2401的通道0设置为飞行数据通道，通道1设置为 GPS数据通道(如图6)，可实现NRF2401“多发单收”的功能，即多路设备发送数据，单路设备接收数据，提高程序的运行效率及数据更新的实时性。

3.2 上位机软件编写

利用Matlab的GUI功能可实现对数据的采集和处理。连接口通过串口对象来建立，通过调用串口对象支持的函数和设置串口对象的属性，用户可自行配置串口。然后将串口的每位数据进行处理和换算，计算坐标并以离散点的形式显示。

4 实际测试与结果分析

4.1 卡尔曼滤波算法参数调整

通过将采集的飞行器姿态数据和输出时域曲线的对比，利用经验法对卡尔曼参数进行适当调整(其中Q为过程噪声，Q增大，动态响应加快，收敛稳定性降低;R为测量噪声，R增大，动态响应减慢，收敛稳定性提高)，相关程序字段如下:

#define KALMAN_Q 0.02 #define KALMAN_R 6.0000

在检测卡尔曼滤波可行性时，采用随机输入波形，并加入高斯白噪声和模拟的过程噪声，卡尔曼滤波效果经过Matlab仿真如图7所示。

由图7可以看出，输出波形可近似反应输入波形的特点，卡尔曼滤波器有效地去除了白噪声和过程噪声。当波形出现骤升或骤降时，通过卡尔曼滤波后波形依然稳定，并不会出现大幅度变化，可见设定的卡尔曼滤波参数是可以采用的。

4.2 飞行姿态算法参数调整

可将飞行器每一时刻的飞行姿态分解为横滚角，俯仰角和偏航角，针对每个自由度，都是一个二阶系统。对着3个变量分别采用PID控制，这样将复杂的非线性MIMO控制问题简化为两变量输入单输出状态［16］。

通过USB连接设备上的液晶屏幕可以显示飞行器飞行时的油门参数、姿态角和GPS定位数据，屏幕功能界面如图8所示。

图7 卡尔曼滤波效果仿真Fig.7 The simulation kalman filtering effect

图8 屏幕功能界面Fig.8 Screen function

针对飞行器稳定悬停的姿态角可适当地对飞机的PID参数进行调整。通过经验法更改内环PD和外环PID的参数，程序中参数设置如下。

ctrl.roll.shell.kp=2;ctrl.roll.shell.ki=0.01;ctrl.roll.shell.kd=0.6;ctrl.roll.core.kp=1.8;ctrl. roll.core.kd=0.22;

在更改参数可以对姿态的稳定性加以调整，直到飞行器稳定悬停时偏航角和翻滚角的值均为“000”时为止。数据记录如表1所示。

表1 飞行姿态调整记录表Tab.1 Flight attitude adjustment form

4.3 实测GPS采样数据图形化轨迹

图9 GUI仿真结果Fig.9 GUIsimulation results

基于Matlab软件的GUI编程，将串口接受到的坐标数据进行显示，得到方位坐标。图9为飞行器近似悬停姿态飞行时得到的结果。由测试结果可以看出，此时飞行器所处的方位大概在北纬43.880 7°，东经125.301 2°上。GPS信号出现漂移，但误差在GPS模块的测量精度范围内。GUI图形化界面对于位置信息可清晰直观地显示，省去了对数据复杂换算和处理的步骤。

4.4 实时图像采集结果

无线图传模块采集飞行过程中的图像，并通过液晶屏显示(见图10)，配合由STM32控制、MPU6050作传感器、无刷电机带动的二自由度云台，使飞行器以任何姿态飞行时，摄像头始终保持水平，优化了航拍效果。小型TFT屏幕的图像与上位机屏幕的截图对比如图10所示。

屏幕效果可见，航拍系统可以清晰、真实地反映飞行器所处环境的情况，效果良好，符合预期目标。实验整体作品如图11所示。

图10 飞行屏幕效果Fig.10 Flight screen effect

图11 实验作品Fig.11 Experimentalworks

5 结 语

四旋翼飞行器的实时航拍及GPS定位系统，不仅可以实现飞行器的平稳飞行、姿态控制，还能通过摄像头实时监控飞行环境，GPS模块采集飞行器位置数据，使飞行器的功能更加全面，实现了小型无人机的人性化和实用化。

四旋翼飞行器的实时航拍及GPS定位系统针对市场上销售的民用无人航拍机昂贵、不稳定、功能单一、不易调试等缺点，采用了经济、可靠的硬件方案和高效、节能、稳定的软件方案，使飞行器具有低成本高性能的优点，可为市场上中小型航拍无人机系统提供一定的理论依据。

［1］洪森涛，金智慧，李志强.四旋翼飞行器建模及姿态稳定性分析［J］.电光系统，2008(2):34-37. HONG Sentao，JIN Zhihui，LIZhiqiang.Quadrotor Modeling and Analysis of Posture Stability［J］.Electro-Optical System，2008(2):34-37.

［2］岳基隆，张庆杰，朱华勇.微小型四旋翼无人机研究进展及关键技术浅析［J］.电光与控制，2010，17(10):46-52. YUE Longji，ZHANG Qingjie，ZHU Huayong.Micro Quadrotor Reaserch Progress and the Key Technology Anlysis［J］. Electronics Optics and Control，2010，17(10):46-52.

［3］聂博文.微小型四旋翼飞行器的研究现状与关键技术［J］.电光与控制，2007，14(6):113-117. NIE Bowen.Research Status and Key Technology of Microminiature Quadrotor［J］.Electronics Optics and Control，2007，14(6):113-117.

［4］孙宁，方勇纯.一类欠驱动系统的控制方法综述［J］.智能系统学报，2011，6(3):201-207. SUN Ning，FANG Yongchun.A Class of Underactuated Systems Control Method［J］.CAAI Transactions on Intelligent Systems，2011，6(3):201-207.

［5］SUKTHANKAR R.PCA-SIFT:A More Distinctive Representation for Local Image Descriptions［C］∥Proceedings Conference Computer Vision and Pattern Recognition.Washington，DC:IEEE，2004:511-517.

［6］李俊，李运堂.四旋翼飞行器的动力学建模及PID控制［J］.辽宁工程技术大学学报，2012，31(2):114-117. LI Jun，LI Yuntang.Modeling and PID Control for a Quadrotor［J］.Journal of Liaoning Technical University:Natural Science，2012，31(2):114-117.

［7］马敏，吴海超.基于四元数自补偿四旋翼飞行器姿态解算［J］.制造业自动化，2013，23(12):18-21. MA Min，WU Haichao.Quadrotor Posture Calculating Based on Quaternion Compensation［J］.Manufacturing Automation，2013，23(12):18-21.

［8］高钟毓.惯性导航系统技术［M］.北京:清华大学出版社，2012. GAO Zhongyu.Inertial Navigation System Technology［M］.Beijing:Tsinghua University Press，2012.

［9］庞庆霈，李家文，黄文浩.四旋翼飞行器设计与平稳控制仿真研究［J］.电光与控制，2012，19(3):258-259. PANG Qingpei，LIJiawen，HUANGWenhao.Designment of Quadrotor and Imitation Research of Trim Control［J］.Electro-Optics and Control，2012，19(3):258-259.

［10］黄友锐，曲立国.PID控制参数整定与实现［M］.北京:科学出版社，2010. HUANG Yourui，QU Liguo.PID Control Parameters Setting and Implementation［M］.Beijing:Science Press，2010.

［11］周雄瑞，陈梧桐，陈明春，等.Kalman Predictor and Multitarget Tracking Algorithm［J］.中华技术学院学报，2003，26(5):22-36. ZHOU Xiongrui，CHENWutong，CHEN Mingchun，et al.Kalman Predictor and Multitarget Tracking Algorithm［J］.China Institute of Technology Journal，2003，26(5):22-36.

［12］SONG Qiang，FENG Jiangtao，ZHANG Xiaochen，et al.Obtaining the Real Time Yaw Angle Using Electronic Compass in Quadrotor Systems［C］∥International Conference on System Science and Engineering.Dalian，China:［s.n.］，2012:238-242.

［13］UNGARALA S.On the Iterated Forms of Kalman Filters Using Statistical Linearization［J］.Journal of Process Control，2012(5):125-132.

［14］孙建忠，白凤仙.特种电机控制［M］.北京:中国水利出版社，2005:37-40. SUN Jianzhong，BAIFengxian.The Special Motor Control［M］.Beijing:China WaterPower Press，2005:37-40.

［15］李宁.基于MDK的STM32处理器开发应用［M］.北京:北京航空航天大学出版社，2008:1-4. LINing.Development and Application of STM32 Based on MDK［M］.Beijing:Beihang University Press，2008:1-4.

［16］HOFFMANN F，GODDEMEIER N，BERTRAM T.Attitude Estimation and Control of a Quadrocopter［C］∥IEEE/RSJ International Conference on Intelligent Robots and Systems.Taipei:IEEE，2012:1612-1616.

(责任编辑:张洁)

Real-Time Aerial Photography and Global Positioning System for A Quadrotor Design

QIAN Chenghui，YANG Huiting，REN Tongyang，LIZhixiong，TENG Fei

(College of Instrument Science and Electrical Engineering，Jilin University，Changchun 130026，China)

Because of single function and difficult control of the current unmanned aerial vehicle，a multifunctional quadrotor aircraft，based on PID(Proportion Integration Differentiation)control algorithm，is designed to realise real-time aerial photography transmission and GPS(Global Positioning System)locating in the core of Cortex-M3，controlling the posture of the quadrotor aircraftby the angle of yaw，roll，pitch，acquiring the data including flight posture，throttle range and position parameter，building a dynamicalmodel.Added with the wireless transmission，the data measured practically are transmitted to upper computer，compared with theory data.Then the controlling parameter is adjusted to shorten response time，steady the flight in accordance with algorithm and accomplish plotting and saving with the sensors data.The result shows that the quadrotor can fly steadily with multifunctional control and the distance of real-time aerial photography transmission can reach 110 meters.

quadrotor;Cortex-M3;proportion integration differentiation(PID)control algorithm;global positioning system(GPS);aerial photography

TP242.6

A

1671-5896(2015)04-0454-09

2015-03-27

国家大学生创新实验计划基金资助项目(2014A65299)

千承辉(1975— )，女(朝鲜族)，吉林汪清人，吉林大学高级工程师，博士，主要从事传感器与智能仪器研究，(Tel)86-431-88502381(E-mail)qianch@jlu.edu.cn;通讯作者:任同阳(1993— )，男，长春人，吉林大学本科生，主要从事传感器与智能仪器研究，(Tel)86-18844193015(E-mail)864753404@qq.com。