STM32飞控算法理论

参考 hts://ss.zhihuss/column/c_1208718933341220864

前言

前面我引见了不少有关微型四轴飞翔器嵌入式底层开发的知识，尽管应付不少初学者来说，此中有一些观念比较晦涩难懂，但是它们简曲能够让各人应付嵌入式底层开发有更深刻的了解取把握。好了言横竖传，正在原篇文章中我会向各人引见有关微型四轴飞翔器飞控算法真践方面的相关内容，而至于什么是飞控，我想不少原人买过成品飞控板、组拆过航模或四轴飞翔器的同学都不会陌生，望文生义，飞控指的是飞翔控制算法，即通过获与IMU（惯性传感单元）数据，并给取滤波、PID等算法对飞翔器当前的姿势停行真时解算，获得各电机的转速和转向，从而完成对飞翔器的控制。由于飞控算法自身波及到不少主动控制本理和线性代数方面的真践知识，所以引荐各人先把自控和线代好好温习一遍再去钻研飞控算法，那样了解起来可能会容易一些。

钻研现状

国内钻研现状

DJI

目前国内作四轴飞翔器的公司有不少，此中作飞控系统最好确当属大疆翻新（DJI）。大疆翻新（DJI）目前领有NAZE、WooKong以及A系列的多种商业飞控系统，此中做为NAZA飞控系列的最新一代产品，N3多旋翼飞控系统给取DJI最新的控制导航算法，新删内置的双IMU冗余设想，可真现数据真时互为备份，并联结全新内减震构造设想，赋予了飞翔器更高的牢靠性，为无人机爱好者及止业使用摸索者供给不乱而片面的系统级处置惩罚惩罚方案。

Crazepony

Crazepony微型四轴飞翔器由深圳创客火（Maker Fire）公司设想、制造并发止，其内置的飞控算法可以真现飞翔器的定高悬停、有头（X形式）和无头形式飞翔等罪能。相比于商业飞控来说，Crazepony的飞控开源（蕴含源代码、本理图、工做本理、系统框架、设想思路等），且焦点算法局部有具体的教程文档，既便捷各大高校的学生和爱好者进修相关知识，也利于停行二次开发，以真现原人的创意。

MiniFly

MiniFly是广州市星翼电子科技有限公司（ALIENTEK）最新推出的一款开源微型四轴飞翔器，它所运用的飞控算法可以撑持：定高和手动飞翔、4D翻飞、抛飞、有头（X形式）和无头形式飞翔、一键起飞和降落等罪能。由于MiniFly微型四轴的硬件电路设想次要是基于海外的Crazeflie名目批改而来，所以其硬件机能正在国内划一规格的四轴飞翔器里绝对算是一流的，虽然壮大的硬件机能也为MiniFly真现更为复纯的飞控算法供给了一个绝佳的平台。

海外钻研现状

海外高校和科研机构对飞翔器控制系统的钻研曾经有不少年了，技术相对来说比较成熟，而且根柢都是以开源飞控为主。以下扼要引见几多个目前比较风止的开源飞控系统：

APM

APM(ArduPilot Mega)是正在2007年由DIY无人机社区（DIY Drones）推出的飞控产品，是当今最为成熟的开源硬件名目。APM基于Arduino开源平台，对多处硬件作出了改制，蕴含加快度计、陀螺仪和磁力计组折惯性测质单元（IMU）。由于APM劣秀的可定制性，APM正在寰球航模爱好者领域内迅速流传开来。通过开源软件Mission Planner，开发者可以对APM停行相应的配置，承受并显示传感器的数据，运用Google Map完成主动驾驶等罪能。除此之外，APM连贯外置GPS传感器以后能够加强飞翔的不乱性，并能够完成自主起降、自主航线飞翔、回家、定高、定点等富厚的飞翔形式。APM可以运用外置的超声波传感器和光传布感器，正在室内真现定高和定点飞翔。

PX4

PX4是一个软硬件开源名目（固守BSD和谈），宗旨正在于为学术、爱好和家产集体供给一款低老原、高机能的高端自驾仪。那个名目源于苏黎世联邦理工大学的计较机视觉取几多何实验室、自主系统实验室和主动控制实验室的PIXHawk名目。于2004年推出的PIXHawk飞控是PX4飞控的晋级版原，它领有PX4和APM两套固件和相应的空中站软件。该飞控是目前全世界飞控产品中硬件规格最高的产品，也是当前爱好者手中最炙手可热的产品。PIXHawk领有168MHz的运算频次，并冲破性地给取了整折硬件浮点运算焦点的Cortex-M4的单片机做为主控芯片，内置两套陀螺和加快度计MEMS传感器，互为补充更正，内置三轴磁场传感器并可以外接一个三轴磁场传感器，同时可外接一主一备两个GPS传感器，正在毛病时主动切换。

CC3D

CC3D是Openpilot旗下最风止的飞控系统，此飞控板只给取一颗72MHz的32位STM32单片机和一颗MPU6000就能够完成四旋翼、牢固翼、曲升机的姿势控制飞翔。取所有开源飞控差异，它不须要GPS融合大概磁场传感器参取修正，就能保持长光阳的姿势控制，而且通过设置就可以变动飞机品种、飞翔形式、撑持云台删稳等罪能。另外，CC3D飞控编译完的固件容质只要约莫100KB，代码效率令人诧异，而且其空中站软件集成为了完好的电子舆图，可以通过电台真时监测飞机形态。

MWC

MultiWiiCopter(MWC)飞控是一款典型的Arduino衍消费品，是专为多旋翼开发的低老原飞控，它完好地糊口生涯了Arduino IDE开发和Arduino方法晋级和运用的办法。由于老原低、架构简略、固件比较成熟，因而该飞控正在国内外领有大质爱好者。除了撑持常见的四、六、八旋翼以外，该飞控的最大特点是撑持不少神奇的飞翔器类型，比如三旋翼、Y4型多旋翼（此中两轴为高下对置）等。

PPZ

Paparazzi(PPZ)是一个软硬件全开源的名目，它始于2003年，开发目的是建设一个配置活络且机能壮大的开源飞控名目。PPZ的一大特点是，该开源飞控方案中除了常见的飞控硬件、飞控软件和空中站软件之外，还包孕空中站硬件，蕴含各类调制解调器、天线等方法。从罪能上讲，PPZ曾经濒临一个小型的无人机系统了。该开源项宗旨另一个特点是给取Ubuntu收配系统，它将全副空中站软件和开发环境集成于该系统下，官方称之为Live CD。一张CD加飞控硬件就可完成从开发到运用的全副工做。PPZ目前最风止的硬件版原事有大质的扩展接口，便捷开发者停行DIY。

Crazeflie

Crazeflie是海外最知名的微型四轴飞翔器开源名目，它由三个来自瑞典的嵌入式工程师所创立，宗旨是运用尽可能少的零件来结构一款可以正在室内运用的小型飞翔呆板人。得益于其杰出、严谨的硬件电路设想、不乱且牢靠的飞控系统以及模块化的扩展才华，Crazeflie四轴飞翔器可以完成不少同类型四轴飞翔器无奈作到的高难度实验任务。除此之外，Bitcraze团队还为Crazeflie微型四轴飞翔器名目编写了大质的运用和发教程，便捷全世界的四轴爱好者们停前进修和开发（国内Crazepony和MiniFly两大开源微型四轴名目就正在很急流平上参考了Crazeflie中的一些设想理念和内容）。

根柢观念

电机规划

四轴飞翔器的四个电机呈十字形布列，驱动四片桨旋转孕育发作向上的推力。由于四个电机的轴距几多何核心的距离相等，所以当对角两个轴孕育发作的升力雷同时能够担保力矩的平衡，四轴不会向任何一个标的目的倾转。而当四个电机一对正转一对反转时，可使得绕竖曲轴标的目的旋转的反扭矩平衡，担保了四轴航向的不乱。

取传统的曲升机相比，四轴飞翔器有着下列的劣势：各个旋翼对机身所施加的反扭矩取旋翼的旋转标的目的相反，因而当处于同一对角线的两个电机向雷同标的目的旋转时（差异对角线上的电机转向相反），就可以平衡旋翼对机身的反扭矩。

电机规划形式

如上图所示，依据用户自界说的机头的位置差异，四轴飞翔器可以分为×形式和+形式。×形式的机头标的目的位于两个电机之间，而+形式的机头标的目的位于某一个电机上。×和+便是默示正对机头标的目的时飞翔器的外形。相对而言，×形式不乱一些，但止动更活络。但假如要完成翻跟头等特技止动，可能须要用+形式。

动力学本理

四轴飞翔器正在空间共有6个自由度（划分沿3个坐标轴做平移和旋动弹做），那6个自由度的控制都可以通过调理差异电机的转速来真现。根柢活动形态划分为：垂曲活动、俯仰活动、滚转活动、偏航活动、前后活动和侧向活动。

垂曲取俯仰活动

垂曲活动

垂曲活动相对来说比较容易。正在上图(a)中，因有两对电机转向相反，可以平衡其对机身的反扭矩，当同时删多四个电机的输出罪率，旋翼转速删多使得总的拉力删大，当总拉力足以按捺整机的分质时，四轴飞翔器便离地垂曲回升。反之，同时减小四个电机的输出罪率，四轴飞翔器则垂曲下降，曲至平衡落地，真现了沿Z轴的垂曲活动。当外界扰动质为零时，正在旋翼孕育发作的升力就是飞翔器的自重时，飞翔器便保持悬停形态。担保四个旋翼转速同步删多或减小是垂曲活动的要害。

俯仰活动

正在上图(b)中，电机1的转速回升，电机3的转速下降，电机2、电机4的转速保持稳定。为了不因为旋翼转速的扭转惹起四轴飞翔器整体扭矩及总拉力扭转，旋翼1取旋翼3转速该变质的大小应相等。由于旋翼1的升力回升，旋翼3的升力下降，孕育发作的不平衡力矩使机身绕Y轴旋转（标的目的如图所示），同理，当电机1的转速下降，电机3的转速回升，机身便绕y轴向另一个标的目的旋转，真现飞翔器的俯仰活动。

滚转取偏航活动

滚转活动

取上图(b)本理雷同，正在图(c)中，扭转电机2和电机4的转速，保持电机1和电机3的转速稳定，则可使机身绕X轴旋转（正向和反向），真现四轴飞翔器的滚转活动。

偏航活动

四轴飞翔器偏航活动可以借助旋翼孕育发作的反扭矩来真现。旋翼动弹历程中由于空气阻力做用会造成取动弹标的目的相反的反扭矩，为了按捺反扭矩影响，可使四个旋翼中的两个正转两个反转，且对角线上的各个旋翼动弹标的目的雷同。反扭矩的大小取旋翼转速有关，当四个电机转速雷同时，四个旋翼孕育发作的反扭矩互相平衡，四轴飞翔器不发作动弹；当四个电机转速不彻底雷同时，不平衡的反扭矩会惹起四轴飞翔器动弹。正在图(d)中，当电机1和电机3的转速回升，电机2和电机4的转速下降时，旋翼1和旋翼3对机身的反扭矩大于旋翼2和旋翼4对机身的反扭矩，机身便正在充分反扭矩的做用下绕Z轴动弹，真现飞翔器的偏航活动，转向取电机1、电机3的转向相反。

前后取侧向活动

前后活动

要想真现飞翔器正在水平面内前后、摆布的活动，必须正在水平面内对飞翔器施加一定的力。正在图(e)中，删多电机3转速，使拉力删大，相应减小电机1转速，使拉力减小，同时保持其余两个电机转速稳定，反扭矩依然要保持平衡。按图(b)的真践，飞翔器首先发作一定程度的倾斜，从而使旋翼拉力孕育发作水平重质，因而可以真现飞翔器的前飞活动。向后飞翔取向前飞翔正好相反。虽然正在图(b)和图(c)中，飞翔器正在孕育发作俯仰、翻腾活动的同时也会孕育发作沿X、Y轴的水平活动。

侧向活动

正在图(f)中，由于构造对称，所以侧向飞翔的工做本理取上面讲到的前后活动彻底一样。

飞翔形式

飞翔形式分为有头形式和无头形式两种。此中飞翔器正在飞翔的历程中，其活动的前后摆布以天文坐标系为参考坐标系，则为无头形式飞翔；而有头形式则是飞翔器活动的前后摆布以原身的坐标系为参考坐标系。

那里对有头和无头形式停行开展解说。任何飞翔器都一定有个原身的坐标系，也便是飞翔器的头和尾，那也便是前面说的飞翔器的原身坐标系。假如敦促遥控器向前活动，飞翔器总是向它头的标的目的飞翔，这么那个飞翔器便是运止正在有头形式。假如敦促遥控器的向前飞翔，飞翔器还是向它起飞时头批示的标的目的飞翔，纵然那个时候飞翔器正在飞翔的历程中扭转了机头标的目的（哄骗了遥控的航向角），这么那个飞翔器照常运止正在无头形式下。

因为飞翔器的无头形式是以天文坐标系为参考，而有头形式则是以飞翔器原身的坐标系为参考，所以应付差异的飞翔形式，姿势解算算法是不太一样的。应付无头形式，姿势解算局部可以运用磁力计来测质飞翔器相应付地球磁场的角度，从而算出机头正在磁场中的标的目的。此外，也可以间接对航向角停行积分，算出飞翔器相应付起飞时机头旋转的角度，目前，Breeze微型四轴飞翔器运用的便是那种真现方式。

软件框架

嵌入式系统软件真现局部没有运用真时收配系统（RTOS），而是间接通过编写裸机代码，依靠中断嵌套来完成整体罪能的。正在代码的初始化阶段，步调对STM32的按时器4停行了初始化，它可以中断while死循环，用于更新远程遥控数据、机身姿势融合、PID控制计较输出和电机PWM输出等收配。

总之，按时器4中断是焦点中断，所有的飞控算法都是正在那里真现的，机身的不乱也是依靠那个中断来真现的。接下来，原文章将联结步调执止流程图来具体地解说按时器4中断内部的组织构造和真现的罪能：

Breeze微型四轴步调执止流程图

如上图所示，步调会正在主循环中不停轮询各个按时器的标识表记标帜位，从而孕育发作三个大小差异的光阳段。正在那些光阳段内，系统即可以完成像接管遥控器指令、更新传感器数据以及更新电机控制等任务。主步调通过分别差异频次的按时器循环，使得所有任务可以分时并发地执止，那有效地降低了函数串止执止时的延时问题，并且进步了主控对资源的操做率。

系统初始化

主函数正在初步执止阶段会先挪用Hardware_Init()函数停行硬件相关方法和罪能的初始化。初始化完成之后步调就会进入主循环while(1)中，主循环也是整个软件罪能真现的要害，步调一旦进入便会正在里面不停地运止，只要发作中断时步调才会暂时分隔主循环，而去执止劣先级高的中断效劳步调。

主循环-100Hz循环

主循环体内会首先判断timer_loop_flag_100hz的值能否为实，而timer_loop_flag_100hz标识表记标帜位是正在TIM4按时器中断中每10ms置位一次，即该循环会每10ms执止一次此中的工做。

正在那个循环之中，STM32会读与MPU6050和MS5611数据并停行融合。因为原论文给取的是软件解算MPU6050的姿势，所以读与的数据应为陀螺仪和加快度计的AD值，之后再将数据停行标定、滤波、校正后通过四元数融合获得三轴欧拉角，详细如下图所示：

欧拉角计较流程

加快度传感器支罗数据容易失实，组成姿势解算出来的欧拉角舛错。只用角度单环的状况下，系统很难不乱运止，因而可以参预角速度做为内环。角速度由陀螺仪支罗数据输出，支罗值正常不存正在受外界影响状况，抗烦扰才华强，并且角速度厘革灵敏，当受外界烦扰时回复迅速加强了系统的鲁棒性。

姿势双环PID控制

如上图所示，把角度做为外环，角速度做为内环，即通过将角度环的输出值做为角速度环的输入来停行姿势双环PID控制。

主循环-50Hz循环

那个循环每20ms执止一次，其任务次要是解析通过无线通信发送过来的控制指令，并联结当前四轴飞翔器的姿势，更新飞控焦点步调，使得其可以依照指令停行挪动。

主循环-10Hz循环

那个循环会以每100ms的光阴间隔执止一次，它的任务次要是当四轴飞翔器显现异样状况时，可以控制飞翔器颠簸地降落。异样状况蕴含电池电质过低、飞翔高度超出领域以及无线通信失败等。

算法解说

姿势解算简介

姿势解算也叫作姿势阐明，姿势预计或姿势融合，它是指依据支罗到的IMU数据（陀螺仪、加快度计、磁力计等）来求解获得四轴飞翔器的地面姿势，姿势解算分为快捷解算和深度解算两种。

假如依据陀螺仪的三轴角速度对光阳的积分获得了四轴飞翔器的俯仰、翻腾和航向角，那是快捷解算。快捷解算获得的姿势是存正在误差的，而且误差会随同积分而累加，假如再联结三轴地磁和三轴加快度数据停行漂移弥补和校正，获得精确的姿势，那便是深度解算。不过由于通过快捷解算获得的姿势误差往往较大，因而目前姿势解算但凡指的便是深度解算。

姿势数学默示

数学模型

姿势是用来形容一个刚体的固连坐标系和参考坐标系之间的角位置干系。四轴飞翔器运用的参考坐标系是当地水平坐标系，即天文坐标系，而其原身的固连坐标系叫作载体坐标系。天文坐标系有不少种，如下图所示，那里运用的是比较罕用的NED（即“北东地”）坐标系。

NED坐标系

四轴飞翔器正在地面飞翔时，咱们运用下图中的姿势角来形容其正在空间里的角度干系。此中姿势角包孕有翻腾角Roll（记做 

 ）、俯仰角Pitch（记做 

 ）和航向角Yaw（记做 

 ）。

但凡咱们正常选择把 

 轴做为四轴飞翔器的正后方，这么俯仰角 

 则为载体绕 

 轴旋转的角度，指向水平面以下为正，指向水平面以上为负，角度领域从−90°至90°；翻腾角 

 为载体绕 

 轴旋转的角度，坐标 

 指向水平面以上为正，指向水平面以下为负，角度领域从−180°至180°；而航向角 

 为机体绕 

 轴旋转的角度，俯视图逆时针为正，顺时针为负。

机体坐标系

如天文坐标系图所示，界说导航坐标系为 

 ，坐标本点 

 为载体的动弹核心，天文坐标系正在载体活动时做为基准坐标系，所以求解载体航止姿势时，须要先将载体坐标系内测质到的数据转换到天文坐标系中，再停行姿势解算。

如机体坐标系图所示，界说载体坐标系为 

 ，其坐标系本点 

 正在载体的量心大概核心， 

 轴沿载体水平标的目的向前， 

 轴指向朝着载体正后方看的右侧标的目的， 

 轴则垂曲于 

 平面沿载体竖轴向上，且载体坐标系 

 满足左手法例。

默示方式

欧拉角

欧拉角由莱昂哈德·欧拉创设，用来形容刚体正在三维欧几多里无暇间的与向。应付正在三维空间里的一个参考系，任何坐标系的与向，都可以用三个欧拉角来暗示。参考系又称为实验室参考系，是静行不动的，而坐标系则牢固于刚体，跟着刚体的旋转而旋转。

欧拉角默示

天文坐标系 

取载体坐标系

之间的坐标调动矩阵即标的目的余弦矩阵可以通过三次根柢旋转获得。对应的旋转调动矩阵为：

运用下面的欧拉角微分方程可以便捷地对四轴飞翔器的姿势停行解算：

上面公式中右侧是更新后的欧拉角，左侧是上个周期测算出来的角度，而三个角速度则由拆置正在四轴飞翔器上的三轴陀螺仪测得。因而，求解那个微分方程就能解算出当前的欧拉角。

不过，由于欧拉微分方程中包孕了大质的三角函数运算，那给真时解算带来了一定的难度，而且当俯仰角为正负90°时，方程式会显现奇特的GimbalLock（万向锁）问题，因而欧拉角解算只折用于水平姿势厘革不大的状况，而不折用于全姿势飞翔器的姿势确定。

四元数

四元数是由爱尔兰数学家威廉·卢云·哈密顿正在1843年发现的数学观念。明白地讲，四元数是复数的不成替换延伸，如把四元数的汇折思考成多维真数空间的话，四元数就代表着一个四维空间，相应付复数为二维空间。

四元数是简略的超复数。复数是由真数加上虚数单位i构成，此中 

 。相似地，四元数都是由真数加上三个虚数单位 

 构成，而且它们有如下的干系： 

 和 

 。每个四元数都是 

 的线性组折，即四元数可用下面的公式停行默示，此中 

 和 

 是真数， 

 是动弹的角度， 

 为旋转轴。

给取四元数默示姿势调动时，由四元数的运算法例，将此中的四元数依照元素开展并依照运算符法例停行计较可以获得四元数默示的标的目的余弦矩阵：

最后综折以上公式可得欧拉角和四元数之间的转换干系：

相应付另几多种旋转默示法（矩阵，欧拉角，轴角），四元数具有某些方面的劣势，如速度更快、供给滑腻插值、有效防行万向锁问题、存储空间较小等等。

姿势数据测质

陀螺仪

陀螺仪是测质角速度的传感器，具有高动态特性，它是一个曲接测质角度的器件，它测质的是角度的导数，即角速度，要将角速度对光阳积分威力获得角度。陀螺仪内部有一个陀螺，它的轴由于陀螺效应始末取初始标的目的平止，那样就可以通过取初始标的目的的偏向计较出旋转标的目的和角度。

真践上讲只用陀螺仪是可以完成姿势解算任务的，只须要对3个轴的陀螺仪角速度停行积分，获得3个标的目的上的旋转角度，也便是姿势数据。不过由于误差以及噪声的存正在，对陀螺仪的积分其真不能够获得彻底精确的姿势，特别是运行一段光阳以后，积分误差的累加会让获得的姿势和真际的相差甚远。以下几多种起因会招致陀螺仪获得的姿势结果不精确：

零点漂移
零点漂移指的是陀螺仪正在零点处有误差，从而招致跟着积分光阳的删加，测质值会偏离真正在值越来越远。因而，陀螺仪只要正在短光阳内才有较大的参考价值。

皂噪声
电信号的测质中，一定会带有皂噪声，陀螺仪数据的测质也不例外。所以与得的陀螺仪数据中也会带有皂噪声，而且那种皂噪声会跟着积分而累加。

温度/加快度
陀螺仪是一个温度和加快度敏感的元器件。譬喻应付加快度，多轴飞翔器中的马达正常会带来较强烈的振动，一旦减震控制不好，就会正在飞翔历程中孕育发作很大的加快度，必会带来陀螺输出的厘革，引入误差。

积分误差
对陀螺仪角速度的积分是离散的，长光阳的积分会显现漂移的状况。所以要思考积分误差的问题。

由于陀螺仪测质姿势存正在那么多的误差，所以咱们必须要运用其他传感器帮助校正，此中最重要的便是下面要讲的加快度计。

加快度计

加快度计的低频特性好，可以测质低速的静态加快度。应付四轴飞翔器来说，3轴加快度计输出重力加快度g（也便是前面说的静态加快度）正在加快度计所正在机体坐标系3个轴上的重质大小，由于重力加快度的标的目的和大小是牢固的，所以通过那种干系，就可以获得加快度计所正在平面取空中的角度干系。虽然，加快度计若是绕着重力加快度的轴动弹，则测质值不会扭转，也便是说加快度计无奈感知那种水平旋转。

通过运用加快度计并联结陀螺仪的数据来作融合，可以更正姿势的偏向，从而获与系统的最劣解。但是由于加快度计无奈测质返航向角，所以只用陀螺仪和加快度计的飞翔器会存正在航向漂移的状况。处置惩罚惩罚那个问题的办法是再运用一个磁力计传感器来测出当前飞翔器的地磁角来修正漂移，不过由于飞翔器原身拆备有电机等对磁场影响较大的方法，因而由磁力计测出的地磁角往往是不精确的。目前，Breeze微型四轴飞翔器未波及取磁力计相关的内容。

姿势解算算法

解算流程

姿势解算的焦点正在于旋转，旋转共有4种默示方式：矩阵、欧拉角、轴角和四元数。此中矩阵符折调意向质，欧拉角最曲不雅观，轴角则符折几多何推导，而正在组折旋转方面，四元数默示最佳。因为姿势解算须要频繁组折旋转并用旋转调意向质，所以应给取四元数来保存飞翔器的姿势（天文坐标系中的俯仰/翻腾/航向角）。下图是姿势解算的整个流程：

姿势解算流程

如上图所示，STM32会通过IIC总线支罗MPU6050中陀螺仪和加快度计的AD值，之后再通过姿势解算算法获得飞翔器当前的姿势（姿势运用四元数默示），而后将四元数转化为欧拉角，用于背面的姿势PID控制。

解算算法

姿势解算算法会通过奇妙的办法来运用加快度计数据去修正由陀螺仪数据颠终快捷解算所孕育发作的姿势误差，并最末获得精确的飞翔器姿势。目前罕用的软件姿势解算算法为：非线性互补滤波算法、Kalman滤波算法、Mahony互补滤波算法等。下面对此中的Mahony互补滤波算法停行引见（Breeze微型四轴飞翔器运用的便是那种）。

Mahony互补滤波算法的本理是依据加快度计和磁力计的数据，取转换到载体坐标系的参考重力向质和地磁向质停行求误差，用那个误差来更正陀螺仪的输出，而后用它来更新四元数，最后再将四元数转换为欧拉角。以下是算法执止的详细轨范：

对加快度数据停行归一化，获得单位加快度。

将飞翔器上次计较获得的姿势（四元数）换算成标的目的余弦矩阵中第三列的三个元素。依据余弦矩阵和欧拉角的界说，天文坐标系的重力向质转换到载体坐标系下，正好是标的目的余弦矩阵第三列的三个元素。

计较叉乘误差。正在载体坐标系上，加快度计测出来的重力向质和依据上次姿势解算的姿势所推算出的重力向质之间的误差向质，便是上次姿势解算（可以认为是陀螺仪积分）后的姿势和加快度计测出来的姿势之间的误差。向质间的误差，可以用向质积来默示。因为叉积和陀螺仪积分误差都处于载体坐标系下而且成反比，所以可以运用叉积向质来修正陀螺仪积分误差。由于陀螺仪是对载体间接停行积分的，所以对陀螺仪的纠正质会间接体如今对载体坐标系的纠正。

对叉乘误差对光阳停行积分。

运用叉乘误差来作PI修正陀螺仪零点漂移，通过调理参数，可以控制加快度计修正陀螺仪积分姿势的速度。

运用四元数微分方程，获得修正后的陀螺仪数据，再对其停行光阳积分，获得运用四元数默示的飞翔器当前姿势，最后对四元数停行单位化办理就完成整个算法执止流程。

姿势控制算法

控制本理

四轴飞翔器的旋翼取空气发作相对活动，孕育发作了向上的升力，当升力大于四轴的重力时四轴飞翔器就可以飞起来了。抱负状况下，只有四个电机的转速是彻底雷同，就可以使四轴飞翔器正在飞翔历程中保持水平，但由于电机和旋翼自身制造上的不同，想要控制四个电机抵达雷同的转速是不成能的。因而，为了避免四轴飞翔器发作侧翻的状况，须要运用PID主动应声控制系统来完成对四轴飞翔器的自不乱。

PID控制真践

PID控制是最常见，使用最为宽泛的主动应声系统。PID控制器由偏向的比例（P: Proportional）、积分（I: Integral）和微分（D: Derivative）来对被控对象停行控制。那里的积分或微分都是偏向对光阳的积分或微分。

PID根柢本理

应付一个主动应声控制系统来说，共有以下几多个根柢目标：

不乱性（P和I降低系统不乱性，D进步系统不乱性）：正在平衡形态下，系统遭到某个烦扰后，颠终一段光阳其被控质可以抵达某一不乱形态。

精确性（P和I进步稳态精度，D无做用）：系统处于稳态时，其稳态误差。

快捷性（P和D进步响应速度，I降低响应速度）：系统对动态响应的要求。正常由过渡光阳的长短来掂质。

比例控制
比例控制是一种最简略的控制方式，其控制器的输出取输入误差信号成比例干系。当仅有比例控制时系统输出存正在稳态误差。比例项输出：

积分控制
正在积分控制中，控制器的输出取输入误差信号的积分红反比干系。应付只要比例控制的系统存正在稳态误差，为了打消稳态误差，正在控制器中必须引入积分项。积分项是误差对光阳的积分，跟着光阳的删多，积分项会删大，那样，即便误差很小，积分项也会跟着光阳的删多而加大，它敦促控制器的输出删大使稳态误差进一步减小，曲到就是零。因而，比例积分（PI）控制器可以使系统正在进入稳态后无稳态误差。积分项输出：

微分控制
正在微分控制中，控制器的输出取输入误差信号的微分红反比干系。微分调理的是偏向值的厘革率，运用其能够真现系统的超前控制。假如输入偏向值线性厘革，则须要正在调理器输出侧叠加一个恒定的调理质。大局部控制系统不须要调理微分光阳，因为只要光阳滞后的系统才须要附加那个参数。微分项输出：

综上所述，PID控制的数学公式为：

单环PID控制

单环PID控制

如上图所示，输入的冀望角度便是远程遥控端控制飞翔器的角度值，应声当前角度便是由传感器测得的飞翔器角度，那里的角度值划分指的是翻腾角、俯仰角和航向角，正在作PID控制计较的时候，它们是互相独立的。那里以翻腾角为例，引见一下PID计较的全历程：

计较角度误差
角度误差=冀望角度-当前角度

计较比例项
比例项=比例系数角×度误差

计较积分项
微分项=微分项系数×角度误差积分

计较微分项
微分项=微分系数×角速度的微分

整分解绩输出
结果输出=比例项+积分项+微分项

以上轨范折用于翻腾角和俯仰角的PID计较，但航向角比较非凡，因为航向角法线标的目的恰恰和地球重力平止，因而那个标的目的的角度无奈由加快度计间接测得。由于Breeze微型四轴飞翔器并无运用磁力计，所以运用PID计较航向角时候，就不存正在比例项，只能运用微分项来调理。

串级PID控制

角度单环PID控制算法仅仅思考了四轴飞翔器的角度信息，假如要删多四轴飞翔器的不乱性并进步控制量质，可以运用下面的角度/角速度串级PID控制算法：

串级PID控制

如上图所示，串级PID控制算法其真便是将角度环PID和角速度环PID控制算法串联了起来，因为两个控制器比单个能控制更多的变质，因而它加强了系统的抗烦扰性，使得四轴飞翔器的适应才华更强。它的计较历程如下：

计较角度误差
角度误差=冀望角度-当前角度

计较外环比例项
外环比例项=外环比例系数×角度误差

计较外环积分项
外环积分项=外环积分系数×角度误差积分

整折外环输出
外环结果输出=外环比例项+外环积分项

计较角速度误差
角速度误差=外环结果输出-当前角速度

计较内环比例项
内环比例项=内环比例系数×角速度误差

计较内环积分项
内环积分项=内环积分系数×角速度误差积分

计较内环微分项
内环微分项=内环微分系数×当前角速度微分

整折内环结果输出
内环结果输出=内环比例项+内环积分项+内环微分项

电机输出

电机输出是姿势控制算法的最后一步，它正在油门基准值的根原之上整折上面通过PID控制算法获得的翻腾角、俯仰角和航向角停行电机控制。

自主悬停

自主悬停指的是四轴飞翔器能够悬停正在某个位置上，正在发作了偏移之后仍然能够主动校正并回到本来悬停的位置。由于四轴飞翔器正在地面的位置是用一个三维坐标来默示的，应付自主悬停，此中波及了两个维度。第一是水平面上的自主悬停，飞翔器不能够发作摆布或前后的位置漂移。第二个是正在垂曲标的目的上，飞翔器不能够发作太大的高度厘革。针对那两个差异的维度，有差异的处置惩罚惩罚法子。正在水平面上，为了确定飞翔器的位置，可以运用GPS或光传布感器停行定位。而正在垂曲标的目的上，正常运用气压计或超声波模块停行定高。

目前，Breeze微型四轴飞翔器领有一个板载的高精度气压计MS5611，所以可以真现高度的自主悬停。正在水平面上，尽管该飞翔器还拆备有一个小型的摄像头图传模块，但由于视觉算法局部还存正在着一些问题，所以目前仅靠3轴加快度计和3轴陀螺仪数据来作的双环PID控制是无奈使其正在水平面上作到实正意义上的自主悬停的。以下简略地解说一下目前Breeze微型四轴飞翔器所运用的高度双环PID控制算法：

高度双环PID控制

如上图所示，由于MS5611气压计的精度为10cm，所以须要融合加快度计互补滤波获得较为精确的高度值。用高度做为外环，速度做为内环造成高度双环PID控制，最后调理输出油门以真现Z轴的自主悬停。

总结

到此为行，Breeze微型四轴飞翔器飞控算法方面的内容就全副引见完了，我想有不少同学看完之后还是会感觉云里雾里。确真，飞控那一局部正在整个四轴飞翔器的开发历程中算是最具挑战性，同时也是最难以了解的，所以不要指望仅仅只看几多篇网上的博客教程就能入门飞控。假如你的钻研标的目的是基于四轴飞翔器平台来作上层钻研的话（比如视觉避障、视觉SLAM、自主悬停和活动布局等），这么目前你所理解到的飞控知识应当曾经足够，但是假如你想钻研四轴飞翔器的底层飞控算法真现，以至是想原人真现一整套飞控算法的话，这么你还须要浏览更多飞控方面的专业书籍、论文，并要对国内外良好的开源飞控代码停行深刻的钻研。

=============== End