四旋翼飞行器姿态与高度控制器的设计

翁理国，李 倩，王 伟

（南京信息工程大学 信息与控制学院，江苏 南京 210044）

MAVS是一种由动力驱动、无人驾驶、可重复使用的航空器.因具有体积小、重量轻、机动灵活、环境适应性强等优点，已成为集军用、商用、民用多位一体的无人驾驶工具［1］.随着微机电系统（micro electromechanical systems，简称MEMS）的发展，单旋翼直升机一直是微型飞行器发展的研究平台，其复杂性及不稳定性却限制了它的发展.如今，一种新型的微型多翼飞行器，特别是多旋翼飞行器越来越受到人们的重视.由于四旋翼微型飞行器具有稳定性高的特点，因此广泛应用于军事侦察、气象探测、环境监测和空中交通应急等方面［2］.

在实际应用中，飞行器需要自主飞行完成上述艰巨的任务.由于它体积小、结构简单，因此通信设备的安装存在技术上的难题.到目前为止，许多科研团队在解决这些难题上取得了巨大的成就［3－6］.为实现自主飞行，研究人员采用了多种控制方法，如自适应、模型预测及模糊控制等［7］.通常情况下，飞行控制器的性能受限于外部扰动和模型误差，因此，设计真正有效的、具有鲁棒性的控制器具有重要意义.

作者拟研究姿态和高度的动力学控制方法.为了实现姿态控制，采用基于历史信息的控制器，即记忆控制器，这种控制器的最大优势在于控制性能与外部干扰无关，能够提高系统的鲁棒性.为了准确控制飞行器的飞行高度，采用基于最优控制理论的LQG控制方法设计控制器.

1 被控对象

图1为四旋翼微型飞行器，是该设计的被控对象.机体的主体部分由碳纤维制作，以获得较轻的重量和较好的强度.飞行器动力由4个T－MOTOL直流无刷电机提供，其频率在50～500Hz范围，最大转速可达837rad·s－1.采用4个APC1245螺旋桨，飞行器可载重约1kg，搭载1节2 100mAh的锂聚合物电池，空载飞行时间约10min.

2 建模和控制器设计

图2为控制回路结构图.如图2所示，一个完整的飞行器控制系统可分成4个部分：X，Y，Z方向控制器和偏航角控制器.该设计主要研究其中的姿态控制及高度控制.

2.1 姿态角建模

由于四旋翼飞行器的6个自由度是通过调节电机的转速得到，因此它是一个强耦合、非线性的多输入多输出系统.考虑到在实际飞行过程中姿态角变动范围较小，可以忽略各个自由度之间的耦合，把系统简化为各个独立的单输入单输出线性系统，分别设计控制器.此外，四旋翼飞行器的结构具有对称性，滚转角和俯仰角的结构相同，因此在建模时只考虑滚转角.

将四旋翼飞行器放置在3维坐标系中，假设其载体坐标系为N，机体质量为m，外力为F∈R3，转矩为τ∈R3，将转矩与姿态角的非线性关系近似为线性关系（此时假设姿态角变化很小），根据动力学定理，可得到以下关系式［8］

飞行器的姿态是载体坐标系相对参考坐标系的位置关系.载体坐标系是以飞行器重心为原点，记为On，而参考坐标系是以地面为原点，记为Oi.V＝（u，v，w）中的u，v，w为载体坐标系3个轴方向的角速度，它们分别对应飞行器的前方向Xn轴、飞行器的右方向Yn轴和飞行器的下方向Zn轴.Q＝（p，q，r）中的p，g，r为参考坐标系下的角速度，它们分别对应磁场的北极方向Xi轴、重力方向Zi轴和OiXiZi平面的垂直方向Yi轴.

欧拉角ξ＝（φ，θ，ψ），其中φ为滚转角，θ为俯仰角，ψ为偏航角.将载体坐标系转换到参考坐标系的旋转矩阵如下

此时矩阵R满足：R－1＝RT，det（R）＝1.

参考坐标系下3个方向的角速度Q与欧拉角的3个角速度的关系如下

其中：欧拉矩阵Γ（ξ）［9］为

通过方程式（4）对时间的微分以及方程式（2）可得到

其中，sk（Q）可将Q从R3变换到R3×3.

滚转角速度的二阶微分方程表达式为

其中：Jφ和τφ分别为绕Xi轴的转动惯量和转矩.

一般地，控制量u和实际获得的转矩τφ近似为线性关系，因此可将式（8）变为

2.2 记忆控制

在姿态控制器设计中，考虑到四旋翼飞行器具有高度非线性和强耦合的特性，设计一个具有良好稳定性和跟踪性的控制器十分必要，作者采用记忆控制方法来提高控制器的鲁棒性能［10］.

令滚转角运动偏差为ε＝φ－φ＊，其中φ＊为获取的滚转角的目标值，由式（9）得到的滚转角运动偏差微分表达式为

为方便处理，引入一个新的变量e，将原先涉及φ的二阶跟踪问题简化为用e表示的一阶稳定系统，即

将e的微分代替，得到

定义系统控制输出为

通过公式（12），（13）将一阶稳定系统e变为

记忆控制的基本思想是利用当前与先前的系统响应以及过去的记忆信息来构建控制方法，因此也被称为经验控制［11］.考虑到实际应用中，当前的系统状态对未来的影响最大，因此选择当前系统的一阶线性函数构建控制方法.

通过欧拉微分，将式（14）一阶稳定系统变为

其中：T为采样周期；vk为记忆控制输入.

由式（15）得到的前一时刻方程式为

将式（15）减去式（16），得到

由于记忆控制需要使用过去的控制经验vk－1、当前的跟踪误差ek及先前的跟踪误差ek－1，故定义vk为

考虑到e被定义为一个抖动范围为T2λ的滑动平面，当e逐渐变小时，跟踪误差ε及其导数也将变小.式（21）表明如果函数f（·）的变化不是无限快，可认为跟踪误差和它的导数也很小（此时T非常小）.

综上所述，根据循环周期T、目标值φ＊的姿态角模型及式（13）的变换方程uk－1＝Jφ（vk－1＋ξk－1），可得记忆控制器表达式为

根据上文可知，为使f（·）在不同函数下都能保持较好的跟踪性能，假设f（·）函数如下

图3为记忆控制器的仿真结果，其中参数T＝0.02，k0＝1，J＝0.173 2.由图3可以清楚地看到，不论在f1（·）还是在f2（·）函数下，记忆控制器都能保持极好的跟踪性能，实现了良好的鲁棒性.

2.3 高度建模

四旋翼飞行器的高度控制是通过同时调节4个电机转速来实现的.根据电机驱动原理，控制输入u（s）与转速n（s）的输出呈线性关系，同时转速n（s）的输出与产生的浮力F（s）也近似呈线性关系［12－13］，于是可以得到下面的传递函数

其中：k，T1均为常数.浮力F与加速度、速度与位置的关系式可表示为

其中：φ，θ分别为滚转角和俯仰角；Z为高度；Cx＝cosx；fZ为空气阻力；m为飞行器的质量.由于四旋翼飞行器主要以定点悬浮为主，飞行过程中的倾斜角很小，忽视空气阻力，对式（26）进行线性化，并通过拉普拉斯变换得到如下公式

把式（25）代入式（27）当中，可得从控制输入到位置输出之间的传递函数为

通过实际飞行的输入输出实验数据，利用Matlab中的ident工具拟合可得到式（28）中的未知参数.图4为仿真模型验证结果.由图4可知，仿真模型是准确的.

为实现控制器的输出能够准确跟踪参考输入信号的变化，引入一个额外的状态变量xp（t）＝扩展的高度状态方程为

2.4 LQG 控制

LQG是一个基于最优控制理论的控制器设计方法［14］，该文采用LQG理论设计控制器实现高度控制.

用状态空间将式（28）的传递函数表示为

其中：w（t）为参考输入；I为单位矩阵

定义二次性能指标泛函为

构造哈密顿函数为

伴随方程及其边界条件［15］为

最优控制应满足

又由于R正定，且其逆存在，于是有

假设λ（t）和状态变量xa（t）呈线性关系，即

其中：P为里卡蒂代数方程式

的解.

将式（37）代入式（38），得

由于控制器的设计需要所有的状态变量，通过上面的状态反馈获得加速度和高度数据，但是仍有3个状态量无法测得.为此，设计卡尔曼滤波器推测系统中间状态变量，并且其中一些状态量可对机载传感器的高频噪声进行滤波，提高控制器的控制品质.

3 程序流程图

图5为记忆控制方法流程图和LQG控制方法流程图.

在图5的左图中，T表示控制循环周期，pre表示前一时刻的控制量，ref表示获取的遥控器发送过来的目标值，该设计获取的是滚转角的目标值.在图5的右图中，状态变量X1，X2，X3分别表示卡尔曼滤波推测出的Z方向的加速度、位置及速度，其中X1，X2是用来对机载传感器的高频噪声进行滤波的.

4 实验及其分析

通过室外飞行实验来验证设计的姿态和高度控制器的性能.MAVS的目标值由2.4GHz的无线遥控器给定，飞行状态由无线传输模块XBee－pro传回，以实现地面站与飞行器之间的数据传输.

4.1 姿态控制实验

通过实际飞行来检验姿态控制器的性能及仿真结果的正确性.

图6为飞行器悬停实验结果.从图6中可以看出，飞行器姿态角变动保持在±2°.即使在实际飞行环境下有强风和其他干扰，姿态角的变动也是在稳定范围之内.图7为飞行器鲁棒性实验结果.由图7可知，控制器几乎不受模型变化的影响，能够很好地跟踪目标.

4.2 高度控制实验

通过实际飞行来检验高度控制器的性能及仿真结果的正确性.手动操作遥控器使飞行器在空中定点悬停一段时间，然后切换飞行模式，接收遥控器发送来的目标值.图8为飞行器高度的跟踪实验结果.由图8可知，实际飞行的实验结果基本与模拟结果一致，验证了该设计数学模型的可行性与准确性，此控制器有很好的稳定性与追踪性.

5 结束语

以自主研发的四旋翼飞行器为控制对象，研究它的姿态与高度控制器的设计方法.在姿态控制器设计中，采用基于记忆的控制方法，实验结果表明此控制器具有良好的稳定性和跟踪性，尤其是姿态模型改变及外部干扰下，控制器仍然获得良好的控制性能，体现了该控制器的鲁棒性.在高度控制器设计中，采用基于最优控制的LQG控制方法，实验结果表明此控制器也能达到预期控制效果.

［1］Martinez V.Modelling of the flight dynamics of a quad rotor helicopter［J］.A MSc Thesis in Cranfield University，2007，71（2）：149－438.

［2］Achtelik M，Bachrach A，He R J，et al.Stereo vision and laser odometry for autonomous helicopters in GPS－denied indoor environments［C］／／Grant R，Douglas W，et al.International Society for Optical Engineering，Orlando：SPIE，2009：733219－733229.

［3］Bachrach A，He R J，Roy N .Autonomous flight in unknown indoor environments［J］.International Journal of Micro Air Vehicles，2009，1（4）：217－228.

［4］Wang W，Nonami K，Mitsuo H.Model reference sliding mode control of small helicopter XRB based on vision［J］.International Journal of Advanced Robotic Systems，2008，5（3）：235－242.

［5］Hoffmann G M，Huang H，Waslander S L，et al.Precision flight control for a multi－vehicle quad rotor helicopter test bed［J］.Control Engineering Practice，2011，19（9）：1023－1036.

［6］Kendoul F.Survey of advances in guidance，navigation，and control of unmanned rotorcraft systems［J］.Journal of Field Robotics，2012，29（2）：315－378.

［7］Xia M，Zhang Y C，Weng L G，et al.Fashion retailing forecasting based on extreme learning machine with adaptive metrics of inputs［J］.Knowledge－Based Systems，2012，36：253－259.

［8］Tayebi A，McGilcary S.Attitude stabilization of a VTOL quadrotor aircraft［J］.Control Systems Technology，IEEE Transactions on，2006，14（3）：562－571.

［9］Nonami K，Kendoul F，Suzuki S，et al.Autonomous flying robots：unmanned aerial vehicles and micro aerial vehicles［M］.Verlag：Springer，2010.

［10］Rudin K，Hua M D，Ducard G.A robust attitude controller and its application to quad rotor helicopters［C］／／18th IFAC World Congress，2011：10379－10384.

［11］Song Y D.Memory－based control of nonlinear dynamic systems part I－design and analysis［C］／／Industrial Electronics and Applications，1st IEEE Conference on，IEEE，2006：1－6.

［12］Kendoul F，Lara D，Lozano R.Real－time nonlinear embedded control for an autonomous quad rotor helicopter［J］.Journal of Guidance，Control，and Dynamics，2007，30（4）：1049－1061.

［13］Wang W，Song Y Z，Nonami K，et al.Attitude controller design for a six－rotor type MAV［J］.Key Engineering Materials，2011，480（4）：1155－1160.

［14］Kendoul F，Fantoni I，Lozano R.Asymptotic stability of hierarchical inner－outer loop－based flight controllers［C］／／Proceedings of the 17th IFAC World Congress，2008：6－11.

［15］Lewis F L，Vrabie D，Syrmos V L.Optimal control［M］.New York：John Wiley ＆Sons，2012.