小型四旋翼无人机姿态控制研究

徐琬婷，张强强，梅 源

（1.芜湖职业技术学院 汽车与航空学院，安徽 芜湖 241006；2.沈阳航空航天大学 人工智能学院，辽宁 沈阳 110136）

随着嵌入式技术及计算机、传感器技等相关技术的发展,小型四旋翼无人机以其结构简单、易操作等优势在各个领域都得到了广泛的应用。小型四旋翼无人机由于自身承载能力有限，需要搭载体积小、惯性低和分辨率受限的传感器，在极端环境下，容易受温度、噪声和震动等因素影响，从而降低了无人机的姿态控制可靠性。为了解决这一问题，本设计利用了卡尔曼滤波有效增加了传感器采集数据的准确性，并采用了多种非线性控制方法，如PID 控制算法、滑模控制算法和LQR 控制算法等，有效增加了无人机的飞行性能、稳定性、适应性。

1 系统构成及飞行姿态控制原理

1.1 系统总体架构和功能概述

小型四旋翼飞行器的主要组成部分包括STM32103C8T6 主控芯片、OLED 显示模块、无线通信模块、MPU6050 姿态信息采集模块、光流模块、电源模块等。系统启动后，传感器开始收集姿态和位置信息，并将数据传输回单片机进行处理，实时通过液晶显示屏显示。本次测试所用的小型四旋翼无人机系统结构如图1 所示。

图1 无人机系统结构

小型四旋翼无人机的控制系统以STM32 为主控芯片，负责飞行动作和数据处理。采用MPU6050 姿态传感器获取四旋翼飞行位置姿态信息，并利用GL9306 光流模块作为位移传感器感知无人机位置并实时发送给处理器进行处理。SPL06 气压计模块测得高度信息，并通过OLED 显示屏实时展示位置姿态参数。HotRC 无线通信模块实现与四旋翼无人机的通信和数据传输，以实现对无人机的即时控制。当系统遭受强干扰、电源电压低或飞行高度超过设定阈值范围时，系统将紧急迫降以保障设备安全［1］。

1.2 四旋翼无人机的飞行原理

四旋翼无人机主要分为“X”形和“十”形，本次研究聚焦于“十”形结构，当桨叶高速旋转时，螺旋桨产生向上的升力。当电机达到一定转速时，4 个螺旋桨所提供的升力大于其自身重力G，使无人机起飞。

为了确保无人机顺利起飞，必须保证4 个桨叶具有相同的速度和角度，并产生相等的升力。螺旋桨旋转方向示意图见图2，一对桨叶以相同速度旋转方向相反的状态旋转，在静态平衡下实现四旋翼本体抵消反扭矩效应及陀螺效应［2］。

图2 四旋翼无人机的螺旋桨旋转方向示意图

四旋翼无人机可以通过两组电机的协同作用实现多种姿态变化，包括悬停、翻滚、垂直运动、俯仰和偏航5 种运动状态。而四旋翼无人机的姿态变化与4 个螺旋桨的转向和转速密切相关，只需精确控制各个旋翼的运动即可驾驭整体飞行。

1.3 悬停和垂直运动

四旋翼无人机应在悬停的基础上完成各种运动，因而悬停是最基本的运动状态。当螺旋桨产生的升 力F总（F总=F1+F2+F3+F4，其中F1、F2、F3、F4分别为机翼1、2、3、4 的升力）大于四旋翼无人机所受的重力时，将产生向上的加速度，使得无人机起飞；当重力与升力相等时，实现了无人机的悬停状态。在此基础上，通过同时调整四个电机转速可以实现垂直运动。四翼无人机受力分析如图3 所示。若F总=重力G，则四旋翼无人机处于悬停状态；若F总＜重力G，则四旋翼无人机会垂直下降。

图3 四旋翼无人机受力分析

1.4 翻滚运动

设Front 指向四旋翼无人机的前方，当四旋翼无人机处于悬停或前进状态时执行顺时针或逆时针转操作可使四翼无人机进行滚动运动。在保持垂直方向总升力不变的情况下，通过调整1 号和3 号电机的转速来实现顺时针或逆时针翻滚。同时，控制当前姿态变化的2 号和4 号旋翼增加转速以保持四旋翼无人机高度稳定。当F1＞F2=F4＞F3时，四旋翼无人机将进行逆时针滚动；而当F3＞F2=F4＞F1时，则会进行顺时针滚动，如图4 所示。

1.5 俯仰运动

当需要将无人机控制到特定位置时，仅通过翻滚是不足以完成任务的，还需结合俯仰运动。俯仰运动与翻滚运动的控制原理相同，通过保持2 号和4号电机转速不变，并调节1 号和3 号电机转速来实现前后移动。然而，在非控制当前姿态变化的旋翼1号和3 号转速保持不变时四旋翼高度不变。当F2＞F1=F3＞F4时，四旋翼无人机前倾；当F4＞F1=F3＞F2时，四旋翼无人机后仰。

1.6 偏航运动

除了具备上下、前后直线运动功能外，四旋翼无人机还能实现偏航运动，偏航运动通过同时控制两组旋翼的转速来实现。其中2 号和4 号电机为一组，1 号和3 号电机为一组，在不同转速下利用反扭力差异来达到偏航运动的目的。当F2=F4＞F1=F3时，四旋翼无人机逆时针偏航；当F2=F4＜F1=F3时，四旋翼无人机顺时针偏航。

2 软件数据处理

小型四旋翼飞行器在飞行过程中需要实时处理大量准确的数据，否则经过多次计算后会产生较大误差，从而对四旋翼无人机的姿态判断和飞行性能造成影响。为了减少环境因素和干扰信号对飞行的影响，在搭载传感器输出数据时需要进行滤波处理，并结合控制算法以提高小型四旋翼无人机的精确性和抗干扰能力［3］。

2.1 数据处理

MPU6050 姿态传感器可直接输出6 轴信息，直接输出的6 轴数据精度可达到0.01°，经过对6 轴信息进行姿态解算可以得到四旋翼无人机的姿态角。定义绕X 轴旋转为俯仰角θ,绕Y 轴旋转为滚转角ϕ,绕Z 轴旋转为偏航角ψ。

2.1.1 DMP 解算方式

MPU6050 是一款高性能的六轴惯性测量单元，内部集成了运动处理单元DMP（Digital Motion Processor），通过DMP，用户可以直接从MPU6050 中读取四元数（q1，q2，q3，q4），并将导航坐标系转换为机体坐标系，其转换关系可表示为

其中：

且四元数满足条件

q1，q2，q3，q4分别为

（xb,yb,zb）为机体坐标，（xn,yn,zn）为导航坐标。

由式（1）可得，四旋翼无人机的姿态角为θ、ϕ、ψ,即

在解算四旋翼无人机飞行姿态角时，可用DMP硬件进行四元数计算，并利用式（2）中的转换关系将其转化为姿态角。在使用硬件DMP 进行姿态解算时，仍然存在漂移现象（包括温度漂移和零点漂移）、自身振动对姿态角产生抖动影响和传感器受到具有不确定性的环境因素（如磁场变化等）的干扰，因此，需要采用数据处理算法对数据进行滤波处理以使环境变量的影响最小化。

2.1.2 卡尔曼滤波方式

卡尔曼滤波是一种用于估计系统状态的数学方法，它根据当前时刻的观测值和前一个时刻的预测值之间的误差，通过计算系统状态进行最优估计，并预测下一个时刻的状态［4］。

MPU6050 解算过程可分为3 步。首先，从MPU6050 读取3 轴加速度计和3 轴陀螺仪的数据。其次，通过积分处理3 轴加速度计值，并通过几何运算将3 轴陀螺仪数据转换为相应的姿态角。由于电场、磁场等因素的干扰，所得到的姿态数据容易受到影响，因此，需引入卡尔曼滤波算法对数据进行融合滤波处理，以提高姿态数据的准确性。图5 为DMP姿态解算方式和卡尔曼滤波姿态解算方式的MATLAB 仿真结果。

图5 DMP 姿态解算方式和卡尔曼滤波姿态解算方式MATLAB 仿真结果

由图5 可知，用DMP 方式解算姿态角时存在大量干扰信号，输出的姿态角波动比较大，而使用卡尔曼滤波滤除了大部分的干扰信号，输出的姿态角更加接近真实值，大大地改善了四旋翼无人机控制系统的控制性能。

2.2 控制算法分析

为了更好地验证经过滤波后的效果，系统分别采用串级PID 控制算法、滑模控制算法和LQR 控制算法等对无人机姿态进行控制，以选择最好的方法提高小型无人机的可靠性［5］。

2.2.1 串级PID 控制算法

串级PID 控制算法包含2 个控制环，外环用于位置控制，内环用于姿态控制。以位置环的输出作为姿态环的输入，为保证四旋翼无人机运行稳定性，对位置和姿态环输出进行限幅处理。串级PID 控制器原理如图6 所示。

图6 串级PID 控制器控制原理图

根据图6，输入位置信息为（x，y，z）=（5，5，5），经过系统反馈后，得到位置信号偏差，并在位置环中应用PID 算法进行处理以获得期望姿态角度θ、ϕ和ψ，然后将姿态角度传入姿态环中进行PID运算，从而生成PWM 输出并驱动四旋翼无人机飞行至指定位置，仿真结果如图7 所示。

图7 四旋翼无人机串级PID 控制仿真结果

由图7 知，采用串级PID 控制的四旋翼无人机表现出较快的响应速度，在约3 s 内即能达到稳定状态。此外，从滚转角和俯仰角的变化曲线也可以观察到初始阶段存在较大波动，而偏航角始终保持不变，则说明在姿态调节过程中无人机并未受电机力矩变化等因素影响而发生旋转等对稳定性产生负面影响。超调量达到了16%，证明采用串级PID 控制四旋翼无人机具备良好的控制效果。

2.2.2 滑模控制算法

滑模控制器仍采用双闭环结构，外环为位置环，其输出作为内环姿态环的期望输入，并由系统当前状态实时调节系统的运行速度。滑模控制算法分析仿真结果如图8 所示。由图8 知，在零初始状态下给定输入位置信号为（x，y，z）=（5，5，5），小型四旋翼的响应得到显著改善。系统反应迅速，X、Y、Z 轴在不到2 s 的时间内即可达到指定位置，并且超调量仅为4%。但滚转角和俯仰角出现波动，导致机身抖动，降低了四旋翼系统的稳定性和抗干扰性能。

图8 滑模控制算法仿真结果

2.2.3 LQR 控制算法

LQR（Linear Quadratic Regulator）是一种线性二次型调节器［6］，LQR 基于全状态反馈原理，通过加权求和不同状态得到最优控制量。作为一种最优控制策略，LQR 属于全状态反馈控制结构，在系统运行中以最小代价实现系统操作，LQR 控制算法的结构原理如图9 所示。

运行LQR 控制算法时，需要对系统状态准确估计，并对其进行离散化处理。在离散情况下，可以从递归和迭代的角度思考，并通过多次迭代达到设定的稳定值。本研究采用LQR 控制方法对四旋翼飞行器进行仿真，仿真结果如图10 所示。

由仿真结果知，当输入位置信息（x，y，z）=（5，5，5）时，四旋翼无人机展现出了迅速的响应能力，起飞初期在X 轴和Y 轴方向的飞行速度最高可达到3 m/s。在4 s 内，四旋翼无人机已完成调节并进入稳态状态，并且其在Z 轴方向上输出值已达到稳态值的90%。在第5 s 时，四旋翼无人机成功实现设定位置并完全进入稳态稳定飞行阶段。相较于之前2 种算法而言，无人机到达稳态的时间比较长。

3 结论

本文主要研究了基于STM32 单片机的小型四旋翼无人机的姿态控制问题，并对四旋翼无人机性能组态调整的快速响应进行了仿真测试。仿真结果表明，姿态信息经过卡尔曼滤波后，能够有效降低误差和外部干扰信号对数据的影响，显著提升飞行器所收集数据的准确性和可靠性。

所用到姿态控制的3 种算法各有优缺点，滑模控制算法具有快速响应特性和较小超调量，在系统出现扰动时可以迅速调整。LQR 控制算法对系统模型的精确性要求较高，稳定性良好，但动态性能相对较差，串级PID控制算法的反应速度不及滑模控制算法迅捷，并且存在一定超调量，具备更佳的稳态特性和抗干扰能力。