四轴飞行器定速巡航系统设计毕设论文.doc

装订线长 春 大 学 毕业设计（论文）四轴飞行器定速巡航系统设计摘要 四轴飞行器作为低空低成本的遥感平台，在各个领域应用广泛。相比其他类型的飞行器，四轴飞行器硬件结构简单紧凑，而软件复杂。本文介绍四轴飞行器的一个实现方案，重点讲软件算法，包括加速度计校正、姿态计算和姿态控制三部分。校正加速度计采用最小二乘法。计算姿态采用姿态插值法、梯度下降法或互补滤波法，需要对比这三种方法然后选出一种来应用。控制姿态采用欧拉角控制或四元数控制。最后比较各种方法的效果，并附上C语言的算法实现代码。关键词 四轴飞行器；姿态；控制 Four aircraft cruise control system designAbstract Quadcopter is a low-cost low altitude remote sensing platforms,Which widely used in various fields.Compared to other types of aircraft,the quadcopter has simpler hardware,result in more complex software.This paper describes an implementation of the quadcopter,including the calibration of accelerometer,estimation of attitude,and controlof attitude.It used the Least Squares Method to calibration the accelerometer.It uses Attitude Interpolation Method,Gradient Descent Method or Complementary Filter Method to estimation attitude.It uses Euler angles or quaternion to control the attitude.Finally,there are comparisons of the methods.The C-language implementation of the methods is appended.Key words Quadcopter；Attitude；Control目 录1. 引言42. 飞行器的构成52.1硬件构成52.1.1机械构成52.1.2电气构成62.2软件构成72.2.1.上位机72.2.2. 下位机73. 飞行器原理73.1.坐标系统83.2. 姿态的表示和运算83.3.动力学原理94. 姿态测量104.1.传感器校正104.1.1.陀螺仪104.1.2.加速度计和电子罗盘114.2.数据融合114.2.1.概述114.2.2.姿态插值法114.2.3.梯度下降法124.2.4.互补滤波法135.姿态控制145.1欧拉角控制145.2.四元数控制156. 算法效果156.1加速度计校正156.2姿态计算157. 结论及存在的问题16参考文献：171. 引言四轴飞行器最开始是由军方研发的一种新式飞行器。随着MEMS传感器、单片机、电机和电池技术的发展和普及，四轴飞行器成为航模界的新锐力量。到今天，四轴飞行器已经应用到各个领域，如军事打击、公安追捕、灾害搜救、农林业调查、输电线巡查、广告宣传航拍、航模玩具等，已经成为重要的遥感平台。以农业调查为例，传统的调查方式为到现场抽样调查或用航空航遥感。抽样的方式工作量大，而且准确性受主观因素影响；而遥感的方式可以大范围同时调查，时效性和准确性都有保证，但只能得到大型作物的宏观的指标，而且成本很高。不连续的地块、小种作物等很难用上遥感调查。因此，低空低成本遥感技术显得相当重要，而四轴飞行器正符合低空低成本遥感平台的要求。目前应用广泛的飞行器有：固定翼飞行器和单轴的直升机。与固定翼飞行器相比，四轴飞行器机动性好，动作灵活，可以垂直起飞降落和悬停，缺点是续航时间短得多、飞行速度不快；而与单轴直升机比，四轴飞行器的机械简单，无需尾桨抵消反力矩，成本低。本文就小型电动四轴飞行器，介绍四轴飞行器的一种实现方案，重点讲解四轴飞行器的原理和用到的算法，并对几种姿态算法进行比较。2. 飞行器的构成四轴飞行器的实现可以分为硬件和软件两部分。比起其他类型的飞行器，四轴飞行器的硬件比较简单，而把系统的复杂性转移到软件上，所以本文的主要内容是软件的实现，特别是算法、公式的推导。2.1硬件构成飞行器由机架、电机、螺旋桨和控制电路构成。2.1.1机械构成机架呈十字状，是固定其他部件的平台，本项目采用的是尼龙材料的机架。电机采用无刷直流电机，固定在机架的四个端点上，而螺旋桨固定在电机转子上，迎风面垂直向下。螺旋桨按旋转方向分正桨和反桨，从迎风面看逆时针转的为正桨，四个桨的中心连成的正方形，正桨反桨交错安装。整体如图2-1。 图2-1 四轴飞行器整机2.1.2电气构成电气部分包括：控制电路板、电子调速器、电池，和一些外接的通讯、传感器模块。控制电路板是电气部分的核心，上面包含MCU、陀螺仪、加速度计、电子罗盘、气压计等芯片，负责计算姿态、处理通信命令和输出控制信号到电子调速器。电子调速器简称电调，用于控制无刷直流电机。电气连接如图2-2所示。 图2-2 四周飞行器电气连接图硬件清单如表2-1 表2-1 四轴飞行器硬件清单2.2软件构成2.2.1.上位机上位机是针对飞行器的需要，在Qt SDK上写的一个桌面程序，可以通过串口与飞行器相连，具备传感器校正、显示姿态、测试电机、查看电量、设置参数等功能。2.2.2. 下位机下位机为飞行器上MCU里的程序，主要有三个任务：计算姿态、接受命令和输出控制。下位机直接控制电机功率，飞行器的安全性、稳定性、可操纵性都取决于它。下位机的三个任务实时性都要求很高，所以计算姿态的频率设为200Hz，输出控制的频率为100Hz，而接收到命令后，立即处理。因为电子调速器接受的信号为PWM信号，高电平时间在1ms2ms之3.1.坐标系统间，所以控制信号输出频率也不能太高。3. 飞行器原理3.1.坐标系统飞行器涉及两个空间直角坐标系统：地理坐标系和机体坐标系。地理坐标系是固连在地面的坐标系，机体坐标系是固连在飞行器上的坐标系。四轴飞行器运动范围小，可以不考虑地面曲率，且假设地面为惯性系。地理坐标系采用“东北天坐标系”，X轴指向东，为方便罗盘的使用，Y轴指向地磁北，Z轴指向天顶。机体坐标系原点在飞行器中心，xy 平面为电机所在平面，电机分布在|x|=|y|,z=0的直线上，第一象限的电机带正桨，z轴指向飞行器上方。如图3-1所示。 图3-1 地理坐标系和机体坐标系图示（坐标系方向重合时）3.2. 姿态的表示和运算飞行器的姿态，是指飞行器的指向，一般用三个姿态角表示，包括偏航角(yaw)、俯仰角(pitch)和滚转角(roll)。更深一层，姿态其实是一个旋转变换，表示机体坐标系与地理坐标系的旋转关系，这里定义姿态为机体坐标系向地理坐标系的转换。旋转变换有多种表示方式，包括变换矩阵、姿态角、转轴转角、四元数等。在本文中，矩阵用加粗大写字母表示，如EAR，左上标和左下标表示从机体坐标系（Aircraft）变换到地理坐标系（Earth）；四元数用加粗小写字母表示，如EAq，上下标意义与变换矩阵一样；向量用带箭头加粗小写字母表示，如Av，左上标A表示向量的值是在机体坐标系的坐标值。因为姿态实质是一个旋转变换，根据刚体有限转动的欧拉定理，旋转变换是可以串联的，所以一个姿态可以经过一个旋转变换，变成另一个姿态。类比点和向量的概念，姿态相当于点，旋转相当于向量，点可以通过加向量，变成另一个点。如果用矩阵表示旋转，旋转的串联由矩阵乘法来实现。如果用四元数表示旋转，则由四元数的乘法来实现旋转串联。用四元数来表示旋转，组合旋转时比用其他方法运算量更少，所以无论在计算机图形学、飞行器控制等涉及刚体旋转的领域，四元数都有举足轻重的地位9。飞行器的姿态计算是围绕姿态四元数进行的，下面简要介绍一下四元数的运算。一个四元数由4个实数组成。 q=wq xq yq zqT (3-1) 规范化的四元数可以表示旋转，见(3-2)式,单位向xw yw zwT为旋转的转轴。 wq=cos(/2) xq=xw *sin(/2) yq=yw *sin(/2) (3-2) zq=zw *sin(/2)=记四元数乘法的符号为。四元数乘法跟矩阵一样，有结合律，没有交换律。运算过程见(3-3)式。 wr = wpwq - xpxq - ypyq - zpzq xr= wpxq + xpwq + ypzq - zpyq yr= wpyq - xpzq + ypzq + zpxq (3-3) zr= wpzq + xpyq - ypxq + zpwq3.3.动力学原理螺旋桨旋转时，把空气对螺旋桨的压力在轴向和侧向两个方向分解，得到两种力学效应：推力和转矩。当四轴飞行器悬停时，合外力为0，螺旋桨的推力用于抵消重力，转矩则由成对的正桨反桨抵消。当飞行器运动时，因为推力只能沿轴向，所以只能通过倾斜姿态来提供水平的动力，控制运动由控制姿态来间接实现。假设四轴为刚体，根据质点系动量矩定理，角速度和角加速度由外力矩决定10，通过控制四个螺旋桨，可以产生需要的力矩。首先对螺旋桨编号：第一象限的为0号，然后逆时针依次递增，如图(3-1)。同步增加0号和1号、减小2号和3号桨的功率，可以在不改变推力的情况下，提供x轴的力矩；同步增加1号和2号、减小0号和3号桨的功率，可以在不改变推力的情况下，提供y轴的力矩；同步增加1号和3号、减小0号和2号桨的功率，可以在不改变推力的情况下，提供z轴的力矩。以上“增加”和“减小”只是表明变化的方向，可以增加负数和减小负数，提供的力矩就沿对应轴的负方向了。把三个轴的力矩叠加起来，就得到各螺旋桨功率变化与提供的力矩的对应关系，可以用一个矩阵等式表示，见(3-5)式。DT是螺旋桨的功率变化量，为41矩阵，每行分别对应0到3号螺旋桨；向量m是力矩，为31矩阵。mx、my和mz是各轴的力矩系数，用于把力矩转换成功率变化量，具体数值与电机力矩特性、电机安装位置等有关。4. 姿态测量获取当前姿态是控制飞行器平稳飞行的基础，姿态的测量要求低噪声、高输出频率，当采用陀螺仪等需要积分的传感器时，还需要考虑积分发散等问题。近年来MEMS传感器越来越成熟、应用广泛，成为低成本姿态测量的首选器件11，因此该项目使用的传感器全部都是MEMS传感器，在使用传感器的值进行姿态计算之前，有必要校正传感器。4.1.传感器校正由于实验条件限制，传感器的校正只有两项，分别对应两种类型的传感器：陀螺仪静止时0输出的传感器、加速度计与罗盘测量某向量场强度的传感器。4.1.1.陀螺仪对于陀螺仪等静止时0输出的传感器，可以很方便地校正零偏。把传感器固定好，这时对输出值Xf求平均，得到的A即为零偏，实际使用时，把测得的值减去零偏，得到的值就是校正值。实际应用的公式如(4-2)，A为零偏值，31矩阵，单位：LSB；Yi为校正好的值，31矩阵，单位：rad/s；Xi为测量原始值，单位：LSB； gain为转换系数，单位：(rad/s)/LSB，由传感器的数据手册给出。4.1.2.加速度计和电子罗盘加速度计和罗盘都是测量所在点的某个向量场的值的传感器，静态时加速度计测的是等效重力加速度场，电子罗盘测的是地磁场。下面仅介绍加速度计的校正，罗盘的校正同理。加速度计测量的对象是比力，也就是等效重力加速度和运动加速度的和，当静止时，运动加速度为0，加速度计的测量值为等效重力加速度，可以利用这一点校正加速度计。加速度计的校正的思路为：对测量值平移和缩放，把测量值拟合到重力加速度。因此校正的任务为：寻找最佳的平移和缩放参数，使总体测量数据的更靠近重力加速度。记测量值为 xm ym zm T，校正后的值为 xc yc zc T，平移参数为 ox oy oz T，缩放参数为 gx gy gz T。 xc = ( xm + ox )*gx yc = ( ym + oy )*gy (4-3) Zc = ( zm + oz )*gz4.2.数据融合4.2.1.概述有了传感器的数据，就可以用来计算姿态了。计算姿态主要用到3个传感器：陀螺仪、加速度计、电子罗盘。加速度计测量对象为比力，受运动加速度影响大，特别是受飞行器机架的振动的影响，振动振幅高达2G。电子罗盘测磁场强度，受环境影响也很大，特别是受电子调速器、电机等大电流器件影响。而陀螺仪则受外部影响弱，稳定性好，但输出量为角速度，需要积分才能得到姿态，无法避免误差的累积。为了得到稳定的、近实时的姿态，对各传感器的数据取长补短，需要研究各种数据融合方法。4.2.2.姿态插值法首先介绍自己构思的“姿态插值法”，过程分成两部分：一、如果已知初始姿态，可以利用陀螺积分，不断推算下一个姿态，这部分动态性能好，但误差会累积；二、利用加速度计和罗盘，可以直接算出一个姿态，这个姿态的期望是正确的，但正如上文所述，这个姿态噪声大，不稳定。为了把两个系统的优点结合起来，对两个系统的结果进行插值，得到的值作为当前的姿态。下面讲“姿态插值法”的计算过程。首先是第一个部分陀螺积分算姿态。陀螺的输出为时间离散的角速度，要对时间积分才得到角度。记陀螺输出的角速度为向量g= xg yg zg T，单位为rad/s，假定采样间隔足够短，在一个采样时间间隔里，角速度不变，且转角足够小，忽略三角函数高阶项，各轴间相互的影响忽略不计，该间隔里的旋转可以用四元数表示成。接着讲第二个部分。姿态的定义为两个坐标系统的旋转变换，因此只要知道两对在两个坐标系对应的向量，就可以把姿态求出来。两对变量为加速度和磁场强度，测量出的加速度和磁场强度是在机体坐标系的，而地理坐标系的加速度和磁场是常量，存在一个旋转，可以把机体坐标系的加速度和磁场强度转换到与地理坐标系的对应的常量重合，这个旋转就是所求的姿态。由于传感器系统误差、噪声等影响，测量出的加速度和磁场的夹角不是恒定值，不可能精确旋转到与常量一样，只能求最接近的旋转。最接近的原则可定为：一、旋转后四个向量共面；二、加速度和磁场成一定角度，旋转后的角平分线与常量的角平分线重合。 图4-1 姿态插值法数据流程图4.2.3.梯度下降法从实际的使用效果来说，姿态插值法已经满足要求，但用了三角函数，即使经过优化，运算量还是很大，不方便应用到低性能MCU或提高运算频率。Sebastian O.H. Madgwick提出了更加有效的方法用梯度下降法计算姿态12。其思路为：陀螺仪不断积分姿态；同时把加速度和磁场强度的误差表示成姿态的函数，令误差最小的点为极值点，然后用梯度下降法逼近误差最小的点，即纠正了陀螺的积分误差。这个方法，陀螺计算的姿态占主要，加速度计和罗盘只是辅助纠正。用陀螺积分姿态的方法跟“姿态插值法”的第一部分一样，关键是如何用加速度和磁场强度纠正误差。首先要定义误差，最直观就是定义为测量值与常量值之差，这样加速度和磁场强度就分别有一个误差了。因为只考虑方向，相减前要规范化。为了减少运算量、利用常量值里的0和1分量，相减之前统一到机体坐标系，而不是地理坐标系。旋转矩阵的逆等于其转置矩阵。( ) 图4-2 梯度下降法数据流程图4.2.4.互补滤波法Mark Euston提出了运算量比梯度下降法更小的互补滤波法13。相比梯度下降法用加速度和磁场强度计算姿态的梯度，互补滤波法是把加速度和磁场强度的误差构造成纠正旋转，叠加到陀螺测出的角增量上，实现高效的数据融合。统一到同一坐标系后，规范化的加速度和磁场强度的测量值和常量值可以作一个叉积，叉积的模为角度误差的正弦，小角情况下认为正比于角度，方向根据右手法则，可以作为纠正的旋转轴，乘上一个系数后，可以与陀螺算出的角增量叠加。D 图4-3 互补滤波法数据流程图5.姿态控制姿态计算出来后，就可以输出控制了。根据被控姿态的表示方式，分为欧拉角控制和四元数控制。控制的思路为：设定一个目标姿态，调整螺旋桨，使测量出的姿态变为目标姿态。为了避免复杂的精确动力学建模，选用PID控制器。5.1欧拉角控制由于欧拉角对应3个轴的旋转，当前姿态和目标姿态的差值可以作为控制输入量，角度的误差直接可以对应力矩的输出。如果当前姿态和目标姿态相差不大，可以忽略旋转顺序的影响。首先把姿态四元数转成欧拉角。欧拉角有多种定义方式，这里定义x轴正方向为航向，绕z轴转的角度为偏航角(yaw)，绕y轴转的角度为俯仰角(pitch)，绕x轴转的角度为滚转角(roll)，旋转顺序为ZYX。具体转换公式见(5-1)。 yaw: ya = arctan( 2wqzq + 2xqyq , 1- 2y2q - 2z2q ) Pitch: pa = arcsin( 2wqyq - 2zqxq ) (5-1) Roll: ra = arctan2( 2wqxq + yqzq , 1- 2x2q - 2y2q )5.2.四元数控制用欧拉角来控制姿态，每次控制都要算3次三角函数，运算量很大。为了避免三角函数，可以直接用姿态四元数来控制。思路跟欧拉角控制一样，先求姿态差，再把姿态差输入到PID控制器，来输出油门变化量。6. 算法效果6.1加速度计校正 注：数据在三个轴正负半轴附近采集，各采16组、共96组数据。6.2姿态计算为了比较几种姿态计算算法的效果，先在下位机采集数据，然后在电脑上离线处理，这样可以用相同的数据进行计算和比较。数据分两组，分别对应静态和动态的情况，测量时电机都是开的，因此把电机振动也考虑进来了。先比较静态的情况。因为几种姿态融合方法的思路都是：陀螺为主、加速度计和罗盘用于纠正陀螺误差，因此动态性能取决于陀螺，静态性能取决于加速度计和罗盘，所以静态的情况最能反映姿态融合算法的优劣，三种方法算出的滚转角，取了其中连续的1000个样点，即连续5秒时间的数据。为了公平比较，先把参数调整到临界值，即刚好能纠正陀螺漂移的值。由图可以看出，姿态插值法和互补滤波法效果差不多，梯度下降法噪声振幅比前两者都大。7. 结论及存在的问题虽然通过使用上面介绍的各种方法，已经成功让飞行器飞起来了，但为了把飞行器应用到遥感，至少还需要三个改进：一、改进控制算法，使机体更平稳；二、增加卫星导航定位，实现预定航线飞行；三、要用增稳云台，遥感传感器要装在云台上。参考文献：1 彭军桥.非共轴式碟形飞行器研究D.上海大学2001级硕士研究生学位毕业论文:1-68.2 吴东国.基于四旋翼飞行器平台的低空遥感技术在公路环境调查中的应用J.公路交通技术.2012.第6期:137-138.3 姬江涛，扈菲菲，贺智涛，杜新武，刘剑君.四旋翼无人机在农田信息获取中的应用J.农机化研究.2013.第2期:1-4.4 赵晨，杜勇.四旋翼无人机在输电线路巡视中的应用J.湖北电力.2012.第36卷第6期:35-36.5 张鹏，程飞，曹宇强，孙来，王琪.一种新型四轴搜救飞行器设计J.科技广场.2010 年09期:145-147.6 王帅，周洋.用于危险区域物品清理的四旋翼飞行抓捕手J.兵工自动化.2011-03.第30卷第3期:78-80.7 刘庆元，徐柳华，沈彩莲，王小平.基于无人飞行器遥感影像的数字摄影测量关键技术研究J.测绘科学.2010.第35卷第1期:28-30.8 岳基隆，张庆杰，朱华勇.微小型四旋翼无人机研究进展及关键技术浅析J.电光与控制.2010-10.第17卷第10期:46-52.9 Philip J.Schneider, David H.Eberly.计算机图形学几何工具算法详解M.第一次印刷.北京.电子工业出版社.2005:633-641.10 李俊峰，张雄.理论力学M.第二版.北京.清华大学出版社.2010:213-216.11 蒋庆仙.关于MEMS惯性传感器的发展及在组合导航中的应用前景J.测绘通报.2006.09:5-8.12 Sebastian O.H.Madgwick, Andrew J.L.Harrison,Ravi Vaidyanathan.Estimation of IMU and MARG orientation using a gradient descent algorithmJ.IEEE International Conference on Rehabilitation Robotics.2011.1:1-7.13 Mark Euston, Paul Coote, Robert Mahony , Jonghyuk Kim and Tarek Hamel.A Complementary Filter for Attitude Estimation of a Fixed-Wing UAVJ.IEEE/RSJ International Conference on Intelligent Robots and Systems.2008.Sept,22-26 :34目 录目 录第一章 总 论1一、项目概述1二、可行性研究报告编制依据和范围2三、项目主要经济技术指标3四、*国家森林公园概况3第二章 项目背景及必要性8一、项目背景8二、项目建设的必要性与可行性10第三章 项目选址分析13一、项目选址13二、项目城市概况13三、经济发展概况14四、公共设施依托条件及施工条件17第四章 需求分析与建设规模18一、*国家森林公园现状与存在问题分析18二、*国家森林公园日容量预测19三、*国家森林公园景区厕所需求面积分析20四、*国家森林公园景区厕所建设规模的确定20第五章 项目建设方案21一、景区厕所工程建设方案21二、景区引水上山工程建设方案27三、基础设施工程建设方案32第六章 环境保护与劳动卫生安全34一、环境保护34二、劳动卫生与安全35第七章 节约能源36一、节能的相关法律及设计规范36二、节约资源措施37第八章 项目实施进度与管理39一、项目实施进度39二、项目管理40第九章 工程招标44一、招标依据及原则44二、项目招标范围及内容45第十章投资估算及资金筹措48一、固定资产投资估算依据48二、投资估算资金筹措方案48第十一章 项目研究结论与建议53一、项目建成后可达到的预期目标53二、结论53三、建议53共 18 页 第 18 页