四元数个人心得分享



  • 前言

    四元数是一种可以用来描述物体旋转姿态的数学工具,相比于其他描述方式,四元数不会像欧拉角一样有万向锁问题,且计算方式相对欧拉角和旋转矩阵简洁,总的来说,好用就完事。
    网上关于四元数的资料大多只贴四元数的运算规则,然后来一句非常抽象,非常不友好,本篇希望尽量以容易理解的方式介绍四元数。
    本篇中提到四元数默认为单位四元数,单位四元数仅表示旋转,非单位四元数还代表放缩
    单位四元数即模为1的四元数


    关于四元数

    表达方式

    四元数由四部分组成,一个实部和三个虚部,是一类超复数,其中一种表达形式为 a + bi + cj + dk。
    在编程时,一般为以下形式:

    typedef struct q { 
      float w;
      float x;
      float y;
      float z;
    } Q;
    

    物理意义

    首先,物体在三维空间中的姿态可表示为物体在世界坐标系下沿着某根轴旋转某个角度表示,而这个轴可由一个三维空间中的单位向量确定,因此,只需四个坐标便可表示三维物体的旋转姿态,四元数表示三维旋转的方法就是基于这种思路的。
    在世界坐标系Oxyz中,有三维单位向量 alpha(l,m,n) ,将物体从初始位置(即物体的自身坐标系Ox'y'z'与世界坐标系重合的位置)沿着alpha以右手法则(类似安培定则)旋转theta,则此时物体的姿态可用四元数
    cos(theta / 2) + (sin(theta/2) * l) i + (sin(theta/2) * m) j + (sin(theta/2) * n) k
    表示。

    运算方式(四元数用q表示)

    四元数的模

    类似向量的模,即

      buf = p_q->w * p_q->w + p_q->x * p_q->x + p_q->y * p_q->y + p_q->z * p_q->z;
      buf = sqrtf(buf);
    

    |q| = buf;

    四元数共轭

    类似于复数共轭q的共轭一般表示为q*

    void quaternion_Conjugate(Q *p_q) {
      p_q->x *= -1.0f;
      p_q->y *= -1.0f;
      p_q->z *= -1.0f;
    }
    
    四元数逆

    对于非单位四元数q,q的逆q-1 = q* / |q|
    对于单位四元数,q-1 == q*
    q inverse(q) == inverse(q) q == q0 == (1,0,0,0)

    四元数单位化

    一般使用单位四元数表示旋转姿态,单位化方法如下:

    void quaternion_Unitization(Q *p_q) {
      static float buf;
    
      buf = p_q->w * p_q->w + p_q->x * p_q->x + p_q->y * p_q->y + p_q->z * p_q->z;
      buf = sqrtf(buf);
      p_q->w /= buf;
      p_q->x /= buf;
      p_q->y /= buf;
      p_q->z /= buf;
    }
    
    四元数乘法

    对于四元数乘法q1q2(四元数乘法不是点积也不是叉积),四元数乘法的物理意义是将位于q1姿态的物体进行q2旋转(相对于q1自身坐标系)
    四元数乘法遵循结合律,即 q1q2q3 == q1(q2q3),但不遵循交换律,即q1q2 不一定等于 q2q1
    对于四元数乘法q3 = q1q2,公式如下:

    void quaternion_Multiplication(Q *p_q1, Q *p_q2, Q *p_q3) {
      p_q3->w = (p_q1->w * p_q2->w) - (p_q1->x * p_q2->x) - (p_q1->y * p_q2->y) - (p_q1->z * p_q2->z);
      p_q3->x = (p_q1->w * p_q2->x) + (p_q1->x * p_q2->w) + (p_q1->y * p_q2->z) - (p_q1->z * p_q2->y);
      p_q3->y = (p_q1->w * p_q2->y) - (p_q1->x * p_q2->z) + (p_q1->y * p_q2->w) + (p_q1->z * p_q2->x);
      p_q3->z = (p_q1->w * p_q2->z) + (p_q1->x * p_q2->y) - (p_q1->y * p_q2->x) + (p_q1->z * p_q2->w);
    }
    
    四元数除法

    q3 = q2/q1 = inverse(q1) q2
    其实并没有严格的对于四元数除法的定义,单纯是因为之前有用到四元数求相对姿态方便起见起的名字,除法是相对于乘法而来,物理意义为两者之间的相对姿态(q1经过q3旋转得到q2),更准确来说,是q2相对于q1在q1自身坐标系中的姿态
    代码如下:

    void quaternion_Division(Q *p_q1, Q *p_q2, Q *p_q3) {
      quaternion_Conjugate(p_q1);
      quaternion_Multiplication(p_q1, p_q2, p_q3);
      quaternion_Conjugate(p_q1);
    }
    
    在空间中旋转一个点(旋转方向)

    对于空间中某点p(r,s,t),将该点(方向)进行q旋转,可用四元数 h(0,r,s,t),与q进行如下运算
    qhq*
    得到的即为旋转之后的方向(结果的q.w必定为0)

    用四元数计算相对姿态

    需要用两个陀螺仪进行姿态补偿,即,将两陀螺仪固定在一起,转动,得到的相对姿态应该不变
    使用之前提到的四元数除法即可

    从四元数到欧拉角

    欧拉角有内旋外旋,每种各有 3 x 2 x 2 = 12种,总计24种,解算方法也有24种,常用为zyx顺序内旋欧拉角,解算方法如下

    void quaternion_To_Euler(Q *p_q, Euler *p_e) {
      p_e->yaw = atan2f(2 * (p_q->w * p_q->z + p_q->x * p_q->y), 1 - 2 * (p_q->z * p_q->z + p_q->y * p_q->y)) / PI * 180;
      p_e->pitch = asinf(2 * (p_q->w * p_q->y - p_q->x * p_q->z)) / PI * 180;
      p_e->roll = atan2f(2 * (p_q->w * p_q->x + p_q->y * p_q->z), 1 - 2 * (p_q->y * p_q->y + p_q->x * p_q->x)) / PI * 180;
    }
    

    yaw为+-180,pitch为+-90,roll为+-180

    如有错误,欢迎指正,欢迎交流



  • b站有视频不错,关于四元数的
    https://www.bilibili.com/video/BV1Cs411X7kT



  • This post is deleted!

 

Copyright © 2018 bbs.dian.org.cn All rights reserved.

Looks like your connection to Dian was lost, please wait while we try to reconnect.