四轴飞行器课件(转)

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