BetaFlight深入传感设计之六：四元数计算方法

1. 四元数理论
2. 程序四元素
3. 机体坐标系和地球坐标系转换
- 3.1 机体坐标系向地球坐标系旋转
- 3.2 地球坐标系向机体坐标系旋转
4. 总结
5. 补充资料
6. 参考资料

BetaFlight和iNavFlight都是出自CleanFlight分支，两者的侧重点不太一样。从四元数应用的角度来看，iNav不仅代码比较完整，且对四元数操作进行了相应的数学逻辑封装。

我们这里针对四元数的应用，结合代码进行一次整理和理解。

1. 四元数理论

1.1 定义

矢量式

$\overrightarrow{Q} = q_0 + \overrightarrow{q}$

复数式

$\overrightarrow{Q} = q_0 + q_1 \cdot i + q_2 \cdot j + q_3 \cdot k$

注：高中课本的复数就是超复数的一种特殊情况，其中 $q_2 = q_3 = 0$ 。

三角式

$\overrightarrow{Q} = cos \cfrac{\theta}{2} + \overrightarrow{u} sin \cfrac{\theta}{2}$

1.2 基本运算

1.2.1 矢量加减

$\overrightarrow{P} \pm \overrightarrow{Q} = (p_0 + p_1 \cdot i + p_2 \cdot j + p_3 \cdot k) \pm (q_0 + q_1 \cdot i + q_2 \cdot j + q_3 \cdot k) = (p_0 \pm q_0) + (p_1 \pm q_1) \cdot i + (p_2 \pm q_2) \cdot j + (p_3 \pm q_3) \cdot k$

1.2.2 标量乘法

$\cdot \overrightarrow{Q} = a \cdot q_0 + a \cdot q_1 \cdot i + a \cdot q_2 \cdot j + a \cdot q_3 \cdot k$

1.3 矢量点叉乘

1.3.1 矢量点乘

$\overrightarrow{P} \cdot \overrightarrow{Q} = \rvert P \rvert \rvert Q \rvert cos \theta_{PQ}$

$\overrightarrow{P} \cdot \overrightarrow{Q} = (p_0 + p_1 \cdot i + p_2 \cdot j + p_3 \cdot k) \cdot (q_0 + q_1 \cdot i + q_2 \cdot j + q_3 \cdot k) = p_0 q_0 + p_1 q_1 + p_2 q_2 + p_3 q_3$

1.3.2 矢量叉乘

$\overrightarrow{P} \times \overrightarrow{Q} = -\overrightarrow{Q} \times \overrightarrow{P}$

$\rvert \overrightarrow{P} \times \overrightarrow{Q} \rvert =\rvert P \rvert \rvert Q \rvert sin \theta_{PQ}$

$\overrightarrow{P} \times \overrightarrow{Q} = (\overrightarrow{P} \times \overrightarrow{Q} )_x + (\overrightarrow{P} \times \overrightarrow{Q} )_y + (\overrightarrow{P} \times \overrightarrow{Q} )_z$

$(\overrightarrow{P} \times \overrightarrow{Q} )_x = P_yQ_z - P_zQ_y$
$(\overrightarrow{P} \times \overrightarrow{Q} )_y = P_zQ_x - P_xQ_z$
$(\overrightarrow{P} \times \overrightarrow{Q} )_z = P_xQ_y - P_yQ_x$

$\overrightarrow{P} \times \overrightarrow{Q} = (p_0 + p_1 \cdot i + p_2 \cdot j + p_3 \cdot k) \times (q_0 + q_1 \cdot i + q_2 \cdot j + q_3 \cdot k) = (p_0q_0 - p_1q_1 - p_2q_2 - p_3q_3) + (p_0q_1 + p_1q_0 + p_2q_3 - p_3q_2) \cdot i + (p_0q_2 + p_2q_0 + p_3q_1 -p_1q_3) \cdot j +(p_0q_3 + p_3q_0 +p_1q_2 - p_2q_1) \cdot k = r_0 + r_1 \cdot i + r_2 \cdot j + r_3 \cdot k$

换成矩阵表达式(便于后续计算机计算)：

$M\rq(Q)P$

[\begin{matrix} r_{0} \\ r_{1} \\ r_{2} \\ r_{3} \end{matrix}]

= M(P)Q =

[\begin{matrix} p_{0} & - p_{1} & - p_{2} & - p_{3} \\ p_{1} & p_{0} & - p_{3} & p_{2} \\ p_{2} & p_{3} & p_{0} & - p_{1} \\ p_{3} & - p_{2} & p_{1} & p_{0} \end{matrix}]

[\begin{matrix} q_{0} \\ q_{1} \\ q_{2} \\ q_{3} \end{matrix}]

⎣ ⎡ r_{0} r_{1} r_{2} r_{3} ⎦ ⎤ = M (P) Q = ⎣ ⎡ p_{0} p_{1} p_{2} p_{3} - p_{1} p_{0} p_{3} - p_{2} - p_{2} - p_{3} p_{0} p_{1} - p_{3} p_{2} - p_{1} p_{0} ⎦ ⎤ ⎣ ⎡ q_{0} q_{1} q_{2} q_{3} ⎦ ⎤

[\begin{matrix} r_{0} \\ r_{1} \\ r_{2} \\ r_{3} \end{matrix}]

= M\rq(Q)P =

[\begin{matrix} q_{0} & - q_{1} & - q_{2} & - q_{3} \\ q_{1} & q_{0} & q_{3} & - q_{2} \\ q_{2} & - q_{3} & q_{0} & q_{1} \\ q_{3} & q_{2} & - q_{1} & q_{0} \end{matrix}]

[\begin{matrix} p_{0} \\ p_{1} \\ p_{2} \\ p_{3} \end{matrix}]

⎣ ⎡ r_{0} r_{1} r_{2} r_{3} ⎦ ⎤ = M^{'} (Q) P = ⎣ ⎡ q_{0} q_{1} q_{2} q_{3} - q_{1} q_{0} - q_{3} q_{2} - q_{2} q_{3} q_{0} - q_{1} - q_{3} - q_{2} q_{1} q_{0} ⎦ ⎤ ⎣ ⎡ p_{0} p_{1} p_{2} p_{3} ⎦ ⎤

1.4 除法求逆

$\overrightarrow{P} \otimes \overrightarrow{R} = 1$

将记作： $\overrightarrow{R} = \overrightarrow{P^{-1}}=\overrightarrow{P^*}$

$\overrightarrow{P} \otimes \overrightarrow{P^*} = （p_0 + p_1 \cdot i + p_2 \cdot j + p_3 \cdot k） \otimes （p_0 - p_1 \cdot i - p_2 \cdot j - p_3 \cdot k）= q_0^2 + q_1^2 + q_2^2 + q_3^2 = \rVert P \rVert$

所以: $\overrightarrow{P^{-1}} =\cfrac{\overrightarrow{P^*}}{ \rVert P \rVert}$

2. 程序四元素

2.1 四元数定义

typedef struct {
    float q0, q1, q2, q3;
} fpQuaternion_t;
1
2
3

2.2 四元数初始化

2.2.1 单位标量初始化

static inline fpQuaternion_t * quaternionInitUnit(fpQuaternion_t * result)
{
    result->q0 = 1.0f;
    result->q1 = 0.0f;
    result->q2 = 0.0f;
    result->q3 = 0.0f;
    return result;
}
1
2
3
4
5
6
7
8

2.2.2 矢量初始化

static inline fpQuaternion_t * quaternionInitFromVector(fpQuaternion_t * result, const fpVector3_t * v)
{
    result->q0 = 0.0f;
    result->q1 = v->x;
    result->q2 = v->y;
    result->q3 = v->z;
    return result;
}
1
2
3
4
5
6
7
8

2.3 表达转换

注：进行转换的输入参数必须归一化。

2.3.1 复数式转三角式

static inline void quaternionToAxisAngle(fpAxisAngle_t * result, const fpQuaternion_t * q)
{
    fpAxisAngle_t a = {.axis = {{1.0f, 0.0f, 0.0f}}, .angle = 0};

    a.angle = 2.0f * acos_approx(constrainf(q->q0, -1.0f, 1.0f));

    if (a.angle > M_PIf) {
        a.angle -= 2.0f * M_PIf;
    }

    const float sinVal = sqrt(1.0f - q->q0 * q->q0);

    // Axis is only valid when rotation is large enough sin(0.0057 deg) = 0.0001
    if (sinVal > 1e-4f) {
        a.axis.x = q->q1 / sinVal;
        a.axis.y = q->q2 / sinVal;
        a.axis.z = q->q3 / sinVal;
    } else {
        a.angle = 0;
    }

    *result = a;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23

2.3.2 三角式转复数式

static inline fpQuaternion_t * axisAngleToQuaternion(fpQuaternion_t * result, const fpAxisAngle_t * a)
{
  fpQuaternion_t q;
  const float s = sin_approx(a->angle / 2.0f);

  q.q0 = cos_approx(a->angle / 2.0f);
  q.q1 = -a->axis.x * s;
  q.q2 = -a->axis.y * s;
  q.q3 = -a->axis.z * s;

  *result = q;
  return result;
}
1
2
3
4
5
6
7
8
9
10
11
12
13

问题1：为什么 $q_1 q_2 q_3$ 是负值??? 按照定义的角度看应该是正的呀，有知道的朋友请评论批注，谢谢！

2.4 基本运算

2.4.1 矢量加法

static inline fpQuaternion_t * quaternionAdd(fpQuaternion_t * result, const fpQuaternion_t * a, const fpQuaternion_t * b)
{
    fpQuaternion_t p;

    p.q0 = a->q0 + b->q0;
    p.q1 = a->q1 + b->q1;
    p.q2 = a->q2 + b->q2;
    p.q3 = a->q3 + b->q3;

    *result = p;
    return result;
}
1
2
3
4
5
6
7
8
9
10
11
12

2.4.2 标量乘法

static inline fpQuaternion_t * quaternionScale(fpQuaternion_t * result, const fpQuaternion_t * a, const float b)
{
    fpQuaternion_t p;

    p.q0 = a->q0 * b;
    p.q1 = a->q1 * b;
    p.q2 = a->q2 * b;
    p.q3 = a->q3 * b;

    *result = p;
    return result;
}
1
2
3
4
5
6
7
8
9
10
11
12

2.5 矢量点叉乘

2.5.1 矢量点乘

略.

注：貌似点乘在实际飞控应用中用的不多，代码中未见定义。

2.5.2 矢量叉乘

根据叉乘定义和右手螺旋法则，我们知道叉乘结果是两个矢量模与 $\theta$ 乘积，方向垂直于两个叉乘矢量。

当 $\theta = 0$ 的时候，叉乘结果为零；
当 $\theta = 90$ 的时候，叉乘结果为两个矢量模，并垂直于两个叉乘矢量；

所以，在实际应用中，归一化的矢量叉乘结果往往用于表达两个矢量的相关性。当两个物理量表达的是同一个物理特性时，叉乘结果的值越趋于零，表示误差越小。

static inline fpQuaternion_t * quaternionMultiply(fpQuaternion_t * result, const fpQuaternion_t * a, const fpQuaternion_t * b)
{
  fpQuaternion_t p;

  p.q0 = a->q0 * b->q0 - a->q1 * b->q1 - a->q2 * b->q2 - a->q3 * b->q3;
  p.q1 = a->q0 * b->q1 + a->q1 * b->q0 + a->q2 * b->q3 - a->q3 * b->q2;
  p.q2 = a->q0 * b->q2 - a->q1 * b->q3 + a->q2 * b->q0 + a->q3 * b->q1;
  p.q3 = a->q0 * b->q3 + a->q1 * b->q2 - a->q2 * b->q1 + a->q3 * b->q0;

  *result = p;
  return result;
}
1
2
3
4
5
6
7
8
9
10
11
12

2.6 零标量四元数旋转

2.6.1 CW方向旋转

static inline fpVector3_t * quaternionRotateVector(fpVector3_t * result, const fpVector3_t * vect, const fpQuaternion_t * ref)
{
    fpQuaternion_t vectQuat, refConj;

    vectQuat.q0 = 0;
    vectQuat.q1 = vect->x;
    vectQuat.q2 = vect->y;
    vectQuat.q3 = vect->z;

    quaternionConjugate(&refConj, ref);
    quaternionMultiply(&vectQuat, &refConj, &vectQuat);
    quaternionMultiply(&vectQuat, &vectQuat, ref);

    result->x = vectQuat.q1;
    result->y = vectQuat.q2;
    result->z = vectQuat.q3;
    return result;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18

2.6.2 CCW方向旋转

static inline fpVector3_t * quaternionRotateVectorInv(fpVector3_t * result, const fpVector3_t * vect, const fpQuaternion_t * ref)
{
    fpQuaternion_t vectQuat, refConj;

    vectQuat.q0 = 0;
    vectQuat.q1 = vect->x;
    vectQuat.q2 = vect->y;
    vectQuat.q3 = vect->z;

    quaternionConjugate(&refConj, ref);
    quaternionMultiply(&vectQuat, ref, &vectQuat);
    quaternionMultiply(&vectQuat, &vectQuat, &refConj);

    result->x = vectQuat.q1;
    result->y = vectQuat.q2;
    result->z = vectQuat.q3;
    return result;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18

2.7 其他运算

2.7.1 四元素模平方

static inline float quaternionNormSqared(const fpQuaternion_t * q)
{
    return sq(q->q0) + sq(q->q1) + sq(q->q2) + sq(q->q3);
}
1
2
3
4

2.7.2 四元素求厄

static inline fpQuaternion_t * quaternionConjugate(fpQuaternion_t * result, const fpQuaternion_t * q)
{
    result->q0 =  q->q0;
    result->q1 = -q->q1;
    result->q2 = -q->q2;
    result->q3 = -q->q3;

    return result;
}
1
2
3
4
5
6
7
8
9

2.7.3 四元素归一化

static inline fpQuaternion_t * quaternionNormalize(fpQuaternion_t * result, const fpQuaternion_t * q)
{
    float mod = fast_fsqrtf(quaternionNormSqared(q));
    if (mod < 1e-6f) {
        // Length is too small - re-initialize to zero rotation
        result->q0 = 1;
        result->q1 = 0;
        result->q2 = 0;
        result->q3 = 0;
    }
    else {
        result->q0 = q->q0 / mod;
        result->q1 = q->q1 / mod;
        result->q2 = q->q2 / mod;
        result->q3 = q->q3 / mod;
    }

    return result;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19

3. 机体坐标系和地球坐标系转换

问题2：这里为什么NED (sensor frame) 和 NEU (navigation)是这个关系，逻辑在哪里？？？

3.1 机体坐标系向地球坐标系旋转

void imuTransformVectorBodyToEarth(fpVector3_t * v)
{
    // From body frame to earth frame
    quaternionRotateVectorInv(v, v, &orientation);

    // HACK: This is needed to correctly transform from NED (sensor frame) to NEU (navigation)
    v->y = -v->y;
}
1
2
3
4
5
6
7
8

3.2 地球坐标系向机体坐标系旋转

void imuTransformVectorEarthToBody(fpVector3_t * v)
{
    // HACK: This is needed to correctly transform from NED (sensor frame) to NEU (navigation)
    v->y = -v->y;

    // From earth frame to body frame
    quaternionRotateVector(v, v, &orientation);
}
1
2
3
4
5
6
7
8