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

lida2003

已于 2022-10-20 17:24:11 修改

阅读量1k

点赞数 2

分类专栏： xFlight 文章标签：嵌入式硬件 stm32 betaflight

于 2022-10-19 19:39:04 首次发布

本文链接：https://blog.csdn.net/lida2003/article/details/127414334

版权

xFlight 专栏收录该内容

102 篇文章 128 订阅

订阅专栏

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{bmatrix} r_0 \\ r_1 \\ r_2 \\ r_3 \end{bmatrix} = M(P)Q =\begin{bmatrix} 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{bmatrix}\begin{bmatrix} q_0 \\ q_1 \\ q_2 \\ q_3 \end{bmatrix}$

$\begin{bmatrix} r_0 \\ r_1 \\ r_2 \\ r_3 \end{bmatrix} = M\rq(Q)P = \begin{bmatrix} 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{bmatrix}\begin{bmatrix} p_0 \\ p_1 \\ p_2 \\ p_3 \end{bmatrix}$

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;

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;
}

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;
}

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;
}

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：为什么 $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;
}

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;
}

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;
}

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;
}

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;
}

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);
}

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;
}

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;
}

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;
}

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);
}