四旋翼飞行器是一种能够实现垂直起降、悬停、偏航等复杂动作的多旋翼无人机，因而在军事和民用领域的应用前景十分乐观^[1]，但其自身系统的复杂性以及易受外界空气动力和不确定因素影响的问题一直困扰着科研人员。

    针对上述问题，科研人员对四旋翼飞行器系统模型的控制方法和滤波算法进行了研究。经典PID算法是一种不依赖于系统模型的控制方法，该方法结构简单，理论成熟^[2]，但参数整定比较繁琐。文献[3-4]中反步法虽然对非线性系统有很好的跟踪效果且反应速度快，但是其鲁棒性差，抗干扰能力弱，且需要建立精确的系统模型。文献[5]用滑膜控制法对四旋翼飞行器进行控制，虽然滑膜控制相对反步法具有较好的鲁棒性，但此类算法都没有对系统的干扰进行实时估计和补偿，也没有对各个通道进行解耦处理，当外界干扰较大时，其控制效果并不理想。控制方法的选取虽然可以减小对四旋翼模型的依赖性，保证内部系统的相对稳定，但要想实现四旋翼飞行器在复杂的环境中安全飞行，滤波算法的研究也是不可或缺的。

    文献[6]利用粒子滤波对飞行器姿态进行估计，其在非线性系统中表现出较强的优越性，但其计算量较大，难以满足四旋翼飞行器实时控制的效果。文献[7]利用互补滤波对四旋翼飞行器数据进行融合，该算法根据陀螺仪和加速度计传感器特性的不同，将两者的优点融合起来得到较好的数据，但其和粒子滤波一样也存在系统延迟问题。文献[8-9]提出基于卡尔曼滤波的姿态估计算法，其能够有效地滤除外界噪声，确保获取数据的精度，但当系统滤波偏差较大时会出现滤波发散的现象，从而导致系统不能正常工作。

    综合以上问题，本文提出改进EKF的自抗扰控制方法，利用自抗扰控制器对飞行器姿态进行控制，将受噪声干扰的姿态经改进EKF进行滤波，使得飞行姿态更加稳定，有效提高了系统的稳定性和抗干扰性。

1 四旋翼飞行器动力学模型

    为得到四旋翼飞行器的数学模型，首先建立两个坐标系：惯性坐标系E(OXYZ)和机体坐标系B(OX′Y′Z′)。其重心和质心均与飞行器原点O(O′)重合，如图1所示。

    则四旋翼的动力学模型为^[10]：

    由上式可知，四旋翼飞行器是一个欠驱动、非线性、强耦合系统。本文采用自抗扰控制器作为控制系统，同时利用改进EKF对带有噪声的飞行器姿态进行修正，其控制系统模型如图2所示。

2 ADRC系统

2.1 ADRC系统结构

    自抗扰控制是通过跟踪微分器(TD)安排过渡过程，利用扩张观测器(ESO)实现状态间耦合项的跟踪和估计，同时对系统总扰动进行估计，并根据扰动估计值采用非线性状态误差反馈(NLSEF)进行实时补偿。其二阶的结构如图3所示^[11]。

2.2 姿态控制系统的自抗扰解耦控制 

    四旋翼姿态子系统方程如下:   

    式中，b₁、b₂为控制器增益。

    自抗扰控制器品质的好坏和其内部各模块参数的整定有关，因此，对自抗扰控制器各模块参数的整定是必要的。

2.3 自抗扰解耦控制参数的整定

    因为ADRC中各模块互不影响，因此，可以对各模块的参数单独整定，最后进行总体调整。

2.3.1 TD参数整定

    跟踪微分器中的速度因子r影响着跟踪信号的速度，其影响效果如图4所示。其中T=0.01，x=5是初始信号，y是跟踪波形，分别对应着速度因子r=1、5、14的曲线。

    由图4可知，速度因子r越大，跟踪速度越快，容易造成超调现象，失去了微分跟踪器的意义；r越小跟踪过程越长，系统的实时性变差。 

2.3.2 ESO参数整定

    ESO是自抗扰控制器最重要的部分，一般选取a₁=0.5，a₂=0.25。σ是fal函数原点附近线性区间的宽度，若σ过大则可能无法准确跟踪一些非线性信号，大大降低了ADRC的控制效果；σ太小则会在原点位附近出现信号跳变现象，降低了系统的稳定性。而β₁、β₂、β₃仅影响ESO的收敛速度，不因控制对象的改变而改变[12]。

2.3.3 NLSEF参数整定

    NLSEF中控制量增益b₁、b₂对控制效果的影响较大。控制量增益大会导致系统发散，影响系统的安全性；控制增益越小，则系统的实时性较差，故应根据不同的控制系统动态调整。

3 改进EKF滤波原理及算法

    本文提出一种改进的EKF算法,首先,计算观测矩阵时进行迭代处理以提高滤波精度。然后，采用两个迭代的EKF对四旋翼飞行器的姿态数据进行预测和更新，将它们输出的状态变量加权整合后输出飞行器的最优状态估计，其原理图如图5所示。

    对于一个离散随机系统：其中W_k、V_k分别为系统k时刻的过程噪声和观测噪声且均为均值为零的高斯白噪声，对应的方差分别为Q_k、W_k。改进EKF算法过程如下：

4 仿真与实验结果分析

    本文在前期的工作中已完成了双环PID、改进EKF的自抗扰飞控系统的仿真和实验，飞行器在改进EKF的自抗扰控制器控制下的实际飞行效果如图6所示。

4.1 改进EKF的自抗扰稳定性控制实验

    为了验证该方法对四旋翼飞行器稳定性控制的效果，给定飞行器横滚角的期望值为θ=20°，其仿真结果如图7所示。

    由图7可知，俯仰角可以在0.9 s左右达到期望值，且状态调整过程中没有出现任何振荡，可以看出该控制方式对飞行器稳定性控制效果十分理想。

4.2 改进EKF的自抗扰和串级PID对比实验

    给定横滚角的初始值均为0°，且信号是周期性变化的方波，其中横滚通道方波增益为φ=15°。在改进EKF的自抗扰和串级PID控制下的系统输出曲线如图8所示。

    由图8可知，两种控制方式对横滚角都有很好的跟踪效果，但串级PID控制器的姿态角响应曲线会产生震荡现象，而改进的EKF的自抗扰控制则无此现象。 

4.3 高度控制对比实验

    给定四旋翼飞行器初始高度为0 m，期望高度为2 m，其在改进EKF的自抗扰和串级PID控制下的系统输出如图9所示。

    由图9可知，在期望值2 m条件下，串级PID控制的超调量在50%以上，稳定时间在0.9 s左右，而改进EKF的自抗扰控制曲线超调量在5%以下，稳定时间大约在0.5 s左右。 

4.4 改进EKF的ADRC、ADRC、串级PID抗干扰性对比实验

    四旋翼实际飞行时，系统会受到传感器噪声干扰和外部突变的影响，为了模拟传感器噪声干扰，实验对横滚角的反馈变量加入了高斯白噪声；为了模拟外部突变的影响,在200 ms时刻对横滚角回路的角加速度加入幅值是20、脉宽为100 ms的矩形波作为突变信号来检测控制器的抗干扰性能,仿真结果如图10所示。

    由图10可知，同等条件下，改进EKF的自抗扰控制器抗干扰效果更加的突出。

4.5 飞行器实际飞行横滚角、高度曲线

    实际飞行时，给定横滚角幅值为12左右的突变信号，给定高度的幅值大约是8。利用数传把数据传到电脑上，其在上位机上的曲线分别如图11所示。

    由图11可知，在改进EKF的自抗扰控制器下的四旋翼飞行器，其可以快速实现姿态角的恢复及高度的控制，实现飞行器稳定的飞行。

5 结论

    实验结果表明，该方法控制下，飞行器可以从50°横滚角的情况下迅速恢复到平衡，且高度控制、抗干扰能力较之串级PID控制反应更加迅速，平稳。在接下来的工作中，会对四旋翼飞行器的避障和路径规划进行研究，利用超声波测量四旋翼与障碍物之间的距离，采用单目视觉对飞行器进行定位，最终实现四旋翼飞行器自主飞行。

参考文献 

[1] 方勇纯，申辉，孙秀云，等.无人直升机航向自抗扰控制[J].控制理论与应用，2014，31(2)：238-243.

[2] 刘一莎，杨晟萱，王伟.四旋翼飞行器的自抗扰飞行控制方法[J].控制理论与应用，2015，32(10)：1351-1360.

[3] 田聪玲.基于反步法的四旋翼飞行器非线性控制[D].哈尔滨：哈尔滨工业大学，2014.

[4] MADANI T，BENALLEGUE A.Backstepping control for a quadrotor helicopter[C].Intelligent Robots and Systems，2006 IEEE/RSJ International Conference on.2006.

[5] 王璐，李光春，王兆龙，等.欠驱动四旋翼无人飞行器的滑膜控制[J].哈尔滨工程大学学报，2012，33(10)：1248-1253.

[6] 曲仕茹，马志强.改进的粒子滤波在四旋翼姿态估计中的应用[J].飞行力学，2013，31(5)：458-461，466.

[7] 万晓凤，康利平，余运俊，等.互补滤波算法在四旋翼飞行器姿态解算中的应用[J].测控技术，2015，34(2)：8-11.

[8] 汪绍华，杨莹.基于卡尔曼滤波的四旋翼飞行器姿态估计和控制算法研究[J].控制理论与应用，2013，30(9)：1109-1115.

[9] 聂鹏，李佩华，李正强，等.基于卡尔曼滤波的小型无人机姿态估计算法研究[J].沈阳航空航天大学学报，2013，30(6)：53-57.

[10] 张广昱，袁昌盛.基于自抗扰理论的小型四旋翼飞行器姿态控制[J].航空工程进展，2014，5(3)：338-342.

[11] 韩京清.自抗扰控制器及其应用[J].控制与决策，1998(1)：19-23.

[12] 杨立本，章卫国，黄得刚.基于ADRC姿态解耦的四旋翼飞行器鲁棒轨迹跟踪[J].北京航空航天大学学报，2015，41(6)：1026-1033.

作者信息:

姜海涛，常  青，王耀力 

(太原理工大学 信息与计算机学院，山西 太原030024)