四旋翼飞行器控制系统设计

作者：杨建华赵红娟汶少阳时间：2016-09-28来源：电子产品世界收藏

编者按：本文基于四旋翼飞行器的工作原理和性能特点，给出了飞行器控制系统设计方案，进行了姿态和高度数据采集的软件设计，并基于卡尔曼滤波算法完成了传感器数据融合，设计了PID控制器并完成了软件实现。实验结果表明，该控制系统能够稳定可靠运行，具有较强的鲁棒性。

2.2 MPU6050数据读写

本文引用地址：http://www.eepw.com.cn/article/201609/310490.htm

2.2.1 MPU6050初始化

　　首先，我们要解除I2C休眠状态，(电源管理，典型值：0x00(正常启用))，然后确定陀螺仪采样率，典型值：0x07(125Hz)，陀螺仪自检及测量范围，典型值：0x18(不自检，2000deg/s)，以及加速计自检、测量范围及高通滤波频率，典型值：0x09(不自检，4G)。如图4所示。

　　除了对MPU6050进行工作模式的设定以外，还需设定芯片的零漂误差，在每次采集数据后，都应作差。这是因为MPU6050自身会受电磁场、温度等的影响，会产生偏差。该值通过事先固定好MPU6050并进行多次采样，通过加权滤波算法求取平均值得到。

2.2.2 滤波算法

　　由于原始的欧拉角没有经过滤波，陀螺仪的值会随着时间产生累积误差，即经过一定的时间之后会出现姿态错误的现象。因此，我们采用卡尔曼滤波算法对得到的原始的欧拉角进行滤波和数据融合，来消除噪声和累积误差，最终得到真实欧拉角。如图5所示。

四旋翼-3.jpg

　　在该系统中，我们从三轴陀螺仪得到的姿态数据有两组：X、Y轴的加速度转换的角度值和陀螺仪采集的X、Y轴的角速度，我们将加速度转化的角度值设为temp，角速度转化的角度值设为angle，angle的值等于上一时刻的真实角度值(即滤波得到的值)加上这一时刻角速度的值乘以时间积分dt，假设最初的协方差值为P[2][2] = [1, 0;0, 1]，加速度过程协方差为Q_angle=0.001，加速度过程协方差为Q_gyro=0.003, 陀螺仪误差初值q_bias设为0.05，测量协方差R_angle=0.5, 积分时间dt=0.0025;我们通过上面的五个公式不断地更新协方差P和卡尔曼增益Kg，获得最终的真实角度值(即卡尔曼滤波角度)。MPU6050与卡尔曼滤波的结合流程图如图6所示。

3 飞行器稳定控制的实现

　　所有传感器以及驱动部分均已配置好后开始配置主控程序的流程。本实验通过配置一个通用定时器，每经过一定时间后，进入一次中断服务函数，我们称之为控制中断。在中断服务函数里，提取传感器数据，进行PID运算以及调整PWM波。此外还要读取通过无线接收到的数据，以及通过无线向上位机发送信息。

3.1 控制定时器的设定

　　设置定时器3为控制定时器，设置其每2.5ms中断一次。设定定时器3的自动重装载值TIM_Peri为2500，分频系数TIM_Prescaler为72-1，采用向上计数。因为不需要比较输出，只需要计数并按照设定进入中断即可，因此不需要设置0Cx相关寄存器。

　　最后配置定时器3的抢占优先级为2，子优先级为0，这样防止与通讯串口或超声波模块传输数据用的串口发生冲突等情况。清除中断标志位后，使能定时器3定时器，则系统就会每经过2.5ms进入中断，采集传感器数据，并进行PID调节。

3.2 控制流程

　　控制流程是指定时器3计时结束，触发中断后，进入到中断服务函数里进行的操作。在进入中断后，首先清除定时器3的中断标志位，进入超声波测距子程序，并返回高度值。进入高度PID控制器进行控制量运算，然后输出PWM波。之后通过I2C协议开始读取MPU6050的数据，并进行数据融合，然后经过卡尔曼滤波得到比较稳定的姿态角信息，再分别经过俯仰PID控制器、横滚PID控制器以及偏航PID控制器进行控制量调整，得到的PWM控制变量进行叠加，最终将叠加后的四个电机的PWM控制变量输出到四个电机。

　　另一方面，为了实现无线数据的发送与接收，在这里，定义一个计数变量ms，每次进入中断后进行自加，根据该变量的值进行判定，是通过无线发送数据，还是通过无线接收数据，这样可以有效地防止无线通讯发生混乱。在中断服务函数里，控制流程图如图7所示。

四旋翼-4.jpg