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

Posted 2022-11-11 lida2003

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

• 1. 四元数理论
• • 1.1 定义
  • 1.2 基本运算
  • • 1.2.1 矢量加减
    • 1.2.2 标量乘法
  • 1.3 矢量点叉乘
  • • 1.3.1 矢量点乘
    • 1.3.2 矢量叉乘
  • 1.4 除法求逆
• 2. 程序四元素
• • 2.1 四元数定义
  • 2.2 四元数初始化
  • • 2.2.1 单位标量初始化
    • 2.2.2 矢量初始化
  • 2.3 表达转换
  • • 2.3.1 复数式转三角式
    • 2.3.2 三角式转复数式
  • 2.4 基本运算
  • • 2.4.1 矢量加法
    • 2.4.2 标量乘法
  • 2.5 矢量点叉乘
  • • 2.5.1 矢量点乘
    • 2.5.2 矢量叉乘
  • 2.6 零标量四元数旋转
  • • 2.6.1 CW方向旋转
    • 2.6.2 CCW方向旋转
  • 2.7 其他运算
  • • 2.7.1 四元素模平方
    • 2.7.2 四元素求厄
    • 2.7.3 四元素归一化
• 3. 机体坐标系和地球坐标系转换
• • 3.1 机体坐标系向地球坐标系旋转
  • 3.2 地球坐标系向机体坐标系旋转
• 4. 总结
• 5. 补充资料
• • 5.1 ENU Coordinates
  • 5.2 NED Coordinates
  • 5.3 Earth-Centered Earth-Fixed Coordinates
  • 5.4 Azimuth-Elevation-Range Coordinates
• 6. 参考资料

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

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

1. 四元数理论

1.1 定义

1. 矢量式

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

2. 复数式

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

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

3. 三角式

$\overrightarrow{Q}=cos\frac{\theta }{2}+\overrightarrow{u}sin\frac{\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 标量乘法

$a\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}=|P||Q|cos{\theta }_{PQ}$

$\overrightarrow{P}\cdot \overrightarrow{Q}=(p_0+p_1\cdot i+p_2$