kalman进行姿态融合时如何处理设备震动时加速计带来的干扰?

s19910223 · 发表于 2013-4-15 21:17:13

最近在用Mini-RHRS做姿态融合，使用的是kalman滤波算法，预测方程用的是：陀螺仪得到的四元数，测量方程用的是：加速计和磁力计获得的四元数
参考的这篇文章
An extended Kalman filterfor quaternion-based orientation estimation using MARG sensors
用的是高斯牛顿法通过加速计和磁力计来获取四元数
但是有个问题，当设备在剧烈运动时，或者线运动时，姿态表现的不平稳
eg板子水平快速移动，横滚角也会出现抖动。保持板子姿态，并给予震动，姿态会出现非常不稳定的情况（Mini-RHRS原代码不会出现这个问题）

思考：kalman有两套输入，陀螺仪和加磁的四元数，由于震动的时候加速计3轴分量的和加速度不为1g（大于或小于1g），导致算出来的四元数是有误差的
进行kalman融合后的姿态角也就问题了。
现在能想到的是：通过调整Q，R来动态解决，或者线运动时不进行姿态解算

请问各位这个问题该如何处理呢，各位有什么好的方法么？
先谢谢啦

lxl_lw · 发表于 2013-4-15 23:35:46

风闻：加速过程很短暂，四轴机体惯性会“过滤”掉这段传感器数据不可靠带来的影响。看了别人做四轴，几乎都没有考虑这个。我也想不到解决办法，打算不考虑这个不稳定阶段试试。

ahuang227 · 发表于 2013-4-16 08:38:55

感觉应该在加速度计不等于零的时候不参加融合。

asha · 发表于 2013-4-16 13:27:08

1、可以在震动时候不参与融合；
2、对外部加速度的R进行估计；
3、改进模型。
PS 高斯牛顿是指的什么？EKF？

asha · 发表于 2013-4-16 13:30:11

看了一下，是高斯牛顿迭代法吧？我感觉这个方法越用影响越大，MIMI-AHRS，可能在振动的时候不参数运算。

s19910223 · 发表于 2013-4-22 23:06:01

asha 发表于 2013-4-16 13:30
看了一下，是高斯牛顿迭代法吧？我感觉这个方法越用影响越大，MIMI-AHRS，可能在振动的时候不参数运算。 ...

忙起来忘记自己在论坛里发帖了
asha一言惊醒梦中人呐，现在改了下，就是有线运动的时候，不用加速计磁力计的数据进行姿态融合了，效果好了很多
但是又出现新的问题，线运动停止后，会存在一个1-2s的收敛时间，现在得想法子把这玩意给弄掉。

高斯牛顿迭代确实用的有点问题，试着改用梯度下降法，效果也稍微好点。

mini AHRS，其实用的是龙格塔库法（不记得几阶了），直接用陀螺仪角速度，转为四元数率，进行积分，积分前角速度是经过加速计磁力计修正后的（具体咋修正的我也没太看明白）

换模型的话，因为也不懂这玩意，只能参考现有的模型。

调整R的话，到是查到了自适应kalman滤波，还没弄明白。

s19910223 · 发表于 2013-4-22 23:08:04

lxl_lw 发表于 2013-4-15 23:35
风闻：加速过程很短暂，四轴机体惯性会“过滤”掉这段传感器数据不可靠带来的影响。看了别人做四轴，几乎都 ...

还没打算弄四轴，只是做单纯的姿态解算，弄了半天还没，还没出厂的代码效果好，郁闷呐

s19910223 · 发表于 2013-4-22 23:08:35

ahuang227 发表于 2013-4-16 08:38
感觉应该在加速度计不等于零的时候不参加融合。

试了下，这个可以，但是又出现收敛时间的问题了

asha · 发表于 2013-4-23 10:45:12

s19910223 发表于 2013-4-22 23:06
忙起来忘记自己在论坛里发帖了
asha一言惊醒梦中人呐，现在改了下，就是有线运动的时候，不用加速计磁力 ...

你那里有miniAHRS的代码？龙哥库塔是解算四元数微分方程的，精度不是很高。对于你用牛顿迭代出现收敛是因为，这个方法本来需要多次迭代才能收敛也就是一个计算周期内多迭代几个，最速下降法好点是因为本身收敛速度就比牛顿法快。对于你所说的miniAHRS中用磁和加表对角速率提前进行了矫正这个我不恨明白我估计应该不会这么做。

s19910223 · 发表于 2013-4-23 14:56:59

asha 发表于 2013-4-23 10:45
你那里有miniAHRS的代码？龙哥库塔是解算四元数微分方程的，精度不是很高。对于你用牛顿迭代出现收敛是 ...

miniAHRS 关键代码有人贴出来了，好几个帖子都这段代码的身影，说是修正姿态差，没太弄明白
http://www.amobbs.com/forum.php? ... p;page=4#pid6097112

虽然现在震动问题减小了，但这个收敛时间，有没有什么好方法处理啊

asha · 发表于 2013-4-25 13:57:27

s19910223 发表于 2013-4-23 14:56
miniAHRS 关键代码有人贴出来了，好几个帖子都这段代码的身影，说是修正姿态差，没太弄明白
http://www.a ...

我还以为你说的MINI-AHRS是XSENSE公司的那个，你贴的这个代码是个大路货，你把加表那块去掉，我估计还会好点。

nust5042 · 发表于 2015-8-8 21:48:22

楼主搞好了吗？求源码，向您学习，谢谢 122807637@qq.com

kalman进行姿态融合时如何处理设备震动时加速计带来的干扰?

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