案例分析—从理论上详解四轴飞行器飞行事理与双闭环PID控制_油门_传感器

四轴翱翔器是微型翱翔器的个中一种，相对付固定翼翱翔器，它的方向掌握灵巧、抗滋扰能力强、翱翔稳定，能够携带一定的负载和有悬停功能，因此能够很好地进行空中拍摄、监视、侦查等功能，在军事和民用上具备广泛的利用前景。

四轴翱翔器关键技能在于掌握策略。
由于智能掌握算法在运行繁芜的浮点型运算以及矩阵运算时，微处理器打算能力受限，难以达到翱翔掌握实时性的哀求；而PID掌握大略，易于实现，且技能成熟，因此目前主流的掌握策略紧张是环绕传统的PID掌握展开。

1 四轴翱翔器的构造与基本翱翔事理

四轴翱翔器构造紧张由主控板和呈十字交叉构造的4个电子调速器、电机、旋浆组成，电机由电子调速器掌握，主控板紧张卖力解算当前翱翔姿态、掌握电调等功能。

以十字翱翔模式为例，l号旋翼为头，1、3号旋翼逆时针旋转，2、4号旋翼顺时针旋转，如图1所示。

图1 四轴翱翔器构造图

参照翱翔状态表1变革电机转速，由于四个电机转速不同，使其与水平面倾斜一定角度，如图l所示。
四个电机产生的协力分解为向上的升力与前向分力。
当重力与升力相等时，前向分力驱动四轴翱翔器向倾斜角度的方向水平翱翔。
空间三轴角度欧拉角分为仰俯角、横滚角、航向角：倾斜角是仰俯角时，向前、向后翱翔；倾斜角是横滚角时，向左、向右翱翔；而倾斜航向角时，向左、右旋转运动，左(右)旋转是由于顺时针两电机产生的反扭矩之和与逆时针两电机产生的反扭矩之和不等，即不能相互抵消，机身便在反扭矩浸染下绕z轴自旋转。

表1 十字翱翔模式翱翔状态表

2 姿态解算

四轴翱翔器利用姿态解算打算出空间三轴欧拉角。
构造框架如图2所示，陀螺仪采样三轴角速率值，加速度传感器采样三轴加速度值，而磁力传感器采样得到三轴地磁场值，将陀螺仪、加速度传感器、磁力传感器采样后的数据进行标定、滤波、校正后得到三轴欧拉角度，个中陀螺仪和加速度传感器选用MPU6050芯片，磁力传感器选用HMC5883L芯片，采取IIC总线与主控板通信。

图2 姿态解算构造图

由于传感器存在器件偏差，因此在利用前须要标定。
陀螺仪在静止时，角速率为0；但实际情形由于器件偏差并不为0，因此可在静止时采样500次数据，再求均匀，得出偏移量，标定陀螺仪数据；加速度传感器可依据在静止时，三轴重力加速度平方和的开方为重力加速度的标定方程，利用最小二乘法求出标定偏移值和偏差值进行标定。
而磁力传感器校正，可将器件静置于桌面旋转一周找出最小值和最大值，通过电子罗盘校正打算公式打算出标定偏移值和偏差值。

由于陀螺仪永劫光采集角速率会有飘移，因此须要利用加速度传感器的值与磁力传感器的值进行校正。
将加速度的丈量矢量和磁场的丈量矢量与参考矢量做叉积后相加

式(1)中：ex、ey、ez为两叉积之和，ax、ay、az为加速度的丈量矢量，mx、my、mz为磁场的丈量矢量，axref、ayref、azref为加速度的参考矢量，mxref、myref、mzref为磁场的参考矢量，参考矢量是通过实时四元数值与本次丈量值打算出来。

再将叉积改动角速率漂移值：

式(2)中ωx(t)、ωy(t)、ωz(t)为角速率，kpex(t)为比例项改动，

为积分改动项。

将校正后的角速率通过二阶毕卡算法转化为四元数，公式如式(3)。

3 高度打算

高度打算是通过气压传感器采集的大气压值打算出来，将气压传感器采集值进行校正后，在通过温度二阶补偿，得到准确的大气压值，末了经由气压转换为高度公式

式(6)中Altitude为打算出来的实际高度，CurrentPressure为当前气压值，StartPressure为起飞之前气压值。
气压传感器选用MS5611芯片，个中集成了温度传感器和气压传感器，采取IIC总线与主控板通信。

4 PID掌握

4.1 双闭环PID掌握

当四轴翱翔器正常翱翔时，突遇外力(风等)或磁场滋扰，使加速度传感器或磁力传感器采集数据失落真，造成姿态解算出来的欧拉角缺点，只用角度单环情形下，使系统很难稳定运行，因此可以加入角速率作为内环，角速率由陀螺仪采集数据输出，采集值一样平常不存在受外界影响情形，抗滋扰能力强，并且角速率变革灵敏，当受外界滋扰时，回答迅速；同理，高度环中气压传感器同样也会受到外界滋扰，引入z轴加速度环可有效避免外界滋扰造成的影响，增强了系统的鲁棒性。

四轴翱翔器双闭环PID掌握，如图3、图4所示。
角度作为外环，角速率作为内环，进行姿态PID掌握；当须要定高时，高度作为外环，z轴加速度作为内环，进行高度PID掌握。
个中，PID输出为油门值，油门给定电子调速器值，电子调速器掌握电机使空间三轴欧拉角和高度变革。

图3 姿态PID掌握总体流程图

图4 高度PID掌握总体流程图

PID掌握算法采取位置式数字PID掌握：

式(7)中u(t)为PID输出值，e(t)为期望值与实际值之差，

为积分量，

为微分量，kp、ki、kd为比例、积分、微分系数。

在将积分量，微分量离散化得到PID打算公式

式(8)中T为更新韶光。

基于公式(8)，姿态PID掌握算法

式(9)为角度环PID打算公式，(10)为角速率环PID打算公式。
AngelPIDOut(t)为角度环PID输出，AngelRatePIDOut(t)为角速率环PID输出，e(t)=期望角度一实际角度，e＇(t)=AngelPIDOut(t) - 实际角速率。

同理高度PID掌握算法：

式(11)为高度环PID打算公式，公式(12)为加速度环PID打算公式，AltitudePIDOut(t)为高度环PID输出，AcceleratePIDOut(t)为加速度环PID输出。
e(t)=期望高度一实际高度， e＇(t)=AltitudePIDOut(t) - (z轴加速度 - 重力加速度值)。

4.2 油门输出打算

PID输出值先经由限幅处理，再输出给油门，防止某些时候输出油门值过大，造成过冲，使系统难以稳定，公式如下

AngelOut=AngelPIDOut(t)(限定范围 - 100到100) (13)

AltitudeOut=AcceleratePIDOut(t)(限定范围 - 10到10) (14)

式中AngelOut。
为欧拉角输出值，AltitudeOut为高度输出值。

末了经由十字翱翔模式油门输出公式，打算出4个电机输出油门

式(15)中throttlelOut到throttle4Out为油门1到油门4输出值，依据公式(13)输出的AngelOut分为三轴角度：pitchOut为仰俯角输出值，rollOut为横滚角输出值，yawOut为航向角输出值。

4.3 油门补偿

以十字翱翔模式为例，PID掌握条件为线性环境，而给出的油门值与转速的关系为非线性，且两对向电机再同一油门下转速存在差异。
利用光电传感器测出不同油门量对应的转速，通过matlab软件绘制出该曲线，个中一对向电机油门与转速的关系如图5所示，两电机在相同油门下转速存在差异且不平行，因此将油门曲线分段，并通过打算公式

补偿油门值 =α(实际油门值 - β) (16)

进行油门补偿，式(16)中α、β为补偿偏移系数，得出曲线如图5(b)所示。

图5 油门 - 转速曲线图

5 测试

本次测试姿态解算更新周期T ≈ 4 mS，PID掌握更新周期T ≈ 10 ms，期望值为水平0度。
将四轴翱翔器掌握仰俯角的一对电机固定住，另一对边能自由旋转，即能改变横滚角度，如图6所示。
将横滚角倾斜到 - 70°，遥控器油门迅速推到1400，平衡后如图6所示。

图6 姿态PID掌握测试

将四轴翱翔器采集的欧拉角值通过无线模块NRF24L01发送到PC机上，吸收的数据显示到上位机进行剖析。
上位机显示横滚角波形如图7所示，波形图横坐标单位为10 ms，纵坐标单位为度。

图7 双闭环PID掌握横滚角波形图

由图7可知，横滚角波形经由一大波一小波震荡后近似归为期望值零点，且超调量较小，系统很快进入稳定状态。

其他欧拉角测试结果类似。

在加入双闭环高度PID掌握，四轴翱翔器在室外翱翔效果如图8所示，可以看出翱翔器翱翔稳定，达到设计哀求。

图8 室外翱翔测试

6 结论

紧张研究了基于四轴翱翔器的双闭环PID掌握算法。
在姿态PID掌握和高度PID掌握等分别增加了内环角速率环和加速度环，不仅抗滋扰能力强，并且回答迅速，增强了系统的鲁棒性。