如何将四元数方向转化为旋转举证_四元数、欧拉角及方向余弦矩阵的相互转化公式...

最开始接触姿态结算的时候，四元数、欧拉角和方向余弦矩阵的相互转化便是在进阶路上的一条小老虎，看的多了，终于也算熟悉了。

引用别人的“凡是把一本书读100遍的人，没有一个人不成大器”这句话，希望对大家有所勉励。

基础理论

对于在三维空间里的一个参考系，任何坐标系的取向，都可以用三个欧拉角

来表现，一个与参考系重合的坐标系，通过绕 Z 轴的偏航角（Yaw），绕 Y 轴的

俯仰角（Pitch）和绕 X 轴的横滚角（Roll）旋转到当前姿态，常用坐标系如下：

北东地坐标系

一、方向余弦矩阵 (参考《捷联惯性导航技术》张天光等)

假定初始时刻载体坐标系和世界坐标系重合， 那么任一向量在载体坐标系和世界坐标系中的表达式相同， 即

,

先绕Z轴旋转角度

,旋转矩阵为：

推导如图：

绕Z轴旋转，其他轴类似，Y轴旋转时方向相反

再绕 Y 轴旋转角度

再绕 X 轴旋转角度

此时

然保持不变， 但是在载体坐标系中的表达式则变为

因此从世界坐标系到载体坐标系的变换矩阵可由下式给出

那么，从载体坐标系到世界坐标系的转变矩阵可以表示如下：

此外，注意到当姿态角很小的时候，

可以简化成：

二、欧拉角与四元数

按Z-Y-X的旋转变换顺序有：

四元数与旋转矩阵（常用来做坐标变换）

2.1 b系转到R系的坐标变换矩阵

2.2 R系转到b系的坐标变换矩阵

(为上述矩阵的转置)

对比R和

即可很容易将四元数转换成欧拉角。

三、四元数的更新

Q对时间求导数：

因为

所以

其中

也是大地坐标系下的角速度,由四元数旋转得：

推出

所以

代码实现：

void abcd2cbn(strcut* data)
{
    float a, b, c, d, ab, ac, ad, bb, bc, bd, cc, cd, dd;

    a  = data->q0
    b  = data->q1
    c  = data->q2
    d  = data->q3

    ab = a * b;
    ac = a * c;
    ad = a * d;
    bb = b * b;
    bc = b * c;
    bd = b * d;
    cc = c * c;
    cd = c * d;
    dd = d * d;

    data.cbn11 = 1 - 2 * (cc + dd);
    data.cbn12 = 2 * (bc - ad);
    data.cbn13 = 2 * (bd + ac);
    data.cbn21 = 2 * (bc + ad);
    data.cbn22 = 1 - 2 * (bb + dd);
    dataa.cbn23 = 2 * (cd - ab);
    data.cbn31 = 2 * (bd - ac);
    data.cbn32 = 2 * (cd + ab);
    data.cbn33 = 1 - 2 * (bb + cc);
}