吉林建筑大学学士学位论文
仿真来调试了。本次毕业设计用到仿真的地方主要有ADC电压采集程序部分，观察采集到的电压值是否准确；另外就是无线模块程序调试的时候，在线仿真调试数据是否发送和接收，并且判断是否正确，最后观察飞行器惯性测量单元MPU6050是否读取到数据部分，这里是仿真调试运用最多的地方。当然，在程序调试比如无线发送和接收部分，也可以通过串口打印显示来调试。
4.4 飞控软件设计
本次飞行器软件设计中，最主要的程序设计部分就是姿态传感器MPU6050数据读取和姿态欧拉角计算再到PID电机控制算法。
4.4.1 MPU6050数据读取
MPU6050数据读取是通过I2C协议跟MCU通信，程序设计直接读取MPU6050寄存器就可以，在此过程中，除了程序本身I2C驱动程序以外，需要编写MPU6050初始化程序和读取数据函数。其流程图如图4-1所示。
MPU6050初始化程序设计中，需要初始化电源管理寄存器PWR_MGMT_1、采样分频寄存器SMPLRT_DIV、MPU6050配置寄存器CONFIG、加速度计配置寄存器ACCEL_CONFIG、陀螺仪配置寄存器GYRO_CONFIG。各寄存器地址如表4-1所示。
表4-1 MPU6050数据寄存器地址
地址名 地址 地址名 地址 地址名 地址 图4-1 MPU6050数据读取流程图否是否检测到器件？是读取数据I2C初始化开始MPU6050初始化ACCEL_XOUT_H 0x3B ACCEL_XOUT_L 0x3C GYRO_XOUT_H GYRO_XOUT_L 0x43 0x44 ACCEL_YOUT_H 0x3D ACCEL_YOUT_L 0x3E GYRO_YOUT_H GYRO_YOUT_L 0x45 0x46 ACCEL_ZOUT_H 0x3F ACCEL_ZOUT_L 0x40 GYRO_ZOUT_H GYRO_ZOUT_L 0x47 0x48
30
吉林建筑大学学士学位论文
其中设置采样分频寄存器SMPLRT_DIV初值为0x07，此时陀螺仪采样率125Hz；寄存器CONFIG的初值设置为0x06为低通滤波5Hz；GYRO_CONFIG寄存器初值设置为0x00，则陀螺仪量程500deg/s；ACCEL_CONFIG寄存器初值为0x00，则设置成为不自检加速度计量程2g，源代码如下所示。
void MPU6050Init (void) {
const INT8U ucData[5] = {0x00,0x07,0x06,0x00,0x00}; I2CMWriteNByte(SlaveAddress,1,PWR_MGMT_1,&ucData[0],1); delay_ms (200);
I2CMWriteNByte(SlaveAddress,1,SMPLRT_DIV,&ucData[1],1); //0x07 I2CMWriteNByte(SlaveAddress,1,CONFIG1,&ucData[2],1); //0x06 I2CMWriteNByte(SlaveAddress,1,GYRO_CONFIG,&ucData[3],1); //0x00 I2CMWriteNByte(SlaveAddress,1,ACCEL_CONFIG,&ucData[4],1); //0x00 delay_ms (200); }
其中SlaveAddress是MPU6050的设备地址，即为0xd0。读取数据源代码如下： int16 GetData(int8_t REG_Address) {
uint8_t H,L;
I2CMReadNByte(SlaveAddress&0xFE,1 ,REG_Address,&I2CMRevBuf[0] ,1); H = I2CMRevBuf[0];
I2CMReadNByte(SlaveAddress &0xFE,1 ,REG_Address+1,&I2CMRevBuf[1] ,1); L = I2CMRevBuf[1];
return (H<<8)+L; /*合成数据*/ }
31
吉林建筑大学学士学位论文
其中REG_Address为MPU6050寄存器地址。程序运行时通过GetData（）读取数据函数读取所需陀螺仪和加速度计的原始数据。比如需要读取加速度计X轴的数据，直接写入Accel_y=GetData(ACCEL_YOUT_H)就可以，此时读出的原始数据不能直接用，需要进一步数据处理，才能得到欧拉姿态角。
4.4.2 姿态计算IMU
通过前一步数据读取，然后经过计算才能得到姿态欧拉角，其数据处理流程图如图4-2所示。
四元数计算是一个超复数形式的数学计算方法，四元数可以理解为一个实数和一个向量的组合，也可以理解为四维的向量。
四元数IMC计算读取原始数据零偏置处理分辨率计算姿态解算的核心在于旋转，一般旋转有4种表示方式：矩阵表示、欧拉角表示、轴角表示和四元数表示。矩阵表示适合变换向量，欧拉角最直观，轴角表示则适合几何推导，而在组合旋转方面，四元数表示最佳。因为姿态解算需要频繁组合旋转和用旋转变换向量，所以采用四元数保存组合姿态、辅以矩阵来变换向量的方案。
四元数计算过程相当复杂，程序量很大，完全是数学计算公式，此段程序通过把MPU6050读取出来的数据值进行处理最终得到欧拉角度值。
图4-2 姿态欧拉角计算流程图互补滤波欧拉角4.4.3 PID电机控制
PID 控制器是一个在工业控制应用中常见的反馈回路部件，由比例单元P（Proportion）、积分单元I（Integration）和微分单元D（Differentiation）组成。PID控制器作为最早实用化的控制器已有近百年历史，现在仍然是应用最广泛的工业控制器。PID控制器简单易懂，使用中不需精确的系统模型等先决条件，因而成为应用最为广泛的控制器。
32
吉林建筑大学学士学位论文
在此次设计中，最终目的是想要飞行器平稳飞行，其中的设计就免不了运用PID电机调速控制。如图4-3所示是一个小功率直流电机的调速框图。给定速度其差值为e?t??n0?t??n?t?，经过PID控制器调整后输出n0?t?与转速n?t?进行比较，
电压控制信号u?t?，u?t?经过功率放大后，驱动直流电动机改变其转速。
图4-3 小功率电机调速系统
PID数学计算公式如式 4-1 所示：
1d(e(t))u(t)?KP[e(t)??e(?)d??Td] （4-1）
Ti0dt式中
KP――控制器的比例系数
Ti－－控制器的积分时间，也称积分系数 Td――控制器的微分时间，也称微分系数
t常规的模拟PID控制系统原理框图如图4-4所示。该系统由模拟PID控制器和被控对象组成。其中r?t?是系统输入误差值，y?t?是系统的最终输出值，输入值与输出值构成控制偏差e?t??r?t??y?t?。而e?t?作为PID控制器的输入，u?t?作为PID控制器的输出和被控对象的输入。
33
吉林建筑大学学士学位论文
比例r(t)+e(t)积分u(t)被控制对象y(t)-微分
图4-4 PID电机控制系统框图
根据PID计算公式和原理，编写出PID控制程序，其源代码如下所示： FP32 PID_ROL(FP32 e,FP32 e1,FP32 kp,FP32 ki ,FP32 kd) {
static FP32 e_s = 0,sum = 0; /*e_s用于保存上一次的误差值，用于计算微
分项，Sum用于计算累加和，计算积分项*/
FP32 r; /* r存放PID计算值 */
sum = sum + e; /* 累加和计算 */ r = kp*e+ki*sum+kd*(e1-e_s); /*从左至右分别是比例、积分、微分*/ e_s = e1; /*保存这一次的误差值用于下一次微分计算*/
return r; /* 返回计算值 */ }
然而，PID算法的重点不在于他的原理，而是着重PID三个参数的调节。PID调试过程步骤如下：
1．控制回路中的比例项P
比例控制是一种最简单的控制方式。其控制器的输出与输入误差信号成比例关系，当仅有比例控制时系统输出存在稳态误差。P是解决幅值震荡，P大了会出现幅值震荡的幅度大，但震荡频率小，系统达到稳定时间长。
2．控制回路中的积分项I
在积分控制中，控制器的输出与输入误差信号的积分成正比关系。对一个自
34