四旋翼飞行器姿态角的自切换串级PID控制方法

丁 娃，吴功平，何文山，李谋远

（武汉大学动力与机械学院，湖北 武汉 430072）

1 前言

四旋翼无人机在空间中没有约束，具有六个自由度，但只有机臂末端的四个电机作为输入量进行控制。它是典型的欠驱动系统，该系统的特点还在于强耦合、非线性和多变量[1]。因其飞行原理简单、结构紧凑、单位体积提供扭矩大、机动性能好等特点而被应用于不同行业，在航拍、农业喷洒、军事侦察、公安追捕、高压电力巡检等方面发挥着日益重要的作用[2]。在四旋翼飞行器的控制当中，姿态控制部分是整个飞行控制系统的核心部分，在国内外被广泛研究。四旋翼飞行器可垂直起飞和降落，便于操控，机动性灵活。但是当四旋翼飞行器面对恶劣的环境时，如空气气流的干扰会导致飞行失稳，改进控制算法的优越性对于保护飞行器安全的飞行和管理、提升作业质量具有重要的意义。

最近十几年来，大量的控制算法被应用于四旋翼飞行器的姿态控制。文献[3]提出采用滑膜控制可以有效的控制系统稳定，但是其鲁棒性取决于良好的模型参数，而这些参数很难精准测量。文献[3]提出采用自适应的控制方法可以解决系统参数的不确定性，但是外部干扰的影响并未能得到充分的解决，如气流干扰。文献［4］提出采用基于神经网络的控制方法，但是这种方法采用神经单元直接逼近不确定性，使得计算量更大，对硬件要求也更高。目前，主流应用的开源算法中主要采用双环PID控制，外环作用于姿态角、内环作用于角速度，这种算法可以达到一定的效果，但控制器参数固定不变，只能将姿态控制在一定小角度范围内，另外，其控制稳定性取决于良好的数学模型，室外作业环境多变，难以达到理想的控制效果［5］。基于此衍生出了很多控制方法，如串级自抗扰控制、基于神经网络的PID控制，但是这些方法使得原本就欠驱动的系统更加复杂，复杂的浮点型运算及矩阵运算对处理器的计算能力要求也更高［6］。

兼顾飞机稳定性与实际运行效率，提出一种新的串级PID分控系统，外环综合模糊PID和经典PID特点，自主切换，期望值与实际值相差较大时采用前者，不依赖精确模型、自适应性好，可快速恢复，使系统获得良好的动态性能；期望值与实际值相差较小时采用后者，简单有效，可消除静态误差，使系统获得良好的稳定性；并对切换过程进行分析设计，使两种控制方法平稳过渡。内环采用PD控制器，提升整个系统的响应速度。

2 建立四旋翼模型

对四旋翼模型的运动学和动力学分析，是控制仿真的基础。为便于进行串级PID分控模拟，在构建数学模型时做出如下假设：

（1）四旋翼的质心与机体坐标的原点一致；（2）地理坐标系是惯性坐标系。即高度发生变化时，重力加速度值固定不变；（3）四旋翼为刚体，不会产生变形；（4）忽略空气摩擦，忽略无人机平面与旋翼之间的高度差。

为建立适当的四旋翼动力学模型，还需引入合适的坐标系。目前，无人机导航中常用的坐标系包括地心惯性坐标系、地球固联坐标系、地理坐标系、机体坐标系等。如图1所示，综合分析对比后选择地理坐标系（n系）与机体坐标系（b系）相结合的方式进行解算［8］。

图1 四旋翼机体坐标系与地理坐标系Fig.1 Airframe Coordinate System and Geographical Coordinate System of Four Rotors

经过三次绕坐标轴转动后，地理坐标系可转换至机体坐标系，利用数学矩阵表达式可推导出由地理坐标系向机体坐标系的转换矩阵W［9］：

式中：θ—俯仰角；γ—横滚角；ψ—偏航角。假设四旋翼机体完全对称；m—无人机的重量；Ixx、Iyy、Izz—飞行器绕x轴，y轴和z轴的转动惯量；Jr—旋翼绕转动轴的转动惯量；Ω =[Ω1Ω2Ω3Ω4]T—、四个旋翼的转速矢量和；Ω=-Ω1+Ω2-Ω3+Ω4—四个旋翼转速的总和；U1—四个旋翼生成的合力；U2、U3、U4—四旋翼绕三个坐标轴转动的力矩。定义由无人机旋翼生成的力与力矩矢量U作为控制系统的输入，可得：

式中：l—四旋翼几何中心与电机中心之间的长度，单位为m；b—推力系数，单位为N·s2；d—阻力系数，单位为N·m·s2。

经过一系列的转换计算，最终得出飞行器在地理坐标系中的动力学方程如上式[10]。

3 串级PID分控器设计

由式（2）可知，四旋翼飞行器为多输入多输出的非线性系统，且实际飞行时周围环境多变，为了达到较好的控制效果，使其能迅速调整，保持一定的稳定性，如图2所示。采用串级PID分控器。其外环控制姿态角，由模糊PID 控制和PID 控制两部分组成，实际运行时根据输入变量的情况在两者之间自主切换，并设置平滑切换方式进行过渡，内环为PD 控制器，对角速度进行控制，调整速度较快。

图2 四姿态角串级PID分控原理图Fig.2 Four Attitude Angle Cascade PID Control Schematic Diagram

3.1 构建PID控制器

这里所使用的PID 控制器和PD 控制器均基于Parallel PID Controller[11]，在该控制器的基础上，构建模糊PID 控制器。选取姿态角偏差e和偏差变化率ec构成输入语言变量。

建立一种双输入三输出的模糊控制器，对原有PID的结构进行修正，利用模糊控制器的输出值对固定参数Kp，Ki，Kd进行实时修整，实现构建模糊PID控制器的目的。其结构示意图，如图3所示。

图3 模糊PID结构示意图Fig.3 Structure Schematic Diagram of Fuzzy PID

3.1.1 变量模糊化

通过实验确定两个输入变量的基本论域为e=［-5，5］和ec=［-4，4］，将其划分为7个模糊子集：｛负大，负中，负小，零，正小，正中，正大｝（即对应英文简写NB，NM，NS，ZO，PS，PM，PB），输出论域为Kp为［-2，2］，Ki为［-1，1］，Kd为［-5，5］。

同样划分为7个模糊子集。综合考虑系统性能后将隶属度函数选取为三角形隶属函数，以Kp为例，利用Simulink中的fuzzy模块可得其隶属度函数，如图4所示。

图4 Kp隶属函数Fig.4 Membership Function Graph of Kp

3.1.2 模糊推理及去模糊

根据建立的非线性四旋翼飞行模型，设定相应的模糊控制规则，建立原则如下：

当e较大时，选取较大的Kp来提升系统反应速度，选取较小的Ki来减少超调，选取较小的Kd；

当e和ec变化中等大小时，选取较小的Kp，Kd取适中值；

当e较小时，Kp应取较大值，增大Ki值减小静态误差。

考虑到模糊规则对模糊控制器而言至关重要，所以在初步建立起该规则后，结合整体实验效果和专家经验做出了一些调整，最终建立模糊控制规则表具体，如表1～表3所示。

表1 Kp模糊控制规则表Tab.1 Basic Fuzzy Control Rule Table of Kp

表2 Ki模糊控制规则表Tab.2 Basic Fuzzy Control Rule Table of Ki

表3 Kd模糊控制规则表Tab.3 Basic Fuzzy Control Rule Table of Kd

Simulink中相应模糊规则逻辑的输入和输出表面，如图5所示。

图5 Kp Ki Kd 对应变化表面Fig.5 Corresponding Change Surface of Kp Ki Kd

去模糊化使用“重心法［12］”，最终推理的输出结果为横坐标与隶属度函数曲线所围成面积的重心，实际输出如下：

式中：fij=ui(e)·uj(ec)；ui(e)、uj(ec)—e和ec的隶属度；

uij根据设定的模糊规则来确定；

Ku—比例因子。

与其它的去模糊化方法相比而言，重心法的优势在于更加平滑的输出推理控制，即使输入信号发生略微变化，系统输出值也会对应的发生改变。

3.2 平滑切换方式设定

通过控制器的输入值作为判断依据直接进行切换会导致系统具有突变性，所以对该过程进行设计，引入平滑过渡因子w，使得两种控制方法平稳过渡。令控制器输出O=w*I1+(1-w)*I2；平滑过度因子越大，模糊PID控制比重越大、PID的控制作用比重越小，对w进行构造：

其中，x1和x2为设置的模糊控制切换点，输入偏差大于x2时采用模糊PID 控制，进行快速调整；输入偏差小于x1时，采用PID 控制，当输入值在两者之间时，依据w进行不同比重的综合控制。

系数ρ对参数w会产生影响，进而导致不同的控制特性。平滑过渡曲线，如图6所示。ρ取不同值时对w产生的影响也不同，取x1为0.01°，x2为0.1°，两种控制模式之间的切换过渡区域为［-0.1°，-0.01°］和［0.01°，0.1°］。

图6 平滑过渡曲线Fig.6 Smooth Transition Curve

根据曲线可以看出，ρ较小时，随着误差的增大，模糊PID控制的作用快速增加；ρ较大时，在过渡前期，随着误差增加，模糊PID 控制作用缓慢增大，PID 起主导作用，在过渡后期，模糊PID起主导作用。

3.3 构建串级PID分控系统

通过假设飞行器飞行时角度变化较小，即γ̇=θ̇=ψ̇=0，则可将无人机动力学方程（3）简化为下式：

通过拉普拉斯变换可得无人机状态空间方程的系统传递函数为：

控制四旋翼的姿态是通过控制电机的转速，所以还需在模型中建立电机的数学模型，这里采用经典电机模型，其传递函数方程近似为可得系统的传递函数为：

针对本文的测试四旋翼飞行器进行实验测量计算，得出上式中的各参数对应数值，如表4所示。

表4 四旋翼无人机参数Tab.4 UAV Parameters

将参数带入后可得三个姿态角的传递函数为：

以三个姿态角中的偏航角为例进行验证分析，根据传递函数建立对应的串级PID分控系统，对偏航角进行模拟控制，并与串级PID控制器进行对比。为避免系统的突变性，在分控系统的外环中加入设计好的平滑切换模块，当输入量e的绝对值大于x1时，为模糊PID控制；小于或等于x2时，采用PID控制；介于两者之间时，根据e的取值在两者之间取不同比重，具体框图，如图7所示。

图7 偏航角控制框图Fig.7 Control Block Diagram of Yaw Angle

依据对三个方向上姿态角的模拟控制，结合式（3）对四旋翼飞行器进行仿真，验证控制效果。其基本框图，如图8所示。左侧串级PID分控器根据输入期望姿态角和反馈值进行控制，得到飞行器的力矩矢量，右侧模型根据输入力矩，解算出电机转速总和，进而得出四旋翼的姿态角信息和位置信息。

图8 无人机仿真模型Fig.8 Simulation Model of UAV仿真控制结果

在串级分控系统建立后，针对实验室的四旋翼飞行器进行Matlab/Simulink仿真控制实验，并进行了对比验证。以四旋翼飞行器的偏航角为例，输入不同的信号来模拟无人机的飞行要求，并与传统PID和串级PID两种控制方式进行比较。采用串级控制时，内环和外环相对独立，所以在调整内环参数时，不用考虑外环控制模型，由内而外进行调整，具体仿真结果，如图9所示。

图9 仿真对比验证图Fig.9 Figure of Simulation Comparison

为了更直观看出串级PID分控的控制效果，对图9（a）和图9（c）仿真结果进行数据量化。具体数值、如表5、表6所示。

表5 偏航角控制效果对比Tab.5 Control Effect Comparison of Yaw Angle

表6 偏航角抗干扰效果对比Tab.6 Comparison of Anti-Jamming Effect of Yaw Angle

如表5 所示，在t=1s 时输入阶跃信号，三种控制器中串级PID分控系统反应快，超调量较小，仅需0.181s即可达到稳定，控制效果最佳；传统PID控制超调量大，调节时间久；串级PID控制系统相比传统PID控制有了较大提升，但与这里所设计的控制器相比，达到平衡需0.293s，时间久，控制精度也低一些。从图9（b）所输入的脉冲信号控制结果也可以看出串级PID分控系统的动态控制性能较好。为验证所建控制器的抗干扰性能，如表6 所示。在t=2s 时，加入幅值为1 的阶跃干扰信号，可以看出，串级PID分控的曲线更平缓，产生波动小，控制精度高，系统达到稳态时间最短，抗干扰能力较强，很快就可恢复平衡。如图9（d）所示，在正弦信号中加入持续随机干扰信号进行进一步验证，Simulink模型模拟对比，结果表明串级PID分控系统的控制曲线更平滑，更接近实际输入，自适应性好。实验结果表明，串级PID分控系统与另外两种控制器相比，可在短时间内快速精准的达到控制要求，提升无人机飞行时的稳定性。

为测试控制模型的实际效果，如图10所示，将自组无人机的姿态控制改为串级PID分控，并进行多次现场飞行实验。该机型重量为1.4kg，轴距0.45m，采用Pixhawk开源飞控，采用Sunny Sky电机，型号为Angel A2212-KV980，采用Flower Power 电池作为飞行器动力源，规格为3S-11.1V-5200mAh。

图10 四旋翼验证平台Fig.10 UAV Verification Platform

在室外微风情况下，将四轴飞行至一定高度，切换至CIRCLE（绕圈）模式下进行效果验证。在地面站中将旋转半径设置为200cm，转向速率设置为5deg/s，采样间隔440ms，对无人机的位置信息进行采集保存至文本，通过MATLAB 绘制四轴经纬度和海拔高度曲线，如图11所示。可以看出，串级PID分控系统可满足控制需求，有一定的自稳定性。

图11 CIRCLE模式修正后飞行轨迹Fig.11 Corrected Flight Path in CIRCLE Mode

4 总结

为解决四旋翼飞行器在复杂多变环境下的稳定性问题，通过牛顿欧拉定律对无人机进行了动力学建模，提出一种新的串级PID分控系统并进行验证。根据变量的输入情况自主平滑切换外环控制方式，结合了PID和模糊PID控制器的优点，保障实际运行效率的情况下，兼顾动态与静态性能；内环采用PD控制器对角速度的变化趋势进行快速调整，提高了系统抗动态干扰的能力。无人机的飞行控制是一个极其复杂的过程，振动和高动态变化的飞行状态是四旋翼飞行扰动的主要干扰源。

在Simulink 中模拟并验证所建立的模型和控制器，将其移植到无人机上观察效果，输入不同类型的信号来验证相应性能，结果显示该控制系统鲁棒性强、稳定性好。所建模型在面对持续干扰信号和突变干扰时，与常规控制器相比飞行效率高，反应速度快，具有更好的智能性与自主性。在野外风载状况下飞行时，自身姿态调节快速，抵抗干扰能力强，满足了四旋翼飞行器实用性的需求，为今后无人机实现更高要求的飞行任务奠定了坚实的基础。