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

峰回路转

原帖由 (涉嫌广告已屏蔽) 于 2012-2-3 13:10 发表
accel_1g是重力加速度单位，是9.81xxxx
无论yaw-pitch-roll顺序定义的欧拉角pitch范围，还是arcsin函数计算结果的范围，都是正负90度。。。。何来超过90度。。。
比如roll和yaw不变为0度，你想象中的pitch慢慢变大 ...

看来我没搞明白旋转角度与平面夹角的区别，这会清楚了，谢谢

峰回路转

这几天得进展：keil的浮点的确很扯淡。。。。。。:em20:

峰回路转

目前间的的测试如下：
    ACCRead();                                //读取acc
      ea_EulerAngle[0]=-atan2(ax,az);           //根据acc算出欧拉（弧度）
//    ea_EulerAngle[1]= asin(ay / 0.981);   
//    ea_EulerAngle[0]=-atan(ax/sqrt((ay*ay)+(az*az)));
      ea_EulerAngle[1]=atan(ay/sqrt((ax*ax)+(az*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]*rad;                         //将弧度转换为角度，便于观察
          Pitch=ea_EulerAngle[1]*rad;


         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);        //四元转欧拉(弧度)

////////////////////////acc融合测试////////////////////////////////////////////
           ACCRead();                                //读取acc
      ea_EulerAngle[0]=-atan2(ax,az);           //根据acc算出欧拉（弧度）
//    ea_EulerAngle[1]= asin(ay / 0.981);   
//    ea_EulerAngle[0]=-atan(ax/sqrt((ay*ay)+(az*az)));
      ea_EulerAngle[1]=atan(ay/sqrt((ax*ax)+(az*az)));
          ea_EulerAngle[2]=0;

      ea_EulerAngle[0]=ea_EulerAngle[0]*0.7+ea_fRoll*0.3;           //简单的融合
          ea_EulerAngle[1]=ea_EulerAngle[1]*0.7+ea_fPitch*0.3;


          Quaternion_FromEulerAngle( ea_EulerAngle[0],ea_EulerAngle[1],ea_fYaw ); //欧拉转四元 更新
          ea_Quaternion[0]=w;
          ea_Quaternion[1]=x;
          ea_Quaternion[2]=y;                          //放入初始化数组
          ea_Quaternion[3]=z;
////////////////////////////////////////////////////////////////////////////////////////////////////////
          Roll=ea_fRoll*rad;                                                //弧度转化为度
          Pitch=ea_fPitch*rad;
          Yaw=ea_fYaw*rad;

输出结果基本不漂移，晃动起来就没谱了。呵呵
roll翻转一周，显示过程：   0 - 90 - 180 - （-90）- 0
pitch翻转一周，显示过程：0 - 90 -    0      - （-90）-0  正常的话当pitch越过90度时，roll应当输出180度，现在当pitch到达90度附近（<90度），roll会有异常的输出，有30度左右的输出。当pitch越过90后就正常了
，下面改考虑滤波和更好地数据融合了。。。

[ 本帖最后由 峰回路转 于 2012-2-9 16:02 编辑 ]

sfp208

受教多多，谢谢你的无私

許婉汝

大家好~
看了分享我也試著做了一下
我用matlab當做模擬介面
照著同樣的方式寫code
再讓matlab讀訊號進去做旋轉
四元數乘完後再判定姿態的角度
但是當我陀螺儀只有單軸動的時候轉出來沒問題
可是當我先轉了一軸在轉第二軸，第三軸不動時
送出來的訊號結果就會很不合理(姿態判定不對!)
更別提同時三個軸一起動了~
請問大家的程式這樣做時有遇到這樣的問題嗎?

我在想我的程式應該就是照著分享的邏輯這樣寫
應該不會有甚麼問題
而且目前就我讀到的資料說若每次轉動的角度很小
選"XYZ"或"ZYX"等等的順序應該不影響結果
我雖然每個軸會轉到70度左右,但我的取樣頻率是100HZ
每0.01秒的轉動角度其實非常小
我實在搞不明白為什麼選"XYZ"或"ZYX"或照著本文分享的旋轉矩陣程式碼
轉出來會有不一樣的結果(本文的是XZY嗎?)

我是這個領域的新手+第一次發文(請多多包涵^^")
我有找到"慣性技術"這本書做參考
看了1,6,7章(1,7章看得比較熟)
大家的回文我也來看了好幾遍了@@
可是還是一直呈現撞牆無解的情況

dongfang

LS的这位，请把你的设备随便做一个3D动作(但最后是回位)，把3轴陀螺仪的输出（角速度）全部保存下来，然后乘以采样时间，贴到我的excel里去算一下。

如果excel最后一行的“地面角度”接近于初始状态，那就对了。

itsme

原帖由 dongfang 于 2011-6-23 10:28 发表
最近研究飞控的时候，感受到，网上大量他人共享开放的信息给我提供了很多便利。（尽管也混杂了大量无法使用的信息）

捷联系统的四元数法姿态算法的难点不是资料找不到，而是资料大多数没有标明欧拉角的定义（拜科 ...

:em26:

giyahunter

支持一下
什么时候跟进DCM算法？

pdabug

很好的讨论，看不懂

kissghost

可以移植到APM上面么

ailangzhou

不懂啊。。

cw628

原帖由 (涉嫌广告已屏蔽) 于 2012-2-3 13:20 发表
所谓奇点或者万向锁的问题，我解释一下，你可以拿本书比划着想象。
按yaw-pitch-roll顺序定义的欧拉角，如果顺序中间的pitch是90度，那么先yaw多少度和后roll多少度是等价的，根本分不清楚。从公式上来看，roll和ya ...

想请问一下，我现在作全姿态的算法就遇到这个问题，当一个轴转到90度时，另外一个正交的轴就出现所谓的奇点问题了，请教一下这种问题怎么解决？？

SZLYH

太高深了

kzjken

用下面的语句将四元数[w x y z]转换成欧拉角roll，pitch，yaw

Roll  = atan2(2 * (w * z + x * y) , 1 - 2 * (z * z + x * x));      
Pitch = asin(2 * (w * x - y * z));
Yaw  = atan2(2 * (w * y + z * x) , 1 - 2 * (x * x + y * y));

Roll和Pitch的范围是-180到180度，Pitch用asin计算范围只有-90到90度。
最大的问题时当Pitch（超过）90度，也就是说88度89度90度，然后递减又是89度88度87度，这个90度的分解处，roll和yaw角会有一个180度的转变（假定原来roll和yaw都是0，在pitch角90度的分解处会突然变成180度）
请问这个该如何解决?
感谢~~~

heuyck

奇点欧拉角没法解决的，因为是死穴。
楼上说的反向换个思路想想也很正常。
比如飞机飞行方向（yaw），在pitch过90度以后，飞行方向当然完全相反了。飞机肚皮（roll）也朝上了。两个角度都变成180度不是正和描述一致吗？

heuyck

如果说控制有断层，用“误差四元数"去做PID控制会是连续的。

aishiqi

我用这个算法出现了个问题，不知道为什么，能不能交流一下？

GerogeZhou

你好，我正在学习飞控的资料，看了你的文章，一些问题想请教，能否给个微信号或手机，谢谢。