请教：德国人算法中，将偏航轴的运动耦合到其它两轴上的算法，是一个什么原理呢

FpvCamera · 发表于 2010-10-17 19:30:35

代码如下，是V0.71h的代码，函数Mittelwert()中：

   if(!Looping_Nick && !Looping_Roll && (EE_Parameter.GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV))
      {
         tmpl = (MesswertGierBias * Mess_IntegralNick) / 2048L;
         tmpl *= Parameter_AchsKopplung1;  //125
         tmpl /= 4096L;
         tmpl2 = (MesswertGierBias * Mess_IntegralRoll) / 2048L;
         tmpl2 *= Parameter_AchsKopplung1;
         tmpl2 /= 4096L;
         if(labs(tmpl) > 128 || labs(tmpl2) > 128) TrichterFlug = 1;
      }
   else  tmpl = tmpl2 = 0;

这里是一个什么原理呢

FpvCamera · 发表于 2010-10-18 14:41:17

怎么没有人响应了，论坛上的大侠，出来冒个泡指点一二呀

feng_matrix · 发表于 2010-10-18 15:02:52

德国人的飞控代码没仔细看过，仅看你贴的这段，猜测是偏航轴的角速度比例负反馈
是用来平衡正反桨的扭矩，防止四轴机体自旋。

程序中的2048/4096等，都是非负整数算法中用来矫正数制变化的，目地是避免在8位单片机上使用浮点运算。

比如10位陀螺仪ADC采样结果范围0－1024：
采样值512代表转换为浮点数后的0，采样值1024代表：＋512×陀螺仪精度系数，采样值0代表：－512×陀螺仪精度系数

这样的两个直接非负整数变量在做加减运算时，就要相应减加基数512、1024、2048、或者4096等
这样的两个直接非负整数变量在做乘除运算时，就要相应除乘基数512、1024、2048、或者4096等

德国人的代码里有大量的2048、4096等等，就是这么来的
谁让这家伙不早点换AVR32，害的很多四轴爱好跟着他眼晕。

FpvCamera · 发表于 2010-10-18 15:06:53

回复【2楼】feng_matrix 悟
-----------------------------------------------------------------------

非常感谢大侠呀，还是一如既往的热心。大侠能否在论坛上倡导下，带个队，做出点我们自己特色的飞控来

FpvCamera · 发表于 2010-10-18 15:08:34

回复【2楼】feng_matrix 悟
-----------------------------------------------------------------------

主要是这个不明白：

tmpl = (MesswertGierBias * Mess_IntegralNick) / 2048L

MesswertGierBias ：是本次角速度的偏差

Mess_IntegralNick : 是角速度的积分，相当于角度。

这两个相乘，得到的是个啥意义？

feng_matrix · 发表于 2010-10-18 15:18:14

比例积分PI，P代表陀螺仪检测的瞬时角速度，I代表角速度一阶积分（角度），就是角速度的累积合
结果再按照一定比例放缩后，负反馈控制两组正反转电机的占空比，平衡扭矩

ROLL，PITCH轴其实也是PI算法，积分有累积误差，其它两轴多了加速度倾角来校正积分误差。

四轴在不装备罗盘或GPS时，航向轴只能稳定，而不能绝对指向，就是I量的积分误差造成

多数航模用锁尾陀螺仪都是这个PI算法。

feng_matrix · 发表于 2010-10-18 15:19:50

回复【3楼】FpvCamera
回复【2楼】feng_matrix 悟
-----------------------------------------------------------------------
非常感谢大侠呀，还是一如既往的热心。大侠能否在论坛上倡导下，带个队，做出点我们自己特色的飞控来
-----------------------------------------------------------------------

大侠谈不上，只是你想玩的东东我以前玩过，至于带队做自己特色的飞控，需要有闲时间与闲钱，你刚开始可能低估了要付出的代价。

FpvCamera · 发表于 2010-10-18 15:27:43

回复【6楼】feng_matrix 悟
-----------------------------------------------------------------------

代价应该是考虑比较清楚了。我之前也diy过其它东西，反正就是烧钱。时间嘛，呵呵，这个不好说。有位名人说过：时间就像海绵的水，挤挤总是有的。

FpvCamera · 发表于 2010-10-18 15:51:31

回复【5楼】feng_matrix 悟
-----------------------------------------------------------------------

高人呀。真希望大侠出来带队呀。现在自己一个人看算法，看得一个头两个大。

zyfy · 发表于 2011-1-5 10:03:20

mark

yuxuanqq · 发表于 2011-3-28 00:35:37

void Mean(void)函数里：
// Coupling fraction
if(! LoopingNick && !LoopingRoll && (ParamSet.GlobalConfig & CFG_AXIS_COUPLING_ACTIVE))
{
tmp13 = (FilterGyroRoll * AngleNick) / 2048L;
tmp13 *= FCParam.AxisCoupling2; // 65
tmp13 /= 4096L;
CouplingNickRoll = tmp13;

tmp14 = (FilterGyroNick * AngleRoll) / 2048L;
tmp14 *= FCParam.AxisCoupling2; // 65
tmp14 /= 4096L;
CouplingRollNick = tmp14;

tmp14 -= tmp13;
YawGyroHeading += tmp14;
if(!FCParam.AxisCouplingYawCorrection)  ReadingIntegralGyroYaw -= tmp14 / 2; // force yaw

tmpl = ((GyroYaw + tmp14) * AngleNick) / 2048L;
tmpl *= FCParam.AxisCoupling1;
tmpl /= 4096L;

tmpl2 = ((GyroYaw + tmp14) * AngleRoll) / 2048L;
tmpl2 *= FCParam.AxisCoupling1;
tmpl2 /= 4096L;
if(labs(tmpl) > 128 || labs(tmpl2) > 128) FunnelCourse = 1;

TrimNick = -tmpl2 + tmpl / 100L;
TrimRoll = tmpl - tmpl2 / 100L;
}
else
{
CouplingNickRoll = 0;
CouplingRollNick = 0;
TrimNick = 0;
TrimRoll = 0;
}

ReadingIntegralGyroRoll2 += FilterGyroRoll + TrimRoll;
ReadingIntegralGyroRoll  += FilterGyroRoll + TrimRoll- AttitudeCorrectionRoll;

// Increase the roll/nick rate virtually proportional to the coupling to suppress a faster rotation
if(FilterGyroNick > 0) TrimNick += ((int32_t)abs(CouplingRollNick) * FCParam.AxisCouplingYawCorrection) / 64L;
else                   TrimNick -= ((int32_t)abs(CouplingRollNick) * FCParam.AxisCouplingYawCorrection) / 64L;
if(FilterGyroRoll > 0) TrimRoll += ((int32_t)abs(CouplingNickRoll) * FCParam.AxisCouplingYawCorrection) / 64L;
else                TrimRoll -= ((int32_t)abs(CouplingNickRoll) * FCParam.AxisCouplingYawCorrection) / 64L;

void MotorControl(void)函数里：

PDPartNick = PPartNick + (int32_t)((int32_t)GyroNick * GyroPFactor + (int32_t)TrimNick * 128L) / (256L / STICK_GAIN); //  +D-Part

关于TrimNick 变量的用处，可能不像feng_matrix说的那样“偏航轴的角速度比例负反馈”。

这个变量很蹊跷，俯仰和滚转有关，滚转又和俯仰有关。就是一个角度乘上一个角速率，呵呵。奇妙啊。而且呢还有这几个常量是可以在tools软件里设置的：FCParam.AxisCoupling1；FCParam.AxisCoupling2；FCParam.AxisCouplingYawCorrection。

那大家就可以拿一个烟盒来比划一下啦！

当烟盒俯（或仰）一定角度时（例如90度），滚转轴再正（或负）滚一个角度（例如90度）：加下现在烟盒姿态；

归零，当烟盒滚转轴正（或负）滚一个角度（例如90度），再俯（或仰）一定角度时（例如90度）：加下现在烟盒姿态；

两次姿态完全不同吧。

所以，就是说，但俯仰了一个角度后，滚转轴测得并不是真正的滚转（真正的滚转是地理坐标系里的），那么就要补偿上去一个值，就是Trim系列变量的作用。

当然啦，我们的四轴不可能滚转的或者俯仰的角度很大，翻转除外（这也是为什么翻转的时候不执行代码），所以呢我们就乘上个定值系数例如：FCParam.AxisCouplingYawCorrection。这只是个省事的办法，其实不同的角度要乘的系数是不一样的，但是这里就当是一样啦。因为我们的处理器能力有限

可能不对，请谅解

yuxuanqq · 发表于 2011-3-28 00:41:00

我看的是0.72p英文版，可能代码有些不同

yuxuanqq · 发表于 2011-3-28 01:27:34

可能上边的例子说的不是很清楚，那么再来举个例子：

还是烟盒呵呵

先让烟盒抬头也就是仰30度，在让他正偏航90度，你发现什么啦？呵呵，综合效果竟然变成了滚转了90度。

也就是当你正偏航转90度得时候，这个偏航角速率就让在引起滚转和低头（也就是俯）两个角速率，你能不去修正他们吗？

这就是为什么加这段代码啦。也叫他解耦合。

yuxuanqq · 发表于 2011-3-28 14:06:00

好，上传一个文件，可以参考一下其中关于平台误差角的计算公式。

点击此处下载 ourdev_626129BDVP0F.doc(文件大小:73K) (原文件名:惯性导航系统.doc)

它描述的底一项误差和第三项误差，在MK的代码里，都是用加速度积分出的姿态予以修正的，而第二项也就是耦合误差，是这段程序的意义。但是要注意的是文件里的公式在应用到四轴自动驾驶仪中需要有些变化。因为四轴俯仰和滚转的控制不是独立的。

yuxuanqq · 发表于 2011-3-28 14:08:46

我一直在回答这个问题，不是为了别的，是为了说明为什么MK飞的那么稳可以弹钢琴！

我们对细节的把握比德国人差多拉。

也说明，人家理论上的把握很到位，但是相信MK也不是一个人在做，是团队的结晶。不同专业、不同理论水平。

yuxuanqq · 发表于 2011-3-28 14:35:58

再传一本书，关注121和122页

点击此处下载 ourdev_626139A0NCDS.pdf(文件大小:6.91M) (原文件名:惯性技术_邓正隆.pdf)

shaolinzuqiu · 发表于 2011-4-18 10:58:28

论坛办的不错，对于审核，我表示理解，希望可以快一点，

请教：德国人算法中，将偏航轴的运动耦合到其它两轴上的算法，是一个什么原理呢

阿莫论坛20周年了！感谢大家的支持与爱护！！