5种常用的四轴飞行器PID算法讲解集合

在某莫上看到的，makeflyeasy大神的作品就分享给大家科普一下

先分享一些算法的效果

1. 三角函数直接解算欧拉角+互补滤波+单级PID版本
效果：简单暴力，但是 补滤波效果差，单级 PID响应慢，打舵跟随效应差。
2. 三角函数直接解算欧拉角+卡尔曼滤波+单级PID版本
效果：卡尔曼滤波噪声偏大，滞后略微严重，单级PID难操作，打舵响应慢，跟随效应差。不过比较适合初学四轴的人，难度比四元数加串级PID版本低，易于理解。

3.四元数姿态解算+互补滤波（德国开源四轴）+串级PID版本
效果：四元数难理解，基于PI控制的互补滤波不适合非专业人员，PID参数较单级PID参数难调。打舵响应极佳，稳定程度高，易于操作，是目前四轴的主流算法
附带程序下载：四轴-四元数 互补滤波 串级PID测试版.rar(187 KB, 下载次数: 2685)

现在分享5中最常用的PID算法

三角函数直接解算：

加速度计输出的数值为多少个 g，就是多少倍加速度，在物体在静止时会受到1个g的重力加速度，如果加速度计水平放置，那么Z轴数据就是1，XY轴就是0，原理不懂的话不用看了，好好读完高中再来研究四轴。当加速度计XY轴产生偏转时，XYZ轴的数值会产生变动，此时根据反三角函数即可解算出当前的角度。

公式：

AngleAx=atan(Angle_ax/sqrt(Angle_ay*Angle_ay+Angle_az*Angle_az))*57.2957795f;

AngleAy=atan(Angle_ay/sqrt(Angle_ax*Angle_ax+Angle_az*Angle_az))*57.2957795f;

后面的数值是 180/PI 目的是弧度转角度

这种方法简单实用但是不是太准确，因为不光重力能产生加速度，运动也能产生加速度，当运动产生的加速度引入时，就会干扰解算精度，因此对运动的还原性较差。

此外，网上的版本多是用反正弦函数解算的，那样其实是错的，假使我要解算物体在XZ坐标轴所构成的平面的角度时朝Y轴偏移了一点，那么解算出来的值就是错的，原因自己画图想想吧。

互补滤波：

当我们用三角函数直接解算出姿态后，需要对其经行滤波以及和陀螺仪的数据进行融合。因为加速度是很容易受外界干扰的，一个手机开了震动模式放在水平面上，实际角度是 0 度，但是解算出来的值是在 0 度正负某个范围内呈均匀分布的，这样的值显然不适合使用，因此需要陀螺仪的帮组。陀螺仪输出的数据是多少度一秒，对这个数据积分就可以算出系统偏转过的角度。陀螺仪受震动影响小，故短时间内可以信任它，但是陀螺仪会有温飘，其误差是随着温度而改变的，陀螺仪出厂后还会存在一定的静差，而且积分也有误差，故长时间不能信任陀螺仪。由于加速度计长时间来说值得信任，故可以用互补滤波来融合二者的优点，消减二者的缺点。

公式： Angle=0.95*(Angle-Angle_gy*dt)+0.05*AngleAx;   陀螺仪数据正负号根据自己需要而改变

在上式中我们可以看出互补滤波是由两个小式子相加得到的，小式子前有一个系数，二者相加为 1 ，我们可以理解这两个数是我们对加速度计和陀螺仪的信任度，你信任哪个的程度大点，哪个的权值就相应的变大，其输出数据在最终结果中占的比重也越大。前面一个式子是用陀螺仪积分计算角度，后面一个式子是用加速度计解算出的角度，当加速度计比重很小时就可以压制加速度噪声，也就是进行了低通滤波，前式也就是对陀螺仪进行了高通滤波。从公式中不难得出互补滤波的原理，陀螺仪占的比重偏大，短时间内以陀螺仪数据为准，加速度计占的比重较小，长时间内以加速度计来校准角度数据。

一般情况下来讲，陀螺仪比加速度计的比值取值为 0.95 ： 0.05 或 0.98 ： 0.02 。当然也可以根据实际情况降低陀螺仪的比重。

单极PID：

当你知道系统当前状态和期望状态后，如何将系统从当前状态调整到期望状态是个问题，在此我们可以用 PID 进行调整， PID 分为位置式和增量式，位置式适合舵机等系统，在此使用的是增量式。

公式： PID=P*e(n)+I*[(e(n)+e(n-1)+...+e(0)]+D*[e(n)-e(n-1)]

D 后面的当前误差减前次误差也可以直接使用陀螺仪的数据代替，原理一样。

单级PID整定方法参见我写的第一篇帖子！

串级PID：

单极 PID 适合线性系统，当输出量和被控制量呈线性关系时单极 PID 能获得较好的效果，但是四轴不是线性系统，现代学者认为，四轴通常可以简化为一个二阶阻尼系统。为什么四轴不是线性系统呢？首先，输出的电压和电机转速不是呈正比的，其次，螺旋桨转速和升力是平方倍关系，故单极 PID 在四轴上很难取得很好效果，能飞，但是不好飞。

为了解决这个问题，我们提出了串级 PID 这个解决方法。

串级 PID 就是两个 PID 串在一起，分为内环和外环 PID 。在此，我们使用内环 PID 控制，外环 PI 控制。

单极 PID 输入的是期望角度，反馈的是角度数据，串级 PID 中外环输入反馈的也是角度数据，内环输入反馈的便是角速度数据。通俗来讲，内环就是你希望将四轴以多少度每秒的速度运动，然后他给你纠正过来，外环就是根据角度偏差告诉内环你该以多少度一秒运动。这样，即使外环数据剧烈变化，四轴的效果也不会显得很僵硬。

在内环中， PID 三个数据作用分别是： P （将四轴从偏差角速度纠正回期望角速度） D （抑制系统运动） I （消除角速度控制静差）

外环 PI 中，两个数据的作用是： P （将四轴从偏差角度纠正回期望角度） I （消除角度控制静差）

整定方法：

1 ，将内外环 PID 都归 0 ，适当增加内环的 P ，调整 P 至四轴从正面朝上自然转动到正面朝下时能感受到阻力，且没有抖动，有抖动就应减小 P ，当 P 减小到无抖动或者轻微抖动时即可。

2 ，让内环的 D 慢慢增加，到你用手能明显感受到转动四轴产生排斥外力的阻力即可， D 能抑制 P 产生的振荡，但是 D 过大也会导致高频振荡，调整 D 至系统无振荡且能抑制外界的力即可。

3 ，给内环一点点 I ，注意的是 I 的积分要在油门开启后才开始，油门关闭就清 0 ，且必须有积分限幅。 I 推荐取越小越好，我取的是 0.01 ， I 取大了会导致系统振荡。

4 ，将内环 P 减半，将外环 P 调至内环的 50-70 倍，根据系统产生的高频振荡降低内环的 D ，直至高频振荡消除即可。

5 ，给外环一点点 I ，同 3.

6 ，根据实际情况对参数进行优化调整，调整过程中要注意区分各个参数的作用，时刻记住， P 是回复力，大了会低频振荡， D 是抑制力，大了会高频振荡， I 是静差消除力，越小越好，大了会产生振荡。
坛子里面分享的串级PID算法:
【开源】分享自己写的串级PID算法
http://bbs.elecfans.com/jishu_486485_1_1.html

卡尔曼滤波：

参见资料包内的 新手平衡小车的卡尔曼滤波算法总结.doc 写的很好，不想重复了，说白了就是5个公式，用协方差矩阵对加速度计的值进行线性拟合再融合。

不过后来了解到卡尔曼并不是个很好的选择，也还是会存在一定的噪声，除非用扩展卡尔曼。
附上坛子里面一个讲的比较好的卡尔曼滤波：
很形象的！卡尔曼滤波的原理说明，附源代码
http://bbs.elecfans.com/jishu_484128_1_1.html

教你在单片机上套公式实现卡尔曼滤波器
http://bbs.elecfans.com/jishu_484132_1_1.html

四元数互补滤波：

我也不懂，来自于德国的开源四轴，目前匿名等四轴也是用这个解算方法。

坛子里面找到的：

MikroKopter:来自德国的开源四轴飞行器项目

http://bbs.elecfans.com/jishu_4757431_1.html

转载来自http://bbs.elecfans.com/jishu_486490_1_1.html