四旋翼无人机的导航与控制

U.D.C.:627.3.387DissertationfortheMaster’sDegreein Prof.WangChanghongAcademicDegreeAppliedfor： MasterofEngineering ControlScienceandEngineering Dateof July,Degree-Conferring-Institution：HarbinInstituteof 四旋翼无人机是一种新型的可垂直起降的小型无人机，它通过改变四个对称分布的旋翼转速来调整姿态与位置，具有可悬停、机动性好、结构简单等多种优点，具有十分广阔的应用前景。首先对飞行机理进行了分析，设计了以碳纤维为主体材料的机械结构。并以机械结构为基础，建立了六自由度的无人机数学模型，根据实际的控制要求对模型进行了简化。分别设计了PID姿态控制器和Backstep姿态控制器对无人机的姿态控制进行了仿真，得到了在阶跃控制信号输入、脉冲干扰力矩输入以及阵风干扰力矩输入下的姿态角响应曲线，验证了两种算法的有效性。仿真结果表明Backstep方法对四旋翼无人机这种非线性强耦合的系统具有相对较好的效果。其次，根据四旋翼无人机对航姿信息的要求设计了以MEMS陀螺仪，加速度计与磁阻传感器为基础的航姿系统，设计了一种基于扩展Kalman滤波的航姿估计算法。采用四元数作为状态变量，利用陀螺仪的输出构成状态方程，通过加速度计与磁阻传感器构造观测方程来进行扩展Kalman滤波解算。相对于通常的航姿估计算法，这种算法减少了运算量，更适合于基于嵌入式微处理器的航姿估计。实验结果表明这种算法在静态情况下可以较好的得到无人机的姿态输出，动态性能还需要进一步测试。最后，根据系统性能和功能的要求对无人机的机载主控制系统与地面站系统进行了设计。并进行了悬停、姿态信号、测试等一系列飞行试验，对试验结果进行了分析，验证了算法的有效性。：四旋翼无人机；PID；Backstep；航姿系QuadrotorisanewmodelofsmallUAVsthatcouldtakeoffandlandvertically.Itadjustsitspositionandattitudebyvaryingfoursymmetricalpropellers’speeds,withmeritssuchashoveringability,goodmaneuverability,andsimplifiedstructure.Thepaperfirstyzedtheflightprincipleanddesignedthestructureofmicroquadrotorbyusingcarbonfiber.Thenittoestablishedthequadrotormodelbasedonmechanicalstructurewith6degreesof dom,andsimplifiedthemodelduetopracticalcontrolissues.ThispaperdesignedPIDattitudecontrollerandBackstepattitudecontroller.Throughthesimulation,wegottheattituderesponsecurvesunderseveralconditions.Thesimulationresultsprovethefeasibilityoftwoalgorithms,andindicatethatBackstepmethodwasbetterinthecontrolofquadrotorsystemwhichwasnonlinearandhighlySecondly,duetotherequirementsofquadrotoronheadingandattitudeinformation,thispaperdesignedanattitudeandheadingreferencesystembasedongyros,acceleratorsandmagneticsensors,anddesignedextendKalmanfiltertoestimateattitudeandheadinginformation.Inthefilter,thequaternionwaschosenasstatevariables,gyroswereusedtoupdatethestatsvariables,andacceleratorsandmagneticsensorswerecombinedtoestablishmeasurementequation.Incomparisonwiththetraditionalattitudeestimationmethod,thisalgorithmcangreatlyreducecomputationalcomplexityandwasmoresuitabletoattitudeestimationbasedonembeddedmicroprocessor.Experimentresultsdemonstratedthatthistypeofalgorithmwaseffectiveinattitudeestimation.Last,thepaperdesignedmaincontrollerandgroundcontrolsystemforquadrotorbasedonthesystematicandfunctionalrequirements.Flighttestsincludehovering,attitudetracingandanti-interferencehoveringwereconductedandprovedtoverifytheeffectivenessofthecontrolalgorithm.:Quadrotor,PID,Backstep, 第1章绪 研究的目的与意 国内外研究状 四旋翼无人机的研究状 小型无人机导航系统研究状 本文主要研究内 第2章四旋翼无人机的机理分析与模型建 四旋翼无人机飞行机理分 四旋翼无人机机体结构设 四旋翼无人机运动方 本章小 第3章四旋翼无人机航姿系 基于EKF航姿系统算 状态方程建 观测方程建 EKF姿态解 航姿系统硬件与软件实 航姿系统硬件设 航姿系统软件流 实验结 本章小 第4章四旋翼无人机姿态控制算法设 控制结构分 基于PID的姿态控制算 PID控制结构分 第1随着科技信息时代的发展，未来信息化条件下的将对的高射与其他相比有更好的优越性。其设计灵巧，空间利用率高，可重复使用，2001年，中，飞行器第一次扮演者，逐渐由“配角”转变的思考。2007年4月3日，英一架改装的“旋风”F2成功遥控4架飞行器，实施协同，完成搜索和摧毁目标的任务。由于无人机在、中担负了重要的、监视、侦察不同型号的无人机，其国正在使用的至少有18种型号，总数超过已将无人机看作未来空袭和全球中的利器，持续斥巨资开展飞行器相关技术的研究工作。陆、海、空三军以及国防高级研究计划局预计在2005—2009财年投入16.62亿进行数十个无人机系统的技术研发作。英、法、德等国对飞行器的投入也非常大。国际宇航公司预测，到年全球飞行器市场的份额将达到100亿欧元，全球将拥有12万架无人[4-5]近年来，由于军事模式的转变，获得战场信息，争取信息主动权越来山区环境中战斗小分队提供及时的信息保障，因此需要无人飞行器具有能够垂直起降和悬停的能力。目前正在使用的飞行器系统大都采用常规固定翼气动布局；采用滑跑、手掷或者的发射装置弹射起飞；采用掠飞或者盘旋等方式对目标进行侦察、监视。在城市环境中，传统的飞行器较难从复杂多变的楼宇间获取战术信息。因此，一种能够从狭小地域上放飞、在复杂空情中灵活机动的战术飞行器成为飞行器领域研究的热点。小型四旋翼无人机能够垂直起降和悬停，能够适应复杂的城市，山区起降环境，具有“悬停并凝视”目标的能力，而且还可以抵近建筑物飞行，对目标物提确定位。并能依靠四个旋翼产生的升力进行姿态与位置控制，且其结构紧凑，机动性能良好，具有较强的适应能力，应用范围广，因而成为无人机的研究热点之一。停监视，甚至可以降落到建筑物上对地面状况进行观察。较小的电机与风扇，四旋翼无人机的机构更加紧凑，而且它起飞无需跑道，应用前景。军事上可应用于获取、地面战场侦察和监视、近距离空中支持然之后的搜索与救援，巡逻监视和目标，缉毒和反，高压线、大促进有价值的二次开发。例如可以结合现有的视觉技术开发视觉定位与定姿系统，进一步提高测量的精度6-9]；可以以此为基础开发无线能量传输技术10]，从根本上解决微小型小型无人机的能源问题等。由于该无人机模型具有非线性和耦合性11，同时由于气动参数摄动，该模型还有不确定性，它的控制问题具备了当前控制问题中的各种难点，因此它能够提供各种控制方法的验证平台。结构简单、使用常规部件和能源、方便等优点，使其制造和使用成本相对综上所述，开展小型四旋翼无人机的理论和技术研究在开展学术理论研究、推动相关产业和技术发展、加强国防建设、创造经济效益等方面都具有重大的意义。四旋翼无人机的研究OS4是EPFL自动化系统开发的一种小型四旋翼飞行器，研究的重全自主飞行。目前，该项目已经进行了两个阶段。OS4I最大长度约73cm，质量为235g；它使用了DraganflyerⅢ的十字框架和旋翼，电机型号为Faulhaber1724，微惯性测量单元为Xsens的MT9-B。研究人员通过万向节将它固定于飞试平台之上，使其只具有3个转动自由度；电机驱动模块、能源供给、数据处理、以及飞行控制单元都由飞行器外部提供；至2004年，已经分别验证了多种控制算法[12-13](例如：PID、LQ、Backstep、Sliding–mode)，都很好的实现了飞行器姿态控制。OS4II的机身最大长度72cm，重520g；机230g的锂电池，能提供自主飞30min的能量。它与OS4I的区别主BLDC；使用皮带装置代替了电机箱；控制器、传感器、电池和电机驱动模块等都直接安装在机体上，不再由机体外部提供。2006年1月，EPFL已经实现了OS4II在室内环境中基于惯导的自主悬停控制。HMX4[14]76cm700g5个彩色标记。地面摄像头通过并测量标记的位置与面积，获得飞行器的位置和3个姿态角，角3轴陀螺仪测量获得，主要进行飞行器姿态增稳控制。研究人员将整使其只能在水平面内运动，已经实现了自主悬停控制，使用Backstep控制算法达到了较好的效果。之后，HMX4研究人员又开发了一套基于地面和机载双头的视觉定位与定姿系统，进一步提高了测量的精度。这种基于视觉的图1-1OS4 图1-2OS4Fig.1-1OS4 Fig.1-2OS4GTMARS[15]是佐治亚理工大学面向火星探测任务而设计的无人机系统。20kg0.92m30min。GTMARS开始为折叠状态，陆器登陆火星后，机构将自动展开；它能自主起飞和降落，巡航速度可达72km/h；当能量不足时，它可以返回到着陆器补充能量，着陆器装载有能德国Microdrones公司开发出了一款小型四旋翼无人机，型号为MD4-200。这个无人机采用MD公司自行设计的盘式电机，机身全部用炭纤维制作，悬停时电流参考值为2A，装有GPS，并配有相应的地面站系统，以及传输系统。图1-3 图1-4Fig.1-3 Fig.1-4EADSQuattrocopter[16]也是一款飞行效果较好的四旋翼飞行器，重约0.5千克，长约65cm。控制系统由主处理器，MEMS惯性测量单元，压差传感器，GPS接收装置等组成，最长飞行时间可达到20分钟。 图1-5MD4- 图1-6EADSFig.1-5MD4- Fig.1-6EADStnfrd大学使用一个改进后的Drngonflyer作为其自主飞行器ultiAgnt控制研究（SC）的测试平台。选择Drgnflyer四旋翼无人机进行方案验证。Drgnflyr原先的被tnfrd大学自主设计的控制器所取代。这款控制器用于执行所有的传感和通讯任务，它由如下组建构成：一个被称为icrotrin的IU，两块PIS微控制，一个超声速声纳定位传感器，一个PS单元和一个有效距离为150300由几台P机和一个配有用于飞行器的标准杆的笔记本电脑组成。IU从传感器得到相关数据后估算出当前飞行器的高度及其变化速度，然后将这两个数据输出，由于飞行器的支架在升力很大时会有，这两个数据很可能混有较大的噪声，因而准确性较低。系统同时加入了一个红外距离传感器包括微控制、导航系统、各种测距传感器以及视觉辅助设备。控制方法比较多样，PID、LQ、Backstep、Sliding–mode等都有应用。小型无人机导航系统研究，准确的航姿信息是小型无人机控制的基础，姿态方位参考系统(AttitudeandHeadingReferenceSystemAHRS)就是专门为小型无人机提供三轴姿Crossbow公司的AHRS500GA是一种高性能、全固态的姿态、航向测量系统。广泛应用于航空领域。这种高可靠性、的惯导系统提供了在静态和动态两种状态下的姿态、航向测量。AHRS500GAMEMS陀螺和加速度计，通过使用其独有的Kalman滤波算法，测定出动态、静态两场为参照提高了陀螺对其漂移的纠偏功能。数据输出为数字RS-422串行数据总线。外部不需要其他的配件。其静态精度俯仰角和横滚角<0.5°、航向角Innalabs公司的InnalabsAHRS[18]是一个高性能的捷联惯导系统，可以在3D空间中测量任何载体的全角度姿态。InnalabsAHRS根据地球重力矢量，相对磁北的航向(方位角)来估计横滚俯仰角度。利用国家地理数据中心和英国地理机构制造的世界磁力模型，来针对地理北向自动计算真北。InnalabsAHRS对于静止或带加速度的任何运动物体可以进行高精度的定位，短时间的磁干扰也不会影响AHRS的航向精度。特别设计的软件可以对Innalabs荷兰Xsens公司的MTI系统[19]是一个微型的测量姿态和航向的系统。速度以及磁场强度，使用Kalman滤波来对姿态进行估计。MTI是一款性能非分辨率为0.05°，外型尺寸只有58mm×58mm×22mm。要都是应用MEMS传感器，通过加速度计、陀螺仪与磁场强度传感器来进行数据融合来得到较好的姿态信息，数据融合的方法大多数使用Kalman滤波，测量精度一般也在0.5°～1°的范围内。基于EKF的航姿系统算法，并通过实际数据验证了算法的可行性。第四章分别用经典PID控制方法与基于Lyapunov稳定性原理的Backstep方法对姿态控制算法进行了设计，并通过了算法的有效了四旋翼无人机飞行试验的数据，通过对悬停、等飞行试验验证了四旋2-1Fig.2-1Schemeofquadrotor为了简化分析，假设四个螺旋桨都能够很好的安装，即四个螺旋桨都安装在同一个平面上，且到无人机几何中心的距离都相等；四个旋翼杆都正交安装；机体在飞行过程中不会出现机体的形变等。参照文献[20]x设计轴线指向前方，y轴垂直于飞机对称平面指向机身左侧，z轴在飞机对称平x轴垂直并指向机体上方，原点与无人机重心重合。地面坐标系中x轴指向地理北极，y轴指向西，zx、y轴垂直指向天，即西北天坐标系。俯仰角θ(pitchangle)：机体轴x与水平面间夹角，低头为偏航角ψ(yawangle)xx轴之横滚角φ(rollangle)zx31y轴的正力矩，这3的升力，同时增加旋1y轴的负力矩，这个力矩可以减小无人机的俯仰角，使24x轴的正力矩，这2的升力，同时增加4y轴的负力矩，这个力矩可以减小无人机的横滚角，2-11、32、4逆时针2、41、3的转速，来提供。同理，当需要逆时针转动1、32、4的转质量可将飞行状态数据下传到地面站系能够抵抗一定强度的阵风干扰与脉冲干扰一次充电飞行时间达到横滚角与俯仰角变化范围：-30°～30°同一个平面内，最终设计的四旋翼无人机如图2-2所示。Fig.2-2比较高，使用时需要安装也增加了机体的质量；外转无刷电机的转速较低，比较适合小型旋翼类无人1.5kg2kg2kg电压大小、价格等因素的综合考虑，初步选用基爱斯模型配件厂生产28301221。同时选用同一厂家生产的与电机配套的电机调速器。2-1C28-30-12Table2-1MotorparametersofC28-30-这款电机的选择主要考虑了它的负载能力，电机厂商给定的负载能力为1kg，这个负载能力指标是在配置了合适的螺旋桨的前提提供的，对于课题中的使用会稍有些下降，所以在电机的选择过程中留出了很大的裕度。即使实际使用中这个参数会有些下降也足以满足系统的需求。碳纤维加工，碳纤维材料的机械强度很高，并且密度很低，只有1.7g/cm3左右，对称的两个电机轴之间的距离为520mm，通过实验最终选择的螺旋桨为10英寸的塑料三叶桨。通过粗略的测量电机的工作电流，在悬停过程中，四个20A4000mAh11.1V锂电池，足够飞15分钟。最终加上控制电路以及电池的无人机总1200g。本款四旋翼无人机的具体参数如表2-2所示。2-2Table2-2Parametersof本节主要根据前面章节的分析建立四旋翼无人机的运动方程[21-24]2.1节，为了简化分析，对四旋翼无人机进行一下理想化假设：四个 cos cos sinsinsincoscossin(2-sincoscossinsinsinsincoscossincoscosXbody 00FB Fi F

F

ncocosnnF)sinsincoscossin

(2-y i zz

cos x(FDx2)/ y(FDy2)/ Dxy,z为空气阻力系数；m为四旋翼无人机的质量。以上为四旋翼无人机位移上的模型。d(J)M

(2-其中p rT为机体角速度矢量，

J

yJzy

Ml(F2F4Ml(F3F1

(2-(2-MC(F1F2F3F4 除此之外，飞行时，外转子电机与螺旋桨高速旋转，当飞行器姿态改变时，由于陀螺效应的存在，高速旋转的电机外转子与螺旋桨会产生一个附加的陀螺力矩，陀螺力矩的一般表达式为25：Mgyro H为转动部分的动量矩，在四旋翼无人机中，定义i为各个旋翼的角速度，Jr为转动部分的z轴转动惯量，即电机外转子与螺旋桨的转动惯量。由于在电0H00

(2- MMHM

(2-

Jy p )qrr(1234)q (F2F4 J q

)pr

r()p (FF r

xJ )pq (FF

F (, )与机体坐标系的三轴的角速度分量(p，q，r)之间的关系式p(sintan)q(costanqcosrsin(sin/cos)q(cos/cos

(2-(2-虽然在角度较大时方程2-15中的耦合项已经不可忽略，但是通过仿真分析可方程2-5、2-6、2-14与2-15构成了四旋翼无人机的运动方程。u1F1F2F3 Fu2F2F u4F1F2F3为系统的输入，将

1234看成系统的干扰。将方程 Jy p )qrrufq q(JzJx)prjrupl f J J J 4r(JxJy)pqC 4 x1,x2,x3,x4,x5,x6xz,xz,xx, x,xy, J x2 ( z)xx rux 4 f J ( x)xx rux 2 f JxJy)xxC xf(x,u)

2 J4 1ucosucos D 1 g 8 m z 1u(sinxcosxcosxsinxsinx)

Dx2 x10 u(sinxsinxcosxcosxsinx) Dx2m y12mC0.0026,l DD106N(m/s)2,D104N(m/ JxJy0.015kgm,J0.026kgm,j10kg G(s) 0.1s

(2-k为电机调速器控制信号与电机升力之间的比例关系，具体的数值对仿真都是通过调试得出，k值大小的影响也不大，所以在这里取k=1来进行后面的是以航姿信息的准确性为前提的。MEMS惯件由于体积小、质量轻、价格由于ES惯件的噪声和零漂相对于其他高精度惯件要大得多，单靠一种传感器控制系统的要求。一种可行的方法就是通过不同种类传感器的数据进行数据融合来得到飞行器的航姿参数。单靠速率陀螺捷联解算得到的姿态由于陀螺输出漂移的存在，误差会一直累积，无法长时间使用；而加速度计与磁阻传感器由于受较大，容易引入干扰，单独使用也无法达到要求，但这两种传感器长时间使用时不会产生误差的积累。本课题中采用扩展ln滤波来对速率陀螺、加速度计和磁阻传感器的数据新型数据3135。基于EKF在航姿系统中我们定义的坐标系与无人机上面所定义的坐标系有一些区别，主要是为了与常用的导航系统坐标系一致，解算出的姿态参数只需要改变一下符号即可变换到机体坐标系上，对使用没有任何影响。下面的坐标系定义仅在本节使用36。在导航坐标系的原点选在无人机重心处，取ynxn轴指向东，zn轴沿垂线方向指向天，即东北天坐标系。在载体坐标系的原点位于无人机重xb沿机体横轴指向右，yb沿机体纵轴指向前，zb沿机体的竖轴指向上，满足右手定则。、、分别为无人机的航向角，俯仰角和姿态角。coscoscossinsincossincoscossincossinsincossin(3- sincosTq2q2q2 T 2(

2(q1q2q0q3)q2q2q2q2

2( )2(qqqq)

(3- 2 0 2(qqqq 2(qqqq q2q2q2q23根据姿态矩阵的元素，可以确定(，，)的值，其中sin1T32，需要根据方程3-4对主值进行判断。

1

T31

(3- T tan112

T22

T33 T22 T π

T330,0,

T220,主 0,T 0,T 0,T 0,T 状态方程X[q0 q3 (ywy (zwz)z1x z

(wX

y 2y (zwz x (w x、y、z分别为三个陀螺的输出，w为三个陀螺仪输出误差wxwwy z q31 qX

yX 2 2 x q1 q 0X(k)(k,k1)X(k1)(k1)W(k (k) (k) (k)T 2(k) (k) (k)Tx其中(k,k1) x

y(k 2

z(k

x(k 2 z(k)

y(k)

(k) q1(kq

q2(kq

q3(k)q(k) q3(kq(k

q0(kq0(k

q1(k)q 将W(k)近似为白噪声序列，其方差阵Q(k)非负定。(k)为噪声驱动阵。MEMS观测方程

Z[ax m 其中ax、ay、az为加速度计测量出的机体坐标系中的三轴加速度，m为向角。m由式(3-3)计算得到。 HxcosmHzsin HsinsinHcosHsin 其中HxHyHz为三轴磁阻传感器的输出，为了得到跟接近真实值的航向角观测信息，取m与m为通过一步预测后得到俯仰角与横滚角，因为在求取观测量时，m与m是当前所能得到的最准确的俯仰角与横滚角。ax 0 2g(q1q3q0q2 aT10 2g(qqqq

(3-y 2 0 a g g(q2q2q2q2z 3a 2g(q1q3q0q2 x 2g(qqqq xZayh(X)

g(q2223q201

qq)azqq)

21(qq2q3 1 0 m q2q2q2

Z(k)H(k)X(k)V(k

H(k)

2qc 2qc4q3 0c22c211c220 32c2 cq2q2q2q2cqqq 1 0EKF姿态解Xˆ(k,k1)(k,k1)Xˆ(kP(k,k1)(k,k1)P(k1)T(k,k1)(k1)Q(k1)T(kK(k)P(k,k1)HT(k)[H(k)P(k,k1)HT(k)

(3-(3-(3-

Xˆ(k)Xˆ(k,k1)K(k){Z(k)h[Xˆ(k,k

(3-P(k)[IK(k)H(k)]P(k,k

(3-航姿系统硬件DSP处理组成。它能提供无人机的三轴角速度，三个方向上的加速度以及磁场选择TMS320F2808做为主处理，主要考虑到这款的引脚相对其他的DSP来讲比较少，而且片内具有64K的flash器，不需要外接TMS320F2808主频达到100MIPS，低功耗设计，电压1.8V，Flash编程电压3.3V，支持JTAG边界扫描接口；片内集成了64K的flash器，18K的SARAM，具有16个通道的12位ADC，两个UART，SPI接口；并具有16x1632x32位的乘法累加器和16x16位的双乘法累加器。角速率陀螺与加速度计采用ADI公司的ADIS16350，它集成了三轴MEMSMEMSSPI串行接口按照一定的通信协议直接将陀螺与加速度计的六个测量值出来，同时ADIS15350还提供内部的温度值。ADIS16350一些主要指标如Table3-1Mainindexof±104.75V～5.25磁阻传感器采用Honeywell公司的三轴磁阻传感器HMC1043，图3-1为传SR+SR-引脚上来产生3-2为磁阻传感器单轴的输出3-1HMC1043Fig.3-1SchematicofFig.3-2Singleaxisoutputcharacteristicof图32中可以看出，当磁场强度为零时，磁阻传感器的输出并不为零，而是有一个偏差，从文献[3]中可以知道，这个偏差对于每个是固定的。图31中带有FF标志的一系列端口用来附加电流来调整这个偏差的。但是在我们的实际使用中，并不需要对这个偏差进行调整。在使用中，同一位置，先后采集置位后的输出与复位后的输出做平均即可得到这个偏差，或者做差直接得到与磁场强度成比例的输出。根据文献[38]的数据，H1043的输出灵敏度是1//gus，当供电电压选取3.3V时，输出灵敏度即为3.3mV/guas。由于地磁场最大为0.625gus，在没有干扰的情况下最大的输出可以达到2mV左右，而且H1043在3.3供电时零位偏差最大可达到4，这样最大的输出有可能达到6V，必须要将磁阻传感器的输出信号进行放大才能到，否则有用信号基本会淹没在采样噪声中。而SP片上采样的范围为0.3～3，综合考虑，选取放大倍数为200。为了减小放大后信号的噪声，选取工业级的运算放大器A8608，它集成了4路运放，三路用在放大三个轴的磁阻传感器输出，另一路对分压得到的参考电压做电压跟随器，连接到SP的C引脚上，用作测试使用。图3-3Fig3-3Attitudeandheadingreference航姿系统软件行精确定时，每个周期产生一次中断，在中断中进行数据、处理以及结图3-4Fig.3-4AHRSsoftwareflow首先系统上电后进行系统的初始化，包括DSP中的各种寄存器设计、频率的选择等，然后对系统定时器进行设置，系统需要10ms进行一次进入中断后，首先启动磁阻传感器的/D，然后立刻AIS6350的输出，然后等待/D转换结束，/D转换后的结果后进入数据预处理程序，预处理程序对陀螺仪与加速度计的输出进行数值转换并补偿掉陀螺仪的常值零偏，并对到三种传感器进行低通滤波，滤除高频噪声，然后进行航姿解算，最后将解算得到的姿态信息通过串口输出。实验航向角航向角俯仰角俯仰角 2横滚角横滚角 3-5Fig3-5AttitudeoutputofAHRSat3-5可以看出，前面设计EKF算法可以有效的抑制陀螺的零偏，航向1°0.5°范围本章主要介绍了基于MEMS陀螺仪、加速度计与磁阻传感器的四旋翼无究，根据传感器的特点建立了基于EKF的数据融合算法，并实际测量数4-1Fig.4-1Overallcontrolblockdiagramof将位置控制器给出的三轴线速度与GPS反馈回来的速度输入到速度控制器中，然后通过速度控制器给出需要的旋翼总升力与目标姿态角、，输入到姿态控制器中，进而通过姿态控制器控制四个旋翼的拉力来对姿态PID控制理论可姿态控制回路：通过航姿参考系统进行三个姿态角、、速度控制回路：在速度回，通过GPS对飞行器的线速度进行反馈，位置控制回路：在位置环上，GPS将对飞行器的位置信息进行反馈，位置基于PIDPID控制。PID控制器因具有结构简单、易实现、PID的控制性能影响PID控制结构 Jy )qrrufq J ( x)prrup J J JxJ

Jy( 从方程(4-1)u2、u3、u4直接影响着x、y、z三轴的角加速4-2PID的姿态控制框图。将运动学模型输出的三个姿态角反馈到输入端，将差值输入到PID控制器4-2PIDFig.4-2ControlsystemblockdiagrambasedonPID算法仿真与本节主要通过仿真对PID控制算法的有效性进行分析，对于这种控制模型，最重要参数的是系统的响应速度与超调量。本节通过对经过调试选择的PID参数进行各种仿真实验来说明PID姿态控制算法的性能。2-5、2-13建立，主要根据四个旋翼产生的拉力和力，并将单旋翼拉力的输出限制在1N~5N之间。在环境下对上面的分析进行仿真，通过调试取三个通道的将四旋翼无人机的初始横滚角、俯仰角、航向角设为(-30°,-30°,°)横滚角横滚角0 俯仰角俯仰角0 航向角航向角0 4-3Fig.4-3Attitudeoutputofstep从图4-3可以看出系统的阶跃响应稳定时间大概在3s，有10°在悬停时，测试系统的脉冲能力，t=2s时在三个通道的力矩0.5Nm0.1s的干扰力矩，三轴信号的输出如图4-4。4-4可以看出当一个较大的脉冲干扰力矩作用在机体上时，无人机经过一个较大幅度的震荡之后大概3s的时间即可恢复悬停的状态。在悬停时，测试系统的抗阵风干扰能力，t=1s～8s之间在三个通道的力矩输入端同时加入幅值为0.1Nm的干扰力矩来模拟阵风干扰的效果，三轴信号的输出如图4-5。横滚横滚角0

俯仰俯仰角

航向航向角0

Fig.4-4Attituderesponsetopulsedisturbance横滚角横滚角0俯仰俯仰角0航向航向角0

4-5Fig.4-5Attituderesponsetogustdisturbance图4-5中，在t=1s～8s之间加上了一个常值的干扰力矩来模拟阵风的干扰，无人机的三个姿态角在3s之内基本可以稳定，通过四个旋翼的升力变化来产生抵的力矩。当干扰消除后，还需要大概3s时间来恢复悬停的状4-1中可以看出，系统存在着耦合项，即任两个方向上的角速度会PID控制中，是将三个姿态角分别进行控制，反馈的信号只有单通道的角度，方程4-1中的耦合项在算法设计中都看成了干扰0.5Nm0.1s的干扰力矩，无人机的三个姿态角输出如图4-6。横滚角横滚角0俯仰俯仰角0航向航向角0

Fig.4-6Attituderesponsewhenthepulsedisturbancetorqueaddedtothepitchandyaw46横滚通道上相当于产生了一个较大的干扰力矩，横滚角会受到耦合项很大的影响，对于这种情况，PID方法很难达到特别好的控制效果，并且当考虑到位置更加适合这种非线性和存在耦合的系统的控制方法[39-42]基于Backstep方法的姿态控制算Backstep方法的理论基础为Lyapunov稳定性定理，即对于一个微分方程表征的系统，通过构造一个正定的Lyapunov能量函数，使该函数对时间的全导数负定或半负定来保证该系统的全局稳定性。使用Lyapunov分析系统的稳定性时，关键是构造一个合适的Lyapunov函数，当系统的状态方程较为复杂时，构造一个合适的Lyapunov函数有时会遇到。Backstep提供了构造系统Lyapunov函数的有效方法。从文献[43-45]中可以看出Backstep方注意的是Backstep方法在设计中要求系统结构必须是严格反馈系统或可经过变换化为严格反馈系统，即可以表示为式4-2的形式。 1 11xf(x 1 11 f(x,x)g(x,x xnfn(x1, ,xn)gn(x1, fi，gi仅与反馈状态x1, ,xi有关。

(4-Backstep的设计思想是针对满足严格反馈控制结构的系统，通过反向递推的方法构造Lyapunov函数和控制器，设计的基本方法是将复杂的系统分分Lyapunov函数和中间虚拟控制量，直至完成整个控制器的设计。x1f(x1) x2定义误差变量z1x1，其导数为z1x1gx2fgz2g1 选取Lyapunov函数V1为

V1z 2V(z1)z1z1gz1z2z1(g1f

(4-1(kzf 1

1Vkz2gz z2的导数为

1 1zxu1(gxf)1u s2 2

1(gxf)s2 s2取Lyapunov函数V2

V1(z2z2 Vkz2gzzz(uf

1 1 u(k2z2gz1fs 其中k2为大于零的常数 Vkz2k 1 2Lyapunov4-124-3在Backstep姿态控制根据上一小节的理论基础和参考文献[13,36]中的Backstep设计具体x1

J( z)xx

rux

(4- 4 f z1x1d

V(z)1 2V(z1)z1(x1dx2为了保证z1的稳定性，引入：x2x1dk1z1,k1

(4-(4-Lyapunov函数为：

V(z)k 1z2x2x1d 1

(4-(4-V(z,z)1(z2z2 V(z,z)(k21)zzkz2kz2z 1 1 1 2V(z,z)kz2kz2,k 1 2 uJx(JyJz)xxjrux(k21)zkzkz l 4 f 1 22 同理根

u u )xx ux(k1)zk

kz l 2 f 3 44 JJ z( y)xx(k21)zkzk

C

2 5 66

z3x3dzx k 3 z 其中k16Backstep算法仿真与分图4-7基于Backstep方法的姿态控制框Fig.4-7Controlsystemblockdiagrambasedon将四旋翼无人机的初始横滚角、俯仰角、航向角设为(-30°,-30°,°)横滚角横滚角俯仰俯仰角0航向航向角0

Fig.4-8Attitudeoutputofstep4-8可以得到系统在从-30°～0°3s的时间达10°PID控制时的效果基本相同。在悬停时，测试系统的脉冲能力，t=2s时在三个通道的力矩输0.5Nm0.1s的干扰力矩，三轴信号的输出如图4-9。PID控制方法一致，系统经过了一个段时间的震荡1sPID控制算法在在悬停时，测试系统的抗阵风干扰能力，t=1s～8s之间在三个通道的0.1Nm4-10。从图4-10中可以看出，当系统存在阵风干扰的时候，四旋翼无人机可以0.5°PID控制方法中接近20°的扰动，Backstep控制的抗风扰效果要强很多。000

4-9Fig.4-9Attituderesponsetopulsedisturbance横滚角横滚角0

0

0

4-10Fig.4-10Attituderesponsetogustdisturbance8s0.3Nm的干扰力矩，三轴信号的输出如图4-11。横滚横滚角0俯仰俯仰角0航向航向角50

4-11Fig.4-11Attituderesponsetogustdisturbance图4-11中可以看出，当阵风干扰增加到0.3Nm时，系统也可以在大概3s的时间后达到稳态，并且当阵风后，系统在很短的时间内即可通过调整螺0.5Nm0.1s4-12。4-12横滚横滚角0---俯仰俯仰角0航向航向角420-

Fig.4-12Attituderesponsewhenthepulsedisturbancetorqueaddedtopitchand从上述的仿真与分析来看，Backstep方法对四旋翼无人机姿态控制具有相对较好的效果，但是仅通过仿真实验无法得出准确的结论，很可能是PID分析。但是从上述仿真分析可以得出Backstep方法对于四旋翼无人机的姿PIDBackstep的阶跃响应，能力等，从结果来看，Backstep方法设计的控制器对5-1Fig.5-1Structureofquadrotorcontrol其中DSP主要负责飞行控制算法运行、接收指令的判断、GPS等传感器数据，并向地面站传输实时数据；CPLD主要进行I/O口的扩展，无可以在地面通过无线传递指令，无线进行接收，同时解算出相应通道的信号，通过CPLD，将指令传递到DSP主处理器块上，然后通过UART串行通口将指令传递到DSP中。系统通过或者无线传输模块接收到指令，通过上述两种不同的方式传递到主处理器DSP中，通过飞行控制算法解算出四个电机的控制参数，通过发生器产生信号提供给电机调速器，从而调整电机的转速来达到定姿或定位等功能。同时主处理器DSP在飞行过程中不断的将速度，姿态传感器和DSP处理组成，具体设计前面第四章已经详细介绍了。根据图5-1整个飞行控制系统就分成了三个主要部分：主处理器部分、航姿系统部分主处理器中，预计设计两层电路，主控板为DSP与CPLD电路以及所有的接口电路。扩展板为GPS、超声测距模块和气压计等传感器。这样在实际使用中，如果仅使用飞行时单独使用主控制板即可完成飞行。由主控制器根据处理速度以及飞控系统的小型化考虑，选用TI公司的TMS320F2812作为主处理器。TMS320F2812采用高性能的静态CMOS技术，主频高达150MHz，时钟周期6.67ns，低功耗设计，编程电压为3.3V。同时，TMS320F2812具有高性32位CPU16×1632位×32为的乘法16×16位的双乘法累加器。使DSP集成开发环境CCS2.2TMS320F2812具有以下特点TMS320F281X系列DSPCMOS150MIPS，低功耗设计，电压1.8V，Flash编程电压3.3V，支持JTAG高性32CPU：16x1632x32位的乘法累加操作；16x16位的双乘法累加器；哈佛总线结构，统一寻址模式，4MB的程序/数据寻址空间，并具有高效的代码转换功能（支持C/C++和汇编。片上器：128Kx16bit的Flash器和18Kx16bit的数据/程序存储器；提供外部器扩展接口，具有最多1MB的寻址空间。外设：两个管理器EVA和EVB，每一个管理器模块包括定时器、比较器、捕捉单元、逻辑电路、正交编码脉冲电路以及中断逻辑电路等。双路12位的ADC，可实现双路信号同时采样，流水线最快转换周期60nsCPU定时器。支持高速同步串行接口（SPI、两个UART接口模块、增强的eCAN2.0B接口模块和多通道缓冲串口（McBSPCPLD主要进行的和串口的扩展，选择EPM7128AETC100，EPM7128是可编程的大规模逻辑器件，为AL公司的MAX7000系列产品，可用门单元为2500个，管脚间最大延迟为5ns，工作电压为+5V，使用AlQuartusⅡ设计软件进行开发。从控制系统方案图中可以看出，主控板上至少需要4个UART，但是TMS320F2812只提供两个UART，这就需要扩展出两个UART。选用两片TL16C550PT作为串口扩展。电机调速器的控制信号为信号，为了起见，在主处理器的输出端与电机调速器输入端设置了光耦，防止电机调速器的电流过图5-2Fig.5-2Maincontrolsystem主控制器软件主系统控制频率为100Hz，即周期为10ms，采用TMS320F2812中的定时器进行精确定时，每个周期产生一次中断，在中断中进行传感器信号、控制算法的实现以及控制量的输出。采用进行控制的程序流程如图5-3。图5-3Fig.5-3Softwareflowpatternofmaincontrol断子程序。进入中断后，首先所有传感器的数据，然后控制指令，根面实现对飞行器飞行状态监测、机载图像实时显示、飞行航迹规划、人工5-4为地面站的实物图。主要由计算机、控制手柄、无线数据传输模块组成。通过对控制手柄的，控制信号通过USB接口将控制指令传递到计算机中，然后通过VC++底层的操作，将控制指令通图5-4Fig.5-4Groundcontrol5-5Fig5-5Interfaceofgroundcontrol飞行模块中虚拟的飞行仪表为地面人员提供无人机的飞行状态信息，人机状态参数及机载，在飞行结束后回放飞行过程，便于飞行后的数据分飞行航迹规划模块可以根据飞行器的性能和飞经的地理环境、环境和地面辅助控制模块包括人工辅助控制和辅助着陆控制。人工辅助控制行状态数据和图像手动控制至飞行器安全着陆。辅助着陆控制可以在着陆。操作员可通过地面站控制手柄进行无人机的飞行控制。本节主要通过飞行试验对无人机的姿态控制性能进试。在四旋翼无人机飞行过程中，采用给定姿态角指令，通过无线传输模块以10Hz的频悬停

5-6Fig.5-6Captureof三个姿态角输出如图5-7。横滚角5横滚角0 0 0 Fig.5-7Attitudeoutputwhenquadrotorwas2°范围内。产生波动的原因可能会带来一些机体的；二是由于四个螺旋桨产生的气流的原因；三是由于航信号测在飞行过程中，通过给定四旋翼无人机三个姿态角来对无人机的飞行进行控制，将给定角度与无人机输出角度通过无线传输模块同时下传到地面站系5859的曲线。864俯仰俯仰角0----

图5-8俯仰角输Fig.5-8Pitchtracing航向航向角输出 图5-9航向角输从图5-8和图5-9中可以看出四旋翼无人机可以较好的上给定的姿态俯仰角的输出与控制特性基本一致，所以实验中只给出了俯仰角的曲线。能力y轴上，观察系统的姿态角输出图5-10所示.横滚角横滚角0

俯仰角俯仰角00

454565-10Fig.5-10Attituderesponsetopulsedisturbance5-101.8s的时候，系统出现了一个较大的干扰，俯仰角出现了一个很大的扰动，三个姿态角都受到了影响，在大概2s之后系控制需求，设计了以DSP为主处理的飞行控制系统，CPLD主要进行接收I/O10ms为工作周期，在一个周期中进行传感器数据的、控制信号的生成以及飞行状态的下传。最后通过飞行实验对四旋翼无人机的飞行稳定性进行了验证，通过对悬停、指令与脉冲干定的能力，为四旋翼无人机的下一步研究打下了基础。 小型四旋翼无人机是一种新型的多旋翼型无人机，具有结构简单，成本低，噪音小，安全性好等多种优点，具有广阔的应用前景。本文研究了小型四旋翼无人机的导航与控制，主要完成的工作包括：根据四旋翼无人机对航姿信息的要求设计了以MEMS陀螺仪，加速度计与磁阻传感器为基础的航姿系统，并设计了基于扩展Kalman滤波的信息融使用Backstep方法设计了姿态控制器，仿真结果表明，这种方法的标定测试，并在测试转台上进量效果的评估。并将GPS整合到航姿系统中，进行位置信息的估计；使用Backstep方法进行实际飞行试验，并分 高倩,徐文.国外微型无人机发展概况.飞航,2003,(06):14-PaulG．Fahistrom.IntroductiontoUAVSystemsSecondEdition.UAVSystems,Inc.,COLUMBIA,MARYLAND,USA,2003:158-204.李占科,宋笔锋,宋海龙.微型飞行器的研究现状及其关键技术[J].飞行力学UnmannedAircraftSystems,2005-2030[R].OfficeoftheSecretaryofDefense(USA)Report.August,2005MichaelWMcKee.VTOLUAVscomeofage:USNavybeginsdevelopmentof VerticalTakeoffUAVContract.htmSharpC.S,ShakerniaO,SastryS.S.AVisionSystemforLandinganUnmannedAerialVehicle.RoboticsandAutomation[C].Proceedings2001ICRA,IEEEInternationalConferenceon.2001,Volume2:1720-1727SaripalliS,MontgomeryJ.F,SukhatmeG..S.Vision-basedAutonomousLandingofanUnmannedAerialVehicle.IEEEInt.Conf.onRoboticsandAutomation.E.Altug,J.P.Ostrowski,R.Mahony,ControlofaQuadrotorHelicopterusingVisualFeedback,Proceedingsofthe2002IEEEInternationalConferenceonRoboticsandAutomation,WashingtonD.C,May2002:72-77WilliamC.Brown.StatusoftheMicrowavePowerTransmissionComponentsfortheSolarPowerSalite.IEEETransactionsonMicrowaveTheoryandScottDHanford.ASmallSemi-autonomousRotary-wingUnmannedAirVehicle[D].UniversityofPennsylvania,2005BOUABDALLAHS,NOTHA,SIEGWARTR.PIDvsLQcontroltechniquesappliedtoanindoormicro-quadrotor[C].ProceedingsoftheIEEEInternationalConferenceonInligentRobotsandSystems(IROS).2004:2451-2456.BOUABDALLAHS,SIEGWARTR.Backstepandsliding-modetechniquesappliedtoanindoormicroquadrotor[C].ProceedingsofInternationalConferenceonRoboticsandAutomation.Barcelona.AltugE,OstrowskiJ.P.,andTaylor,C.2003.Quadrotorcontrolusingdualvisualfeedback.ProceedingsoftheIEEEInternationalConferenceonRoboticsandAutomation(ICRA),Taipei,,September,pp.4294-4299SureshKK,KahnAD,YavrucukI.GTMARS-FlightControlsandComputerArchitecture[M].Atlanta:GeorgiaInstituteofTechnology,2000RichardK.ArningandStefanSassen.FlightControlofMicroAerialVehicles.InAIAAGuidance,Navigation,andControlConferenceandExhibit,2004海，交通大学，2009 吴森堂，费玉华.飞行控制系统.航空航天大学MckerrowP,ModelingtheDraganflyerfour-rotorhelicopter,IEEEInternationalConferenceonRoboticsandAutomation,2004,27:153-158P.Pounds,J.Gresham,R.Mahony,J.Robert,andP.Corke.Towardsdynamicallyfavourablequad-rotoraerialrobots.InProceedingsoftheAustralasianConferenceonRoboticsandAutomation,Canberra,ACT,Australia,2004PaulPounds,RobertMahony,PeterCorke,ModellingandControlofaQuad-Rotor,InProceedingsoftheAustralasianConferenceonRoboticsandAutomation,Aucland,NewZealand.,December2006AlexandreRobin,DesignofaFlightControllerforanX4-FlyerRotorcraft,MasterDegreeThesis,Huntsville,TheUniversityofAlabamainHuntsville邓正隆.惯性技术.哈尔滨工业大学.A.Tayebi,S.Mcgilvray,AttitudeStabilizationofaFour-rotoraerialrobot,IEEEConferenceonDecisionandControl,2004,12:14-17A.Tayebi,S.Mcgilvray,AttitudeStabilizationofaVTOLQuadrotorAircraft,IEEETransactionsonControlSystemsTechnology,2006,5:562-571GAOZhong-yu