Madgwick AHRS算法笔记

Madgwick AHRS算法笔记

引言

最近项目需要搞AHRS，重新读了一下Madgwick的AHRS算法论文[1]，在这里把笔记整理如下，如有错误，欢迎指正。

坐标系

1. 地理坐标系

地理坐标系原点位于载体质心，通常选取东北天坐标系，即其 x x 轴指向水平东方，其 y y 轴指向水平北方，其 z z 轴沿地垂线向上。

2. 载体坐标系

载体坐标系原点位于载体质心，通常选取右前上坐标系，即其 x x 轴沿载体横轴向右，其 y y 轴沿载体纵轴向前， 其 z z 轴沿载体竖轴向上。

四元数

1. 四元数基本理论

1.1 定义

四元数是一种四维超复数，由一个实数单位加上三个虚数单位 i i ， j j ， k k 组成，其数学表达式如下：

q̂ =q1+q2i+q3j+q4k=[q1q2q3q4],i2=j2=k2=ijk=−1(1) (1) q ^ = q 1 + q 2 i + q 3 j + q 4 k = [ q 1 q 2 q 3 q 4 ] , i 2 = j 2 = k 2 = i j k = − 1

其中实数部分为四元数中的标量部分，虚数部分为四元数的矢量部分。

1.2 运算

• 模值

  ∣∣q̂ ∣∣=q21+q22+q23+q24(2) (2) | q ^ | = q 1 2 + q 2 2 + q 3 2 + q 4 2

• 共轭

  q̂ ∗=[q1−q2−q3−q4](3) (3) q ^ ∗ = [ q 1 − q 2 − q 3 − q 4 ]

• 叉乘

  â ⨂b̂ =⎡⎣⎢⎢⎢⎢a1b1−a2b2−a3b3−a4b4a1b2+a2b1+a3b4−a4b3a1b3−a2b4+a3b1+a4b2a1b4+a2b3−a3b2+a4b1⎤⎦⎥⎥⎥⎥T(4) (4) a ^ ⨂ b ^ = [ a 1 b 1 − a 2 b 2 − a 3 b 3 − a 4 b 4 a 1 b 2 + a 2 b 1 + a 3 b 4 − a 4 b 3 a 1 b 3 − a 2 b 4 + a 3 b 1 + a 4 b 2 a 1 b 4 + a 2 b 3 − a 3 b 2 + a 4 b 1 ] T

  四元数叉乘并不满足交换律，但当 â  a ^ ， b̂  b ^ 为纯矢量，即 a1=0 a 1 = 0 ， b1=0 b 1 = 0 时，四元数叉乘等价于矢量叉乘，满足反交换律。

2. 四元数表示旋转

归一化的四元数可以用于表示三维空间中刚体或坐标系的旋转。假设空间任意向量 v̂  v ^ 在坐标系 A A 和坐标系 B B 中的投影分别为 [vAvxvAvyvAvz] [ v A v x v A v y v A v z ] 和 [vBvxvBvyvBvz] [ v B v x v B v y v B v z ] ，在坐标系 A A 中可以找到这样的转轴 Ar̂  A r ^ 和转角 θ θ ，使得向量 v̂  v ^ 绕转轴旋转后的向量 v̂ ′ v ^ ′ 在坐标系 A A 中的投影为 [vBvxvBvyvBvz] [ v B v x v B v y v B v z ] 。这种旋转过程可以使用四元数 q̂ ABq̂  q ^ B A q ^ 表示，其数学表达式如下：

q̂ ABq̂ =[cosθ2−rxsinθ2−rysinθ2−rzsinθ2](5) (5) q ^ B A q ^ = [ c o s θ 2 − r x s i n θ 2 − r y s i n θ 2 − r z s i n θ 2 ]

其中上标 A A 表示参考坐标系，下标 B B 表示目标坐标系， rx r x ， ry r y ， rz r z 表示转轴 Ar̂  A r ^ 在坐标系 A A 中的投影。对于逆向旋转，可用四元数的共轭表示：

q̂ BAq̂ =q̂ ABq̂ ∗=[q1−q2−q3−q4](6) (6) q ^ A B q ^ = q ^ B A q ^ ∗ = [ q 1 − q 2 − q 3 − q 4 ]

向量 v̂  v ^ 由坐标系 A A 转换到坐标系 B B 的投影公式如下：

v̂ Bv̂ =q̂ ABq̂ ⨂v̂ Av̂ ⨂q̂ ABq̂ ∗(7) (7) v ^ B v ^ = q ^ B A q ^ ⁡ ⨂ v ^ A v ^ ⁡ ⨂ q ^ B A q ^ ∗

四元数 q̂ ABq̂  q ^ B A q ^ 所描述的旋转过程可以用旋转矩阵 RABR R B A R 表示，其数学公式如下：

v̂ Bv̂ =v̂ Av̂ ⋅RTABRT=[v̂ Av̂ xv̂ Av̂ yv̂ Av̂ z]⎡⎣⎢⎢⎢2q21−1+2q222(q2q3−q1q4)2(q2q4+q1q3)2(q2q3+q1q4)2q21−1+2q232(q3q4−q1q2)2(q2q4−q1q3)2(q3q4+q1q2)2q21−1+2q24⎤⎦⎥⎥⎥T(8) (8) v ^ B v ^ = v ^ A v ^ ⋅ R T B A R T = [ v ^ A v ^ x v ^ A v ^ y v ^ A v ^ z ] [ 2 q 1 2 − 1 + 2 q 2 2 2 ( q 2 q 3 + q 1 q 4 ) 2 ( q 2 q 4 − q 1 q 3 ) 2 ( q 2 q 3 − q 1 q 4 ) 2 q 1 2 − 1 + 2 q 3 2 2 ( q 3 q 4 + q 1 q 2 ) 2 ( q 2 q 4 + q 1 q 3 ) 2 ( q 3 q 4 − q 1 q 2 ) 2 q 1 2 − 1 + 2 q 4 2 ] T

3. 四元数表示姿态

四元数可以表示刚体姿态，这里的姿态是指载体坐标系相对于地理坐标系的旋转。这种旋转过程在本文中使用四元数 q̂ SEq̂  q ^ E S q ^ 表示，其中上标 S S 表示载体坐标系即参考坐标系，下标 E E 表示地理坐标系即目标坐标系。

姿态解算

1. 姿态的角速度更新方法

1.1 利用姿态求解线速度

考虑载体坐标系下的空间任意一定点 RSR R S R ，将其映射到地理坐标系中，其数学表达如下：

RER=q̂ SEq̂ ⨂RSR⨂q̂ ∗SEq̂ ∗(9) (9) R E R = q ^ E S q ^ ⁡ ⨂ R S R ⁡ ⨂ q ^ ∗ E S q ^ ∗

对时间 t t 求导，可得点 RER R E R 在地理坐标系中的线速度 vEv v E v ，其数学表达式如下：

vEv=R˙ER˙=q˙SEq˙⨂RSR⨂q̂ ∗SEq̂ ∗+q̂ SEq̂ ⨂RSR⨂q˙∗SEq˙∗(10) (10) v E v = R ˙ E R ˙ = q ˙ E S q ˙ ⁡ ⨂ R S R ⁡ ⨂ q ^ ∗ E S q ^ ∗ + q ^ E S q ^ ⁡ ⨂ R S R ⁡ ⨂ q ˙ ∗ E S q ˙ ∗

其中上标一点表示对时间 t t 求微分。

由式 (9) ( 9 ) 可得：

q̂ ∗SEq̂ ∗⨂RER=RSR⨂q̂ ∗SEq̂ ∗(11) (11) q ^ ∗ E S q ^ ∗ ⁡ ⨂ R E R = R S R ⁡ ⨂ q ^ ∗ E S q ^ ∗

RER⨂q̂ SEq̂ =q̂ SEq̂ ⨂RSR(12) (12) R E R ⁡ ⨂ q ^ E S q ^ = q ^ E S q ^ ⁡ ⨂ R S R

将式 (11)(12) ( 11 ) ( 12 ) 带入式 (10) ( 10 ) 可得：

vEv=q˙SEq˙⨂q̂ ∗SEq̂ ∗⨂RER+RER⨂q̂ SEq̂ ⨂q˙∗SEq˙∗(13) (13) v E v = q ˙ E S q ˙ ⁡ ⨂ q ^ ∗ E S q ^ ∗ ⁡ ⨂ R E R + R E R ⁡ ⨂ q ^ E S q ^ ⁡ ⨂ q ˙ ∗ E S q ˙ ∗

考查 q̂ SEq̂ ⨂q̂ ∗SEq̂ ∗ q ^ E S q ^ ⁡ ⨂ q ^ ∗ E S q ^ ∗ 可得：

q̂ SEq̂ ⨂q̂ ∗SEq̂ ∗=[1000](14) (14) q ^ E S q ^ ⁡ ⨂ q ^ ∗ E S q ^ ∗ = [ 1 0 0 0 ]

由式 (14) ( 14 ) 可得：

q˙SEq˙⨂q̂ ∗SEq̂ ∗+q̂ SEq̂ ⨂q˙∗SEq˙∗=0(15) (15) q ˙ E S q ˙ ⁡ ⨂ q ^ ∗ E S q ^ ∗ + q ^ E S q ^ ⁡ ⨂ q ˙ ∗ E S q ˙ ∗ = 0

将式 (15) ( 15 ) 带入式 (13) ( 13 ) 可得：

vEv=q˙SEq˙⨂q̂ ∗SEq̂ ∗⨂RER−RER⨂q̂ ˙SEq̂ ˙⨂q∗SEq∗(16) (16) v E v = q ˙ E S q ˙ ⁡ ⨂ q ^ ∗ E S q ^ ∗ ⁡ ⨂ R E R − R E R ⁡ ⨂ q ^ ˙ E S q ^ ˙ ⁡ ⨂ q ∗ E S q ∗

考查 q˙SEq˙⨂q̂ ∗SEq̂ ∗ q ˙ E S q ˙ ⁡ ⨂ q ^ ∗ E S q ^ ∗ 标量部分可得：

Scale(q˙SEq˙⨂q̂ ∗SEq̂ ∗)=q0˙q0+q1˙q1+q2˙q2+q3˙q3=12ddt(q20+q21+q22+q23)=0(17) (17) S c a l e ( q ˙ E S q ˙ ⁡ ⨂ q ^ ∗ E S q ^ ∗ ) = q 0 ˙ q 0 + q 1 ˙ q 1 + q 2 ˙ q 2 + q 3 ˙ q 3 = 1 2 d d t ( q 0 2 + q 1 2 + q 2 2 + q 3 2 ) = 0

由式 (17) ( 17 ) 可知 q˙SEq˙⨂q̂ SEq̂ ∗ q ˙ E S q ˙ ⁡ ⨂ q ^ E S q ^ ∗ 为纯矢量， vEv v E v 为矢量叉乘相减，而矢量叉乘又满足反交换律，因此式 (16) ( 16 ) 可重写为：

vEv=2q˙SEq˙⨂q̂ ∗SEq̂ ∗⨂RER(18) (18) v E v = 2 q ˙ E S q ˙ ⁡ ⨂ q ^ ∗ E S q ^ ∗ ⁡ ⨂ R E R

1.2 利用角速度求解线速度

另一方面，线速度可以由角速度叉乘位移得到，其数学表达如下：

vEv=ω̂ Eω̂ ⨂RER(19) (19) v E v = ω ^ E ω ^ ⁡ ⨂ R E R

考虑到载体坐标系下的角速度 ωSω ω S ω ，式 (19) ( 19 ) 可重写为：

vEv=q̂ SEq̂ ⨂ω̂ Sω̂ ⨂q̂ ∗SEq̂ ∗⨂RER(20) (20) v E v = q ^ E S q ^ ⁡ ⨂ ω ^ S ω ^ ⁡ ⨂ q ^ ∗ E S q ^ ∗ ⁡ ⨂ R E R

1.3 姿态更新方程

对比式 (18) ( 18 ) 和 (20) ( 20 ) 可得：

q˙SEq˙=12q̂ SEq̂ ⨂ω̂ Sω̂ (21) (21) q ˙ E S q ˙ = 1 2 q ^ E S q ^ ⁡ ⨂ ω ^ S ω ^

因此结合角速度，通过数值积分，可以求解角速度姿态四元数 qω,tSEqω,t q ω , t E S q ω , t ：

qω,tSEqω,t=q̂ est,t−1SEq̂ est,t−1+12q̂ est,t−1SEq̂ est,t−1⨂ω̂ Sω̂ Δt=q̂ est,t−1SEq̂ est,t−1+q˙ω,tSEq˙ω,tΔt(22) (22) q ω , t E S q ω , t ⁡ = q ^ e s t , t − 1 E S q ^ e s t , t − 1 + 1 2 q ^ e s t , t − 1 E S q ^ e s t , t − 1 ⁡ ⨂ ω ^ S ω ^ ⁡ Δ t = q ^ e s t , t − 1 E S q ^ e s t , t − 1 + q ˙ ω , t E S q ˙ ω , t ⁡ Δ t

其中下标 est e s t 表示估计值，是融合角速度和向量观测求解的四元数，下标 t t 表示采样序号。

2. 姿态的向量观测更新方法

2.1 姿态最优化问题

假设 d̂ Ed̂  d ^ E d ^ 是大地坐标系下的某一参考矢量， ŝ Sŝ  s ^ S s ^ 是该向量在载体坐标系下的测量矢量。参考矢量 d̂ Ed̂  d ^ E d ^ 可以通过姿态四元数 q̂ SEq̂  q ^ E S q ^ 投影到载体坐标系下，其数学表达如下：

d̂ Sd̂ =q̂ ∗SEq̂ ∗⨂d̂ Ed̂ ⨂q̂ SEq̂ (23) (23) d ^ S d ^ = q ^ ∗ E S q ^ ∗ ⁡ ⨂ d ^ E d ^ ⁡ ⨂ q ^ E S q ^

四元数共轭表示逆向旋转

投影矢量 d̂ Sd̂  d ^ S d ^ 与测量矢量 ŝ Sŝ  s ^ S s ^ 理论上相一致，因此姿态四元数 q̂ SEq̂  q ^ E S q ^ 应该满足如下条件：

minq̂ SEq̂ ∈ℜ4f(q̂ SEq̂ ,d̂ Ed̂ ,ŝ Sŝ )(24) (24) m i n q ^ E S q ^ ∈ R 4 f ( q ^ E S q ^ , d ^ E d ^ , s ^ S s ^ )

f(q̂ SEq̂ ,d̂ Ed̂ ,ŝ Sŝ )=d̂ S