常用的四轴飞行器PID传感器算法

1. 三角函数直接解算欧拉角+互补滤波+单级PID版本
效果:简单暴力,但是补滤波效果差,单级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
 

你也可以分享到: