姿态解算（一般情况下的卡尔曼）原创

brb2010 · 发表于 2013-3-19 20:12:27

在论坛里潜水很久了，上周我的姿态解算经历了断断续续一个学期后终于有所成果了。贴出来和大家分享下。第一次发帖子估计不会很漂亮，见谅。不废话了进入正题：

首先从卡尔曼开始讲起，网上很多都是用的基本卡尔曼公式，但我觉得用一般情况下的卡尔曼，更容易建立状态方程。
一般离散线性系统的状态方程如下:

其对应的卡尔曼滤波迭代公式如下：
先给出初值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

brb2010 · 发表于 2013-3-19 20:18:44

传感器的初始化校准参考了这里的例子http://mbed.org/users/aberk/code ... 440a03255a7/IMU.cpp

flyforyou85 · 发表于 2013-3-19 20:36:06

不错，我的四轴硬件已经搭好了，就差软件了，希望楼主能将姿态解算部分代码上传下！谢谢
！

brb2010 · 发表于 2013-3-20 13:30:32

flyforyou85 发表于 2013-3-19 20:36
不错，我的四轴硬件已经搭好了，就差软件了，希望楼主能将姿态解算部分代码上传下！谢谢
！ ...

哦，我完全是按上面的想法做的，你参考就可以了。具体的程序还在实验阶段，可读性不强。等整体优化好，会贴出来共享的

aworker · 发表于 2013-3-20 13:55:29

写得好！

xaper · 发表于 2013-3-20 14:54:11

加油，坛子里缺的就是这种理论帝。

brb2010 · 发表于 2013-3-20 14:56:17

aworker 发表于 2013-3-20 13:55
写得好！

谢谢鼓励。有什么问题和心得希望一起分享！

brb2010 · 发表于 2013-3-20 14:57:53

xaper 发表于 2013-3-20 14:54
加油，坛子里缺的就是这种理论帝。

我之前也是在潜水，看了很多大家写的东西。但感觉不是很全面。

jade0606 · 发表于 2013-3-20 16:09:28

本帖最后由 jade0606 于 2013-3-20 16:14 编辑

喔!整體上不錯,不過,對我而言,那是理論上正確,實際操作出大亂子的理論.
就我所知,歐拉角的姿態解算加上卡爾曼濾波會有協方差矩陣爆掉的問題.
不管是六軸(三軸陀螺儀+三軸加速度計)或九軸(三軸陀螺儀+三軸加速度計+三軸磁傳感器)或五軸(二軸陀螺儀+三軸加速度計),結果一樣...
我們曾試過壓制協方差矩陣,但是後來我們不敢使用那樣的算法...
不壓制的情形下,轉動角度大一點就爆了...很慘...

如果你試過後也相同,別氣餒,姿態解算還有很多方法,再研究研究吧....

flyforyou85 · 发表于 2013-3-20 16:30:54

brb2010 发表于 2013-3-20 13:30
哦，我完全是按上面的想法做的，你参考就可以了。具体的程序还在实验阶段，可读性不强。等整体优化好，会 ...

thanks ! i try it by myself!

brb2010 · 发表于 2013-3-20 17:57:55

jade0606 发表于 2013-3-20 16:09
喔!整體上不錯,不過,對我而言,那是理論上正確,實際操作出大亂子的理論.
就我所知,歐拉角的姿態解算加上卡爾 ...

你只得转动角大一点就爆掉是什么意思呢？协方差爆掉又是只什么呢？我觉得只有噪声而没有其他扰动的话，这个协方差是可以给个常数的。没记错的话他只是表征两个相邻噪声的相关程度的。

brb2010 · 发表于 2013-3-20 17:58:35

flyforyou85 发表于 2013-3-20 16:30
thanks ! i try it by myself!

best wish to you

xaper · 发表于 2013-3-25 16:06:29

你的低通滤波器是怎么设计的，能介绍下吗

brb2010 · 发表于 2013-3-25 17:21:29

xaper 发表于 2013-3-25 16:06
你的低通滤波器是怎么设计的，能介绍下吗

我直接用的mpu6050里的低通滤波器。低通滤波实际就是一个惯性环节。在频域分析中，超过转折频率后幅值开始衰减，从而达到了将高频信号滤掉。物理上典型的是rc电路。不知道我这么说说清楚没有。

一指流沙 · 发表于 2013-3-25 17:43:27

谢谢LZ，本人要做课程设计，谢谢提出宝贵意见

asha · 发表于 2013-3-27 14:53:11

好像有的开源的UAV飞控用的就是这个模型，说实话这个模型真不咋样，不过优点也非常明显，计算量小

wkong501 · 发表于 2013-3-27 15:53:09

楼主，你的上位机调试软件用的是什么？wmc自带的的还是你自己写的？我也在学做四轴，刚刚接触。

brb2010 · 发表于 2013-3-30 18:06:28

asha 发表于 2013-3-27 14:53
好像有的开源的UAV飞控用的就是这个模型，说实话这个模型真不咋样，不过优点也非常明显，计算量小 ...

看过些其他的，比较难，没有用。接下来可以看看。你有什么好的意见吗

brb2010 · 发表于 2013-3-30 18:10:31

wkong501 发表于 2013-3-27 15:53
楼主，你的上位机调试软件用的是什么？wmc自带的的还是你自己写的？我也在学做四轴，刚刚接触。 ...

是lisn3188在“miniIMU AHRS 姿态板首试动如脱兔静若处子” 这篇里提供的。一直在出新版本。现在这版已经很好用了。你可以去看

asha · 发表于 2013-4-1 08:43:43

brb2010 发表于 2013-3-30 18:06
看过些其他的，比较难，没有用。接下来可以看看。你有什么好的意见吗

可以用四元数为状态量，或者误差四元数的都可以。

Name_006 · 发表于 2013-4-1 09:20:01

支持理论帝

brb2010 · 发表于 2013-4-1 12:11:36

asha 发表于 2013-4-1 08:43
可以用四元数为状态量，或者误差四元数的都可以。

我看有的姿态解算速度可以达到很快的。主要的差别在哪里呢？是因为三角函数会占用时间，还是使用矩阵计算呢

asha · 发表于 2013-4-1 16:14:55

brb2010 发表于 2013-4-1 12:11
我看有的姿态解算速度可以达到很快的。主要的差别在哪里呢？是因为三角函数会占用时间，还是使用矩阵计算 ...

都挺费时间的，不过三角函数你可以用三阶近似，我试过，精度基本不会损失。矩阵运算那个是很费时间，不过还好

talangxue · 发表于 2013-4-2 09:39:17

跟上，确实是好贴

tonamatata · 发表于 2013-4-2 10:26:41

先标记以后慢慢研习

brb2010 · 发表于 2013-4-5 11:32:23

asha 发表于 2013-4-1 16:14
都挺费时间的，不过三角函数你可以用三阶近似，我试过，精度基本不会损失。矩阵运算那个是很费时间，不过 ...

那我看有的可以做到很快有几百hz的，是哪里做了优化呢。还有电机的震动是怎么处理的呢？

brb2010 · 发表于 2013-4-5 11:33:00

Name_006 发表于 2013-4-1 09:20
支持理论帝 ...

互相学习了

zywei_09 · 发表于 2013-4-5 12:59:28

brb2010 发表于 2013-4-5 11:32
那我看有的可以做到很快有几百hz的，是哪里做了优化呢。还有电机的震动是怎么处理的呢？ ...

我用互补滤波，电机震动用滑动均值滤波处理，没2ms读一次数据做一次结算，解算频率是500Hz

flotox · 发表于 2013-4-5 13:05:19

MARKMARKMARK

brb2010 · 发表于 2013-4-5 20:16:50

zywei_09 发表于 2013-4-5 12:59
我用互补滤波，电机震动用滑动均值滤波处理，没2ms读一次数据做一次结算，解算频率是500Hz ...

那你传感器采样周期是多长时间，我的采集一次加速度计和陀螺仪的就已经快 2ms了。按你的说法就没办法做什么均值滤波处理了。是差在哪里了呢

zywei_09 · 发表于 2013-4-5 22:20:02

brb2010 发表于 2013-4-5 20:16
那你传感器采样周期是多长时间，我的采集一次加速度计和陀螺仪的就已经快 2ms了。按你的说法就没办法做什 ...

每2ms做一次采样

brb2010 · 发表于 2013-4-7 09:50:04

zywei_09 发表于 2013-4-5 22:20
每2ms做一次采样

你是如何保证在pwm（控制电调的信号周期2.5ms）的一个周期里做完所有这些运算的，用中断吗，还是延时?而且是输出4路的pwm以哪一路为准呢？

zywei_09 · 发表于 2013-4-7 10:01:28

brb2010 发表于 2013-4-7 09:50
你是如何保证在pwm（控制电调的信号周期2.5ms）的一个周期里做完所有这些运算的，用中断吗，还是延时?而 ...

PWM用STM32的一个通用定时器的PWM输出模式生成，改这个定时器寄存器值即可，其他的由芯片自己完成，貌似和一个周期里做完没什么联系。

末语 · 发表于 2013-4-10 22:27:48

好贴子 mark

xizi · 发表于 2013-4-10 23:26:22

顶一下。

asha · 发表于 2013-4-11 13:09:58

对振动有作用不是卡尔曼的贡献，是低通滤波的贡献。

intelcorei6 · 发表于 2013-4-11 15:39:25

bucuo

Privia · 发表于 2013-4-11 18:19:52

lianyisixue · 发表于 2013-4-17 16:52:12

以前上课学过，但是基本上都忘了，好好学习一下~

brb2010 · 发表于 2013-4-18 09:01:01

asha 发表于 2013-4-11 13:09
对振动有作用不是卡尔曼的贡献，是低通滤波的贡献。

我不这样认为，你看下这两张仿真图就知道了。可以看出，振动作为一种噪声，卡尔曼也是可以有一定效果的。只是振动产生的噪声太大话，基本没有效果了。我这样觉得。
实际值取s(t)=sin(10*t);观测量取z(t)=sin(10*t)+v(t)，v(t)为噪声；u(t)=10*cos(10)+w(t)，w(t)为噪声。模型为帖子里的模型

这是仿真时加了噪声的观测值和控制量u，

这是没有加滤波的实际值和观测值

这是滤波后的和实际值。

这是观测量的噪声加大后的效果

brb2010 · 发表于 2013-4-18 09:04:02

zywei_09 发表于 2013-4-7 10:01
PWM用STM32的一个通用定时器的PWM输出模式生成，改这个定时器寄存器值即可，其他的由芯片自己完成，貌似 ...

恩，电调是有响应时间的吧，就是是2.5ms，400hz的。那么单片机应该是每2.5ms改变一次占空比啊，所以说你的采样和运算都应该在此期间完成的。我这样理解的。你是怎么理解的呢？

asha · 发表于 2013-4-20 09:14:41

brb2010 发表于 2013-4-18 09:01
我不这样认为，你看下这两张仿真图就知道了。可以看出，振动作为一种噪声，卡尔曼也是可以有一定效果的。 ...

有效果是因为你仿真的加速度传感器的振动还基本满足R阵，当振动太大，这时候观测量是完全错误的，这个时候Z-Z~就是个错误值，然而R矩阵在卡尔曼里面是不变的，这个时候一切的估计就错了。只能是提前的低通滤波把错误的数据滤掉。你的仿真有效是因为振动是你自己加上去的，R这个也是你根据这个得到的，没有效果才怪了，真实情况可不会这样，振动方差是不可获得了。

svon · 发表于 2013-4-20 09:53:18

brb2010 发表于 2013-3-20 13:30
哦，我完全是按上面的想法做的，你参考就可以了。具体的程序还在实验阶段，可读性不强。等整体优化好，会 ...

支持啊！多造福大众啊！

brb2010 · 发表于 2013-4-20 09:54:29

asha 发表于 2013-4-20 09:14
有效果是因为你仿真的加速度传感器的振动还基本满足R阵，当振动太大，这时候观测量是完全错误的，这个时 ...

最后那张图就是你说的情况

zhaoshimax · 发表于 2013-4-20 14:46:57

期待LZ解析代码示例

twuking · 发表于 2013-4-20 15:06:12

楼主，用这个算法是否可以计算出运行轨迹。用这个算法做个鼠标是否可行？

asha · 发表于 2013-4-20 15:23:22

brb2010 发表于 2013-4-20 09:54
最后那张图就是你说的情况

这个代码不用自己敲 PPZ上就有，这个模型我见过很多次不过我一点都不推荐，为何？因为本身观测量，也就是通过加速度传感器获得的角度有时候就是错误的，那么反馈就直接到了惯性系统中，如果振动一直有，那么你得到的结果会一直是错误的，到头来一看可能还不如不用滤波。就因为这样，因此我才说实际滤波效果都是低通贡献的。我建立的模型是四元数为状态量，加速度传感器为状态量，中间估计外部加速度的方差，

windancerhxw · 发表于 2013-4-22 21:55:58

恩，灰常的详细。工作中有用到这个东东，lz分析的灰常的好，先Mark一下

s19910223 · 发表于 2013-4-22 23:22:51

asha 发表于 2013-4-20 15:23
这个代码不用自己敲 PPZ上就有，这个模型我见过很多次不过我一点都不推荐，为何？因为本身观测量，也 ...

对啊这个加速度方差估计有没有什么好方法，这个R我是自己分段判断的，震动过大，直接把R设得非常大（比如100），震动小的话，就是简单的对静止的R翻个倍。
效果感觉还可以，但是还是有欠缺的，R的估计可以提供一个思路么（但我感觉这玩意就是不可预估的啊）？谢谢啦

zywei_09 · 发表于 2013-4-26 18:57:05

brb2010 发表于 2013-4-18 09:04
恩，电调是有响应时间的吧，就是是2.5ms，400hz的。那么单片机应该是每2.5ms改变一次占空比啊，所以说你 ...

额。。。好吧，我不知道电调有响应时间。我是用定时器产生任务的时钟信号，没2ms做一次姿态解算任务，和一次控制电机任务。感觉裸机跑确实有不便，准备加操作系统了。

flotox · 发表于 2013-4-26 19:06:48

MARKMARKMARK

-阿发- · 发表于 2013-10-19 14:52:36

没有看懂。。。数学没学好。。

梁国俭 · 发表于 2013-10-20 18:24:04

谢谢分享

电气工程化动自 · 发表于 2014-3-20 18:40:51

silence2455 · 发表于 2014-3-31 11:09:14

楼主大哥，Heading = arctan(Yh / Xh )，Yh= X sinRollsinPitch +Y cosRoll+ Z sinRollcosPitch，Xh =X cosPitch+ Z sinPitch

这个公式好像只在静态的时候有效吧，动态的时候磁力计的数据和倾角的变化不同步，用这个公式不能得出好的航向角，

我用陀螺仪解算航向角和它做互补滤波也不行。。。

请问你有好的求教航向角的方法吗？

LYY · 发表于 2014-4-22 14:30:58

asha 发表于 2013-4-1 08:43
可以用四元数为状态量，或者误差四元数的都可以。

你好，我学习飞控不久，请问你是说用四元数参与卡尔曼滤波的计算吗？四元数法不是可以直接进行姿态解算吗？我最近在看姿态解算，感觉概念太多，有点糊涂了

asha · 发表于 2014-4-26 08:25:27

LYY 发表于 2014-4-22 14:30
你好，我学习飞控不久，请问你是说用四元数参与卡尔曼滤波的计算吗？四元数法不是可以直接进行姿态解算 ...

现在讨论的是数据融合，是四元数计算后面的校正部分

点雨落山岚 · 发表于 2014-4-26 10:33:24

我看看，学习学习。

RedFang · 发表于 2014-11-17 21:33:58

非常好！！学习了

zsjinwei · 发表于 2014-11-18 21:51:06

第一条公式的意思是，加速度计的姿态角估计值等于角速度加噪声？0*X是什么意思？

shengzetam · 发表于 2014-12-19 22:35:21

brb2010 发表于 2013-3-20 14:57
我之前也是在潜水，看了很多大家写的东西。但感觉不是很全面。

我怎么感觉一楼是高级黑呢

2013的弹子球 · 发表于 2014-12-20 18:45:57

加计每次读一个数据丢掉最早的一个数据再取平均值，实时性好些

2013的弹子球 · 发表于 2014-12-20 18:47:14

还有这个系统不是线性的，用经典卡尔曼会不好。

hehao9051 · 发表于 2015-1-1 00:16:35

之前看到的都是大概，今天终于看到了一个讲解详细的了，谢谢！

yzh · 发表于 2015-1-1 21:04:14

谢谢分享！

乘风归去 · 发表于 2015-1-27 21:48:09

希望楼主出一个姿态解算的教程普度纵生……

hnzbding · 发表于 2015-1-27 23:02:53

写得不错，赞一个

luoyu510183 · 发表于 2015-1-29 10:16:48

asha果然是大神。进行卡尔曼滤波的时候要确保你的观测值是正确的，要明确加速度计在什么时候是准确的，如果用一个错误观测值去校正陀螺仪输出，这个结果可想而知吧。

brelo · 发表于 2015-3-27 19:46:45

希望楼主能将姿态解算部分代码上传下！

会会 · 发表于 2017-1-4 16:28:15

asha 发表于 2013-4-1 16:14
都挺费时间的，不过三角函数你可以用三阶近似，我试过，精度基本不会损失。矩阵运算那个是很费时间，不过 ...

请问三阶是指泰勒展开式的三阶吗

shihezichen · 发表于 2017-1-17 23:28:16

请问, 姿态算法实际应用的时候, 还需要注意哪些方面?

Xiyuhexuan · 发表于 2017-1-18 07:56:04

mark

GLY666 · 发表于 2019-11-7 14:26:23

学习一下，搞到现在感觉自己的数学功底不够了

姿态解算（一般情况下的卡尔曼）原创

本帖子中包含更多资源

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

本帖子中包含更多资源

浏览过的版块