讲课PPT_修改后

1、LOGO四旋四旋翼飞行器翼飞行器 Name: 岑汝平岑汝平 Email ：ContentsContents123456微型微型飞行器飞行器：固定翼固定翼、旋旋翼翼式式及及直升机直升机三种三种。 四旋翼飞行器在布局形四旋翼飞行器在布局形式上属于旋翼的一种，相式上属于旋翼的一种，相对于别的旋翼式飞行器来对于别的旋翼式飞行器来说四旋翼飞行器结构紧凑，说四旋翼飞行器结构紧凑，能产生更大的升力，而且能产生更大的升力，而且不需要专门的反扭矩桨保不需要专门的反扭矩桨保持飞行器扭矩持飞行器扭矩平衡平衡.飞行器简介飞行器简介飞行器飞行器简介简介贝斯贝斯索索 货运无人机货运无人机航拍、侦查无人机航拍、侦查无人机飞

2、行器飞行器用途：用途：飞行器简介飞行器简介 20122012年年2 2月，宾夕法月，宾夕法尼亚大学尼亚大学的的VijayKumarVijayKumar教授在教授在TEDTED上做出了四旋翼上做出了四旋翼飞行器发展历史上飞行器发展历史上里程碑式的演讲。里程碑式的演讲。这一场充满数学公这一场充满数学公式的演讲居然大受式的演讲居然大受欢迎，迄今已经有欢迎，迄今已经有三百多万次观看，三百多万次观看，是是TEDTED成百上千个演成百上千个演讲中浏览量最高的讲中浏览量最高的演讲之一。演讲之一。PIXPIX:ETH Zurich :ETH Zurich 实验室研发实验室研发. ARM cortex_M4.

3、ARM cortex_M4处理器处理器. . 双双GPS GPS . Nuttx. Nuttx实时操作系统实时操作系统. . 配备地面站配备地面站LilyLily: :. .采用防水结构采用防水结构. .图像图像+GPS+GPS定位跟踪定位跟踪. .图像软件去抖图像软件去抖. .实时实时1080P1080P图传图传飞行器简介飞行器简介国内研究现状国内研究现状: : DJIDJI（大疆公司）（大疆公司）：S1000S1000六旋翼飞行器六旋翼飞行器- -内置内置GPSGPS导导航功能，可以在户外很大的范围内飞行。航功能，可以在户外很大的范围内飞行。S1000S1000提供了挂提供了挂载相机的连接

4、架，让用相机的人们有了从天空向下的拍载相机的连接架，让用相机的人们有了从天空向下的拍摄视角。特别地，与传统的飞机和直升机航拍不同，多摄视角。特别地，与传统的飞机和直升机航拍不同，多旋翼系统小巧灵活，能让拍摄者自由地控制角度和距离旋翼系统小巧灵活，能让拍摄者自由地控制角度和距离. .飞行器简介飞行器简介飞行器简介飞行器简介我们的目的是我们的目的是ContentsContents123456四旋翼飞行器动力学分析及建模四旋翼飞行器动力学分析及建模K为旋翼系数，为电机转速G=m*g重力包括了四个机体支架、电池、四个电机和旋翼、主控板等 C为阻力系数，S为旋翼面积， 为空气密度， 为螺旋桨旋转的速度x

5、V四旋翼飞行器动力学分析及建模四旋翼飞行器动力学分析及建模四旋翼飞行器动力学分析及建模四旋翼飞行器动力学分析及建模四旋翼飞行器动力学分析及建模四旋翼飞行器动力学分析及建模四旋翼飞行器动力学分析及建模四旋翼飞行器动力学分析及建模四旋翼飞行器动力学分析及建模四旋翼飞行器动力学分析及建模四旋翼飞行器动力学分析及建模四旋翼飞行器动力学分析及建模四旋翼飞行器动力学分析及建模四旋翼飞行器动力学分析及建模KV90022222+2 d26412xmrmhMRMHIm22z42MRImd123xyzFKxxmFKyymFmgKzzm四旋翼飞行器建模四旋翼飞行器建模xyzFxmFymFm gzm42311324(

6、FF ) d(FF) dxyzIIMMMMI1123424233141324uFFFFuFFuFFuFFFF为Z 轴方向线运动的控制量为滚转角与X轴方向线运动的控制量 为俯仰角与Y轴方向线运动的控制量为偏航角的控制量1u2u3u4u111234(sincoscossinsin )(sinsincoscossin )coscosxyzuxmuymumyzmu dIu dIuI角度传递函数为: 位置传递函数为: 30.905s9.7986sContentsContents123456四旋翼四旋翼飞行器硬件飞行器硬件系统系统 飞行器的动力来源螺旋桨的反作用力：整套系统的执行器包含 螺旋桨、无刷电机、

7、无刷驱动器、电池。规格型号： 螺旋桨规格，一般由4位数字表示，前两位数表示直径，后两位表示螺距。以1060浆为例，10表示桨的直径是10英寸，60表示浆角（螺距，6.0英寸，也就是152.4mm)。四旋翼四旋翼飞行器硬件飞行器硬件系统系统桨的衡量参数：桨的平衡性。经验：桨当然是选择APC和DJI。大多航拍还是建议选择APC，是正品APC哦，仿的拿来试飞是个不错的选择，炸了也不心疼。APC效率高。大载重的可以选择碳纤维桨。载重很大的话可以选择榉木桨，不易变形。 四旋翼四旋翼飞行器硬件系统飞行器硬件系统1 电机KV值：大KV配小桨，小KV配大桨。KV值是每1V的电压下电机每分钟空转的转速，例如KV

8、800,在1V的电压下空转转速是800转每分钟。2 电机型号：定子粗的，力气大。电机型号，如2212，3508，4010，这些数字表示电机定子的直径和高度（如下图）。前面两位是定子直径，后面两位是定子高度，单位是毫米。四旋翼四旋翼飞行器硬件系统飞行器硬件系统经验一：整机重量，应该小于电机最大动力的2/5。经验二：选电机一定要看参数表，如果没有参数表，请不要选择。尽量选择大品牌的电机。四旋翼四旋翼飞行器硬件系统飞行器硬件系统电调主流品牌好盈，中特威。 把电调当成变频器的：电调是在将输入的直流电，通过电调的6路开关管不断的在切换电流方向，输出三相交流电，但是换相的频率不是由电调决定的，因为电调的换

9、相频率是由电机决定，电调换相频率必须和电机保持同步，超前的换相频率会使电机失步停转，滞后的换相频率会使电机的效率很低。所以电调并没有主动的在调整换相频率，而是被动的。 四旋翼四旋翼飞行器硬件系统飞行器硬件系统1.一般电调都有5V稳压输出（可以为飞控、遥控器直接提供电源）最大3A2.如果没5V输出的 建议使用LM2575 、 LM2596 最好不要使用78053.电调驱动方式 PWM（周期20MS 占空比4.2%9.8%）四旋翼四旋翼飞行器硬件系统飞行器硬件系统1、电池容量： 5200mAh，意味着以5.2A电流放电，可以放1小时。当然，我们只是这么理解。实际放电时间，需要参考电池厂家提供的相关

10、技术参数。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、650mm22*0.8/ 25.4四旋翼四旋翼飞行器硬件系统飞行器硬件系统自行设计与加工的飞行器自行设计与加工的飞行器机架

11、机架四旋翼四旋翼飞行器硬件系统飞行器硬件系统自行设计与加工的飞行器机架自行设计与加工的飞行器机架四旋翼四旋翼飞行器硬件系统飞行器硬件系统也可直接购买也可直接购买F450这种机架（工程塑料）这种机架（工程塑料）实惠！实惠！四旋翼四旋翼飞行器硬件系统飞行器硬件系统碳素机架：优点碳素机架：优点 价格高、重量轻、强度高价格高、重量轻、强度高四旋翼四旋翼飞行器硬件系统飞行器硬件系统MPU6050 、HMC5883提供三个轴度上的加速提供三个轴度上的加速度、角速度、方位数据组成九轴演算度、角速度、方位数据组成九轴演算 (输出数据精度输出数据精度可达可达0.1) HMC5883(电子罗盘电子罗盘)MPU60

12、50 陀螺陀螺仪仪+加速度计加速度计四旋翼四旋翼飞行器硬件系统飞行器硬件系统1 1、陀螺仪陀螺仪输出的是角速度输出的是角速度 角速度直接积分不就是角速度直接积分不就是角度了吗？角度了吗？为什么需要这么多的东西那为什么需要这么多的东西那？2 2、加速度加速度用来做什么？用来做什么？3 3、罗盘罗盘又用来做什么？又用来做什么？4 4、干嘛需要用地坐标进行描述飞行器？、干嘛需要用地坐标进行描述飞行器？四旋翼四旋翼飞行器硬件系统飞行器硬件系统 陀螺仪是对陀螺仪是对震动敏感震动敏感，也就是是说在，也就是是说在短时短时间间的情况下陀螺仪积分出来是很准确的。但是的情况下陀螺仪积分出来是很准确的。但是由于角度

13、的变化或是时间的关系会影响陀螺仪由于角度的变化或是时间的关系会影响陀螺仪的准确性此时我们需要利用的准确性此时我们需要利用加速度、罗盘加速度、罗盘主要主要是用来是用来修正修正陀螺仪陀螺仪。罗盘主要是确定地理坐标。罗盘主要是确定地理坐标四旋翼四旋翼飞行器硬件系统飞行器硬件系统BMP180BMP180：为飞行器提供高度、温度信息。为飞行器提供高度、温度信息。超声波超声波：在近地面的时候可以提供更准确的信息：在近地面的时候可以提供更准确的信息BMP180气压计气压计超声波模块超声波模块四旋翼四旋翼飞行器硬件系统飞行器硬件系统 飞行器控制核心最好采用飞行器控制核心最好采用ARMARM内核的单片机、内核的

14、单片机、DSPDSP。市面。市面上的飞控一般是上的飞控一般是STM32STM32的的 cortex-M3cortex-M3、 cortex-M4cortex-M4内核内核( (带带DSPDSP浮点单元浮点单元) ) 、飞思卡尔、飞思卡尔MK60MK60等。等。 四旋翼四旋翼飞行器硬件系统飞行器硬件系统 选用处理能力强的单片机的选用处理能力强的单片机的原因原因： 1 1、计算四元数的过程中为了保证精度需要大量的浮点、计算四元数的过程中为了保证精度需要大量的浮点运算、三角运算。运算、三角运算。 2 2、一般要保证将飞行器的控制周期确定在、一般要保证将飞行器的控制周期确定在10msz10msz左右左

15、右（常用的控制系统保证控制周期（常用的控制系统保证控制周期7-15ms7-15ms都可以的）都可以的） 建议：建议：在不添加操作系统的情况下最好是开定时器去控制，在不添加操作系统的情况下最好是开定时器去控制，在在while(1)while(1)里面存放数据回传、接收的函数块。这样既能保里面存放数据回传、接收的函数块。这样既能保证数据传输又不会影响控制进程。证数据传输又不会影响控制进程。四旋翼四旋翼飞行器硬件系统飞行器硬件系统飞行器硬件框图飞行器硬件框图ContentsContents123456飞行器姿态解算飞行器姿态解算（1）地面坐标系（OEXEYEZE）地理坐标系一般选用地轴系。原点O设在

16、地面上的某一点，OEXE轴指向正北方向，OEYE轴指向正东方向，OEZE轴与XEOEYE平面垂直，方向为垂直向上。（2）机体坐标系（OXYZ）机体坐标系是固定在飞行器身上并与之一起移动动态坐标系。原点O为飞行器重心，OX轴与机身轴线平行并规定指向机头方向为正方向，相对OX轴顺时针旋转90为OY轴，OZ轴垂直XOY平面向上。（MWC是使用的机体坐标系）飞行器姿态解算飞行器姿态解算地面坐标系：当我们的飞机头朝北水平放置时载体坐标系和参考坐标系是重合的，那么接下来我们绕飞机的Z轴旋转30，这个旋转的欧拉角就是我们所说的Yaw，绕飞机的Y轴旋转30我们得到Pitch，绕飞机X轴旋转得到Roll。飞行器

17、姿态解算飞行器姿态解算欧拉角是基于飞机欧拉角是基于飞机本身轴旋转得到的，本身轴旋转得到的，但是得到的姿态却是但是得到的姿态却是相对于地面的参考坐相对于地面的参考坐标系而说的。标系而说的。飞行器姿态解算飞行器姿态解算1、演示、演示姿态姿态 2、讲解讲解代码代码3、讲解四元数讲解四元数理论理论 飞行器姿态解算飞行器姿态解算四元素转欧拉角四元素转欧拉角 欧拉角使用roll，pitch，yaw三个值来分别表示绕(世界的)x轴、y轴、z轴旋转的角度量值。其取值是在0, 360间。 飞行器姿态用四元数表示为Q0，Q1，Q2，Q3，其中Q0表示一个旋转角度（后面有描述）， Q1，Q2，Q3表示的是一个空间向

18、量，就是飞行器从姿态原点（物体的三个欧拉角都是0的姿态），围绕向量（ Q1，Q2，Q3 ）旋转一个角度f(Q0)。 飞行器姿态解算飞行器姿态解算 向量a a向量b b得到的也是一个向量，向量方向和ab都垂直，大小等于ab组成的平行四边形面积即：向量a a向量b b =|a a|b b|sin在这里表示两向量之间的角夹角（0 180）。知识回顾：知识回顾：飞行器姿态解算飞行器姿态解算四元数相关定理四元数相关定理定理定理1 1：令p为三维（投影）空间内的一个点，用齐次坐标将其表示成四元数的形式即为： p = (x:y:z:w) = (x, y, z), w = v, w ； 令q为任一非零四元数。

19、那么：结论结论1)1) 表达式 qpq-1的结果将使p=v v, w变换到p=vv, w， 二者模长相等。结论结论2)2) 任何非零实数与q相乘，上式仍然成立。结论结论3)3) 如果上式中的q为N(q) = 1（即q为单位四元数），那么q = v v sin , cos 表示一个旋转动作：将p沿着单位轴v v 旋转2即得到p。性质：性质：四元素四个数的平方和等于1。 飞行器姿态解算飞行器姿态解算 根据四元素第一个结论可得：根据四元素第一个结论可得： 这里这里Q Q是四元数是四元数Q Q* *是是q q的共轭，就是把的共轭，就是把ijkijk的系数取相反数。的系数取相反数。r r是旋转前的向量，

20、是旋转前的向量，r r就就是旋转以后的向量。是旋转以后的向量。飞行器姿态解算飞行器姿态解算 上述上述黄色黄色这个这个式子式子得到得到: : 在载体系定义一个矢量 ，设其在参考系中表示为 。则有 ，其中 ， 为共轭复数。*222222222222()()()0 ()2()2() 2()()2() 2()2()() nbrqr qabicjdkxiyjzk abicjdkabcdxbcad ybdac z ibcad xabcdycsab z jbdac xcdab yabcdz kbrxiyjzk*qnr*nbrqr qqabicjdk飞行器姿态解算飞行器姿态解算 用用矩阵来表达矩阵来表达成成四

21、元素旋转矩阵四元素旋转矩阵：用用这个这个矩阵左乘矩阵左乘需要旋转的向量，就需要旋转的向量，就可以得到旋可以得到旋转后的向量了转后的向量了。这里。这里的的旋转，指的是同一个坐标旋转，指的是同一个坐标系下，一个向量旋转的到另一个系下，一个向量旋转的到另一个向量向量。可是我们可是我们时间的情况时间的情况是是同一个向量，在两个坐标系下的不同一个向量，在两个坐标系下的不同同表达表达。飞行器姿态解算飞行器姿态解算四元数四元数微分方程微分方程：四元数微分方程本来只是基于四元数微分方程本来只是基于角速度角速度的，也就是的，也就是说，已知上个周期的姿态，和本次测量得到的角速说，已知上个周期的姿态，和本次测量得到

22、的角速度，得到本周期的姿态，在角速度里加入误差反馈，度，得到本周期的姿态，在角速度里加入误差反馈，来调节姿态，起到减小误差的来调节姿态，起到减小误差的作用作用。飞行器姿态解算飞行器姿态解算四元数四元数微分方程微分方程：00112233001020bbbxyzbbbxzybbbyzxbbbzyxqqqqqqqq 分别分别表示载体坐标系相对于地理坐标系表示载体坐标系相对于地理坐标系沿沿各个各个轴向的角度分量。轴向的角度分量。 ,bbbxyz飞行器姿态解算飞行器姿态解算四元素与欧拉角的关系四元素与欧拉角的关系 ( ( 、 为欧拉角为欧拉角 a、b、c、d 为四元素为四元素 q0、 q1、q2、q3)

23、 当我们利用四元数表示出飞行器修正后的当我们利用四元数表示出飞行器修正后的“姿态姿态”时，就可以反向计算出欧拉角。时，就可以反向计算出欧拉角。飞行器姿态解算飞行器姿态解算飞行器姿态角求解过程：飞行器姿态角求解过程： 1 1 把机体上的罗盘数据通过四元数把机体上的罗盘数据通过四元数旋转矩阵转化到地坐标系进行标定。旋转矩阵转化到地坐标系进行标定。 2 2 将标准的重力、以及标定后的地将标准的重力、以及标定后的地磁矢量转换回机体坐标系。磁矢量转换回机体坐标系。 3 3将步骤将步骤2 2中的矢量与加速度矢量进中的矢量与加速度矢量进行比较，计算误差。行比较，计算误差。44利用四元数微分方程将误差迭代带利

24、用四元数微分方程将误差迭代带陀螺仪数据中并修正旋转方程。陀螺仪数据中并修正旋转方程。55规范四元数。规范四元数。66转换成欧拉角表示。转换成欧拉角表示。飞行器姿态解算飞行器姿态解算姿态修正过程：姿态修正过程： 1、将机体坐标系的电子罗盘测到的矢量转成地理坐标系下的磁场矢量hxyz（测量值）然后令其在Y方向为0、X方向大小等于磁场矢量在hxy平面上的投影长。得到标定后的矢量bxyz。 思考：为什么要把X、Y轴这样标定？飞行器姿态解算飞行器姿态解算姿态修正过程：姿态修正过程： 我们知道重力在地面坐标系下向量是（0,0,1），可是地磁我们并不清楚，我们只能假设地面坐标系x轴方向朝北，这样的话y方向地

25、磁是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*m

26、z*(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 -

27、 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); 飞行器姿态解算飞行器姿态解算姿态修正过程

28、：姿态修正过程： 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

29、*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;00112233001020bbbxyzbbbxzyb

30、bbyzxbbbzyxqqqqqqqq飞行器姿态解算飞行器姿态解算姿态修正过程：姿态修正过程： 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

31、 飞行器姿态解算飞行器姿态解算 我们刚开始随便假设的四元数，已经表征了两个坐标系之间的旋转关系了，只不过这个旋转矩阵跟真实的矩阵相比误差很大，我们假设这个矩阵是正确的，然后得到两个坐标下同一个向量的差别，用这个差别来纠正这个矩阵。这是这个算法的根本所在。 飞行器姿态解算飞行器姿态解算说具体一点，我们知道重力在地理坐标系下向量是（0,0，g），如果我们的矩阵是准确的，那么用这个矩阵将（0,0，g）转换到机体坐标，那我们应该得到在机体坐标系下的重力的向量，如果矩阵很准确，那我们从加速度计读取的数值应该也是这个向量，可是哪能那么巧，所以呢我们读取加速度计得到重力向量在机体坐标系下的向量，这是测量值，

32、这个值和刚刚的理论值有差别，用这个差别来纠正旋转矩阵。程序一遍一遍的跑，这个误差就会一点点减小了。飞行器姿态解算飞行器姿态解算MWC_ IMU代码解读代码解读 i2c_writeReg(MPU6050_ADDRESS, 0 x1C, 0 x10); /ACCEL_CONFIG +/-8G ;i2c_writeReg(MPU6050_ADDRESS, 0 x1B, 0 x18); /GYRO_CONFIG 2000 deg/secMWC 中加速度计量程为+/-8G 陀螺仪量程为2000 deg/sec 飞行器姿态解算飞行器姿态解算陀螺仪读取函数陀螺仪读取函数void Gyro_getADC ()

33、 i2c_getSixRawADC(MPU6050_ADDRESS, 0 x43); GYRO_ORIENTATION( (rawADC02 , (rawADC22 , (rawADC42 ); GYRO_Common();飞行器姿态解算飞行器姿态解算加速度读取及处理函数：加速度读取及处理函数：void ACC_getADC () i2c_getSixRawADC(MPU6050_ADDRESS, 0 x3B); ACC_ORIENTATION( (rawADC03 , (rawADC23 , (rawADC43 ); 加速度加速度数据左移了三位数据左移了三位 陀螺仪陀螺仪数据左移了两位数据左

34、移了两位飞行器姿态解算飞行器姿态解算 还记得我们校准加速度记的时还记得我们校准加速度记的时候输出的数据为什么是候输出的数据为什么是0,0,512 吗？吗？8G的量程对应的量程对应16位有符号数位有符号数32768，则，则1g对应的对应的数值应该是数值应该是4096，又因为，又因为MWC在采集数据以后进在采集数据以后进行了右移处理（右移行了右移处理（右移3位位 相当于除以相当于除以8）所以最后）所以最后1g 对应对应512飞行器姿态解算飞行器姿态解算void rotateV(struct fp_vector *v,float* delta) fp_vector v_tmp = *v; v-Z -

35、= deltaROLL * v_tmp.X + deltaPITCH * v_tmp.Y; v-X += deltaROLL * v_tmp.Z - deltaYAW * v_tmp.Y; v-Y += deltaPITCH * v_tmp.Z + deltaYAW * v_tmp.X; 飞行器姿态解算飞行器姿态解算绕绕X轴旋转轴旋转飞行器姿态解算飞行器姿态解算绕绕Y轴旋转轴旋转飞行器姿态解算飞行器姿态解算绕绕Z轴旋转轴旋转飞行器姿态解算飞行器姿态解算一种旋转一种旋转 绕绕Z轴旋转轴旋转=yaw-绕绕X轴旋转轴旋转=pitch-绕绕Y轴轴旋转旋转=roll余弦旋转矩阵余弦旋转矩阵1112132

36、12223313233000zyzxyxCCCCCCCCCC飞行器姿态解算飞行器姿态解算000bibXbibYbibXbibZbibYbibZbib角增量输出角增量输出飞行器姿态解算飞行器姿态解算飞行器姿态解算飞行器姿态解算 int32_t sqGZ = sq(EstG32.V.Z); int32_t sqGX = sq(EstG32.V.X); int32_t sqGY = sq(EstG32.V.Y); int32_t sqGX_sqGZ = sqGX + sqGZ; float invmagXZ = InvSqrt(sqGX_sqGZ); invG = InvSqrt(sqGX_sqGZ

37、 + sqGY); angleROLL = _atan2(EstG32.V.X , EstG32.V.Z); anglePITCH = _atan2(EstG32.V.Y , invmagXZ*sqGX_sqGZ);飞行器姿态解算飞行器姿态解算ContentsContents123456数据滤波数据滤波传感器输出数据中噪声的存在（螺旋桨切割空气时所产生的高频振动引起的）。数据滤波数据滤波常用的滤波算法： 1、均值滤波 2、滑动窗口滤波（滑动平均滤波法） 3、限幅滤波法 4、中值滤波法说明：把连续N个采样值看成一个队列，队列长度固定为N。每次采样到一个新数据放入队尾，并扔掉队首的一次数据。把队列中的N各数据进行平均运算，既获得新的滤波结果。优点：对周期性干扰有良好的抑制作用，平滑度高；试用于高频振荡的系统数据滤波数据滤波#define N 12 /队列长度unsigned char value_bufN;unsigned char filter() unsigned char I, value; int sum=0; value_bufi+ = get_ad(); /采集到的数据放入最高位 for(i=0;i450)/限制积分幅度 Counter_KI_Out0=450;else if(Counter_KI_Out00