1.导言
四轴飞翔器是一种运用四个旋翼作为飞翔引擎来进行空中飞翔的飞翔器。进入20世纪以来，电子技术飞速开展四轴飞翔器开端走向小型化，并融入了人工智能，使其开展趋于无人机，智能机器人。
四轴飞翔器不光完结了直升机的笔直升降的飞翔功用，一起也在必定程度上下降了飞翔器机械结构的规划难度。四轴飞翔器的平衡操控体系由各类惯性传感器组成。在制造进程中，对全体机身的中心、对称性以及电机功用要求较低，这也正是制造四轴飞翔器的优势地点，而且相较于固定翼飞机，四轴也有着可笔直起降，机动性好，易维护等长处。
在实践运用方面，四轴飞翔器可以在杂乱、风险的环境下可以完结特定的飞翔使命，也可以用于监控交通，环境等。比方，在四轴飞翔器上设备甲烷等有害气体的检测设备，则可以在高空定点地检测有害气体；进入辐射区查看核设施；做军事侦查；乃至转移资料，建立房子等等。本规划运用四轴搭载云台完结航拍使命，当然经过改装也可以履行其他使命。
本规划首要研讨了四轴飞翔器的姿势结算和飞翔操控，并规划制造了一架四轴飞翔器，对要害传感器做了标定，并运用用matlab剖析数据，规划算法，还进行了单通道平衡实验调试，进行试飞实验取得了必定的作用。
2.体系计划
本规划选用STM32F4作为中心处理器，该处理器内核架构 ARM Cortex-M4，具有高功用、低成本、低功耗等特色。
主控板包含传感器MPU6050电路模块、无线蓝牙模块、电机发动模块，电源办理模块等；遥控运用产品遥控及接收机。操控芯片捕获接收机的PPM指令信号，传感器与操控芯片之间选用IIC总线衔接，MCU与电调之间用PWM传递操控信号。
软件算法才用依据四元数的互补滤波解算姿势叫，操控算法才用经典PID操控器操控云台舵机和四轴电机。如图2-1为本规划全体框图。
3.体系硬件规划
针对前面提出的全体规划计划，本规划采纳模块化战略,将各个功用部分开来规划，终究组合起来。
3.1 电源办理模块
四轴飞翔器要求全体规划质量较轻，体积较小，因而在电池的选取方面，选用体积小、质量轻、容量大的锂电池供电最适宜。体系的中心芯片为STM32F103，常用作业电压为3.3V，一起惯性丈量传感器，蓝牙通讯模块的惯例供电电压也为3.3V，锂电池的电压为11.4V，要使体系正常作业，需要将11.4V的锂电池电压稳压到3.3V。常用的78系列稳压芯片已不再适用，有必要挑选功用更好的稳压芯片。
经归纳考虑，本电路选用LM1117-3.3和LM2940-5电源部分的中心芯片。电池电源经过LM2940-5降到5V后在输入LM1117-3.3稳压为3.3V。因为电机部分电流较大，故在飞控电路部分参加了过流维护，运用500mA的保险丝。电路图如下。
表3-1 四轴飞翔器硬件清单
4.体系软件规划
软件规划上由操控中心STM32F4读取传感器信息，解算姿势角，以姿势角为被操控量交融遥控信息后，输出到四个电机及两个舵机以完结四轴飞翔操控和云台的安稳补偿。下图是软件流程：
4.1.四元素核算姿势角的完结
依据前面给出的姿势解算方程与四元数，即可得到姿势核算体系的核算原理如下图（4-1）
本规划依据互补滤波的思想上完结的四元素算法，其间心思路为运用加快度测得的重力向量与估量姿势得到重力向量的差错来纠正陀螺仪积分差错，然后运用纠正后的陀螺仪积分得到姿势角。
首要无妨设处理后的加快度数据为：ax,ay,az，单位m/s^2。加快度计的向量为 （ax,ay,az）陀螺仪数据为：gx,gy,gz，单位rad/s。陀螺仪向量 （gx,gy,gz），由式（4-5）可得由载体到导航坐标系的四元数方式转化矩阵为：
依据余弦矩阵和欧拉角的界说，地舆坐标系的重力向量，转到机体坐标系，是 中的第三列的三个元素，即
。所以加快的向量与估量重力向量叉积：
然后运用向量的叉积， 可视为差错向量，这个叉积向量仍旧是坐落机体坐标系上的，而陀螺积分差错也是在机体坐标系，而且叉积的巨细与陀螺积分差错成正比，正好拿来纠正陀螺。因为陀螺是对机体直接积分，所以对陀螺的纠正量会直接表现在对机体坐标系的纠正。用上面得到的成果校对陀螺仪：
此处k为一个常量系数。
再运用二阶毕卡法解四元数微分方程（4-6），更新四元数为下一次核算做准备。毕卡二阶算法为：
终究将四元数转变为欧拉角：
数据流程图如下：
经过以上算法我成功得出了飞翔器的姿势角，在敞开电机的情况下，视点差错在+/-2°以内，满意了操控要求。姿势核算作用如下图所示，其间赤色和蓝色是直接由加快度核算出的俯仰角和横滚角，青色和黄色为姿势结算后的俯仰角和横滚角。由图中数据可看出，解算的姿势角不仅能即时的反响视点改动切曲线滑润，阐明姿势解算算法有用。
4.2.操控规划
因为四轴飞翔器共同的机械结构，即结构上的对称规划，使得四轴在俯仰角的操控欲横滚角的操控上有这近乎相同的操控特性，且两者相对独立。四轴飞翔器的俯仰，横滚，偏航，升降可以经过四个输入量来操控。经过设定一个希望视点，调整电机转速，使得测得的姿势角安稳在希望角。操控律的规划首要选用是闭环操控。以姿势角做为被操控量，选用经典的PID操控算法。
四轴飞翔器体系是一个时变且非线性的体系，选用传统PID算法的单一的反响操控会使体系存在不同程度的超谐和振动现象，无法得到抱负的操控作用。本文将前馈操控引进到了四轴飞翔器体系的操控中，有用地改进了体系的实时性，提高了体系的反响速度；而且依据四轴飞翔器体系的特色，对数字PID算法进行了改进，引进了微分先行环节，改进了体系的动态特性；使得操控器可以更好地习惯四轴飞翔器体系的实践情况。
在姿势角的操控中，本规划将操控器捕获到的遥控器信号转化为一个视点，作为希望角，与解算出来的丈量角作差，得到差错error。将error乘以一个份额系数kp。在只要kp作用下，体系会有静差所以考虑运用积分ki操控消除稳态差错。但积分操控会下降体系的动态功用，乃至形成闭环体系不安稳，因而要对积分进行限幅，防止积分过大。
关于微分，假如选用传统的D办法，在人为操作四轴时会产生输入的设定值改动频频且起伏较大，然后形成体系的振动。对人为操控非常晦气，为了处理设定值的频频改动给体系带来的不良影响，本文在姿势角操控上引进了微分先行PID算法，其特色是只对输出量进行微分，即只对陀螺仪角速度丈量值进行微分，而不对姿势角的设定值进行微分。这样，在设定值产生改动时，输出量并不会改动，而被控量的改动相对是比较平缓的，这就很好地防止了设定值的频频改动给体系形成的振动，显着地改进了体系的动态功用。操控周期定为4ms，姿势操控体系示意图如下：
图5-1姿势操控体系示意图
经过前面一章的介绍咱们现已的到达了俯仰，横滚，航向三个操控量，然后将它们别离输入三个独立的如上图所示的PID操控器，咱们可以得到三个PID输出：pid_roll，pid_pitch，pid_yaw将这三个输出量做简略的线性运算输出给电机。部分代码如下：
#define PIDMIX(X,Y,Z) Motor_Thr + pid_pitch* Y+ pid_roll*X + pid_yaw*Z
MOTOR1=MOTORLimit(PIDMIX(+1,+1,-1)); //REAR_R 后右电机
MOTOR2=MOTORLimit(PIDMIX(-1,-1,-1)); //FRONT_R 前右电机
MOTOR3=MOTORLimit(PIDMIX(-1,+1,+1)); //REAR_L 后左电机
MOTOR4=MOTORLimit(PIDMIX(+1,-1,+1)); //FRONT_L 前左电机
4.3. PID参数调理
PID参数的整定是PID操控的要害环节，直接影响到操控的作用。故一个PID规划的好不好往往要看其参数能否调理好，本规划的PID参数调理选用凑试法。凑试法是经过实践的闭环体系，经过调查体系的呼应曲线，在本规划中经过调查被调量，PID输出，设定值三条曲线，判别出kp,ki,kd对体系呼应的影响，重复测验，终究到达满意呼应，然后到达确认操控参数的kp,ki,kd的意图。在参数调理进程总遵从以下准则[17]:
（1）在输出不振动时，增大份额增益P。
（2）在输出不振动时，减小积分时刻常数Ti。
（3）在输出不振动时，增大微分时刻常数Td。
（4）一般过程：
a.确认份额增益P
确认份额增益P时，首要去掉PID的积分项和微分项，一般是令Ti=0、Td=0（详细见PID的参数设定阐明），使PID为纯份额调理。输入设定为体系答应的最大值的60%~70%，由0逐步加大份额增益P，直至体系呈现振动；再反过来，从此刻的份额增益P逐步减小，直至体系振动消失，记载此刻的份额增益P，设定PID的份额增益P为当时值的60%~70%。份额增益P调试完结。
b.确认积分时刻常数Ti
份额增益P确认后，设定一个较大的积分时刻常数Ti的初值，然后逐步减小Ti，直至体系呈现振动，之后在反过来，逐步加大Ti，直至体系振动消失。记载此刻的 Ti，设定PID的积分时刻常数Ti为当时值的150%~180%。积分时刻常数Ti调试完结。
c.确认积分时刻常数Td
积分时刻常数Td一般不必设定，为0即可。若要设定，与确认 P和Ti的办法相同，取不振动时的30%。
d.体系空载、带载联调，再对PID参数进行微调，直至满意要求。
（5）终究调试好的规范应该是，PID输出曲线在有一个阶跃呼应来是，呼应一大一小两个波，小波是大波的四分之一。
四轴飞翔器的PID整定，咱们首要四轴固定在单轴平衡渠道上，让飞翔器完结单轴平衡，首要调查姿势角的（1）安稳性，能否平衡在希望视点；（2）呼应性，当操作指令改动时，四轴能否即时的呼应希望的改动；（3）操作性，由操作员感触四轴的姿势是否已与操作，会不会产生呼应过冲。
在参数调整时，先调P,将I,D给0，先给一个小值P1，假如飞翔器不能安稳在一个视点，则P1给小了，下一次给一个较大值P2，假如飞翔器产生震动则证明P2给大了，那么适宜的P在P1-P2之间，重复实验几回可找到P震动的临界点P0,然后坚持P0不变依照调P的办法来调D,D是用来消除差错的，当颤动差不多被消除时，此刻咱们有较适宜的P0,D0，在这两个值邻近再试几组参数，调查作用得到最优参数。调好P,D后此刻四轴的安稳状况与希望状况间也学会有静差，接下来参加I，参数有小到大，当静差差不多被消除时，咱们再对P,I,D三组参数在小范围内联调。终究确认恰当的积分限幅值，完结整个PID参数的整定。
5.体系立异
- 选用STM32F407这样一款高功用芯片作为操控中心，核算快速，扩展空间大。
- 云台飞控一体化规划，既能完结飞翔使命也能完结云台安稳。
- 姿势算法选用依据四元数的互补滤波，姿势角无奇点，比起卡尔曼李春波等高端算法有着核算量小的特色且能投入运用，大大节省了cpu核算时刻，也下降了对cpu的功用要求。
- 运用四轴作为云台载体有着灵活机动，可让摄像头取得比较好的视界，且云台能消除四轴机体颤动。
- 便利改装用于履行其他使命。
6.评测与定论
6.1.四轴飞翔器的调试
6.1.1电源调试
将电路板焊接完结后，为电路板供电，用万用表测得个供电芯片电压如下
表6-1各电源芯片电压值
|
元件名 |
输入电压（V） |
输出电压（V） |
|
LM2940 |
12.4 |
5.01 |
|
LM1117 |
5.01 |
3.42 |
由表中数据可知电源办理部分可满意体系供电要求。
6.1.2 姿势角调试
在完结操控器底层的硬件驱动后，开端姿势角算法的调试。运用蓝牙模块将解算出的姿势角数据发回上位机，摇摆四轴机体，调查上位机数据曲线与姿势演示立方块。
图6-1 姿势角数据对比曲线
图6-1中黄色和蓝色为直接由加快度核算出的俯仰与横滚角，赤色和青色为姿势解算后的姿势角，可显着看出直接由加快度核算出的姿势角噪声大，不滑润，不能实在反映姿势角的改动。而由四元数算法解算后的姿势角反响快，噪声小，足以满意操控要求。光调查曲线并不直观，还可以调查上位机中的立方块，当小立方块的姿势与四轴机体可以坚持一致改动时阐明姿势解算杰出。经过以上手法可知姿势角解算满意需求。
6.1.3 操控电机调试
在第五章，现已针对四轴飞翔器进行了PID的操控规划,但为了得到实践运用的PID操控参数咱们还需要经过重复实验来整定出最优参数。为了便利参数的调试，在本规划进程中才用了先调平衡一个方向，再调另一个方向，终究联到达平衡的作用。为此规划了如下图所示试实验渠道：
图6-3 四轴单轴平衡实验
在调试进程中，为了防止频频的下载程序带来的不变，选用串口蓝牙发送参数的做法，大大提高了调试功率。当俯仰与横滚两个轴都可以独立的平衡时，可以脱离单轴渠道开端在空中进行航向角的安稳性调试。
6.2.四轴飞翔器的试飞
当时面的各项调试都完结了之后，就准备要进行试飞实验。飞翔实验是对操控体系的功用和技术指标进行验证的终究手法，也是衡量四轴规划是否成功的重要标志。
试飞前要保证体系各部分作业正常，安稳。查看各个接口衔接是否正确，各部件设备是否结实，电池电量是否足够。翻开电源前查看遥控器油门是否在最低方位，起飞前先轻推油门保证电机作业正常。全部准备就绪，即可进行试飞。将四种飞翔器放在水平地上上，开端发动姿势初始化程序，听到电调提示音后，缓慢添加油门，螺旋桨转速上升，将飞机拉离地上。由实践情况可看出当姿势产生歪斜时，姿势解算及PID操控可以及时调整电机转速，安稳飞翔姿势。飞翔器还能依据遥控指令的改动完结相应的动作。本此规划的四轴飞翔器可完结笔直升降的要求，能坚持姿势的安稳，机体晃动小，在和风的搅扰下可以主动调整姿势，保证平稳飞翔，且体系呼应快，续航时刻大概在8分钟。因而本次的规划是有用的。
附录
部分代码
void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az)
{
static float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;
float delta_2=0;
const static float FACTOR = 0.002;
float norm=0.0f;
float vx, vy, vz;
float ex, ey, ez;
float q0q0 = q0*q0;
float q0q1 = q0*q1;
float q0q2 = q0*q2;
float q1q1 = q1*q1;
float q1q3 = q1*q3;
float q2q2 = q2*q2;
float q2q3 = q2*q3;
float q3q3 = q3*q3;
norm = sqrt(ax*ax + ay*ay +az*az); // 丈量正常化 把加快度计的三维向量转成单位向量。
ax = ax /norm;
ay = ay / norm;
az = az / norm;
vx = 2*(q1q3 – q0q2); // 估量方向的重力
vy = 2*(q0q1 + q2q3);
vz = q0q0 – q1q1 – q2q2 + q3q3;
ex = (ay*vz – az*vy);
ey = (az*vx – ax*vz);
ez = (ax*vy – ay*vx);
halfT=0.002;
gx = gx + ex*FACTOR/halfT; //校对陀螺仪丈量值 用叉积差错来做PI批改陀螺零偏
gy = gy + ey*FACTOR/halfT;
gz = gz + ez*FACTOR/halfT;
delta_2=(2*halfT*gx)*(2*halfT*gx)+(2*halfT*gy)*(2*halfT*gy)+(2*halfT*gz)*(2*halfT*gz);
q0 = (1-delta_2/8)*q0 + (-q1*gx – q2*gy – q3*gz)*halfT; // 整合四元数率 四元数微分方程 四元数更新算法，二阶毕卡法
q1 = (1-delta_2/8)*q1 + (q0*gx + q2*gz – q3*gy)*halfT;
q2 = (1-delta_2/8)*q2 + (q0*gy – q1*gz + q3*gx)*halfT;
q3 = (1-delta_2/8)*q3 + (q0*gz + q1*gy – q2*gx)*halfT;
norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3); // 正常化四元
q0 = q0 / norm;
q1 = q1 / norm;
q2 = q2 / norm;
q3 = q3 / norm;
//转化为欧拉角
Q_ANGLE.Pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch
Q_ANGLE.Roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 – 2 * q2* q2 + 1)* 57.3; // roll
Q_ANGLE.Yaw = -atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2*q2 – 2 * q3 * q3 + 1)* 57.3; // yaw
}
