BetaFlight深入传感设计之五：MahonyAHRS & 方向余弦矩阵理论

lida2003

已于 2023-12-22 11:39:00 修改

阅读量1k

点赞数 3

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

于 2022-10-15 12:35:47 首次发布

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

版权

xFlight 专栏收录该内容

102 篇文章 120 订阅

订阅专栏

BetaFlight深入传感设计之五：MahonyAHRS & 方向余弦矩阵理论

1. 基础预备知识
2. 四元数解算姿态
3. 欧拉角初始化四元数
- 3.1 【推导】欧拉角旋转
- 3.2 【代码】BetaFlight
4. 四元数初始化姿态矩阵
- 4.1 【定义】姿态矩阵
- 4.2 【代码】BetaFlight
5. 【BetaFlight】MahonyAHRS
6. 参考资料
7. 补充：基于惯性测量单元（IMU）的方向余弦矩阵（DCM）理论

该理论主要来自William Premerlani和Paul Bizard的Direction Cosine Matrix IMU: Theory，这是出处。大家除了去看英文原文资料，关于中文方面的翻译github上有链接这里提供，有兴趣的朋友可以去仔细研究。

鉴于前面对Baro/Mag/IMU/GPS传感模块的驱动以及业务方面的研读，我们遇到一个比较蹩脚的问题，就是如何基于这些传感对飞行方向进行校准。

虽然基础知识都有，但是真的在实际应用上相较于GPS距离，飞行速度，飞行方向等几何学知识来讲，方向余弦矩阵稍微还是需要拐个弯经过大脑逻辑思考才有机会理解的。

鉴于这部分内容在业务上严重阻碍了我们深入理解，包括一些物理现象导致的问题。在此将会根据自己的体会和理解进行讨论分析。

1. 基础预备知识

1.1 机体坐标系

相对于机体建立的坐标系，我们称之为机体坐标系：

Xb轴沿着机身指向机头方向（纵轴方向）
Yb轴指向机头方向的右侧，垂直于纵剖面
Zb轴在飞行器纵剖面，上垂直于轴指向下方

1.2 欧拉角

欧拉角是用来唯一地确定定点转动刚体位置的三个一组独立角参量，由章动角 $\theta$ 、进动角 $\phi$ 和自转角 $\psi$ 组成。因为欧拉首先提出，故得名。

$\alpha$ 是x轴与交点线的夹角；
$\beta$ 是z轴与Z轴的夹角；
$\gamma$ 是交点线与X轴的夹角。

转换到飞行器上，微小量变化情况下，角度对应关系如下：

绕自己的z轴旋转 $\alpha$ ==》自转角 $\psi$ ==》偏航角（yaw），机体轴在水平面上的投影与地轴之间的夹角，以机头右偏为正，又称方位角
续绕自己的x轴旋转 $\beta$ ==》章动角 $\theta$ ==》俯仰角（pitch），机体轴与地平面（水平面）之间的夹角，飞机抬头为正
最后绕自己的y轴旋转 $\gamma$ ==》进动角 $\phi$ ==》滚转角（roll），飞机对称面绕机体轴转过的角度，右滚为正，又称倾斜角

1.2.1 概念解释

欧拉角static

1.2.2 动态概念

欧拉角dynamic

1.2.3 应用概念

欧拉角 aircraft

1.3飞控操作

1.3.1 Yaw

yaw

1.3.2 Pitch

pitch

1.3.3 Roll

roll

1.3 旋转矩阵

这个是数学里面的东西，懒得翻高中和大学的书了。大体的概念就是矢量通过旋转矩阵可以改变方向。

注：通常我们这里改变方向，不改变矢量长度的，都是单位矩阵，不会影响模长，仅改变方向。

这里从二维空间开始介绍，然后在给出三维空间的公式。二维空间是比较容易理解的。

1.3.1 二维空间

在二维空间中，旋转可以用一个单一的角 $\theta$ 定义。作为约定，正角表示逆时针旋转。

$M(\theta)=\begin{bmatrix} cos \theta & -sin \theta \\ sin \theta & cos \theta \end{bmatrix} = cos \theta \begin{bmatrix} 1 & 0 \\ 0 & 1 \end{bmatrix} + sin \theta \begin{bmatrix} 0 & -1 \\ 1 & 0 \end{bmatrix} = exp\lparen \theta \begin{bmatrix} 0 & -1 \\ 1 & 0 \end{bmatrix} \rparen$

1.3.2 三维空间

在三维空间中，生成旋转矩阵的一种简单方式是把它作为三个基本旋转的序列复合。关于右手笛卡尔坐标系的 x, y和 z轴的旋转分别叫做 roll, pitch 和 yaw 旋转。

绕 x-轴的主动旋转 $\theta_x$ , 对应roll( $\gamma$ ) 角，和右手螺旋的方向相反（在yz平面顺时针）。

$R_x(\theta_x) = \begin{bmatrix} 1 & 0 & 0 \\ 0 & cos \theta_x & -sin \theta_x \\ 0 & sin \theta_x & cos \theta_x \end{bmatrix} = \begin{bmatrix} 1 & 0 & 0 \\ 0 & cos \gamma & -sin \gamma \\ 0 & sin \gamma & cos \gamma \end{bmatrix}$

绕 y-轴的主动旋转 $\theta_y$ , 对应pitch( $\alpha$ ) 角，和右手螺旋的方向相反（在zx平面顺时针）。

$R_y(\theta_y) = \begin{bmatrix} cos \theta_y & 0 & sin \theta_y \\ 0 & 1 & 0 \\ -sin \theta_y & 0 & cos \theta_y \end{bmatrix} = \begin{bmatrix} cos \alpha & 0 & sin \alpha \\ 0 & 1 & 0 \\ -sin \alpha & 0 & cos \alpha \end{bmatrix}$

绕 z-轴的主动旋转 $\theta_z$ , 对应yaw( $\beta$ ) 角，和右手螺旋的方向相反（在xy平面顺时针）。

$R_z(\theta_z) =\begin{bmatrix} cos \theta_z & -sin \theta_z & 0 \\ sin \theta_z & cos \theta_z & 0 \\ 0 & 0 & 1 \end{bmatrix} = \begin{bmatrix} cos \beta & -sin \beta & 0 \\ sin \beta & cos \beta & 0 \\ 0 & 0 & 1 \end{bmatrix}$

总的旋转矩阵(3-2-1旋转)可以表达为：

$M_b^{b\rq}(\theta_{zyx}) = R_z(\theta_z) R_y(\theta_y) R_x(\theta_x) = R_z(\beta)R_y(\alpha) R_x(\gamma) =\begin{bmatrix} cos \beta cos \alpha & - sin \beta cos \gamma + cos \beta sin \alpha sin\gamma & sin \beta sin \gamma + cos \beta sin \alpha cos \gamma \\ sin \beta cos \alpha & cos \beta cos \gamma + sin \beta sin \alpha sin \gamma & -cos \beta sin \gamma + sin \beta sin \alpha cos\gamma \\ -sin \alpha & cos \alpha sin \gamma & cos \alpha cos \gamma \end{bmatrix}$

1.4 复数

高中数学学过复数，复数可以在复平面(二维平面)内表达为：

$\overrightarrow{z} = r \cdot cos \theta + i \cdot r \cdot sin \theta = a + b \cdot i$
其中： $\vert \overrightarrow{z} \vert = r = \sqrt{a^2 + b^2}$

1.5 四元数

四元数实际上是在一个超三维空间的表达：

$\overrightarrow{Q} = q_0 + q_1 \cdot i + q_2 \cdot j + q_3 \cdot k = cos \cfrac{\theta}{2} +\overrightarrow{u} sin \cfrac{\theta}{2} = cos \cfrac{\theta}{2} +(l \cdot i + m \cdot j + n \cdot k) sin \cfrac{\theta}{2}$

模长： $\vert \overrightarrow{Q} \vert = \sqrt{q_0^2 + q_1^2 + q_2^2 + q_3^2}$ ， $\theta为实数，\overrightarrow{u}为单位向量。$

注：复数是四元数的特殊场景， $q_2 = q_3 =0$ 。

1.5.1 约束条件

【满足】分配律
【满足】结合律
【不满足】交换律

1.5.2 矢量加减

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

1.5.3 标量乘法

$\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.5.4 矢量乘法

$\overrightarrow{P} \otimes \overrightarrow{Q} = （p_0 + p_1 \cdot i + p_2 \cdot j + p_3 \cdot k） \otimes （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$

换成矩阵表达式（便于后续计算机计算）：

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

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

1.5.5 除法求逆

$\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}$

1.6 矢量点叉乘

1.6.1 向量点乘

$\overrightarrow{A} \cdot \overrightarrow{B} = \rvert A \rvert \rvert B \rvert cos \theta_{AB} = A_xB_x + A_y B_y + A_z B_z$

1.6.2 向量叉乘

$\overrightarrow{A} \times \overrightarrow{B}=-\overrightarrow{B} \times \overrightarrow{A}$

$\rvert \overrightarrow{A} \times \overrightarrow{B} \rvert =\rvert A \rvert \rvert B \rvert sin \theta_{AB}$

$\overrightarrow{A} \times \overrightarrow{B} = (\overrightarrow{A} \times \overrightarrow{B} )_x + (\overrightarrow{A} \times \overrightarrow{B} )_y + (\overrightarrow{A} \times \overrightarrow{B} )_z$

$(\overrightarrow{A} \times \overrightarrow{B} )_x = A_yB_z - A_zB_y$
$(\overrightarrow{A} \times \overrightarrow{B} )_y = A_zB_x - A_xB_z$
$(\overrightarrow{A} \times \overrightarrow{B} )_z = A_xB_y - A_yB_x$

2. 四元数解算姿态

2.1 【推导】理论(教科书)

注：下面我们引用教科书：【惯性导航/秦永元，背景，科学出版社，2006，ISBN：7-03-016428-8】的推导过程。

2.2 【推导】旋转矩阵+姿态矩阵

机体坐标向R固定系坐标映射：

$C_b^R=\begin{bmatrix} T11 & T12 & T13 \\ T21 & T22 & T23 \\ T31 & T32 & T33 \end{bmatrix}=\begin{bmatrix} C_{xx} & C_{xy} & C_{xz} \\ C_{yx} & C_{yy} & C_{yz} \\ C_{zx} & C_{zy} & C_{zz} \end{bmatrix}=\begin{bmatrix} q_1^2 + q_0^2 - q_3^2 - q_2^2 & 2(q_1 q_2 - q_0 q_3) & 2 (q_1 q_3 + q_0 q_2) \\ 2(q_1 q_2 + q_0 q_3) & q_2^2 - q_3^2 + q_0^2 - q_1^2 & 2(q_2 q_3 - q_0 q_1) \\ 2(q_1 q_3 - q_0 q_2) & 2(q_2 q_3 + q_0 q_1) & q_3^2 - q_2^2 - q_1^2 + q_0^2 \end{bmatrix}$

${C_b^R}^T=\begin{bmatrix} T11 & T21 & T31 \\ T12 & T22 & T32 \\ T13 & T23 & T33 \end{bmatrix}=\begin{bmatrix} C_{xx} & C_{yx} & C_{zx} \\ C_{xy} & C_{yy} & C_{zy} \\ C_{xz} & C_{yz} & C_{zz} \end{bmatrix}=\begin{bmatrix} q_1^2 + q_0^2 - q_3^2 - q_2^2 & 2(q_1 q_2 + q_0 q_3) & 2(q_1 q_3 - q_0 q_2) \\ 2(q_1 q_2 - q_0 q_3) & q_2^2 - q_3^2 + q_0^2 - q_1^2 & 2(q_2 q_3 + q_0 q_1) \\ 2 (q_1 q_3 + q_0 q_2) & 2(q_2 q_3 - q_0 q_1) & q_3^2 - q_2^2 - q_1^2 + q_0^2 \end{bmatrix}$

三维空间旋转矩阵逆转换：

由上面两式可以推导出以下角度姿态公式：

roll $\gamma =\arctan(\cfrac{2(q_2 q_3 + q_0 q_1)}{q_3^2 - q_2^2 - q_1^2 + q_0^2})$
pitch $\alpha = \arcsin(2(q_0 q_2 - q_1 q_3))$
yaw $\beta =\arctan(\cfrac{2(q_1 q_2 + q_0 q_3)}{q_1^2 + q_0^2 - q_3^2 - q_2^2})$

2.3 【代码】Fusion AHRS algorithm

【相同】roll 方向： $\gamma$
【相同】pitch 方向： $\alpha$
【相同】yaw方向： $\beta$

FusionQuaternionToEuler

440 /**
441  * @brief Converts a quaternion to ZYX Euler angles in degrees.
442  * @param quaternion Quaternion.
443  * @return Euler angles in degrees.
444  */
445 static inline FusionEuler FusionQuaternionToEuler(const FusionQuaternion quaternion) {
446 #define Q quaternion.element
447     const float halfMinusQySquared = 0.5f - Q.y * Q.y; // calculate common terms to avoid repeated operations
448     FusionEuler euler;
449     euler.angle.roll = FusionRadiansToDegrees(atan2f(Q.w * Q.x + Q.y * Q.z, halfMinusQySquared - Q.x * Q.x));
450     euler.angle.pitch = FusionRadiansToDegrees(FusionAsin(2.0f * (Q.w * Q.y - Q.z * Q.x)));
451     euler.angle.yaw = FusionRadiansToDegrees(atan2f(Q.w * Q.z + Q.x * Q.y, halfMinusQySquared - Q.z * Q.z));
452     return euler;
453 #undef Q
454 }

从Git代码来看四元数与欧拉角之间的转换关系如下：

Q.w： $q_0$
Q.x： $q_1$
Q.y： $q_2$
Q.z： $q_3$

2.4 【代码】BetaFlight

【相同】roll 方向： $\gamma$
【相同】pitch 方向： $\alpha\rq =\pi/2 - \alpha$
【相同】yaw方向： $\beta$

imuUpdateEulerAngles

STATIC_UNIT_TESTED void imuUpdateEulerAngles(void)
{
    quaternionProducts buffer;

    if (FLIGHT_MODE(HEADFREE_MODE)) {
       imuQuaternionComputeProducts(&headfree, &buffer);

       attitude.values.roll = lrintf(atan2_approx((+2.0f * (buffer.wx + buffer.yz)), (+1.0f - 2.0f * (buffer.xx + buffer.yy))) * (1800.0f / M_PIf));
       attitude.values.pitch = lrintf(((0.5f * M_PIf) - acos_approx(+2.0f * (buffer.wy - buffer.xz))) * (1800.0f / M_PIf));
       attitude.values.yaw = lrintf((-atan2_approx((+2.0f * (buffer.wz + buffer.xy)), (+1.0f - 2.0f * (buffer.yy + buffer.zz))) * (1800.0f / M_PIf)));
    } else {
       attitude.values.roll = lrintf(atan2_approx(rMat[2][1], rMat[2][2]) * (1800.0f / M_PIf));
       attitude.values.pitch = lrintf(((0.5f * M_PIf) - acos_approx(-rMat[2][0])) * (1800.0f / M_PIf));
       attitude.values.yaw = lrintf((-atan2_approx(rMat[1][0], rMat[0][0]) * (1800.0f / M_PIf)));
    }

    if (attitude.values.yaw < 0) {
        attitude.values.yaw += 3600;
    }
}

2.5 补充信息

2.5.1 Body 3-2-1 sequence with Euler angles

【相同】roll 方向： $\gamma$
【相同】pitch 方向： $\alpha$
【相同】yaw方向： $\beta$

Wiki Conversion_between_quaternions_and_Euler_angles给出的不同的转换顺序，其Roll和Pitch正负关系和程序方向一致，但是Yaw正负反向。

2.5.2 Quaternion to Euler angles conversion

【相同】roll 方向： $\gamma$
【相同】pitch 方向： $\alpha$
【相同】yaw方向： $\beta$

注：主要来自文献： Blanco, Jose-Luis (2010). “A tutorial on se (3) transformation parameterizations and on-manifold optimization”. University of Malaga, Tech. Rep. CiteSeerX 10.1.1.468.5407.

3. 欧拉角初始化四元数

3.1 【推导】欧拉角旋转

通过组合欧拉旋转的四元数表示，其初始化过程我们可以这么认为：

飞机在滑行到跑道时首先进行偏航（机身-Z）转弯
然后在起飞时进行俯仰（机身-Y）
最后在空中滚动（机身-X）

该方法采用了Tait–Bryan角描述方式，并通过矢量乘积的方式推导：

$\overrightarrow{Q} = \begin{bmatrix} cos \cfrac{\beta}{2} \\ 0 \\ 0 \\ sin \cfrac{\beta}{2} \end{bmatrix}\begin{bmatrix} cos \cfrac{\alpha}{2} \\ 0 \\ sin \cfrac{\alpha}{2} \\ 0 \end{bmatrix}\begin{bmatrix} cos \cfrac{\gamma}{2} \\ sin \cfrac{\gamma}{2} \\ 0 \\ 0 \end{bmatrix} = \begin{bmatrix} cos \cfrac{\gamma}{2} cos \cfrac{\alpha}{2}cos \cfrac{\beta}{2} + sin \cfrac{\gamma}{2} sin \cfrac{\alpha}{2} sin \cfrac{\beta}{2}\\ sin \cfrac{\gamma}{2} cos \cfrac{\alpha}{2}cos \cfrac{\beta}{2} - cos \cfrac{\gamma}{2} sin \cfrac{\alpha}{2} sin \cfrac{\beta}{2}\\ cos \cfrac{\gamma}{2} sin \cfrac{\alpha}{2} cos \cfrac{\beta}{2} + sin \cfrac{\gamma}{2} cos \cfrac{\alpha}{2} sin \cfrac{\beta}{2}\\ cos \cfrac{\gamma}{2} cos \cfrac{\alpha}{2} sin \cfrac{\beta}{2} - sin \cfrac{\gamma}{2} sin \cfrac{\alpha}{2} cos \cfrac{\beta}{2} \end{bmatrix}$

3.2 【代码】BetaFlight

imuComputeQuaternionFromRPY

static void imuComputeQuaternionFromRPY(quaternionProducts *quatProd, int16_t initialRoll, int16_t initialPitch, int16_t initialYaw)
{
    if (initialRoll > 1800) {
        initialRoll -= 3600;
    }

    if (initialPitch > 1800) {
        initialPitch -= 3600;
    }

    if (initialYaw > 1800) {
        initialYaw -= 3600;
    }

    const float cosRoll = cos_approx(DECIDEGREES_TO_RADIANS(initialRoll) * 0.5f);
    const float sinRoll = sin_approx(DECIDEGREES_TO_RADIANS(initialRoll) * 0.5f);

    const float cosPitch = cos_approx(DECIDEGREES_TO_RADIANS(initialPitch) * 0.5f);
    const float sinPitch = sin_approx(DECIDEGREES_TO_RADIANS(initialPitch) * 0.5f);

    const float cosYaw = cos_approx(DECIDEGREES_TO_RADIANS(-initialYaw) * 0.5f);
    const float sinYaw = sin_approx(DECIDEGREES_TO_RADIANS(-initialYaw) * 0.5f);

    const float q0 = cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw;
    const float q1 = sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw;
    const float q2 = cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw;
    const float q3 = cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw;

    quatProd->xx = sq(q1);
    quatProd->yy = sq(q2);
    quatProd->zz = sq(q3);

    quatProd->xy = q1 * q2;
    quatProd->xz = q1 * q3;
    quatProd->yz = q2 * q3;

    quatProd->wx = q0 * q1;
    quatProd->wy = q0 * q2;
    quatProd->wz = q0 * q3;

    imuComputeRotationMatrix();

    attitudeIsEstablished = true;
}

4. 四元数初始化姿态矩阵

4.1 【定义】姿态矩阵

前面四元数解算姿态过程中已经给出姿态矩阵定义如下：

$C_b^R=\begin{bmatrix} q_1^2 + q_0^2 - q_3^2 - q_2^2 & 2(q_1 q_2 - q_0 q_3) & 2 (q_1 q_3 + q_0 q_2) \\ 2(q_1 q_2 + q_0 q_3) & q_2^2 - q_3^2 + q_0^2 - q_1^2 & 2(q_2 q_3 - q_0 q_1) \\ 2(q_1 q_3 - q_0 q_2) & 2(q_2 q_3 + q_0 q_1) & q_3^2 - q_2^2 - q_1^2 + q_0^2 \end{bmatrix}$

4.2 【代码】BetaFlight

imuComputeRotationMatrix

STATIC_UNIT_TESTED void imuComputeRotationMatrix(void){
    imuQuaternionComputeProducts(&q, &qP);

    rMat[0][0] = 1.0f - 2.0f * qP.yy - 2.0f * qP.zz;
    rMat[0][1] = 2.0f * (qP.xy + -qP.wz);
    rMat[0][2] = 2.0f * (qP.xz - -qP.wy);

    rMat[1][0] = 2.0f * (qP.xy - -qP.wz);
    rMat[1][1] = 1.0f - 2.0f * qP.xx - 2.0f * qP.zz;
    rMat[1][2] = 2.0f * (qP.yz + -qP.wx);

    rMat[2][0] = 2.0f * (qP.xz + -qP.wy);
    rMat[2][1] = 2.0f * (qP.yz - -qP.wx);
    rMat[2][2] = 1.0f - 2.0f * qP.xx - 2.0f * qP.yy;

#if defined(SIMULATOR_BUILD) && !defined(USE_IMU_CALC) && !defined(SET_IMU_FROM_EULER)
    rMat[1][0] = -2.0f * (qP.xy - -qP.wz);
    rMat[2][0] = -2.0f * (qP.xz + -qP.wy);
#endif
}

5. 【BetaFlight】MahonyAHRS

我们最终是期望给出BetaFlight深入传感设计之三：IMU传感模块中关于飞控姿态计算的解释。

回顾下姿态计算任务：

imuUpdateAttitude
 └──> imuCalculateEstimatedAttitude
     ├──> [Gyro & Acc update]
     ├──> [Mag update]
     ├──> imuMahonyAHRSupdate  //基于惯性测量单元（IMU）的方向余弦矩阵（DCM）理论
     └──> imuUpdateEulerAngles  //根据四元数计算姿态角，详见章节四

以下是代码实际处理过程：

imuMahonyAHRSupdate
 ├──> static float integralFBx = 0.0f,  integralFBy = 0.0f, integralFBz = 0.0f    // integral error terms scaled by Ki
 ├──> const float spin_rate = sqrtf(sq(gx) + sq(gy) + sq(gz))  // Calculate general spin rate (rad/s)
 ├──> float ex = 0, ey = 0, ez = 0  // Use raw heading error (from GPS or whatever else)
--- 1.根据GPS的 course over ground计算误差 ---
 ├──> <useCOG>
 │   ├──> <while (courseOverGround >  M_PIf>
 │   │   └──> courseOverGround -= (2.0f * M_PIf)
 │   ├──> <while (courseOverGround < -M_PIf>
 │   │   └──> courseOverGround += (2.0f * M_PIf)
 │   ├──> const float ez_ef = (- sin_approx(courseOverGround) * rMat[0][0] - cos_approx(courseOverGround) * rMat[1][0])
 │   ├──> ex = rMat[2][0] * ez_ef
 │   ├──> ey = rMat[2][1] * ez_ef
 │   └──> ez = rMat[2][2] * ez_ef
--- 2. 使用磁力计计算误差 ---
 ├──> <USE_MAG><useMag && recipMagNorm > 0.01f> // For magnetometer correction we make an assumption that magnetic field is perpendicular to gravity (ignore Z-component in EF). This way magnetic field will only affect heading and wont mess roll/pitch angles
 │   ├──> float mx = mag.magADC[X]
 │   ├──> float my = mag.magADC[Y]
 │   ├──> float mz = mag.magADC[Z]
 │   ├──> float recipMagNorm = sq(mx) + sq(my) + sq(mz)  // Use measured magnetic field vector
 │   ├──> recipMagNorm = invSqrt(recipMagNorm)  // Normalise magnetometer measurement
 │   ├──> mx *= recipMagNorm
 │   ├──> my *= recipMagNorm
 │   ├──> mz *= recipMagNorm
 │   ├──> const float hx = rMat[0][0] * mx + rMat[0][1] * my + rMat[0][2] * mz   // (hx hy 0) - measured mag field vector in EF (assuming Z-component is zero)
 │   ├──> const float hy = rMat[1][0] * mx + rMat[1][1] * my + rMat[1][2] * mz
 │   ├──> const float bx = sqrtf(hx * hx + hy * hy)  // (bx 0 0) - reference mag field vector heading due North in EF (assuming Z-component is zero)
 │   ├──> const float ez_ef = -(hy * bx)   // magnetometer error is cross product between estimated magnetic north and measured magnetic north (calculated in EF)
 │   ├──> ex += rMat[2][0] * ez_ef  // Rotate mag error vector back to BF and accumulate
 │   ├──> ey += rMat[2][1] * ez_ef
 │   └──> ez += rMat[2][2] * ez_ef
--- 3.根据加速度计计算误差 ---
 ├──> <useAcc && recipAccNorm > 0.01f>  // Use measured acceleration vector
 │   ├──> float recipAccNorm = sq(ax) + sq(ay) + sq(az)
 │   ├──> recipAccNorm = invSqrt(recipAccNorm)   // Normalise accelerometer measurement
 │   ├──> ax *= recipAccNorm
 │   ├──> ay *= recipAccNorm
 │   ├──> az *= recipAccNorm
 │   ├──> ex += (ay * rMat[2][2] - az * rMat[2][1])  // Error is sum of cross product between estimated direction and measured direction of gravity
 │   ├──> ey += (az * rMat[2][0] - ax * rMat[2][2])
 │   └──> ez += (ax * rMat[2][1] - ay * rMat[2][0])
--- 4. PI反馈控制 ---
 ├──> <imuRuntimeConfig.dcm_ki > 0.0f>  // Compute and apply integral feedback if enabled
 │   └──> <spin_rate < DEGREES_TO_RADIANS(SPIN_RATE_LIMIT)>   // Stop integrating if spinning beyond the certain limit
 │       ├──> const float dcmKiGain = imuRuntimeConfig.dcm_ki
 │       ├──> integralFBx += dcmKiGain * ex * dt    // integral error scaled by Ki
 │       ├──> integralFBy += dcmKiGain * ey * dt
 │       └──> integralFBz += dcmKiGain * ez * dt
 ├──> <else> // prevent integral windup
 │   ├──> integralFBx = 0.0f    
 │   ├──> integralFBy = 0.0f
 │   └──> integralFBz = 0.0f
 ├──> [Apply proportional and integral feedback]
 │   ├──> gx += dcmKpGain * ex + integralFBx 
 │   ├──> gy += dcmKpGain * ey + integralFBy
 │   └──> gz += dcmKpGain * ez + integralFBz
--- 5. 计算新四元数(定时采样增量法) ---
 ├──> [Integrate rate of change of quaternion]
 │   ├──> gx *= (0.5f * dt)
 │   ├──> gy *= (0.5f * dt)
 │   └──> gz *= (0.5f * dt)
 ├──> quaternion buffer
 ├──> buffer.w = q.w
 ├──> buffer.x = q.x
 ├──> buffer.y = q.y
 ├──> buffer.z = q.z
 ├──> q.w += (-buffer.x * gx - buffer.y * gy - buffer.z * gz)
 ├──> q.x += (+buffer.w * gx + buffer.y * gz - buffer.z * gy)
 ├──> q.y += (+buffer.w * gy - buffer.x * gz + buffer.z * gx)
 ├──> q.z += (+buffer.w * gz + buffer.x * gy - buffer.y * gx)
--- 重新归一化四元数 ---
 ├──> [Normalise quaternion]
 │   ├──> float recipNorm = invSqrt(sq(q.w) + sq(q.x) + sq(q.y) + sq(q.z))
 │   ├──> q.w *= recipNorm
 │   ├──> q.x *= recipNorm
 │   ├──> q.y *= recipNorm
 │   └──> q.z *= recipNorm
--- 重新计算飞控姿态矩阵 ---
 ├──> imuComputeRotationMatrix()  // Pre-compute rotation matrix from quaternion
 └──> attitudeIsEstablished = true

我们将结合William Premerlani和Paul Bizard的direction-cosine-matrix-imu-theory来分析下这段代码，期望能够获得比较好的理解。

5.1 GPS方向误差修正

根据William Premerlani和Paul Bizard的direction-cosine-matrix-imu-theory，可以看到：

YawCorrectionGround公式有出入???
YawCorrectionPlane公式一致

注：实际过程发现William Premerlani和Paul Bizard的direction-cosine-matrix-imu-theory和这部分代码也有一定差距，后续还需要进一步深入研究。对于有了解的同学也请评论留言，以便我们更快的修正稳重描述。

注：有兴趣的朋友可以看下这里的分析BetaFlight深入传感设计之九：传感坐标系/机体坐标系/导航坐标系/经纬度坐标系

--- 1.根据GPS的 course over ground计算误差 ---
 ├──> <useCOG>
 │   ├──> <while (courseOverGround >  M_PIf>
 │   │   └──> courseOverGround -= (2.0f * M_PIf)
 │   ├──> <while (courseOverGround < -M_PIf>
 │   │   └──> courseOverGround += (2.0f * M_PIf)
 │   ├──> const float ez_ef = (- sin_approx(courseOverGround) * rMat[0][0] - cos_approx(courseOverGround) * rMat[1][0])
 │   ├──> ex = rMat[2][0] * ez_ef
 │   ├──> ey = rMat[2][1] * ez_ef
 │   └──> ez = rMat[2][2] * ez_ef

GPS error

5.2 磁力计误差修正

在William Premerlani和Paul Bizard的direction-cosine-matrix-imu-theory没有找到对应的计算公式。

通过分析：

Step1：获取芯片地磁采集数据：
$\overrightarrow{M} = \begin{bmatrix} m_x \\ m_y \\ m_z \end{bmatrix}$
Step 2：归一化矢量
Step 3：根据飞控姿态，旋转地磁矢量
$\overrightarrow{H} = \begin{bmatrix} h_x \\ h_y \\ h_z \end{bmatrix}$
Step4：计算指北矢量(考虑地球磁场模型，且忽略z轴方向)
$\overrightarrow{B} = \begin{bmatrix} b_x \\ 0 \\ 0 \end{bmatrix}$
Step5：地磁误差为计算的指北矢量与实际地磁指向矢量叉乘结果
$\overrightarrow{Err} = \overrightarrow{H} \times \overrightarrow{B} = \begin{bmatrix}
h_x \
h_y \
h_z
\end{bmatrix}\begin{bmatrix}
b_x \
0 \
0
\end{bmatrix} $
Step6：套用前面的矢量叉乘公式

$(\overrightarrow{H} \times \overrightarrow{B} )_x = H_yB_z - H_zB_y = 0$
$(\overrightarrow{H} \times \overrightarrow{B} )_y = H_zB_x - H_xB_z = 0$
$(\overrightarrow{H} \times \overrightarrow{B} )_z = H_xB_y - H_yB_x = -h_yb_x$

Step7：最后误差旋转到机体坐标系。

--- 2. 使用磁力计计算误差 ---
 ├──> <USE_MAG><useMag && recipMagNorm > 0.01f> // For magnetometer correction we make an assumption that magnetic field is perpendicular to gravity (ignore Z-component in EF). This way magnetic field will only affect heading and wont mess roll/pitch angles
 │   ├──> float mx = mag.magADC[X]
 │   ├──> float my = mag.magADC[Y]
 │   ├──> float mz = mag.magADC[Z]
 │   ├──> float recipMagNorm = sq(mx) + sq(my) + sq(mz)  // Use measured magnetic field vector
 │   ├──> recipMagNorm = invSqrt(recipMagNorm)  // Normalise magnetometer measurement
 │   ├──> mx *= recipMagNorm
 │   ├──> my *= recipMagNorm
 │   ├──> mz *= recipMagNorm
 │   ├──> const float hx = rMat[0][0] * mx + rMat[0][1] * my + rMat[0][2] * mz   // (hx hy 0) - measured mag field vector in EF (assuming Z-component is zero)
 │   ├──> const float hy = rMat[1][0] * mx + rMat[1][1] * my + rMat[1][2] * mz
 │   ├──> const float bx = sqrtf(hx * hx + hy * hy)  // (bx 0 0) - reference mag field vector heading due North in EF (assuming Z-component is zero)
 │   ├──> const float ez_ef = -(hy * bx)   // magnetometer error is cross product between estimated magnetic north and measured magnetic north (calculated in EF)
 │   ├──> ex += rMat[2][0] * ez_ef  // Rotate mag error vector back to BF and accumulate
 │   ├──> ey += rMat[2][1] * ez_ef
 │   └──> ez += rMat[2][2] * ez_ef

5.3 加速度误差修正

根据William Premerlani和Paul Bizard的direction-cosine-matrix-imu-theory和矢量叉乘计算方法可以活的如下计算公式。

--- 3.根据加速度计计算误差 ---
 ├──> <useAcc && recipAccNorm > 0.01f>  // Use measured acceleration vector
 │   ├──> float recipAccNorm = sq(ax) + sq(ay) + sq(az)
 │   ├──> recipAccNorm = invSqrt(recipAccNorm)   // Normalise accelerometer measurement
 │   ├──> ax *= recipAccNorm
 │   ├──> ay *= recipAccNorm
 │   ├──> az *= recipAccNorm
 │   ├──> ex += (ay * rMat[2][2] - az * rMat[2][1])  // Error is sum of cross product between estimated direction and measured direction of gravity
 │   ├──> ey += (az * rMat[2][0] - ax * rMat[2][2])
 │   └──> ez += (ax * rMat[2][1] - ay * rMat[2][0])

在这里插入图片描述

acc error

5.4 PI反馈控制

在William Premerlani和Paul Bizard的direction-cosine-matrix-imu-theory里面没有比较详细的介绍。

--- 4. PI反馈控制 ---
 ├──> <imuRuntimeConfig.dcm_ki > 0.0f>  // Compute and apply integral feedback if enabled
 │   └──> <spin_rate < DEGREES_TO_RADIANS(SPIN_RATE_LIMIT)>   // Stop integrating if spinning beyond the certain limit
 │       ├──> const float dcmKiGain = imuRuntimeConfig.dcm_ki
 │       ├──> integralFBx += dcmKiGain * ex * dt    // integral error scaled by Ki
 │       ├──> integralFBy += dcmKiGain * ey * dt
 │       └──> integralFBz += dcmKiGain * ez * dt
 ├──> <else> // prevent integral windup
 │   ├──> integralFBx = 0.0f    
 │   ├──> integralFBy = 0.0f
 │   └──> integralFBz = 0.0f
 ├──> [Apply proportional and integral feedback]
 │   ├──> gx += dcmKpGain * ex + integralFBx 
 │   ├──> gy += dcmKpGain * ey + integralFBy
 │   └──> gz += dcmKpGain * ez + integralFBz

5.5 毕卡算法（一阶定时采样增量法）

--- 5. 计算新四元数(定时采样增量法) ---
 ├──> [Integrate rate of change of quaternion]
 │   ├──> gx *= (0.5f * dt)
 │   ├──> gy *= (0.5f * dt)
 │   └──> gz *= (0.5f * dt)
 ├──> quaternion buffer
 ├──> buffer.w = q.w
 ├──> buffer.x = q.x
 ├──> buffer.y = q.y
 ├──> buffer.z = q.z
 ├──> q.w += (-buffer.x * gx - buffer.y * gy - buffer.z * gz)
 ├──> q.x += (+buffer.w * gx + buffer.y * gz - buffer.z * gy)
 ├──> q.y += (+buffer.w * gy - buffer.x * gz + buffer.z * gx)
 ├──> q.z += (+buffer.w * gz + buffer.x * gy - buffer.y * gx)

毕卡算法
一阶近似算法

6. 参考资料

【1】BetaFlight深入传感设计：传感模块设计框架
【2】BetaFlight深入传感设计之一：Baro传感模块
【3】BetaFlight深入传感设计之二：Mag传感模块
【4】BetaFlight深入传感设计之三：IMU传感模块
【5】BetaFlight深入传感设计之四：GPS传感模块
【6】Mahony姿态解算算法笔记（一）
【7】Mahony姿态解算算法笔记（二）

7. 补充：基于惯性测量单元（IMU）的方向余弦矩阵（DCM）理论

在这里插入图片描述

lida2003

关注

3
点赞
踩
5

收藏

觉得还不错? 一键收藏
2
评论
BetaFlight深入传感设计之五：MahonyAHRS & 方向余弦矩阵理论

该理论主要来自William Premerlani和Paul Bizard的Direction Cosine Matrix IMU: Theory，这是出处。大家除了去看英文原文资料，关于中文方面的翻译github上有链接这里提供，有兴趣的朋友可以去仔细研究。鉴于前面对Baro/Mag/IMU/GPS传感模块的驱动以及业务方面的研读，我们遇到一个比较蹩脚的问题，就是如何基于这些传感对飞行方向进行校准。虽然基础知识都有，但是真的在实际应用上相较于GPS距离，飞行速度，飞行方向等几何学知识来讲，方向余弦矩阵稍微
复制链接

扫一扫

专栏目录