四旋翼飞行姿态测试平台设计及稳定性分析

高佳佳 高嵩 曹凯 陈超波

摘 要：四旋翼飞行器非线性、强耦合以及欠驱动的特性极大地增加了算法设计与姿态控制的难度。为了减少四旋翼飞行器飞行过程中不必要的干扰因素和事故率，自主设计了四旋翼飞行姿态测试平台。通过对系统进行受力分析和动态建模，采用能够快速稳定的双闭环串级PID控制算法设计了姿态控制器，并使用卡尔曼滤波算法进行姿态估计，然后分别在Matlab环境和平台上验证了飞行姿态的稳定性。实验结果表明了平台设计的合理性和可行性，是一种有效的四旋翼飞行姿态测试平台。

关键词：四旋翼飞行器；测试平台；姿态控制；双闭环串级PID控制；卡尔曼滤波

中图分类号：TP29 文献标识码：A

Abstract： The quadrotor aircraft has the characteristics of nonlinearity，strong coupling and underactivity，which greatly increase the difficulty of algorithm design and the attitude control.In order to reduce the non-essential disturbance factors and accident rate during the flight of the aircraft，and independently designing a test platform to test quadrotor flight attitude.Through the force analysis and dynamic modeling of the system，the attitude controller is designed by a double closed-loop cascaded PID control algorithm which can stabilize quickly，and the attitude is estimated using Kalman filter algorithm.The stability of flight attitude is verified respectively on Matlab environment and the platform.The experimental results show that the platform design is reasonable and feasible，and it is an effective test platform.

Keywords： quadrotor aircraft；test platform；attitude control；double closed-loop cascade PID control；Kalman filter

1 引 言

四旋翼飛行器具有机动灵活、操纵方便、垂直起降的优点，可执行的任务相对复杂化和多样化，诸如监视、营救、电力巡线和物流快递等繁琐危险的任务[1]。但是在飞行过程中存在许多不确定的因素，具有复杂的空气动力学特性，因此，学者们利用各种控制算法来设计飞行器控制系统[2-4]，如反步控制、非线性鲁棒控制、LQR控制器等。

为了减少四旋翼飞行器飞行过程中的非必要干扰因素及事故率，遵循国家限定飞行条例，四旋翼飞行器室内测试平台的设计变得尤为重要。文献[5]中在室内分布一圈红外摄像头和Qball-X4飞行器构建实验测试环境，利用红外摄像头对飞行器进行定位来确定飞行器飞行是否稳定，如图1（a）所示，测量精度高和实时响应速度快，但是定位测量的方法较为复杂，利用无线网络进行通信，容易出现信号干扰甚至中断，成本也相当昂贵。文献[6]中三轴半实物仿真转台主要是模拟飞行器运动的三维姿态，快速验证控制算法的有效性，但是结构复杂，体积庞大，造价较高，如图1（b）所示。文献[7][8]中测试平台使用一根刚性支杆固定在地面底座上，顶端采用三个万向节和拉绳固定连接飞行器，如图

1（c）所示。虽然飞行器的姿态没有受到较大影响，但是限制了X和Y方向上的一部分运动，形成小规模的扰动，从而影响控制稳定性。文献[9]的测试平台是在地面底座安装180°万向球柱，刚性杆与飞行器直接固定连接，如图1（d）所示。虽然可以解放X和Y方向上的一部分运动，但是对飞行姿态的影响较大，还会受到刚性杆的一部分反作用力，建模分析和采用控制律时，需要考虑更多的影响因素，增加了控制的复杂度。

本文中，搭建了一种成本低廉，又尽可能不影响飞行器飞行状态的测试平台，在地面底座上安装2自由度球头柱关节，形成X、Y轴的运动，抵消一部分刚性杆产生的反作用力，减少系统扰动，采用360°万向节连接飞行器，解决姿态限制问题，虽然锁定了一部分自由度，但是降低了控制的复杂度。

大量实验结果表明，非线性鲁棒控制可以取得较好的飞行控制效果，反步控制方法在姿态角控制方面表现更佳。但是都依赖于精准的数学模型，工程计算量大，数据的实时处理要求较高，实现起来具有一定困难。为此本文设计了一种易于实现，不需要精准的模型，能够快速稳定的双闭环串级PID控制算法对姿态角进行控制，并在自主设计的测试平台上进行受力分析和动态建模，验证了基于双闭环串级的四旋翼飞行器姿态控制的稳定性。仿真实验和实际试飞结果表明，在自主设计的测试平台上，基于该算法的四旋翼飞行器的姿态控制具有较好的控制效果。

2 四旋翼飞行器动作原理

四旋翼飞行器在飞行时的动作可分为6种，即沿轴向的3种平行运动和围绕轴向的3种旋转运动。平行运动为沿着x，y，z方向的运动，主要研究飞行器的位置，旋转运动则可分为横滚运动（Roll）、俯仰运动（Pitch）、偏航运动（Yaw），主要分析飞行器的姿态。参考系一般分为大地坐标系和机体坐标系。大地坐标系（惯性坐标系或者导航坐标系，用e或者G表示）用于研究飞行器相对大地的运动状态以及空间位置坐标。机体坐标系（用b或B）坐标原点取机体的重心，用于研究飞行器相对于重心的旋转运动。四旋翼飞行器动力模型如图2所示，xB方向指向飞行器的前（横滚轴）yB方向指向飞行器的右（俯仰轴）和zB方向指向飞行器的下（偏航轴）方向[10]。

四旋翼飞行器产生基本动作的原理为：为了抵消旋翼飞行器陀螺效应和空气动力扭矩效应，四旋翼飞行器上两两相邻的电机转速方向是相反的，保证4个电机转速一致时机身不发生转动。电机1和2转速同时增大或减小，与此同时电机3和4转速减小或者增大，四旋翼飞行器产生横滚运动。电机1和4转速减小或者增大，同时电机2和3转速增大或者减小，四旋翼飞行器产生俯仰运动。电机1和3转速同时增大或者减小，电机2和4电机转速同时减小或者增大，四旋翼飞行器产生偏航运动。4个电机转速同时增大或这减小，升力大于重力向上运动，升力小于重力向下运动[11]。

3 四旋翼飞行器姿态解算和动力计算

3.1 姿态解算

欧拉角是基于飞行器本身轴旋转得到的（与参考坐标系无关），然而得到的飞行器姿态却是相对于参考坐标系而言的。四旋翼飞行器飞行时机体坐标和大地坐标系之间的变化可视为三次绕着定轴的旋转，绕轴顺序为Z-Y-X，如图3所示[12]。旋转之后的矩阵即为机体坐标系b到大地坐标系e的转换矩阵，如公式1所示。

欧拉角的微分方程包含大量的三角函数运算，给四旋翼飞行器姿态解算造成不便，况且“万向节死锁”的存在，导致欧拉角方法并不适用于全姿态四旋翼飞行器的姿态确定[13]。由此我们引入四元数姿态解算，而四元数线性方程组计算量小，易于操作，比较贴近工程实际。

推导得到的四旋翼飞行器模型与实际飞行中的动态模型略有简化，如忽略空气阻力、球面万向节摩擦，轻质拉杆的重量等，加入的动态模型影响参数越多，对应的控制算法的复杂程度也会随之增加，因此忽略了一部分对系统影响较小的因素，有利于控制算法的设计。

4 四旋翼飞行器测试平台

在旋翼飞行器开发测试阶段，因为仿真实验平台的大量缺乏，导致飞行器复发性故障如传感器饱和、电机故障以及传输信号中断导致失控。其次，国家颁布的限制飞行区域、天气影响和远距离数据传输的数据包缺失等对室外飞行的限制和影响条件的增加。

对比相关飞行器测试平台资料，文献[5]中测试平台的测量精度和准确性较高，但是定位测量算法复杂，无线网传输容易受到干扰，成本也相当昂贵。文献[6]的半实物仿真平台一般是模拟飞行器的飞行状态，验证控制算法的有效性。文献[7][8]中测试平台限制了飞行器X和Y方向的运动，会对横滚和俯仰角度形成小规模扰动。文献[9]测试平台一定程度上限制了飞行姿态，还容易受到飞行器对杆的反作用力。因此，针对于它们存在的一些问题，搭建了如图5所示的测试平台。

测试平台主要由以下五个部分组成：①四旋翼飞行器机架（聚酯纤维复合材料，重量大约为450 g）；②飞行控制板（集成惯性测量单元、GPS模块等）；③电机模块（直流无刷电机和20 A电子调速计）；④带有360°万向节的轻质拉杆；⑤2自由度球头柱关节。

考虑实验场地、成本以及易于操作等因素，本文在地面底座上加装2自由度球头柱关节，以便于可以解放一部分飞行器在X和Y轴的运动，抵消刚性杆对飞行器的部分反作用力，从而减少系统误差。采用360°万向节连接固定飞行器，解决飞行姿态限制问题，飞行器可以灵活的进行姿态调节，从而增强控制稳定性。这种设计可以带来两方面的好处，一方面是测试平台可以帮助锁定X-Y-Z方向上的一部分自由度，简化了控制模型，降低了飞行器控制的复杂程度，并且减少了飞行过程中飞行器因为设定的参数不正确或者通信错误出现失控而造成的损坏。另一方面，操作人员可以直观地观察到飞行过程中飞行器的飞行状态，以及飞行的稳定性。如果飞行器在误差范围内可以稳定悬停，轻质拉杆和万向节都会随着保持直立，即当系统稳定运行时，只是约束了其在Z方向上的运动。反之，则说明当前系统不具有稳定性，需要重新调整参数。

系统设计中，预先对应地设计了手动操作和自动操作两套测试方案，手动操作方案通过操控2.4 GHz无线遥控器来控制四旋翼飞行器悬停在控制，这种方案的优点是通过操作员的视觉反馈，迅速将飞行器上升并悬停，但是手动操作很可能使飞行器沿Z轴方向的力较大，从而飞行器会受到支杆对于它的反作用力，最终影响稳定性。自动操作方案使用PC端通过串口对带有惯性测量单元（3轴加速度计+3轴陀螺仪）的MCU（微控制器）下发飞行指令，然后MCU给电机下发实时控制指令，最终达到悬停位置，这种方案实现了完全意义上的无人机自主飛行，但是调整时间较长，到达目标位置需要依靠较多的传感器回传的数据，在多传感器数据融合使用时，就容易出现误差或冗余数据。

当飞行器稳定悬停后，飞行状态信息以及姿态信息可以通过无线数传模块（WIFI、蓝牙等方式）以及有线串口进行传输，为了减轻机身重量，本文采用控制器自带的MicroUSB接口进行有线传输。控制器将飞行器目前的姿态数据（三维方向角Pitch、Roll、Yaw、加速度计、气压计、磁力计、陀螺仪）通过串口传输到电脑端，并自动绘制成相应的曲线，通过对比分析曲线可以清晰的判断四旋翼飞行器飞行状态的稳定性。

5 四旋翼飞行器控制算法

5.1 双闭环串级PID控制

在四旋翼飞行器正常飞行时，遇到磁场干扰或者强外力时，会使磁力计和加速度传感器采集到的数据部分失真或者有大的偏差，导致欧拉角解算错误，进一步的控制它的角速度，可以增加飞行器的稳定性（增加阻尼）并提高它的控制效果，于是角度-角速度串级PID控制算法应运而生。双闭环系统的引入就解决了此问题，由于陀螺仪采集到的角速度数据，抗干扰能力强，并且角速度变化灵敏。同样的高度环中气压传感器也会受到外界干扰，因而在内环中引入Z轴的加速度分量，增强了系统的鲁棒性[15]。

四轴飞行器双闭环串级PID控制，图6（a）为姿态环PID控制整体框图、图6（b）是高度环PID控制整体框图。角度作为外环，角速度作为内环，进行姿态PID控制；定高模式下，高度作为外环，Z轴加速度作为内环，进行高度PID控制。其中，PID输出为油门值，并通过电子调速器控制电机使空间三轴欧拉角和高度变化[16]。

计算出来实际四旋翼飞行器飞行高度，其中BP为当前气压值、BS为起始气压值，选用的气压传感器中集成了温度传感器和气压传感器。

5.2 滤波算法

四旋翼飞行器系统中如果使用一些简单的数据处理方式很难满足精度和实时性上的要求。本文中，采用卡尔曼滤波将加速度传感器和陀螺仪的数据进行融合，抑制噪声的干扰使得四旋翼飞行器达到稳定状态，提高测量精度[18]。

卡尔曼滤波是一种最优线性状态估计方法，输入当前的测量值和上一个周期的估计值，估计当前的状态，然后估计当前传感器数据和上上一个状态的数据，为当前最优估计。例如位置估计时，就是位置坐标输入的传感器数据，最后输出的最优估计用来作为四旋翼飞行器当前位置被导航算法以外的程序调用。算法的核心思想就是根据传感器当前的测量值和上一时刻的预测量和误差，计算当前的最优量，再预测下一时刻的值[19][20]。

6 实验分析与结论

稳定悬停的指标可以通过三个轴上的加速度和陀螺仪上横滚、俯仰、偏航角度反映出来。横滚和俯仰的角度偏差越小以及偏航角度的变化量越小说明系统的稳定性越好。为了更加真实地模拟飞行器的稳定悬停，在Matlab2016a中构建了模拟飞行器，设定高度为1 m，横滚角和俯仰角为0°（0 rad），偏航角为30°（0.5236 rad），，在启动仿真后，可以看到模拟飞行器起飞到稳定悬停的过程，如图7所示。

Matlab仿真输出的横滚角度、俯仰角度和偏航角度分别如图8（a）、（b）、（c）所示。每个图分为左中右三部分，分别表示实际测量值、期望值以及角度控制的输出值。以图8（a）为例，测量的横滚角度出现波动是因为飞行器在接近期望值时进行微调，同时可以看到角度控制输出值也在实时响应。

在室内测试平台上进行实际稳定悬停时，使用手动控制，起飞稳定后转换为稳定悬停档，使飞行器可以稳定悬停。图9（a）、（b）、（c）分别为四旋翼飞行器在实际飞行中横滚角度、俯仰角度、偏航角度。由于飞行器在飞行时受到反向气流等外界影响，实验结果与期望值存在误差也是在所难免。

由图9（a）和（b）可以直观地看到平稳飞行状态下横滚角度和俯仰角度分别稳定在1.62°（0.028rad）和0.32°（0.005rad）左右，均在稳定误差范围以内，满足实验要求。从图9（c）可知，平稳飞行状态下偏航角度误差基本稳定在-78.95°附近，单位时间的角度变化量为±0.5°（0.0087rad），在稳定误差范围以内，满足实验要求。

采用不需要精准数学模型，可以快速稳定的双闭环串级PID控制算法进行位姿的闭环控制，一方面，验证了四旋翼飞行器在自主设计的测试平台上的姿态稳定性，另一方面，证明了经典控制器在小扰动情况下控制方位角的能力。仿真数据与实际飞行获取的数据相比，可以看出由于室内反向气流、测试平台的机械误差等因素的影响，理论数据和实际数据之间虽然存在一定的误差，但是对于四旋翼飞行器的姿态控制依旧具有良好的控制效果，各项指标均可以控制在误差范围之内，表明了该平台设计的合理性和可行性，是一套有效的四旋翼飞行姿态测试平台。未来的目标是优化姿态控制器和平台的设计，在此基础上开发一套完全自主飞行器。

参考文献

[1] 何道敬，杜晓，乔银荣.无人机信息安全研究综述[J/OL].计算机学报，2017：1—21.

[2] 张镭，李浩.四旋翼飞行器模糊PID姿态控制[J].计算机仿真，2014，31（08）：73—77.

[3] 甄红涛，齐晓慧，夏明旗，等.四旋翼无人机鲁棒自适应姿态控制[J].控制工程，2013，20（05）：915—919.

[4] XU G，MO Z.Modified adaptive flight control of quadrotor based on single neuron PID[C]. International Conference on Information Science and Technology.Yangzhou，China，IEEE，2013：313—316.

[5] 张友民，余翔，刘丁，等.四旋翼无人系统的容错控制算法设计与实现[J].控制工程，2016，23（12）：1874—1882.

[6] 高鹏飞，郑云龙.某型无人直升机飞行控制系统半实物仿真[J].兵工自动化，2017，36（06）：90-92+96.

[7] BOUABDALLAH S，SIEGWART R.Backstepping and sliding-mode techniques applied to an indoor micro quadrotor[C].IEEE International Conference on Robotics and Automation.2006，3：2247—2252.

[8] BOUABDALLAH S，SIEGWART R.PID vs LQ control techniques applied to an indoor micro quadrotor[C].IEEE Internationel Conference on Intelligent Robots and Systems 2004，3：2451—2456.

[9] TAYEBI A，MCGILVRAY S. Attitude stabilization of a VTOL quadrotor aircraft[J].IEEE Transactions on Control Systems Technology，2006，14（3）：562-571.

[10] 宋宇，宋雋炜.四旋翼无人机的姿态稳定控制器设计仿真[J].计算机仿真，2017，（08）：85—88.

[11] 赵常均，宫勋，白越，等.四旋翼飞行器中升力波动的干扰与抑制[J].光学精密工程，2014，22（09）：2431—2437.

[12] 张帅，王志萍，夏飞.四旋翼飞行器姿态的非线性控制仿真研究[J].计算技术与自动化，2017，36（03）：1—5.

[13] 李汉博，秦文华.四旋翼惯性导航姿态解算算法的改进与仿真[J].计算机测量与控制，2016，24（07）：187-189+194.

[14] MAHONY R，KUMAR V，CORKE P.Multirotor aerial vehicles：Modeling，estimation，and control of quadrotor[J].IEEE Journal of Robotics and Automation.2012，19（3）：20-32.

[15] 戴启浩，马国梁.四旋翼飞行器的自抗扰控制方法研究[J].计算技术与自动化，2017，36（02）：46—50.

[16] 薛佳乐，程珩.基于串级PID四旋翼飞行器控制系统研究[J].电子技术应用，2017，43：134-137+142.

[17] 吴宏鑫，沈少萍.PID控制的应用与理论依据[J].控制工程，2003，10（1）：38—42.

[18] 王立，章政，孙平.一种自适应互补滤波姿态估计算法[J].控制工程，2015，22（ 5） ：881—886.

[19] 张国云，李亚斌，向烂群，等.四旋翼飞行器悬停算法设计与实现[J].计算技术与自动化，2016，35（01）：77—81.

[20] XIONG Jing-jing，ZHENG En-hui.Optimal Kalman filter for state estimation of a quadrotor UAV[J].Optik-International Journal for Light and Electron Optics，2015，（11）：2862—2868.