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

Posted 2022-10-27 lida2003

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

• 1. 基础预备知识
• • 1.1 机体坐标系
  • 1.2 欧拉角
  • • 1.2.1 概念解释
    • 1.2.2 动态概念
    • 1.2.3 应用概念
  • 1.3飞控操作
  • • 1.3.1 Yaw
    • 1.3.2 Pitch
    • 1.3.3 Roll
  • 1.3 旋转矩阵
  • • 1.3.1 二维空间
    • 1.3.2 三维空间
  • 1.4 复数
  • 1.5 四元数
  • • 1.5.1 约束条件
    • 1.5.2 矢量加减
    • 1.5.3 标量乘法
    • 1.5.4 矢量乘法
    • 1.5.5 除法求逆
  • 1.6 矢量点叉乘
  • • 1.6.1 向量点乘
    • 1.6.2 向量叉乘
• 2. 四元数解算姿态
• • 2.1 【推导】理论(教科书)
  • 2.2 【推导】旋转矩阵+姿态矩阵
  • 2.3 【代码】Fusion AHRS algorithm
  • 2.4 【代码】BetaFlight
  • 2.5 补充信息
  • • 2.5.1 Body 3-2-1 sequence with Euler angles
    • 2.5.2 Quaternion to Euler angles conversion
• 3. 欧拉角初始化四元数
• • 3.1 【推导】欧拉角旋转
  • 3.2 【代码】BetaFlight
• 4. 四元数初始化姿态矩阵
• • 4.1 【定义】姿态矩阵
  • 4.2 【代码】BetaFlight
• 5. 【BetaFlight】MahonyAHRS
• • 5.1 GPS方向误差修正
  • 5.2 磁力计误差修正
  • 5.3 加速度误差修正
  • 5.4 PI反馈控制
• 6. 参考资料
• 7. 补充：基于惯性测量单元（IMU）的方向余弦矩阵（DCM）理论

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

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

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

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

1. 基础预备知识

1.1 机体坐标系

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

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

1.2 欧拉角

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

转换到飞行器上，角度对应关系如下：

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

1.2.1 概念解释

1.2.2 动态概念

1.2.3 应用概念

1.3飞控操作

1.3.1 Yaw

1.3.2 Pitch

1.3.3 Roll

1.3 旋转矩阵

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

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

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

1.3.1 二维空间

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

$M(\theta )=\left[\begin{array}{cc}cos\theta & -sin\theta \\ sin\theta & cos\theta \end{array}\right]=cos\theta \left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]+sin\theta \left[\begin{array}{cc}0& -1\\ 1& 0\end{array}\right]=exp(\theta \left[\begin{array}{cc}0& -1\\ 1& 0\end{array}\right])$

1.3.2 三维空间

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

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

$R_x({\theta }_x)=\left[\begin{array}{ccc}1& 0& 0\\ 0& cos{\theta }_x& -sin{\theta }_x\\ 0& sin{\theta }_x& cos{\theta }_x\end{array}\right]=\left[\begin{array}{ccc}1& 0& 0\\ 0& cos\gamma & -sin\gamma \\ 0& sin\gamma & cos\gamma \end{array}\right]$

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

$R_y({\theta }_y)=\left[\begin{array}{ccc}cos{\theta }_y& 0& sin{\theta }_y\\ 0& 1& 0\\ -sin{\theta }_y& 0& cos{\theta }_y\end{array}\right]=\left[\begin{array}{ccc}cos\alpha & 0& sin\alpha \\ 0& 1& 0\\ -sin\alpha & 0& cos\alpha \end{array}\right]$

• 绕 z-轴的主动旋转${\theta }_z$, 对应yaw($\beta$) 角，和右手螺旋的方向相反

  以上是关于BetaFlight深入传感设计之五：MahonyAHRS & 方向余弦矩阵理论的主要内容，如果未能解决你的问题，请参考以下文章

  BetaFlight深入传感设计：传感模块设计框架

  BetaFlight深入传感设计之二：Mag传感模块

  BetaFlight深入传感设计之二：Mag传感模块

  BetaFlight深入传感设计之四：GPS传感模块

  BetaFlight深入传感设计之三：IMU传感模块

  BetaFlight深入传感设计之三：IMU传感模块