姿态解算(一般情况下的卡尔曼)原创
在论坛里潜水很久了,上周我的姿态解算经历了断断续续一个学期后终于有所成果了。贴出来和大家分享下。第一次发帖子估计不会很漂亮,见谅。不废话了进入正题:首先从卡尔曼开始讲起,网上很多都是用的基本卡尔曼公式,但我觉得用一般情况下的卡尔曼,更容易建立状态方程。
一般离散线性系统的状态方程如下:
其对应的卡尔曼滤波迭代公式如下:
先给出初值P(0|0),然后迭代。
针对要滤波的四轴,分别对单轴建立状态方程,这样可以有效的减少矩阵运算,有没有带来其他的问题目前还没有发现。等下一步做优化的时候再做进一步的分析,大家觉得有问题也可以提出来,毕竟姿态这部分完全是自学的。状态方程如下:
其中X为状态变量,即加速度计测出的姿态角。Z为观测量。ω为角速度,即载体加速度,对应于一般离散线性系统的状态方程中的u。
将上式离散化有:
将其代入卡尔曼滤波的公式。过程中发现卡尔曼对个别参数并不敏感。尤其是w,v这两个干扰误差的协方差和均值。再迭代方程中我试着做了一定范围的改变,对结果没有明显的影响,可能是本身因为就是干扰量,他的值就是不确定的。
下一步,设航向角(Z轴旋转)为φ,俯仰角(Y轴旋转)为θ ,横滚角(X轴旋转)为γ。然后用欧拉角法,利用陀螺仪的角速度值换算到导航坐标,作为卡尔曼滤波的控制量u。
最后利用导航坐标和机体坐标之间的转换矩阵,用加速度计的值求出姿态角作为观测值。
用反三角函数得:
使用atan2函数的时候。Ax应带负号而不是Az。
加速度和角速度的值都需要先做标定,建立基准。由于加上电机后对加速度计的影响可谓相当的大。我这里取了80次的值取平均,再加上低通滤波器,对电机振动有了较好的克服。不过牺牲了不少速度,和灵敏度。我不知道大家用的是什么方法。倒是陀螺仪对电机的振动基本没什么响应。
地磁还没有弄好,不过准备调好这两个轴的控制后再去上地磁,做偏航角。
下面贴个视频,用手机拍的,大家将就了
http://v.youku.com/v_show/id_XNTI5MjA4NDA0.html?firsttime=29 传感器的初始化校准参考了这里的例子http://mbed.org/users/aberk/code/IMURover/file/7440a03255a7/IMU.cpp 不错,我的四轴硬件已经搭好了,就差软件了,希望楼主能将姿态解算部分代码上传下!谢谢
! flyforyou85 发表于 2013-3-19 20:36 static/image/common/back.gif
不错,我的四轴硬件已经搭好了,就差软件了,希望楼主能将姿态解算部分代码上传下!谢谢
! ...
哦,我完全是按上面的想法做的,你参考就可以了。具体的程序还在实验阶段,可读性不强。等整体优化好,会贴出来共享的 写得好! 加油,坛子里缺的就是这种理论帝。 aworker 发表于 2013-3-20 13:55 static/image/common/back.gif
写得好!
谢谢鼓励。有什么问题和心得希望一起分享!{:tongue:} xaper 发表于 2013-3-20 14:54 static/image/common/back.gif
加油,坛子里缺的就是这种理论帝。
我之前也是在潜水,看了很多大家写的东西。但感觉不是很全面。 本帖最后由 jade0606 于 2013-3-20 16:14 编辑
喔!整體上不錯,不過,對我而言,那是理論上正確,實際操作出大亂子的理論.
就我所知,歐拉角的姿態解算加上卡爾曼濾波會有協方差矩陣爆掉的問題.
不管是六軸(三軸陀螺儀+三軸加速度計)或九軸(三軸陀螺儀+三軸加速度計+三軸磁傳感器)或五軸(二軸陀螺儀+三軸加速度計),結果一樣...
我們曾試過壓制協方差矩陣,但是後來我們不敢使用那樣的算法...
不壓制的情形下,轉動角度大一點就爆了...很慘...{:lol:}
如果你試過後也相同,別氣餒,姿態解算還有很多方法,再研究研究吧.... brb2010 发表于 2013-3-20 13:30 static/image/common/back.gif
哦,我完全是按上面的想法做的,你参考就可以了。具体的程序还在实验阶段,可读性不强。等整体优化好,会 ...
thanks !i try it by myself! jade0606 发表于 2013-3-20 16:09 static/image/common/back.gif
喔!整體上不錯,不過,對我而言,那是理論上正確,實際操作出大亂子的理論.
就我所知,歐拉角的姿態解算加上卡爾 ...
你只得转动角大一点就爆掉是什么意思呢?协方差爆掉又是只什么呢?我觉得只有噪声而没有其他扰动的话,这个协方差是可以给个常数的。没记错的话他只是表征两个相邻噪声的相关程度的。 flyforyou85 发表于 2013-3-20 16:30 static/image/common/back.gif
thanks !i try it by myself!
best wish to you 你的低通滤波器是怎么设计的,能介绍下吗 xaper 发表于 2013-3-25 16:06 static/image/common/back.gif
你的低通滤波器是怎么设计的,能介绍下吗
我直接用的mpu6050里的低通滤波器。低通滤波实际就是一个惯性环节。在频域分析中,超过转折频率后幅值开始衰减,从而达到了将高频信号滤掉。物理上典型的是rc电路。不知道我这么说说清楚没有。 谢谢LZ,本人要做课程设计,谢谢提出宝贵意见
好像有的开源的UAV飞控用的就是这个模型,说实话这个模型真不咋样,不过优点也非常明显,计算量小 楼主,你的上位机调试软件用的是什么?wmc自带的的还是你自己写的?我也在学做四轴,刚刚接触。 asha 发表于 2013-3-27 14:53 static/image/common/back.gif
好像有的开源的UAV飞控用的就是这个模型,说实话这个模型真不咋样,不过优点也非常明显,计算量小 ...
看过些其他的,比较难,没有用。接下来可以看看。你有什么好的意见吗 wkong501 发表于 2013-3-27 15:53 static/image/common/back.gif
楼主,你的上位机调试软件用的是什么?wmc自带的的还是你自己写的?我也在学做四轴,刚刚接触。 ...
是lisn3188在“miniIMU AHRS 姿态板首试 动如脱兔 静若处子”这篇里提供的。一直在出新版本。现在这版已经很好用了。你可以去看 brb2010 发表于 2013-3-30 18:06 static/image/common/back.gif
看过些其他的,比较难,没有用。接下来可以看看。你有什么好的意见吗
可以用四元数为状态量,或者误差四元数的都可以。 支持 理论帝 {:lol:} asha 发表于 2013-4-1 08:43 static/image/common/back.gif
可以用四元数为状态量,或者误差四元数的都可以。
我看有的姿态解算速度可以达到很快的。主要的差别在哪里呢?是因为三角函数会占用时间,还是使用矩阵计算呢 brb2010 发表于 2013-4-1 12:11 static/image/common/back.gif
我看有的姿态解算速度可以达到很快的。主要的差别在哪里呢?是因为三角函数会占用时间,还是使用矩阵计算 ...
都挺费时间的,不过三角函数你可以用三阶近似,我试过,精度基本不会损失。矩阵运算那个是很费时间,不过还好 跟上,确实是好贴 先标记 以后慢慢研习 asha 发表于 2013-4-1 16:14 static/image/common/back.gif
都挺费时间的,不过三角函数你可以用三阶近似,我试过,精度基本不会损失。矩阵运算那个是很费时间,不过 ...
那我看有的可以做到很快有几百hz的,是哪里做了优化呢。还有电机的震动是怎么处理的呢? Name_006 发表于 2013-4-1 09:20 static/image/common/back.gif
支持 理论帝 ...
互相学习了 brb2010 发表于 2013-4-5 11:32 static/image/common/back.gif
那我看有的可以做到很快有几百hz的,是哪里做了优化呢。还有电机的震动是怎么处理的呢? ...
我用互补滤波,电机震动用滑动均值滤波处理,没2ms读一次数据做一次结算,解算频率是500Hz MARKMARKMARK zywei_09 发表于 2013-4-5 12:59 static/image/common/back.gif
我用互补滤波,电机震动用滑动均值滤波处理,没2ms读一次数据做一次结算,解算频率是500Hz ...
那你传感器采样周期是多长时间,我的采集一次加速度计和陀螺仪的就已经快 2ms了。按你的说法就没办法做什么均值滤波处理了。是差在哪里了呢 brb2010 发表于 2013-4-5 20:16 static/image/common/back.gif
那你传感器采样周期是多长时间,我的采集一次加速度计和陀螺仪的就已经快 2ms了。按你的说法就没办法做什 ...
每2ms做一次采样 zywei_09 发表于 2013-4-5 22:20 static/image/common/back.gif
每2ms做一次采样
你是如何保证在pwm(控制电调的信号周期2.5ms)的一个周期里做完所有这些运算的,用中断吗,还是延时?而且是输出4路的pwm以哪一路为准呢? brb2010 发表于 2013-4-7 09:50 static/image/common/back.gif
你是如何保证在pwm(控制电调的信号周期2.5ms)的一个周期里做完所有这些运算的,用中断吗,还是延时?而 ...
PWM用STM32的一个通用定时器的PWM输出模式生成,改这个定时器寄存器值即可,其他的由芯片自己完成,貌似和一个周期里做完没什么联系。 好贴子mark 顶一下。 对振动有作用不是卡尔曼的贡献,是低通滤波的贡献。 bucuo{:biggrin:} {:lol:}{:lol:}{:lol:} 以前上课学过,但是基本上都忘了,好好学习一下~ asha 发表于 2013-4-11 13:09 static/image/common/back.gif
对振动有作用不是卡尔曼的贡献,是低通滤波的贡献。
我不这样认为,你看下这两张仿真图就知道了。可以看出,振动作为一种噪声,卡尔曼也是可以有一定效果的。只是振动产生的噪声太大话,基本没有效果了。我这样觉得。
实际值取s(t)=sin(10*t);观测量取z(t)=sin(10*t)+v(t),v(t)为噪声;u(t)=10*cos(10)+w(t),w(t)为噪声。模型为帖子里的模型
这是仿真时加了噪声的观测值和控制量u,
这是没有加滤波的实际值和观测值
这是滤波后的和实际值。
这是观测量的噪声加大后的效果 zywei_09 发表于 2013-4-7 10:01 static/image/common/back.gif
PWM用STM32的一个通用定时器的PWM输出模式生成,改这个定时器寄存器值即可,其他的由芯片自己完成,貌似 ...
恩,电调是有响应时间的吧,就是是2.5ms,400hz的。那么单片机应该是每2.5ms改变一次占空比啊,所以说你的采样和运算都应该在此期间完成的。我这样理解的。你是怎么理解的呢? brb2010 发表于 2013-4-18 09:01 static/image/common/back.gif
我不这样认为,你看下这两张仿真图就知道了。可以看出,振动作为一种噪声,卡尔曼也是可以有一定效果的。 ...
有效果是因为你仿真的加速度传感器的振动还基本满足R阵,当振动太大,这时候观测量是完全错误的,这个时候Z-Z~就是个错误值,然而R矩阵在卡尔曼里面是不变的,这个时候一切的估计就错了。 只能是提前的低通滤波把错误的数据滤掉。 你的仿真有效是因为振动是你自己加上去的,R这个也是你根据这个得到的,没有效果才怪了,真实情况可不会这样,振动方差是不可获得了。 brb2010 发表于 2013-3-20 13:30 static/image/common/back.gif
哦,我完全是按上面的想法做的,你参考就可以了。具体的程序还在实验阶段,可读性不强。等整体优化好,会 ...
支持啊!多造福大众啊! asha 发表于 2013-4-20 09:14 static/image/common/back.gif
有效果是因为你仿真的加速度传感器的振动还基本满足R阵,当振动太大,这时候观测量是完全错误的,这个时 ...
最后那张图就是你说的情况 期待LZ解析代码示例 楼主,用这个算法是否可以计算出运行轨迹。用这个算法做个鼠标是否可行? brb2010 发表于 2013-4-20 09:54 static/image/common/back.gif
最后那张图就是你说的情况
这个代码不用自己敲PPZ上就有,这个模型我见过很多次不过我一点都不推荐,为何?因为本身观测量,也就是通过加速度传感器获得的角度有时候就是错误的,那么反馈就直接到了惯性系统中,如果振动一直有,那么你得到的结果会一直是错误的,到头来一看可能还不如不用滤波。就因为这样,因此我才说实际滤波效果都是低通贡献的。 我建立的模型是四元数为状态量,加速度传感器为状态量,中间估计外部加速度的方差, 恩,灰常的详细。工作中有用到这个东东,lz分析的灰常的好,先Mark一下 asha 发表于 2013-4-20 15:23 static/image/common/back.gif
这个代码不用自己敲PPZ上就有,这个模型我见过很多次不过我一点都不推荐,为何?因为本身观测量,也 ...
对啊 这个加速度方差估计有没有什么好方法,这个R我是自己分段判断的,震动过大,直接把R设得非常大(比如100),震动小的话,就是简单的对静止的R翻个倍。
效果感觉还可以,但是还是有欠缺的,R的估计可以提供一个思路么(但我感觉这玩意就是不可预估的啊)?谢谢啦 brb2010 发表于 2013-4-18 09:04 static/image/common/back.gif
恩,电调是有响应时间的吧,就是是2.5ms,400hz的。那么单片机应该是每2.5ms改变一次占空比啊,所以说你 ...
额。。。好吧,我不知道电调有响应时间。我是用定时器产生任务的时钟信号,没2ms做一次姿态解算任务,和一次控制电机任务。感觉裸机跑确实有不便,准备加操作系统了。 MARKMARKMARK 没有看懂。。。数学没学好。。{:sad:} 谢谢分享 {:smile:}{:smile:}{:smile:}{:smile:}{:smile:}{:smile:}{:smile:}{:smile:} 楼主大哥,Heading = arctan(Yh / Xh ),Yh= X sinRollsinPitch +Y cosRoll+ Z sinRollcosPitch,Xh =X cosPitch+ Z sinPitch
这个公式好像只在静态的时候有效吧,动态的时候磁力计的数据和倾角的变化不同步,用这个公式不能得出好的航向角,
我用陀螺仪解算航向角和它做互补滤波也不行。。。
请问你有好的求教航向角的方法吗?
asha 发表于 2013-4-1 08:43
可以用四元数为状态量,或者误差四元数的都可以。
你好,我学习飞控不久 ,请问你是说用四元数参与卡尔曼滤波的计算吗?四元数法不是可以直接进行姿态解算吗?我最近在看姿态解算,感觉概念太多 ,有点糊涂了 LYY 发表于 2014-4-22 14:30
你好,我学习飞控不久 ,请问你是说用四元数参与卡尔曼滤波的计算吗?四元数法不是可以直接进行姿态解算 ...
现在讨论的是数据融合,是四元数计算后面的校正部分 我看看,学习学习。 非常好!!学习了 http://cache.amobbs.com/new2012/forum/201303/19/195847xbjjdp3pvjvoj3zz.png.thumb.jpg
第一条公式的意思是,加速度计的姿态角估计值等于角速度加噪声?0*X是什么意思? brb2010 发表于 2013-3-20 14:57
我之前也是在潜水,看了很多大家写的东西。但感觉不是很全面。
我怎么感觉 一楼是 高级黑 呢 加计每次读一个数据丢掉最早的一个数据再取平均值,实时性好些 还有这个系统不是线性的,用经典卡尔曼会不好。 之前看到的都是大概,今天终于看到了一个讲解详细的了,谢谢! 谢谢分享! 希望楼主出一个姿态解算的教程 普度纵生…… 写得不错,赞一个 asha果然是大神。进行卡尔曼滤波的时候要确保你的观测值是正确的,要明确加速度计在什么时候是准确的,如果用一个错误观测值去校正陀螺仪输出,这个结果可想而知吧。 希望楼主能将姿态解算部分代码上传下! asha 发表于 2013-4-1 16:14
都挺费时间的,不过三角函数你可以用三阶近似,我试过,精度基本不会损失。矩阵运算那个是很费时间,不过 ...
请问三阶是指泰勒展开式的三阶吗 请问, 姿态算法实际应用的时候, 还需要注意哪些方面? mark 学习一下,搞到现在感觉自己的数学功底不够了
页:
[1]