毕业设计论文终极终极稿 三亿文库
毕业设计论文终极终极稿
湖南工业大学本科毕业设计（论文） 比例-积分（PI）控制、比例-积分-微分（PID）控制等。 6.2 PID控制思想 对于四旋翼飞行器姿态的控制有两种方式：姿态四元数控制和欧拉角控制。两种方法的基本思想相同：设定一个目标姿态，计算当前姿态与目标姿态的偏差，输入至PID控制器，使其输出相应的调节量至电子调速器，调整螺旋桨转速，达到目标姿态。不同点是控制的姿态数据不同，使得两者带给主控器的计算量不同。由于欧拉角要通过四元数反三角函数计算得来，所以，欧拉角控制的方法计算量较大。但这对于主频为72M的STM32主控器来说还是可以承受的。欧拉角控制易于理解，程序实现也较简单，所以采用欧拉角控制。 6.3 PID控制器程序实现 本设计采用位置式的PID算法控制。 设u(k)为PID控制器在第K次采样时刻的输出值。可以得到离散化的PID表达式如下： Kp*Tu（(k）?Kpe(k)?TI其中，令
Kp*Tde(j)?[e(k)?e(k?1)] ?Tj?0kKp*T= Ki即为积分系数。 TIKp*Td=Kd 即为微分系数。
T将此PID表达式转化为程序语言。 PID程序中定义单个float型变量： error_rol，error_pit，error_yaw分别接收实际ROLL，PITCH，YAW角与目标值的差值。 定义PID结构体： struct PID {float P，pout，I，iout，D，dout，IMAX，OUT；}PID；
定义PID型变量PID_ROL，PID_PIT，PID_YAW，分成三组PID，分别调节三个方向的欧拉角偏差。 这里要对微分项说明一点，由于微分项为D*[e(k)?e(k?1)]，而[e(k)?e(k?1)]的差值为角度，对角度的微分即为角速度。由于陀螺仪输出的直接就是角速度，因此，可以将微分项改为微分系数与陀螺仪相应轴的输出量。这里一定要注意，由于D是阻尼作用，所以，陀螺仪输出的数据符号必须为负，否则就变成增益而不是阻尼了。 以对横滚角ROLL的控制为例，程序流程图如下：
24 湖南工业大学本科毕业设计（论文） 开始输出当前值now与目标值tar计算偏差error=tar-nowError_i+=error计算P_out=P*error计算I_out=I*Error_i计算D_out=D*gryo.x计算PID_ROL.OUT=P_out + I_out + D_out结束图6.3
PID控制程序流程图
6.4 PID控制器参数整定 四旋翼飞行器的PID参数整定是一个比较复杂的过程，在调参的过程中，总会有这样那样的问题出现，这样更加使人对四旋翼飞行器的理解深刻。使人对问题的分析解决能力得到提升。 这里介绍一种用绳子绑住飞行器整定参数的方法。要注意绑绳子的方式跟飞行器是＋模式还是X模式有关。一般来讲，＋模式的飞行器相对好调一些，因为ROLL、PITCH方向只分别涉及到两个电机，容易调些。 本设计中的四旋翼飞行器设计为＋模式，在该模式下，当要调节ROLL方向时，只需用绳子绑住PITCH方向的两条机臂，必须保证ROLL方向机臂与水平面平行并可以灵活转动。调PITCH方向同理。 要了解PID控制中P、I、D三个参数的具体含义。P可以表示为回复力大小，它的目标是使飞行器回到水平面。D可以表示为阻尼力，它抑制P的作用但回不回得到水平面不是它关心的问题。I的作用是消除稳态误差，使飞行器最终稳定在水平面。 首先，令积分系数和微分系数都为0，从比例系数为很小的值调起。令P=1时，发现飞行器往一边偏，偏离水平面的角度较大。只有把油门推大一些，偏离角度才会稍微减小，这说明回复力不够，即P的值小了。把P的值加大到1.5，飞行器依然向一边偏，但是偏离水平面的角度较之前变小了。继续将P值加大到3.5，发现飞行器开始左右震荡，幅度越震越大，说明P过大了。所以P值应该在1.5~3.5之间。经过一次次尝试，发现P的值为3.1的时候，飞行器左右等幅25 湖南工业大学本科毕业设计（论文） 震荡，至此，认为P已经调好了。 接下来调D。D是具有阻尼作用的，希望D的加入能够消除P引起的震荡。同样的，从一个很小的值调起。令D值为0.001，发现基本没有什么效果。增加D的值至0.05，效果不太明显，说明还小了。当D增加到0.15时，发现飞行器的震荡更加剧烈了，说明D 的值过大了，经过耐心的试参数，最终确定，D=0.096。 根据自己的实际情况，由于存在稳态误差，需要加入滞后校正环节I。 飞行器PID参数整定经验如下： 1、首先要做的工作是确保电子调速器的油门行程一致，即初始化电调成功。 2、确保电机和螺旋桨已经拧紧，否则将威胁到人身安全。 3、调节PID参数首先要了解P、I、D三个参数的意义及作用。明白P的作用是回复到水平面的回复力。D的作用是阻尼力，起抑制作用。 4、调PID的步骤是先要找到等幅震荡时候的P值，然后再加入D值进行调节。 5、要根据自己的真实情况来调节参数。 6、调PID参数是一个很耗时间、很考验耐心的事情，同时也是件很危险的事情。做好安全措施，不要急躁，多试一些数，发现问题冷静思考，相信一定会成功的。 26 湖南工业大学本科毕业设计（论文） 第七章 结论及存在的问题 目前，四旋翼飞行器已经可以平稳飞行，可以将实时数据发送至上位机，遥控器控制正常，基本功能均已实现。但同时还存在以下问题： 1、没有为加入一些LED灯来作为指示 2、使用MPU6050发送的姿态数据计算出ROLL，PITCH及YAW。但YAW的值由于漂移较大，没有进行PID控制。所以飞行器会自身旋转。后续工作可以加入HMC5883L地磁传感器，使其更加稳定。 3、没有加入气压传感器来为飞行器定高，抗风能力也不强。 4、由于飞控板是自己设计并制作的，有一定缺陷，比如MPU6050的固定问题。建议后续工作可以绘制相关的PCB电路板，做成成型的模块，可以增加系统可靠性，也可以改善飞行器的外形，使其更加美观，同时也可以降低成本。
27 湖南工业大学本科毕业设计（论文） 第八章 心得体会 自从选定这个题目之后，我心里就很忐忑，因为飞行器这方面的知识知之甚少，而且时间也不算充足。 这个课题要求的知识面比较广，很多东西从来没有接触过，比如惯性导航系统的知识，四旋翼飞行器的飞行原理，MPU6050的姿态解算，还有四旋翼飞行器用到的电子调速器，无刷电机，遥控器、接收机等等这些东西完全没有概念，不知所云。 首先从主控器STM32F103C8T6学起。因为有ST公司提供的固件库，使得对主控器的操作变得简单。熟悉了STM32的操作方法后，我直接进入对MPU6050的研究，在网上搜索了大量资料，看到这个惯性器件有内置的运动处理单元可以直接输出姿态数据，让我很兴奋，因为这无疑减小了自己的工作量。在学习使用DMP的使用过程中，见识了一些很厉害的网友，他们发的技术贴对我完成DMP姿态解算帮助巨大。在经过检验自己的姿态解算正确无误之后，我开始学习电字调速器的使用，加入了匿名小四轴聊天群。由于自己知之甚少，经常在群里遭人鄙视，但学习就是这样，没有低级的问题就没有很大的提升。虚心求教使人进步。 清楚了电子调速器的使用，之后便开始对遥控器编程，解码无线接收器的信息。在跟网友的讨论和学习中，我成功用遥控器通过主控器对电调完成初始化。即遥控器解码问题得到了解决。紧接着就是PID控制程序的设计和学习。然后开始调试工作。 这次的毕业设计极大地激发了我对四旋翼飞行器的兴趣和热爱。我认为四旋翼飞行器可以说是一个丰富多彩的技术平台，它考验了开发者对硬件的了解程度，对算法的学习理解，对硬件编程的能力及控制技术的实践和感悟。同时也历练了人的性格――耐心，有韧性，相信自己，不能因为一个模块迟迟没有突破就放弃自己。 这个四旋翼飞行器确实还有很多可以改进的地方，我会在今后的学习中继续探索改进，也希望学弟学妹们能够继续在这个多元化的技术平台上学到更多有用的东西。 28
联系客服：cand57</MPU6050教程_百度文库
两大类热门资源免费畅读
续费一年阅读会员，立省24元！
MPU6050教程
阅读已结束，下载文档到电脑
想免费下载本文？
定制HR最喜欢的简历
下载文档到电脑，方便使用
还剩7页未读，继续阅读
定制HR最喜欢的简历
你可能喜欢对串级PID和单级PID的理解(基于匿名微型六轴；来源：本站|作者：小古|发表时间：2015-04；(C)；COPYRIGHT；2014；ANO；Tech；************************；************************；include\ANO_FlyControlfc；先整定内环，后整定外环；先是比例后积分，最后再把微分加
对串级PID和单级PID的理解(基于匿名微型六轴) 来源：本站 | 作者：小古 | 发表时间： | 点击量：14145 /******************** (C) COPYRIGHT 2014 ANO Tech *************************** * 文件名 ：ANO_FlyControl.cpp * 描述 ：飞行控制 **********************************************************************************/ include \ANO_FlyC
/* 先整定内环，后整定外环。 参数整定找最佳，从小到大顺序查
先是比例后积分，最后再把微分加
曲线振荡很频繁，比例度盘要放大
曲线漂浮绕大湾，比例度盘往小扳
曲线偏离回复慢，积分时间往下降
曲线波动周期长，积分时间再加长
曲线振荡频率快，先把微分降下来
动差大来波动慢。微分时间应加长
理想曲线两个波，前高后低4比1 */
/* ROLL和PIT轴向按照以上公式计算PID输出，但YAW轴比较特殊，因为偏航角法线方向刚好和地球重力平行， 这个方向的角度无法由加速度计直接测得，需要增加一个电子罗盘来替代加速度计。如果不使用罗盘的话， 我们可以单纯的通过角速度积分来测得偏航角，缺点是由于积分环节中存在积分漂移，偏航角随着时间的推移 会偏差越来越大。我们不使用罗盘就没有比例项，只仅使用微分环节来控制。 */
ANO_FlyControl::ANO_FlyControl() { yawRate = 120; //重置PID参数 PID_Reset(); }
//重置PID参数 void ANO_FlyControl:{ //因为YAW角度会漂移，所以参数和ROLL、PITCH不一样 pid[PIDROLL].set_pid(70, 15, 120, 2000000); //ROLL角度的内环控制系数,20000:积分上限
pid[PIDPITCH].set_pid(70, 30, 120, 2000000);//PITCH角度的内环控制系数
pid[PIDYAW].set_pid(100, 50, 0, 2000000); //YAW角度的内环控制系数
pid[PIDLEVEL].set_pid(280, 0, 0, 0); //外环控制系数
pid[PIDMAG].set_pid(15, 0, 0, 0); //电子罗盘控制系数 }
【扫盲知识】
串级PID：采用的角度P和角速度PID的双闭环PID算法------>角度的误差被作为期望输入到角速度控制器中 （角度的微分就是角速度）
对于本系统则采用了将角度控制与角速度控制级联的方式组成整个串级 PID 控ID_Reset(void) 制器。
串级 PID 算法中，角速度内环占着极为重要的地位。在对四旋翼飞行的物理模型进 行分析后，可以知道造成系统不稳定的物理表现之一就是不稳定的角速度。 因此，若能够直接对系统的角速度进行较好的闭环控制，必然会改善系统的动态特性 及其稳定性，通常也把角速度内环称为增稳环节。而角度外环的作用则体现在对四旋翼飞 行器的姿态角的精确控制。
外环：输入为角度,输出为角速度 内环：输入为角速度，输出为PWM增量 使用串级pid，分为：角度环控制pid环，和角速度控制环稳定环。主调为角度环（外环），副调为角速度环（内环）。 参数整定原则为先内后外，故在整定内环时将外环的PID均设为0 所谓外环就是只是一个P在起作用，也就是比例在起作用；P也就是修正力度，越大越容易使飞机震荡。
震荡的特点是：频率小、幅度大 */
/* 【横滚（Roll）和俯仰（Pitch）的控制算法】
横滚（Roll）和俯仰（Pitch）的控制算法是一样的，控制参数也比较接近。
首先得到轴姿态的角度差（angle error），将这个值乘以角度系数p后限幅（限幅必须有，否则剧烈打舵时容易引发震荡）作为角速度控制器期望值（target_rate）。target_rate与陀螺仪得到的当前角速度作差，得到角速度误差（rate_error）乘以kp得到P。在I值小于限幅值（这个值大概在5%油门）或者rate_error与i值异号时将rate_error累加到I中。前后两次rate_error的差作为D项，值得注意的是加需要入20hz（也可以采用其它合适频率）滤波，以避免震荡。将P,I,D三者相加并限幅（50%油门）得到最终PID输出。 */
//串环PID调节详情参见：http://blog.csdn.net/super_mic ... 36723
//飞行器姿态外环控制 void ANO_FlyControl::Attitude_Outter_Loop(void) { int32_t errorAngle[2]; Vector3f Gyro_ADC;
//计算角度误差值, 角度误差值=期望值-此刻姿态值
//constrain_int32作用：32位整型数限幅，使其控制输入的最大飞行倾角不大于25度（如果控制量比25度大，飞机早就坠毁了）
//rc.Command[ROLL]：遥控数据 imu.angle.x ：此刻姿态(角度)
//1.得到轴姿态的角度差（errorAngle） //2.这个角度差值进行限幅(constrain_int32)（正负FLYANGLE_MAX）（限幅必须有，否则剧烈打舵时容易引发震荡）作为角速度控制器期望值（target_rate）
errorAngle[ROLL] = constrain_int32((rc.Command[ROLL] * 2) , -((int)FLYANGLE_MAX), +FLYANGLE_MAX) - imu.angle.x * 10;
errorAngle[PITCH] = constrain_int32((rc.Command[PITCH] * 2) , -((int)FLYANGLE_MAX), +FLYANGLE_MAX) - imu.angle.y * 10;
//获取此时陀螺仪上的角速度，取角速度的四次平均值
Gyro_ADC = mpu6050.Get_Gyro() / 4; /*
得到外环PID输出（角速度的差值）(实质是相当于内环的P比例项)-------->
3.target_rate与陀螺仪得到的当前角速度作差，得到角速度误差（RateError）乘以kp（外环控制系数 pid[PIDLEVEL]--->(280, 0, 0, 0)）得到给内环的P。 */
//横滚roll：外环控制。输入为角度,输出为角速度。RateError[ROLL] 作为内环的输入。 RateError[ROLL] = pid[PIDLEVEL].get_p(errorAngle[ROLL]) - Gyro_ADC.x; //Gyro_ADC.x:陀螺仪X轴的值
//俯仰pitch：外环控制。输入为角度,输出为角速度。RateError[PITCH] 作为内环的输入。 RateError[PITCH] = pid[PIDLEVEL].get_p(errorAngle[PITCH]) - Gyro_ADC.y;//Gyro_ADC.y:陀螺仪Y轴的值
/* 偏航（Yaw）的控制算法和前两者略有不同，是将打舵量（遥控数据量rc.Command[YAW]）和角度误差的和作为角速度内环的期望值， 这样可以获得更好的动态响应。角速度内环和横滚与俯仰的控制方法一致，参数（积分限幅值会很小，默认只有万分之8）上有不同。*/
//航向yaw：外环控制。输入为角度,输出为角速度。 RateError[YAW] 作为内环的输入。 RateError[YAW] = ((int32_t)(yawRate) * rc.Command[YAW]) / 32 - Gyro_ADC.z; //Gyro_ADC.z:陀螺仪Z轴的值
//飞行器姿态内环控制: 输入为角速度，输出为PWM增量 //内环的效果就是：减小 P比例控制带来的震荡 void ANO_FlyControl::Attitude_Inner_Loop(void) { int32_t PIDTerm[3];
//注意这里是i的值是0到2 //PIDROLL、PIDPITCH、PIDYAW是枚举类型，也就是0、1、2，也就是下面的pid 、PIDTerm就是3组PID for(u8 i=0; i<3;i++) { //现象：当油门低于检查值时积分清零，重新积分
//猜测：这里应该是担心飞机没飞起来时就开始有积分，会导致起飞时不稳定 if ((rc.rawData[THROTTLE]) 这里实际就是一个完整的PID
三亿文库包含各类专业文献、行业资料、外语学习资料、中学教育、幼儿教育、小学教育、对串级PID和单级PID的理解10等内容。　
　PID 原理普及 1、 对自动控制系统的基本要求: 稳、准、快: 稳定性(P 和 I...朋友已经初 步了解了角度单环 PID 的原理,但是依旧无法理解串级 PID 究竟有... 　PID,串级控制总结1版_信息与通信_工程科技_专业资料。PID,串级控制总结1版PID...(t-1)]+u0 在 PID 的调试过程中,我们应注意以下步骤: 1、 关闭 I 和 D... 　PID串级控制_信息与通信_工程科技_专业资料。PID 串级控制系统摘要:本文将串级控制结构与预测控制算法相结合,提出一种预测-PID 串级控制策略,讨论了设计要 点,并通... 　串级PID调试技巧_能源/化工_工程科技_专业资料。PID调试技巧 【调节串环 PID 大概过程(注意修正反向) 】 1、估计大概的起飞油门。 2、调整角速度内环参数。 3、... 　对串级 PID 和单级 PID 的理解(基于匿名微型六轴) 来源:本站 | 作者:小古 | 发表时间: | 点击量:1565 /*** (C) COPYRIGHT 2014 ANO Tech... 　PID 原理普及 1、 对自动控制系统的基本要求: 稳、准、快: 稳定性(P 和 I...朋友已经初步了解了角度单环 PID 的原理, 但是依旧无法理解串级 PID 究竟有什么... 　对串级 PID 和单级 PID 的理解(基于匿名微型六轴) 来源:本站 | 作者:小古 | 发表时间: | 点击量:1565 /*** (C) COPYRIGHT 2014 ANO Tech... 　我们可以理解这两个数是我们对加速度计和陀螺仪的信任 度,你信任哪个的程度大...单级 PID 整定方法参见我写的第一篇帖子! 串级 PID: 单极 PID 适合线性系统...谈一谈MPU6050姿态融合转
姿态角（Euler角）pitch yaw roll飞行器的姿态角并不是指哪个角度，是三个角度的统称。它们是：俯仰、滚转、偏航。你可以想象是飞机围绕XYZ三个轴分别转动形成的夹角。地面坐标系（earth-surface inertial reference frame）Sg--------OXgYgZg&ignore_js_op&&①在地面上选一点Og②使Xg轴在水平面内并指向某一方向③Zg轴垂直于地面并指向地心(重力方向)④Yg轴在水平面内垂直于Xg轴，其指向按右手定则确定机体坐标系(Aircraft-body coordinate frame)Sb-------OXYZ&ignore_js_op&&①原点O取在飞机质心处，坐标系与飞机固连②x轴在飞机对称平面内并平行于飞机的设计轴线指向机头③y轴垂直于飞机对称平面指向机身右方④z轴在飞机对称平面内，与x轴垂直并指向机身下方欧拉角/姿态角（Euler Angle）&ignore_js_op&&&ignore_js_op&&机体坐标系与地面坐标系的关系是三个Euler角，反应了飞机相对地面的姿态。俯仰角&（pitch）：机体坐标系X轴与水平面的夹角。当X轴的正半轴位于过坐标原点的水平面之上（抬头）时，俯仰角为正，否则为负。&ignore_js_op&&偏航角&（yaw）：机体坐标系xb轴在水平面上投影与地面坐标系xg轴（在水平面上，指向目标为正）之间的夹角，由xg轴逆时针转至机体xb的投影线时，偏航角为正，即机头右偏航为正，反之为负。&ignore_js_op&&滚转角&P（roll）：机体坐标系zb轴与通过机体xb轴的铅垂面间的夹角，机体向右滚为正，反之为负。&ignore_js_op&
首先要明确，MPU6050 是一款姿态传感器，使用它就是为了得到待测物体（如四轴、平衡小车） x、y、z 轴的倾角（俯仰角 Pitch、滚转角 Roll、偏航角 Yaw） 。我们通过 I2C 读取到 MPU6050 的六个数据（三轴加速度 AD 值、三轴角速度 AD 值）经过姿态融合后就可以得到 Pitch、Roll、Yaw 角。
本帖主要介绍三种姿态融合算法：四元数法 、一阶互补算法和卡尔曼滤波算法。
一、四元数法
关于四元数的一些概念和计算就不写上来了，我也不懂。我能告诉你的是：通过下面的算法，可以把六个数据转化成四元数（q0、q1、q2、q3），然后四元数转化成欧拉角（P、R、Y 角）。
& & & & 虽然 MPU6050 自带的 DMP库可以直接输出四元数，减轻 STM32 的运算负担， 这里在此没有使用，因为我是用 STM32 的硬件 I2C 读取 MPU6050 数据的（），DMP库需要对 I2C 函数进行修改，如 DMP 库中的 I2C 写：i2c_write(st.hw-&addr, st.reg-&pwr_mgmt_1, 1, &(data[0]))；有4个输入变量，而 STM32 硬件 I2C 的 I2C 写为：void MPU6050_I2C_ByteWrite(u8 slaveAddr, u8 pBuffer, u8 writeAddr)，只有 3 个输入量（这之间的差异好像是由于 MPU6050 的 DMP 库是针对 MSP430&写的），所以必须进行修改，但是改固件库是一件很痛苦的事，你们应该都懂。当然，如果你用模拟 I2C 的话，是容易实现的，网上的 DMP 移植几乎都是基于模拟 I2C 的。
#include&math.h&
#include "stm32f10x.h"
//---------------------------------------------------------------------------------------------------
// 变量定义
#define Kp 100.0f& && && && && && && && &// 比例增益支配率收敛到加速度计/磁强计
#define Ki 0.002f& && && && && & // 积分增益支配率的陀螺仪偏见的衔接
#define halfT 0.001f& && && && && & // 采样周期的一半
float q0 = 1, q1 = 0, q2 = 0, q3 = 0;& && && & // 四元数的元素，代表估计方向
float exInt = 0, eyInt = 0, ezInt = 0;& && &&&// 按比例缩小积分误差
float Yaw,Pitch,R&&//偏航角，俯仰角，翻滚角
void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az)
& && &&&float vx, vy,
& && &&&float ex, ey,&&
& && &&&// 测量正常化
& && &&&norm = sqrt(ax*ax + ay*ay + az*az);& && &
& && &&&ax = ax /& && && && && && & //单位化
& && &&&ay = ay /
& && &&&az = az /& && &
& && &&&// 估计方向的重力
& && &&&vx = 2*(q1*q3 - q0*q2);
& && &&&vy = 2*(q0*q1 + q2*q3);
& && &&&vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;
& && &&&// 错误的领域和方向传感器测量参考方向之间的交叉乘积的总和
& && &&&ex = (ay*vz - az*vy);
& && &&&ey = (az*vx - ax*vz);
& && &&&ez = (ax*vy - ay*vx);
& && &&&// 积分误差比例积分增益
& && &&&exInt = exInt + ex*Ki;
& && &&&eyInt = eyInt + ey*Ki;
& && &&&ezInt = ezInt + ez*Ki;
& && &&&// 调整后的陀螺仪测量
& && &&&gx = gx + Kp*ex + exI
& && &&&gy = gy + Kp*ey + eyI
& && &&&gz = gz + Kp*ez + ezI
& && &&&// 整合四元数率和正常化
& && &&&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;&&
& && &&&// 正常化四元
& && &&&norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
& && &&&q0 = q0 /
& && &&&q1 = q1 /
& && &&&q2 = q2 /
& && &&&q3 = q3 /
& && &&&Pitch&&= asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch ,转换为度数
& && &&&Roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // rollv
& && &&&//Yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;& && && && && & //此处没有价值，注掉
& & & 要注意的的是，四元数算法输出的是三个量 Pitch、Roll 和 Yaw，运算量很大。而像平衡小车这样的例子只需要一个角（Pitch 或 Roll ）就可以满足工作要求，个人觉得做平衡小车最好不用四元数法。
二、一阶互补算法
& & & &MPU6050 可以输出三轴的加速度和角速度。通过加速度和角速度都可以得到 Pitch 和 Roll 角（加速度不能得到 Yaw 角），就是说有两组 Pitch、Roll 角，到底应该选哪组呢？别急，先分析一下。MPU6050 的加速度计和陀螺仪各有优缺点，三轴的加速度值没有累积误差，且通过算 tan()&&可以得到倾角，但是它包含的噪声太多（因为待测物运动时会产生加速度，电机运行时振动会产生加速度等），不能直接使用；陀螺仪对外界振动影响小，精度高，通过对角速度积分可以得到倾角，但是会产生累积误差。所以，不能单独使用 MPU6050 的加速度计或陀螺仪来得到倾角，需要互补。一阶互补算法的思想就是给加速度和陀螺仪不同的权值，把它们结合到一起，进行修正。得到 Pitch 角的程序如下：
//一阶互补滤波
float K1 =0.1; // 对加速度计取值的权重
float dt=0.001;//注意：dt的取值为滤波器采样时间
angle_ax=atan(ax/az)*57.3;& &&&//加速度得到的角度
gy=(float)gyo[1]/7510.0;& && & //陀螺仪得到的角速度
Pitch = yijiehubu(angle_ax,gy);
float yijiehubu(float angle_m, float gyro_m)//采集后计算的角度和角加速度
& &&&angle = K1 * angle_m + (1-K1) * (angle + gyro_m * dt);
& & 互补算法只能得到一个倾角，这在平衡车项目中够用了，而在四轴飞行器设计中还需要 Roll 和 Yaw，就需要两个 互补算法，我是这样写的，注意变量不要搞混：
//一阶互补滤波
float K1 =0.1; // 对加速度计取值的权重
float dt=0.001;//注意：dt的取值为滤波器采样时间
float angle_P,angle_R;
float yijiehubu_P(float angle_m, float gyro_m)//采集后计算的角度和角加速度
& &&&angle_P = K1 * angle_m + (1-K1) * (angle_P + gyro_m * dt);
& && && &return angle_P;
float yijiehubu_R(float angle_m, float gyro_m)//采集后计算的角度和角加速度
& &&&angle_R = K1 * angle_m + (1-K1) * (angle_R + gyro_m * dt);
& && && &return angle_R;
单靠 MPU6050 无法准确得到 Yaw 角，需要和地磁传感器结合使用。
三、卡尔曼滤波
& & & 其实卡尔曼滤波和一阶互补有些相似，输入也是一样的。卡尔曼原理以及什么5个公式等等的，我也不太懂，就不写了，感兴趣的话可以上网查。在此给出具体程序，和一阶互补算法一样，每次卡尔曼滤波只能得到一个方向的角度。
#include&math.h&
#include "stm32f10x.h"
#include "Kalman_Filter.h"
//卡尔曼滤波参数与函数
float dt=0.001;//注意：dt的取值为kalman滤波器采样时间
float angle, angle_//角度和角速度
float P[2][2] = {{ 1, 0 },
& && && && && &&&{ 0, 1 }};
float Pdot[4] ={ 0,0,0,0};
float Q_angle=0.001, Q_gyro=0.005; //角度数据置信度,角速度数据置信度
float R_angle=0.5 ,C_0 = 1;
float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;
//卡尔曼滤波
float Kalman_Filter(float angle_m, float gyro_m)//angleAx 和 gyroGy
& && &&&angle+=(gyro_m-q_bias) *
& && &&&angle_err = angle_m -
& && &&&Pdot[0]=Q_angle - P[0][1] - P[1][0];
& && &&&Pdot[1]=- P[1][1];
& && &&&Pdot[2]=- P[1][1];
& && &&&Pdot[3]=Q_
& && &&&P[0][0] += Pdot[0] *
& && &&&P[0][1] += Pdot[1] *
& && &&&P[1][0] += Pdot[2] *
& && &&&P[1][1] += Pdot[3] *
& && &&&PCt_0 = C_0 * P[0][0];
& && &&&PCt_1 = C_0 * P[1][0];
& && &&&E = R_angle + C_0 * PCt_0;
& && &&&K_0 = PCt_0 / E;
& && &&&K_1 = PCt_1 / E;
& && &&&t_0 = PCt_0;
& && &&&t_1 = C_0 * P[0][1];
& && &&&P[0][0] -= K_0 * t_0;
& && &&&P[0][1] -= K_0 * t_1;
& && &&&P[1][0] -= K_1 * t_0;
& && &&&P[1][1] -= K_1 * t_1;
& && &&&angle += K_0 * angle_ //最优角度
& && &&&q_bias += K_1 * angle_
& && &&&angle_dot = gyro_m-q_//最优角速度
& && &&&return angle；
& & & 作个总结：三种融合算法都能够输出姿态角（Pitch 和 Roll ），一次四元数法可以输出 P、R、Y 三个倾角，计算量比较大。一阶互补和卡尔曼滤波每次只能输出一个轴的姿态角。
更多相关文章
因为公司最近招聘的力度很大,所以最近公司的面试很多,加之很多同事项目紧,所以让我参加了一些技术面试.不论是作为面试官还是应聘者,参加工作以来我参与的面试的次数我自己也记不清了,所以在此想从面试官和应聘者的角度来谈谈我的面试经历和感受.作为面试官因为上周的事情比较近,我就先从公司最近的面试谈起.我所在 ...
PDF 版下载:/downloads/chenshuo/documents/LearningNetworkProgramming.pdf本文谈一谈我在学习网络编程方面的一些个人经验.&网络编程&这个术语的范围很广,本文指用Sockets API ...
数据库中表的设计是一个老生常谈的话题,对于表的设计却依然存在某些误区,本篇文章对来从范式和性能的角度谈一谈数据库的设计.
设计数据库?
首先第一个问题是,对于表的设计而言,我们究竟需要何种程度的设计.这取决于您数据库的规模,打个比方,就好比您盖一个两层小楼,基本无需什么设计 ...
我从事IT培训和微软企业护航12年,2001年参加MCSE培训班,进入IT领域,由读书到现在出版图书,由没有经验,站在讲台的紧张,到现在的游刃有余,经历了12年的时间,这期间慢慢总结了一套行之有效的学习方法,分享给初学IT技术的同行.我在一个培训中心授课,有个学生参加CCNP的培训班,已经重听了两遍 ...
1.浅复制VS深复制 本文中的复制也可以称为拷贝,在本文中认为复制和拷贝是相同的意思.另外,本文只讨论js中复杂数据类型的复制问题(Object,Array等),不讨论基本数据类型(null,undefined,string,number和boolean),这些类型的值本身就存储在栈内存中(stri ...
写在前面的话:今天来总结一下http相关的request和response,就从以下几个问题入手吧. ======正文开始======== 1.什么是HTTP Request 与HTTP Response? 我们平时打开浏览器,输入网址,点击Enter按键,然后我们想要的网页就呈现在我们的面前,可是 ...
l 什么是网络工程师?要问什么是网络工程师,首先要了解信息化的建设与使用是如何分工的,那么现在信息化的建设与使用是如何分工的呢?
大致分为:网络工程师.服务器工程师.存储工程师.数据库工程师.开发工程师.系统工程师等. 那网络工程师的地位又是怎样的呢?
无论从OSI ...
Beyond Compare(简称BC) 是一款功能强大的文件比较工具,平时的代码合并时经常使用这个工具进行.在一次代码比较的过程中发现了这样的现象:虽然source文件的内容相同,但是BC显示两文件有差异(如下图所示):----我们注意到这两个文件的编码和文件大小不同,但是文本内容却是一模一样的. ...
第一篇文章终于落笔了,本文将从思考的角度去重新认识一下Helloworld.在例子之后会提出不同的问题,引导大家去思考每一句乃至每个细节.同时希望能够让大家以后能够以不断思考不断提问的方式去看待自己的程序.我相信大 ...
以前在做实验的过程中总结和写的一些教程的一些资料,一直没时间发布到博客上面,五一到了,终于有点时间发布啦!关于Linux上面还会有RHCE系列的学习笔记发表
像Apache一样,tomcat同样也可以对网站目录下的文件进行相应的认证操作:BASIC验证是一种常见的验证,这种验证的安全度不高,它的验证不同于表单类的验证,做法也很简单,主要用于Web方面的验证.如下详细介绍B ...
简单ping测试脚本#!/bin/bash HOSTS=&&COUNT=100 SUBJECT=&Lost Packet,plesae note!&E ...
一.入侵检测工具rkhunter1.rkhunter是Linux系统平台下的一款开源入侵检 ...
XP永久了,滚动条滚动次数太多,实在是烦啊,浪费偶宝贵的时间今天下午,想起来以前看过的优化,就凭着记忆,思索出来,将系统整顿了下,不出所料,滚动次数减少了,而且是减的太少了,只转了一圈,严格来说是半圈 下面我就把我的 ...
我们经常会需要将一个块级元素设置大小让它显示在页面中,最常用的就是设置具体的值. 例如在页 ...
LICEcap 是一款屏幕录制工具,支持导出 GIF 动画图片格式,轻量级.使用简单,录制过程中可以随意改变录屏范围 下载
对Button进行一次封装- (UIButton *)buttonWithFrame:(CGRect)frame withNormalTitle:(NSString *)title withOtherStateTit ...
二.插件式框架平台 以插件编程为指导思想,动态反射调用组件为行动方针,深入调研.设计.编码,初步形成现代化软件架构. 简介 有了理论指导,就需要用实践去验证.所谓知行合一. 作为一款通用插件式开发框架,基础的插件内核 ...