|
发表于 2009-5-17 21:37:24
|
显示全部楼层
首先我们使用V0.71h Code Redesign killagreg的代码,这个代码基本都是英文变量名和注释,看起来容易些。所有核心的姿态控制都在fc.c文件里。这个代码可以在MK的CVS中免费下到,位于branches目录下。
MK的融合代码主要做了3件事:
第一件是即时融合,就是实时地根据加速度计的数值反推出陀螺仪积分应有的数值,然后根据当前的陀螺仪积分进行调整。
第二件是长期融合,在代码里它用0.5秒的时间采集加速度计的数据,然后到0.5秒时对这些数据进行平均,依此来得到一个相对稳定的加速度计数值。根据这个数值来相对准确地知道四轴这0.5秒的姿态,然后再修整调整量,做到自动稳定到平衡位置。
第三件是根据调整量的大小,决定是否需要修改陀螺仪中立点。这部分我目前还搞不太明白,所以先暂且不提。有前2部分已经可以做到让四轴稳定到LAMA的程度了。
下面结合代码来详细解说:
首先是即时融合的代码,位于fc.c的890行开始。
if(!Looping_Nick && !Looping_Roll) // if not lopping in any direction
{
int32_t tmp_long, tmp_long2;
if(FCParam.Kalman_K != -1)
{
// determine the deviation of gyro integral from averaged acceleration sensor
tmp_long = (int32_t)(IntegralNick / ParamSet.GyroAccFactor - (int32_t)Mean_AccNick);
tmp_long = (tmp_long * FCParam.Kalman_K) / (32 * 16);
tmp_long2 = (int32_t)(IntegralRoll / ParamSet.GyroAccFactor - (int32_t)Mean_AccRoll);
tmp_long2 = (tmp_long2 * FCParam.Kalman_K) / (32 * 16);
if((MaxStickNick > 64) || (MaxStickRoll > 64)) // reduce effect during stick commands
{
tmp_long /= 2;
tmp_long2 /= 2;
}
if(abs(PPM_in[ParamSet.ChannelAssignment[CH_YAW]]) > 25) // reduce further if yaw stick is active
{
tmp_long /= 3;
tmp_long2 /= 3;
}
// limit correction effect
if(tmp_long > (int32_t)FCParam.Kalman_MaxFusion) tmp_long = (int32_t)FCParam.Kalman_MaxFusion;
if(tmp_long < -(int32_t)FCParam.Kalman_MaxFusion) tmp_long =-(int32_t)FCParam.Kalman_MaxFusion;
if(tmp_long2 > (int32_t)FCParam.Kalman_MaxFusion) tmp_long2 = (int32_t)FCParam.Kalman_MaxFusion;
if(tmp_long2 <-(int32_t)FCParam.Kalman_MaxFusion) tmp_long2 =-(int32_t)FCParam.Kalman_MaxFusion;
}
else
{
// determine the deviation of gyro integral from averaged acceleration sensor
tmp_long = (int32_t)(IntegralNick / ParamSet.GyroAccFactor - (int32_t)Mean_AccNick);
tmp_long /= 16;
tmp_long2 = (int32_t)(IntegralRoll / ParamSet.GyroAccFactor - (int32_t)Mean_AccRoll);
tmp_long2 /= 16;
if((MaxStickNick > 64) || (MaxStickRoll > 64)) // reduce effect during stick commands
{
tmp_long /= 3;
tmp_long2 /= 3;
}
if(abs(PPM_in[ParamSet.ChannelAssignment[CH_YAW]]) > 25) // reduce further if yaw stick is active
{
tmp_long /= 3;
tmp_long2 /= 3;
}
#define BALANCE 32
// limit correction effect
if(tmp_long > BALANCE) tmp_long = BALANCE;
if(tmp_long < -BALANCE) tmp_long =-BALANCE;
if(tmp_long2 > BALANCE) tmp_long2 = BALANCE;
if(tmp_long2 <-BALANCE) tmp_long2 =-BALANCE;
}
// correct current readings
Reading_IntegralGyroNick -= tmp_long;
Reading_IntegralGyroRoll -= tmp_long2;
}
重点地部分是:
// determine the deviation of gyro integral from averaged acceleration sensor
tmp_long = (int32_t)(IntegralNick / ParamSet.GyroAccFactor - (int32_t)Mean_AccNick);
tmp_long /= 16;
tmp_long2 = (int32_t)(IntegralRoll / ParamSet.GyroAccFactor - (int32_t)Mean_AccRoll);
tmp_long2 /= 16;
if((MaxStickNick > 64) || (MaxStickRoll > 64)) // reduce effect during stick commands
{
tmp_long /= 3;
tmp_long2 /= 3;
}
if(abs(PPM_in[ParamSet.ChannelAssignment[CH_YAW]]) > 25) // reduce further if yaw stick is active
{
tmp_long /= 3;
tmp_long2 /= 3;
}
#define BALANCE 32
// limit correction effect
if(tmp_long > BALANCE) tmp_long = BALANCE;
if(tmp_long < -BALANCE) tmp_long =-BALANCE;
if(tmp_long2 > BALANCE) tmp_long2 = BALANCE;
if(tmp_long2 <-BALANCE) tmp_long2 =-BALANCE;
}
// correct current readings
Reading_IntegralGyroNick -= tmp_long;
Reading_IntegralGyroRoll -= tmp_long2;
首先,MK根据IntegralNick变量(这个变量是当前陀螺仪的积分值),利用ParamSet.GyroAccFactor(这个是一个设置值,用来标定加速度计变化1,陀螺仪积分应该变化多少),得出在目前的陀螺仪积分下,对应的加速度计的偏差量应该是多少。然后用这个值减去现在的加速度计数值,得到的就是当前从加速度计角度“看到”的陀螺仪积分偏差。下面的/16用来对这个值进行衰减。在下面如果有遥控器操作(MaxStickNick > 64)那么还要更进一步衰减,甚至Z轴有操作也要再衰减。最后对这个偏差值进行限幅,避免绝对值超过BALANCE的大小。剩下的就只要把修正值修正到陀螺仪积分里就完成了,Reading_IntegralGyroNick就是陀螺仪的积分。
看起来这个代码比较容易理解,不过这里有两点需要注意的。
一是这个代码是利用加速度计来看陀螺仪积分偏差,得出的调整量是加速度计的数值。也许你会觉得奇怪,怎么能用加速度计的数据来加减陀螺仪积分呢?2者似乎是风马牛不相及。不过这里这2者还是有关系的,ParamSet.GyroAccFactor,MK这么做可以将调整量缩小。
二是这个调整量很小,本来加速度计的变化量就很小了,又除了16,甚至还要再除3。最后还能剩多少啊?实际情况是剩的确实不多,我的四轴数据测量后,这个调整值一般也就是3左右,没有看到超过5的。而陀螺仪积分随便就几百上去了,这点根本不算什么。
这么说起来,大家可能对这个调整量的效率产生疑问。确实,这个调整量是很慢。我在我的四轴上实际测量过,按照400Hz的陀螺仪积分频率,在没有积分的情况下,光靠这个加速度计的调整值,要想让陀螺仪积分恢复到和加速度计的对应状态,需要超过2秒钟。
那么大家肯定又有疑问了,这么慢的调整速度,怎么能实现稳定呢?这就是MK的高明之处。俗话说“四两拨千斤”,用在这里还是很合适的。
整个MK的调整速度,不是标准接收机的50Hz,也不是100Hz,而是高达500Hz。可以在main.c里查找变量UpdateMotor,可以看到注释“reset Flag, is enabled every 2 ms by isr of timer0”,没有错,就是2ms触发一次。在Timer0.c的ISR(TIMER0_OVF_vect)里,还可以看到注释“every 2nd run (976Hz/2 = 488 Hz)”,这就是它的执行频率。
心细的人肯定又会继续产生疑问,为什么要这么快?有必要这么快吗?1000KV的马达,3S电池,一分钟最多也就是12000转,换算成秒一秒最多也就转200圈。500Hz的调整频率难道转0.25圈就换一个转速吗?
实际情况是这样的:
首先大家都明白机械振动对于四轴姿态调整的干扰,如果完全没有机械振动,那么想让四轴飞稳会比现在简单很多倍。但既然不可能完全避免,那么我们就必须想办法来克服它。于是就有我以前采用的加权移动平均或类似的算法。但这些算法都有一个缺点,如果想要状态准确,不可避免地就会造成响应速度低。反过来响应速度高则抗震动效果就不会好。这是一对矛盾,一对让人头大的矛盾。
MK也有这个问题,于是他们想到了一个非常巧妙的解决方法。既然马达转速不可能无限快,那么完全可以通过提高控制频率来应对震动。与其过滤震动,不如加快速度跟上震动,将震动引起的错误数据也当作正确的来处理,反正到马达端震动会被消除的,因为机械震动大都是往复的。而且I2C电调本身的速度也很快,所以频率就可以轻松达到500Hz或更高。
既然明白了加快调整的原因,那么对于弱化加速度计的调整量也就不难理解了。加速度计对于震动实在太敏感了,对于基本没做处理(实际是加速度计的数据只有一个移动期为2的移动平均,就是保留一个历史数据然后和新数据相加除2)的加速度计数据,如果调整量大了,势必造成陀螺仪积分变化非常大,一个显而易见的结果就是引起四轴振荡。实际测试也是如此。
虽然上面说了这么多,我想大家还是不完全明白有了这部分代码后能做到什么。下面就来说说这段代码的效果。
有了这些代码后,设置好合适的I,关掉P,拿起四轴,让一个轴倾斜,你会在倾斜的同时感觉到这个轴的马达转速快速提升,和你的倾斜动作完全同步。你什么时候倾斜,马达什么时候加速,你倾斜的角度越大,马达转速越快。这就是效果。
正确的融合结果是四轴的每个轴都可以一直保持这种“反抗精神”。不管你倾斜多久,不管你倾斜角度有多大都不会“放弃”。而且当你基本回到平衡位置时,马达们应该都安静下来了,四轴不会因为以前的倾斜发生震荡来“报复”你,即使你反复左右倾斜试图引起震荡也不会。
如果融合有问题,有可能马达反抗一会儿就放弃了,然后按照当前的倾斜位置作为新的平衡位置。或者引发震荡,造成四轴晃动。
为了引出下面的算法,现在请你将四轴小幅度倾斜一点,不要离平衡位置太远,看看马达会有何表示。
试验的结果,是不是在小幅度的时候,马达基本没有加速的动作?
原因并不是你代码弄错了,而是这种平衡策略的天生缺陷。由于这种平衡需要通过陀螺仪积分也就是绝对倾斜角度来调整马达转速,总有一个情况就是当倾斜角度很小的时候,无法让马达产生足够的调整动力来最终把四轴拉平。这就是为什么MK要引入第二种,也就是长期融合。
长期融合的代码位于fc.c文件的949行。
if(MeasurementCounter >= BALANCE_NUMBER) // averaging number has reached
{
static int16_t cnt = 0;
static int8_t last_n_p, last_n_n, last_r_p, last_r_n;
static int32_t MeanIntegralNick_old, MeanIntegralRoll_old;
// if not lopping in any direction (this should be alwais the case,
// because the Measurement counter is reset to 0 if looping in any direction is active.)
if(!Looping_Nick && !Looping_Roll && !FunnelCourse)
{
// Calculate mean value of the gyro integrals
MeanIntegralNick /= BALANCE_NUMBER;
MeanIntegralRoll /= BALANCE_NUMBER;
// Calculate mean of the acceleration values
IntegralAccNick = (ParamSet.GyroAccFactor * IntegralAccNick) / BALANCE_NUMBER;
IntegralAccRoll = (ParamSet.GyroAccFactor * IntegralAccRoll ) / BALANCE_NUMBER;
// Nick ++++++++++++++++++++++++++++++++++++++++++++++++
// Calculate deviation of the averaged gyro integral and the averaged acceleration integral
IntegralErrorNick = (int32_t)(MeanIntegralNick - (int32_t)IntegralAccNick);
CorrectionNick = IntegralErrorNick / ParamSet.GyroAccTrim;
AttitudeCorrectionNick = CorrectionNick / BALANCE_NUMBER;
// Roll ++++++++++++++++++++++++++++++++++++++++++++++++
// Calculate deviation of the averaged gyro integral and the averaged acceleration integral
IntegralErrorRoll = (int32_t)(MeanIntegralRoll - (int32_t)IntegralAccRoll);
CorrectionRoll = IntegralErrorRoll / ParamSet.GyroAccTrim;
AttitudeCorrectionRoll = CorrectionRoll / BALANCE_NUMBER;
if(((MaxStickNick > 64) || (MaxStickRoll > 64) || (abs(PPM_in[ParamSet.ChannelAssignment[CH_YAW]]) > 25)) && (FCParam.Kalman_K == -1) )
{
AttitudeCorrectionNick /= 2;
AttitudeCorrectionRoll /= 2;
}
核心部分:
MeanIntegralNick /= BALANCE_NUMBER;
MeanIntegralRoll /= BALANCE_NUMBER;
// Calculate mean of the acceleration values
IntegralAccNick = (ParamSet.GyroAccFactor * IntegralAccNick) / BALANCE_NUMBER;
IntegralAccRoll = (ParamSet.GyroAccFactor * IntegralAccRoll ) / BALANCE_NUMBER;
// Nick ++++++++++++++++++++++++++++++++++++++++++++++++
// Calculate deviation of the averaged gyro integral and the averaged acceleration integral
IntegralErrorNick = (int32_t)(MeanIntegralNick - (int32_t)IntegralAccNick);
CorrectionNick = IntegralErrorNick / ParamSet.GyroAccTrim;
AttitudeCorrectionNick = CorrectionNick / BALANCE_NUMBER;
// Roll ++++++++++++++++++++++++++++++++++++++++++++++++
// Calculate deviation of the averaged gyro integral and the averaged acceleration integral
IntegralErrorRoll = (int32_t)(MeanIntegralRoll - (int32_t)IntegralAccRoll);
CorrectionRoll = IntegralErrorRoll / ParamSet.GyroAccTrim;
AttitudeCorrectionRoll = CorrectionRoll / BALANCE_NUMBER;
if(((MaxStickNick > 64) || (MaxStickRoll > 64) || (abs(PPM_in[ParamSet.ChannelAssignment[CH_YAW]]) > 25)) && (FCParam.Kalman_K == -1) )
{
AttitudeCorrectionNick /= 2;
AttitudeCorrectionRoll /= 2;
}
这里MeanIntegralNick是陀螺仪积分,IntegralAccNick是加速度计的积分总合,是经过BALANCE_NUMBER次姿态调整累加下来的。ParamSet.GyroAccFactor依旧是那个关键的系数。
代码首先将陀螺仪积分求平均,得到一个平均值。同样对加速度计的积分也进行了这样的处理,只是代码里MK先将加速度计的数值换算成对应的陀螺仪积分后再求得平均。于是下面就是2个陀螺仪积分的对比了,和上面的算法有些不同。
在下面的代码就更好懂了,先求出这2个积分的差异,然后利用ParamSet.GyroAccTrim缩小,最后再除以BALANCE_NUMBER,小到一点点的地步。后面的代码也对遥控器操作情况下再度进行衰减。在我的四轴上,这个调整量基本小于0.1,有时候甚至都是0.000XXX的级别。为什么要这么小?原因就是变量AttitudeCorrectionNick的应用是每次积分都用到了(fc.c的317行, Reading_IntegralGyroNick += Reading_GyroNick - AttitudeCorrectionNick;),所以如果大了,那么一下子就大幅度改变了陀螺仪积分,引起麻烦。
这里有个隐含的疑问,既然Reading_IntegralGyroNick每次都有进行过调整,那么和第一种算法会不会冲突呢?实际情况不会。因为第一种调整虽然调整量比第二种大很多倍,而且调整速度也非常快,加速度计的数据虽然长期看来确实可以反映出四轴的姿态,而在1/500秒看来基本可以算是随机的。所以有很大情况这2种调整量不会互相覆盖。
加入了这个代码以后,再小幅度倾斜四轴看看。四轴会慢慢地自己增加马达转速,直到把自身拉平才会罢休。这就可以大幅度减小漂移的程度。从图表角度看,随着四轴的倾斜,陀螺仪积分也随之水涨船高,转速自然就上去了。
有了这2部分代码以后,四轴基本可以自己做到自主悬停了。
以上分析只是从代码和现象层面进行的理解,MK肯定也采用了一种比较经典的控制算法,只是目前我没有找到相关的资料。欢迎大家继续探索。 |
|