效果：简单暴力，但是补滤波效果差，单级PID响应慢，打舵跟随效应差。

2. 三角函数直接解算欧拉角+卡尔曼滤波+单级PID版本

效果：卡尔曼滤波噪声偏大，滞后略微严重，单级PID难操作，打舵响应慢，跟随效应差。不过比较适合初学四轴的人，难度比四元数加串级PID版本低，易于理解。

 

3.四元数姿态解算+互补滤波（德国开源四轴）+串级PID版本

效果：四元数难理解，基于PI控制的互补滤波不适合非专业人员，PID参数较单级PID参数难调。打舵响应极佳，稳定程度高，易于操作，是目前四轴的主流算法

附带程序下载：

 四轴-四元数 互补滤波 串级PID测试版.rar (187 KB, 下载次数: 3855)

 

 

现在分享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