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

2.2 MPU6050数据读写

2.2.1 MPU6050初始化

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

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

2.2.2 滤波算法

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

　　在该系统中，我们从三轴陀螺仪得到的姿态数据有两组：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 总结

　　本文实现了对四旋翼飞行器控制的软件程序编写。主要研究了PWM波的输出以及对MPU6050的数据采集与处理。重点是控制流程的设计，即包含多个PID控制器的协同工作。与上位机和遥控之间的通讯是通过SPI协议进行读写，在本论文中不做深入研究。

　　在实际编写中，是用STM32库函数版本进行代码编写，大大简化了程序编写的难度，并且可读性也更强。

　　最后进行了系统调试实验，定点定姿定高飞行控制姿态角的收敛情况如图8所示。从实验结果可以看出该控制系统在有一定的外部干扰情况下，能够在40秒左右达到控制目标。

参考文献：

　　[1]杨明志，王敏.四旋翼微型飞行器控制系统设计[J],计算机测量与控制,2008,16(4):485-490.

　　[2]李劲松,宋立博,颜国正.基于自适应逆控制方法的小型四旋翼无人直升机姿态控制[J].上海交大学报,2012,46(6):956~961

　　[3]王璐,李光春,王兆龙等.欠驱动四旋翼无人飞行器的滑模控制[J].哈尔滨工程大学学报,2012,33(10):1249~1253

　　[4]甄红涛,齐晓慧,夏明旗等.四旋翼无人机鲁棒自适应姿态控制[J].控制工程,2013,20(5):916~919.

　　[5]王丽新.基于滑模理论的四旋翼直升机的姿态控制研究[D].沈阳:东北大学,2009.

　　[6]汪绍华,基于卡尔曼滤波的四旋翼飞行器姿态估计和控制算法研究(英文)[J].控制理论与应用,2013,30(9):1109~1115

本文来源于中国科技期刊《电子产品世界》2016年第9期第48页，欢迎您写论文时引用，并注明出处。