吴 颖，谢 云，李 森，李 力

（广东工业大学 自动化学院，广东 广州 510000）

0 引言

四旋翼是由4 个电机和螺旋桨驱动的无人机。由于四旋翼具有较简单的结构，在科研、工业和农业等方面[1⁃2]被广泛应用。但当前四旋翼的控制研究主要在低速飞行控制方面，而较少有人关注四旋翼的高速（大于8 m/s）飞行控制研究。四旋翼的高速稳定飞行对螺旋桨在高速飞行下的变形、四旋翼空气动力学模型等研究具有相当重要的意义[3]。具有系绳约束的无人机有许多应用，如可以用于无人机的供电[4]、机器人远程控制的辅助[5]，还可以用于室内定位[6]和无人机的定点位置控制[7]。与文献[8]相似，本文利用系绳使四旋翼能够在有限的空间做高速运动，但本文提出的控制方法是适用于质量较大（大于1 kg）的无人机，且还提出了系绳拉力的动力学模型。

四旋翼飞行控制涉及四旋翼的位置控制和姿态控制，大部分论文研究四旋翼的姿态控制，都是用欧拉角表示四旋翼的姿态[9⁃10]。但用欧拉角表示四旋翼的姿态不仅会出现奇异性，而且没办法保证四旋翼平滑的姿态运动轨迹。而如果用旋转矩阵表示四旋翼的姿态，则可以防止四旋翼的姿态出现奇异性，且可以对四旋翼的姿态进行轨迹规划，使姿态运动轨迹更加平滑。而不同于文献[11]利用so（3）的性质对姿态误差进行定义，本文是通过旋转矩阵和罗德里格斯公式，将旋转矩阵表示的四旋翼姿态误差转化为绕旋转轴旋转一定角度的变换。

四旋翼的位置控制采用基于四旋翼当前位置设计自适应控制器的方法，自适应控制器在四旋翼的控制中有非常广泛的应用，既可以应用于四旋翼的姿态控制[12]，也可以应用于四旋翼在负载变化情况下的定高控制[13]、四旋翼的位置控制[14]和轨迹跟踪[15]等。但这些研究都是在讨论四旋翼低速飞行控制，而本文要讨论的是四旋翼高速飞行控制，这要求四旋翼系统响应有更短的调节时间和更好的稳态性能。

而由于本文设计的控制器对控制模型精确度要求不高，鲁棒性强，且空气阻力比系绳拉力要小得多，故本文不考虑四旋翼的空气动力学。所以本文在旋转矩阵表示四旋翼姿态误差的情况下设计姿态的比例控制算法和改进的位置自适应控制器，可以根据四旋翼的当前位置和期望轨迹调节期望速度，将姿态误差控制在一定范围内。

1 系统模型

如图1 所示，四旋翼的结构是一个“+”型的结构，由4 个电机和螺旋桨驱动。惯性坐标轴和机体坐标轴分别记作Oexeyeze和Obxbybzb。在建立四旋翼动力学模型前，假设绳子在四旋翼上的悬挂点、四旋翼的质心、几何中心和机体坐标系在同一点上，惯性坐标系则是相对于地面固定不变的。

图1 四旋翼模型

1.1 旋转矩阵

文中用旋转矩阵来表示四旋翼的姿态，假设xeb，yeb，zeb分别表示机体坐标系的x,y,z轴在惯性坐标系下的坐标（两坐标系的原点重合），则机体坐标系到惯性坐标系下的旋转矩阵为：

若点q在机体坐标系下的坐标记为qb，则点q在惯性坐标系下的坐标为：

1.2 运动学与动力学模型

四旋翼是一个有6 个自由度的刚体，但其控制输入只有4 个电机的转速，所以四旋翼是一个欠驱动的系统。四旋翼的运动学与动力学模型如下：

式中：x∈R3表示四旋翼在惯性坐标系下的位置；v∈R3表示惯性坐标系下的速度；R∈R3×3表示从机体坐标系到惯性坐标系的旋转矩阵；ωb∈R3表示机体坐标系下的角速度；m∈R 表示四旋翼的质量；J∈R3×3表示四旋翼的惯性矩阵；f∈R 表示4 个螺旋桨产生的总推力；τ∈R3表示螺旋桨产生机体坐标系下的推力矩；g是表示重力加速度；Ft∈R3是系绳产生的拉力；e3=[0 0 1]T。∧运算的定义为，若ω=[ω1ω2ω3]T，则：

1.3 推力和转动力矩模型

文中忽略电机的动力学模型，假设四旋翼的螺旋桨产生的推力与电机的转速平方成正比。因此，系统的控制输入就是电机的转速，因此，螺旋桨产生的总推力和机体坐标系下的动力矩为：

式中：cT和cM都是常数；d是四旋翼的臂长。

1.4 系绳拉力模型

假设系绳是拉紧的，没有重量且没有弹性，不可伸缩的，因此系绳对四旋翼提供了位置限制。系绳拉力的模型如下：

式中：ft是指拉力的长度；pt是指拉力的方向。通过牛顿第二定理和向心加速度的公式推导可得pt和ft：

式中系绳的长度lt=|x|。

2 轨迹跟踪

在设计四旋翼的控制器之前，假设控制器以高频率读取四旋翼状态的测量值，包括四旋翼的位置、速度、姿态和角速度，并且假设四旋翼状态测量值的误差足够小，可以忽略不计。因此，本文目标是设计一个控制器，让四旋翼跟随给定的轨迹飞行。给定一个期望的圆轨迹xd和四旋翼的偏航角ψd，控制器结构如图2 所示。

图2 轨迹跟踪控制器结构

四旋翼位置控制器的输出是四旋翼的期望机体坐标系z轴方向b3d和四旋翼的期望推力，四旋翼的姿态控制器根据位置控制器的输出，即四旋翼的期望机体坐标系z轴方向b3d和期望的偏航角ψd，计算输出四旋翼的期望控制力矩。四旋翼是一个欠驱动系统，四旋翼的位置控制依赖于姿态控制器的性能，当姿态控制器控制性能较差，姿态误差较大时，位置控制器的性能也会变差，位置误差也会变大。只有当姿态误差趋近于0 时，位置误差才有可能趋近于0。

2.1 姿态控制

大部分四旋翼的姿态控制器的姿态表示都是用欧拉角，但其实相比旋转矩阵和四元数表示，用欧拉角表示姿态会出现奇异性问题，而且欧拉角是一个很不直观的方法，即使已知四旋翼当前姿态的欧拉角，也很难知道四旋翼机头的具体朝向。四旋翼姿态的欧拉角与旋转矩阵之间的变换关系如下：

由变换关系可以看出，即使对姿态欧拉角进行轨迹规划，四旋翼的姿态也不会平滑的变换。所以本文采用旋转矩阵表示四旋翼的姿态，但旋转矩阵有一个缺点是有9 个变量，所以采用罗德里格斯公式，把旋转矩阵转化为一个一一对应的旋转轴ω∈R3和一个旋转角度θ∈[0,2π)，且由于该变换是一一对应的，因此可以避免四旋翼姿态的奇异性问题。四旋翼期望姿态的旋转矩阵Rd可由ψd和b3d推导得出。而Rd的第3 列向量是期望姿态的机体坐标系的z轴在惯性坐标系下的坐标，即期望机体坐标系z轴方向b3d，因此可得：

由给定的期望偏航角ψd可得：

由于在本文的研究中，机体坐标轴的z轴方向在惯性坐标轴z轴的分量肯定与惯性坐标轴z轴的正方向同向，由四旋翼的动力学模型可假定cosφd和cosθd均大于等于0，则可求出cosφd，cosθd，sinφd，sinθd，即可求出Rd。求出Rd之后，要利用期望姿态的旋转矩阵和四旋翼当前姿态的旋转矩阵Rc求出四旋翼机体坐标系下的期望角速度，所以误差旋转矩阵应取Re=(Rc)-1Rd。已知罗德里格斯公式的表达式如下：

则根据式（7）可得：

而且，如果R=I，则θ=0°，ω可取任意值。

因此，可以记姿态误差旋转矩阵为：

对误差旋转矩阵利用式（7）和∨运算转化成一个表示四旋翼姿态误差的向量eR∈R3，有：

式中∨为∧运算的逆运算。接下来定义四旋翼机体角速度的误差为：

式中ω为当前机体角速度。

因此，四旋翼姿态控制器的输出为：

将式（9）代入式（11），可得四旋翼姿态控制器输出：

2.2 位置控制

四旋翼的位置控制器依赖于四旋翼的姿态控制器，所以当四旋翼在高速飞行时，对姿态控制器的跟踪误差要求很高，如果四旋翼的姿态误差太大时，四旋翼的位置跟踪误差就会发散而无法收敛。为了解决这个问题，本文设计一个位置控制器，能够使四旋翼在保持高速飞行的同时逐步调节姿态误差。位置控制器能够以四旋翼当前位置和期望轨迹去计算四旋翼的期望速度，然后通过比例控制算法控制四旋翼跟随期望轨迹。

由于本文四旋翼是进行高速圆周运动，所以若给定四旋翼的期望轨迹：

则四旋翼在当前时刻的期望速度为：

式中x为当前时刻四旋翼的位置。

定义四旋翼的速度误差为：

式中v为当前时刻四旋翼的速度。

四旋翼位置控制器输出四旋翼期望推力和fd以及四旋翼期望机体坐标系z轴方向b3d分别为：

3 仿真数据与分析

采用Simulink 对四旋翼进行仿真，四旋翼飞行器的参数如下：

根据建立的四旋翼模型，测试传统PID 控制器和本文提出的自适应调节控制器在不同规划轨迹下的性能，取四旋翼的姿态控制器参数为kR=[11 11 11]T和kω=[4.8 4.8 9.8]T，若四旋翼的初始位置x0，初始姿态R0为：

四旋翼低速飞行的期望轨迹xd，ψd分别为：

四旋翼高速飞行的期望轨迹xd，ψd分别为：

若四旋翼的位置控制器为传统PID 控制器，且PID控制器的参数如表1 所示。

表1 PID 控制器的参数

在控制器期望轨迹输入为低速飞行轨迹的情况下，四旋翼的控制响应如图3 所示。

图3 PID 控制响应

四旋翼的姿态误差超调量较大，且姿态误差响应震荡较大，达到稳定需要不少于5 s，而且系统线速度的调节时间也比较长，稳态性能也比较差。而传统PID 控制器在输入高速飞行轨迹的情况下，四旋翼的姿态误差和位置误差都会发散，无法跟随期望轨迹飞行。

而在给定参数kv=[0.6 0.6 3]T的情况下，由图4、图5 所示，在低速飞行下，四旋翼的姿态误差能在1 s 内迅速减小趋近于0，且在2 s 内速度误差能够逐渐收敛趋近于0，且沿圆轨迹飞行时最大切向加速度的变化也比较平滑，四旋翼线速度的稳态性能也比PID 控制器要好。

图4 自适应控制低速飞行响应

图5 自适应控制高速飞行响应

四旋翼能够实现稳定加速至期望速度，且能在2 s内实现稳定地跟随期望轨迹。而在高速飞行情况下，四旋翼的姿态误差也能在1 s 内迅速减小并逐渐趋近于0，而且最大速度能够达到10.5 m/s，沿圆轨迹飞行的最大切向加速度也能达到4 m/s2，能够稳定加速至期望速度，从而实现稳定跟随期望轨迹。

因此，相比于传统PID 控制器，本控制器在高速飞行情况下，能够稳定地控制四旋翼的速度达到高速，且在低速飞行的情况下也比PID 控制器的控制效果更好。

4 结论

本文提出一种能够控制四旋翼高速稳定飞行的控制方法，该方法分别提出了四旋翼姿态和位置的控制器，在姿态控制方面，用旋转矩阵表示四旋翼的姿态，通过罗德里格斯公式与四旋翼的姿态动力学模型对四旋翼的姿态误差进行了定义，设计了比例控制算法控制四旋翼的姿态。在位置控制方面，利用四旋翼的状态和期望轨迹得到期望速度，在误差较大时减小速度。利用系绳使四旋翼能够在有限空间高速飞行，同时提出四旋翼飞行时受到的拉力动力学模型。通过仿真数据分析得到以下结论：本文提出的姿态控制器能使四旋翼的姿态误差迅速减小并趋于稳定。相比于传统PID 控制器，位置控制器在高低速情况下都能使四旋翼稳定跟随期望轨迹速度，最高能够达到10.5 m/s。基于本控制器，四旋翼还可以实现各种复杂的控制策略，并且可以对四旋翼的姿态进行规划，这也将是下一步的研究方向。