四旋翼无人机设计与制作
4.6 校准载体航向角
通过三轴数字电子罗盘可以校准陀螺仪积分获得的航偏角以消除累计误差。如果电子设备干扰强烈的情况下需要暂停数字罗盘的数据融合[37]，因为这样测到的数据误差非常的大。对地磁传感器常用的校准方法之一是平面校准法[38]：使用数字罗盘前，对其进行初始化，然后进行磁场校准，将传感器水平放置并旋转一周得到新的圆周圆心位置((xmax?xmin)/2,(ymax?ymin)/2) [39]。这是对水平的xy平面的校准，校准前后对比如图4.4所示。
图 4.4 xy平面校准前后对比
同理需要对竖直平面xz进行校准，校准前后的对比图，如图4.5所示。
图 4.5 xz平面校准前后对比
bHb???HxbHyRHzb?在导航坐标系R系各轴的分量HR???，?HxTRHyT
当载体静止时，设地球磁场强度H在载体坐标系b系各个轴的分量为
HzR?根据 ?，
HR?CbRHb可得：
–27–
四旋翼无人机设计与制作
?HxR??cos??R?? ?Hy???0?HzR?????sin?sin?sin?cos??sin?cos?b??cos?sin???Hx??Hb? (4-28) sin???y?b?cos?cos??Hz????R
将4-22式求得的?、?代入上式，即可求出HxR和Hy。航向角?可由式4-24得
出：
??arctaHRHnyR x–28–
(4-29)
四旋翼无人机设计与制作
5 四旋翼飞行器系统软件设计
5.1 系统程序设计
5.1.1 姿态参考系统软件设计
姿态参考系统的软件部分主要是对MEMS传感器数据和三轴数字电子罗盘传感器的数据进行处理和数据融合，图5.1给出了姿态参考系统的软件流程图。
开始系统初始化初始化IIC接口初始化MEMS传感器初始化数字电子罗盘读取传感器ID是否正确？Y配置MEMS传感器参数N配置地磁传感器参数读取加速度数据读取陀螺仪数据读取地磁传感器数据低通滤波or平均滤波卡尔曼滤波去极值滤波四元数姿态结算姿态结算互补滤波输出姿态角结束
图 5.1 姿态参考系统程序流程图
–29–
四旋翼无人机设计与制作
通过软件流程图可以清楚地看到，系统首先完成MCU内部初始化的工作，即配置STM32使用到的外设，包括时钟系统、NVIC中断控制器、USART串口通信、I2C接口、定时器；然后进行硬件初始化以及系统的姿态解算，采用模块化的设计，具体包括以下4个部分：(1)初始化硬件；(2) MEMS惯性测量单元的数据采集模块；(3)多传感器数据融合的姿态解算模块；(4)姿态信息输出模块。
首先开启定时器中断，在2.5ms的计时中断实现通过I2C接口读取MEMS传感器和三轴数字电子罗盘的测量数据，根据传感器的特点进行滤波后，再利用初始校正所得到的校正数据对读取的数据作校正，校正完后转换成实际的物理量，再对其做加权平均减小误差，然后进行四元数姿态解算和地磁数据校准，最后进行数据融合输出姿态角信息。
5.1.2 PID控制算法设计
根据四旋翼飞行器的工作原理知，因为其结构对称，所以改变电机的转速差能够实现对飞行器的姿态控制，而飞行器的位置控制要靠电机的总升力和飞行器姿态改变的[40]。所以，将飞行控制系统分为位置控制系统和姿态控制系统分别进行控制设计。由于飞行器姿态会直接影响到飞行器的位置，认为飞行器姿态控制器为内环控制器，位置控制器为外环控制器。这种数字式PID控制有位置式PID控制算法和增量式PID控制算法两种形式，这里采用的是增量式PID算法[41]，其表达式为：
k?e(k)?Ik?(ek)?Dk[?(ek)??(e?k??u(k)?P ? (5-1)
?1)?u(k)??u(k)?u(k其中，kP为比例系数，kI为积分系数，kD微分系数。u(k)是第k次采样时刻计算的PID输出值，e(k)是第k次采样时刻控制器输入的偏差。
?e(k)?e(k)?e(k?1)，?e(k?1)?e(k?1)?e(k?2)
四旋翼飞行器的姿态可分为俯仰角、横滚角和偏航角，针对每一个自由度都是一个二阶系统[42]。对每个姿态角都进行PID控制，这样就可以将复杂的非线性多变量输入多变量输出控制问题化简为两变量输入单变量输出的问题[43]。如式5-2所示。
?P1???P2? ??P3??P??4PP??P??Ph??PP??P??Ph?? (5-2)
PP??P??Ph??PP??P??Ph??–30–
四旋翼无人机设计与制作
这里P1、P2、P3、P4代表的是控制四个电机所需要的PWM值，Ph表示的是飞行器停留在空中需要的PWM值，可以由遥控器输入或者自动高度修正，P?、P?、P?分别表示的是俯仰角偏差、横滚角偏差和航向角偏差、及其变化率所需的PWM值，把PWM限制在一定的范围之内。
P?、P?、P?采用串级PID控制算法，内回路是角速度控制回路，外回路是角度控制回路。三个角度的控制规律是一致的，以俯仰角的控制为例，如图5.2所示，其中?为控制飞行器的期望俯仰角，?和??分别表示飞行器俯仰角角度和俯仰角角速
r度。?1是外环PID的输出值，?out是内环PID控制器的输出值，且?out?P?。
期望的欧拉角? +r +角度PID控制-??1-??角速度PID控制?out电机四旋翼飞行器姿态
图 5.2 串级PID控制图
首先需要求出四旋翼飞行器姿态误差信号也就是期望值的姿态角与当前获取到的姿态角的差值，然后通过串级PID控制算法求得各个电机的调整量，将调整信号传递给四个旋转电机，改变电机的转速来控制整个系统的姿态，使其姿态误差始终趋于最小，形成双级闭环回路控制系统。
–31–