西华大学毕业设计说明书 处理,需要延时等待。如果读取晚了,最直观的影响就是飞行器姿态调节缓慢,无法正常飞行。所以随意的去读取数据或者读取的时机选择得不好时(太快或太慢)就会出现数据不连续。
为了解决这个问题,使用了MPU6050芯片的数据就绪中断模式,再利用单片机的外部中断,我们就可以在MPU6050数据就绪时立刻获得飞行器的姿态。
使用MPU6050数据就绪中断模式需要配置以下的寄存器,如表6.8-6.10所示。
表6.8 寄存器58-中断标志寄存器
Register Register (Hex) (Decimal) 3A 58 Bit7 - Bit6 MOT_INT Bit5 - Bit4 FIFO _OFLOW _INT Bit3 I2C _MST _INT Bit2 - Bit1 - Bit0 DATA _RDY _INT 这个寄存器展示各个中断源的中断标志。每个位在读取后被清除。这个寄存器我们需要注意DATA_RDY_INT位。当数据就绪中断(Data Ready interrupt)产生,该位自动置 1。读取寄存器该位清 0。
表6.9 寄存器55-中断配置寄存器
Register (Hex) 37 Register (Decimal) 55 Bit7 INT_ LEVEL Bit6 INT_ OPEN Bit5 LATCH _INT _EN Bit4 INT_RD _CLEAR Bit3 Bit2 Bit1 Bit0 - FSYNC FSYI2C _INT NC _BY_LEVEL _INT PASS _EN _EN 这个寄存器配置 INT引脚中断信号的产生。
INT_LEVEL该位等于0, INT引脚的逻辑电平为高电平。该位等于1,INT引脚的逻辑电平为低电平。
LATCH_INT_EN该位等于0,INT引脚产生50us脉冲。该位等于1,INT引脚保持有效电平直到中断被清除。
INT_RD_CLEAR该位等于0,只有读取 INT_STATUS (寄存器58) 会导致中断状态位清除。
INT_RD_CLEAR该位等于1,任何读取动作会导致中断状态位清除。 经过分析后,程序里是这样配置的:
INT_LEVEL等于1,INT引脚的逻辑电平为低电平有效。 INT_OPEN该位等于0,the INTpin is configure das push-pull。 LATCH_INT_EN等于1,INT引脚保持有效电平直到中断被清除。 INT_RD_CLEAR该位等于1,任何读取动作会导致中断状态位清除。
第 29 页
西华大学毕业设计说明书 表6.10 寄存器56-中断使能寄存器
Register (Hex) 38 Register (Decimal) 56 Bit7 Bit6 MOT_EN Bit5 Bit4 Bit3 Bit2 - Bit1 - Bit0 DATA _RDY_EN FIFO I2C_MST _OFLOW _INT_EN _EN 该寄存器使能中断源的中断产生。
MOT_EN 该位置1,该位使能运动检测(Motion detection)产生中断。 FIFO_OFLOW_EN 该位置1,该位使能FIFO缓冲区溢出产生中断。 I2C_MST_INT_EN 该位置1,该位使能 I2C主机所有中断源产生中断。
DATA_RDY_EN该位置1,该位使能数据就绪中断(Data Ready interrupt),所有的传感器寄存器写操作完成时都会产生。
这个寄存器只需要配置DATA_RDY_EN位。
把MPU6050数据就绪中断配置完成后,还需要配置单片机外部中断,下降沿触发。 配置完成后,将中断引脚连接上示波器观察,发现数据就绪中断的周期是10ms,说明每10ms数据就会更新一次。
6.3 PID调试
6.3.1 PID姿态控制
按偏差的比例,积分和微分进行控制的1(简称为PID调节器、也称PID控制器),是连续系统中技术成熟、应用最为广泛的一种调节器。它结构简单,参数易于调整,在长期应用中已积累了丰富的经验。特别是在工业过程中,由于控制对象的精确数学模型难以建立,系统的参数经常发生变化,运用控制理论分析综合要耗费很大的代价,却得不到预期的效果,所以人们往往采用PID调节器,根据经验进行在线整定,以便得到满意的控制效果。PID框图如图6.7所示。
比例 e(t) r(t) 积分 u(t) 执行机构 对象 电c(t) 微分
图6.7 PID框图
第 30 页
西华大学毕业设计说明书 飞行控制系统使用的数字PID控制算法,P是比例环节,I是积分环节,D是微分环节。控制处理器是采样控制,只能根据采样时刻偏差值计算控制量进行离散式控制形式。在离散化PID过程中令T为采样周期,k为采样序号,连续时间t用离散时间k?T表示,用求和形式代替连续时间积分形式,用增量形式代替连续时间微分形式。
t?k?T (1)
?e(t)dt0t?T?e(jT)?T?ej (2)
j?0j?0kkde(t)e(kT)?e[(k?1)T]ek?ek?1?? (3) d(t)TT 将(3)代入模拟PID的计算表达式
u(t)?Kp[e(t)?1tT1?e(t)dt?TD0de(t)]?u0 (4) dt其中Kp为比例系数,T1为积分常数,TD为微分常数。 可得到离散PID表达式
Tuk?Kp[ek?T1Jk?ej?j?0TD(ek?ek?1)]?u0 (5) T式(5)中,定义积分系数Ki?KpTT,微分系数Kd?KpD。
TT1在3飞行器原理和6.1.2姿态解算里都分析到,控制四旋翼飞行器是控制它的角度,即姿态角。利用PID算法控制四旋翼飞行器,那么最简单,同时也是最容易想到的一种控制策略就是角度单环PID控制器,系统框图如图6.7所示:
控制信号 (期望角度) PID角度单环控制器 无刷 电机 电机 供电 当前姿态角(DMP) 传感器模块
图6.8 角度单环PID控制策略
第 31 页
西华大学毕业设计说明书 根据图6.8和式(5)可以整理出单环PID控制的程序思路如下: 当前角度误差 = 期望角度 - 当前角度 单环PID_P项 = Kp * 当前角度误差
单环PID_I项 = Ki * 当前角度误差 + 上一次单环PID_I项 单环PID_D项 = Kd * ( 当前角度误差 - 上一次角度误差 ) 单环PID输出 = 单环PID_P项 + 单环PID_I项 + 单环PID_D项
将单环PID_输出转换为单片机的PWM波输出量,既可以调节当前无刷电机的转速。然而在实际的飞行测试中,发现角度单环PID控制策略效果并不理想,需要进一步改进控制策略。
在上述角度单环PID控制算法仅仅考虑了飞行器的角度信息,需要增加控制的量。线加速度和转动角速度作为MPU6050的原始数据是两个很容易获取的量。相较于线加速度,角速度更能够及时体现出当前飞行器的运动情况。为了增加飞行器的稳定性(增加阻尼)并增强系统的快速响应,选择控制飞行器的角速度。于是角度/角速度-串级PID控制算法应运而生。简而言之,串级PID就是两个PID控制算法,只不过把它们套起来了。因为有两个控制器控制飞行器,它会比单个控制器控制更多的变量,使得飞行器的适应能力更强,增强了系统的抗干扰性(也就是增强稳定性)。串级PID原理框图如图6.9所示:
PI 角度外环控制器 PID 角速度内环控制器 无刷 电机 电机 供电 控制信号 (期望角度) 当前姿态角(DMP) 传感器模块(角速度数据)
图6.9 角度/角速度串级PID控制策略
采用控制品质更加优秀的串级PID控制策略,根据图6.9和式(5)可以整理出串级PID控制的程序思路如下: 外环(为角度环):
当前角度误差 = 期望角度 - 当前角度 外环PID_P项 = Kp * 当前角度误差
外环PID_I项 = Ki * 当前角度误差 + 上一次外环PID_I项 外环PID输出 = 外环PID_P项 + 外环PID_I项
第 32 页
西华大学毕业设计说明书 内环(为角速度环):
当前角速度误差 = 外环PID输出 - 当前角速度 内环PID_P_rate项 = Kp_rate *当前角速度误差
内环PID_I_rate项 = Ki_rate *当前角速度误差 + 上一次内环PID_I_rate项 内环PID_D_rate项 = Kd_rate * ( 当前角速度误差 - 上一次角速度误差 )
内环PID_rate输出 = 内环PID_P_rate项 + 内环PID_I_rate项 + 内环PID_D_rate项
这里的内环PID_rate输出就是飞行器需要调节自身的量,把当前姿态修正到期望姿态,控制姿态需要通过调节电机PWM波实现,为了将PWM波和内环PID_rate输出联系起来,构建了以下的公式。
moto1=MOTO_MxMi(moto1,1700,1105); moto2=MOTO_MxMi(moto2,1700,1105); moto3=MOTO_MxMi(moto3,1700,1105); moto4=MOTO_MxMi(moto4,1700,1105);
moto1=throttle-PID_PIT_RATE.OUT-PID_ROL_RATE.OUT-PID_YAW_RATE.OUT; moto2=throttle-PID_PIT_RATE.OUT+PID_ROL_RATE.OUT+PID_YAW_RATE.OUT; moto3=throttle+PID_PIT_RATE.OUT+PID_ROL_RATE.OUT-PID_YAW_RATE.OUT; moto4=throttle+PID_PIT_RATE.OUT-PID_ROL_RATE.OUT+PID_YAW_RATE.OUT;
throttle是电机值。PID_PIT_RATE.OUT,PID_ROL_RATE.OUT和PID_YAW_RATE.OUT是内环PID_rate输出。以moto1为例,moto1=MOTO_MxMi(moto1,1700,1105);每个电机需要限幅到最大值1700最小值1105,这样可以防止电机转速过大。最后得到的moto值输送到PWM波占空比控制程序,进而控制电机。 6.3.2 飞控系统PID调试
飞控系统最主要的调试是进行PID调试,一组良好的PID参数可以让飞行器平稳的起飞和降落。
在调整PID参数时,需再飞行器的起飞油门基础上进行PID参数的调整,否则飞行器可能只是在小油门调试的时候稳定了,在飞行过程中很可能又会晃荡。为了方便飞行器实物调试和观察现象,借助了工具万向轮,在万向轮上飞行器拥有很高的自由度,具体如图6.10所示。
第 33 页
百度搜索“77cn”或“免费范文网”即可找到本站免费阅读全部范文。收藏本站方便下次阅读,免费范文网,提供经典小说综合文库基于STM32的四旋翼飞行控制系统毕业设计 - 图文(8)在线全文阅读。
相关推荐: