请教三维电子罗盘问题

wall.e · 发表于 2009-8-26 10:58:54

我用磁阻传感器HMC5843 与重力加速度MMA7455 做数字罗盘，

资料里写着
公式2，θ 是横滚角，ϕ 是俯仰角，再加上X，Y，Z 三轴磁分量，可得到（Xh，Yh）。若没有用此公式，会有大的误差输出。
Xh=X * cos(ϕ)+Y * sin(θ) * sin(ϕ)-z * cos(θ) * sin(ϕ)
Yh=Y * cos(θ)+z * sin(θ)

请问在公式里的cos　与　sin 要怎么用，我现在是这样用的，我这样用有没有错误呢
现在是当指南针度数为0时,改变θ ϕ 是俯仰角　-９０~９０度数时，指南针的会有180度的误差
改变横滚角 -９０~９０度时，指南针会有+-　４５度的误差，
是我sin/cos　函数　用的有问题还是怎样呢？　　请求帮助谢谢了　

程序是用GCC写的　
// moutx 俯仰角
// mouty 横滚角
//xa x轴
//ya x轴
//za x轴
//outx_s转换后的　Ｘ轴
//outy_s转换后的　y轴
//outz_s转换后的　z轴

outx_s=moxin(1000,-1000,xk_h,xk_l,xa);//把采集来的数转换成-1000~1000的数
outy_s=moxin(1000,-1000,yk_h,yk_l,ya);
outz_s=moxin(1000,-1000,zk_h,zk_l,za);
//横滚角与俯仰角转成-90~90度
moutx=moxin(90,-90,mx_h,mx_l,mx);//把
mouty=moxin(90,-90,my_h,my_l,my);

//先计算好COS 与ＳＩＮ
ctpx=cos((float)moutx*pi/180);
stpx=sin((float)moutx*pi/180);
ctpy=cos((float)mouty*pi/180);
stpy=sin((float)mouty*pi/180);

计算xh,yh
outx=(int)((float)outx_s*ctpx+(float)outy_s*stpy*stpx-(float)outz_s*ctpy*stpx);
outy=(int)((float)outy_s*ctpy+(float)outz_s*stpy);

// ls2 为输出０~３５９的度数
ac=(float)outx/(float)outy;
if(outy<0)ls2=180-atan(ac)*90/1.570796326794897;
if((outy>0)&&(outx<0))ls2=-atan(ac)*90/1.570796326794897;
if((outy>0)&&(outx>0))ls2=360-atan(ac)*90/1.570796326794897;
if((outy==0)&&(outx<0))ls2=90;
if((outy==0)&&(outx>0))ls2=270;

feng_matrix · 发表于 2009-8-26 11:33:42

HoneyWell的公式没问题的，楼主全部使用弧度浮点运算就对了

feng_matrix · 发表于 2009-8-26 11:38:53

还有三轴地磁解耦如果只用三轴加速度计，受外力加速度影响很大，在运动/振动等环境中，输出方向角误差较大
完整解决方案是三轴陀螺仪＋三轴加速度＋三轴磁场计，当然再辅助GPS方位信息就更完美了

晒一个我以前贴在EDN上的三轴地磁罗盘的算法流程图：

(原文件名:a10.jpg)

(原文件名:a12.jpg)

346675655 · 发表于 2009-8-26 12:53:56

标记一下

lwy86 · 发表于 2009-8-26 13:05:49

mark

wall.e · 发表于 2009-8-26 15:47:28

feng_matrix 悟兄，敢问你说这个完整解决方案是三轴陀螺仪＋三轴加速度＋三轴磁场计　的试算公式给共享下吗，
我的问题解决了，
//先计算好COS 与ＳＩＮ
ctpx=cos((float)(180-moutx)*pi/180);
stpx=sin((float)(180-moutx)*pi/180);
ctpy=cos((float)(180-mouty)*pi/180);
stpy=sin((float)(180-mouty)*pi/180);
这样计算就好了，哎，我对的数学不好，整了我两天了　

feng_matrix · 发表于 2009-8-26 16:53:52

【5楼】 wall.e

算法中除卡曼滤波外就是你1楼贴的公式啊，所以我说那个是对的呀
你没发现图中虚线框中的输入输出与你的公式是一样的吗？它是HoneyWell官方文档里的，不会有错
KALMAN滤波的原理与算法网上也很多，你到GOOGLE里搜下一大堆

aaa1982 · 发表于 2009-8-26 17:09:30

feng_matrix 悟先生能不能提供一些相关知识的文档，帮忙指明一下应该看什么，呵呵

最近一直在做相关东西，有几个问题希望请教一下：

1、为什么需要三轴磁强计？
我个人一直觉得三轴加速度计在静态情况下可以准确算出pitch和roll，就可以把两轴磁传感器测量的值补偿到水平方向，从而可以算出航向角度。为什么需要三轴磁强计？是不是只在补偿运动加速度带来的影响的时候才用得到第三轴（Z轴）。

2、在有加速度的情况下，怎么利用陀螺仪结合加速度计准确算出PITCH和ROLL？
我看过lz看的那篇应用笔记，运动中产生的加速度对于依靠重力加速度测量pitch和roll的加速度计必然会产生很大的影响。但是honeywell没有再进一步说加入陀螺仪以后应该怎么做。看到您2楼的帖子，感到描述的很清楚，能进一步解释一下或者提供一些资料么，很感谢。

看过一些惯导的书，感觉都没有切合我的实际问题。能推荐一些比较经典文档或者书籍么？（关于惯性导航的，上学的时候学的不是这个，呵呵）

还有您edn上面帖子的网址能提供一下么，十分感谢

spely · 发表于 2009-8-26 17:27:55

mark下

feng_matrix · 发表于 2009-8-26 17:58:01

【7楼】 aaa1982

1.一楼的公式里不是用到XYZ三轴磁场强吗？怎么说只要两轴呢？

任何测量都是有参照物的，我在二楼的图里画了两个参照物：重力轴与地磁轴
三维空间里要精确测量被测物体与参照物的相对关系，一定会有3变量的，这点无论是三轴加速度还是三轴地磁都一样

拿图中较简单的Yh分量的计算为例：Yh= ym * cos(ROLL)+ zm * sin(ROLL)
那么保持ROLL角度不变，系统以蓝色ym轴旋转，实际Yh解耦分量是改变的
而你只测ym,xm两轴，Yh解耦分量就不会改变，这与实际是不符的

所以两轴平面罗盘只有在系统保持水平时才能准确，并且是越靠近赤道越准确，因为磁力线平行于地面
而极地点的磁力线几乎垂直地面，所以一般的平面指南针在南北极无法使用，也是这个道理

2.陀螺仪与加速度的数据融合是惯性导航方面的问题，这个有很多模型算法的
我也是业余玩玩的，没看过啥惯导书，所有信息都是GOOGLE里搜来的，选择KALMAN滤波是因为它似乎最经典

惯性导航的源代码可以看看国外开源自驾仪之类项目，四轴分坛里有德国MK四桨飞行器的源代码
你如看懂它，其实核心只有八个字：积分反演＋数据融合，说白了就是用加速度纠正陀螺仪积分偏差
只不过他是个简化的算法，只用PI做负反馈平衡控制，并未显式的算出PITCH/ROLL角度罢了

luzhengmao · 发表于 2009-8-26 20:47:02

感觉其实2轴的电子罗盘在4轴飞行器上应该没啥问题
4轴基本还是平的。

aaa1982 · 发表于 2009-8-26 22:45:17

太感谢feng_matrix 悟：

突然发现一个错误的想法已经在我潜意识里面存在一年多了，太可怕了，呵呵。

因为磁罗盘的基础是假定地磁场在一个固定的区域H分量也就是水平分量是不变的，所以才可以在水平的时候用XY两轴测量航向角。但我居

然认为如果发生了翻滚和俯仰，依然可以只通过XY轴结合翻转俯仰角度反算出水平X和水平Y。因为地磁场的垂直分量对于罗盘没有什么贡

献，所以我潜意识里一直认为Z轴磁强计在磁罗盘里面没有直接的作用，充其量是在俯仰或翻滚角度太大的时候替代一下X轴或者Y轴发挥一

下作用（近一年来我都是这么认识的）。全然没有发现俯仰翻滚过后Z轴传感器的测量值是有水平分量的成分的。

楼主的应用笔记我一年前就看过了，但是觉得明白大概意思了就没细看公式，回来又看了于一下，才发现一个错误的认识存在了一年的时

间。幸亏发现了这个帖子，要不然不知道到什么时候才能认识到真相，呵呵

aaa1982 · 发表于 2009-8-26 23:00:47

vfeng_matrix 悟

您在EDN上面的关于这个帖子的连接能给一个么。

向您请教关于数据融合的问题，能从直观上帮着解释一下么？

1、为什么X和Z轴的加速度结合Y轴的陀螺仪算得的Pitch 就可以尽量避免运动加速度的影响，换句话说，如果没有运动加速度，可能就不需要有Y轴的陀螺仪，加入了Y轴的陀螺仪怎么就能补偿运动加速度的影响了呢。YZ两轴的加速度结合X轴陀螺仪算ROLL的原理应该和上面问的一样吧。

2、解算出来的水平Y轴和X轴磁场强度为什么还要通过Z轴陀螺仪的校正才能输出航向角呢？

实在感谢

另外看懂卡尔曼滤波需要什么额外的数学知识么？总觉得有点畏惧，呵呵

感谢

feng_matrix · 发表于 2009-8-28 10:48:21

【12楼】 aaa1982

1.陀螺仪输出角速度，是瞬时量，一般角速度姿态平衡上是不能直接使用，多数惯导系统控制需要角度信号
所以需要角速度与时间积分计算角度，得到的角度变化量与初始角度相加，就得到目标角度，其中积分时间Dt越小，输出角度越准
但陀螺仪的原理决定了它的测量基准是自身，并没有系统外的绝对参照物，加上Dt是不可能无限小
所以积分的累积误差会随着时间流逝迅速增加，最终导致输出角度与实际不符，所以陀螺仪只能工作在相对较短的时间尺度内。

加速度测量的是重力方向，有系统外绝对参照物“重力轴”，在无外力加速度的情况下，能准确输出ROLL/PITCH两轴姿态角度
并且此角度不会有累积误差，在更长的时间尺度内都是准确的。但是加速度传感器测角度也有缺点
加速度传感器实际上是用MEMS技术检测惯性力造成的微小形变，而惯性力与重力本质就是一个东东
所以它就不会区分重力加速度与外力加速度，当系统在三维空间做变速运动时，它的输出就不正确了。
很可惜，多数需要惯导姿态角的系统，都不是工作在静止状态下。

所以在没有其它参照物的基础上，要得到较为真实的姿态角，就要扬长避短，结合两者的优点，摈弃其各自缺点
最简单的办法就是加权，设计算法在短时间尺度内增加陀螺仪的权值，在更长时间尺度内增加加速度权值，这样系统输出角度就更真实了

其实MK四轴的平衡算法也是这样，首先对陀螺仪做PI运算，其中I的真正含义就是积分反演角度
有了陀螺仪PI算法，四轴就有了瞬时增稳，就可以遥控飞了，但是它不会永远水平
由于累积误差的作用，很快中立点就不是水平位置了，这时候就需要用加速度不断的纠正陀螺仪积分误差。

你可以看到MK算法中有根据加速度方向不断把积分量I递减清零的代码，就是这个融合算法的核心了
MK立足于一个高级航模玩具，为了在低成本8位单片机上运行，不去显式的计算姿态角，只把校正后的PI值输出负反馈控制电机了
这样的好处是基本上只用整型算法就能完成运算，而要显式的计算姿态角，更专业的做法就是KALMAN滤波显式求解姿态
卡曼滤波也是在对历史数据积分，并且可以同步融合陀螺仪与加速度数据，陀螺仪与加速度贡献权值还可以通过滤波参数调整
所以它就成了惯性数据处理的经典算法，他的缺点是浮点运算量较大，对系统资源要求较高
很少有人在资源较少的8位单片机上玩卡曼滤波的。

总结一下就是：
PITCH/ROLL角速度积分－>PITCH/ROLL姿态角，再结合加速度纠正累积误差
YAW角速度积分－>方向角，再结合地磁传感器、GPS纠正累积误差

当然导航并非只能用加速度传感器，其它有绝对参照物的传感器都可以
比如人除了耳蜗外，还有视觉，根据身边的景物人也不会有累积误差导致摔跤的问题
但一般人闭上眼睛，即便有耳蜗能保持平衡，摔跤可能性也会增加。
还有其它传感器，比如红外，航模上有种德国红外平衡仪，就是检测地平线上下，红外背景强度不同来保证绝对姿态的。
卫星上以前还听说过星光导航的，应该也是用它来作为一个相对静止的参照物。

2.地磁传感器解耦后就可以直接输出方位角，但地磁传感器一样有缺点，它的绝对参照物是地磁场的磁力线。
地磁的特点使范围大，但强度较低，约零点几高斯，非常容易受到其它磁体的干扰，比如电机定子（数千甚至上万高斯）。
如果融合了Z轴陀螺仪的瞬时角度，就可以使系统数据更加稳定。
这点我实际对比过，用ADIS16405的地磁解耦后直接算角度，系统静止时随机跳动在1度上下
而融合陀螺仪数据后，随机跳动就在零点几度，精度提高不止一倍，如果在有随机性磁场干扰的情况下，作用更好。

3.卡曼滤波的数学原理我没学过不懂，你实在有兴趣还是找个数学老师问吧。

EDN上搜feng_matrix就能找到的，不过原来那个应用ADIS16405实物照片帖子，被EDN版主当做广告误删除了，现在搜到的帖子就是2楼的那两幅示意图

aaa1982 · 发表于 2009-8-28 11:19:21

十分十分感谢feng_matrix 悟的指点，我先看一下，有问题再向您请教。

另外：搜到您的一篇帖子，好像昨天使您的生日，祝您生日快乐。

aaa1982

又仔细看了一下，感觉把自己的认识提高了一个档次，太感谢了。

feng_matrix · 发表于 2009-8-28 12:19:56

感谢 aaa1982 的祝福！

每年生日都会感慨，搞技术工作真没出息，一转眼孩子都会走路了
可惜心理年龄还停留在刚出道时的水平，想想惭愧啊

samguoguo · 发表于 2009-8-28 14:57:23

HMC5843磁阻传感器＋加速度传感器的电子罗盘算法我们在项目中发现有以下几个问题设计时工程师常会不注意，从而导致调试出现问题：
1，PCB设计时没有注意HMC5843的摆放方向，正确的是X轴指向北极。
2，HMC5843的正面和方面摆放，其算法是有很大不一样的，比如贴在手机的屏的背面，那么Z轴就是－Z了。
3，HMC5843周边铁磁物质的影响，特别是喇叭一类的强磁性物质，以及需要避免HMC5843底部出现大电流走线。
有需要HMC5843方面的详细资料可以发邮件给我，guoxiusheng@dsdcn.com

meizhi · 发表于 2009-9-13 10:06:09

mark

zhp3245 · 发表于 2009-9-25 11:46:19

mark

tianst · 发表于 2009-12-25 09:37:19

mark

ksh84222 · 发表于 2009-12-26 09:47:19

好东西!

aliveghost · 发表于 2010-1-11 14:53:02

mark，好东西，正在学习使用ADIS16350中...

guo69 · 发表于 2010-1-28 20:43:49

好东西，先mark一下

simondz · 发表于 2010-2-1 22:23:34

长了知识，标记一下，谢谢！

whh217 · 发表于 2010-2-1 22:26:45

标记

JX0316 · 发表于 2010-3-29 20:48:36

有谁做过电子指南针的，帮帮忙传点资料，网上资料少得可怜，最好是能输出三轴的．　　基于ＰＮＩ１１０９６或ＫＭＺ５２的电子指南针最好了．急用，谢谢各位了！

lxqqqqq · 发表于 2010-4-9 02:06:50

学习了

qzzz · 发表于 2010-4-9 02:32:37

学习了谢谢

sunzhaod · 发表于 2010-4-20 08:11:27

mark

shmily · 发表于 2010-4-20 09:26:47

mark

rei1984 · 发表于 2010-4-21 19:00:40

mark

fanglin06 · 发表于 2010-5-20 17:40:43

回复【楼主位】wall.e
-----------------------------------------------------------------------

我想问下，加速计怎么样计算出倾斜角度呢？

kakaka · 发表于 2010-5-20 19:35:05

mark

xixiangfeng · 发表于 2010-7-13 22:26:05

好东西，不过一般平面上面就用二维就够了吧。打算就用HMC1022 二维磁阻传感器做一个来试试！

denglu · 发表于 2010-12-22 21:33:05

mark

abrokenheart · 发表于 2010-12-27 22:02:58

回复【楼主位】wall.e
-----------------------------------------------------------------------

楼主可以给我一份源代码？谢谢了！jiaxiao_01@163.com

3466756555 · 发表于 2010-12-31 13:41:12

回复【31楼】fanglin06
回复【楼主位】wall.e
-----------------------------------------------------------------------
我想问下，加速计怎么样计算出倾斜角度呢？
-----------------------------------------------------------------------

一般的加速度传感器是不会区分重力加速度和外力加速度的

这看似是一个“缺陷”

用加速度计算倾斜角正是用了这一“缺陷”

由于重力加速度的方向是绝对的参照物（指向地心）

静止时的水平面上 Z轴的加速度=重力加速度1G XY轴的加速度为0

倾斜时 Z轴的分量就会映射到 X、Y轴上

根据XY轴上的重力加速度分量来推断出当前倾斜的角度

06082314 · 发表于 2011-1-10 10:30:08

学习，学习~~

shachengxian · 发表于 2011-1-17 13:43:56

电子罗盘导航纯粹是开玩笑，大部分好的数据都是在严格的磁场环境下获得的。就算是标定也需要用无磁转台。

rockgoogle · 发表于 2011-2-3 20:16:49

记号！

zhikai_wu · 发表于 2011-2-9 08:00:14

Mark

chengluoran · 发表于 2011-2-9 09:23:39

MARK

zpwkxg · 发表于 2011-2-21 12:43:55

谢谢feng_matrix分析！

MAPGPS · 发表于 2011-2-26 12:22:57

我的AR.Drone四轴在起飞后1米高度处悬停很稳, 可以认为机身是水平的了.
如何利用这点来在每次起飞时自动校准三维电子罗盘, 即让机身悬停时自动转一圈(YAW 360度)并采样(X,Y,Z)?

zerodml · 发表于 2011-2-28 10:14:01

回复【12楼】aaa1982
----------------------------------------------------------------
我现在很想学习一下方面知识，请问能把具体算法公式发给我一下么？我邮箱zerodml@126.com.ben.不胜感激！

dadatou · 发表于 2011-6-22 09:10:03

回复【43楼】MAPGPS
我的ar.drone四轴在起飞后1米高度处悬停很稳, 可以认为机身是水平的了.
如何利用这点来在每次起飞时自动校准三维电子罗盘, 即让机身悬停时自动转一圈(yaw 360度)并采样(x,y,z)?

-----------------------------------------------------------------------

不是这样吧，本身机身就会对地磁产生影响吧，应该要罗盘相对四轴转一圈意义更大！个人理解。。。。

thisjoy · 发表于 2011-7-14 11:35:12

mark^……

yanzaihong · 发表于 2011-7-14 12:41:33

mark^……

sn2101 · 发表于 2011-7-19 13:07:39

记号

qiaokeli08 · 发表于 2011-7-22 13:31:23

请问你们说的honeywell的资料是什么资料啊？我怎么一直没看到过啊！还有就是我用的三轴加速度计和三轴磁强计做的电子罗盘为什么在水平放置时准确，但是不平的时候就不准了啊？我用的也是楼主用的算法啊！回复【2楼】feng_matrix 悟
-----------------------------------------------------------------------

psbhero · 发表于 2011-8-1 14:56:48

MARK

b260123292 · 发表于 2011-8-2 10:46:36

刚开始调LSM303DLH，mark以备参考，感谢大家。

Kevin10417 · 发表于 2011-8-30 17:06:01

受教了

wingoing · 发表于 2011-10-12 09:46:39

mark

shensh · 发表于 2011-11-8 12:28:43

我也在搞，学习一下，顶

tree888 · 发表于 2011-12-6 11:07:48

请问feng_matrix悟，三轴地磁在你贴的三轴姿态解算框图中是怎么利用的？
那些数据是怎么融合起来的呢？

tree888 · 发表于 2011-12-6 11:14:48

请问你们说的honeywell的资料是什么资料啊？

hejiehenry · 发表于 2012-1-10 17:32:45

回复【13楼】feng_matrix 悟
-----------------------------------------------------------------------

feng_matrix 悟兄讲的深刻啊，佩服佩服~~

thxl_kis · 发表于 2012-3-5 15:59:44

我也来学习一下

jackwang123 · 发表于 2012-4-2 18:46:45

这个帖子早看到几年就好了

EleHero2012 · 发表于 2012-4-2 20:46:58

非常感谢feng_matrix详细耐心的讲解，我最近也想学习四轴，这让我了解了不少，强力顶一下。希望大家有什么好的想法和发现都分享出来共同进步。有问题大家一起解决。

christec · 发表于 2012-6-29 16:53:57

feng_matrix 发表于 2009-8-26 11:33
HoneyWell的公式没问题的，楼主全部使用弧度浮点运算就对了

请教个问题:

我用的是HMC5883L, 模块与水平面倾斜大概40度左右Z的值才为0, 是不是模块有问题? 我这的经纬度大概是N31, E121.

首先我想到了磁倾角. 然后一想好像不对, 有磁倾角是肯定的, 但是地平面本来就和南北磁极的连线有一定角度的.

我在文档上看到: "地球的磁场强度为0.5-0.6高斯，与地平面平行，永远指向磁北极" 请问是我的模块有问题吗?

feng_matrix · 发表于 2012-7-6 13:21:50

christec 发表于 2012-6-29 16:53
请教个问题:

我用的是HMC5883L, 模块与水平面倾斜大概40度左右Z的值才为0, 是不是模块有问题? 我这的经 ...

一般情况，在赤道附近，磁力线方向基本都是水平于地面的，在南北极是有大倾角的

还有一种情况是传感器附近有磁场干扰，因为地磁非常弱小，只有零点几个高斯

如果传感器旁边有铁制螺丝、壳体，甚至电流较大的导线都会导致传感器输出异常

一块普通的磁铁，表面磁场强度都有几千高斯

所以地磁传感器的相关开发，布局很重要

wjhltk · 发表于 2014-10-2 08:45:08

feng_matrix 发表于 2009-8-26 11:38
还有三轴地磁解耦如果只用三轴加速度计，受外力加速度影响很大，在运动/振动等环境中，输出方向角误差较大
...

分析的比较详细，记录一下！

inkfish321 · 发表于 2014-10-2 10:13:34

学习一下，过段时间要用

情迷MJ比莉珍 · 发表于 2014-10-2 12:55:07

好腻害的阳仔啊

zkl1097 · 发表于 2014-10-17 10:27:02

各位的讨论很不错啊

Timghost · 发表于 2019-4-19 16:18:22

很好的资料，学习了！

请教三维电子罗盘问题

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

请教三维电子罗盘问题　