捷联惯导算法心得

kmani · 发表于 2013-10-7 20:40:35

agnd 发表于 2012-10-30 13:15
问一下,我用两套一样的陀螺加计组合,安在不同位置,它们互为对方的观测者行吗? ...

不行，加起来除以2.
观测者的优点正是被观测者的缺点。

kmani · 发表于 2013-10-7 22:05:33

john800422 发表于 2013-8-10 23:56
四元數+電子羅盤沒有詳細看過
目前是使用互補濾波實現航向的

你是台湾的？

kmani · 发表于 2013-10-7 22:09:36

本帖最后由 kmani 于 2013-10-7 22:31 编辑

kmani 发表于 2013-10-1 10:39
感谢楼主，但是你文中有个描述性错误。
第62行，应该是第三行的三个元素而不是第三列的三个元素。 ...

汗，没有问题，搞错了坐标系变换方向。

那个磁场的变化太绕了。
从机体坐标系转换到导航坐标系，又从导航坐标系转换到机体坐标系。转换矩阵都是陀螺更新出来的四元数构成的。

规定从机体坐标系到导航坐标系的转换矩阵是A，那么从导航坐标系到机体坐标系的转换矩阵就是A的逆矩阵：A` 。
这样看起来两次转换完毕之后的磁场矢量应该和最初测量的磁场矢量是相等的，但是在从机体坐标系转到导航坐标系之后，做了一次校准。
就是让bx^2=hx^2+hy^2，by=0，bz=hz，理论上hx=某个值，hy=0，hz=另外一个值（这样就得到了导航坐标系中准确的磁场矢量(1,0,x)，可以理解成导航坐标系中的(0,0,1)）。假如没有这一次校正，在两次坐标系转换之后的磁场和测量的磁场时一样的。

看了几天终于搞懂了，是悟性太差了吗，看来这是硬伤。

看完之后，我不打算使用PI融合方式，这种融合缺点很大（也可以说成优点，优点就是收敛快），当飞机加速运动时，角度就会加快收敛于错误的角度。

john800422 · 发表于 2013-10-8 03:41:48

kmani 发表于 2013-10-7 22:05
你是台湾的？

是的

kmani · 发表于 2013-10-8 09:54:07

本帖最后由 kmani 于 2013-10-8 10:02 编辑

john800422 发表于 2013-10-8 03:41
是的

嘿嘿，好羡慕的感觉。
你是台湾哪个地方的，台湾美女多不？台湾工资高不？
台湾房价跟国内比贵不贵？
台湾是不是用繁体字？那你怎么会简体字的呢？
你认识林清安不？台湾的一个教授，他是教PROE的。

john800422 · 发表于 2013-10-8 11:39:07

住在北部

目前還是學生
工資、房價甚麼的差別
不是很清楚

美女應該都差不多吧

是用繁體字，簡體字看久就習慣了
比較麻煩是用辭不太一樣

不認識林清安

kmani · 发表于 2013-10-8 21:09:01

john800422 发表于 2013-10-8 11:39
住在北部

目前還是學生

我也是学生，目前研二。
你呢？
我好想去台湾玩啊，我要是去台湾了，就找你玩啊。

john800422 · 发表于 2013-10-9 00:18:12

目前大四，讀物理的

歡迎，不過不常到處跑，知道的地方也不多

金牛AKI · 发表于 2013-10-10 11:58:08

看不懂希望早日看懂哈哈

jwl5879 · 发表于 2013-10-16 20:17:47

john800422 发表于 2013-3-30 00:59
我的理解

这位大神，能不能把你这副截图的相关资料共享一下啊，小弟不胜感激~~

jxglg · 发表于 2013-10-16 21:56:35

好文章。谢谢楼主。

jwl5879 · 发表于 2013-10-17 10:44:28

楼主大神你好，弱弱问一下地理坐标系是东北天，那为什么重力向量是（0,0,1g）而不是（0,0，-1g）呢？

john800422 · 发表于 2013-10-17 10:56:11

jwl5879 发表于 2013-10-16 20:17
这位大神，能不能把你这副截图的相关资料共享一下啊，小弟不胜感激~~

自己寫的資料
https://github.com/Hom19910422/Quadcopter

jwl5879 · 发表于 2013-10-18 09:51:47

john800422 发表于 2013-10-17 10:56
自己寫的資料
https://github.com/Hom19910422/Quadcopter

谢谢你，敢问下既然地理坐标系是东北天那重力向量为什么不是（0,0,-1g）呢？

john800422 · 发表于 2013-10-18 11:06:59

jwl5879 发表于 2013-10-18 09:51
谢谢你，敢问下既然地理坐标系是东北天那重力向量为什么不是（0,0,-1g）呢？ ...

只是座標定義的問題

從加速度計就可以看出來了
靜止時，Z軸讀到的值是"正"的
而 Z 軸正是指向＂天＂

所以若是 (0, 0, +1)，
表示亦是指向＂天＂的重力加速度

jwl5879 · 发表于 2013-10-18 19:37:13

john800422 发表于 2013-10-18 11:06
只是座標定義的問題

從加速度計就可以看出來了

原来是这样，我还没发现片子静止朝上时z轴数据是正的呢，没有看过数据，嘿嘿

zhanwang_sky · 发表于 2013-10-18 19:37:33

强帖， Mark！

-阿发- · 发表于 2013-10-19 14:53:55

力挺！！学习了。。

ES_SHARE · 发表于 2013-10-19 15:59:29

高手资料，慢慢研究，继续关注

zhangyun2743 · 发表于 2013-10-20 10:57:37

真是好文章，超级喜欢这个帖子，感谢楼主~！！！！

zhaojianjiang · 发表于 2013-10-20 16:16:34

谢谢楼主，后悔数学没学好，用的时候看不懂，慢慢看

kingjie0210 · 发表于 2013-10-22 16:00:47

刚入门，绝对好文章，继续关注

363546178 · 发表于 2013-10-23 20:02:06

学习一下~~~

longfeix86 · 发表于 2013-10-23 20:22:55

帮顶一个，非常不错的资料

6091820503 · 发表于 2013-10-24 17:04:46

导航坐标系是东北地坐标系
最基本的是只使用陀螺仪进行积分，适合反应频率高的场合，也可以使用加速度计进行校正，或者再加上磁强计进行校正
四元数微分方程如下：

kakarotto · 发表于 2013-10-25 14:07:03

mark

精辟，接收学习

kingjie0210 · 发表于 2013-10-25 16:16:06

6091820503 发表于 2013-10-24 17:04
导航坐标系是东北地坐标系
最基本的是只使用陀螺仪进行积分，适合反应频率高的场合，也可以使用加速度计进 ...

这个四元数微分方程，跟这个帖子里的不一样

SHIYOULAN · 发表于 2013-10-25 23:45:22

相当不错的见解

363546178 · 发表于 2013-10-28 18:55:05

看完之后感觉思路瞬间清晰了~

jackk · 发表于 2013-10-31 09:54:33

mark. mark. mark

prow · 发表于 2013-10-31 17:24:16

mark以后看看

wenziheni · 发表于 2013-11-2 09:08:20

好文章

济南电子爱好者 · 发表于 2013-11-4 19:10:14

虽然目前已经完成了姿态的解算,得到了四元数,但是不太明白如何把当前的姿态四元数用来控制电机的转动上 ? 我这里不是指的如何调整PD 参数 .

babyhua · 发表于 2013-11-19 20:21:07

顶！！！！！！！！！！！！！！！！！！！！！！！！！

zhahc · 发表于 2013-11-25 22:02:47

seanwood 发表于 2013-3-21 09:56
有人问加电子罗盘的融合是怎么回事，我再献丑一回，开课讲解喽，来鼓掌来顶
我嚓，顶楼不能编 ...

楼主你好，从机体坐标系的电子罗盘测到的矢量转成地理坐标系下的磁场矢量hxyz（测量值）
      // compute reference direction of flux
      hx = 2*mx*(0.5 - q2q2 - q3q3) + 2*my*(q1q2 - q0q3) + 2*mz*(q1q3 + q0q2);
      hy = 2*mx*(q1q2 + q0q3) + 2*my*(0.5 - q1q1 - q3q3) + 2*mz*(q2q3 - q0q1);
      hz = 2*mx*(q1q3 - q0q2) + 2*my*(q2q3 + q0q1) + 2*mz*(0.5 - q1q1 - q2q2);
这三个公式怎么推导出来的，我翻了好多书都没找到类似的公式。求解啊……

chushibinsaobao · 发表于 2013-11-26 12:54:47

感谢楼主分享！！真实好东西

zhahc · 发表于 2013-11-28 20:02:21

zhahc 发表于 2013-11-25 22:02
楼主你好，从机体坐标系的电子罗盘测到的矢量转成地理坐标系下的磁场矢量hxyz（测量值）
// comp ...

已经懂了

lylvcongyi · 发表于 2013-12-2 15:21:15

mark 支持楼主宝贵的参考资料

闲鱼翻身 · 发表于 2013-12-2 15:51:15

感谢LZ啊~！收下！

chushibinsaobao · 发表于 2013-12-2 18:13:13

楼主你真厉害！我最近一直在看飞控的姿态解算，一直有很多问题。特别是加速度计和磁力计如何修正陀螺仪的，今天看了你得贴子，受益匪浅，但是还是有点问题，想向你讨教。本人是新注册俄会员，现在加不了QQ！我的是419500541

chushibinsaobao · 发表于 2013-12-2 18:21:11

楼主我有个问题！

  exInt = exInt + ex * Ki * halfT;
  eyInt = eyInt + ey * Ki * halfT;
  ezInt = ezInt + ez * Ki * halfT;
难道exyz的单位是和陀螺仪的单位一致???

chushibinsaobao · 发表于 2013-12-3 16:27:12

大师在吗？个人很些问题想向你请教：我现在看咱们那个姿态解算又有了新的疑问：加速度计和磁力计测量的是当前值，但是它们与四元数姿态矩阵进行求解误差的时候，我怎么觉得这个四元数姿态矩阵是上一次的，而加速度计和磁力计是这次的，它们本来就不一样啊！怎么能用这次的测量值来修正上一次的坐标呢
是不是应该这样：先用陀螺仪积分求出此刻的四元数姿态矩阵，然后再用加速度计和磁力计进行修正

chushibinsaobao · 发表于 2013-12-3 16:32:51

zhahc 发表于 2013-11-25 22:02
楼主你好，从机体坐标系的电子罗盘测到的矢量转成地理坐标系下的磁场矢量hxyz（测量值）
// comp ...

亲，我也是新手，也在学习飞控。不过对于这个问题我的理解是这样的。
那个计算公式的意义就是将基于机体坐标系测量得到的磁力计的值换算到大地坐标系，并且它们的欧拉角顺序是ZYX，
用的就是四元数矩阵。

jj632856828 · 发表于 2013-12-4 08:50:27

MARKMARKMARK

lzchuo · 发表于 2013-12-4 09:04:16

学习了

chushibinsaobao · 发表于 2013-12-4 10:56:21

大师在吗？个人很些问题想向你请教：我现在看咱们那个姿态解算又有了新的疑问：加速度计和磁力计测量的是当前值，但是它们与四元数姿态矩阵进行求解误差的时候，我怎么觉得这个四元数姿态矩阵是上一次的，而加速度计和磁力计是这次的，它们本来就不一样啊！怎么能用这次的测量值来修正上一次的坐标呢
是不是应该这样：先用陀螺仪积分求出此刻的四元数姿态矩阵，然后再用加速度计和磁力计进行修正

北辰 · 发表于 2013-12-4 12:06:39

学习了顶楼主

franki · 发表于 2013-12-4 16:28:28

好东西啊。。。。顶

chinaboy25 · 发表于 2013-12-4 17:18:19

精辟

cndavy · 发表于 2013-12-5 05:47:15

seanwood 发表于 2012-9-5 08:01
这里误差没说清楚，不是指向量差。这个叉积误差是指将带有误差的加计向量转动到与重力向量重合，需要绕什 ...

似乎明白点了，重力转换因为 0 ，0，1 取第三列，地磁不太明白怎么计算出来的

cndavy · 发表于 2013-12-5 06:12:29

seanwood 发表于 2013-3-21 09:56
有人问加电子罗盘的融合是怎么回事，我再献丑一回，开课讲解喽，来鼓掌来顶
我嚓，顶楼不能编 ...

      // integrate quaternion rate and normalise

      q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;

      q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;

      q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;

      q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;

这个怎么来的？请赐教。是四元数加法？

chushibinsaobao · 发表于 2013-12-6 12:18:33

cndavy 发表于 2013-12-5 06:12
// integrate quaternion rate and normalise

q0 = q0 + (-q1*gx - q2*gy ...

亲！这个公式其实就是求解四元数微分方程，使用的是龙格库塔法，并且是一阶龙格库塔法！具体可以参考研究生课程数值分析！
四元数微分方式就是

chushibinsaobao · 发表于 2013-12-7 11:54:06

怎么没有人来讨论姿态解算呢！！同志们快来了！我最近也在学习飞控的姿态解算！希望能在这里和大家一起分享！！
下面给大家传个我写的PPT吧！希望同志们有什么好东西能和大家一起分享一下！
？？？？不会吧不支持PPT哈哈！！

看样子只能发个压缩包了！！

绝版彬仔 · 发表于 2013-12-9 23:26:24

学习..............

jzhang123 · 发表于 2013-12-10 10:05:31

mark 牛B

hhhhhchch1 · 发表于 2013-12-10 10:09:56

谢拉，刚打算学学。

wb156351 · 发表于 2013-12-10 10:39:45

收藏一下，慢慢看

lxalxy · 发表于 2013-12-10 12:20:41

好贴，mark一下，慢慢看

zjim001 · 发表于 2013-12-12 11:08:08

真在研究惯导产品，谢谢楼主的无私分享，很有用，收藏了！

chushibinsaobao · 发表于 2013-12-15 11:03:11

怎么没有人讨论啊！！大师能不能给我们初学者点拨一下

chushibinsaobao · 发表于 2013-12-17 16:09:45

大师能不能再传授一些啊！

kingjie0210 · 发表于 2013-12-17 17:01:57

请问一下，这篇心得用的是什么算法？是互补滤波算法吗？具体在哪里体现的，麻烦理解的大师给我这个新手解释一下子吧，严重感谢

kingjie0210 · 发表于 2013-12-17 17:09:13

kmani 发表于 2013-10-8 21:09
我也是学生，目前研二。
你呢？
我好想去台湾玩啊，我要是去台湾了，就找你玩啊。 ...

你好，问个问题，1、这篇心得中用的导航坐标系是东北天还是北东地，为什么导航坐标系中准确的磁场矢量为(1,0,x)，0是怎么来的？2、这篇心得中用的是互补滤波算法吗？在哪儿体现的？希望在的话热心帮我解答一下，我新手，谢谢

kingjie0210 · 发表于 2013-12-17 19:12:47

john800422 发表于 2013-10-17 10:56
自己寫的資料
https://github.com/Hom19910422/Quadcopter

你好，问个问题，1、这篇心得的代码中用的导航坐标系是东北天还是北东地，为什么导航坐标系中准确的磁场矢量为(1,0,x)，0是怎么来的，0对应的是东吗？2、这篇心得中用的是互补滤波算法吗？在哪儿体现的？希望在的话热心帮我解答一下，我新手，谢谢

aaasjc · 发表于 2013-12-17 19:18:43

不断的学习ING

zhahc · 发表于 2013-12-22 15:57:03

john800422 发表于 2013-8-15 21:24
該講的程式、註解都有了
還要講甚麼?

黄脸猫哥，你的互补算法移植了，但是在航向角从180->181的时候会突变到0，反方向到181，请问这个如何解决的

kingjie0210 · 发表于 2013-12-22 21:26:47

chushibinsaobao 发表于 2013-12-6 12:18
亲！这个公式其实就是求解四元数微分方程，使用的是龙格库塔法，并且是一阶龙格库塔法！具体可以参考研究 ...

兄弟，能否结合四元数解释一下四阶龙格库塔法，K1、K2、K3、K4系数的确定方法

kingjie0210 · 发表于 2013-12-23 14:44:31

楼主开头说“地理坐标系：东、北、天，以下简称地理。在这个坐标系里有重力永远是（0,0,1g），地磁永远是（0,1,x）（地磁的垂直不关心）两个三维向量”，后边解释磁力计+加计+陀螺版代码时说“因为地理地磁水平夹角，我们已知是0度（抛去磁偏角的因素，固定向北），所以by=0，bx=某值，但地理参考地磁矢量在垂直面上也有分量bz，地球上每个地方都是不一样的”，这前后地理坐标系不一样吗，为什么前边说地磁永远是（0,1,x），后边解释代码时又说by=0了？哪位大师解释一下

木君之上 · 发表于 2013-12-23 15:16:04

要不是我学过惯性导航。。。这些东西怎么看的懂。。。。表示当时就被这门课虐惨了

yhybear0 · 发表于 2013-12-23 23:36:36

好文章，慢慢看

xshhou · 发表于 2013-12-24 09:37:19

mark

chushibinsaobao · 发表于 2013-12-25 17:26:47

kingjie0210 发表于 2013-12-23 14:44
楼主开头说“地理坐标系：东、北、天，以下简称地理。在这个坐标系里有重力永远是（0,0,1g），地磁永远是（ ...

东、北、天分别对应X,Y,Z,标准的地磁场在纬度方向的分量为零，其实也就是在Y轴上没有分量。所以by=0；而我们的飞机在飞行的时候肯定不可能头一直朝北，所以在实际磁强计测量的值就有by的分量。

luhuizszw · 发表于 2013-12-25 17:30:53

直接mark

seanwood · 发表于 2013-12-26 10:04:58

chushibinsaobao 发表于 2013-12-25 17:26
东、北、天分别对应X,Y,Z,标准的地磁场在纬度方向的分量为零，其实也就是在Y轴上没有分量。所以by=0；而 ...

东北天不是XYZ，也不是YXZ，而是地理坐标系。地磁在地理坐标系上是0,1,x

而XYZ是机体坐标系，一般都是以飞行器飞行轴向为X，即横滚在X轴上滚。俯仰在Y轴上俯仰，在Z轴上转向。
当飞机水平朝北的时候，X轴与南北重合，Y与东西重合，Z与天地重合。所以XYZ为1，0，x。

winson08 · 发表于 2013-12-26 10:26:30

牛逼，正在学习惯性导航算法，向牛人学习

gzhmcu · 发表于 2013-12-26 12:37:53

谢谢分享

new-bird · 发表于 2014-1-4 16:07:20

MARK,学习中

AI谁谁 · 发表于 2014-1-5 11:33:39

mark 学习

ad1984fr · 发表于 2014-1-23 13:33:10

MARK了一年的帖子终于耐心的学习完了，对姿态结算了解的更深入了。谢谢楼主！

shiruquaan23 · 发表于 2014-1-24 16:55:35

楼主很给力，顶一个

wll3026 · 发表于 2014-1-25 09:01:41

牛人~ 围观学习！

茶亦爽 · 发表于 2014-1-31 16:30:04

楼主十分强大，感谢无私分享。

兄弟会十戒之首 · 发表于 2014-1-31 18:13:20

对于这种扫盲提升贴，我第一件事就是点赞，然后保存，然后看

z350126876 · 发表于 2014-2-1 09:01:20

mark

onlyzore · 发表于 2014-2-1 11:52:07

kmani 发表于 2012-12-12 12:37
带通滤波。

带通滤波会带来延时！

vague1945 · 发表于 2014-2-2 00:19:34

近期终于上升到做飞控的级别了，必须要学习一下

竹蜻蜓 · 发表于 2014-2-17 12:02:27

热心肠的技术牛人，不说了眼泪哗哗的

竹蜻蜓 · 发表于 2014-2-17 12:03:04

热心肠的技术牛人，不说了

zwqhzy99 · 发表于 2014-2-17 17:46:30

东西不错，多谢分享了。

macp · 发表于 2014-2-21 10:36:58

这个确实高级难诶。。。。

鼻儿眼睛花 · 发表于 2014-2-21 10:52:32

kmani 发表于 2013-10-8 21:09
我也是学生，目前研二。
你呢？
我好想去台湾玩啊，我要是去台湾了，就找你玩啊。 ...

你的四轴做的咋样了啊？

yat · 发表于 2014-2-21 11:34:10

mark 捷联惯导算法心得

shuran · 发表于 2014-2-26 15:41:54

楼主这个帖子很牛叉啊

cltx · 发表于 2014-2-26 21:36:22

高手，多谢分享！

leicheng · 发表于 2014-2-26 22:14:34

本帖最后由 leicheng 于 2014-2-26 22:24 编辑

楼主写的这个算法属于互补滤波的一种算法。我想问的问题是，如果采用卡尔曼滤波，能不能不求四元数，直接用姿态角作为状态变量行吗？姿态角的更新直接用△T*ω,姿态角的测量量通过加计和磁强计来计算。

木君之上 · 发表于 2014-2-27 09:29:39

又看一次，强贴！

63329272 · 发表于 2014-2-27 10:59:38

seanwood 发表于 2012-8-20 08:35
请注意初始角是欧拉角，所以要遵循欧拉角的定义来计算。
通常思维计算出来的倾角不知道数学上是什么名字 ...

LZ，能不能再提供下z的初始值计算，谢谢

63329272 · 发表于 2014-2-27 11:20:43

seanwood 发表于 2012-9-13 10:27
磁力計離電機多遠能避免干擾我沒試過，不過我試過離DC-DC電感5-7cm后，干擾就微乎其微了。
另外據我所知電 ...

你好，请问如何进行磁力计的椭圆校正？

63329272 · 发表于 2014-2-27 13:55:21

htjgdw 发表于 2012-9-6 21:26
我试验了楼主贴的代码。发现在静止的情况下计算的欧拉角比较正确，但是稍微转动一下角度，计算额结果就会有 ...

我也遇到了同样的问题，你现在有找到解决方法吗？是否可以开源一下

htjgdw · 发表于 2014-2-27 14:44:01

63329272 发表于 2014-2-27 13:55
我也遇到了同样的问题，你现在有找到解决方法吗？是否可以开源一下

我后来工作忙就放下了。

捷联惯导算法心得

本帖子中包含更多资源

本帖子中包含更多资源

本帖子中包含更多资源

本帖子中包含更多资源