基于互补滤波算法和串级PID的四旋翼飞行器设计方法与流程

本发明属于无人机技术领域，具体涉及一种基于互补滤波算法和串级PID的四旋翼飞行器设计方法。

背景技术：

近年来，随着电子科学技术、先进材料技术、无线通信技术、自动化技术、数字信号处理技术等的进步，小型四旋翼低空无人飞行器得到了迅速发展，在军事和民用领域具有广阔的应用前景。和传统的直升机相比，它有着自身的优势：当前后两个旋翼逆时针旋转，而左右两侧的旋翼顺时针旋转时，旋翼自身产生的扭矩将相互抵消。则尾部旋翼和旋翼倾斜问题可以被忽略。但小型四旋翼飞行器的运动涉及到三个坐标轴上的转动，即俯仰角，横滚角和偏航角，同时又涉及到三个坐标轴上的移动，共包含六个自由度，而控制系统由四个带旋翼的电机组成，是一个欠驱动系统，因而飞行控制系统的设计有着较大的难度。

随着微控制器的快速发展，意法半导体基于高性能、低成本、低功耗的要求设计了ARM Cortex-M3内核，STM32系列处理器就是基于此内核设计。其中增强型系列时钟频率可达72Mhz，具有同类产品中最高性能，为进行复杂的姿态控制算法提供了条件。

在无人机研究中，微小型无人机的技术要求更高更复杂，因此发展相对缓慢，上世纪90年代末才研制出第一个飞行样机。由于其飞行速度相对较低、尺寸较小，因此小型无人机的空气动力学原理会更加复杂，有很多技术问题都无法用现有的空气动力学原理来解释。微小型无人机在低速飞行时，机翼荷载和机体惯性都比较小，很容易受到不稳定气流的干扰，因此就要求微小型无人机的控制系统要更加灵活，同时拥有更高的精度。

四轴飞行器的研究包含了多种交叉学工科，涉及到微机电系统(MEMS)技术、片上(SOC)技术、高效能源技术、微动力系统技术等。

技术实现要素：

本发明针对现有技术的不足，提供了一种基于互补滤波算法和串级PID的四旋翼飞行器设计方法。

本发，包括基于互补滤波算法的姿态融合算法和基于串级PID的控制算法；

飞行器的运动控制系统的核心在于姿态的解算和控制。首先需要建立一个由互相垂直的三个坐标轴组成的惯性参考系，而后由传感器来采集参考系各坐标轴方向上的角速度和加速度数据，进而计算出姿态信息。其中，加速度计测量的加速度数据可以通过与地面坐标系的转换矩阵，计算出姿态。陀螺仪测量的角速度信息可以通过积分获得角度信息。但两个传感器分别受限于自己的动态特性和静态特性上的不足，分别会产生高频噪声和长期的静态漂移。因此，对两者的数据通过一个互补滤波器来进行融合计算，可以得到长期有效的姿态信息。

本发明的有益效果：自动控制系统中，PID是最为典型的控制算法之一，得到广泛应用。传统的单环PID算法控制精度有限，可通过设计一个串级PID算法来增加系统稳定性。串级PID算法以角速度环为内环，角度环为内环，内环反馈直接由角速度传感器获取，避免了误差的引入，而外环的输出直接作为内环的输入。有效提高了控制精度和系统的稳定性。

附图说明

图1为机体坐标系。

图2为参考坐标系。

图3为数据融合算法流程图。

图4为单环增量式PID控制。

图5为串级PID控制器原理图。

图6为飞行器系统的软件流程图。

具体实施方式

以下结合附图对本发明作进一步说明。

在对四旋翼飞行器进行控制之前，首先要进行机体姿态的解算。而机体的方向，角度，速度等信息都需要通过一个坐标系来准确的描述。以机体的质心为原点，机头方向为xb轴正方向确立载体坐标系B:[xb,yb,zb]，将地面的全局坐标系来作为系统的参考坐标系L:[xl,yl,zl]，机体飞行范围相对较小，可忽略地球曲率，通过两坐标系之间的转换可得到机体的姿态信息。描述机体姿态的三个欧拉角，即俯仰角(Pitch)，滚转角(Roll)，偏航角(Yaw)分别用θ，φ，ψ表示，机体坐标系如图1所示。

机身的转动可以分解为依次绕zl轴转动ψ角，绕yl轴转动θ角，绕xl轴转动φ角的三次旋转，由此，可以得到各向量从地面坐标系的机体坐标系的映射关系矩阵。

第一步绕zl轴旋转，得到关于ψ角的映射矩阵：

第二步绕yl轴旋转，得到关于θ角的映射矩阵：

第三步绕xl轴旋转，得到关于φ角的映射矩阵：

由以上三个映射矩阵均为正交矩阵，相乘即可得出参考坐标系与机体坐标系的映射矩阵：

同时可以得出从机体坐标系到参考坐标系的反向映射矩阵：

四元常用于描述空间方位和运动，是一个矢量，由四个元素组成，分别为该矢量的方向和转动角度的函数，因此可以用四元数来描述机体坐标系绕一个矢量转过某一角度的单次旋转。

单位四元数q的表示如上式，q0，q1，q2，q3为四个元素，并且

式中cos(βx),cos(βy),cos(βz)表示矢量q的方向余弦，即与地面参考坐标系三个坐标轴夹角的余弦值，相当于(μx/μ)(μy/μ)(μz/μ)。μ为角矢量，大小为μ，相当于α，μx，μy，μz是它的三个分量。机体坐标系以μ为旋转轴，转动角度μ后可与参考坐标系重合。

四元数和欧拉角之间的转换公式分别是：

映射矩阵Rt的四元数表述为：

四旋翼飞行器的系统误差主要有两个方面，一是静态误差，该部分包含传感器的固有误差和放置引起的偏置误差；二是动态误差，该部分产生的主要原因是飞行过程中会产生一定的震动，由于传感器的动态特性不足而引入误差。

MEMS传感器的固有误差是指当传感器处于静态条件下时，输出存在一定范围内的随机变化。MEMS传感器的数据手册中一般会给出一个噪声密度参数来描述固有误差，一般可通过该参数来对固有误差进行修正。

运动控制系统通常需要建立一个由三个相互垂直的坐标轴组成的惯性参考系，该惯性参考系给MEMS传感器提供了方向参考。理想情况下，传感器每个轴的方向会与参考系的坐标轴完全一致，围绕一个坐标轴的旋转只会引起传感器中对应轴向的输出而不会引起其他变化。但是由于工艺等条件限制，传感器坐标轴与参考系无法完全对准，围绕一个坐标轴的旋转将使另外的坐标轴也产生输出。因此陀螺仪的对准误差包含连个方面，即与参考系中另外两轴的位置偏差。以图2的参考坐标系为例，θXZ表示陀螺仪的X轴相对于坐标系Z轴的角度偏差，当绕Z轴旋转时，该角度偏差引起的X轴上的误差可以由下式定义：

ω_GX＝ω_ZR×sin(θ_XZ)

MEMS运动传感器的对准误差一般包括彼此相关的两种类型，包括轴与封装之间的对准误差和轴与轴之间的对准误差。轴与封装间的对准误差是由封装的机械特性决定的，若系统不能在初始过程中对误差进行校正，这一误差将会引入到系统的总体误差当中。另外由于焊接等机械加工工艺的限制，惯性测量单元在放置于系统上时，同样会造成轴与系统之间的对准误差。对系统封装的机械特性进行一个容差的评估，会对这些对准误差的修正提供参考。

而当MEMS惯性测量单元应用于飞行器控制时，其在动态情况下的误差将成为系统误差的主要组成部分。陀螺仪具有可靠的动态响应，在短时间内可以提供准确的测量数据，但由于上述偏置误差的存在，随着时间的积累，其测量值随积分产生的误差会不断累加，对系统产生极大影响，因此，对于系统姿态的测量还应引入三轴加速度计来对三轴陀螺仪的误差进行修正，加速度计所测得的数据具有长期稳定的特性，无积累误差，但由于其动态特性不佳，在飞行器的机身震动影响下，测得的数据在短期内可能会存在一定的波动。因此获取准确的姿态数据需要对两者的测量值进行数据融合。

对于陀螺仪和加速度计的数据融合，常见的方法有硬件和软件两种，其中软件进行数据融合可以通过卡尔曼滤波或者互补滤波来实现，考虑到互补滤波具有计算量小，处理速度快的特点，本设计中对数据融合采用互补滤波的算法来实现。

三轴加速度计是通过测量重力加速度在机体坐标系三个坐标轴的投影大小，然后通过与地面参考坐标系的转换矩阵计算得到的。但加速度计测得的数据容易受到机身震动带来的影响而产生误差，但该误差并不会随时间积累，因此具有较好的静态特性。因此对加速度计的数据处理常采用低通滤波来消去短时间内的噪声信号，得到长时间内的可靠信号。而陀螺仪的动态特性较好，短期内可通过对角速度的积分来测得角度，但在较长周期内陀螺仪误差会积累产生漂移，当机身姿态变化时，受加速度的影响，也会产生一定的漂移。因此对陀螺仪的数据采用高通滤波处理，滤除长时间内积分产生的漂移。数据融合算法流程图如图3所示。

用互补滤波来融合角度的公式为

angle＝A×(angle+gyro×dt)+B×(acc)

式中A、B为滤波器的两个参数，A具有低通滤波特性，B具有高通滤波特性，并且A+B＝1。angle代表角度，gyro代表陀螺仪输出的角速度，与积分周期dt相乘得到角度，acc表示加速度测得的角度。由上式可计算互补滤波器的增益K为：

自动反馈控制系统中，PID控制器应用最为广泛。PID控制算法无需复杂的数学建模，通过对系统偏差的比例(P)，积分(I)，微分(D)来进行控制。早期的四旋翼飞行器常采用单环增量式PID控制，仅采用角度信息作为反馈，原理如图4所示。

单环PID控制过程中，首先用期望角度减去当前角度得到偏差。比例项直接由偏差与比例系数相乘得到。由于角度的微分为角速度，可直接由陀螺仪的测量得到，与微分系数相乘即可得到微分项。总控制量由比例项和微分项相加得到。改控制算法简单有效，但只考虑了角度信息为反馈，控制精度不佳。

为了提高控制的精度，增加系统的稳定性，在单环PID控制算法中加入角速度环，形成串级PID控制器，原理如图5所示。

飞行器串级PID算法以角速度控制器作为内环，以角度控制器为外环计算出期望角速度，作为内环的输入信号，陀螺仪采集到的角速度直接作为反馈信息，有效避免了外界干扰造成的影响。由两个控制器来控制飞行器，有效增强了稳定性，提高了角度变化灵敏度，使得飞行器具有更强的适应能力。

整个飞行器系统的软件流程图如图6所示。

飞行器的主控芯片采用STM32F103C8T6，无实时操作系统，因此程序主要依靠定时器来不断执行循环操作，查询各个中断条件，通过嵌套中断来实现整体功能。

首先是对系统各个模块进行初始化处理操作，检测系统各个模块是否正常。之后由于传感器可能存在偏置产生的误差，需要进行校正。结合之前的分析，偏置误差产生的原因可能来自传感器自身工艺限制带来的轴间对准误差，也可能因焊接放置时轴与机身坐标系没有严格对准产生误差，若不进行校正，则偏置误差会引入之后的计算对系统控制造成影响。校正偏置需要约两秒种的时间，此时系统不断读取各个轴向上的数据并通过滑动平均滤波得到偏差值，存储各个轴向的偏差值并在之后的数据读取中减去偏差值来校正偏置误差。

之后进入100hz的主循环过程，在该循环中，系统完成的任务有传感器数据读取，滤波，融合，并进行PID控制来维持机身稳定。这里对传感器进行的滤波主要是对加速度计进行一次滑动平均滤波，以减少因传感器的动态特性不足带来的噪声信号。滑动平均滤波每读取一次数据便可以更新一次数据，具有实时性，且计算量和空间占用量较小。而后经过上述的互补滤波算法来完成层对滤波后的加速度计数据和陀螺仪数据的融合处理。计算当前角度与目标角度的偏差，通过双环PID算法来控制机身达到目标姿态。

对于遥控器发来的控制信息捕获通过一个50hz的循环来完成，在该循环中获取遥控器控制信息后，讲数据发送给处理器来解析目标姿态信息，并结合传感器数据融合得到的当前角度，通过双环PID算法计算控制量，达到目标姿态。

系统通过以上两个循环操作，便可实现对飞行器的稳定控制功能。