四轴飞行器的设计之四元数与滤波算法——STC15单片机实战指南连载
时间：10-02
整理：3721RD
点击：
上一节讲完PID控制电机的参数整定（http://bbs.elecfans.com/jishu_581842_1_1.html），这一节来分析四元数与滤波算法
重力加速度计可以换算成角度值，角速度计输出的是角速度。我们需要获取飞行器的姿态就需要知道角度和角速度，角度和角速度输出的波形分别如图20-22、20-23所示。
便于分析滤波，接下来再看看两者的对比图，如图20-24所示。
右图可以看出，角速度相对平滑，动态特性好，静态特性差，角度值动态特性差，抗干扰能力低。直接利用传感器输出的角度值不能满足控制要求，因为干扰太大，利用角速度积分也不可以，因为积分会导致误差的累积，最终导致系统崩溃，因此我们需要一种滤波算法来对角度值进行滤波。这里有个很重要的知识点，四元数和欧拉角的换算，关于换算公式和论证，读者自行查阅资料，网上一搜一大堆，这里不赘。
接下来以角度为例，介绍滤波的作用。角度波形在没有滤波之前，如图20-25所示，波形有明显的毛刺。经滤波之后的角度波形如图20-26所示，对比发现滤波后的波形去除了尖峰，继而抗干扰能力明显增强，可以满足设计要求。
最后再来介绍一下滤波参数的调节以及调到什么程度为好，这个度量又是什么呢?飞蜓二號程序中的函数接口为“void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az)”，其中gx、gy、gz分别对应x、y、z三轴的角速度，ax，ay，az分别对应x、y、z三轴的角度，最后分别得到Pitch为滤波后x轴的角度，Roll作为y轴的角度。
注意：参数是在10ms控制周期下的，若改变了控制周期参数要自行重新调试，便于讲解，再来看看三个滤波参数。
#define Kp 10.1f // 10.1f
#define Ki 0.001f // 0.011f
#define halfT 0.0035f // 0.0035f
Kp参数调节跟随相应速率。数值越大滤波后的波形跟随元波形的程度越大，若数值过大的话会导致滤波效果降低，数值过小会导致跟随缓慢。不同的Kp值，将有不同的滤波效果，便于读者观察，这里我们取三个Kp值，分别为100、1和10，滤波之后的效果分别如图20-27、20-28和20-29所示。其中黄色为滤波前的波形图，红色为滤波后的波形图，读者需要注意的是，由于纸质书为黑白色，看不出红、黄，因此这里笔者将其幅度值大的规定为滤波前的波形图，幅度值小的就为滤波后的波形图。
对比图20-27、20-28可知，Kp为100时，跟随程度几乎和原波形一致，但明显没有滤除干扰;Kp为1时，波形变化明显跟随不上，因此Kp要适当，这就有了下面的Kp值调节。
有了Kp值的研究，接下来再来研究一下Ki、halfT值，这些参数道理类似，只是所体现的功能有别罢了。限于篇幅，这里只给出比较合适参数的Ki、halfT的参考图，分别如图20-30和20-31所示。具体的调试过程留读者自行动手琢磨，单片机的学习，就是要在不断的调试中积累经验，增长技术。
最后说明下和本文配套的STC15开发板目前正在电子发烧友销售，如果需要请戳这里购买： http://bbs.elecfans.com/product/stc15.html 我将持续更新内容.
重力加速度计可以换算成角度值，角速度计输出的是角速度。我们需要获取飞行器的姿态就需要知道角度和角速度，角度和角速度输出的波形分别如图20-22、20-23所示。
图20-22 角度波形图
图20-23 角速度波形图
便于分析滤波，接下来再看看两者的对比图，如图20-24所示。
图20-24 角度和角速度对比图
右图可以看出，角速度相对平滑，动态特性好，静态特性差，角度值动态特性差，抗干扰能力低。直接利用传感器输出的角度值不能满足控制要求，因为干扰太大，利用角速度积分也不可以，因为积分会导致误差的累积，最终导致系统崩溃，因此我们需要一种滤波算法来对角度值进行滤波。这里有个很重要的知识点，四元数和欧拉角的换算，关于换算公式和论证，读者自行查阅资料，网上一搜一大堆，这里不赘。
接下来以角度为例，介绍滤波的作用。角度波形在没有滤波之前，如图20-25所示，波形有明显的毛刺。经滤波之后的角度波形如图20-26所示，对比发现滤波后的波形去除了尖峰，继而抗干扰能力明显增强，可以满足设计要求。
图20-25 未进行滤波之前的角度波形图
图20-26 滤波之后的角度波形图
最后再来介绍一下滤波参数的调节以及调到什么程度为好，这个度量又是什么呢?飞蜓二號程序中的函数接口为“void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az)”，其中gx、gy、gz分别对应x、y、z三轴的角速度，ax，ay，az分别对应x、y、z三轴的角度，最后分别得到Pitch为滤波后x轴的角度，Roll作为y轴的角度。
注意：参数是在10ms控制周期下的，若改变了控制周期参数要自行重新调试，便于讲解，再来看看三个滤波参数。
#define Kp 10.1f // 10.1f
#define Ki 0.001f // 0.011f
#define halfT 0.0035f // 0.0035f
Kp参数调节跟随相应速率。数值越大滤波后的波形跟随元波形的程度越大，若数值过大的话会导致滤波效果降低，数值过小会导致跟随缓慢。不同的Kp值，将有不同的滤波效果，便于读者观察，这里我们取三个Kp值，分别为100、1和10，滤波之后的效果分别如图20-27、20-28和20-29所示。其中黄色为滤波前的波形图，红色为滤波后的波形图，读者需要注意的是，由于纸质书为黑白色，看不出红、黄，因此这里笔者将其幅度值大的规定为滤波前的波形图，幅度值小的就为滤波后的波形图。
图20-27 Kp=100时滤波前和滤波后的波形图
图20-28 Kp=1时滤波前和滤波后的波形图
对比图20-27、20-28可知，Kp为100时，跟随程度几乎和原波形一致，但明显没有滤除干扰;Kp为1时，波形变化明显跟随不上，因此Kp要适当，这就有了下面的Kp值调节。
图20-29 Kp=10时滤波前和滤波后的波形图
由图20-29可知，Kp为10的时候，即滤除了干扰毛刺，又有好的跟随动态特性，因此可满足设计要求。有了Kp值的研究，接下来再来研究一下Ki、halfT值，这些参数道理类似，只是所体现的功能有别罢了。限于篇幅，这里只给出比较合适参数的Ki、halfT的参考图，分别如图20-30和20-31所示。具体的调试过程留读者自行动手琢磨，单片机的学习，就是要在不断的调试中积累经验，增长技术。
图20-30 Ki比较适当时滤波前和滤波后的波形图
图20-30 halfT比较适当时滤波前和滤波后的波形图
最后说明下和本文配套的STC15开发板目前正在电子发烧友销售，如果需要请戳这里购买： http://bbs.elecfans.com/product/stc15.html 我将持续更新内容.
看看。正在学习中，支持下！
看看。正在学习中，支持下！
看看。正在学习中，支持下！
看看。正在学习中，支持下！
学习中...LZ有木有代码，
学习一下！