姿态解算中的欧垃角与四元数

关于欧垃角与四元数的转换

在做运动物体姿态求解时，我碰到了一些问题，以此留些记录以方便同行或者以后的自己借鉴或者说是回顾。

1. 坐标系

   我们一般常用的坐标系是东北天坐标系，地理坐标系，以及刚体固连坐标系（也就是自身坐标系）。
   东北天坐标系：这个坐标系描述的姿态角在欧垃角描述下的表示方式，即表示的欧垃角是 航向角->俯仰角->滚转角。滚转角和俯仰角的正方向满足右手定则，但是航向角不满足右手定则，航向角以北偏东为正。
   地理坐标系和自身坐标系：个人理解其实就是一个相对地球（绝对）坐标系和相对自身的坐标系。

2. 四元数与坐标转换
   在姿态角的求解里四元数是什么？旋转的一种表示形式。
   $Q= q0+q1+q2+q3$
   $q0= \cos \frac{\varphi}{2}$
   $q1=l \sin \frac{\varphi}{2}$
   $q2=m\sin \frac{\varphi}{2}$
   $q3=n\sin \frac{\varphi}{2}$
   在上面的公式中我们用$\varphi$表示旋转的角度，l,m,n表示旋转的转轴，这三个值是一个三维单位向量的x,y,z值。关于四元数的一些其他特性这里就不在赘述了，大家有兴趣可以去百度一下。
   当地理坐标系里的一个点以Q进行了一次旋转之后，它的坐标会变成什么形式呢？
   
   因为（l,m,n）是一个单位向量，所以
   
   上面的矩阵指的是在b系坐标转换到R系坐标时需要在b系坐标上左乘的坐标转换矩阵。（b系是自身坐标系，R系是地理坐标系）

3. 转标转换中的欧垃角
   上面我们提过四元数在姿态解算中表示的是一种旋转，而欧垃角是什么呢？欧垃角是一种姿态角的表示方式，是用三次旋转来表示姿态，也就是用三次旋转表示 一次和旋转。
   于是从我上面的话里我们就可以找到从四元数转换到欧垃角的方法，即将2中提到的坐标转换矩阵拆成三个转换矩阵，这三个矩阵分别表示绕Z轴旋转，绕X轴旋转，绕Y轴旋转。
   一个问题来了怎么用矩阵来表示欧垃角的旋转呢？这里我们可以通过四元数来得到$\varphi$表示的是欧垃角中的角度，（n,m,l）表示的是x,y,z轴，即三次旋转分别是（0，0，1），（0，1，0），（1，0，0）。将这些值带入就可以得到三个坐标转换矩阵，再将三个坐标转换矩阵通过一定顺序相乘，就可以得到用欧垃角表示的坐标转换矩阵（如下）。
   
   
   
   

4. 欧垃角与四元数的转换
   通过上面的两个坐标转换矩阵我们不难发现欧垃角与四元数的转换关系，对此我下面列一些代码来表示结果

static three_axis Quaternion_to_Euler(Quarternion quaternion)
{
    three_axis Rad;
    float q0, q1, q2, q3;
    float sum;

    q0 = quaternion.q0;
    q1 = quaternion.q1;
    q2 = quaternion.q2;
    q3 = quaternion.q3;

    sum = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
    q0 = q0 / sum;
    q1 = q1 / sum;
    q2 = q2 / sum;
    q3 = q3 / sum;

    Rad.x = asin(2.0f*(q2*q3 + q0*q1));
    Rad.y = atan2(-2 * q1 * q3 + 2 * q0 * q2, q3*q3 - q2 * q2 - q1 * q1 + q0 * q0);
    Rad.z = atan2(2 * q1*q2 - 2 * q0*q3, q2*q2 - q3*q3 + q0*q0 - q1*q1);
    return Rad;
}
static Quarternion Euler_to_Quaternion(three_axis Rad)
{
    Quarternion quaternion;

    quaternion.q0 = 0.5*sqrt(1 + cos(Rad.y)*cos(Rad.z) + cos(Rad.x)*cos(Rad.z) + cos(Rad.x)*cos(Rad.y) + sin(Rad.x)*sin(Rad.y)*sin(Rad.z));
    if (quaternion.q0 == 0)
    {
        quaternion.q0 = 1;
        quaternion.q1 = 0;
        quaternion.q2 = 0;
        quaternion.q3 = 0;
    }
    else
    {
        quaternion.q1 = (sin(Rad.x) + sin(Rad.y)*sin(Rad.z) + sin(Rad.x)*cos(Rad.y)*cos(Rad.z)) / 4 / quaternion.q0;
        quaternion.q2 = (sin(Rad.y)*cos(Rad.z) - cos(Rad.y)*sin(Rad.z)*sin(Rad.x) + sin(Rad.y)*cos(Rad.x)) / 4 / quaternion.q0;
        quaternion.q3 = (-cos(Rad.y)*sin(Rad.z) + sin(Rad.y)*cos(Rad.z)*sin(Rad.x) - sin(Rad.z)*cos(Rad.x)) / 4 / quaternion.q0;
    }

    return quaternion;
}