四元数的运用 - - Powered by Discuz!

标题: 四元数的运用 [打印本页]

作者: JOB000 时间: 2017-12-12 17:04
标题: 四元数的运用
四元数初始化
q0-3为四元数四个值,用最上面公式根据加计计算出来的欧拉角来初始化

--------------------------------------------------
四元数微分方程
四元数更新算法，一阶龙库法，同样4个量（入、P1-3）也为四元数的四个值,即上面的q0-3。
Wx,y,z是三个陀螺的这个周期的角速度，比如欧拉角微分方程中的0.01弧度/秒，T为更新周期,比如上面的0.02秒。

再来一张，另外一本书上的，仔细看和上面是一样的delta角度，就是上面的角速度*周期，单位为弧度

--------------------------------------------------
四元数微分方程更新后的规范化
每个周期更新完四元数，需要对四元数做规范化处理。因为四元数本来就定义为四维单位向量。
求q0-3的平方和，再开根号算出的向量长度length。然后每个q0-3除这个length。

四元数转欧拉角公式
把四元数转成了方向余弦矩阵中的几个元素，再用这几个元素转成了欧拉角
先从四元数q0-3转成方向余弦矩阵：

再从方向余弦矩阵转成欧拉角

代码：
      //更新方向余弦矩阵
      t11=q.q0*q.q0+q.q1*q.q1-q.q2*q.q2-q.q3*q.q3;
      t12=2.0*(q.q1*q.q2+q.q0*q.q3);
      t13=2.0*(q.q1*q.q3-q.q0*q.q2);
      t21=2.0*(q.q1*q.q2-q.q0*q.q3);
      t22=q.q0*q.q0-q.q1*q.q1+q.q2*q.q2-q.q3*q.q3;
      t23=2.0*(q.q2*q.q3+q.q0*q.q1);
      t31=2.0*(q.q1*q.q3+q.q0*q.q2);
      t32=2.0*(q.q2*q.q3-q.q0*q.q1);
      t33=q.q0*q.q0-q.q1*q.q1-q.q2*q.q2+q.q3*q.q3;
      //求出欧拉角
      imu.euler.roll = atan2(t23,t33);
      imu.euler.pitch = -asin(t13);
      imu.euler.yaw = atan2(t12,t11);
      if (imu.euler.yaw < 0){
            imu.euler.yaw += ToRad(360);
      }
----------------------------------------------------

欢迎光临 (http://www.51hei.com/bbs/)