共轴双桨自动驾驶直升机

c_nmusic

最近一直在研究MK的加速度计和陀螺仪的融合代码。我看的是经过killagreg重新修改过的MK 0.71h的代码，里面把原版的德文函数名、变量名、注释等大部分都翻译成了英文，看起来容易不少。 V0.71h Code Redesign killagreg.rar (186.5 KB, 下载次数: 135)

2009-4-29 14:21 上传

点击文件名下载附件

经过一段时间的理解，我觉得MK的融合部分一共通过3种方式实现。

第一种是即时融合，通过当前加速度计的数值做移动平均得出大致的偏转角，然后和陀螺仪积分进行对照，当中用到一个系数。得到的差异经过限幅后补偿进I里。这种逻辑UAVP也有，不过UAVP只是将幅度限制到了+1，-1之间。而且这个逻辑是UAVP 3.15里的唯一融合逻辑。

具体代码位于fc.c文件的890-946行。

第二种是稍微长期一点的融合。通过一个256次累加陀螺仪和加速度计的积分，然后将积分总数除以256得出平均的加速度计积分和陀螺仪积分之间的差异，依次作为融合参数。

这个逻辑UAVP中没有，而且这个逻辑我觉得是比较关键的一个逻辑。如果只有第一种逻辑非常容易引起震荡，原因就是在四轴平衡时积分并不为0。但这种逻辑会和第一种逻辑打架，所以参数调整好是重点。UAVP中的取代代码是在积分过程中不断对积分值进行衰减，用+1，-1的方式让积分逐渐回到0。

还有一个就是由于这个逻辑是将加速度计和陀螺仪的积分比较，而加速度计只要有偏转积分就会一直增加，而陀螺仪不是。所以在实际使用时要注意调整量的大小。在MK里对这个调整量连续除了2个256。

这部分代码位于fc.c文件的949-992行。

第三种逻辑是用来修正陀螺仪的0点漂移的。这部分代码我看的不是很明白，而且看起来这个代码默认没有启用，所以也没花太多时间去理解。

这部分代码位与fc.c的1002-1083，可以看到它最后都修正AdNeutralRoll和AdNeutralNick了，就是陀螺仪的中立点参数。

目前我也实现了前2种融合逻辑，现在的问题是相应速度太慢，都倾斜1秒钟了马达才开始有反映。:em25:
还需要不断调整啊。:loveliness:

一锅

我顶！！！！又一次阅读了一遍

c_nmusic

刚才又对这个数据融合有了一点心得，发到这里和大家分享一下。

本来如果陀螺仪没有漂移问题，当四轴机身水平时，陀螺仪的积分（也就是四轴的机身倾斜角度）应该=0。这样才能实现稳定悬停。

可是现实是残酷的，ENC-03的漂移不但存在，而且很厉害。所以我们就必须通过加速度计的倾斜程度来推断四轴到底倾斜了多少，依次来补偿陀螺仪的漂移。

就像刚才说的，如果用的混合算法可以做到当四轴水平且积分=0的时候，也就是我们成功的时候。依照这个为目标，目前我的融合算法大约有0.2秒左右的延迟。也就是当四轴已经平衡了，但积分还没有归0。再加上马达、桨的延迟，就造成了现在的晃来晃去的问题。

所以，如果能实现一种算法，再四轴将要水平之前提前让积分归0，加上马达、桨的延迟，或许就可以完美地实现悬停了。

根据MK的第一种算法，由于对陀螺仪的积分补偿是一直累加上去的，所以增加的越多，衰减起来也就越慢。这样就只会造成调整延后而不是提前。我想这也许就是MK要引入另外一个补偿的原因吧。

又仔细看了一下MK相关部分的代码，证明了我的猜测。第二种补偿算法的补偿量和第一种的符号相反，不是增加而是互相衰减。

[ 本帖最后由 c_nmusic 于 2009-4-29 16:22 编辑 ]

c_nmusic

上2个图说明比较容易些。



这是原始的陀螺仪积分和加速度计的关系，在马达不转，没有震动干扰的情况下。
我将四轴顺时针转一下，然后逆时针再转一下，最后回到水平位置。
可以看到陀螺仪的积分很快就远离0点了，即使我已经最后将四轴放到地面上了也一样。
这就是陀螺仪的漂移问题。同样速度顺时针转90度再逆时针转90度最后的积分并不为0。



这是利用MK的第一种补偿后的结果。可以看到不管怎么转，陀螺仪的积分都被和加速度计“捆绑”到了一起，不再偏离0点了。

不过这些都是无马达干扰情况下的数据，等到马达一转起来，连加速度计都不准的时候，问题就比较麻烦了。

c_nmusic

经过这段时间的刻苦“摔机”，终于明白了商品化电调和I2C电调的在四轴飞行中的重大区别。

商品化电调可以飞四轴，也可以飞稳。但是，这是在没有要求自动回中情况下的。也就是如果操作遥控器前飞，四轴倾斜30度后，即使不操作遥控器，四轴也无法自动回中。或者说是在5秒内无法自动回中。即使代码里这么写了也无效。

原因就是商品化电调的低响应速度。如果你想做到自动回中，那么势必需要在四轴倾斜时施加反向的力把四轴拉平，这个力和倾斜角度以及期望会中的时间相关。而商品化电调恰恰就弱在这里。如果你给它一个比较大的马达转速，就不要期望他能很快地将转速降低下来。这就造成不要自动回中还能飞稳，一旦加上这个相对大的I参数，整个四轴就开始大幅度抖动，好像抬轿子。通常的做法是在出现晃动时加大P参数，可商品电调的低响应速度会在加大P后让你连起飞都无法完成，还没离开地面呢就开始抖。

在这几天试验下来，我按照MK的四轴的反应调整了I参数，可以做到在倾斜时用一个比较合适的力量把四轴拉平，然后在试飞时调整P参数。结果我一直调到无法起飞，也无法找出一个合适的P可以即做到自动平衡，又可以顺利起飞。

今天换上了I2C电调，效果就是好。同样的代码，我增大了I、P参数，一点问题都没有。在商品化电调完全不能飞的参数下，我的四轴现在飞行已经相当平稳了。


就从录像效果来说，用商品电调和I2C电调基本看不出差别，都可以飞得很稳。我在测试中也确实做到用商品电调飞稳到震动幅度小于3度，基本看不出来了。这2个差别主要在操作者这里，用商品电调操作者基本要控制住四轴的一切行动，包括进行方向操作后的回到平衡位置的动作，都必须顾及到。原因就是商品电调内部的PID控制器，阻止了自动回中的大修正量一下子变小这个动作。为了飞稳不晃，只有降低I参数。而用I2C电调就没这个问题了，方向控制完了松开遥控器四轴就会立刻自己会到平衡位置。目前我的四轴也可以做到这点了。

下面就要进一步减小晃动幅度。看起来用I2C电调后加速度计的干扰比以前厉害多了，经过10次加权移动平均后还是这么厉害。也许和电调的快速调整有关吧。



一旦加速度震动上去了，势必会影响陀螺仪积分，这也就会造成四轴晃动了。

[ 本帖最后由 c_nmusic 于 2009-5-3 16:29 编辑 ]

狂人摄影

楼主是搞科研的吧，一下子做了这么多代产品，不错，看着都有点痴迷了，如果有很好的成品了请报个价买你一套回来鼓捣也算是解毒吧。

c_nmusic

原帖由 狂人摄影 于 2009-5-3 16:24 发表
楼主是搞科研的吧，一下子做了这么多代产品，不错，看着都有点痴迷了，如果有很好的成品了请报个价买你一套回来鼓捣也算是解毒吧。

目前基本上还没有哪个可以说是“成品”，虽然已经交了N多学费了。:em15:

thl315

我做的红外平衡仪试飞成功. 仅用方向舵和油门控制飞机. 下一步是接上GPS模块,试验自主飞行.
GPS导航程序用另一片M8. 很幸运的仅用92%的空间完成了程序.
在此再次感谢楼主无私奉献的PPM代码.  
附视频如下:
[cc]flash/player.swf?videoID=13800_5066311&autoStart=false&share=true[/cc]

[ 本帖最后由 thl315 于 2009-5-3 17:10 编辑 ]

c_nmusic

楼上厉害哦，赞一个！
:em26:

thl315

楼主继续更新阿,我们很关注...

c_nmusic

最近倒是写了一些东西，不过由于目前没有多余配件了，所以新四轴的飞行视频还要等几天才能放上来。

thl315

很珍贵的资料.谢谢楼主.

liang_work

楼主,有没有QQ啊,想向你请教一下.
我的QQ:672884926

c_nmusic

好啊，加我QQ 15262586

c_nmusic

新机架第一个视频，目前动态性能还不够稳定，一有方向操作就会晃。
代码还需要进一步优化。

thl315

不错阿.都听到剪树叶的声音了.

c_nmusic

原帖由 thl315 于 2009-5-9 22:10 发表
不错阿.都听到剪树叶的声音了.

不是故意的哦，可怜的柳树....:em12:

c_nmusic

目前的问题，主要还是在距离平衡位置正负15度的时候，自动平衡的速度很慢。
这就造成了方向控制上的问题，大幅度可以恢复，但快恢复到平衡位置的时候恢复速度就慢下来，于是就见四轴冲着我开始“俯冲”....:funk:

刚才试验增大I，可是要想飞稳，I增大后P也要增大，否则无法抵消大I引起的大角速度。可是P又不能无限大下去，刚才测试P490的情况，和陀螺仪的数据一乘发现纠正量要5万多，早已超出调整范围。:em12:

看来P和I的有效区间只有1个而不是多个，想做到小角度快速平衡还需要别的策略。

c_nmusic

大概根据我记忆中的有效P、I参数画了一个图。



在那个图形中间区域的是可以飞行的参数区域，看起来范围不是很大。

zrsky

顶起，精彩的帖子