Robofun 機器人論壇

標題: 六軸數據(加速規+陀螺儀)實作四元數法轉歐拉角問題!! [打印本頁]

作者: sam183132 時間: 2014-10-17 20:53
標題: 六軸數據(加速規+陀螺儀)實作四元數法轉歐拉角問題!!
各位大大你好~小弟是碩士生~!
最近研究需要利用六軸數據(加速規+陀螺儀)來實作四元數法轉歐拉角的應用，並實作出慣性傳感器的3D旋轉模型，有看過網路上分享的方法，但有些地方不太瞭解：
1.想請問各位大大，如果不使用四元數法轉歐拉角，那要如何計算出三軸的歐拉角數值(Roll,Pitch,Yaw)?

2.小弟有參考網路上的一個方法：

  //gx,gy,gz -> 為陀螺儀三軸的數值
  //ax,ay,az -> 為加速規三軸的數值

public void Quaternions1(float gx, float gy, float gz, float ax, float ay, float az)
      {
            float norm;
            float vx, vy, vz;
            float ex, ey, ez;

            // 測量正常化
            norm =  Math.Sqrt(ax * ax + ay * ay + az * az);
            ax = ax / norm;
            ay = ay / norm;
            az = az / norm;

            // 估計方向的重力
            vx = 2 * (q1 * q3 - q0 * q2);
            vy = 2 * (q0 * q1 + q2 * q3);
            vz = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3;

            // 錯誤的領域和方向傳感器測量參考方向之間的交叉乘積的總和
            ex = (ay * vz - az * vy);
            ey = (az * vx - ax * vz);
            ez = (ax * vy - ay * vx);

            // 積分誤差比例積分增益
            exInt = exInt + ex * Ki;
            eyInt = eyInt + ey * Ki;
            ezInt = ezInt + ez * Ki;

            // 調整後的陀螺儀測量
            gx = gx + Kp * ex + exInt;
            gy = gy + Kp * ey + eyInt;
            gz = gz + Kp * ez + ezInt;

            // 整合四元數率和正常化
            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;

            // 正常化四元
            norm = Math.Sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
            q0 = q0 / norm;
            q1 = q1 / norm;
            q2 = q2 / norm;
            q3 = q3 / norm;

         //
Yaw=    atan2f( 2 * (q0 * q1 + q2 * q3), q0q0 - q1q1 - q2q2 + q3q3 )*57.3;//yaw

// Pitch=    asin(-2 * (q1 * q3 + 2 * q0* q2)* 57.3; // pitch

// Rool = atan2 ( 2 * (q1 * q2 + 2 * q0 * q3), q0q0 + q1q1 - q2q2 - q3q3 )* 57.3; //roll

            Roll = (float)(Math.Atan2(2 * q2 * q3 + 2 * q0 * q1, (-2) * q1 * q1 - 2 * q2 * q2 + 1) * 57.3);
            Pitch = (float)(Math.Asin((-2) * q1 * q3 + 2 * q0 * q2) * 57.3);
            Yaw = (float)(Math.Atan2(2 * q1 * q2 + 2 * q0 * q3, (-2) * q2 * q2 + (-2) * q3 * q3 + 1) * 57.3);

      }

使用六軸數據(加速規+陀螺儀)實作出來的三軸歐拉角數值（Roll,Pitch,Yaw）都會亂飄，也就是三軸的歐拉角數值都會亂跳，並不會固定在某個數值浮動，不知道問題出在哪，請各位大大幫忙解惑~感謝!!

作者: vegewell 時間: 2014-10-19 16:26
回復 1# sam183132

這裡有產品 UM6 Orientation Sensor,可自行秀出 Euler Angles (yaw, pitch, and roll)不用另外程式碼去算,

http://www.chrobotics.com/shop/orientation-sensor-um6
===========================================
>>>歐拉角數值（Roll,Pitch,Yaw）都會亂飄,

If you want to get Euler angles from your quaternion (y』know, heading, pitch & roll) then you』ll need to do something like this
Vector Quaternion::toEuler()
{
Vector ret;
double sqw = _w*_w;
double sqx = _x*_x;
double sqy = _y*_y;
double sqz = _z*_z;

ret.x() = atan2(2.0*(_x*_y+_z*_w),(sqx-sqy-sqz+sqw));
ret.y() = asin(-2.0*(_x*_z-_y*_w)/(sqx+sqy+sqz+sqw));
ret.z() = atan2(2.0*(_y*_z+_x*_w),(-sqx-sqy+sqz+sqw));

return ret;
}
This function will pack a vector with heading, pitch and roll information. x = heading, y = pitch, z = roll.
==============================================================================
可以從某些library,裡找到數學運算程式,如 FreeIMU library,

作者: g921002 時間: 2014-10-22 00:40
確認陀螺儀跟加速規的座標軸是不是一致(N,E,D-->X,Y,Z)

作者: sam183132 時間: 2014-10-22 23:52
感謝各位大大的回覆，最後我發現了問題之所在~
原因是因為我輸入的陀螺儀數值有問題，我輸入的陀螺儀數值單位是deg/s，
但公式需要的陀螺儀數值，其單位是rad/s，所以我將輸入的陀螺儀數據更正之後就可以了~!!

感謝各位大大的幫忙^^

歡迎光臨 Robofun 機器人論壇 (https://robofun.net/forum/)