本文研究内容和方法
本文研究基于MEMS传感器的姿态参考系统，通过对姿态测量传感器数据的分析，设计出了有效去噪的滤波方法；通过大量的查找资料对姿态解算算法和数据融合算法有了更深的理解，最后应用于设计的飞行控制器上实现了姿态角的测量。最后通过大量的实验验证了它们的准确性，实验数据和曲线验证了该姿态参考系统能够稳定的工作，具有很好地工作性能。
本文一共分为五章，主要内容安排如下：
第一章绪论部分主要介绍了该项目的研究背景及意义、四旋翼飞行器在国内外的研究现状和发展趋势。
第二章主要介绍了四旋翼飞行器的飞行原理和系统结构框架。
第三章详细介绍了四旋翼无人机控制系统的硬件设计的工作。介绍了MEMS传感器的原理、特性和型号的选择和硬件电路图。飞行器控制芯片选择STM32，外围电路包括有姿态测量系统、电源模块、无线通讯、串口通讯、电机驱动、遥控器控制电路、GPS模块。
第四章说明了姿态参考系统的核心算法----捷联惯性导航算法的研究和实现过程。
第五章针对软件实现部分进行了介绍，给出了编程的软件流程图和串级PID控制和定高控制方法。
2 四旋翼飞行器工作原理2.1 四旋翼飞行器的飞行原理
四旋翼飞行器有两种模式，也就是X字模式（如图2.1所示）和十字模式（如图2.2所示）。其实这两种模式差别不大，到X模式使用广泛，因此我们采用X字模式。四旋翼飞行器的四个电机对称分布在各个轴上，并且同一条轴线上电机的旋转方向要保证相同，相邻的电机旋转方向相反[。如果电机1、 3按照逆时针方向旋转的话，电机2、4就要按照顺时针方向旋转，这样做为了克服反扭矩的影响。我们要通过控制4个电机的转速来完成飞行器俯仰、横滚、偏航等动作。
图 2.1 X型四旋翼飞行器模型 图 2.2 十字型四旋翼飞行器模型
2.2 四旋翼飞行器系统结构
四旋翼无人机采用模块化设计，如图2.3所示。分别由控制模块、姿态测量系统、电源供电系统、无线通信模块、GPS卫星定位系统、遥控器控制模块、电机驱动模块、串口通信模块、地面站系统。
四旋翼飞行器控制器的核心任务是姿态的测量，它的作用是为飞行器控制系统提供实时、精确的飞行状态测量数据。常见的四旋翼飞行器人们大多是采用基于MEMS传感器来测量飞行器姿态数据[[ii]。但是这些初始的传感器数据并不能直接应用于姿态解算，需要对传感器数据进行滤波处理，并且需要对陀螺仪漂移问题进行实时的数据补偿，这样做能够有效提高飞行器姿态测量精度，确保控制系统的姿态角的准确性和稳定性。
图 2.3 四旋翼飞行器系统结构框架
四旋翼飞行器的主控板选择的是意法半导体公司生产的STM32f103zet芯片，STM32系列的单片机是基于Cortex-M3内核的处理器，功耗低，处理速度非常快，最高工作频率可达72MHz，7通道DMA控制器，支持定时器、ADC、SPI、IIC、USART等外设，多达112个I/O口，8个Timer定时器， 5个串行USART接口，3个SPI接口，2个IIC接口[[iii]。
电源模块采用11.1V锂电池外部供电，连接电子调速器为控制器提供5V电压。控制器上还有3.3V稳压芯片，为控制芯片供电。
遥控器控制模块，控制器对遥控器数据进行捕获处理该部分我们通过对STM32定时器进行输入捕获配置，捕获接收机发出的PWM信号，把该信号转化成控制量在经过PID控制把输出量给四个电机，进而控制飞行器的动作。
GPS卫星定位导航系统，配合上位机在上位机上输入一些GPS坐标点，控制系统就会自动生成航线，并且能够从GPS系统中读取定位数据[[iv]，并且与存储的定位坐标做实时的对比，然后修正航线，将定位坐标显示在上位机上，处理并显示当前位置。
3 四旋翼飞行器硬件系统设计3.1 微惯性组合系统传感器组成3.1.1 MEMS陀螺仪传感器
陀螺仪是一种能用来维持方向与角速度（获取角速度）的装置，设计原理是角动量守恒。简单的说就是一个高速旋转的物体的旋转轴所指的方向在不受外力影响时不会改变。这种用来保持方向而制造出来的装置就叫陀螺仪[[v]，如图3.1所示。陀螺仪多用于导航定位系统中，姿态控制系统中多采用三轴陀螺仪，如图3.2所示。
图3.1 陀螺仪 图 3.2 MEMS三轴陀螺仪
3.1.2 MEMS加速度计传感器
能将物体加速度的信息转换为电信号的传感器称之为加速度传感器。在姿态控制系统中，加速度传感器用来测量与重力方向的夹角。当应用到实际中时我们就可以理解加速度传感器输出的信号是当地坐标系下加速度在导航坐标系下投影。加速度计能够在没有加速度存在的条件下可以感应重力产生的加速度，然而在有加速度存在时，根本无法测量出姿态角，需要陀螺仪传感器的数据相结合，才能够实现动态条件下的姿态测量[[vi]。
3.1.3 三轴数字罗盘传感器
数字电子罗盘也叫指南针，顾名思义指南针是用来指示方向的。传统罗盘通过磁针来感应地磁场方向，电子罗盘通过磁阻传感器测量地磁方向信息，再将所测信息转换为信号输出。数字电子罗盘的优势在于它克服了只能够在水平面使用的缺点，这种数字电子罗盘内部有倾斜补偿装置，这个装置一般是由加速度传感器来完成，如果在完全动态的情况下，也需要陀螺仪检测姿态角，通过这个角度和磁场方向信息可以补偿得到准确的角度信息，而姿态测量系统中的电子罗盘实际上就是三轴数字电子罗盘。
3.2 姿态测量系统传感器选型
目前市场上出现的一款InvenSense公司的MPU6050芯片内部集成了三轴加速度计和三轴陀螺仪，这样做不仅消除了焊接电路时易造成加速度计和陀螺仪之间的对准误差的问题，而且因为芯片内部结构上有数字可编程低通滤波器。所以在飞行器经受较大震动的时候，可以用软件设置适当频率的低通滤波器，滤掉高频震动，这种方法很有效的减少了四旋翼机身震动对姿态测量的影响。因此MPU6050被广泛应用于姿态控制系统之中，其特征如下：
（1）三轴角速度传感器具有±250、±500、±1000 与±2000（°/s）测量范围[[vii]；三轴加速度量程控制范围有±2g、±4g、±8g、和±16g。
（2）具备较低功耗：芯片供电电压VDD为2.5V±5%、3.0V±5%、3.3V±5%[[viii]；陀螺仪工作电流5mA，待机电流为5uA；加速度计工作电流为500uA，在10Hz低功耗模式下仅需40uA的电流[[ix]。
（3）陀螺仪和加速度计都具备16位ADC 同步采样功能。
（4）IIC接口传输频率可高达400KHz，内建频率发生器在所有温度范围只有1%频率变化。
综合MPU6050特性，我们采用如图3.3所示的电路读取三轴加速度和三轴陀螺仪数据。
图 3.3 MPU6050电路图
HMC5883传感器是三轴数字罗盘，它可以用来测量四轴飞行器所处位置的三轴磁场信息，该传感器内置了三轴磁阻模块和放大采样电路，直接输出数字信号，用来测量航向角并进行姿态解算，HMC5883电路图如图3.4所示。HMC5883的特点如下：
1. IIC 数字量输出总线接口，设计使用简单，尺寸非常小。
2.有较高的测量精度，内置12位 A/D转换。
图 3.4 HMC5883电路图
4. 拥有自动校准功能，简化了应用的步骤。
5. 内置有自测试电路，量产测试非常方便，不需要增加额外的高昂测试设备[[x]。
6. 功耗较低，供电电压只需要1.8V，睡眠模式功耗-2.5uA，测量模式功耗-0.6mA。
3.3 电源系统设计
为了满足飞行控制系统的需要，电源系统为飞行控制器和功能模块提供了12V、5V和3.3V电压，电路设计如图3.5所示。选择12V的锂电池作为供电电源，通过稳压芯片将12V电压降为5V，为GPS模块、超声波传感器、接收机等提供供电电源；然后通过AMS1117_3.3稳压芯片再把5V电压降为3.3V，为飞行控制主板、姿态测量传感器和无线通信模块提供电能。
图 3.5 电源系统电路图
3.4 其它硬件模块3.4.1 无线通信模块
该模块为上位机和控制器建立了联系。通过两块NRF24L01进行数据的传输，控制器将姿态和高度等信息传输至地面站，地面站将位置信息和导航信息告诉控制器，从而达到可控的要求。NRF24L01的工作频段为2.4 ~ 2.5GHz，而且具备自动重发功能，有6个数据传输通道，最大传输速率高达2Mbits[[xi]。STM32主控板可以通过SPI接口对NRF24L01的寄存器进行配置，无线通信模块的电路设计图，如图3.6所示。
图 3.6 NRF24L01电路图
3.4.2 电机和电机驱动模块
我们常用的直流电机按结构及工作原理可以划分为无刷电机和有刷电机。无刷电机是航模电机的主流选择，因为它力量大而且耐用。小型的四旋翼飞行器一般选用空心杯电机，属于有刷电机一类。由于我们设计的是大型四旋翼飞行器，故采用的是无刷电机。
无刷电机根据厂商的不同，种类也是非常的多，市面上的主要有朗宇、新西达、银燕等品牌。在这些无刷电机中口碑最好的就是朗宇电机，用的人非常的多，因此采用朗宇无刷电机。
四旋翼飞行器所用的无刷电机主要有电机尺寸和电机KV值两个参数。电机尺寸指的是电机转子的直径和高度，电机KV值是指外加1V电压时对应的每分钟空载转速。根据参数设置最后选择采用朗宇A2212、KV1400无刷电机。电机实物图，如图3.9所示。
无刷电机的驱动就是俗称的电子调速器也称电调，如图3.10所示。黑色和红色的是11.1V电源线，红色接电源正极，黑色接电源负极。右边三根线和电机的三根线相连，如果转向反了，只需将其中任意两根线互换就可以。白红黑三根线是和电调相连的，白色为信号线，红色为控制板提供5V供电电源，黑色的线接控制器的GND。
3.4.3 机架和螺旋桨的选型
对于机架的选择，差的机架会使姿态传感器读取到的数据噪声较大，加大四旋翼飞行器在飞行过程中的不稳定性，因此选用结构稳定，质量较轻的碳纤机架，这样很大程度上也提高了飞行器的载重。机架实物图如图3.11所示。
螺旋桨是由电机转动带动为四旋翼飞行器提供升力的。螺旋桨分为正反桨，顺时针方向转的电机需要配正桨，逆时针转的电机需要配反桨。桨的选型需要结合所选的电机来配备，从桨的型号我们可以读出桨的直径和角度，例如1047中的前两位10表示桨的直径单位是英寸，后两位代表桨的角度。的螺旋桨，同样转速情况下产生的升力就会越大，也就需要更大的电机驱动，这就需要根据电机的大小去选择桨的型号。在设计时选用1047的桨，桨的实物图如图3.12所示。
图 3.11 四旋翼飞行器机架 图 3.12 1047正反桨
3.4.4 遥控控制模块
该遥控器控制模块是由一个7通道的遥控器和配套的接收机组成的，接收机上的油门、副翼、升降舵、方向舵通道分别与控制器上的接口相连。控制器能通过定时器的捕获功能，捕获到遥控器通过接收机传出的控制信号，遥控器就是这样控制四旋翼飞行器的飞行动作的。遥控器和接收机的实物图如图3.13所示。
图 3.13遥控器和接收机实物图
4 四旋翼飞行器姿态参考系统设计4.1 姿态参考系统原理
姿态参考系统是利用惯性导航器件来测量载体姿态角的一种惯性导航系统。因为MEMS传感器的数据带有噪声，因此姿态参考系统需要对MEMS传感器的测量信息进行滤波处理。然后对滤波信号进行姿态解算。由于姿态传感器各自特点的不同，所以需要对每个传感器信号进行滤波然后进行数据融合，以此来提高测量的精度和系统的动态特性。姿态参考系统的原理图，如图4.1所示。
图 4.1 姿态参考系统原理图
从MEMS传感器采集到的数据传输到控制器处理的过程中会受到很多因素的干扰，造成传输误差，为了减小误差，提高检测的精度就需要用滤波器进行数据处理。滤波器可以通过硬件实现，也可以通过软件实现，具有较大的灵活性[[xii]。
从加速度传感器提取到的信号经过姿态解算后的姿态角信号在和陀螺仪数据进行融合的过程中以低频信号为主的。根据这一特性，加速度传感器的输出信号可以通过低通滤波器或者均值滤波器提高信噪比。
陀螺仪信号具有高动态的特点，它的信号噪声主要为高斯白噪音，我们可以通过卡尔曼滤波器来滤除这种高斯白噪声。因为需要融合加速度传感器获得的姿态角信号，所以选择均值滤波来去除噪声。
电子罗盘传感器测量的是地磁强度，我们都知道物体在运动时地磁强度变化慢，它在姿态测量系统中属于低频信号，因此采用均值滤波来滤除噪声信号。
欧拉角和四元数方法都是针对陀螺仪姿态测量进行处理的数学方法，可以有效地解算处姿态角。把解算出来的姿态角进行数据融合。考虑到高动态下陀螺仪测得的数据解算出的姿态角比较准确因此在高动态条件下选用陀螺仪信号，低动态下加速度信号解算出的姿态角更稳定因此在低动态条件下选择加速度信号。
4.2 传感器信号处理4.2.1 加速度传感器信号处理
加速度传感器的输出信号主要表示直线加速度大小和与重力方向的夹角[[xiii]。 常用的数字滤波器有很多种，例如：限值滤波、递推平均滤波法（又称滑动平均滤波法）、算术平均滤波法、中位值滤波法和中位值平均滤波法等[[xiv]。
在实际的姿态解算中一般采用低通滤波器或者均值滤波器去除高频噪声干扰信号。均值滤波因其具备低通滤波的特性并且计算简单，因此选用均值滤波来处理加速度信号。
4.2.2 陀螺仪信号处理
陀螺仪信号滤波要选择卡尔曼滤波算法。对于卡尔曼滤波器，首先要清楚系统状态的描述方法，系统状态的更新方法和系统测量值的更新方法，从卡尔曼滤波算法的数学公式，我们可以归纳出它的核心思想：首先根据系统上一次得来的最优值计算出当前的估计值和协方差，再根据协方差大小计算出卡尔曼增益的大小，最后根据当前估计值和测量值计算出当前最优值和协方差。下面分步建立卡尔曼滤波方程式：
预估计最优值方程：
X(k|k-1) = A(k,k-1) * X(k-1|k-1)+ B(k) * U(k) (4-1)
上式中，X(k|k-1)表示的是根据k-1时刻的最优值计算出的k时刻估计值；X(k|k-1)表示k-1时刻的最优值；A(k,k-1)是状态转移矩阵；B(k)是输入控制加权矩阵；U(k)表示k时刻的输入控制信号，这些参数需要根据具体的应用场合进行设计。
预估计最优值协方差：
P(k|k-1) = A(k,k-1) * P(k-1|k-1)* A(k,k-1) + Q(k) (4-2)
其中，P(k|k-1)表示预估计最优值X(k|k-1)对应的协方差；P(k-1|k-1)表示X(k-1|k-1)对应的协方差，表示了对预测值的信任度；Q(k)表示k时刻系统过程的协方差，即对上一次测量估计值的信任程度，Q矩阵值越大表示信任度越低，需要根据具体应用进行设计。P和Q矩阵的区别在于Q矩阵是根据模型直接得到的，P矩阵是算法过程中通过计算获得的。
计算卡尔曼增益矩阵：
K(k)=P(k|k-1) * H(k) / (H(k) * P(k|k-1) * H(k) + R(k)) (4-3)
其中，K(k)表示卡尔曼增益；R(k)表示k时刻观测过程的covariance，即对测量的信任程度；H(k)表示观测矩阵。
更新估计：
X(k|k)=X(k|k-1)+ K(k) * (Z(k) – H(k) *X(k|k-1)) (4-4)
P(k|k)=(1- K(k)* H(k)) * P(k|k-1) (4-5)
其中，Z(k)表示k时刻的观测值，I为单位矩阵。
4.2.3 电子罗盘信号处理
电子罗盘信号非常容易受到高频干扰，在系统应用中，应当过滤系统的高频噪声，最好采用去极值滤波算法[[xv]。去极值滤波就是将连续测量的n个采样值，按照数据的大小顺序进行排序，去掉最大值和最小值后对剩下的n-2个数值计算求取平均值，这种方法是最常用的滤波算法[[xvi]。
4.3 坐标系
坐标系是描述物体在空间的相对位置和运动规律的，而导航技术就是为了确定载体的空间位置。只有选定参考坐标系，才能对系统的运动进行描述。不同的坐标系下载体的描述规律和运动形式也是不同的，选择合适的坐标系是非常重要的[[xvii]。
目前比较常用的坐标系有：
(1)地理坐标系(g系)
坐标系和地球固连，其原点位于地球球心，通常选取东北天坐标系，即坐标Xg轴指向水平东方，Yg轴指向水平北方，Zg轴垂直于当地水平面，沿当地垂线向上[[xviii]。
(2)导航坐标系(n系)
一般选取当地地理坐标系作为导航坐标系，坐标Xn轴指向地理东方，Yn轴指向地理北方，Zn轴垂直于当地水平面，沿当地垂线向上[[xix],如图4.2所示。
(3)载体坐标系(b系)
载体坐标系，原点位于机体的质心位置，通常选取右前上坐标系，其Xb轴沿机
体横轴向右，Yb轴沿机体纵轴向前，Zb轴沿机体竖轴向上，如图4.2所示。
图 4.2 载体坐标系和导航坐标系
4.4 姿态角定义
姿态角也是我们常说的欧拉角，是根据导航坐标系和载体坐标系之间的关系来定义的。欧拉角是飞行器的三个姿态角即俯仰角、横滚角和偏航角，根据欧拉旋转定律可以用三次旋转使得飞行器本身的坐标系与地理参考系重合，每一次的旋转以机体坐标系的x、y、z轴中的一个坐标轴来转动，转过的角度即为欧拉角，三次坐标的变换矩阵相乘的积就是欧拉姿态矩阵[[xx]，形如4-1：
(4-6)
最终的姿态矩阵与这三次转动的先后顺序是有关系的，通常我们都按照Z-X-Y轴的顺序。定义机体绕本体系x轴转动的角度为俯仰角；机体绕本体系y轴转动的角度为横滚角；机体绕本体系z轴转动的角度为航向角；三个角当转动方向与旋转轴符合右手定则为正方向[[xxi]。俯仰角、横滚角和航向角合称欧拉角。得到如下姿态矩阵4-2：
(4-7)
4.5 四元数姿态解算算法
四元数其实是 1个单位实数和3个虚数单位、 和 的线性组合，一般可表示为d + a + b + c， a、b、c、d代表实数[[xxii]。
四元数乘法运算关系如下：
式中，表示的是四元数的乘法。
四元数与姿态矩阵之间的关系：
设有参考坐标系R，坐标轴X0、 Y0、 Z0，坐标轴方向的单位向量为、 、[[xxiii]。刚体相对于坐标系R作定点转动，定点为O，把坐标系b与刚体固联，b系的坐标轴为x、y、z，坐标轴方向的单位向量为、 、 [[xxiv]。假设初始时刻b系与R系重合。在刚体上任取一点A，从 O点向该点引向量,如图4.3所示。则该位置向量描述了刚体的空间角位置[[xxv]。
图4.3 刚体的等效转换
设刚体以= 相对R系旋转，初始时刻位置处于=，经过时间t后位置向量处于。根据欧拉定理，刚体从位置转到位置的转动可等效成绕瞬轴转过角一次完成[[xxvi]。这样，位置向量做圆锥运动，和位于同一圆上，和位于同一圆锥面上。
在圆上取一点B，是，由图4.3可得。
所以
由三重矢量计算公式：
即
所以
将上式向R系内投影：
记
，，
根据叉乘关系表达式：
记
(4-8)
则
所以
(4-9)
令
(4-10)
则式(4-4)可以写成：
(4-11)
记刚体固联坐标系为b系。
所以
而在转动过程中，位置向量和b系都同刚体固联，所以位置和b系的相对位角位置不变，即有
因此可得：
该式说明即为b系至R系的坐标变换矩阵，根据式(4-3)和(4-5)
即
(4-12)
令
(4-13)
并以、、、构造四元数：
(4-14)
可得如下结论：
(1)四元数 描述了刚体的定点运动[[xxvii]， 包含了等效旋转的全部信息，为旋转瞬轴的旋转方向，为转过的角度。
(2)四元数可以确定出b系至R系的坐标变换矩阵。将(4-8)代入(4-7)得：
(4-15)
由于，所以可进一步得出如下结论：
(4-16)
如果将向量和看作零标量的四元数，则和间的变换关系可采用四元数乘法表示：
该式称为坐标变换的四元数乘表示方法，其中为R系至b系的旋转四元数。证明如下：
(4-17)
对比式(4-11)知上式矩阵中右下角的33方块即为，所以式(4-12)可写成：
即
该式称为坐标变换的矩阵表示法。所以四元数乘法表示法和矩阵表示法是等价的。
如果参考坐标系R是导航坐标系n,刚体的固联坐标系b为机体坐标系，则坐标变换矩阵就是姿态矩阵，而由姿态矩阵可计算出姿态角[[xxviii]。
记，由于在坐标系旋转的过程中坐标系始终保持直角坐标系，所以为正交矩阵
对比上式可得姿态角
(4-18)
因此，四元数包含了所有的姿态信息，姿态解算实际上是如何计算四元数。下面来介绍如何计算四元数：
(1)初始化四元数
假设当前的坐标系为地理坐标系，则四元数列向量
(2)从传感器获取载体加速度和角速度
从MPU6050读取三轴加速度计的测量值即加速度、、，陀螺仪的测量值即角速度，，。
(3)将加速度计得出来三个轴的加速度值、、转化为三维的单位向量得到：
(4-19)
(4)将地理坐标系的重力向量转换到机体坐标系可得三轴的重力量，，：
(4-20)
(5)将地理坐标系转换到载体坐标系下的重力向量和载体坐标系测量的加速度向量外积，得到两坐标系的误差[[xxix]：
(4-21)
(6)陀螺仪误差是导致机体坐标系误差的根本原因，因此用两坐标系误差的PI来补偿陀螺仪使得机体坐标系更加准确。
(4-22)
其中：和是调整参数，在实际调试中确定。其中可以等于0，可以以0为初始值，0.01步进调节。
(7)四元数姿态更新方程[[xxx]。
四元数微分方程为：
(4-23)
其中，
将上式写成矩阵形式
(4-24)
对四元数一阶微分方程进行一阶毕卡算法可得：
(4-25)
(8)对四元数进行规范化处理，于是得下式4-21：
(4-26)
(9)以上得到的新的四元数代表完成了一次四元数的运算，将此四元数回到开头，将旧的四元数更新为新四元数，作为下一次四元数运算的初始数，再从(1)开始下一次的四元数运算[[xxxi]。与此同时将新的四元数更新规范化后转化成三个欧拉角得下式(4-22)，完成了姿态的初步运算[[xxxii]：
(4-27)
4.6 校准载体航向角
通过三轴数字电子罗盘可以校准陀螺仪积分获得的航偏角以消除累计误差。如果电子设备干扰强烈的情况下需要暂停数字罗盘的数据融合[[xxxiii]，因为这样测到的数据误差非常的大。对地磁传感器常用的校准方法之一是平面校准法[[xxxiv]：使用数字罗盘前，对其进行初始化，然后进行磁场校准，将传感器水平放置并旋转一周得到新的圆周圆心位置 [[xxxv]。这是对水平的平面的校准，校准前后对比如图4.4所示。
图 4.4 xy平面校准前后对比
同理需要对竖直平面进行校准，校准前后的对比图，如图4.5所示。
图 4.5 xz平面校准前后对比
当载体静止时，设地球磁场强度在载体坐标系b系各个轴的分量为，在导航坐标系R系各轴的分量，根据
可得：
(4-28)
将4-22式求得的、代入上式，即可求出和。航向角可由式4-24得出：
(4-29)
5 四旋翼飞行器系统软件设计5.1 系统程序设计5.1.1 姿态参考系统软件设计
姿态参考系统的软件部分主要是对MEMS传感器数据和三轴数字电子罗盘传感器的数据进行处理和数据融合，图5.1给出了姿态参考系统的软件流程图。
图 5.1 姿态参考系统程序流程图
通过软件流程图可以清楚地看到，系统首先完成MCU内部初始化的工作，即配置STM32使用到的外设，包括时钟系统、NVIC中断控制器、USART串口通信、I2C接口、定时器；然后进行硬件初始化以及系统的姿态解算，采用模块化的设计，具体包括以下4个部分：(1)初始化硬件；(2) MEMS惯性测量单元的数据采集模块；(3)多传感器数据融合的姿态解算模块；(4)姿态信息输出模块。
首先开启定时器中断，在2.5ms的计时中断实现通过I2C接口读取MEMS传感器和三轴数字电子罗盘的测量数据，根据传感器的特点进行滤波后，再利用初始校正所得到的校正数据对读取的数据作校正，校正完后转换成实际的物理量，再对其做加权平均减小误差，然后进行四元数姿态解算和地磁数据校准，最后进行数据融合输出姿态角信息。
5.1.2 PID控制算法设计
根据四旋翼飞行器的工作原理知，因为其结构对称，所以改变电机的转速差能够实现对飞行器的姿态控制，而飞行器的位置控制要靠电机的总升力和飞行器姿态改变的[[xxxvi]。所以，将飞行控制系统分为位置控制系统和姿态控制系统分别进行控制设计。由于飞行器姿态会直接影响到飞行器的位置，认为飞行器姿态控制器为内环控制器，位置控制器为外环控制器。这种数字式PID控制有位置式PID控制算法和增量式PID控制算法两种形式，这里采用的是增量式PID算法[[xxxvii]，其表达式为：
(5-1)
其中，为比例系数，为积分系数，微分系数。是第次采样时刻计算的PID输出值，是第次采样时刻控制器输入的偏差。
，
四旋翼飞行器的姿态可分为俯仰角、横滚角和偏航角，针对每一个自由度都是一个二阶系统[[xxxviii]。对每个姿态角都进行PID控制，这样就可以将复杂的非线性多变量输入多变量输出控制问题化简为两变量输入单变量输出的问题[[xxxix]。如式5-2所示。
(5-2)
这里、、、代表的是控制四个电机所需要的PWM值，表示的是飞行器停留在空中需要的PWM值，可以由遥控器输入或者自动高度修正，、、分别表示的是俯仰角偏差、横滚角偏差和航向角偏差、及其变化率所需的PWM值，把PWM限制在一定的范围之内。
、、采用串级PID控制算法，内回路是角速度控制回路，外回路是角度控制回路。三个角度的控制规律是一致的，以俯仰角的控制为例，如图5.2所示，其中为控制飞行器的期望俯仰角，和分别表示飞行器俯仰角角度和俯仰角角速度。是外环PID的输出值，是内环PID控制器的输出值，且。
图 5.2 串级PID控制图
首先需要求出四旋翼飞行器姿态误差信号也就是期望值的姿态角与当前获取到的姿态角的差值，然后通过串级PID控制算法求得各个电机的调整量，将调整信号传递给四个旋转电机，改变电机的转速来控制整个系统的姿态，使其姿态误差始终趋于最小，形成双级闭环回路控制系统。
|
QQ好友和群
QQ空间
腾讯微博
腾讯朋友
收藏
淘帖
顶
踩
