一种新型无人驾驶飞行器姿态控制系统

2020-11-10余哲明

济南大学学报（自然科学版） 2020年6期

柳赟，余哲明，宗伟

(1. 华北电力大学电气与电子工程学院，北京102206； 2. 杭州海康威视数字技术股份有限公司，浙江杭州310051)

无人驾驶飞行器(简称无人机)是一种靠自身程序控制或远距离遥控控制的飞行器。从技术角度出发，可以将无人机分为无人固定翼飞行器、无人多旋翼飞行器等。相比于其他常见的载人飞行器，无人机体积更小，飞行更智能，成本更低[1]，同时可以装载各种传感器、机载云端、电子设备等，在执行各种高空遥感、测绘、航拍、电力巡检、侦察等任务方面具有明显优势。

四轴飞行器是一种常见的无人多旋翼飞行器，其机架上安装有4个电机，通过控制4个电机转动产生推力，进而带动整个飞行器飞行。由于四轴飞行器的输出为4个电机，在改变飞行器姿态时，需要同时改变4个电机的输出大小[2-3]，因此存在功耗大的问题，限制了无人机的续航。

本文中设计了一种新型结构的飞行器飞行控制系统，利用四元数对飞行器姿态进行估计，解决欧拉方程求解速度慢和万向节锁死的问题；采用四阶龙格-库塔算法近似求解飞行器姿态，使四轴飞行器具有更长的续航时间、更灵活的姿态变换和更小的功耗等优点。

1 飞行器结构和工作原理

目前常见的多轴飞行器的旋翼均是多轴水平安装的，在水平飞行时，只能通过调节多个旋翼的转速差，使飞行器机体以和地面呈现夹角的姿态飞行，从而获得水平方向的动力，进而实现飞行器的前向、后向、左向、右向或者自转等姿态。在飞行器飞行过程中，如果姿态过大，容易导致飞行控制系统中的三轴陀螺仪过载，使飞行器出现失控、坠毁等现象，因此需要调整旋翼布置方式，设计一种新的飞行器。

本文中设计的飞行器的动力来源为顶端机架上的上、下2个电机。新型飞行器结构如图1所示，其中电机M1、M2均为顺时针旋转，且电机M1配正向螺旋桨，电机M2配反向螺旋桨，这种配置保证了2个电机在旋转过程中对机体的作用力均为升力。同时，旋转方向不同的2个电机使得机体在飞行时可以通过调整电机的差速来防止自身发生自旋。通过改变电机M1、M2的转速以及舵机M3、M4的动作，飞行器可以非常灵活的实现悬停、俯仰、横滚等基本飞行动作。

图1 新型飞行器结构

以图1为例，当舵机M3、M4保持油门中位，电机M1、M2转速保持同步增大或减小时，飞行器可以实现在垂直方向上升或下降。飞行器航向角的变化由电机M1、M2的转速差控制，即正转速差时，飞行器顺时针旋转；逆转速差时，飞行器逆时针旋转。飞行器俯仰角可以通过改变舵机M3输出来实现。同理，飞行器的横滚可以通过改变舵机M4的输出来实现。飞行器姿态与电机转速、舵机输出的关系如表1所示。

表1 飞行器姿态与电机转速、舵机输出的关系

2 飞行器姿态

要实现飞行器程序控制，最重要的是飞行器姿态的实时计算。本文中利用飞行控制板上的三轴陀螺仪、三轴加速度计传感器实时读取飞行器的横滚角、俯仰角、偏航角的数据，通过姿态融合来求解飞行器的姿态[4]。

2.1 欧拉角微分方程求解飞行器姿态

飞行器机体与坐标系x、y、z轴的旋转角度称为欧拉角。由于需要求解飞行器机体实时的欧拉角ψ、φ、γ，因此需要建立欧拉角速度方程组[5]。地球坐标系与机体坐标系的关系如图2所示。其中，ψ为进动角，φ为章动角，γ为自转角，x0y0z0、x1y1z1、x′y′z′分别为地球坐标系、机体坐标系、机体当前转动角度。

x0y0z0—地球坐标系； x1y1z1—机体坐标系；x′y′z′—机体当前转动角度； ψ—进动角； φ—章动角； γ—自转角； ω—机体坐标系相对于地球坐标x0y0z0的旋转角速度，在坐标系x1y1z1中坐标轴上的分量分别为ωx1、ωy1、ωz1。图2 地球坐标系与机体坐标系的关系

设ω为机体坐标系相对于地球坐标x0y0z0的旋转角速度，经过向量分解，由图2可以看出，ω在坐标系x1y1z1中坐标轴上的分量分别为ωx1、ωy1、ωz1，因此

(1)

从而可得如下的欧拉角速度方程组：

(2)

式(2)是一个非线性变系数微分方程组，ψ、φ、γ分别为欧拉方程中的3个角。当知道某时刻的角速度ωx1、ωy1、ωz1，并在给定的起始条件下，通过此方程组就可以求解出飞行器的姿态角ψ、φ、γ。

欧拉角求解飞行器姿态虽然很直观，但是存在如下2个问题： 1)利用数值积分法来求解非线性变系数微分方程的计算量大，在单片机上解算速度慢，难以满足实时控制要求。2)在机体姿态变化过大时，方程是奇异的，会出现万向节锁死情况。

综上，用欧拉法来计算飞行器的姿态存在缺陷，因此本文中采用四元数法来计算飞行器的姿态。

2.2 四元数法求解飞行器姿态

四元数法能够描述2个坐标系或2个矢量之间的旋转变化，可以解决飞行器机体姿态变化大时方程退化、计算量大等问题，已逐渐成为描述刚体角运动的主流方法。

四元数是由3个单位正交矢量i、j、k和一个实数单位组成的超复数[6]，通常可以写为

λ=λ0+λ1i+λ2j+λ3k

，

(3)

式中λ0、λ1、λ2、λ3均为实数。

与欧拉法同理，为了表征飞行器三轴角度与四元数导数之间的函数关系，以下用四元数方程表示。

规范四元数q=(q0q1q2q3)T，则四元数方程组的矩阵形式为[6]

(4)

龙格-库塔法是在实际设计中经常用来求解四元数方程组的。

引入符号

(5)

(6)

(7)

则式(4)可以写为

(8)

可以看出，式(8)为一阶四维微分方程，因此满足正交关系式

(9)

(10)

其中

(11)

式中：tk为时间节点；Δtk为初始设定值。

用龙格-库塔法不仅减小了计算量，而且方程是非奇异的，满足了单片机飞行器姿态计算的实时性要求[7]。

3 系统整体架构

根据1节中对新型飞行器的结构和工作原理的分析，结合市场上现有飞行控制的结构，本文中设计的新型飞行器控制系统整体结构如图3所示。控制系统整体采用双回路闭环系统，内回路基于龙格-库塔法进行姿态控制，外回路进行轨迹控制。

根据控制系统结构，飞行器姿态控制系统结构如图4所示。

图3 新型飞行器控制系统整体结构

GPS—全球定位系统； MOSFET—金属-氧化物半导体场效应晶体管; RC—飞行姿态控制器。图4 飞行器姿态控制系统结构

飞行器的姿态控制系统主要由STM32F407VGT6单片机、MCU6050芯片(集成陀螺仪、加速度计)、无刷直流电机、高压舵机、电源管理模块以及通信模块组成。

1)STM32F407VGT6单片机。飞行器的姿态计算需要很大的计算量，其中STM32F407VGT6单片机主频最高可以达到168 MHz，与常见的STM32F103系列单片机相比，F407拥有更强的计算能力，保证了各个传感器计算达到实时，使飞行器的控制更加稳定。单片机的主要任务是通过集成电路(I2C)总线读取MCU6050芯片的三轴的加速度计和三轴的陀螺仪的数据，用上述提到的四元数算法将这些数据解算成飞行器实时的飞行姿态，并根据来自遥控的控制信号和飞行器的控制原理来对飞行器的上、下2个电机和左、右2个舵机进行实时控制。

2)MPU6050芯片。MPU6050芯片是一个六轴传感器，将加速度计芯片和陀螺仪芯片集成到一起[8-9]，使主控芯片能在一次读取周期内得到6个轴所需的所有数据，不仅极大地保证了加速度数据与陀螺仪数据的同时性，也节省了数据读取时间，使控制系统能以更短周期刷新控制信号。三轴陀螺仪能够测量飞行器飞行时横滚、偏航、俯仰3个方向的角速度变化量，对这3个方向的角速度进行积分，可以得到实际的角度变化。

陀螺仪对振动不敏感，输出数据具有一定的平滑性；但是，经过长期使用时数据产生漂移，因此单纯使用陀螺仪时，数据会产生累积偏差[10]，需要用加速度计的数据来进行修正。加速度计读取的是三轴角加速度的变化量，虽然加速度对振动很敏感，但是其长期数据是可信的，将两者的数据进行互补，就得到实际的俯仰、横滚、偏航3个方向的角度变化。MPU6050芯片模块原理如图5所示。

3)电源管理模块。该模块用于为整个动力设备提供动力以及实时测量剩余电量，保证飞行器飞行安全。TP4059为充电芯片，通过通用串行总线(USB)接口给锂电池充电，XC6204芯片为稳压芯片，可以输出稳定的电压，保证整个系统的安全。锂电池组经过电源模块降压后可以分别给微控制单元(MCU)、接收机、电机、高压舵机供电。此外，该模块包含的电量管理模块在检测到电池电量不足时，会自动给MCU发送信号，蜂鸣器会立刻报警，同时以安全的速度降落，最大限度地保证飞行器的安全[11]。电源管理模块电路如图6所示。

图5 MPU6050芯片模块原理

图6 电源管理模块电路

4 系统软件设计

作为飞行器飞行控制，必须有以下严谨的要求：1)不采用软件延时，保证足够高的系统效率； 2)不随意添加其他程序，保证可靠的执行顺序结构； 3)重要任务需统计每2次相邻调用任务时间间隔。满足上述要求的最好方法就是仅开启一个优先级较高的定时器，通过时间片轮转调度方法来调度不同任务，如图7所示。

在设计时，根据任务的重要程度，不同任务被设计在不同执行周期的任务列中，共设计1、2、5、10、20、40、50、100 ms共8个周期任务。每个任务的执行时间均少于1 ms，以保证每次调用都能执行完成。高级定时器定时周期为1 ms，每1 ms标记相应标志位，当达到某个任务的执行周期时，单片机自动执行该周期内预先设定的所有任务。该设计合理分配单片机的资源，既保证飞行控制系统所有的任务都能得到有效执行，又使得飞行器具有较高的实时响应性，控制效果更好[12]。

图7 时间片轮转调度示意图

根据不同任务优先级设置不同的任务周期。其中，传感器数据读取、龙格-库塔姿态估算算法以及比例积分微分(PID)算法计算输出设为最高优先级，任务周期为1 ms，遥控接收设为第二阶梯，任务周期为5 ms，电池电压检测及警告设为第三阶梯，任务周期为20 ms。

5 实验与结果

利用基于四阶龙格-库塔法对飞行器姿态进行实时计算，电机由PID算法控制。在对PID参数默认设定之后，在室内对本文中设计的飞行器进行飞行测试。

飞行器初次飞行的测试条件如下： 1)锂电池组11.1 V、5 200 mA·h； 2)PID控制参数分别为：比例系数P=1.5，积分系数I=1，微分系数D=0.5； 3)测试环境为室内，无风。

飞行器在遥控和程序的操控下，顺利完成悬停、俯仰、横滚等基本飞行动作。测试结果表明，本文中设计的新型飞行器和其飞行控制系统能在人为遥控下完成一系列基本的飞行动作，在对控制器PID参数进行微调之后，可以实现更加平稳的飞行。同时，利用串口助手将飞行器俯仰角响应进行回传，在不同设定值时测试系统性能，响应曲线如图8所示。

图8 飞行器俯仰角响应曲线

由图可以看出，当系统设定值改变后，PID输出迅速改变，且超调量非常小。此时，飞行器实时状态值以较顺滑的状态趋向于设定值，当飞行器实时状态和设定值基本吻合时，PID输出不再改变，系统没有振荡。再次修改系统设定值，将系统设定值调小时，飞行器实时状态值在较短时间内改变到设定值，PID输出稳定，飞行器实时状态偏差小，整套系统控制非常平稳。飞行器控制系统包括飞行器模型、姿态控制、轨迹控制3个部分，在MATLAB平台中搭建飞行器控制系统仿真模型，如图9所示。

姿态算法采用四阶龙格-库塔法，设置姿态角的期望分别为ψ=0°、φ=7°、γ=0° ，对控制参数进行调整，仿真结果如图10所示。由图可以看出，无人机系统可以快速且无超调响应期望，满足性能要求。

6 结语

本文中针对现有飞行器姿态估计算法存在当飞行器姿态变化过大时，会导致万向节锁死的缺点，基于四阶龙格-库塔法的飞行器姿态估算算法，提出一种飞行器姿态控制系统，利用STM32F407VGT6单片机作为飞行器主控，利用时间切片轮法对不同要求的子任务进行运算，最终的飞行测试以及MATLAB软件仿真结果表明，此控制系统可以有效控制飞行器的飞行姿态。