四旋翼无人机的位置和姿态控制算法研究

刘子龙，窦鹏程

(上海理工大学 光电信息与计算机工程学院，上海 200093)

1 引 言

近年来，随着四旋翼无人机市场的发展，有越来越多的人关注到这一行业.由于它低成本和简单的构造，使得其在商业上中大量使用并且很多人选择这项研究工作.精确的姿态和位置控制是满足四旋翼飞行器任务要求的基本和必要条件，因为四旋翼具有高耦合、非线性、内部不确定性以及不可预测的外部环境干扰，所以控制系统应有较强的抗干扰能力和鲁棒性.

针对以上四旋翼无人机的控制问题，目前相关的控制算法有PID控制[1，5，6]、线性二次型调节器[2，3]、滑模控制[7-9]、反步法[7，12]、鲁棒控制[10]、自适应控制[11]等.但这些方法依赖精确的数学模型，且容易受到内部不确定性和未知干扰的影响.文献[5]提出了一种基于增强学习算法的四旋翼PID调节方案，利用强化学习的PPO算法来调整控制器的增益；文献[6]建立了四旋翼飞行器非线性模型，与PID控制器串联形成完整的控制系统模型，经过对PID参数调优可以在较短时间内实时控制进行响应，但是文献[5，6]控制器参数的调整过程较为耗时且控制性能有限，因此对于复杂多变的飞行环境需要一些更鲁棒、实时的控制算法.文献[7]等提到了滑模法，该方法具有响应速度快、鲁棒性强的特点，但容易引起系统抖振.文献[12]提出了反步积分控制器设计位置回路和姿态回路，仿真结果表明该方法具有良好的跟踪效果和较强的空气扰动抑制能力，但是其抗扰性能是在事先建立了外界干扰的精确模型基础之上.关于姿态控制，文献[13，14，17]提出了姿态角度-角速度双闭环控制策略：文献[13]采用自抗扰控制和比例-微分控制；文献[14]分别采用自抗扰控制和积分滑模控制；文献[17]采用串级自抗扰控制；验证了自抗扰控制的有效性.

迭代学习控制(Iterative Learning Control)是由Arimoto等人在1984年提出[15]，通过在以前试验的控制输入中添加一个校正项来生成新的控制输入，这个校正项就是前一次或者前几次试验时测的误差信息.迭代学习控制具有拟人的学习过程与特性，不依赖于系统的精确数学模型，能在给定时间范围内，以简单的算法通过每次迭代更新控制输入信号来减少期望参考轨迹信号于系统输出之间的误差.因此，迭代学习控制对外界干扰和系统不确定性具有一定的鲁棒性[19].

韩京清提出的自抗扰控制[4，16]，用于处理具有不确定性和外部干扰的非线性系统.自抗扰控制通过扩张状态观测器(ESO)估计状态和实时干扰，是一种不依赖于精确系统模型的控制方法，具有跟踪速度快、控制精度高和抗干扰能力强等特点.因此，采用自抗扰控制对四旋翼姿态存在内部强耦合和外界不确定干扰的情况是一个可行的方法.

本文基于以上考虑，一方面为提高系统的抗扰性，另一方面在不人为进行参数调整情况下尽可能多满足四旋翼大范围和快速性的运动场景.在位置轨迹跟踪上，提出一种反步积分-迭代学习控制器(BI-ILC)，旨在充分发挥反步积分法具有轨迹跟踪性能强等优点，由于四旋翼的重复飞行是一种常见行为，因此结合迭代学习控制能改善非线性对象的跟踪性能，可以保证随着迭代次数的增加而渐进的实现跟踪.在姿态环使用自抗扰控制，无需详细模型就能估计和补偿未知扰动.上述策略结合形成双通道控制，达到最优的控制效果，李雅普诺夫稳定性理论和仿真验证了该方法的有效性.

2 四旋翼数学模型

不妨做出下述假定：

1)设定四旋翼无人机的结构严格对称，运动过程活动范围较小，整体考虑的情况下视为理想模型；

2)设定四旋翼无人机收到的地心引力不会随着机体的运动而产生变化，且忽略外部其他干扰因素.

得到的四旋翼无人机动力学模型[20]如式(1)、式(2)所示：

(1)

(2)

式中，m是四旋翼的质量；g是重力加速度常数；ω是单个螺旋桨的转速；b是一个定值；d是固定系数；Fi(i=1，2，3，4)=bω2各自代表电机转动时带动相应的螺旋桨所生成的拉力；l是质心与螺旋桨的中心距离；U1是总的升降力矩；U2是横滚力矩；U3是俯仰力矩；U4是偏航力矩.

3 四旋翼位置和姿态控制

3.1 位置控制器设计

位置控制器设计分为高度控制器设计和水平控制器设计，二者具有相似性，下面以高度控制器为例.从四旋翼数学模型(1)中可以看出，四旋翼高度由控制同时受姿态角的影响.高度通道的状态方程为：

(3)

3.1.1 反步积分控制

首先定义高度跟踪误差为:

e=x1d-x1

(4)

其中，x1d为期望的高度轨迹.

对式(4)进行求导，即:

(5)

为使e→0，选择Lyapunov函数V1为:

(6)

计算式(6)的导数为:

(7)

将x2d当作虚拟控制量，则当期望的虚拟控制为:

(8)

通过式(8)可得虚拟控制x2与期望虚拟控制x2d的误差为:

(9)

对式(9)进行求导，即:

(10)

为使e，δ→0，选取:

(11)

计算式(11)的导数为:

(12)

此时，通过式(9)和式(10)可得高度输入控制量U1为:

(13)

在实际飞行过程中，滚转角φ和俯仰角θ在很小的范围内变化，可近似为0，因此cosφ≈1，cosθ≈1.

3.1.2 迭代学习控制

迭代学习控制是设计控制算法在t时刻产生的k次迭代输入信号uk(t)，使得在t∈[0，T]时间内的跟踪性能得到提高.结合四旋翼模型(1)考虑可重复非线性系统：

(14)

式中，t∈[0，T]，x、f(x)∈Rn，B(x)∈Rn×r，y、g(x)∈Rm，u∈Rr.

采用如下形式的PD型学习律：

(15)

其中t∈[0，T]，Γ(yk(t))、L为增益矩阵.

定理1.设公式(14)及公式(15)满足条件：

1)‖I-Γ(g(x))gx(x)B(x)‖≤ρ≤1 (x∈Rn)

2)xk(0)=x0(k=0，1，2…)

(16)

由于：

(17)

可知，存在：

(18)

式中，0<θi(t)<1，i=1，2，…，m.

使：

(19)

因此，

(20)

式(20)两端取范数，则有：

(21)

式中，

k=γM2b1kB+γb2kg+γM2kf+γlM2，l=‖L‖；

3.2 姿态控制器设计

自抗扰控制对“总扰动”进行观测，不仅实现内部解耦，同时抑制外部扰动.自抗扰控制器包括跟踪微分器(TD)、扩张状态观测器(ESO)和非线性误差反馈控制律(NLSEF).所提出的四旋翼姿态控制策略如图1所示.

图1 四旋翼横滚角控制结构图Fig.1 Quadrotor roll angle control structure diagram

以四旋翼横滚通道为例，设计自抗扰控制器.将四旋翼模型(1)的横滚角写成如公式(22)所示：

(22)

式中，u是横滚通道控制输入；x1是横滚角；x2是横滚角速度；x3是横滚通道扰动总和.

1)跟踪微分器(TD)

安排过渡过程扩大了比例系数和微分系数的选取范围，降低了整定难度，也提高了控制器的鲁棒性，使控制器有更好的适应性.其形式如公式(23)所示：

(23)

式中，v是期望信号；v1是期望信号的跟踪值；v2是期望跟踪值的微分；r是速度因子；h是滤波因子.

2)扩张状态观测器(ESO)

ESO是整个系统的核心，它可以估计出系统的内部不确定量和外部干扰然后用状态误差反馈控制律对系统的总干扰进行补偿.其形式如公式(24)所示：

(24)

式中，b0=1/Ix；e是观测值与实际值的差值；zi(i=1，2，3)是观测器对系统状态xi(i=1，2，3)的观测值；δ是滤波因子；βi(i=1，2，3)是观测器增益，均为可调参数.

fal(e，α，δ)的表达式如公式(25)所示：

(25)

式中，α是常值.

3)非线性状态误差反馈(NLSEF)

利用非线性函数对误差和误差的微分进行处理，之后进行加权，依据“小误差大增益，大误差小增益”的设计思路.其形式如公式(26)所示：

(26)

式中，k1，k2是控制器增益，均为可调参数.

4 仿真结果与分析

在本节中，依据前面所建立的四旋翼数学模型和所提出的控制器在Simulink环境下搭建四旋翼仿真系统，分别验证位置环和姿态环算法的有效性.四旋翼无人机模型参数如表1所示.四旋翼无人机高度通道仿真参数和横滚角通道仿真参数如表2和表3所示.

表1 四旋翼无人机的系统参数Table 1 System parameters of Quadrotor UAV

表2 四旋翼无人机高度通道仿真参数Table 2 Height simulation parameters of Quadrotor UAV

表3 四旋翼无人机横滚角通道仿真参数Table 3 Roll angle simulation parameters of Quadrotor UAV

4.1 高度跟踪仿真

设无人机的初始状态在坐标系原点，选取期望高度变化为正弦曲线.仿真结果如图2～图6所示.

在没有外界干扰的情况下，由图2可知，跟踪轨迹与期望轨迹基本一致.在加入了高斯白噪声信号后，如图3所示的反步积分控制器出现明显的震荡；如图4所示所提出的反步积分-迭代学习控制器高度跟踪轨迹在一定时间内渐进收敛到它的期望值，且误差在很小的范围之内.图5所示为在相同条件下，无迭代学习控制器和增加迭代学习控制器的局部放大效果对比.如图6所示为四旋翼位置初始值为0，随着迭代次数的增加，实际跟踪轨迹逐渐趋向期望轨迹，误差收敛到极小值.由上述仿真结果可知，反步积分器可以使系统稳定达到期望的轨迹，但在内部不确定和外部干扰的情况下，会影响跟踪的效果，结合了迭代学习控制器后可以在不改变原有控制器参数的情况下提高系统的稳定性，保证系统具有良好的性能和平滑的轨迹，这表明所提出的反步积分-迭代学习控制方法是有效的.

图4 有扰动情况下反步积分-迭代学习控制跟踪图Fig.4 BI-ILC tracking diagramwith disturbance

4.2 姿态跟踪仿真

选取无人机初始姿态为0°，设定横滚角度为20°.响应曲线如图7～图9所示.

如图7所示为自抗扰控制器的响应曲线，并与使用较为广泛的串级PID控制器进行对比，可以看出，自抗扰控制在快速性和稳定性等方面都明显更优于串级PID控制.加入相同干扰信号的结果如图8和图9所示，在同样不调整控制器参数的情况下，自抗扰控制器能更好地抑制干扰，具有较强的鲁棒性.

图7 无扰动情况下内环姿态跟踪图Fig.7 Attitude tracking diagram without disturbance

5 结 论

本文描述了一种控制策略，将反步积分-迭代学习控制器和自抗扰控制器运用在位置-姿态双闭环控制系统上.通过李雅普诺夫稳定性分析，深入研究了四旋翼控制系统的稳定性和控制性能.此外，进行了仿真分析验证.结果表明所提控制策略能有效的控制四旋翼系统，且在不确定性情况下仍能达到预期的性能指标，改善了四旋翼的飞行性能，保证了四旋翼的高稳定性.下一步的研究，将结合所设计的控制器应用到实际的四旋翼无人机.