有人问惯导的姿态算法，没啥神秘的，我来写一下。

heuyck · 发表于 2011-6-2 10:24

惯导的姿态算法用在固定翼带GPS飞控上，因为精度要求很低，所以不用很复杂，没啥神奇的。
卡尔曼滤波算法也还好，不过想要精确地建模对不是自控之类出身的我来说太难了。。。

言归正传，我手头有两本惯导书，袁信、郑锷的《捷联式惯性导航原理》，邓正隆的《惯性技术》。
要用的东西里面都有讲的，我摘录出了要点，要是看不懂请去找原书。
书我就不上传了，比较大，自己按名字去GOOGLE一下一大把。

欧拉角算法：矩阵上到下三个角度（老是记不住读啥）是roll pitch和yaw，上面带点的是更新后的角度，不带点的是上个周期算出来的角度。
Wx,y,z也是这个顺序的三个陀螺的这个周期的角度（=周期*角速度）。

四元数初始化，你可以根据刚上电地面加速度计静止时的倾斜角来计算初始四元数。
q0-3为四元数四个值，四元数的概念请参考以上两书，不做解释。

四元数更新算法，一阶龙库法，同样4个量（入、P1-3）也为四元数的四个值。
Wx,y,z是三个陀螺的这个周期的角速度，T为更新周期。

四元数的规范化，每次更新后都需要做规范化处理，再转化为余弦矩阵求出欧拉角（即姿态角），具体见两书，很好找。

tonny1025 · 发表于 2011-6-2 10:27

晕啊！！！！！！！！！
顶了慢慢看。

heuyck · 发表于 2011-6-2 10:30

欧拉角算法主要是存在pitch为90度时的奇点，还有三角函数的误差累计问题，不过有加速度计的校正关系不大。算法写起来简单。
四元数误差累计比较小，规范化也很容易，也不需要余弦矩阵的正交化处理，只是要算出欧拉角还要转化为余弦矩阵再求反三角，计算量也不算少了。
对固定翼来说，这点都无所谓了。只是欧拉角算法可能在飞机螺旋（pitch为90度）时完全乱掉。所以我是用了四元数。

heuyck · 发表于 2011-6-2 10:34

加速度计与陀螺的混合算法：卡尔曼对非线性系统来说不怎么样，不少人用K值（就是各打50大板）滤波。
例如：混合后的角度=陀螺角度*0.7+加速度角度*0.3。
其实就是类似PID中的P，如果要纠正陀螺的零偏，用点PID的I就行了。加速度的差异积分到一定量后改变陀螺的0点。

heuyck · 发表于 2011-6-2 10:37

觉得有用的顶一个
觉得看不懂的也顶一个
哈哈

fujianshu · 发表于 2011-6-2 10:43

很好

兔哥 · 发表于 2011-6-2 10:52

看不懂,纯帮顶..........

dstdx72 · 发表于 2011-6-2 11:38

:em26: 顶！这个才是做飞控的好东西啊哈哈
一直懒得上网查，更不想花钱买书
一直为姿态问题困扰，烦心哪
楼主真是好心人，这下总算找到姿态解算公式了:em15:

seibu · 发表于 2011-6-2 11:43

＠＃￥%︿＆＊＊%︿￥＃＠！
暈得很，看不懂

dstdx72 · 发表于 2011-6-2 11:43

原帖由 heuyck 于 2011-6-2 10:34 发表
加速度计与陀螺的混合算法：卡尔曼对非线性系统来说不怎么样，不少人用K值（就是各打50大板）滤波。
例如：混合后的角度=陀螺角度*0.7+加速度角度*0.3。
其实就是类似PID中的P，如果要纠正陀螺的零偏，用点PID的I就 ...

这个也很重要，能否叙述得再详细点？
关键运行中，无论陀螺和加速度计，都是不断变化的值，其中还包含误差，你根本不知道哪些数据是正确的，某个数据何时是正确的，怎么能求出一个正确的稳定值，来作为比例(P)计算的基准呢？

既然LZ已经讲了这点，就详细点嘛，指导下大家噻

xldisk · 发表于 2011-6-2 11:48

不懂，帮顶

hydefee · 发表于 2011-6-2 12:17

和天书一样了，高数早忘光光了。。。

yyqop01 · 发表于 2011-6-2 12:44

不懂，技术贴，帮顶

heuyck · 发表于 2011-6-2 12:46

原帖由 dstdx72 于 2011-6-2 11:43 发表

这个也很重要，能否叙述得再详细点？
关键运行中，无论陀螺和加速度计，都是不断变化的值，其中还包含误差，你根本不知道哪些数据是正确的，某个数据何时是正确的，怎么能求出一个正确的稳定值，来作为比例(P)计 ...

加速度计做平均值的周期，加计平均值和陀螺融合的K值，两者如何得到一个最优值我也不清楚，应该是跟惯导平台受震动的幅度和频率有关吧。我用的陀螺有低通滤波器，内置数字和外置RC都有，设置值跟最后结果也有关系。
从使用在固定翼的方面来说，角速度不大，精度要求不高，即使出现误差累积，也能通过GPS纠正过来。
我把飞控绑在一个KV比较高的电机边上，从0油门到全油门测试，估计能代表大多数固定翼的震动频率了。
然后加点缓冲的材料，远离电机，震动影响应该不大了。
当然这些是我现在的想法，试验中肯定会有问题，还得改。
更好的更精确的算法肯定也有，但简单算法如果够用的话，就没必要再花精力了。

浪天涯迹 · 发表于 2011-6-2 12:47

不懂了,,,,但要顶一下,

人外有天,天外有人,天人合一,

md35 · 发表于 2011-6-2 12:51

看看就算了，头都大了，还是用红外温度的吧。简单。

wfwg · 发表于 2011-6-2 12:55

好东西，学习了。

dstdx72 · 发表于 2011-6-2 13:17

原帖由 heuyck 于 2011-6-2 12:46 发表

加速度计做平均值的周期，加计平均值和陀螺融合的K值，两者如何得到一个最优值我也不清楚，应该是跟惯导平台受震动的幅度和频率有关吧。我用的陀螺有低通滤波器，内置数字和外置RC都有，设置值跟最后结果也有关系。 ...

哎，LZ还是没回答我的问题哟，要是觉得这是机密，那就算了
我现在遇到的困扰就是陀螺零点漂移，这个问题必须很重视
陀螺本身随时间的零点漂移是很小很小的，但它的困难之处在于漂移是完全随机不可预测的和不断积累的
而且零点对角度测量影响很大，因为角度是积分出来的，零点漂移哪怕只有极小的一点，积分后的误差也很恐怖
实际上，零漂相对于噪声和震动，可以说小得很，但影响却很大，所以几千块以下的陀螺，虽然从指标上看很好，但都无法满足直接使用要求，必须定时修正
这就对修正算法提出了要求
由于飞控在飞行中，无论陀螺还是加速度计，随时处于不断变化之中，所以这个修正算法必须是动态修正算法，而且由于飞机姿态在空中永远不可能处于精确水平
简单的企图在一个较长时间取平均值或滤波来做零点基准这个想法就免了，无论时间多长，求出来的平均值都不可能是真实的，误差都很大（因为飞机在不停俯仰或横滚），而纠正零漂所要求的精度，必须远远低于噪声和震动
即便可以定期用加速度计修正姿态角，但无论哪个修正算法，修正后的误差，都跟陀螺的原始输出误差有关，原始误差越大，修正后的也越大。（加速度计如何滤除有害加速度信息本身就很困难，要提高到可以作为角度修正基准的精度就更不可能）
LZ说固定翼对姿态角的误差容许较高，即便如此，长时间的误差积累也可能是致命的-----除非你指望在炸机前陀螺零点又往回飘，有这个可能，但也有可能在往回飘之前就炸了，即便不炸，那飞机的晃动也是不得了的
实际试飞表明，没有很有效的动态修正算法，漂移引起的误差可以大到足以引起炸机，除非一直在千米高空飞行
（GPS实时信息是xy平面的，可以用于修正航向，但不可能用于姿态修正，企图用GPS修正姿态的想法就免了）
我看你提到“其实就是类似PID中的P，如果要纠正陀螺的零偏，用点PID的I就行了。加速度的差异积分到一定量后改变陀螺的0点。”，很有兴趣，觉得有点意思，能否叙述详细点？要进行P运算，必须有个计算P差值的基准，关键在高动态情况下，这个基准如何取得？
LZ能否说详细点，一起探讨探讨？

wu253339539 · 发表于 2011-6-2 13:48

读书时最不喜欢数学了，看到COS ,SIN就头疼

心在中天 · 发表于 2011-6-2 13:52

学习了:em26:

有人问惯导的姿态算法，没啥神秘的，我来写一下。

主题

好东西

【站内推荐】 /1