基于非线性模型预测的四旋翼无人机轨迹跟踪控制

闫玉盼，饶 兵，刘砚菊，冯迎宾

(1.沈阳理工大学自动化与电气工程学院，沈阳 110159；2.沈阳天眼智云智能技术研究院有限公司，沈阳 110179)

近年来，无人机因其敏捷性和实用性，被成功应用于灾害搜索救援、警方执法和电力系统检测等任务中[1-3]。 无人机精准的位置控制是顺利完成上述工作任务的基础，但由于四旋翼无人机具有非线性和强耦合等特点，为四旋翼无人机开发鲁棒性强、稳定性高的控制器仍存在巨大挑战。为此，研究者提出了多种控制方法，如PID、自适应控制、LQR 控制、滑模控制、反步控制等。 PID算法原理简单，不需要建立精确的数学模型，但在双闭环PID 控制中需要设计18 个参数，数目众多，调参工作复杂[4]。 文献[5]针对PID 参数整定问题，将模糊控制理论与PID 控制系统相结合，提出了模糊自适应PID 控制器，实现四旋翼无人机对PID 参数的自适应整定。 LQR 控制将四旋翼无人机的非线性模型线性化，设计闭环最优控制指标，实现位姿控制[6]。 反步控制的抗干扰能力较弱，稳定性差，并且对初始条件的要求较高[7]。 上述线性控制策略虽然在稳定的环境中取得良好的位姿控制效果，但是在实际应用中未充分考虑环境干扰和系统约束等问题，控制效果不佳。

随着计算机技术的发展，无人机已经配有成熟的自动驾驶仪平台，如PX4 或Pixhawk 等[8]，计算能力较强、处理器性能高，基于优化的模型预测控制(model predict control，MPC)算法被广泛用于四旋翼无人机的控制系统[9-10]。 MPC 具有显式处理系统复杂约束的能力，采用滚动优化、反馈矫正的策略，在线修正每个时刻的预测轨迹，具有较强的抗干扰能力[11]。 Chen 等[12]提出了串级形式的MPC 控制器，将MPC 分别用于位置控制和姿态控制中，可以处理每个控制量的约束范围，保证四旋翼无人机稳定飞行。 周杰等[13]针对轨迹跟踪中出现的超调问题，在MPC 的代价函数中引入了偏差变化率，改善了轨迹跟踪性能。

以上MPC 控制方案都是基于线性化后的四旋翼无人机模型提出的，而实际的四旋翼无人机模型是非线性化的。 因此本文提出基于非线性模型的预测控制(nolinear model predict control，NMPC)方法，以四旋翼无人机非线性动力学模型作为控制器的预测模型，通过设计目标优化函数，将轨迹跟踪控制问题转化为非线性最优化在线求解问题，以实现轨迹跟踪控制。

1 四旋翼无人机动力学模型

准确的四旋翼无人机动力学模型是设计控制器的基础。 本文选用“X”型四旋翼无人机作为研究对象，假设四旋翼无人机为刚体且结构对称，其重心在几何中心处。 为描述四旋翼无人机的位置和姿态，定义地球坐标系OE-XEYEZE和机体坐标系OB-XBYBZB，如图1 所示。

图1 机体坐标系与地球坐标系Fig.1 Body coordinate system and earth coordinate system

四旋翼无人机的动力由四个螺旋桨高速旋转产生[14]，可实现对四旋翼无人机姿态和位置的调整。 在机体坐标系下，UL为沿ZB轴的总拉力，τx、τy、τz为绕机体轴OEXE、OEYE、OEZE的转动力矩，表示为

式中:ϖi(i＝1，2，3，4)为四个螺旋桨的转速；l为螺旋桨中心到四旋翼无人机重心的距离；cT为推力系数；cM为反扭矩系数。

四旋翼无人机具有六个自由度的输出，本文选用P＝(x，y，z)T和Θ＝(φ，θ，ψ)T分别表示四旋翼无人机在地球坐标系下的位置坐标和欧拉角，其中φ为滚转角，θ为俯仰角，ψ为偏航角。

根据四旋翼无人机的非线性、强耦合的特点和控制器设计的需要，本文采用Euler-Lagrange 方程推导得到四旋翼无人机的非线性动力学模型[15]，如式(3)所示。

式中:为四旋翼无人机的位置加速度；为四旋翼无人机的姿态角加速度；为四旋翼无人机的姿态角速度；m为四旋翼无人机的质量；g为重力加速度；Ixx、Iyy、Izz分别为四旋翼无人机绕XE轴、YE轴、ZE轴的转动惯量。

2 NMPC 控制器设计

2.1 基本原理

模型预测是一种基于在线优化的控制策略，在最优控制中具有约束系统变量和滚动优化的优点。 本文设计的NMPC 控制系统结构如图2 所示，NMPC 控制器包括预测模型、滚动优化和反馈校正三部分。

图2 NMPC 控制框图Fig.2 NMPC control block diagram

首先设预测时域为N，控制时域为W。 在当前采样时刻k，NMPC 控制器结合预测模型、目标函数和约束进行开环最优化求解，得到当前时刻的最优控制序列U(k)、U(k+1)、…、U(k+W-1)，取其第一个控制量U(k)作为四旋翼无人机的控制输入，并将四旋翼无人机的输出Y(k+1)传到反馈校正模块，完成闭环控制。 反馈校正模块将预测模型输出的预测值Xpre(k+1)和四旋翼无人机实际输出的Y(k+1)进行反馈校正后输出校正值Xc(k+1)。 在下一采样时刻k+1，校正值Xc(k+1)和期望轨迹Xref(k+1)做差得出新的状态量Xe(k+1)，在滚动优化中重新求解最优化问题，如此循环构成了完整的模型预测控制过程。

2.2 预测模型

系统的预测模型在预测控制中具有重要作用，预测模型可根据系统当前给定的状态值和输入值确定系统未来的状态。 针对四旋翼无人机的四输入、六输出的非线性特点，结合式(3)，本文选择采用离散状态空间方程作为系统的预测模型。

定义系统的状态量为X(k) ＝[x1，x2，x3，x4，x5，x6，x7，x8，x9，x10，x11，x12]T，其中x1＝x、x2＝y、；系统的输入量为U(k) ＝[u1，u2，u3，u4]T，其中；系统的输出量为Y(k) ＝[x，y，z，φ，θ，φ，。 则四旋翼无人机的非线性离散状态空间模型为

式中f(X(k)，U(k))为非线性离散映射函数，表达式如式(5)所示。

式中:Sα为sinα，Cα为cosα，α∈{x4，x5，x6}，参数ai(i＝1，2，…，6)表达式如式(6)所示。

2.3 滚动优化

为保证四旋翼无人机快速、稳定地跟踪期望轨迹，本文采用二次型函数计算预测状态与期望状态的距离误差，以误差最小化作为代价函数，即通过计算Xpre-Xref和U-Uref的误差最小来求解最优控制量U，其中Xref为参考轨迹、Uref为四旋翼无人机悬停所需的控制量，计算表达式为

式中ϖir(i＝1，2，3，4)为四旋翼无人机悬停时四个螺旋桨的转速，且。

预测时域[k，k+N]内，定义目标函数为

式中:J为目标函数；Q和H分别为状态权重矩阵和输入权重矩阵，Q∈R12×12，H∈R4×4；‖·‖2A表示欧几里得范数，A∈{Q，H}；Xpre(k)为k时刻的预测状态；Xref(k)为k时刻的期望轨迹；Uref(k)为k时刻的期望控制输入。

式(8)的第一项是通过最小化目标函数J求得局部最优解，即未来W个时刻的控制量U(k)、…、U(k+W-1)使得预测时域[k，k+N]内的预测值Xpre(k)、…、Xpre(k+N-1)尽可能接近期望值Xref(k)、…、Xref(k+N-1)；其第二项是对控制量的约束函数，使U-Uref最小，即U(k)的值接近Uref，可有效防止控制量发生突变，甚至四旋翼失控。 另一方面，考虑到四旋翼无人机螺旋桨转速受电压限制，设定控制输入的幅值约束为

式中分别为U的最小控制输入矩阵和最大控制输入矩阵。

结合离散系统动力学方程和系统约束，NMPC 算法转化为有限时域内的非线性规划问题(nonlinear programming，NLP)[16]。 当前时刻为为k时，NLP 表示如下。

式中:Xpre(k) ＝X(k)表示在k时刻时，取预测值Xpre(k)等于当前状态值X(k)；Xpre(k+i)表示预测时域[k，k+N]内第k+i时刻预测状态；δ表示采样时间。

本文使用多重打靶法(multiple shooting method，MSM)[17]求解NLP 问题，将得出最优控制序列的第一个元素U1作为四旋翼无人机的控制输入U(k)。 将获得的四旋翼无人机状态用于下一时刻k+1 的优化问题，上述过程循环执行，直至控制过程完成。

2.4 反馈校正

在实际的四旋翼无人机动态系统控制中，存在干扰和模型不精确的问题，预测模型输出的预测值与实际值存在偏差，因此仅基于预测模型和最优控制求解的控制量不能保证四旋翼无人机紧密跟踪期望轨迹，若不及时利用实际信息进行反馈校正，由式(4)给出的预测值会偏离实际值。 为此，本文设计实时反馈进行校正，将当前时刻的实际测量值X(k)与预测值Xpre(k)做差，引入误差ek，即

误差反映了预测模型的不确定干扰造成的输出值不准确现象，因此可用误差预测未来的输出误差，以修正预测值。 选取反馈系数矩阵K＝[0.12，0.12，0.05，0.1，0.1，0.1，0，0，0，0，0，0]T，通过加权方法预测未来的误差，可得到校正后的预测输出

在k+1 时刻，校正后的预测输出Xc(k+1)通过移位变成初始预测值，如此循环反复在线进行。

3 仿真实验

为验证本文设计的NMPC 算法的有效性，并考虑四旋翼无人机的飞行成本和安全性，首先在Matlab 中搭建仿真平台对本文算法进行仿真验证，四旋翼无人机的模型参数如表1 所示。

表1 四旋翼无人机模型参数Table 1 Parameters of quadrotor model

为验证本文设计的NMPC 方法的优越性，在仿真条件和四旋翼无人机参数一致的情况下，开展对比实验，对比的方法主要包括PID、串级MPC[12]、改进MPC[13]控制方法。 设计两组仿真实验，第一组仿真为跟踪圆形参考轨迹，第二组仿真为跟踪正方形参考轨迹。

3.1 圆形期望轨迹

在四旋翼无人机非线性轨迹跟踪控制中，NMPC 控制器的参数选取如表2 所示。 四旋翼无人机初始状态为。 设圆形参考轨迹为

表2 NMPC 控制器参数Table 2 NMPC controller parameters

式中:xd、yd、zd为四旋翼无人机在地球坐标系OE-XEYEZE中的期望位置，m；t表示时间，t∈[0，20]，s。

本节仿真部分使用x轴、y轴、z轴分别表示OE-XEYEZE中的OEXE轴、OEYE轴、OEZE轴。

图3 为4 种控制方法的三维圆形轨迹跟踪效果。 在初始时刻，NMPC 和改进MPC 控制方案的轨迹跟踪性能较好，超调量小；PID 和串级MPC明显偏离圆形参考轨迹；在14 s 时，在x轴方向加入幅值为0.5的阶跃信号模拟阵风干扰，如图3中箭头所示，NMPC 受到扰动后，轻微偏离轨迹，受扰动影响较小，抗干扰能力强；PID 偏离轨迹，且不能快速恢复跟踪；串级MPC 受扰动最大，存在安全问题；改进MPC 出现明显偏移误差，抗干扰能力差。

图3 圆形轨迹跟踪效果对比图Fig.3 Comparison diagram of tracking effect of circular trajectory

图4 为位置跟踪效果曲线，图5 为位置跟踪误差曲线。

图4 圆形轨迹跟踪效果Fig.4 Tracking effect of circular trajectory

图5 圆形轨迹跟踪误差Fig.5 Tracking errors of circular trajectory

分析图4 ～5如下。

1)跟踪性能

NMPC 控制器能够精确地跟踪期望圆形轨迹，几乎实现无误差跟踪，且超调量小；PID 控制器在z轴上出现较大超调量，由图4(b)局部放大图可见，在曲线的拐点处，PID 控制器存在跟踪误差；串级MPC 的超调量小，却一直存在跟踪误差；改进NMPC 无超调量，误差收敛速度较慢。

2)抗干扰性能

NMPC 和串级MPC 控制器的抗干扰性能最好，受到扰动后可以快速收敛误差；PID 和改进MPC 控制器的抗扰性较差，在干扰情况下无法保证快速收敛。

图6 为姿态角响应曲线。 NMPC 和改进MPC控制器的姿态角响应曲线平滑，扰动情况下无较大振荡，最大误差仅有0.2 rad；PID 在0 ～1.3 s 内和14 s 后均出现大幅度振荡；串级MPC 控制器的姿态角响应曲线出现较大误差，飞行稳定性能差。

图6 圆形轨迹姿态角响应曲线Fig.6 Attitude angle response curve of circular trajectory

图7 为四旋翼无人机的控制量仿真图。NMPC、PID 和改进MPC 控制器对幅值有约束能力，数值相近；串级MPC 的控制量变化较大，不适合应用于实际飞行。

图7 圆形轨迹控制量Fig.7 Control quantity of circular trajectory

3.2 正方形期望轨迹

为验证控制器的鲁棒性，开展正方形轨迹跟踪控制实验。 四旋翼无人机初始状态和NMPC控制参数同上，设正方形的参考轨迹方程为

正方形轨迹跟踪的三维效果如图8 所示，NMPC、串级MPC 和改进MPC 控制器的轨迹跟踪曲线平稳且光滑，其中串级MPC 的轨迹一直存在跟踪误差；PID 在起飞阶段出现较大超调，存在安全性问题。 在14 s 时，x轴方向加入幅值为0.5的阶跃信号模拟阵风干扰，如图6 中所示，PID 和NMPC 的位置发生小范围偏移；串级MPC 和改进MPC 出现较大跟踪误差，改进MPC 的轨迹2 s 后重新恢复了轨迹跟踪，但串级MPC 控制直至仿真结束后仍未修正跟踪误差。

图8 正方形轨迹跟踪效果对比图Fig.8 Comparison diagram of tracking effect of square trajectory

正方轨迹效果和跟踪误差如图9 ～10 所示。

图9 正方形轨迹跟踪效果Fig.9 Tracking effect of square trajectory

图10 正方形轨迹跟踪误差Fig.10 Tracking errors of square trajectory

分析图9 ～10 如下。

1)轨迹跟踪性能:NMPC 控制器的轨迹超调量小，可实现无误差跟踪；PID 的轨迹在拐点处存在0.2 m 左右的跟踪误差；串级MPC 控制器的轨迹在x轴和y轴上误差最大，且一直未修正；改进MPC 消除了超调量，跟踪性能较好。

2)抗干扰性能:由图9 局部放大图可见，在1 s 出现扰动后，NMPC 和串级MPC 均可在2.1 s内可修正误差，具有一定的抗扰性；PID 受扰动影响，出现振荡，经4 s 后误差收敛到0，抗干扰能力较差；改进MPC 控制器的抗干扰性能较强，但误差收敛速度较慢。

图11 为姿态角响应曲线。 NMPC 的滚转角和俯仰角的响应曲线平滑，无波动，且稳定后误差保持在0.1 rad 内；PID 的滚转角和俯仰角响应曲线在0 ～2.5 s 内均出现1 rad 左右的振荡，且在14 s左右俯仰角受扰动影响后再次振荡，PID 在姿态角控制方面稳定性较差；串级MPC 的滚转角和偏航角在0 ～3 s 内出现大幅度振荡，俯仰角存在波动，飞行状态不稳定；改进MPC 的姿态角响应比较稳定。

图11 正方形轨迹姿态角响应曲线Fig.11 Attitude angle response curve of square trajectory

图12 为四旋翼无人机的控制量仿真图。NMPC 和改进MPC 的控制量数值幅度较小，能量消耗较少；PID 和串级MPC 在0 ～2.6 s 内控制量数值变化幅度大，不符合工程实际。

图12 正方形轨迹控制量Fig.12 Control quantity of square trajectory

4 结论

本文设计了基于四旋翼无人机非线性动力学的NMPC 控制器，旨在利用非线性预测模型和NMPC 处理系统约束的能力，将轨迹跟踪问题转化为最优化问题，以完成精准的轨迹跟踪任务。在仿真实验中，将NMPC 与PID、串级MPC 和改进MPC 控制方法进行对比，结果表明，NMPC 控制器提高了控制系统的稳定性和跟踪性能，且具有较强的抗干扰能力。