开源！我的捷联系统的四元数法姿态算法和程序

autokey

原帖由 firedog2011 于 2011-6-23 15:51 发表
搞FPV搞到这么复杂还有乐趣可言吗？唉

乐趣就在这里啊，只是我们没人家厉害，没到这个境界。佩服楼主

[ 本帖最后由 autokey 于 2011-6-23 22:07 编辑 ]

F3C88

编程高手啊:em26:

dongfang

原帖由 dstdx72 于 2011-6-23 12:43 发表
:em26: :em24: 热烈欢迎，狂顶....
http://www.cppblog.com/heath/archive/2009/12/13/103127.html
这个我也看过，哈哈，相当不错
四元数法前几天就试过了，其精度跟积分频率和陀螺输出准确率关系很大
比如陀螺的 ...

没查到你的陀螺信息，我用的是带队列的数字陀螺L3G4200D，温漂测试数据你见过，在http://bbs.5imx.com/bbs/viewthread.php?tid=490438
我觉得用模拟陀螺，至少和L3G4200D相比，缺一个队列功能，这会导致你不能准确知道两次AD数据之间的时间间隔，也就无法得到准确的上一时刻以来的偏转角。而且，我的试验表明，同样的四元数算法，在静止（输入偏转角基本为0）的情况下，要比输入偏转角较大的情况，运算来得快。L3G4200D总是准时在那里采样，有的时候我的程序来不及取掉采样数据，它就把新数据放在一个队列里，而我可以一次性把队列里的数据取走。对于这种来不及计算而堆积起来的数据，我就直接累加，再参与算法，（否则就更来不及算了）。
L3G4200D还有一个卖点: "现有的3轴陀螺仪解决方案依赖两个或三个独立的感应结构，顶多是在同一硅基片上；而意法半导体的陀螺仪则是三轴共用一个感应结构，这一突破性概念可以消除轴与轴之间的信号干扰，避免输出信号受到干扰信号的影响。"
目前我的姿态计算速度一秒在400次不到一点的样子，不包含四元数转出欧拉角。

[ 本帖最后由 dongfang 于 2011-6-23 23:13 编辑 ]

dongfang

原帖由 hifiafrica 于 2011-6-23 14:46 发表
我所见过的自驾仪，都是非常依靠GPS的信号的，我最早玩的AP50，就是三个村田陀螺增稳阻尼，就能飞航线，而且还是三角翼。

不要试图去提高你姿态系统的性能，这个估计很难，要试图把GPS的信号引入你的姿态系统，让 ...

GPS是宏观的导航工具，不能做俯仰横滚姿态的计算的，而这是飞控必须要有的能力。而捷联系统，至少我们能接触到的硬件，都是只能做姿态的。你目前看到我做的只是3轴陀螺仪这一块，很快我就会把加速度信号融合上去，融合是个试验项目，做得好的话，姿态控制能让飞机像根棒子一样飞行。
GPS的使用对我而言是现成的，等先把这两个搞定后，再叠加GPS提供的偏航角和位置信息，就能构成固定翼的基本完整（还缺一个气压传感器）的飞控方案了。

dongfang

原帖由 firedog2011 于 2011-6-23 15:51 发表
搞FPV搞到这么复杂还有乐趣可言吗？唉

不瞒你说，几天前我就是你这样的心情。搞不定，不管是算法也好硬件也好，那是相当的郁闷。不过一旦云开雾散，还是很开心的。

现成的飞控尽管贵，但相对我的时间投入，也算是便宜了。不过买现成的飞控，不能学到什么，还总是担心某天提控回家损失个上千元的。
现在我做的这点东西，硬件成本200元上下，就算掉了，再做一个也就这点钱。软件在自己手里，踏实。

dongfang

原帖由 aband 于 2011-6-23 20:36 发表
楼主没有买《惯性导航》？你说的这些东西在这本书上都有 还有详细的推导

惭愧，我没买。觉得网络是更好的书库。
向你求教一下，《惯性导航》有讲加速度如何融合进角速度计算出的姿态的吗？

aband

原帖由 dongfang 于 2011-6-23 23:48 发表

惭愧，我没买。觉得网络是更好的书库。
向你求教一下，《惯性导航》有讲加速度如何融合进角速度计算出的姿态的吗？

这个倒没专门的讲，但有一章专门讲加速度计的。加速度和角速度融合的方案有很多，如果你对姿态解算的理解足够深，完全可以自己做数据融合方案。
传感器数据融合是系统的东西，不是简单的加加减减就能搞定的，所以一定要深入学习惯导理论。

另外做捷联惯导的话，光有加速度计是不够的，因为它不能提供航向信息。

我一般碰到好书就立即去买实物收藏，也算是对作者的支持。

dongfang

原帖由 aband 于 2011-6-24 11:31 发表


这个倒没专门的讲，但有一章专门讲加速度计的。加速度和角速度融合的方案有很多，如果你对姿态解算的理解足够深，完全可以自己做数据融合方案。
传感器数据融合是系统的东西，不是简单的加加减减就能搞定的，所 ...

我想在实际测试之前，尽量提高融合的有效性，做好试飞前的功课，毕竟现场测试是费时费力的事。不过，我估计现有的惯导理论没有我们所说的这种融合算法。那些做dao dan的，是不会用到我们这种精度这么差、要加速度计融合的陀螺的。
但我已经看到，我们这种加速度计融合的陀螺，实际上也是要理论支持的，判断什么时候加速度计的可靠性高于陀螺，而什么时候加速度计不可靠，这应该是有一定算法的。

可惜这些东西就只有我们个人走实践路线，学习学习MK的思路，等等小打小闹的方法，实际上是浪费大家的时间。

md35

长航时的dao dan也是这样的的算法，精度再高的陀螺，我看也难坚持几小时。

xesable

:em26: 强人啊！

xesable

有时间去看看《捷联惯性导航》三轴陀螺三轴加速计经行微分之后积分后得到姿态数据。（亚马逊上就有的买）
楼主加油！能做出可靠的姿态测量装置很有用的。现在几乎所有的无人机都是建立在这个基础之上的。

dstdx72

:loveliness: 兄弟搞得咋样了？

dongfang

原帖由 dstdx72 于 2011-6-27 19:46 发表
:loveliness: 兄弟搞得咋样了？

最近先跳过了加速度融合，在研究PID控制，用VB写了一个固定翼横滚的PID模拟器。

姿态计算和PID控制,一个不能少:em05:

Aceking1123

四参数法

经典力学中古老的问题之一是刚体运动和坐标变换。在平台式惯性导航系统计算机软件中通常采用欧拉角及其方向余弦矩阵。由理论力学的知识可知，绕定点转动的刚体的角位置可以通过依次转过三个欧拉角的三次转动而获得，也可以通过绕某一瞬时轴转过某个角度的一次转动而获得。对于前者可以采用方向余弦法解决定点转动的刚体定位问题，对于后者可以采用四参数法来解决定位问题。

四参数法也就是使用最广泛的四元数法。四元数理论是数学中的一古老分支，是由哈密而顿于1943年首先提出的，其思想类似平面问题使用复数解的方式。但是该理论建立之后长期没有得到实际应用，随着空间技术、计算技术、特别是捷联式惯性导航技术的发展，四元数的优越性日渐引起人们的重视。其先求解姿态四元数微分方程，再由姿态四元数确定航向角和姿态角。虽然需要四个微分方程，较欧拉角微分方程多一个方程，但进行数值计算求解时只需要进行加减乘除运算，所以求解过程的计算量要比欧拉角法减少得多。

它的优势体现在，与方向余弦法比较，计算量小，存储容量少，仅需要进行简单的四元数规范化处理便可以保证姿态矩阵的正交性，因而成为一种普遍采用的方法。但是不可避免地引入了有限转动的不可交换性误差，特别是当运载体姿态变化比较剧烈，或伴有角振动时，该法会产生严重的姿态漂移误差，所以只能用于工作环境平缓和缓慢的运载体[2]。

文件名: 捷联导航系统 p15起.doc
描述: 捷联导航系统 p15起.doc
下 载地址: http://www.rayfile.com/files/81a4ca85-a222-11e0-804d-0015c55db73d/

Aceking1123

上面这个文档写得很好，可以学习参考下

四元数的问题在于微分量的可靠度，有角振动时这种叉乘的办法就会飘没影儿了，而实测传感器的角振动是相当之大的
不知楼主准备咋解决

[ 本帖最后由 Aceking1123 于 2011-6-29 15:56 编辑 ]

dongfang

陀螺仪的白噪音是存在的，还不小。这里是个两难的选择，要么就不上四元数，只假设模型在平面上飞行,这样的话，只要做3轴积分就行了，白噪音大部分会被积分抵消掉。我的陀螺仪在这种计算模型下，每分钟的漂移在5度以下。但对简单的固定翼飞机盘旋动作，这种算法会产生持续严重的累计偏差。

四元数的把噪音引起的误差放大了大约5倍，往长期走，当然也是不行的。

长期的看，都得靠加速度传感器修正。

但如果飞行器要做3D机动，四元数是不二选择。只做平面运动的飞行器，是不是用四元数，取决于个人的需求，以及能不能找到好的三轴陀螺。

我看了一点点MK的代码，没有用四元数，不过他针对旋转后的姿态判断，是先清零的。我认为这可能会造成MK的局限性，不能连续做复杂动作（尽管没见过MK）。

dstdx72

原帖由 Aceking1123 于 2011-6-29 15:36 发表
...但是不可避免地引入了有限转动的不可交换性误差，特别是当运载体姿态变化比较剧烈，或伴有角振动时，该法会产生严重的姿态漂移误差，所以只能用于工作环境平缓和缓慢的运载体[2]。 ...

我怪说不得呢，我也发现4元数法有这个特点
手拿板子小范围的往复运动，运动多了误差就出来了，原来是这样啊
不过4元数的无理论误差的算法，还是值得称道的
尤其是连续滚转后的误差不大
确点是三角函数乘除次数实在太多了

dstdx72

原帖由 dongfang 于 2011-6-29 18:01 发表
陀螺仪的白噪音是存在的，还不小。这里是个两难的选择，要么就不上四元数，只假设模型在平面上飞行,这样的话，只要做3轴积分就行了，白噪音大部分会被积分抵消掉。我的陀螺仪在这种计算模型下，每分钟的漂移在5度以下 ...

你有兴趣可以试试我那个微分方程，挺好用的哈，可直接用于4轴
和4元数比就是运算量小得多，而且比较耐噪声和角震动
缺点是需要精确校准陀螺，和姿态计算频率必须较高，否则全姿态误差较大
如果修正了陀螺漂移，我这个方程在板子静止的时候是绝对一动不动的
而同样的修正陀螺，四元数法在板子静止的时候是乱蹦的，时不时还来个猛的（有好的加速度修正算法这个问题就不大了）

[ 本帖最后由 dstdx72 于 2011-6-29 19:01 编辑 ]

dongfang

你说的乱蹦，我估计是电磁干扰引起的，建议做些简单的滤波。

我那个算法在我的板子上倒是很乖，静态时漂移和原来的积分算法区别不大。回头看看你的算法表现如何。

dongfang

正好有人PM问我，CLAMP是什么，实际上就是一个限定范围的macro了，定义如下：

#define CLAMP(x , min , max) ((x) > (max) ? (max) : ((x) < (min) ? (min) : x))