5iMX.com 我爱模型 玩家论坛 ——专业遥控模型和无人机玩家论坛（玩模型就上我爱模型，创始于2003年）

标题: 开源！我的捷联系统的四元数法姿态算法和程序 [打印本页]
作者: dongfang    时间: 2011-6-23 10:28
标题: 开源！我的捷联系统的四元数法姿态算法和程序
最近研究飞控的时候，感受到，网上大量他人共享开放的信息给我提供了很多便利。（尽管也混杂了大量无法使用的信息）

捷联系统的四元数法姿态算法的难点不是资料找不到，而是资料大多数没有标明欧拉角的定义（拜科学家意见不统一所赐，欧拉角有12种定义方法），而不同定义的欧拉角，双向转换四元数的公式都是不一样的。而我最终找到了一套适合飞控的算法。

为了不让这些开放信息到了我这里就终结了，也为了后人不要重复无谓的重复劳动，我把我做的东西也开源出来。


算法名：捷联系统的四元数法姿态算法

算法输入：物体的初始姿态，3轴陀螺仪不同时刻的Yaw，Pitch，Roll的角速度；
算法输出：物体的当前姿态。

具体算法：
1. 初始姿态的四元数(w,x,y,z)=(1,0,0,0) 命名为A
2. 读取3轴陀螺仪当前时刻的Yaw,Pitch,Roll角速度，乘以上次计算以来的间隔时间，得到上一时刻以来(Yaw,Pitch,Roll)的变化量，命名为欧拉角b
3. b是Tait-Bryan angle定义的欧拉角，将其转为四元数B
4. A=A×B，做四元数乘法，即可得到当前姿态对应的新的四元数A
5.重复2～4部，即可连续更新姿态
6.将四元数A重新转换为Tait-Bryan angle形式的欧拉角a，就可以以直观的形式查看当前姿态


这里严重感谢http://www.cppblog.com/heath/archive/2009/12/13/103127.html 的作者，他的代码解决了我最重要的问题: Tait-Bryan angle定义的欧拉角和四元数的双向转换。但他是个游戏开发者，没做姿态更新，我补充了姿态更新的代码。


另外补充一句，我使用的是ardunio做的开发，也就是12M的8位AVR单片机，目前，每秒在获取数据400个的速度下，如此大工作量的算法依然能跑。3D旋转N圈，大约10秒钟，各轴漂移在5度左右。

[ 本帖最后由 dongfang 于 2011-6-23 11:01 编辑 ]
作者: dongfang    时间: 2011-6-23 10:31
核心算法1，欧拉角转四元数
void Quaternion::FromEulerAngle(const EulerAngle &ea)
{
        float fCosHRoll = cos(ea.fRoll * .5f);
        float fSinHRoll = sin(ea.fRoll * .5f);
        float fCosHPitch = cos(ea.fPitch * .5f);
        float fSinHPitch = sin(ea.fPitch * .5f);
        float fCosHYaw = cos(ea.fYaw * .5f);
        float fSinHYaw = sin(ea.fYaw * .5f);

        w = fCosHRoll * fCosHPitch * fCosHYaw + fSinHRoll * fSinHPitch * fSinHYaw;
        x = fCosHRoll * fSinHPitch * fCosHYaw + fSinHRoll * fCosHPitch * fSinHYaw;
        y = fCosHRoll * fCosHPitch * fSinHYaw - fSinHRoll * fSinHPitch * fCosHYaw;
        z = fSinHRoll * fCosHPitch * fCosHYaw - fCosHRoll * fSinHPitch * fSinHYaw;
}
作者: dongfang    时间: 2011-6-23 10:32
核心算法2，四元数转欧拉角

EulerAngle Quaternion::ToEulerAngle() const
{
        EulerAngle ea;

        ea.fRoll  = atan2(2 * (w * z + x * y) , 1 - 2 * (z * z + x * x));
        ea.fPitch = asin(CLAMP(2 * (w * x - y * z) , -1.0f , 1.0f));
        ea.fYaw   = atan2(2 * (w * y + z * x) , 1 - 2 * (x * x + y * y));

        return ea;
}
作者: dongfang    时间: 2011-6-23 10:32
核心算法3，四元数乘法
Quaternion Quaternion::Multiply(const Quaternion &b)
{
        Quaternion c;
        c.w=w*b.w        -x*b.x        -y*b.y        -z*b.z;
        c.x=w*b.x        +x*b.w        +y*b.z        -z*b.y;
        c.y=w*b.y        -x*b.z        +y*b.w        +z*b.x;
        c.z=w*b.z        +x*b.y        -y*b.x        +z*b.w;
        c.Normalize();
        return c;
}
作者: dongfang    时间: 2011-6-23 10:33
次要的规范化算法：
void  Quaternion::Normalize(){
        float s=getS();
        w/=s;
        x/=s;
        y/=s;
        z/=s;
}
float Quaternion::getS(){
        return sqrt(w*w+x*x+y*y+z*z);
}
作者: dongfang    时间: 2011-6-23 10:36
我的loop函数，算法的集成部分：

Quaternion nowQ;
void loop() {
  int intx, inty,intz;
  float  pitch,roll,yaw;

  gyro.ReadGyroOutCalibrated_Radian(&pitch, &roll, &yaw);

  EulerAngle dt;
  dt.fRoll=roll;
  dt.fPitch=pitch;
  dt.fYaw=-yaw;

  Quaternion dQ;
  dQ.FromEulerAngle(dt);
  nowQ=nowQ.Multiply(dQ);


  count++;
  if (count>1000){
    EulerAngle nowG=nowQ.ToEulerAngle();

    Serial.print(nowG.fRoll/3.1415926535*180,11);//横滚
    Serial.print(",");
    Serial.print(nowG.fPitch/3.1415926535*180,11);//俯仰
    Serial.print(",");
    Serial.print(nowG.fYaw/3.1415926535*180,11);//偏航
    Serial.print(",");
    Serial.print(nowQ.getS(),11);//偏航
    Serial.println();
    count=0;
  }
}
作者: dongfang    时间: 2011-6-23 10:42
我的excel版本的学习笔记 (, 下载次数: 1458)
作者: dongfang    时间: 2011-6-23 10:45
论坛不让上传rar文件，库函数的源码就不传了。看得懂上面代码的人，100%也能自己做了。

算法的有效性，可以参考excel文件。
作者: votreami    时间: 2011-6-23 10:49
一点也不懂
作者: MAGIC-JW    时间: 2011-6-23 11:21
:em26: 用得上真是宝  

高数线性偶都丢完了 泪奔
作者: dongfang    时间: 2011-6-23 11:33
原帖由 MAGIC-JW 于 2011-6-23 11:21 发表
:em26: 用得上真是宝  

高数线性偶都丢完了 泪奔

me too....实际上，我后来才去网上翻了一下线性代数。这里面的公式推导，我们不用关心，那是理科、数学家的事情。
我们做工科的，找到能用的就行了。
作者: datiao    时间: 2011-6-23 12:11
学习！
作者: dstdx72    时间: 2011-6-23 12:43
:em26: :em24: 热烈欢迎，狂顶....
http://www.cppblog.com/heath/archive/2009/12/13/103127.html
这个我也看过，哈哈，相当不错
四元数法前几天就试过了，其精度跟积分频率和陀螺输出准确率关系很大
比如陀螺的角速率输出误差（含测量误差）较大，加上积分误差，那么当转动90度时可能算出来才81度
理论上讲，积分频率越高，积分误差越小
所以估计得性能很好的mcu才能达到要求
没想到楼主的8位机也能达到这个精度？每秒姿态计算多少次？
大约10秒钟，各轴漂移在5度左右，你用的什么陀螺？性能这么好
我用的lpr550+lpy550，是用了过采样算法把AD分辨率提高了1000倍，才在静态下做到了每分钟小于5度的静态漂移的
作者: leanang    时间: 2011-6-23 13:24
谢谢 dongfang 的无私.
作者: oppop    时间: 2011-6-23 13:33
好东西
作者: hifiafrica    时间: 2011-6-23 14:46
标题: 纯粹的靠低价陀螺，你再好的算法也没用。
我所见过的自驾仪，都是非常依靠GPS的信号的，我最早玩的AP50，就是三个村田陀螺增稳阻尼，就能飞航线，而且还是三角翼。

不要试图去提高你姿态系统的性能，这个估计很难，要试图把GPS的信号引入你的姿态系统，让GPS的航向啊，位置点漂移啊，对你的姿态起到反馈作用。

没有什么飞机能只变化姿态，而不变化航迹的。GPS只要检测到航迹变化，就意味着你的姿态发生变化了。
作者: firedog2011    时间: 2011-6-23 15:51
搞FPV搞到这么复杂还有乐趣可言吗？唉
作者: 栋栋    时间: 2011-6-23 16:23
原帖由 firedog2011 于 2011-6-23 15:51 发表
搞FPV搞到这么复杂还有乐趣可言吗？唉

各有所好，对科学的钻研精神的确令人敬佩。应该是我们的好榜样对吧！
作者: aband    时间: 2011-6-23 20:36
楼主没有买《惯性导航》？你说的这些东西在这本书上都有 还有详细的推导
作者: fujianshu    时间: 2011-6-23 22:02
厉害啊
作者: autokey    时间: 2011-6-23 22:05
原帖由 firedog2011 于 2011-6-23 15:51 发表
搞FPV搞到这么复杂还有乐趣可言吗？唉

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

[ 本帖最后由 autokey 于 2011-6-23 22:07 编辑 ]
作者: F3C88    时间: 2011-6-23 22:47
编程高手啊:em26:
作者: dongfang    时间: 2011-6-23 23:11
原帖由 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    时间: 2011-6-23 23:29
原帖由 hifiafrica 于 2011-6-23 14:46 发表
我所见过的自驾仪，都是非常依靠GPS的信号的，我最早玩的AP50，就是三个村田陀螺增稳阻尼，就能飞航线，而且还是三角翼。

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

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

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

现成的飞控尽管贵，但相对我的时间投入，也算是便宜了。不过买现成的飞控，不能学到什么，还总是担心某天提控回家损失个上千元的。
现在我做的这点东西，硬件成本200元上下，就算掉了，再做一个也就这点钱。软件在自己手里，踏实。
作者: dongfang    时间: 2011-6-23 23:48
原帖由 aband 于 2011-6-23 20:36 发表
楼主没有买《惯性导航》？你说的这些东西在这本书上都有 还有详细的推导

惭愧，我没买。觉得网络是更好的书库。
向你求教一下，《惯性导航》有讲加速度如何融合进角速度计算出的姿态的吗？
作者: aband    时间: 2011-6-24 11:31
原帖由 dongfang 于 2011-6-23 23:48 发表

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


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

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

我一般碰到好书就立即去买实物收藏，也算是对作者的支持。
作者: dongfang    时间: 2011-6-24 22:42
原帖由 aband 于 2011-6-24 11:31 发表


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

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

可惜这些东西就只有我们个人走实践路线，学习学习MK的思路，等等小打小闹的方法，实际上是浪费大家的时间。
作者: md35    时间: 2011-6-25 03:43
长航时的dao dan也是这样的的算法，精度再高的陀螺，我看也难坚持几小时。
作者: xesable    时间: 2011-6-26 21:24
:em26: 强人啊！
作者: xesable    时间: 2011-6-26 21:30
标题: 回复 26楼 dongfang 的帖子
有时间去看看《捷联惯性导航》三轴陀螺三轴加速计经行微分之后积分后得到姿态数据。（亚马逊上就有的买）
楼主加油！能做出可靠的姿态测量装置很有用的。现在几乎所有的无人机都是建立在这个基础之上的。
作者: dstdx72    时间: 2011-6-27 19:46
:loveliness: 兄弟搞得咋样了？
作者: dongfang    时间: 2011-6-28 10:23
原帖由 dstdx72 于 2011-6-27 19:46 发表
:loveliness: 兄弟搞得咋样了？


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

姿态计算和PID控制,一个不能少:em05:
作者: Aceking1123    时间: 2011-6-29 15:36
四参数法
经典力学中古老的问题之一是刚体运动和坐标变换。在平台式惯性导航系统计算机软件中通常采用欧拉角及其方向余弦矩阵。由理论力学的知识可知，绕定点转动的刚体的角位置可以通过依次转过三个欧拉角的三次转动而获得，也可以通过绕某一瞬时轴转过某个角度的一次转动而获得。对于前者可以采用方向余弦法解决定点转动的刚体定位问题，对于后者可以采用四参数法来解决定位问题。
四参数法也就是使用最广泛的四元数法。四元数理论是数学中的一古老分支，是由哈密而顿于1943年首先提出的，其思想类似平面问题使用复数解的方式。但是该理论建立之后长期没有得到实际应用，随着空间技术、计算技术、特别是捷联式惯性导航技术的发展，四元数的优越性日渐引起人们的重视。其先求解姿态四元数微分方程，再由姿态四元数确定航向角和姿态角。虽然需要四个微分方程，较欧拉角微分方程多一个方程，但进行数值计算求解时只需要进行加减乘除运算，所以求解过程的计算量要比欧拉角法减少得多。
它的优势体现在，与方向余弦法比较，计算量小，存储容量少，仅需要进行简单的四元数规范化处理便可以保证姿态矩阵的正交性，因而成为一种普遍采用的方法。但是不可避免地引入了有限转动的不可交换性误差，特别是当运载体姿态变化比较剧烈，或伴有角振动时，该法会产生严重的姿态漂移误差，所以只能用于工作环境平缓和缓慢的运载体[2]。


文件名: 捷联导航系统 p15起.doc
描述: 捷联导航系统 p15起.doc
下 载地址: http://www.rayfile.com/files/81a4ca85-a222-11e0-804d-0015c55db73d/
作者: Aceking1123    时间: 2011-6-29 15:37
上面这个文档写得很好，可以学习参考下

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

[ 本帖最后由 Aceking1123 于 2011-6-29 15:56 编辑 ]
作者: dongfang    时间: 2011-6-29 18:01
陀螺仪的白噪音是存在的，还不小。这里是个两难的选择，要么就不上四元数，只假设模型在平面上飞行,这样的话，只要做3轴积分就行了，白噪音大部分会被积分抵消掉。我的陀螺仪在这种计算模型下，每分钟的漂移在5度以下。但对简单的固定翼飞机盘旋动作，这种算法会产生持续严重的累计偏差。

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

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

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

我看了一点点MK的代码，没有用四元数，不过他针对旋转后的姿态判断，是先清零的。我认为这可能会造成MK的局限性，不能连续做复杂动作（尽管没见过MK）。
作者: dstdx72    时间: 2011-6-29 18:40
原帖由 Aceking1123 于 2011-6-29 15:36 发表
...但是不可避免地引入了有限转动的不可交换性误差，特别是当运载体姿态变化比较剧烈，或伴有角振动时，该法会产生严重的姿态漂移误差，所以只能用于工作环境平缓和缓慢的运载体[2]。 ...


我怪说不得呢，我也发现4元数法有这个特点
手拿板子小范围的往复运动，运动多了误差就出来了，原来是这样啊
不过4元数的无理论误差的算法，还是值得称道的
尤其是连续滚转后的误差不大
确点是三角函数乘除次数实在太多了
作者: dstdx72    时间: 2011-6-29 18:43
原帖由 dongfang 于 2011-6-29 18:01 发表
陀螺仪的白噪音是存在的，还不小。这里是个两难的选择，要么就不上四元数，只假设模型在平面上飞行,这样的话，只要做3轴积分就行了，白噪音大部分会被积分抵消掉。我的陀螺仪在这种计算模型下，每分钟的漂移在5度以下 ...


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

[ 本帖最后由 dstdx72 于 2011-6-29 19:01 编辑 ]
作者: dongfang    时间: 2011-6-29 20:46
你说的乱蹦，我估计是电磁干扰引起的，建议做些简单的滤波。

我那个算法在我的板子上倒是很乖，静态时漂移和原来的积分算法区别不大。回头看看你的算法表现如何。
作者: dongfang    时间: 2011-10-16 22:34
正好有人PM问我，CLAMP是什么，实际上就是一个限定范围的macro了，定义如下：

#define CLAMP(x , min , max) ((x) > (max) ? (max) : ((x) < (min) ? (min) : x))
作者: slbbs    时间: 2011-10-16 23:58
这个算法可以应用到炒股上

动态调整4种投资组合,完成给定固定收益率曲线(航线)条件的方法
作者: 云淡风轻的云    时间: 2011-10-17 00:15
学习了啊啊啊啊
作者: heuyck    时间: 2011-10-17 00:24
楼主诈尸了自挖自坟啊，仔细看了看，发现和我用的不太一样。
我是用一阶龙库算法更新四元数的，感觉楼主这个过程有点象旋转矢量法，不过我没仔细看过旋转矢量法，不知道是不是。
也象我最近想用来解决姿态控制的的误差四元数。。。
作者: gale    时间: 2011-10-17 19:18
:em15:
作者: dongfang    时间: 2011-10-17 21:16
已经好久没有更新姿态算法了，算是跳过了跳过姿态，去研究控制和航线了。

前一阵子在玩X-PLANE时，发现了PID算法中，I的实际意义: 参数I乘以误差积分值，等于系统稳定时，需要的控制量。比如通过控制油门来控制飞行速度的PID模型：
1. 误差=预期速度-实际速度。
2. 误差积分=误差的积分（自算法一开始算起，到目前）
3.误差积分*参数I，在理想的情况下，接近于达到该飞行速度的理想油门量。比如用50%的油门，可以得到稳定的30km/h速度，那如果目标速度是30km/h,误差积分*参数I=50%

参数I是一个非常难调节的部分，调小了，就和没有I控制差不多，调大了，控制会剧烈抖动。得到了I的含义的后，处理方法就简单了，预先算出一条控制量和控制效果的曲线，比如舵机控制量和飞行速度曲线，代入PD算法，然后就可以把I扔了。
作者: skypup    时间: 2011-10-18 09:48
中毒了，受不了了，冲动呀。

马上去买个ATOM或者二手小本本，搞飞控。:em24:

感谢楼主分享快乐。
作者: nter    时间: 2011-10-18 12:18
算法必须定点化，否则AVR可跑不动
作者: wanghua_888    时间: 2011-10-26 15:13
标题: 四元数微分方程求解
四元数微分方程有四个，怎么求解啊？
作者: wanghua_888    时间: 2011-10-26 15:15
你的捷联算法很特别，与解微分方程相比，哪一个精度高？
作者: wanghua_888    时间: 2011-10-26 15:30
各位朋友，我是新手请多多指教，用龙格库塔法解四元数的微分方程怎么解啊?
作者: wanghua_888    时间: 2011-10-26 17:14
dstdx72先生，把你的四元数微分方程贴出来，让大家见识见识吧。
作者: dongfang    时间: 2011-10-26 17:22
四阶龙格-库塔法，如http://wenku.baidu.com/view/9721 ... 9.html?from=related

我看过，但没看懂，问题在于它里面用了矢量相乘还是矩阵相乘，所以公式上虽然简单，但转换为实际算法，我不行。

但我现在找到的4元数算法，90%的可能就是四阶龙格-库塔法的实现。至于是不是，要请使用四阶龙格-库塔法的其他TX晒一下代码，比较一下才能知道了。
作者: scxjun    时间: 2012-1-16 14:02
dongfang
你的程序中没用加速度去修正陀螺仪误差，即使用四元数，可能运行一会下来误差比较大吧
作者: yhd2006    时间: 2012-1-16 15:16
:em26:   对于技术人才必须干的事情是 顶顶顶
作者: 孤傲    时间: 2012-1-16 17:57
学习了
作者: 峰回路转    时间: 2012-1-28 02:36
最近有时间也玩一下姿态解算。楼主看下我的理解正确么
Quaternion nowQ;                    //获得原始姿态的四元数  [a]
void loop() {                       //进入循环
  int intx, inty,intz;
  float  pitch,roll,yaw;

  gyro.ReadGyroOutCalibrated_Radian(&pitch, &roll, &yaw);   //读取陀螺输出。并且校准（弧度）

  EulerAngle dt;                                     //获得dt。就是采样刷新time
  dt.fRoll=roll;                                     //计算roll的变化量 （gyrox*dt）
  dt.fPitch=pitch;                                     //计算pitch的变化量（gyroy*dt）
  dt.fYaw=-yaw;                                        //计算yaw的变化量（有反向，和安装方向有关）（gyroz*dt）

  Quaternion dQ;                                        //计算当前姿态的四元数
  dQ.FromEulerAngle(dt);                                //把dt的欧拉转换为四元数   
  nowQ=nowQ.Multiply(dQ);                                //做四元数的乘法运算，得到新的四元数（nowQ=nowQ*dQ(是dt转换得到)）[a=a*b]


  count++;                                              //计数器++
  if (count>1000){                                       //如果count大于1000则将nowQ四元转换为欧拉角
    EulerAngle nowG=nowQ.ToEulerAngle();

    Serial.print(nowG.fRoll/3.1415926535*180,11);//横滚
    Serial.print(",");                                            //将弧度转换为度，串口输出
    Serial.print(nowG.fPitch/3.1415926535*180,11);//俯仰
    Serial.print(",");
    Serial.print(nowG.fYaw/3.1415926535*180,11);//偏航
    Serial.print(",");
    Serial.print(nowQ.getS(),11);//偏航
    Serial.println();
    count=0;
  }
}
有点迷惑的是这里：
Quaternion dQ;                                        //计算当前姿态的四元数
  dQ.FromEulerAngle(dt);                                //把dt的欧拉转换为四元数   
既然后面做四元乘法用的是a=a*b。那Quaternion dQ;       //计算当前姿态的四元数
有什么用呢。
后面的计算就是将上一次的nowq作为原始姿态和更新的dt做乘法。这样就用三轴陀螺计算出姿态，如果要做acc的融合的话就另说了。
找到了一个算法：
//=====================================================================================================
// IMU.c
// S.O.H. Madgwick
// 25th September 2010
//=====================================================================================================
// Description:
//
// Quaternion implementation of the 'DCM filter' [Mayhony et al].
//
// User must define 'halfT' as the (sample period / 2), and the filter gains 'Kp' and 'Ki'.
//
// Global variables 'q0', 'q1', 'q2', 'q3' are the quaternion elements representing the estimated
// orientation.  See my report for an overview of the use of quaternions in this application.
//
// User must call 'IMUupdate()' every sample period and parse calibrated gyroscope ('gx', 'gy', 'gz')
// and accelerometer ('ax', 'ay', 'ay') data.  Gyroscope units are radians/second, accelerometer
// units are irrelevant as the vector is normalised.
//
//=====================================================================================================

//----------------------------------------------------------------------------------------------------
// Header files

#include "IMU.h"
#include <math.h>

//----------------------------------------------------------------------------------------------------
// Definitions

#define Kp 2.0f                        // proportional gain governs rate of convergence to accelerometer/magnetometer
#define Ki 0.005f                // integral gain governs rate of convergence of gyroscope biases
#define halfT 0.5f                // half the sample period

//---------------------------------------------------------------------------------------------------
// Variable definitions

float q0 = 1, q1 = 0, q2 = 0, q3 = 0;        // quaternion elements representing the estimated orientation
float exInt = 0, eyInt = 0, ezInt = 0;        // scaled integral error

//====================================================================================================
// Function
//====================================================================================================

void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az) {
        float norm;
        float vx, vy, vz;
        float ex, ey, ez;         
       
        // normalise the measurements
        norm = sqrt(ax*ax + ay*ay + az*az);      
        ax = ax / norm;
        ay = ay / norm;
        az = az / norm;      
       
        // estimated direction of gravity
        vx = 2*(q1*q3 - q0*q2);
        vy = 2*(q0*q1 + q2*q3);
        vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;
       
        // error is sum of cross product between reference direction of field and direction measured by sensor
        ex = (ay*vz - az*vy);
        ey = (az*vx - ax*vz);
        ez = (ax*vy - ay*vx);
       
        // integral error scaled integral gain
        exInt = exInt + ex*Ki;
        eyInt = eyInt + ey*Ki;
        ezInt = ezInt + ez*Ki;
       
        // adjusted gyroscope measurements
        gx = gx + Kp*ex + exInt;
        gy = gy + Kp*ey + eyInt;
        gz = gz + Kp*ez + ezInt;
       
        // 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;  
       
        // normalise quaternion
        norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
        q0 = q0 / norm;
        q1 = q1 / norm;
        q2 = q2 / norm;
        q3 = q3 / norm;
}

//====================================================================================================
// END OF CODE
//====================================================================================================
这个四元貌似有acc的融合功能？？？请指教，谢谢

[ 本帖最后由 峰回路转 于 2012-1-28 02:38 编辑 ]
作者: oppop    时间: 2012-1-28 07:38
楼主没有考虑线加速度对角速度的影响吧？
作者: dongfang    时间: 2012-1-28 10:51
原帖由 峰回路转 于 2012-1-28 02:36 发表
最近有时间也玩一下姿态解算。楼主看下我的理解正确么
Quaternion nowQ;                    //获得原始姿态的四元数  [a]
void loop() {                       //进入循环
  int intx, inty,intz;
  float  pi ...


你好，Quaternion dQ，也就是a=a*b中的那个b，是自上一个姿态计算后，发生旋转(增量delta)的角度，可以从角速度读数乘以增量时间得到dt（这里的dt不是时间，是增量角度），再转换成四元数得到。

你的那段带ACC融合的代码看上去不错，但肯定不是标准的四元数算法，因为没看到三角函数，有机会我试一试。
作者: dongfang    时间: 2012-1-28 10:52
原帖由 oppop 于 2012-1-28 07:38 发表
楼主没有考虑线加速度对角速度的影响吧？

不是很明白线加速度对角速度的影响指的是什么，我的代码没有集成acc是事实。
作者: 孤傲    时间: 2012-1-28 11:51
:em15:
作者: dongfang    时间: 2012-1-28 12:37
找到了58楼代码的出处了，代码还在更新中，现在已经支持9dof了，

作者的主页在：
http://code.google.com/p/imumargalgorithm30042010sohm/
http://www.x-io.co.uk/node/8#open_source_imu_and_ahrs_algorithms

c代码在此：http://www.x-io.co.uk/res/sw/madgwick_algorithm_c.zip

姿态解算这么高深的问题，有看得懂的代码，感觉开源真是太了不起了。
作者: 峰回路转    时间: 2012-1-28 19:23
原帖由 dongfang 于 2012-1-28 10:51 发表


你好，Quaternion dQ，也就是a=a*b中的那个b，是自上一个姿态计算后，发生旋转(增量delta)的角度，可以从角速度读数乘以增量时间得到dt（这里的dt不是时间，是增量角度），再转换成四元数得到。

你的那段带AC ...

呵呵。那我就明白了。关于四元的公式用这个就可以了吧：void Quaternion::FromEulerAngle(const EulerAngle &ea)
{
        float fCosHRoll = cos(ea.fRoll * .5f);
        float fSinHRoll = sin(ea.fRoll * .5f);
        float fCosHPitch = cos(ea.fPitch * .5f);
        float fSinHPitch = sin(ea.fPitch * .5f);
        float fCosHYaw = cos(ea.fYaw * .5f);
        float fSinHYaw = sin(ea.fYaw * .5f);

        w = fCosHRoll * fCosHPitch * fCosHYaw + fSinHRoll * fSinHPitch * fSinHYaw;
        x = fCosHRoll * fSinHPitch * fCosHYaw + fSinHRoll * fCosHPitch * fSinHYaw;
        y = fCosHRoll * fCosHPitch * fSinHYaw - fSinHRoll * fSinHPitch * fCosHYaw;
        z = fSinHRoll * fCosHPitch * fCosHYaw - fCosHRoll * fSinHPitch * fSinHYaw;
}
这个函数输入gyrox y z输出wxyz也就是q0 q1 q2 q3 对四元的概念还是有些模糊，看来得好好看看角度，弧度，欧拉，四元之间的关系。
作者: 峰回路转    时间: 2012-1-28 19:25
原帖由 dongfang 于 2012-1-28 12:37 发表
找到了58楼代码的出处了，代码还在更新中，现在已经支持9dof了，

作者的主页在：
http://code.google.com/p/imumargalgorithm30042010sohm/
http://www.x-io.co.uk/node/8#open_source_imu_and_ahrs_algorithms ...

这个代码楼主有没有测试过，他融合了三轴加速度和三轴磁场，期待详解。。。。
作者: dongfang    时间: 2012-1-28 22:34
奇怪，我刚看到了h.e.u.y.c.k的回复，刚想说两句，突然不见了，是被我自己删了么？
作者: tedeum    时间: 2012-1-28 23:11
顶起 楼主是偶像
作者: xaut    时间: 2012-1-29 09:13
做一个46656000行的查表法如何？:em15:
作者: heuyck    时间: 2012-1-29 09:22
标题: 标题
原帖由 dongfang 于 2012-1-28 22:34 发表
奇怪，我刚看到了h.e.u.y.c.k的回复，刚想说两句，突然不见了，是被我自己删了么？

不想发帖了，免得有人jjww。最近讲啥都有人跟。
你看过回帖了解就好了
作者: oppop    时间: 2012-1-29 18:45
原帖由 dongfang 于 2012-1-28 10:52 发表

不是很明白线加速度对角速度的影响指的是什么，我的代码没有集成acc是事实。


由于MEMS陀螺的固有特性，每个坐标上的角速度输出值会受到线加速度的影响，越便宜的陀螺仪这种影响越明显，即，Linear Acceleration Effect on Bias。
换句话说，低端的陀螺仪对陀螺本身的振动非常敏感。高端的陀螺仪贵的原因一方面是固有噪声低和零漂稳定性低，另一方面则是对加速度的敏感度低。因为高端的陀螺仪每个轴向上都会做两对相同的结构来构成差分的形式，来消除振动带来的影响。这样做的话，成本自然就高了。
一般的、起步比较晚的MEMS陀螺仪生产厂商，一般是不会提Linear Acceleration Effect on Bias这个指标的，因为他们这个指标都很差。但是在实际运用中，振动是不可避免的，振动带来的误差往往占很大比例。这就是为什么有些飞控的内部的IMU要加减振垫子的原因。
作者: dongfang    时间: 2012-1-29 20:58
原帖由 oppop 于 2012-1-29 18:45 发表


由于MEMS陀螺的固有特性，每个坐标上的角速度输出值会受到线加速度的影响，越便宜的陀螺仪这种影响越明显，即，Linear Acceleration Effect on Bias。
换句话说，低端的陀螺仪对陀螺本身的振动非常敏感。高端的 ...

首次听说，不知道实际影响有多大，好像感觉不出来么。
作者: dongfang    时间: 2012-1-29 21:34
原帖由 (涉嫌广告已屏蔽) 于 2012-1-29 09:22 发表

不想发帖了，免得有人jjww。最近讲啥都有人跟。
你看过回帖了解就好了

唉，人怕出名猪怕胖，就讨论些技术算了，别扯那些是非了。算法上荒废很久了，已经遇到了AVR的运算极限，STM32也没搞定（本身就是新学，IIC没搞定就已经损失了一个测试板）。

最近各种分心的事很多，加上在研究四轴，感觉精力不济。
你要是有时间的话，我建议你的飞控搞一个飞行测试模式，就是依次测试：
测试SETUP: 测试在100米左右安全微风空域进行
TEST 1: 测试油门到达指定速度SPEED的理想PID参数，参数能和SPEED成函数关系最好。我在xplan的测试中已经发现I*I积分是和SPEED直接关系的。
TEST 2: 测试各SPEED下保持平衡飞行的ROLL的理想PID参数。
TEST 3: 测试各SPEED下保持高度、平衡飞行的PITCH的理想PID参数。
TEST 4: 测试各SPEED下保持高度、平衡飞行，仅使用方向舵转弯，各个转弯力度飞行的方向舵的理想PID参数。
TEST 5: 测试各SPEED下保持高度、平衡飞行，组合使用方向舵和副翼转弯，各个转弯力度飞行的方向舵、副翼、升降舵的理想PID参数。
测试错误控制：一旦飞机失控，高度降低20米以上，立刻使用FAILSAFE参数，救回。

我在xplan上完成过一部分测试，也发现了一些有趣的规律，只是程序写的很烂，每次测试都是直接修改了前一次的测试代码，所以没留下来什么能共享的。
作者: 峰回路转    时间: 2012-1-29 22:56
呵呵。各位都是高手，最近假期较长，想好好研究下，平台用的是fy20的平衡仪，上面有stm32f103brt+adxl335+ly530+lpr530a。需要的东西都全了。。把原来的mcu拆了保存好。万一以后想玩平衡仪了还可以装回去，装上一片新的。加装jtag（swd）接口。这个本身串口是引出的。加装hp03气压传感器。stm32没有eeprom，装了一片24c02.试过吧kk_c移植到这个平台上，测试可以工作。但是加速度计等都没用上，这次乘着长假玩玩。。。

[ 本帖最后由 峰回路转 于 2012-1-29 23:03 编辑 ]
作者: 编程飞行    时间: 2012-1-29 23:47
不懂:em22: 帮顶
作者: 峰回路转    时间: 2012-1-30 02:35
关于欧拉角，gyro输出值积分得到的就是欧拉角么？另外计算时输入的gyro的单位是mv/度/s ? acc的是mv/g
也就是说用gyro积分计算角度偏移量 gyrox_dt=gyrox*dt
用acc计算当前欧拉角 Roll= atan2(ax,az);
                Pitch= atan2(-ay,az);

[ 本帖最后由 峰回路转 于 2012-1-30 03:45 编辑 ]
作者: dongfang    时间: 2012-1-30 22:41
原帖由 峰回路转 于 2012-1-30 02:35 发表
关于欧拉角，gyro输出值积分得到的就是欧拉角么？另外计算时输入的gyro的单位是mv/度/s ? acc的是mv/g
也就是说用gyro积分计算角度偏移量 gyrox_dt=gyrox*dt
用acc计算当前欧拉角 Roll= atan2(ax,az);
                Pitch=  ...

gyro输出值*间隔时间=欧拉角。gyro的单位要查芯片手册，数字芯片不用mv。
作者: heuyck    时间: 2012-1-30 23:04
原帖由 dongfang 于 2012-1-29 21:34 发表

唉，人怕出名猪怕胖，就讨论些技术算了，别扯那些是非了。算法上荒废很久了，已经遇到了AVR的运算极限，STM32也没搞定（本身就是新学，IIC没搞定就已经损失了一个测试板）。

最近各种分心的事很多，加上在研究四 ...

最近研究XPLANE、APM SIM还有MAVLINK，得把空速管什么的数据都弄出来才好仿真。
xplane可是个好东西啊，可惜只有一架PT60。。。找不到其他模型了，不知道你用什么模型模拟的？
APM功能很强大，正在研究，不过是C++写的，看得我一头雾水。最近又有新版APM2出来，好像只卖200刀，也可以研究一下。
MAVLINK也是个好东西，支持MAVLINK的GCS一大堆，而且有免费库可以用，还是C代码的。
作者: heuyck    时间: 2012-1-30 23:38
原帖由 峰回路转 于 2012-1-30 02:35 发表
关于欧拉角，gyro输出值积分得到的就是欧拉角么？另外计算时输入的gyro的单位是mv/度/s ? acc的是mv/g
也就是说用gyro积分计算角度偏移量 gyrox_dt=gyrox*dt
用acc计算当前欧拉角 Roll= atan2(ax,az);
                Pitch=  ...

那个积分不是平时意义上的机体欧拉角，那个是在dt微分时间内的转动欧拉角，其实也不能算是欧拉角，因为欧拉角有严格的转动顺序。只是这里dt微分下，转动顺序的区别很小了。
东方拿它来转成增量四元数，然后用四元数转动的方式去更新原有四元数，得到了新的四元数。

在你贴的那个算法里，gxyz就是就是输入的三轴陀螺的角速率，带四元数q0123更新方程就是一阶龙库算法，增量法一样的。
一阶就是指考虑dt时间内，gxyz的角速率是一直不变的，二三四阶就是再细分这dt时间内的三维转动，我用一阶觉得已经够用了。
那个算法里的halfT就是dt/2，单位是秒，比如我的计算频率为50hz，dt即为20ms，那么这个算法里的halfT就是0.01秒。
gxyz的单位应该是弧度/秒，陀螺的mv转成弧度/秒的比例，可在芯片手册中查得。
这个算法中用acc校准gyro用的是向量叉积的办法，我也是用叉积的。
计算机体坐标系下，四元数的重力和加速度计的重力的向量叉积，算出来的向量正好是机体坐标系中三维xyz的夹角的cos值，也就是叉积。
在误差90度以内，机体坐标系下xyz误差夹角和叉积成正比例关系，正好用来加到陀螺上做PI误差校准。
加速度计的重力向量要砍掉向心加速度，另外与标准重力做个比较，超过一定范围不对四元数做校准。

另外acc按你的算法应该是平面夹角，与机体欧拉角不同，我是这么算欧拉角，用来初始化四元数的，xy可能跟你不一样。
imu.euler.x = atan2(imu.accel.y, imu.accel.z);
imu.euler.y = -asin(imu.accel.x / ACCEL_1G);
作者: heuyck    时间: 2012-1-30 23:46
acc计算欧拉角，四元数重力向量，这几个结合余弦矩阵（转置矩阵）和欧拉角的三次转动方程推算过程，会比较清楚。
作者: oppop    时间: 2012-1-31 00:27
原帖由 dongfang 于 2012-1-29 20:58 发表

首次听说，不知道实际影响有多大，好像感觉不出来么。


这个是振动所产生的加速度的计算公式。振动起来，所产生的加速度是很恐怖的，自己可以算算。
1khz、1um的振动可以产生39.5g的加速度。然后拿这个加速度去乘以Linear Acceleration Effect on Bias可以得到由于振动所产生额外的角速度偏移量，越是低端的陀螺仪，这个值越大。
Acceleration = 4 * p&sup2; * ƒ&sup2; * displacement
作者: surebo    时间: 2012-1-31 10:14
强贴留名，收藏起来
作者: 峰回路转    时间: 2012-1-31 17:42
原帖由 (涉嫌广告已屏蔽) 于 2012-1-30 23:38 发表

那个积分不是平时意义上的机体欧拉角，那个是在dt微分时间内的转动欧拉角，其实也不能算是欧拉角，因为欧拉角有严格的转动顺序。只是这里dt微分下，转动顺序的区别很小了。
东方拿它来转成增量四元数，然后用四元 ...

受教了。。谢谢。
我搞这个完全处于兴趣使然，无奈上学太少很多东西搞不太明白，只好多多请教学习。就当是万里长征了，呵呵
从从开始，首先获得初始状态的四元，先从acc获得初始的欧拉角，测试了一下用Roll= atan2(ax,az); Pitch= atan2(-ay,az);与您的公式imu.euler.x = atan2(imu.accel.y, imu.accel.z); imu.euler.y = -asin(imu.accel.x / ACCEL_1G);结果是相同的，
当x轴旋转90度时，acc_x=1g acc_y=0g acc_z=0g 此时用上面的公式计算出的欧拉角为1.57，这个值是什么单位或者是什么意义呢？

[ 本帖最后由 峰回路转 于 2012-1-31 17:59 编辑 ]
作者: jmp2002    时间: 2012-1-31 21:25
原帖由 峰回路转 于 2012-1-31 17:42 发表

受教了。。谢谢。
我搞这个完全处于兴趣使然，无奈上学太少很多东西搞不太明白，只好多多请教学习。就当是万里长征了，呵呵
从从开始，首先获得初始状态的四元，先从acc获得初始的欧拉角，测试了一下用Roll= ata ...

单位是弧度，你换算成角度就行了：1.57*180/3.14159=90(度)
作者: luopeixuan    时间: 2012-1-31 21:38
原帖由 votreami 于 2011-6-23 10:49 发表
一点也不懂

Me Too!
作者: dongfang    时间: 2012-1-31 21:39
原帖由 (涉嫌广告已屏蔽) 于 2012-1-30 23:04 发表

最近研究XPLANE、APM SIM还有MAVLINK，得把空速管什么的数据都弄出来才好仿真。
xplane可是个好东西啊，可惜只有一架PT60。。。找不到其他模型了，不知道你用什么模型模拟的？
APM功能很强大，正在研究，不过是C ...

我也使用PT60模拟的。APM成品度太高了，看了就感觉不是个人业余工作能达到的。
等会儿我晒几张测试报告吧，我觉得只要xplan是可靠的，它模拟出来的数据就有参考性，即使每架飞机都是不一样的。
作者: 峰回路转    时间: 2012-1-31 22:11
原帖由 jmp2002 于 2012-1-31 21:25 发表

单位是弧度，你换算成角度就行了：1.57*180/3.14159=90(度)

呵呵。的确如此。谢谢
作者: dongfang    时间: 2012-1-31 23:44
XPLAN测试报告：http://bbs.5imx.com/bbs/viewthread.php?tid=573424
作者: 峰回路转    时间: 2012-2-1 02:47
刚才测试了我移植到stm32下的四元，欧拉角互转代码，我理解为，首先将三个已知的欧拉角（例如：roll=2，pitch=3，raw=4）转换为四元数，得到一个四元数（w,x,y,z），然后不做任何处理。直接在转换会欧拉角，那么得到的欧拉角仍然应该为roll=2，pitch=3，raw=4。
按照这个思路测试，结果却不是这么回事？是我理解的不对么？
另外：void Quaternion::FromEulerAngle(const EulerAngle &ea)
{
        float fCosHRoll = cos(ea.fRoll * .5f);
        float fSinHRoll = sin(ea.fRoll * .5f);
        float fCosHPitch = cos(ea.fPitch * .5f);
        float fSinHPitch = sin(ea.fPitch * .5f);
        float fCosHYaw = cos(ea.fYaw * .5f);
        float fSinHYaw = sin(ea.fYaw * .5f);

        w = fCosHRoll * fCosHPitch * fCosHYaw + fSinHRoll * fSinHPitch * fSinHYaw;
        x = fCosHRoll * fSinHPitch * fCosHYaw + fSinHRoll * fCosHPitch * fSinHYaw;
        y = fCosHRoll * fCosHPitch * fSinHYaw - fSinHRoll * fSinHPitch * fCosHYaw;
        z = fSinHRoll * fCosHPitch * fCosHYaw - fCosHRoll * fSinHPitch * fSinHYaw;
}

与EulerAngle Quaternion::ToEulerAngle() const
{
        EulerAngle ea;

        ea.fRoll  = atan2(2 * (w * z + x * y) , 1 - 2 * (z * z + x * x));
        ea.fPitch = asin(CLAMP(2 * (w * x - y * z) , -1.0f , 1.0f));
        ea.fYaw   = atan2(2 * (w * y + z * x) , 1 - 2 * (x * x + y * y));

        return ea;
}
里面的：ea.fPitch = asin(CLAMP(2 * (w * x - y * z) , -1.0f , 1.0f));
与float fCosHRoll = cos(ea.fRoll * .5f);
        float fSinHRoll = sin(ea.fRoll * .5f);
        float fCosHPitch = cos(ea.fPitch * .5f);
        float fSinHPitch = sin(ea.fPitch * .5f);
        float fCosHYaw = cos(ea.fYaw * .5f);
        float fSinHYaw = sin(ea.fYaw * .5f);
里的 .5f 是0.5*采样间隔吗？如果是采样间个的话那我上面的现象就是正常了。。。

[ 本帖最后由 峰回路转 于 2012-2-1 02:48 编辑 ]
作者: 峰回路转    时间: 2012-2-1 02:50
原帖由 dongfang 于 2012-1-31 23:44 发表
XPLAN测试报告：http://bbs.5imx.com/bbs/viewthread.php?tid=573424

恩。建立理论基础的利器啊。。。。我有套apm，xplan下的模拟一直没有试过，主要是用了好几天也没下 载完xplan软件，在官网上下的那个测试版有时间限制，等有时间了好好向您学习

[ 本帖最后由 峰回路转 于 2012-2-1 02:51 编辑 ]
作者: heuyck    时间: 2012-2-1 09:02
原帖由 峰回路转 于 2012-2-1 02:47 发表
刚才测试了我移植到stm32下的四元，欧拉角互转代码，我理解为，首先将三个已知的欧拉角（例如：roll=2，pitch=3，raw=4）转换为四元数，得到一个四元数（w,x,y,z），然后不做任何处理。直接在转换会欧拉角，那么得到 ...

这里的0.5并非采样间隔，想想就知道了，如果是采样间隔，起码应该是0.5*T的形式。这里就是简单的欧拉角/2。
为什么要/2我不清楚，欧拉角和四元数互转推算过程可以去找惯导书，推算过程我看不懂也不想懂，反正最后计算公式就是欧拉角/2的。

公式里都是以弧度做单位，欧拉角有它自己的取值范围，roll和yaw是-π到π，pitch是-π/2到π/2。所以那个欧拉角为2 3 4的例子是无效的，转成四元数再转回欧拉角，欧拉角就回到有效范围里，当然和初始值不相等了。
建议你把输入的欧拉角弄成角度，然后在公式里再转成弧度计算，最后计算得到的欧拉角弧度再转回角度，这样角度值容易看一些。。。。

[ 本帖最后由 heuyck 于 2012-2-1 09:29 编辑 ]
作者: heuyck    时间: 2012-2-1 09:04
CbE = Cγ·Cθ·Cψ =
[cosψcosθ                                        sinψ cos θ                                        -sinθ]
[cosψsinθsinγ - sinψcosγ                sinψsinθsinγ + cosψcosγ                cosθsinγ ]
[cosψsinθcosγ + sinψsinγ                sinψsinθcosγ - cosψsinγ                cosθcosγ]
这是姿态矩阵，也就是余弦矩阵，也可以看成是欧拉角转余弦矩阵的方程，其中γ是横滚roll，θ是俯仰pitch，ψ是航向yaw。

        ea.fRoll  = atan2(2 * (w * z + x * y) , 1 - 2 * (z * z + x * x));
        ea.fPitch = asin(CLAMP(2 * (w * x - y * z) , -1.0f , 1.0f));
        ea.fYaw   = atan2(2 * (w * y + z * x) , 1 - 2 * (x * x + y * y));
这个里面的几个wxyz是把四元数转成了余弦矩阵的几个元素，然后用余弦矩阵里对应的关系用倒三角函数求出r p y。


欧拉角、四元数、余弦矩阵，这三种姿态的表达方式都可以互相转化，如果想了解多一点，还是去看书吧。比如邓正隆的《惯性技术》。

[ 本帖最后由 heuyck 于 2012-2-1 09:14 编辑 ]
作者: 峰回路转    时间: 2012-2-1 17:09
公式里都是以弧度做单位，欧拉角有它自己的取值范围，roll和yaw是-π到π，pitch是-π/2到π/2。
看到这句话茅塞顿开啊。。。。。。。测试了一下果然如此，也就是说roll和yaw取值范围正负180度，pitch取值在正负90度（度格式）
至于那个。5f我也不多想了，根据公式直接改成      
     float fCosHRoll = cos(ea_fRoll * 0.5);
        float fSinHRoll = sin(ea_fRoll * 0.5);
        float fCosHPitch = cos(ea_fPitch * 0.5);
        float fSinHPitch = sin(ea_fPitch * 0.5);
        float fCosHYaw = cos(ea_fYaw * 0.5);
        float fSinHYaw = sin(ea_fYaw * 0.5);
这样来回转换的数值就正确了。。。
ea_fPitch = asin(CLAMP(2 * (w * x - y * z) , -1.57, 1.57));就是对pitch做了一个限制。取值范围就限制在正负π/2(弧度)

[ 本帖最后由 峰回路转 于 2012-2-1 17:15 编辑 ]
作者: 峰回路转    时间: 2012-2-1 18:05
乘胜追击，测试了四元的乘法，Quaternion_Multiply(w,x,y,z,b_w,b_x,b_y,b_z )                        //{
        //Quaternion c;
        c_w=w*b_w        -x*b_x        -y*b_y        -z*b_z;
        c_x=w*b_x        +x*b_w        +y*b_z        -z*b_y;
        c_y=w*b_y        -x*b_z        +y*b_w        +z*b_x;
        c_z=w*b_z        +x*b_y        -y*b_x        +z*b_w;
        Quaternion_Normalize(c_w,c_x,c_y,c_z);
        //return c;
}

发现输出严重错乱。研究中。。
作者: 峰回路转    时间: 2012-2-2 01:57
终于调通了。代码如下
ea_EulerAngle[0]=0;//(atan2(ax,az));           //根据acc算出欧拉（弧度）
        ea_EulerAngle[1]=0;//(-atan2(ay,az));
        ea_EulerAngle[2]=0;
        Quaternion_FromEulerAngle( ea_EulerAngle[0],ea_EulerAngle[1],ea_EulerAngle[2] ); //欧拉转四元 初始值
        ea_Quaternion[0]=w;
        ea_Quaternion[1]=x;
        ea_Quaternion[2]=y;                          //放入初始化数组
        ea_Quaternion[3]=z;

          Roll=ea_EulerAngle[0]*57.29578;                         //将弧度转换为角度，便于观察
          Pitch=ea_EulerAngle[1]*57.29578;

         delay_ms(21);

        while(1)
        {test=!test;
          GyroRead();                                           //读取gyro
          new_EulerAngle[0]=(gx/rad*f);          //读取到的为度/s   /pi/180
          new_EulerAngle[1]=(gy/rad*f);
          new_EulerAngle[2]=(gz/rad*f);

          Quaternion_FromEulerAngle( new_EulerAngle[0],new_EulerAngle[1],new_EulerAngle[2] ); //欧拉转四元 更新
          new_Quaternion[0]=w;
          new_Quaternion[1]=x;                  //得到新的四元
          new_Quaternion[2]=y;
          new_Quaternion[3]=z;

          Quaternion_Multiply(ea_Quaternion[0],ea_Quaternion[1],ea_Quaternion[2],ea_Quaternion[3],new_Quaternion[0],new_Quaternion[1],new_Quaternion[2],new_Quaternion[3] )        ;//四元乘法，和原始姿态融合
          ea_Quaternion[0]=c_w;
          ea_Quaternion[1]=c_x;                  //得到新的四元
          ea_Quaternion[2]=c_y;
          ea_Quaternion[3]=c_z;

          Quaternion_ToEulerAngle(c_w,c_x,c_y,c_z);        //四元转欧拉(弧度)

          Roll=ea_fRoll*57.29578;                                                //弧度转化为度
          Pitch=ea_fPitch*57.29578;
          Yaw=ea_fYaw*57.29578;
         
      LED1=!LED1;
          delay_ms(21);
        }
实际初始化是没有计算角度
出现的问题：得到的roll pitch yaw值转换为角度后数值缩小了10倍，例如：90度，算出来的是9.0度，仔细检查了adc和弧度-度转换没发现问题在哪。还有就是ea_fPitch = asin(CLAMP(2 * (w * x - y * z) , -1.0 , 1.0));中的clamp函数，应该是clamp（x,min,max）if x<min x=min;if x>max x=max. 但是一旦加加上CLAMP ，pitch的输出始终为零。只好写成 ea_fPitch = asin(2 * (w * x - y * z));
目前静态漂移在每分钟18度左右。

[ 本帖最后由 峰回路转 于 2012-2-2 02:05 编辑 ]
作者: 静静的回忆    时间: 2012-2-2 02:14
标题: 回复 17楼 firedog2011 的帖子
没人研究哪来的FPV给你玩
作者: 峰回路转    时间: 2012-2-3 00:06
原帖由 (涉嫌广告已屏蔽) 于 2012-2-2 12:45 发表

asin不用CLAMP限幅，四元数每次都会规范化一下，而且欧拉角的定义就是这个范围，所以此处算出的pitch是绝对不会出现问题的。
静态漂移跟陀螺零偏有关系，与算法无关。校准过的陀螺，yaw轴没有加计校正，大概漂移在 ...

谢谢，把弧度转换的系数统一用#define  rad 180/PI 输出值就正常了。以前有的用的是180/PI 有的用的是57.29578，难道#define  rad 180/PI 得到的rad=572.9578？
下面改研究一下陀螺的零偏和加速度的融合了。呵呵。。。
作者: 竹蜻蜓的天空    时间: 2012-2-3 06:32
:em26:
作者: vv.vv    时间: 2012-2-3 10:05
:em26: :em26:
作者: 峰回路转    时间: 2012-2-3 12:55
ea_rolll=(atan2(ay,az));           //根据acc算出欧拉（弧度）  
ea_pitch=(-atan2(ax,az));
这样算的确有问题。p和r会互相影响
应该用 h_euyck 的公式：
imu.euler.x = atan2(imu.accel.y, imu.accel.z);
imu.euler.y = -asin(imu.accel.x / ACCEL_1G);      //accel_1g = 1 ？
但是这样当pitch翻转超过90度时该怎么办呢？

计算速度融合采用了 输出=acc*0.7+gyro*0.3 的方法。抑制漂移很有效果，问题还是当pitch超过90度时似乎是溢出还是怎么的，输出就不正常了。、
是不是欧拉角限制问题啊？

[ 本帖最后由 峰回路转 于 2012-2-3 12:59 编辑 ]
作者: heuyck    时间: 2012-2-3 13:10
accel_1g是重力加速度单位，是9.81xxxx
无论yaw-pitch-roll顺序定义的欧拉角pitch范围，还是arcsin函数计算结果的范围，都是正负90度。。。。何来超过90度。。。
比如roll和yaw不变为0度，你想象中的pitch慢慢变大，最后超过90度比如91度的过程，欧拉角的实际描述过程如下：
roll和yaw维持0度，pitch慢慢变大；在90度时进入奇点，欧拉角陷入万向锁问题，roll和yaw无法计算和分辨；越过90度到达91度时，pitch减小到89度，roll变成-180度，yaw变成180度。
作者: heuyck    时间: 2012-2-3 13:20
所谓奇点或者万向锁的问题，我解释一下，你可以拿本书比划着想象。
按yaw-pitch-roll顺序定义的欧拉角，如果顺序中间的pitch是90度，那么先yaw多少度和后roll多少度是等价的，根本分不清楚。从公式上来看，roll和yaw也无法从arctan中计算出来，arctan的输入有很小的波动，roll和yaw就乱变。
按其他顺序定义的欧拉角同样有这个问题。

[ 本帖最后由 heuyck 于 2012-2-3 13:21 编辑 ]
作者: fqrmmra    时间: 2012-2-3 14:51
帮顶:em26: 搞技术的都是牛人



欢迎光临 5iMX.com 我爱模型 玩家论坛 ——专业遥控模型和无人机玩家论坛（玩模型就上我爱模型，创始于2003年） (http://wz.5imx.com/) Powered by Discuz! X3.3