i7456 发表于 2011-7-14 13:59:50

加速度传感器 和 角速度传感器 的 数据融合——互补滤波，想自己写 四轴程序 和 自平

核心公式：

angle = (0.98)*(angle + gyro * dt) + (0.02)*(x_acc);

angle 姿态角
gyro角速度
x_acc 加速度计算出来的姿态角

0.98，0.02 系数，和为1，与滤波的要求有关



详细的内容看这里：
http://www.ourdev.cn/bbs/bbs_content.jsp?bbs_sn=4493448&bbs_page_no=1&search_mode=1&search_text=The

Chester 发表于 2011-7-18 10:14:49

谢谢你！

my_love 发表于 2011-7-18 11:14:56

收了

majianjia 发表于 2011-7-18 11:58:33

我还是不喜欢加速度计来打扰陀螺仪...静态的就无所谓了，动态的我感觉只会越来越乱，还是磁阻之类的来修正陀螺仪比较合适

jmp2002911 发表于 2011-7-18 14:28:06

我觉得还是德国四轴的算法更科学。加速度计受震动影响大，单个值可能会很大，会把姿态搞乱。在短时，应该相信陀螺，用加速度计来修正陀螺的漂移

i7456 发表于 2011-7-18 14:42:49

回复【3楼】majianjia 阿嘉^_^
-----------------------------------------------------------------------

http://code.google.com/p/arducopter/
这个Ardupilot 也用到了三轴的磁阻传感器，但是运行一段时间后方向就会偏，不知道是程序的问题，还是外界磁场变化的干扰。

i7456 发表于 2011-7-18 14:45:58

回复【4楼】jmp2002911
-----------------------------------------------------------------------

上面的公式 也是在短时，相信陀螺仪；静止时的结果，相当于的加速度传感器的平均值，也不存在单个值很大，把姿态搞乱的现象。

mk的算法，实在看不懂。这个很简单。

jmp2002911 发表于 2011-7-18 16:18:33

你的公式里不是每一次计算都有0.02的加速度计的值吗？相当于加权平均吧？

asha 发表于 2011-7-18 17:43:00

我就搞不清楚了，大家这些公式是怎么来的？人家的这些公式里面的系数都是不同应用条件下都不同的，直接照搬就能对了？不知道你这些互补系数是怎么来的。这个和载体的运动状态是相关的。

sufangzhou 发表于 2011-7-20 17:30:42

这个好像只是普通的加权平均吧？？
我记得自适应的互补滤波权值是动态的

i7456 发表于 2011-7-21 10:58:25

回复【8楼】asha
-----------------------------------------------------------------------

我又没让你直接用公式中的那个系数。至于系数取多少，楼主位的链接中有个PDF文档，讲的很清楚。

况且我也指出了：

“0.98，0.02 系数，和为1，与滤波的要求有关”

liguole 发表于 2011-7-26 22:37:56

回复【10楼】i7456
-----------------------------------------------------------------------

我在世界公园附近 希望能得到更详细的交流

i7456 发表于 2011-7-26 23:30:54

回复【11楼】liguole 李国乐
-----------------------------------------------------------------------

邮件吧，qqizx@126.com

aliveghost 发表于 2011-7-26 23:40:11

数据融合， mark

sunicecream 发表于 2011-7-27 02:12:48

MARK一下，以后有钱也玩下四轴

Clinging 发表于 2011-7-27 08:53:57

回复【楼主位】i7456
-----------------------------------------------------------------------

把两个角度的误差PI运算后增加到结果中可能更合适些，这种方法也就在两轮车上用用，和四轴不是一回事

autopass 发表于 2011-7-27 09:33:51

加速度计的数据多次平均之后还是可以用的,但是就是这个平均的时间有点长,有个这样的想法,不知道可不可行:

假设求n个点的平均,那么得出平均的延时就是n * dt / 2,

假设从t0 时刻开始计数加速度积分,到t0 + n * dt / 2时刻,开始计算延时的这段时间的角度偏差 gyro_di,即计数陀螺仪
从n * dt / 2 到n * dt这段时间的积分

然后计算出由加速度计得到的角度,加上gyro_di 直接替换陀螺仪积分

asha 发表于 2011-7-28 19:02:50

回复【10楼】i7456
-----------------------------------------------------------------------

你这最多是个加权滤波，只能靠经验来搞。我可以告诉你，这个公式里的系数如果是个动态的才对，而且加权系数和不一定为1.请看看《惯性导航这本书》

i7456 发表于 2011-7-30 19:07:53

回复【17楼】asha
-----------------------------------------------------------------------

首先，这个不是我的，是老外写的一个入门级的文章，推荐给想入门的人看看，专家可以忽略。
我只是把里面的重点内容复制出来，给大家个预览（没有任何内容的转贴，我一般也是不看的）。

其次，我不是搞惯导的，工作和惯导一点不相关，我只是对四轴稍微有点兴趣，简单了解下而已.你推荐的“《惯性导航这本书》”，我肯定没有精力去看。

另外，你是否能够/愿意写篇文章，用通俗易懂的文字，给大家讲一讲（总结一下“《惯性导航这本书》”）这个动态的系数改取多少，如何取？我等菜鸟感激不尽。

cuzn 发表于 2011-7-31 17:24:11

问下各位达人,能不能使用ENC-03和加速度传感器对平面角度进行精确测控.
还有四轴使用的数据融合的主要目的是不是只是让让四轴水平面平稳,而不管航向?

jmp2002911 发表于 2011-7-31 19:35:00

回复【19楼】cuzn
-----------------------------------------------------------------------

加速度计无法给出航向的角度，因此无法修正航向角。

asha 发表于 2011-7-31 20:37:26

回复【18楼】i7456
-----------------------------------------------------------------------

恩，或许我说的又点重，因为看到太多不对的东西。对于你的提议，我表示无能为力，因为《惯性导航》的确是很复杂的一个东西，而且关于这个动态系数的选择本身就是做不到的，因为你不清楚，加速度传感器里面包含了多少线速度分量。如果可以我建议大家买一本书看一看，《惯性器件建模》，《惯性导航》，《微惯性技术》。

warmonkey 发表于 2011-7-31 20:48:10

LZ公式完全属于胡搞，正规做法是卡尔曼滤波。EKF甚至是UKF有更好的效果。

416446891 发表于 2011-7-31 23:22:30

MARK

nop_linyh 发表于 2011-8-1 12:10:33

mark留言

i7456 发表于 2011-8-1 14:48:42

回复【22楼】warmonkey
lz公式完全属于胡搞，正规做法是卡尔曼滤波。ekf甚至是ukf有更好的效果。
-----------------------------------------------------------------------

对于量子力学来说，经典力学的很多公式、结论都是错误的，但是我们在平时的工作、学习、生活中，用到的最多的还是经典力学。

简单的、经验性的东西也有他应用的场合。

如果你非要拿上面的公式，去控制载人的飞机，肯定是缺xx；
同样，用几元钱的8位单片机跑 卡尔曼滤波、ekf、ukf，做玩具型的飞控，如果你能够/愿意 做出产品，低价卖给大家用，我们菜鸟会感激不尽的。

asha 发表于 2011-8-1 20:56:27

回复【22楼】warmonkey
-----------------------------------------------------------------------

猴子，你也在里面

sufangzhou 发表于 2011-8-3 18:50:35

正好手里有材料，就做了个平衡小车实际测试下了
传感器是MMA7361和ENC-03
嘛，说说我实际的体验吧

数据的融合，在这儿貌似用的多的卡尔曼和互补了
卡尔曼看了很多资料还是不懂。。。。就去copy了一个，结果嘛。。。比较惨。。。最大的问题就是数值收敛太慢了。。。
后来就改成互补滤波了
就是照着MIT的那篇论文写的程序。
嘛，说白了也就那么一行程序而已。。。。

angle = (0.88)*(angle + gyro * dt) + (0.12)*(x_acc);

结果比起单独的传感器要好，但实际上还是抖动的厉害
尤其是ENC的抖动对结果的影响太大
改动了很多次权值后，结果也不理想

后来吧，就想着把这个改成了自适应的。
说是自适应，其实也没什么。
就是先计算合加速度是多少，再和重力加速度做差
再来引入两个新的变量——m，n
它们都根据应用环境的噪声情况综合确定：n要在运动加速度不大时保证互补滤波静态精度；m的选择可以根据互补滤波可以容忍的上限值 n／m确定。
我是选择了n=3,m=15
也就是说，在合加速度与重力加速度的差值(ki)小于n/m=0.2G的时候，做互补
滤波系数就是t=-m*ki+n
而这个t再根据MIT的那论文算出陀螺仪和三轴加速度的权值
这样下来，基本上没有什么问题了。可是数据还是有些抖动
于是又在后面加上了均值。
互补的更新率是1000HZ，10次做一次均值。
总的更新率是100HZ。出来效果还不错

比起卡尔曼来说，互补不仅计算量小，而且也好理解，个人很推荐~~

想把这个用到四轴上不知是否可行？一直很担心电机振动对传感器的影响。。。。

不知道大家的互补是如何做的？？

wshtmcz 发表于 2011-8-3 21:47:01

学习

wshtmcz 发表于 2011-8-3 21:50:58

回复【27楼】sufangzhou
-----------------------------------------------------------------------

您好！
前辈！哪有MIT的那篇论文，我想看一下！麻烦上传个，或者给个链接！谢谢！

sufangzhou 发表于 2011-8-3 22:13:41

回复【29楼】wshtmcz
-----------------------------------------------------------------------

就是LZ给的链接里面有

wshtmcz 发表于 2011-8-4 08:53:29

回复【27楼】sufangzhou
-----------------------------------------------------------------------
您好！
我觉得您这方法不错，想具体请教下，合加速度如何求的？是陀螺仪与加速度计的矢量和？另外所说的滤波系数t是angle + gyro * dt前的系数，如您上式中的0.88？

sufangzhou 发表于 2011-8-4 17:33:36

回复【32楼】wshtmcz
-----------------------------------------------------------------------

合加速度就是三轴加速度的矢量和嘛
我这里的滤波系数就是原文里第15页的那个。。。。（抱歉打不出那个字母。。

lihua 发表于 2011-8-6 01:35:11

http://v.youku.com/v_show/id_XMjY4NjU3NzE2.html
我用L3G4200+ADXL345做的姿态检测，采用的就是互补滤波器，200Hz输出频率，5mS积分周期，静止状态下角度变化不超过0.1度，动态陀螺2000度/秒，更没问题了。看看效果吧，就一个平衡系统而言，EKF，UKF那些不一定是最优的最适合的

asha 发表于 2011-8-6 13:21:11

回复【35楼】lihua
-----------------------------------------------------------------------

哈哈，别总说静态环境下， 精度我能做出0.01。动态下加表不能用，你的陀螺量程虽然大，但是精度和稳定性一定差

wshtmcz 发表于 2011-8-11 11:06:43

回复【32楼】wshtmcz
回复【27楼】sufangzhou
-----------------------------------------------------------------------
您好！
我觉得您这方法不错，想具体请教下，合加速度如何求的？是陀螺仪与加速度计的矢量和？另外所说的滤波系数t是angle + gyro * dt前的系数，如您上式中的0.88？
-----------------------------------------------------------------------

您好！
前辈！我问下：
（1）n怎么确定：什么是互补滤波静态精度？运动加速度不大具体是多少？
（2）m怎么确定：什么是互补滤波可以容忍的上限值？
（3）n=3，m=15是在什么情况下确定的？应该还可以有其他的取值，例如？
（4）τ=-m*ki+n这个怎么得到的？

haigerl 发表于 2011-8-11 11:29:46

mark

robotkid 发表于 2011-8-11 14:46:35

主要是德国的姿态看不懂调节重加和角速传感的比例,卡尔曼也不是很神奇的,陀螺仪的权重加大反响还是有点相差,这些算法自己想是很头疼,有人一说就明白的,但懂的多没心情细讲.

robotkid 发表于 2011-8-11 14:49:54

其它自编的算法都没有德国和放出来的卡尔曼的好.主要还是大家手上的陀螺传感漂移大.

largeboss 发表于 2011-8-11 15:11:32

mark

dancerindark 发表于 2011-8-11 21:44:20

回复【22楼】warmonkey
lz公式完全属于胡搞，正规做法是卡尔曼滤波。ekf甚至是ukf有更好的效果。
-----------------------------------------------------------------------

兄弟，一个补偿滤波器被你视为胡搞啊？
ukf是效果好，可是cpu运算速度要足够的快哦。

asha 发表于 2011-8-13 07:36:50

回复【43楼】dancerindark
-----------------------------------------------------------------------

哈哈，互补滤波是需要动态调整参数的，这个不能算是互补，只能说是个经验公式。

bulinsheng 发表于 2011-8-14 09:57:42

。。。。。。。。。。

sufangzhou 发表于 2011-8-14 13:59:34

回复【38楼】wshtmcz
-----------------------------------------------------------------------

（1）那个n，你可以看看代换的公式，说白了，这个n就是你有多相信陀螺仪，n越大陀螺仪的权值就越大
（2）关于容忍上限（n/m），就是合加速度和重力加速度的差值上限，我设定的是0.2G，一旦超过0.2G，说明外力太大这样算出来的角度误差会很大，还不如就只相信陀螺仪
（3）m，n这个仅仅是我这样的情况下的值，我只能说根据自己情况调整吧。。。
（4）关于这个公式，看看这个吧http://www.doc88.com/p-58671312987.html

wang_ww 发表于 2011-8-16 12:55:09

回复【27楼】sufangzhou
-----------------------------------------------------------------------

你好，我用同样的硬件也遇到了类似的情况，收敛太慢了，能不能指教下如何调试？方便的话留个联系方式，谢谢！

zjihtt 发表于 2011-8-29 14:53:33

好贴，有时间慢慢看。

ntcjx 发表于 2011-9-14 01:01:58

http://cache.amobbs.com/bbs_upload782111/files_45/ourdev_676342C2RQZ0.jpg
(原文件名:互补滤波.jpg)
通过串口 发送到上位机软件显示的
M16采集ADC 简易互补滤波    绿色是MMA7260 X轴输出   红色是ENC-03 角速度蓝色 是 简易互补滤波后的输出角度
请大家给个建议 就这样的精度可以做自平衡车吗，准备做个载人的

wers_l 发表于 2011-9-14 07:56:49

MARK

loongsuns 发表于 2011-9-14 08:51:04

这个公式在mit的网站上老外diy segway的那个页面有文档介绍的挺详细的。

liang_work 发表于 2011-9-15 21:02:49

顶.

liang_work 发表于 2011-9-15 21:20:34

回复【27楼】sufangzhou

后来吧，就想着把这个改成了自适应的。
说是自适应，其实也没什么。
就是先计算合加速度是多少，再和重力加速度做差
再来引入两个新的变量——m，n
它们都根据应用环境的噪声情况综......
-----------------------------------------------------------------------

后面的这一部份看得不是太明白,能实际用一些参数,一步步演示一下吗?多谢!

416446891 发表于 2011-9-15 21:54:16

MARK

oldbreadman 发表于 2011-9-15 23:02:34

最近正在研究这个,学习学习,等我研究透了也好穿裤子...

dujun168 发表于 2011-9-19 14:59:22

mark

lpgyf 发表于 2011-11-3 01:39:36

mark

lpgyf 发表于 2011-11-3 02:20:59

mark

nirvanasyl 发表于 2011-11-3 15:14:17

MARK 数据融合

zerok 发表于 2011-11-3 16:15:32

学习了

qiuzai 发表于 2011-11-14 22:25:11

我要好好学习才行啊！@

szy494468597 发表于 2011-11-22 15:46:52

看不懂   慢慢学习

HYZ1989 发表于 2011-11-22 16:05:40

先收藏了！！

lzf0228 发表于 2011-11-23 18:22:58

回复【19楼】cuzn
-----------------------------------------------------------------------

学习中

denglu 发表于 2011-11-30 10:04:10

这个互补滤波单独使用的话，走直线还行，做回转运动就会发现不是倾斜就是后仰。特别是做圆锥回转更明显。

ligongxiaobie 发表于 2011-11-30 18:43:12

回复【27楼】sufangzhou
-----------------------------------------------------------------------
最近也在弄平衡车 但对互补滤波和卡曼滤波都不会哥们你说的很好 我受教了 不过你试过用互补滤波做转弯吗？？

clqfly 发表于 2011-12-10 14:45:40

mark研究

lw-seed 发表于 2011-12-11 06:34:33

先MARK

largeboss 发表于 2011-12-11 09:56:42

mark

Ca_guo 发表于 2011-12-11 10:15:41

这个公式其实就是低通滤波器（也叫惯性滤波）的离散公式：
Y(k) = (Tc/(Tc+Ts))*Y(k-1) + (Ts/(Tc+Ts))*X(k)
其中Tc为滤波时间系数，Ts为采样时间，X(k)为当前采样值,Y(k)为滤波器输出值,Y(k－1)为滤波器的上次输出。
Tc = 1/wc = 1/(2*pi*fc);fc也就是通常所说的截止频率。如果知道信号的频率要求，fc应该就可以确定下来。

kiema 发表于 2011-12-11 13:46:35

mark

Garbage614 发表于 2011-12-11 22:47:03

mark

zl6977 发表于 2011-12-18 12:34:21

回复【27楼】sufangzhou
-----------------------------------------------------------------------

哥们，我的情况跟你描述的很像啊。也是静态的时候很好，动态的时候受水平加速度干扰太大，还没解决，打算按你说的动态调整权值的方法试试，不知能不能把这个地方说的再详细些呢，或者请您贴一下相关部分的代码？我觉得这里才是核心部分啊，那个MIT的公式一查就有，谁都会，可是你的这个方法就不一定谁都想到了。

zl6977 发表于 2011-12-18 12:43:33

回复【69楼】denglu
-----------------------------------------------------------------------

所以您的建议是如果要平衡小车行走，那么一开始就应该用卡尔曼滤波是么?

suyee830916 发表于 2011-12-21 17:03:29

回复【楼主位】i7456
-----------------------------------------------------------------------

学习了，很感谢

jiajiajia 发表于 2011-12-27 22:03:26

MIT的那篇论文只对角度值作了修正，而角速度值直接用的陀螺仪的测量值，也就是说此处的互补滤波并未对角速度值作修正，这样可以吗？
在用PID算法计算PWM输出值的时候既要用到倾角值也要用到角速度值啊，如果角速度值含有漂移，会不会影响控制效果？

请教大家了~

hanwenzheng37 发表于 2012-1-13 11:07:25

回复【5楼】i7456
-----------------------------------------------------------------------

我咋打不开这个网页啊 ，写的啥内容能不能给我发一下 ？hanwenzheng37@163.com 多谢啦

hanwenzheng37 发表于 2012-1-13 11:26:35

回复【3楼】majianjia 阿嘉^_^
-----------------------------------------------------------------------

对于用磁阻传感器修正陀螺仪可靠吗？如果陀螺仪的精度本身就不高的话，就是没法修正啊

walux 发表于 2012-1-13 11:42:21

mark 加速度传感器 和 角速度传感器 的 数据融合——互补滤波，想自己写 四轴程序 和 自平衡小车 程序的，可以参考

walux 发表于 2012-1-13 11:43:42

mark 加速度传感器 和 角速度传感器 的 数据融合——互补滤波，想自己写 四轴程序 和 自平衡小车 程序的，可以参考

ljgvictory 发表于 2012-1-13 13:45:15

mark

MrLCL 发表于 2012-1-29 12:15:43

顶

ERDTxiduoduo 发表于 2012-1-29 22:25:11

mark

kiema 发表于 2012-1-29 22:38:35

mark

thxl_kis 发表于 2012-3-9 17:44:15

mark

zhuml2012 发表于 2012-3-28 13:19:22

不错，学习了，看来还要懂算法啊

thxl_kis 发表于 2012-4-1 22:18:53

多讨论啊
不错

springsky 发表于 2012-5-28 12:17:04

sufangzhou 发表于 2011-8-3 18:50 static/image/common/back.gif
正好手里有材料，就做了个平衡小车实际测试下了
传感器是MMA7361和ENC-03
嘛，说说我实际的体验吧


高人能否将m,n的确定方法详细的说一下，不胜感激。

uddiao 发表于 2012-7-4 19:33:55

你好，因为论坛升级，已经找不到你帖子里的链接了，，里面的资料能发我一份吗，谢谢了317466157@163.com

long578553882 发表于 2012-9-26 08:00:29

mark......

jlian168 发表于 2013-1-7 10:38:39

Sir:
链接無效!谢谢.

g921002 发表于 2013-1-9 08:44:03

Complementary filtering and Simple Kalman filtering 只是 The Balance Filter 補充版，參考看看囉!!
如果是用姿態估測，其實Complementary filtering就夠了，差別只在加速規的融入係數的Time Constant.

用KF、EKF、UKF...都不會比較好，原因很簡單，就是加速規外力項被歸類在噪訊，通常加速規是屬於量測模型，所以量測模型錯的，結果怎麼會對呢？
結果所以不管那個濾波器，加速規的信度都是比陀螺儀小的，失去了用統計系統模型估測的意義，白白浪費cpu time.

hellocarter 发表于 2013-1-30 19:38:38

asha 发表于 2011-7-28 19:02 static/image/common/back.gif
回复【10楼】i7456
-----------------------------------------------------------------------



楼主这个公式是可以推导出来的，对陀螺仪作高通滤波，加速度计作低通，二者取相同截止频率，最终将微分变成差分方式就可化成楼主的公式，这个公式乍看确实简单，但确实不是不乱凑的，不好用加权的思路去看这个公式，这个公式起码是能够很好地消除陀螺仪的漂移的。

yat 发表于 2015-3-12 19:43:22

mark 互补滤波
页: [1]
查看完整版本: 加速度传感器 和 角速度传感器 的 数据融合——互补滤波，想自己写 四轴程序 和 自平