自行研制的四轴飞行器算法交流.DOC

1、自行研制的四轴飞行器 算法交流 从开始做四轴到现在，已经累计使用了三个月的时间，从开始的尝试用四元数法进行姿态检测，到接着使用的卡尔曼滤波算法，我们走过了很多弯路，我在从上周开始了对德国人四轴代码的研究和移植，发现德国人的代码的确有他的独到之处，改变了很多我对模型的 想法，因为本人是第一次尝试着制作模型，因此感觉很多想法还是比较简单。经过了一周的时间，我将德国人的代码翻译并移植到了我目前的四轴上，并进行了调 试，今天，专门请到了一个飞直升机的教练，对我们的四轴进行试飞，并与一个华科尔的四轴进行了现场比较，现在我们四轴的稳定性已经达到了商品四轴的程度。 下面是我这一周时间内对德国人代码的一些理解

2、： 德国人代码中的姿态检测算法： 首先，将陀螺仪和加速度及的测量值减常值误差，得到角速度和加速度，并对角速度进行积分，然后对陀螺仪积分和加速度计的数值进行融合。融合分为两部分，实 时融合和长期融合,实时融合每一次算法周期都要执行，而长期融合没 256 个检测周期执行一次，（注意检测周期小于控制周期的 2ms） 实时融合： 1.将陀螺仪积分和加表滤波后的值做差； 2.按照情况对差值进行衰减，并作限幅处理； 3.将衰减值加入到角度中。 长期融合： 长期融合主要包括两个部分，一是对角速度的漂移进行估计（估计值是要在每一个控制周期都耦合到角度中的），二是对陀螺仪的常值误差（也就是陀螺仪中立点）进行实时

3、的修正。 1.将陀螺仪积分的积分和加速度积分做差（PS：为什么这里要使用加表积分和陀螺仪积分的积分，因为在 256 个检测周期内，有一些加速度计的值含有有害的 加速度分量，如果只使用一个时刻的加表值对陀螺仪漂移进行估计，显然估计值不会准确，使用多个周期的值进行叠加后做座平均处理，可以减小随机的有害加速度 对估计陀螺仪漂移过程中所锁产生的影响） 2.将上面两个值进行衰减，得到估计的陀螺仪漂移 3.对使考虑了陀螺仪漂移和不考虑陀螺仪漂移得到的角度做差，如果这两个值较大，说明陀螺仪在前段时间内测到的角速率不够准确，需要对差值误差（也就是陀螺仪中立点）进行修正，修正幅度和差值有关 长期融合十分关键，如

4、果不能对陀螺仪漂移做修正，则系统运行一段时间后，速率环的稳定性会降低。 下面比较一下德国四轴中姿态检测部分和卡尔曼滤波之间的关系： 卡尔曼滤波是一种线性系统的最优估计滤波方法。对于本系统而言，使用卡尔曼滤波的作用是通过对系统状态量的估计，和通过加速度计测量值对系统状态进行验 证，从而得到该系统的最优状态量，并实时更新系统的各参数（矩阵），而最重要的一点，改滤波器能够对陀螺仪的常值漂移进行估计，从而保证速率环的正常运 行，并在加速度计敏感到各种有害加速度的时候，使姿态检测更加准确。但是卡尔曼滤波器能否工作在最优状态很大程度上取决于系统模型的准确性，模型参数的标 定和系统参数的选取。然而，仅仅通过

5、上位机观测而得到最优工作参数是不显示的，因为参数的修改会导致整个系统中很多地方发生改变，很难保证几个值都恰好为 最优解，这需要扎实的理论知识，大量的测量数据和系统的仿真，通过我对卡尔曼滤波器的使用，发现要想兼顾锁有的问题，还是有一定难度的。 而德国人的姿态检测部分是在尝试使用一种简单方法去解决复杂问题，他既没有使用传统的四元数法进行姿态检测，也么有使用卡尔曼滤波。他的计算量不比最简单 的卡尔曼滤程序波程序的计算量小，但与卡尔曼滤波相比，更加直观，易于理解，参数调节也更加方便。我个人理解，这个方法是在尝试着对卡尔曼滤波这一复杂相 互耦合的多状态变量的线性系统状态估计过程进行了简单的解耦，从而将姿

6、态的最优估计和陀螺仪漂移的最优估计分隔开，这样，就可以通过比较直观的观测手段对 两个部分的参数进行调整，但是，这种方法的理论性肯定不如使用四元数法和卡尔曼滤波，在一些特殊的情况下还可能出现问题，但是，由于卡尔曼滤波器设计的难 度，使用这种方法还是比较现实的。 控制算法： 德国人的控制算法的核心是对角速度做 PI 计算，P 的作用是使四轴能够产生对于外界干扰的抵抗力矩，I 的作用是让四轴产生一个与角度成正比的抵抗力。 如果只有 P 的作用，将四轴拿在手上就会发现，四轴能够抵抗外界的干扰力矩的作用，而且这个抵抗力非常快速，只要手妄图改变四轴的转速，四轴就会产生一个抵抗力矩，但是，如果用手将四轴扳过

7、一个角度，则四轴无法自己回到水平的角度位置，这就需要 I 的调节作用。 对角速度做 I（积分）预算实际得到的就是角度，德国人四轴里面用的也是角度值，如果四轴有一个倾斜角度，那么四轴就会自己进行调整，直到四轴的倾角为零， 它所产生的抵抗力是与角度成正比的，但是，如果只有 I 的作用，会使四轴迅速产生振荡，因此，必须将 P 和 I 结合起来一起使用，这时候基本上就会得到德国四轴 的效果了。 在对角速度进行了 PI 调节之后，德国人将操纵杆的值融合到结果中去，并对得到的新的值有进行了一次 PI 计算，这个积分参数很小，使用这个积分的作用因为， 四轴在有一个非常小的倾角的情况下产生的抵抗力矩很小，无法

8、使四轴回到水平位置，这就会导致无论怎么手动调节微调，四轴都很难做到悬停，会不停得做水平漂 移运动，这就必须不停的进行调整。 下面是我给德国四轴中飞控程序的一些注释： void Piep(unsigned char Anzahl) while(Anzahl-) if(MotorenEin) return; /auf keinen Fall im Flug! beeptime = 100; Delay_ms(250); /函数:SetNeutral 设定传感器发出参数的中立数值，因为有漂移所以要使其每次工作都要测量出来。 /# / Nullwerte ermitteln /*设置中立点*/ voi

9、d SetNeutral(void) /# /*加速度计中立点*/ NeutralAccX = 0; NeutralAccY = 0; NeutralAccZ = 0; /*陀螺仪中立点*/ AdNeutralNick = 0; AdNeutralRoll = 0; AdNeutralGier = 0; Parameter_AchsKopplung1 = 0; Parameter_AchsGegenKopplung1 = 0;。 /*这个地方我还没有弄得太明白，检测中立点的函数被调用了两次，但是第一次的数据好像没有保存，只用到了 第二次的数据*/ /*记录中立点*/ CalibrierMitt

10、elwert(); Delay_ms_Mess(100); /*记录中立点*/ CalibrierMittelwert(); /*既然只使用了后一次的数据，为什么要进行两次记录中立点的函数*/ if(EE_Parameter.GlobalConfig NeutralAccX = abs(Mittelwert_AccNick) / ACC_AMPLIFY; NeutralAccZ = Aktuell_az; else /*如果发现变化不大，则仍然储存上一次的*/ NeutralAccX = (int)eeprom_read_byte( NeutralAccY = (int)eeprom_read

11、_byte( NeutralAccZ = (int)eeprom_read_byte( /*将所有数据清零，这里带 2 的变量都是加入了陀螺仪漂移补偿值之后得到的角度*/ Mess_IntegralNick = 0; Mess_IntegralNick2 = 0; Mess_IntegralRoll = 0; Mess_IntegralRoll2 = 0; Mess_Integral_Gier = 0; MesswertNick = 0; MesswertRoll = 0; MesswertGier = 0; StartLuftdruck = Luftdruck; HoeheD = 0; Me

12、ss_Integral_Hoch = 0; KompassStartwert = KompassValue; GPS_Neutral(); beeptime = 50; / /*从 EEPROM 中读取陀螺仪积分到达 90时候的值，并储存，当得到的姿态角度大于这个范围时，说明超过了 90 ，就要进行相应的处理*/ Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L; Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagRoll * 2500L; ExternHoeh

13、enValue = 0; / /函数描述 ：求参数的平均数值 / /# / Bearbeitet die Messwerte void Mittelwert(void) / 根据测量值 计算陀螺仪和加速度计数据 /# static signed long tmpl,tmpl2; /*将陀螺仪数据减去常值误差，得到实际的叫速率的倍数*/ MesswertGier = (signed int) AdNeutralGier - AdWertGier; MesswertRoll = (signed int) AdWertRoll - AdNeutralRoll; MesswertNick = (sig

14、ned int) AdWertNick - AdNeutralNick; /DebugOut.Analog26 = MesswertNick; DebugOut.Analog28 = MesswertRoll; /加速度传感器输出 /*加速度计数据滤波，ACC_AMPLIFY=12 得到的 Mittelwert_AccNick 是加速度计数值的 12 倍*/ /*AdWertAccNick 为测量值*/ / Beschleunigssensor + Mittelwert_AccNick = (long)Mittelwert_AccNick * 1 + (ACC_AMPLIFY * (long)

15、AdWertAccNick) / 2L;/具有滤波功能的方法，用当前加速度和上次的加速度平均 Mittelwert_AccRoll = (long)Mittelwert_AccRoll * 1 + (ACC_AMPLIFY * (long)AdWertAccRoll) / 2L; Mittelwert_AccHoch = (long)Mittelwert_AccHoch * 1 + (long)AdWertAccHoch) / 2L; /*计算加速度计的积分，加速度计对运动十分敏感，采用加速度计积分，可以减少瞬间的运动加速度的影响*/ IntegralAccNick += ACC_AMPLIF

16、Y * AdWertAccNick; IntegralAccRoll += ACC_AMPLIFY * AdWertAccRoll; IntegralAccZ += Aktuell_az - NeutralAccZ; / Gier + /*偏航方向无法进行滤波，因此直接进行积分得到偏航角度*/ Mess_Integral_Gier += MesswertGier; Mess_Integral_Gier2 += MesswertGier; /*耦合项应该是另外两个陀螺仪对这个轴上陀螺仪的影响，当四轴在稳定姿态不为水平的时候，进行偏航运动时 候所进行的补偿*/ /*假设目前的俯仰角是 30，而横滚

17、角是 0，这时候如保持俯仰和横滚轴没有任何运动，而将偏航轴转动 90 ，那么实际的俯仰角就会变为 0，横滚角就会变为 30 但是，按照目前的算法，由于俯仰和横滚方向没有运动，因此就不会有陀螺仪的积分，俯仰和横滚角是不变的 ，这就是采用陀螺仪直接积分测角度的不完善性，这时候 使用加速度计对姿态进行修正能够起到作用，但是需要一段时间，使用下面的这段话，就是将偏航轴的运动耦 合在另外两个轴上，使姿态角度能够迅速收敛到真实值上*/ /*注：使用四元数法进行姿态结算可以避免出现这种问题，但这种方法需要有准确的陀螺仪和加表的数学模型，自行研制的四轴飞行器 算法交流 四元数法还需要进行大量的矩阵计算， 而且

18、对四元数姿态进行加速度计的融合不太方便*/ if(!Looping_Nick tmpl *= MesswertGier; tmpl *= Parameter_AchsKopplung1; /125 tmpl /= 2048L; tmpl2 = Mess_IntegralRoll / 4096L; tmpl2 *= MesswertGier; tmpl2 *= Parameter_AchsKopplung1; tmpl2 /= 2048L; else tmpl = tmpl2 = 0; / Roll + MesswertRoll += tmpl; MesswertRoll += (tmpl2*P

19、arameter_AchsGegenKopplung1)/512L; Mess_IntegralRoll2 += MesswertRoll; Mess_IntegralRoll += MesswertRoll - LageKorrekturRoll; /*积分超过半圈的情况*/ if(Mess_IntegralRoll Umschlag180Roll) Mess_IntegralRoll = -(Umschlag180Roll - 10000L); Mess_IntegralRoll2 = Mess_IntegralRoll; if(Mess_IntegralRoll 1010) Messwe

20、rtRoll = +1000; if(AdWertRoll 1017) MesswertRoll = +2000; else if(AdWertRoll 2020) MesswertRoll = +1000; if(AdWertRoll 2034) MesswertRoll = +2000; / Nick + MesswertNick -= tmpl2; MesswertNick -= (tmpl*Parameter_AchsGegenKopplung1)/512L; Mess_IntegralNick2 += MesswertNick; /*LageKorrekturNick 是通过加速度计

21、积分和角速率积分的积分进行做差比较得到 的,*/ Mess_IntegralNick += MesswertNick - LageKorrekturNick; if(Mess_IntegralNick Umschlag180Nick) Mess_IntegralNick = -(Umschlag180Nick - 10000L); Mess_IntegralNick2 = Mess_IntegralNick; if(Mess_IntegralNick 1010) MesswertNick = +1000; if(AdWertNick 1017) MesswertNick = +2000; el

22、se if(AdWertNick 2020) MesswertNick = +1000; if(AdWertNick 2034) MesswertNick = +2000; /+ / ADC einschalten ANALOG_ON; /重新开始模拟量的采集 /+ /*上一步计算完了积分之后，现在将积分赋值，因此后面使用的就将是IntegralNick，IntegralNick2 等数据了 */ Integral_Gier = Mess_Integral_Gier; IntegralNick = Mess_IntegralNick; IntegralRoll = Mess_IntegralR

23、oll; IntegralNick2 = Mess_IntegralNick2; IntegralRoll2 = Mess_IntegralRoll2; /*这部分代码不执行，因为在 EEPROM 中 CFG_DREHRATEN_BEGRENZER 这一位为0*/ if(EE_Parameter.GlobalConfig else if(MesswertNick 200) MesswertRoll += 4 * (MesswertRoll - 200); else if(MesswertRoll PPM_inEE_Parameter.KanalbelegK_POTI1 + 110 if(Pot

24、i2 PPM_in EE_Parameter.KanalbelegK_POTI2 + 110 if(Poti3 PPM_in EE_Parameter.KanalbelegK_POTI3 + 110 if(Poti4 PPM_in EE_Parameter.KanalbelegK_POTI4 + 110 if(Poti1 255) Poti1 = 255; if(Poti2 255) Poti2 = 255; if(Poti3 255) Poti3 = 255; if(Poti4 255) Poti4 = 255; /函数：校正平均值 /# / Messwerte beim Ermitteln

25、 der Nullage /*确定零位*/ /*记录中立点*/ void CalibrierMittelwert(void) /# / ADC auschalten, damit die Werte sich nicht w 鋒 rend der Berechnung 鋘 dern ANALOG_OFF; MesswertNick = AdWertNick; MesswertRoll = AdWertRoll; MesswertGier = AdWertGier; /*要乘以 ACC_AMPLIFY 是为了进行数值的匹配*/ Mittelwert_AccNick = ACC_AMPLIFY *

26、 (long)AdWertAccNick; Mittelwert_AccRoll = ACC_AMPLIFY * (long)AdWertAccRoll; Mittelwert_AccHoch = (long)AdWertAccHoch; / ADC einschalten ANALOG_ON; /开模拟量 if(Poti1 PPM_inEE_Parameter.KanalbelegK_POTI1 + 110 if(Poti2 PPM_inEE_Parameter.KanalbelegK_POTI2 + 110 if(Poti3 PPM_inEE_Parameter.KanalbelegK_POTI3 + 110 if(Poti4 PPM_inEE_Parameter.KanalbelegK_POTI4 + 110) Poti4+;