BetaFlight深入传感设计之五:MahonyAHRS & 方向余弦矩阵理论
该理论主要来自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 概念解释
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 ( θ ) = [ c o s θ − s i n θ s i n θ c o s θ ] = c o s θ [ 1 0 0 1 ] + s i n θ [ 0 − 1 1 0 ] = e x p ( θ [ 0 − 1 1 0 ] ) 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 M(θ)=[cosθsinθ−sinθcosθ]=cosθ[1001]+sinθ[01−10]=exp(θ[01−10])
1.3.2 三维空间
在三维空间中,生成旋转矩阵的一种简单方式是把它作为三个基本旋转的序列复合。关于右手笛卡尔坐标系的 x, y和 z轴的旋转分别叫做 roll, pitch 和 yaw 旋转。
- 绕 x-轴的主动旋转 θ x \theta_x θx, 对应roll( γ \gamma γ) 角,和右手螺旋的方向相反(在yz平面顺时针)。
R x ( θ x ) = [ 1 0 0 0 c o s θ x − s i n θ x 0 s i n θ x c o s θ x ] = [ 1 0 0 0 c o s γ − s i n γ 0 s i n γ c o s γ ] 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} Rx(θx)= 1000cosθxsinθx0−sinθxcosθx = 1000cosγsinγ0−sinγcosγ
- 绕 y-轴的主动旋转 θ y \theta_y θy, 对应pitch( α \alpha α) 角,和右手螺旋的方向相反(在zx平面顺时针)。
R y ( θ y ) = [ c o s θ y 0 s i n θ y 0 1 0 − s i n θ y 0 c o s θ y ] = [ c o s α 0 s i n α 0 1 0 − s i n α 0 c o s α ] 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} Ry(θy)= cosθy0−sinθy010sinθy0cosθy = cosα0−sinα010sinα0cosα
- 绕 z-轴的主动旋转 θ z \theta_z θz, 对应yaw( β \beta β) 角,和右手螺旋的方向相反(在xy平面顺时针)。
R z ( θ z ) = [ c o s θ z − s i n θ z 0 s i n θ z c o s θ z 0 0 0 1 ] = [ c o s β − s i n β 0 s i n β c o s β 0 0 0 1 ] 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} Rz(θz)= cosθzsinθz0−sinθzcosθz0001 = cosβsinβ0−sinβcosβ0001
- 总的旋转矩阵(3-2-1旋转)可以表达为:
M b b ′ ( θ z y x ) = R z ( θ z ) R y ( θ y ) R x ( θ x ) = R z ( β ) R y ( α ) R x ( γ ) = [ c o s β c o s α − s i n β c o s γ + c o s β s i n α s i n γ s i n β s i n γ + c o s β s i n α c o s γ s i n β c o s α c o s β c o s γ + s i n β s i n α s i n γ − c o s β s i n γ + s i n β s i n α c o s γ − s i n α c o s α s i n γ c o s α c o s γ ] 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} Mbb′(θzyx)=Rz(θz)Ry(θy)Rx(θx)=Rz(β)Ry(α)Rx(γ)= cosβcosαsinβcosα−sinα−sinβcosγ+cosβsinαsinγcosβcosγ+sinβsinαsinγcosαsinγsinβsinγ+cosβsinαcosγ−cosβsinγ+sinβsinαcosγcosαcosγ
1.4 复数
高中数学学过复数,复数可以在复平面(二维平面)内表达为:
z → = r ⋅ c o s θ + i ⋅ r ⋅ s i n θ = a + b ⋅ i \overrightarrow{z} = r \cdot cos \theta + i \cdot r \cdot sin \theta = a + b \cdot i z=r⋅cosθ+i⋅r⋅sinθ=a+b⋅i
其中: ∣ z → ∣ = r = a 2 + b 2 \vert \overrightarrow{z} \vert = r = \sqrt{a^2 + b^2} ∣z∣=r=a2+b2
1.5 四元数
四元数实际上是在一个超三维空间的表达:
Q → = q 0 + q 1 ⋅ i + q 2 ⋅ j + q 3 ⋅ k = c o s θ 2 + u → s i n θ 2 = c o s θ 2 + ( l ⋅ i + m ⋅ j + n ⋅ k ) s i n θ 2 \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} Q=q0+q1⋅i+q2⋅j+q3⋅k=cos2θ+usin2θ=cos2θ+(l⋅i+m⋅j+n⋅k)sin2θ
模长: ∣ Q → ∣ = q 0 2 + q 1 2 + q 2 2 + q 3 2 \vert \overrightarrow{Q} \vert = \sqrt{q_0^2 + q_1^2 + q_2^2 + q_3^2} ∣Q∣=q02+q12+q22+q32, θ 为实数, u → 为单位向量。 \theta为实数,\overrightarrow{u}为单位向量。 θ为实数,u为单位向量。
注:复数是四元数的特殊场景, q 2 = q 3 = 0 q_2 = q_3 =0 q2=q3=0。
1.5.1 约束条件
- 【满足】分配律
- 【满足】结合律
- 【不满足】交换律
1.5.2 矢量加减
Q → ± P → = ( q 0 + q 1 ⋅ i + q 2 ⋅ j + q 3 ⋅ k ) ± ( p 0 + p 1 ⋅ i + p 2 ⋅ j + p 3 ⋅ k ) = ( q 0 ± p 0 ) + ( q 1 ± p 1 ) ⋅ i + ( q 2 ± p 2 ) ⋅ j + ( q 3 ± p 3 ) ⋅ k \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 Q±P=(q0+q1⋅i+q2⋅j+q3⋅k)±(p0+p1⋅i+p2⋅j+p3⋅k)=(q0±p0)+(q1±p1)⋅i+(q2±p2)⋅j+(q3±p3)⋅k
1.5.3 标量乘法
a ⋅ Q → = a ⋅ q 0 + a ⋅ q 1 ⋅ i + a ⋅ q 2 ⋅ j + a ⋅ q 3 ⋅ k 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 a⋅Q=a⋅q0+a⋅q1⋅i+a⋅q2⋅j+a⋅q3⋅k
1.5.4 矢量乘法
P → ⊗ Q → = ( p 0 + p 1 ⋅ i + p 2 ⋅ j + p 3 ⋅ k ) ⊗ ( q 0 + q 1 ⋅ i + q 2 ⋅ j + q 3 ⋅ k ) = ( p 0 q 0 − p 1 q 1 − p 2 q 2 − p 3 q 3 ) + ( p 0 q 1 + p 1 q 0 + p 2 q 3 − p 3 q 2 ) ⋅ i + ( p 0 q 2 + p 2 q 0 + p 3 q 1 − p 1 q 3 ) ⋅ j + ( p 0 q 3 + p 3 q 0 + p 1 q 2 − p 2 q 1 ) ⋅ k = r 0 + r 1 ⋅ i + r 2 ⋅ j + r 3 ⋅ k \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 P⊗Q=(p0+p1⋅i+p2⋅j+p3⋅k)⊗(q0+q1⋅i+q2⋅j+q3⋅k)=(p0q0−p1q1−p2q2−p3q3)+(p0q1+p1q0+p2q3−p3q2)⋅i+(p0q2+p2q0+p3q1−p1q3)⋅j+(p0q3+p3q0+p1q2−p2q1)⋅k=r0+r1⋅i+r2⋅j+r3⋅k
换成矩阵表达式(便于后续计算机计算):
[ r 0 r 1 r 2 r 3 ] = [ 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 ] = M ( P ) Q \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 r0r1r2r3 = p0p1p2p3−p1p0p3−p2−p2−p3p0p1−p3p2−p1p0 q0q1q2q3 =M(P)Q
[ r 0 r 1 r 2 r 3 ] = [ 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 ] = M ′ ( Q ) P \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 r0r1r2r3 = q0q1q2q3−q1q0−q3q2−q2q3q0−q1−q3−q2q1q0 p0p1p2p3 =M′(Q)P
1.5.5 除法求逆
P → ⊗ R → = 1 \overrightarrow{P} \otimes \overrightarrow{R} = 1 P⊗R=1
将记作: R → = P − 1 → = P ∗ → \overrightarrow{R} = \overrightarrow{P^{-1}}=\overrightarrow{P^*} R=P−1=P∗
P → ⊗ P ∗ → = ( p 0 + p 1 ⋅ i + p 2 ⋅ j + p 3 ⋅ k ) ⊗ ( p 0 − p 1 ⋅ i − p 2 ⋅ j − p 3 ⋅ k ) = q 0 2 + q 1 2 + q 2 2 + q 3 2 = ∥ 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 P⊗P∗=(p0+p1⋅i+p2⋅j+p3⋅k)⊗(p0−p1⋅i−p2⋅j−p3⋅k)=q02+q12+q22+q32=∥P∥
所以 P − 1 → = P ∗ → ∥ P ∥ \overrightarrow{P^{-1}} =\cfrac{\overrightarrow{P^*}}{ \rVert P \rVert} P−1=∥P∥P∗
1.6 矢量点叉乘
1.6.1 向量点乘
A → ⋅ B → = ∣ A ∣ ∣ B ∣ c o s θ A B = A x B x + A y B y + A z B z \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 A⋅B=∣A∣∣B∣cosθAB=AxBx+AyBy+AzBz
1.6.2 向量叉乘
A → × B → = − B → × A → \overrightarrow{A} \times \overrightarrow{B}=-\overrightarrow{B} \times \overrightarrow{A} A×B=−B×A
∣ A → × B → ∣ = ∣ A ∣ ∣ B ∣ s i n θ A B \rvert \overrightarrow{A} \times \overrightarrow{B} \rvert =\rvert A \rvert \rvert B \rvert sin \theta_{AB} ∣A×B∣=∣A∣∣B∣sinθAB
A → × B → = ( A → × B → ) x + ( A → × B → ) y + ( A → × B → ) z \overrightarrow{A} \times \overrightarrow{B} = (\overrightarrow{A} \times \overrightarrow{B} )_x + (\overrightarrow{A} \times \overrightarrow{B} )_y + (\overrightarrow{A} \times \overrightarrow{B} )_z A×B=(A×B)x+(A×B)y+(A×B)z
(
A
→
×
B
→
)
x
=
A
y
B
z
−
A
z
B
y
(\overrightarrow{A} \times \overrightarrow{B} )_x = A_yB_z - A_zB_y
(A×B)x=AyBz−AzBy
(
A
→
×
B
→
)
y
=
A
z
B
x
−
A
x
B
z
(\overrightarrow{A} \times \overrightarrow{B} )_y = A_zB_x - A_xB_z
(A×B)y=AzBx−AxBz
(
A
→
×
B
→
)
z
=
A
x
B
y
−
A
y
B
x
(\overrightarrow{A} \times \overrightarrow{B} )_z = A_xB_y - A_yB_x
(A×B)z=AxBy−AyBx
2. 四元数解算姿态
2.1 【推导】理论(教科书)
注:下面我们引用教科书:【惯性导航/秦永元,背景,科学出版社,2006,ISBN:7-03-016428-8】的推导过程。
2.2 【推导】旋转矩阵+姿态矩阵
- 机体坐标向R固定系坐标映射:
C b R = [ T 11 T 12 T 13 T 21 T 22 T 23 T 31 T 32 T 33 ] = [ C x x C x y C x z C y x C y y C y z C z x C z y C z z ] = [ 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 ] 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} CbR= T11T21T31T12T22T32T13T23T33 = CxxCyxCzxCxyCyyCzyCxzCyzCzz = q12+q02−q32−q222(q1q2+q0q3)2(q1q3−q0q2)2(q1q2−q0q3)q22−q32+q02−q122(q2q3+q0q1)2(q1q3+q0q2)2(q2q3−q0q1)q32−q22−q12+q02
C b R T = [ T 11 T 21 T 31 T 12 T 22 T 32 T 13 T 23 T 33 ] = [ C x x C y x C z x C x y C y y C z y C x z C y z C z z ] = [ 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 ] {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} CbRT= T11T12T13T21T22T23T31T32T33 = CxxCxyCxzCyxCyyCyzCzxCzyCzz = q12+q02−q32−q222(q1q2−q0q3)2(q1q3+q0q2)2(q1q2+q0q3)q22−q32+q02−q122(q2q3−q0q1)2(q1q3−q0q2)2(q2q3+q0q1)q32−q22−q12+q02
- 三维空间旋转矩阵逆转换:
M b b ′ ( θ z y x ) = R z ( θ z ) R y ( θ y ) R x ( θ x ) = R z ( β ) R y ( α ) R x ( γ ) = [ c o s β c o s α − s i n β c o s γ + c o s β s i n α s i n γ s i n β s i n γ + c o s β s i n α c o s γ s i n β c o s α c o s β c o s γ + s i n β s i n α s i n γ − c o s β s i n γ + s i n β s i n α c o s γ − s i n α c o s α s i n γ c o s α c o s γ ] 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} Mbb′(θzyx)=Rz(θz)Ry(θy)Rx(θx)=Rz(β)Ry(α)Rx(γ)= cosβcosαsinβcosα−sinα−sinβcosγ+cosβsinαsinγcosβcosγ+sinβsinαsinγcosαsinγsinβsinγ+cosβsinαcosγ−cosβsinγ+sinβsinαcosγcosαcosγ
- 由上面两式可以推导出以下角度姿态公式:
- roll γ = arctan ( 2 ( q 2 q 3 + q 0 q 1 ) q 3 2 − q 2 2 − q 1 2 + q 0 2 ) \gamma =\arctan(\cfrac{2(q_2 q_3 + q_0 q_1)}{q_3^2 - q_2^2 - q_1^2 + q_0^2}) γ=arctan(q32−q22−q12+q022(q2q3+q0q1))
- pitch α = arcsin ( 2 ( q 0 q 2 − q 1 q 3 ) ) \alpha = \arcsin(2(q_0 q_2 - q_1 q_3)) α=arcsin(2(q0q2−q1q3))
- yaw β = arctan ( 2 ( q 1 q 2 + q 0 q 3 ) q 1 2 + q 0 2 − q 3 2 − q 2 2 ) \beta =\arctan(\cfrac{2(q_1 q_2 + q_0 q_3)}{q_1^2 + q_0^2 - q_3^2 - q_2^2}) β=arctan(q12+q02−q32−q222(q1q2+q0q3))
2.3 【代码】Fusion AHRS algorithm
- 【相同】roll 方向: γ \gamma γ
- 【相同】pitch 方向: α \alpha α
- 【相同】yaw方向: β \beta β
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_0 q0
- Q.x: q 1 q_1 q1
- Q.y: q 2 q_2 q2
- Q.z: q 3 q_3 q3
2.4 【代码】BetaFlight
- 【相同】roll 方向: γ \gamma γ
- 【相同】pitch 方向: α ′ = π / 2 − α \alpha\rq =\pi/2 - \alpha α′=π/2−α
- 【相同】yaw方向: β \beta β
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角描述方式,并通过矢量乘积的方式推导:
Q → = [ c o s β 2 0 0 s i n β 2 ] [ c o s α 2 0 s i n α 2 0 ] [ c o s γ 2 s i n γ 2 0 0 ] = [ c o s γ 2 c o s α 2 c o s β 2 + s i n γ 2 s i n α 2 s i n β 2 s i n γ 2 c o s α 2 c o s β 2 − c o s γ 2 s i n α 2 s i n β 2 c o s γ 2 s i n α 2 c o s β 2 + s i n γ 2 c o s α 2 s i n β 2 c o s γ 2 c o s α 2 s i n β 2 − s i n γ 2 s i n α 2 c o s β 2 ] \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} Q= cos2β00sin2β cos2α0sin2α0 cos2γsin2γ00 = cos2γcos2αcos2β+sin2γsin2αsin2βsin2γcos2αcos2β−cos2γsin2αsin2βcos2γsin2αcos2β+sin2γcos2αsin2βcos2γcos2αsin2β−sin2γsin2αcos2β
3.2 【代码】BetaFlight
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 = [ 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 ] 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} CbR= q12+q02−q32−q222(q1q2+q0q3)2(q1q3−q0q2)2(q1q2−q0q3)q22−q32+q02−q122(q2q3+q0q1)2(q1q3+q0q2)2(q2q3−q0q1)q32−q22−q12+q02
4.2 【代码】BetaFlight
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
5.2 磁力计误差修正
在William Premerlani和Paul Bizard的direction-cosine-matrix-imu-theory没有找到对应的计算公式。
通过分析:
-
Step1: 获取芯片地磁采集数据:
M → = [ m x m y m z ] \overrightarrow{M} = \begin{bmatrix} m_x \\ m_y \\ m_z \end{bmatrix} M= mxmymz -
Step 2:归一化矢量
-
Step 3:根据飞控姿态,旋转地磁矢量
H → = [ h x h y h z ] \overrightarrow{H} = \begin{bmatrix} h_x \\ h_y \\ h_z \end{bmatrix} H= hxhyhz -
Step4:计算指北矢量(考虑地球磁场模型,且忽略z轴方向)
B → = [ b x 0 0 ] \overrightarrow{B} = \begin{bmatrix} b_x \\ 0 \\ 0 \end{bmatrix} B= bx00
-
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:套用前面的矢量叉乘公式
(
H
→
×
B
→
)
x
=
H
y
B
z
−
H
z
B
y
=
0
(\overrightarrow{H} \times \overrightarrow{B} )_x = H_yB_z - H_zB_y = 0
(H×B)x=HyBz−HzBy=0
(
H
→
×
B
→
)
y
=
H
z
B
x
−
H
x
B
z
=
0
(\overrightarrow{H} \times \overrightarrow{B} )_y = H_zB_x - H_xB_z = 0
(H×B)y=HzBx−HxBz=0
(
H
→
×
B
→
)
z
=
H
x
B
y
−
H
y
B
x
=
−
h
y
b
x
(\overrightarrow{H} \times \overrightarrow{B} )_z = H_xB_y - H_yB_x = -h_yb_x
(H×B)z=HxBy−HyBx=−hybx
- 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])
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姿态解算算法笔记(二)