讲课PPT-修改后的

四旋翼飞行器

Name:岑汝平Email

：997576280@Contents

飞行器简介1飞行器的动力学分析和建模2飞行器姿态解算3四旋翼飞行器硬件系统456

PID&飞行器调试介绍数据处理微型飞行器：固定翼、旋翼式及直升机三种。

四旋翼飞行器在布局形式上属于旋翼的一种，相对于别的旋翼式飞行器来说四旋翼飞行器结构紧凑，能产生更大的升力，而且不需要专门的反扭矩桨保持飞行器扭矩平衡.飞行器简介飞行器简介贝斯索货运无人机航拍、侦查无人机飞行器用途：飞行器简介

2012年2月，宾夕法尼亚大学的VijayKumar教授在TED上做出了四旋翼飞行器发展历史上里程碑式的演讲。这一场充满数学公式的演讲居然大受欢迎，迄今已经有三百多万次观看，是TED成百上千个演讲中浏览量最高的演讲之一。PIX:ETHZurich实验室研发.ARMcortex_M4处理器.双GPS.Nuttx实时操作系统.配备地面站Lily:.采用防水结构.图像+GPS定位跟踪.图像软件去抖.实时1080P图传飞行器简介国内研究现状:

DJI（大疆公司）：S1000六旋翼飞行器-内置GPS导航功能，可以在户外很大的范围内飞行。S1000提供了挂载相机的连接架，让用相机的人们有了从天空向下的拍摄视角。特别地，与传统的飞机和直升机航拍不同，多旋翼系统小巧灵活，能让拍摄者自由地控制角度和距离.飞行器简介飞行器简介我们的目的是Contents

飞行器简介1飞行器的动力学分析和建模2飞行器姿态解算3四旋翼飞行器硬件系统456

PID&飞行器调试介绍数据处理四旋翼飞行器动力学分析及建模K为旋翼系数，ω为电机转速(3)重力G=m*g重力包括了四个机体支架、电池、四个电机和旋翼、主控板等受力分析(1)螺旋桨产生的升力(2)空气阻力C为阻力系数，S为旋翼面积，

为空气密度，

为螺旋桨旋转的速度四旋翼飞行器动力学分析及建模飞行器垂直爬升、下降（十字模型）四旋翼飞行器动力学分析及建模飞行器横滚状态（十字模型）四旋翼飞行器动力学分析及建模飞行器俯仰状态（十字模型）四旋翼飞行器动力学分析及建模

飞行偏航状态（十字模型）四旋翼飞行器动力学分析及建模

飞行横滚状态（X字模型）四旋翼飞行器动力学分析及建模

飞行俯仰状态（X字模型）四旋翼飞行器动力学分析及建模

飞行偏航状态（X字模型）四旋翼飞行器建模

建模分析KV900四旋翼飞行器建模

运动建模釆用牛顿-欧拉模型来进行飞行器运动的建模由牛顿第二定律,在地面坐标系下X,Y,Z三个方向上的直线运动方程为：四旋翼飞行器建模

忽略空气阻力,运动方程简化为:欧拉方程可知，在地面坐标系下的旋转运动方程为：假设以下控制量方程：为Z轴方向线运动的控制量为滚转角与X轴方向线运动的控制量为俯仰角与Y轴方向线运动的控制量为偏航角的控制量四旋翼飞行器建模

角度传递函数为:位置传递函数为:四旋翼飞行器的非线性模型为:线性化Contents

飞行器简介1飞行器的动力学分析和建模2飞行器姿态解算3四旋翼飞行器硬件系统456

PID&飞行器调试介绍数据处理四旋翼飞行器硬件系统

飞行器的动力来源螺旋桨的反作用力：整套系统的执行器包含螺旋桨、无刷电机、无刷驱动器、电池。规格型号：

螺旋桨规格，一般由4位数字表示，前两位数表示直径，后两位表示螺距。以1060浆为例，10表示桨的直径是10英寸，60表示浆角（螺距，6.0英寸，也就是152.4mm)。四旋翼飞行器硬件系统桨的衡量参数：桨的平衡性。

经验：桨当然是选择APC和DJI。大多航拍还是建议选择APC，是正品APC哦，仿的拿来试飞是个不错的选择，炸了也不心疼。APC效率高。大载重的可以选择碳纤维桨。载重很大的话可以选择榉木桨，不易变形。

四旋翼飞行器硬件系统1电机KV值：——大KV配小桨，小KV配大桨。KV值是每1V的电压下电机每分钟空转的转速，例如KV800,在1V的电压下空转转速是800转每分钟。2电机型号：——定子粗的，力气大。

电机型号，如2212，3508，4010，这些数字表示电机定子的直径和高度（如下图）。前面两位是定子直径，后面两位是定子高度，单位是毫米。

四旋翼飞行器硬件系统

经验一：整机重量，应该小于电机最大动力的2/5。经验二：选电机一定要看参数表，如果没有参数表，请不要选择。尽量选择大品牌的电机。四旋翼飞行器硬件系统电调主流品牌好盈，中特威。把电调当成变频器的：电调是在将输入的直流电，通过电调的6路开关管不断的在切换电流方向，输出三相交流电，但是换相的频率不是由电调决定的，因为电调的换相频率是由电机决定，电调换相频率必须和电机保持同步，超前的换相频率会使电机失步停转，滞后的换相频率会使电机的效率很低。所以电调并没有主动的在调整换相频率，而是被动的。

四旋翼飞行器硬件系统一般电调都有5V稳压输出（可以为飞控、遥控器直接提供电源）最大3A如果没5V输出的建议使用LM2575、LM2596最好不要使用7805电调驱动方式PWM（周期20MS占空比4.2%~9.8%）四旋翼飞行器硬件系统1、电池容量：

5200mAh，意味着以5.2A电流放电，可以放1小时。当然，我们只是这么理解。实际放电时间，需要参考电池厂家提供的相关技术参数。

2、放电能力：

30C电池，指的是电池的放电能力。对于30C电池，最大持续放电电流为：电池容量X放电C。

3、电池的分类：

2S（7.2V）~6S(22.2V)四旋翼飞行器硬件系统机架：

1、2kg以下的机子可以选玻纤机架、工程塑料；2kg以上的果断上3K碳纤维（价格有点贵）。2、自制机架（3MM铝板+碳管管夹）。

3、机架轴距=（桨的英寸*25.4/0.8/）*2。桨的尺寸（英寸）=（机架轴距/2）*4、一般机架尺寸：450、550、650mm四旋翼飞行器硬件系统自行设计与加工的飞行器机架四旋翼飞行器硬件系统自行设计与加工的飞行器机架四旋翼飞行器硬件系统也可直接购买F450这种机架（工程塑料）实惠！！！四旋翼飞行器硬件系统碳素机架：优点价格高、重量轻、强度高四旋翼飞行器硬件系统 MPU6050

、HMC5883提供三个轴度上的加速度、角速度、方位数据组成九轴演算

(输出数据精度可达0.1°)HMC5883(电子罗盘)MPU6050陀螺仪+加速度计四旋翼飞行器硬件系统1、陀螺仪输出的是角速度角速度直接积分不就是角度了吗？为什么需要这么多的东西那？2、加速度用来做什么？3、罗盘又用来做什么？4、干嘛需要用地坐标进行描述飞行器？四旋翼飞行器硬件系统

陀螺仪是对震动敏感，也就是是说在短时间的情况下陀螺仪积分出来是很准确的。但是由于角度的变化或是时间的关系会影响陀螺仪的准确性此时我们需要利用加速度、罗盘主要是用来修正陀螺仪。罗盘主要是确定地理坐标四旋翼飞行器硬件系统BMP180：为飞行器提供高度、温度信息。超声波：在近地面的时候可以提供更准确的信息BMP180气压计超声波模块四旋翼飞行器硬件系统飞行器控制核心最好采用ARM内核的单片机、DSP。市面上的飞控一般是STM32的cortex-M3、

cortex-M4内核(带DSP浮点单元)、飞思卡尔MK60等。

四旋翼飞行器硬件系统

选用处理能力强的单片机的原因：

1、计算四元数的过程中为了保证精度需要大量的浮点运算、三角运算。2、一般要保证将飞行器的控制周期确定在10msz左右（常用的控制系统保证控制周期7-15ms都可以的）

建议：在不添加操作系统的情况下最好是开定时器去控制，在while(1)里面存放数据回传、接收的函数块。这样既能保证数据传输又不会影响控制进程。四旋翼飞行器硬件系统飞行器硬件框图Contents

飞行器简介1飞行器的动力学分析和建模2飞行器姿态解算3四旋翼飞行器硬件系统456

PID&飞行器调试介绍数据处理飞行器姿态解算（1）地面坐标系（OEXEYEZE）地理坐标系一般选用地轴系。原点O设在地面上的某一点，OEXE轴指向正北方向，OEYE轴指向正东方向，，OEZE轴与XEOEYE平面垂直，方向为垂直向上。（2）机体坐标系（OXYZ）机体坐标系是固定在飞行器身上并与之一起移动动态坐标系。原点O为飞行器重心，OX轴与机身轴线平行并规定指向机头方向为正方向，相对OX轴顺时针旋转90°为OY轴，OZ轴垂直XOY平面向上。（MWC是使用的机体坐标系）飞行器姿态解算地面坐标系：当我们的飞机头朝北水平放置时载体坐标系和参考坐标系是重合的，那么接下来我们绕飞机的Z轴旋转30°，这个旋转的欧拉角就是我们所说的Yaw，绕飞机的Y轴旋转30°我们得到Pitch，绕飞机X轴旋转得到Roll。飞行器姿态解算欧拉角是基于飞机本身轴旋转得到的，但是得到的姿态却是相对于地面的参考坐标系而说的。飞行器姿态解算1、演示姿态2、讲解代码3、讲解四元数理论

飞行器姿态解算四元素转欧拉角

欧拉角使用roll，pitch，yaw三个值来分别表示绕(世界的)x轴、y轴、z轴旋转的角度量值。其取值是在[0,360]间。

飞行器姿态用四元数表示为Q0，Q1，Q2，Q3，其中Q0表示一个旋转角度（后面有描述），Q1，Q2，Q3表示的是一个空间向量，就是飞行器从姿态原点（物体的三个欧拉角都是0的姿态），围绕向量（Q1，Q2，Q3）旋转一个角度f(Q0)。

飞行器姿态解算

向量a×向量b得到的也是一个向量，向量方向和ab都垂直，大小等于ab组成的平行四边形面积即：向量a×向量b=|a||b|sinθ在这里θ表示两向量之间的角夹角（0°≤θ≤180°）。知识回顾：飞行器姿态解算四元数相关定理定理1：令p为三维（投影）空间内的一个点，用齐次坐标将其表示成四元数的形式即为：

p=(x:y:z:w)=[(x,y,z),w]=[v,w]；

令q为任一非零四元数。那么：

结论1)

表达式

qpq-1的结果将使p=[v,w]变换到p`=[v`,w]，二者模长相等。

结论2)

任何非零实数与q相乘，上式仍然成立。

结论3)

如果上式中的q为N(q)=1（即q为单位四元数），那么q=[

v

sinΩ,cosΩ]表示一个旋转动作：将p沿着单位轴v

旋转2Ω即得到p'。

性质：四元素四个数的平方和等于1。飞行器姿态解算

根据四元素第一个结论可得：

这里Q是四元数Q*是q的共轭，就是把ijk的系数取相反数。r是旋转前的向量，r‘就是旋转以后的向量。飞行器姿态解算

上述黄色这个式子得到:

在载体系定义一个矢量

，设其在参考系中表示为

。则有

，其中

，

为共轭复数。飞行器姿态解算

用矩阵来表达成四元素旋转矩阵：用这个矩阵左乘需要旋转的向量，就可以得到旋转后的向量了。这里的旋转，指的是同一个坐标系下，一个向量旋转的到另一个向量。可是我们时间的情况是同一个向量，在两个坐标系下的不同表达。飞行器姿态解算四元数微分方程：四元数微分方程本来只是基于角速度的，也就是说，已知上个周期的姿态，和本次测量得到的角速度，得到本周期的姿态，在角速度里加入误差反馈，来调节姿态，起到减小误差的作用。飞行器姿态解算四元数微分方程：分别表示载体坐标系相对于地理坐标系沿各个轴向的角度分量。飞行器姿态解算四元素与欧拉角的关系

(、θ、ψ为欧拉角a、b、c、d为四元素q0、q1、q2、q3)

当我们利用四元数表示出飞行器修正后的“姿态”时，就可以反向计算出欧拉角。飞行器姿态解算飞行器姿态角求解过程：

1>把机体上的罗盘数据通过四元数旋转矩阵转化到地坐标系进行标定。

2>将标准的重力、以及标定后的地磁矢量转换回机体坐标系。3>将步骤2中的矢量与加速度矢量进行比较，计算误差。 4>利用四元数微分方程将误差迭代带陀螺仪数据中并修正旋转方程。 5>规范四元数。 6>转换成欧拉角表示。飞行器姿态解算姿态修正过程：

1、将机体坐标系的电子罗盘测到的矢量转成地理坐标系下的磁场矢量hxyz（测量值）然后令其在Y方向为0、X方向大小等于磁场矢量在hxy平面上的投影长。得到标定后的矢量bxyz。

思考：为什么要把X、Y轴这样标定？飞行器姿态解算姿态修正过程：

我们知道重力在地面坐标系下向量是（0,0,1），可是地磁我们并不清楚，我们只能假设地面坐标系x轴方向朝北，这样的话y方向地磁是0，认为地面坐标系下，z方向就是该向量z方向的分量，y方向是0，x方向是该向量xy平面上的投影长。接下来的事就和处理重力向量一样了。变换回机体坐标系，再和实际测量的地磁求误差。飞行器姿态解算hx=2*mx*(0.5f-q2q2-q3q3)+2*my*(q1q2-q0q3)+ 2*mz*(q1q3+q0q2);hy=2*mx*(q1q2+q0q3)+2*my*(0.5f-q1q1-q3q3)+ 2*mz*(q2q3-q0q1);hz=2*mx*(q1q3-q0q2)+2*my*(q2q3+q0q1)+ 2*mz*(0.5f-q1q1-q2q2);bx=sqrt((hx*hx)+(hy*hy));by=0;//人为的标定为0bz=hz;

飞行器姿态解算姿态修正过程：

2、将地理坐标系的标准重力向量(0,0,g)及标定以后的磁场矢量分别左乘上四元素旋转矩阵，转到机体坐标系。

vx=2*(q1q3-q0q2);

vy=2*(q0q1+q2q3);

vz=q0q0-q1q1-q2q2+q3q3;wx=2*bx*(0.5-q2q2-q3q3)+2*bz*(q1q3-q0q2);wy=2*bx*(q1q2-q0q3)+2*bz*(q0q1+q2q3);wz=2*bx*(q0q2+q1q3)+2*bz*(0.5-q1q1-q2q2);

飞行器姿态解算姿态修正过程：

3、将加速度计测出来的重力向量和参考矢量做叉积，磁场的测量矢量和参考矢量也做叉积。都用来修正陀螺。

ex=(ay*vz-az*vy)+(my*wz-mz*wy);

ey=(az*vx-ax*vz)+(mz*wx-mx*wz);

ez=(ax*vy-ay*vx)+(mx*wy-my*wx);

飞行器姿态解算姿态修正过程：

4、由于叉积向量大小与陀螺积分误差成正比，因此在误差上乘上一个系数叠加到陀螺仪数据上。 exInt=exInt+ex*Ki*halfT; eyInt=eyInt+ey*Ki*halfT; ezInt=ezInt+ez*Ki*halfT; gx=gx+Kp*ex+exInt; gy=gy+Kp*ey+eyInt; gz=gz+Kp*ez+ezInt;

飞行器姿态解算姿态修正过程：

6、归一化处理： norm=invSqrt(q0*q0+q1*q1+q2*q2+q3*q3);

q0=q0*norm;

q1=q1*norm;

q2=q2*norm;

q3=q3*norm;飞行器姿态解算姿态修正过程：

5、计算四元数的微分方程、修正旋转矩阵： q0=q0+(-q1*gx-q2*gy-q3*gz)*halfT; q1=q1+(q0*gx+q2*gz-q3*gy)*halfT; q2=q2+(q0*gy-q1*gz+q3*gx)*halfT; q3=q3+(q0*gz+q1*gy-q2*gx)*halfT;飞行器姿态解算姿态修正过程：

7、最后将四元数转化成欧拉角进行表示Q_ANGLE.Z=-atan2(2*q1*q2+2*q0*q3,-2*q2*q2- 2*q3*q3+1)*57.3;//yawQ_ANGLE.Y=-asin(-2*q1*q3+2*q0*q2)*57.3;//pitchQ_ANGLE.X=atan2(2*q2*q3+2*q0*q1,-2*q1*q1 -2*q2*q2+1)*57.3;//roll

飞行器姿态解算

我们刚开始随便假设的四元数，已经表征了两个坐标系之间的旋转关系了，只不过这个旋转矩阵跟真实的矩阵相比误差很大，我们假设这个矩阵是正确的，然后得到两个坐标下同一个向量的差别，用这个差别来纠正这个矩阵。这是这个算法的根本所在。

飞行器姿态解算说具体一点，我们知道重力在地理坐标系下向量是（0,0，g），如果我们的矩阵是准确的，那么用这个矩阵将（0,0，g）转换到机体坐标，那我们应该得到在机体坐标系下的重力的向量，如果矩阵很准确，那我们从加速度计读取的数值应该也是这个向量，可是哪能那么巧，所以呢我们读取加速度计得到重力向量在机体坐标系下的向量，这是测量值，这个值和刚刚的理论值有差别，用这个差别来纠正旋转矩阵。程序一遍一遍的跑，这个误差就会一点点减小了。飞行器姿态解算MWC_IMU代码解读

i2c_writeReg(MPU6050_ADDRESS,0x1C,0x10);//ACCEL_CONFIG+/-8G;i2c_writeReg(MPU6050_ADDRESS,0x1B,0x18);//GYRO_CONFIG2000deg/secMWC中加速度计量程为+/-8G陀螺仪量程为2000deg/sec飞行器姿态解算陀螺仪读取函数voidGyro_getADC(){

i2c_getSixRawADC(MPU6050_ADDRESS,0x43);

GYRO_ORIENTATION(((rawADC[0]<<8)|rawADC[1])>>2, ((rawADC[2]<<8)|rawADC[3])>>2,

((rawADC[4]<<8)|rawADC[5])>>2);

GYRO_Common();}飞行器姿态解算加速度读取及处理函数：voidACC_getADC(){

i2c_getSixRawADC(MPU6050_ADDRESS,0x3B);

ACC_ORIENTATION(((rawADC[0]<<8)|rawADC[1])>>3,

((rawADC[2]<<8)|rawADC[3])>>3,

((rawADC[4]<<8)|rawADC[5])>>3);}

加速度数据左移了三位陀螺仪数据左移了两位飞行器姿态解算还记得我们校准加速度记的时候输出的数据为什么是0,0,512吗？？？8G的量程对应16位有符号数32768，则1g对应的数值应该是4096，又因为MWC在采集数据以后进行了右移处理（右移3位相当于除以8）所以最后1g对应512飞行器姿态解算voidrotateV(structfp_vector*v,float*delta){fp_vectorv_tmp=*v;v->Z-=delta[ROLL]*v_tmp.X+delta[PITCH]*v_tmp.Y;v->X+=delta[ROLL]*v_tmp.Z-delta[YAW]*v_tmp.Y;v->Y+=delta[PITCH]*v_tmp.Z+delta[YAW]*v_tmp.X;}飞行器姿态解算绕X轴旋转θ飞行器姿态解算绕Y轴旋转γ飞行器姿态解算绕Z轴旋转Ψ飞行器姿态解算一种旋转绕Z轴旋转Ψ=yaw->绕X轴旋转θ=pitch->绕Y轴旋转γ=roll余弦旋转矩阵飞行器姿态解算角增量输出飞行器姿态解算飞行器姿态解算

int32_tsqGZ=sq(EstG32.V.Z); int32_tsqGX=sq(EstG32.V.X); int32_tsqGY=sq(EstG32.V.Y); int32_tsqGX_sqGZ=sqGX+sqGZ; floatinvmagXZ=InvSqrt(sqGX_sqGZ); invG=InvSqrt(sqGX_sqGZ+sqGY);

angle[ROLL]=_atan2(EstG32.V.X,EstG32.V.Z); angle[PITCH]=_atan2(EstG32.V.Y,invmagXZ*sqGX_sqGZ);飞行器姿态解算Contents

飞行器简介1飞行器的动力学分析和建模2飞行器姿态解算3四旋翼飞行器硬件系统456

PID&飞行器调试介绍数据处理数据滤波传感器输出数据中噪声的存在（螺旋桨切割空气时所产生的高频振动引起的）。数据滤波常用的滤波算法：

1、均值滤波

2、滑动窗口滤波（滑动平均滤波法）

3、限幅滤波法

4、中值滤波法说明：把连续N个采样值看成一个队列，队列长度固定为N。每次采样到一个新数据放入队尾，并扔掉队首的一次数据。把队列中的N各数据进行平均运算，既获得新的滤波结果。

优点：对周期性干扰有良好的抑制作用，平滑度高；试用于高频振荡的系统数据滤波#defineN12//队列长度unsignedcharvalue_buf[N];unsignedcharfilter(){

unsignedcharI,value;

intsum=0;

value_buf[i++]=get_ad();

//采集到的数据放入最高位

for(i=0;i<N;i++)

{

value_buf[i]=value_buf[i+1];

//所有数据左移，低位扔掉

sum+=value_buf[i];

}

value=sum/N;

return(value);

}数据滤波MWC原型机的滤波方法：当前的测量值占3/4的比重、上一个的数据占1/4的比重。对数据进行滤波。数据滤波加速度、罗盘错误的校准方式造成的结果！！数据滤波加速度、罗盘校准方式数据滤波陀螺仪校准方式将陀螺仪放置与静止状态是采集陀螺仪的零偏值。数据滤波陀螺仪标度变换陀螺仪量程为2000dec/秒

对应16位有符号数32768数据滤波加速度、罗盘需要标度变换需要吗？不需要！！Contents

飞行器简介1飞行器的动力学分析和建模2飞行器姿态解算3四旋翼飞行器硬件系统456

PID&飞行器调试介绍数据处理PID

普通PID一般有两种：位置式PID、增量式PIDPIDCounter_KI_Out[0]+=Counter_Ki*Counter_Err[0];//累计误差if(Counter_KI_Out[0]>450)//限制积分幅度

Counter_KI_Out[0]=450;elseif(Counter_KI_Out[0]<-450)

Counter_KI_Out[0]=-450;Counter_PID_Output[0]=

Counter_Kp*Counter_Err[0]+ Counter_KI_Out[0]+ Counter_Kd*MPU6050_Data[0];PID个人经验：

1、先调节P（D为零）注意分清是超调还是P参数弱了。

2、增加D参数使得系统收敛得到一个稳定的状态。

3、继续增加D使得系统“抖动”然后在继续增加P参数这样反复几次，最终系统可以在外力作用下保持稳定就可以。

4、尽量使用串口助手调试串级PID1、得到轴姿态的角度差（angle

error），将这个值乘以角度系数p后限幅（限幅必须有，否则剧烈打舵时容易引发震荡）作为角速度控制器期望值target_rate。2、target_rate与陀螺仪得到的当前角速度作差，得到角速度误差（rate_error）乘以kp得到P。串级PID3、在I值小于限幅值（大概在5%油门）或者rate_error与i值异号时将rate_error累加到I中。4、前后两次rate_error的差作为D项，值得注意的是加需要入20hz（也可以采用其它合适频率）滤波，以避免震荡。将P,I,D三者相加并限幅（50%油门）得到最终PID输出。PID参数映射到模型上ＰＩＤ参数映射到电机PID参数映射到模型上飞行器调试方法飞行器调试方法谢谢!!!

MATLAB仿真

（1）比例环节P：控制器中的比例环节用来纠正系统偏差，比例项能够快速的响应偏差，但是不能消除系统的稳态误差，比例过大还会引起系统的不稳定；（2）积分环节I：积分项用于消除系