四轴飞行器课件(转)

谢谢大家
中断函数
• void Angle_Calculate() interrupt 1 • { • Read_MPU(); • MPU_pro(); • Get_Control_Data() ; • MainControl(); • }
• 注意：我们的所有函数都是在30mz的频率下运行 的。 • 因为要控制飞行器的姿态所以要用到传感器获取 飞行器的姿态。我选用的mpu6050传感器，自带 3轴角速度和3轴重力加速度计。可以获取飞行器 的角度和角速度。所以我们需要实现获取传感器 数据的底层函数 • InitMPU6050();//初始化MPU-6050 • Read_MPU();//
限幅
• • • • • • • • • • • • • • • • • • • if( PWM_XZ<MinPeriod) PWM_XZ=MinPeriod; else if( PWM_XZ>MaxPeriod) PWM_XZ=MaxPeriod; if( PWM_XF<MinPeriod) PWM_XF=MinPeriod; else if( PWM_XF>MaxPeriod) PWM_XF=MaxPeriod; if( PWM_YZ<MinPeriod) PWM_YZ=MinPeriod; else if( PWM_YZ>MaxPeriod) PWM_YZ=MaxPeriod; if( PWM_YF<MinPeriod) PWM_YF=MinPeriod; else if( PWM_YF>MaxPeriod) PWM_YF=MaxPeriod;
个人的一些理解
• 粗略的可以认为四元数是一种描述四维空间坐标的一种方 式。可以利用它表示空间的伸缩和旋转。由于三维的复数 是不存在的（至今没有证明出），所以数学家们把目光投 向了四维复数。所以今天我们利用四元数来坐姿态的解算， 利用四元数解算世界坐标系，然后转换为欧拉角。欧拉角 是一个坐标系到另一个坐标系的变换，可以通过依次绕不 同的坐标轴的3次连续转动来定义。从物理角度看，欧拉 角表示法可能是最简单的方法之一。我们用陀螺仪和加速 度计所测得的量都是相对于世界坐标系的值，为了在控制 的上的更直观，更方便准确我们将坐标系转换为机载坐标 系，而姿态解算就是这个转换的过程。
• • • • • • • 我们需要mcu做什么？ 1、驱动电机 2、获取加速度、角速度数据 3、姿态解算滤波 4、控制算法 5、接收遥控器信号 6、调试数据输出
对应于驱动程序
• • • • • • • 1、驱动电机 2、获取加速度、 角速度数据 3、姿态解算滤波 4、控制算法 5、接收遥控器信号 6、调试数据输出 • Pwm波驱动 • iic通信 • • • • 四元数+卡尔曼 串级pid Spi+2401 Uart+串口示波器
• 控制飞行器的运动方向只要我们在平衡控 制环内传入控制量，打破水平姿态即可控 制运动。
控制算法
• 由于飞行器在控制当中属于一个二阶阻尼 系统，因此采用串级pid算法可以得到比较 稳定的控制 • 串级pid：
• 飞行器中的串级pid • 外环：角度值 p 或pi • 内环：角速度 p、d
• 可能大家查过资料中有很多写的是外环p、i 内环p、i、d，我这里利用的是内环p、d，原 因是我认为i项主要用于静态误差消除，但是 对于初学者，加上i项调试比较困难，而且效 果不明显。内环利用pd已经可以稳定飞行， 当然同样希望大家努力探索出更好地控制方 法。
2、姿态解算
• 四元数与欧拉角：
• 欧拉角与四元数姿态表示方法是目前工程上最常用 的两种方法。欧拉角表示法具有简便、几何意义明 显等优点，同时姿态敏感器可以直接测出这些参数， 能较方便地求解用这些姿态参数描述的姿态动力学 方程。但采用欧拉角的姿态描述方法存在奇点问题， 且需多次三角运算。而采用四元数表示方法则可以 避免这些问题，因此目前工程上开始采用四元数来 描述飞行器运动及动力学方程中的姿态，而在设计 控制规律时，由于欧拉角的直观性和几何意义，仍 然采用欧拉角描述。
算法上的实现
• 外环： (我的外环仅仅用了p项) • 外环输出值=p*（实际姿态解算角度值-平衡位置 姿态值-控制量） • exValue.Pitch_p =expidPitch.p*(Attitude.pitchControldata_PITCH-XStaticSet ); exValue.Roll_p =expidRoll.p*(Attitude.rollControldata_ROLL-YStaticSet ); • exValue.Yaw_p =expidYaw.p* (Attitude.yawSETYAW+Controldata_YAW);
• void Time0_Init() 12T自动重载 • { • AUXR &= 0x7F; • TMOD &= 0xF0; • TL0 = 0x58; • TH0 = 0x9E; • TF0 = 0; • TR0 = 1; • EA=1; • ET0=1; • }
//10ms@30MHz 定时器0 16位 //定时器时钟12T模式 //设置定时器模式 //设置定时初值 //设置定时初值 //清除TF0标志 //定时器0开始计时
• 具体的调试过程需要写很多篇，因为时间 有限，内容过多不动手做光看也会很无聊， 所以就另写了一篇姿态解算调试文档，大 家可以课下观看。网址发给大家：
控制算法
• 四轴飞行器的飞行建立于平衡控制的基础 之上。 • 飞行器的平衡即为飞行器的x轴以及y轴平 衡。也就是飞行器的俯仰和横滚能够维持 在一个固定下效果：解算前：
解算后
• 发现滤波后的波形去除了尖峰以及抗干扰 能力明显增强，可以满足要求。
算法实现
• • • • 调节参数： #define Kp 1.0f //10.1f #define Ki 0.005f//0.011f #define halfT 0.0053f
1、pwm波驱动
• • • • • • 软件底层的需要： 首先需要设定4路pwm波来控制四路电机 15需要用timer2来设定pwm频率 建议5k-10k频率 void Time2_Init() { AUXR &= 0x7F; //定时器时钟12T模式
• T2L = 0xEB; • T2H = 0xFF; • • } • 还需要配置pwm波发生器的其他寄存器 代码在工程中的pwmgo里面 太长不粘了 • 还有一个函数控制pwm波的占空比，我们就是利用占空比来控制电机 的转速的。
内环pd
• 内环输出=p*（实际角速度值+外环输出角 度值）-d*（当前角速度-上一次角速度） • 也就是将外环输出的角度值作为期望值加 入内环控制当中。
最终的整合用于pwm输出控制四个 电机转速
• 1号电机pwm输出值=油门+pwm最小值-俯 仰控制量+横滚控制量+偏航控制量； • 2号电机pwm输出值=油门+pwm最小值+俯 仰控制量-横滚控制量+偏航控制量； • 3号电机pwm输出值=油门+pwm最小值-俯 仰控制量-横滚控制量-偏航控制量； • 4号电机pwm输出值=油门+pwm最小值+俯 仰控制量+横滚控制量-偏航控制量；
• 做完这些底层函数我们可以开始进行调节 了 • 首先介绍一下传感器
• mpu6050
重力加速度计可以换算成角度值， 角速度计输出的是角速度。 我们需要获取飞行器的姿态就 需要知道角度和角速度 下图是角度输出的波形
下图是角速度输出的波形
对比
• 可以看出角速度相对平滑，动态特性好静 态特性差，角度值动态特性查抗干扰能力 低。直接利用传感器输出的角度值不能满 足控制要求，因为干扰太大，利用角速度 积分也不可以因为积分会导致误差的累积， 最终导致系统崩溃。因此我们需要一种滤 波算法以及坐标系转换的算法来对姿态值 进行解算。
类型
十字型
Y型
飞行器的运动模型
X型
姿态控制
俯仰 横滚 偏航 Pitch Roll Yaw
垂直运动
俯仰
向前
向后
横滚
向右
向左
偏航
四轴飞行器的组成 电池
遥控器
螺 旋 桨
电 机
电 机 驱 动
稳压电源 接收机
主控
大气压力计、电 子罗盘、gps、 osd、数传….
陀螺仪
加速度计
软件篇
• void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az)
• 具体解算过程、矩阵转换等等….请查询四 元数，网上资料很多，但也比较复杂。 （由于本人长时间不用复数以及矩阵转换 等，数学基础不好，为了避免误导大家， 就不和大家细说了。）
•还有一个函数控制pwm波的占空比， •我们就是利用占空比来控制电机的转速的。
•还有一个函数控制pwm波的占空比，我们就是利用占空比来控制电机的转速的。
• 然后还需要设定一个稳定的控制周期。控 制周期的意思就是每隔多长时间对姿态进 行一次控制。根据15的运行速度以及电机 的响应速度，我将控制周期设定在10ms， 每10ms控制一次。因此需要用定时器设定 一个10ms的中断。
从实践出发制作飞行器
前言：
我假定为学习本课程的同学都拥有一定的C语言基础， 并且学习过一定的单片机相关的知识，拥有一些单片机的 开发经验。课程当中 飞行器的开发主控是stc公司出品的 iap15w4k60s4芯片，这是一款基于51内核的单片机。选 择这款芯片的原因主要是为了让初学者更容易理解，底层 驱动更容易开发。当然如果你在开发领域已经是大神级， 那么你应该能够根据这节课程很容易的将程序移植到任意 一款你所喜欢的芯片当中。
我与飞行器相识的过程
讲述内容依赖的硬件