Madgwick AHRS算法笔记
引言
最近项目需要搞AHRS,重新读了一下Madgwick的AHRS算法论文[1],在这里把笔记整理如下,如有错误,欢迎指正。
坐标系
1. 地理坐标系
地理坐标系原点位于载体质心,通常选取东北天坐标系,即其 x x 轴指向水平东方,其 轴指向水平北方,其 z z 轴沿地垂线向上。
2. 载体坐标系
载体坐标系原点位于载体质心,通常选取右前上坐标系,即其 轴沿载体横轴向右,其 y y 轴沿载体纵轴向前, 其 轴沿载体竖轴向上。
四元数
1. 四元数基本理论
1.1 定义
四元数是一种四维超复数,由一个实数单位加上三个虚数单位 i i , , k k 组成,其数学表达式如下:
其中实数部分为四元数中的标量部分,虚数部分为四元数的矢量部分。
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 中的投影分别为 和 [vBvxvBvyvBvz] [ v B v x v B v y v B v z ] ,在坐标系 A A 中可以找到这样的转轴 和转角 θ θ ,使得向量 v̂ v ^ 绕转轴旋转后的向量 v̂ ′ v ^ ′ 在坐标系 A A 中的投影为 。这种旋转过程可以使用四元数 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 表示参考坐标系,下标 表示目标坐标系, rx r x , ry r y , rz r z 表示转轴 Ar̂ A r ^ 在坐标系 A A 中的投影。对于逆向旋转,可用四元数的共轭表示:
向量 v̂ v ^ 由坐标系 A A 转换到坐标系 的投影公式如下:
四元数 q̂ ABq̂ q ^ B A q ^ 所描述的旋转过程可以用旋转矩阵 RABR R B A R 表示,其数学公式如下:
3. 四元数表示姿态
四元数可以表示刚体姿态,这里的姿态是指载体坐标系相对于地理坐标系的旋转。这种旋转过程在本文中使用四元数 q̂ SEq̂ q ^ E S q ^ 表示,其中上标 S S 表示载体坐标系即参考坐标系,下标 表示地理坐标系即目标坐标系。
姿态解算
1. 姿态的角速度更新方法
1.1 利用姿态求解线速度
考虑载体坐标系下的空间任意一定点 RSR R S R ,将其映射到地理坐标系中,其数学表达如下:
对时间 t t 求导,可得点 在地理坐标系中的线速度 vEv v E v ,其数学表达式如下:
其中上标一点表示对时间 t t 求微分。
由式 可得:
将式 (11)(12) ( 11 ) ( 12 ) 带入式 (10) ( 10 ) 可得:
考查 q̂ SEq̂ ⨂q̂ ∗SEq̂ ∗ q ^ E S q ^ ⨂ q ^ ∗ E S q ^ ∗ 可得:
由式 (14) ( 14 ) 可得:
将式 (15) ( 15 ) 带入式 (13) ( 13 ) 可得:
考查 q˙SEq˙⨂q̂ ∗SEq̂ ∗ q ˙ E S q ˙ ⨂ q ^ ∗ E S q ^ ∗ 标量部分可得:
由式 (17) ( 17 ) 可知 q˙SEq˙⨂q̂ SEq̂ ∗ q ˙ E S q ˙ ⨂ q ^ E S q ^ ∗ 为纯矢量, vEv v E v 为矢量叉乘相减,而矢量叉乘又满足反交换律,因此式 (16) ( 16 ) 可重写为:
1.2 利用角速度求解线速度
另一方面,线速度可以由角速度叉乘位移得到,其数学表达如下:
考虑到载体坐标系下的角速度 ωSω ω S ω ,式 (19) ( 19 ) 可重写为:
1.3 姿态更新方程
对比式 (18) ( 18 ) 和 (20) ( 20 ) 可得:
因此结合角速度,通过数值积分,可以求解角速度姿态四元数 qω,tSEqω,t q ω , t E S q ω , t :
其中下标 est e s t 表示估计值,是融合角速度和向量观测求解的四元数,下标 t t 表示采样序号。
2. 姿态的向量观测更新方法
2.1 姿态最优化问题
假设 是大地坐标系下的某一参考矢量, ŝ Sŝ s ^ S s ^ 是该向量在载体坐标系下的测量矢量。参考矢量 d̂ Ed̂ d ^ E d ^ 可以通过姿态四元数 q̂ SEq̂ 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̂ Sd̂ −ŝ Sŝ =q̂ ∗SEq̂ ∗⨂d̂ Ed̂ ⨂q̂ SEq̂ −ŝ Sŝ (25) (25) f ( q ^ E S q ^ , d ^ E d ^ , s ^ S s ^ ) = d ^ S d ^ − s ^ S s ^ = q ^ ∗ E S q ^ ∗ ⨂ d ^ E d ^ ⨂ q ^ E S q ^ − s ^ S s ^
笔者不太明白在上述最优化问题中,为什么目标函数 f(q̂ SEq̂ ,d̂ Ed̂ ,ŝ Sŝ ) f ( q ^ E S q ^ , d ^ E d ^ , s ^ S s ^ ) 是矢量而非标量。另外,由于由参考矢量 d̂ Ed̂ d ^ E d ^ 到投影矢量 ŝ Sŝ s ^ S s ^ 到旋转过程不具有唯一性,因此用这种最优化方式求解的四元数并不一定是实际的姿态四元数。
式 (25) ( 25 ) 被称为目标函数。由式 (24) ( 24 ) 可知,姿态四元数求解被转化为了最优化问题中的极小值求解,利用梯度法求解极小值:
关于梯度 ∇f(q̂ SEq̂ ,d̂ Ed̂ ,ŝ Sŝ ) ∇ f ( q ^ E S q ^ , d ^ E d ^ , s ^ S s ^ ) 的具体数学表达式在Madgwick原文[1]的式 (20)∼(22) ( 20 ) ∼ ( 22 ) 中有详尽描述。
其中 μ μ 表示迭代步进。
2.2 姿态更新方程
为了使最优化方式求解的四元数唯一,我们需要引入两个参考矢量。在这里我们选取大地坐标系下的重力矢量 ĝ Eĝ =[0001] g ^ E g ^ = [ 0 0 0 1 ] 和地磁矢量在 XOZ X O Z 面的投影矢量 b̂ Eb̂ =[0bx0bz] b ^ E b ^ = [ 0 b x 0 b z ] 作为参考矢量。因此,目标函数可重写为:
f(q̂ SEq̂ ,â Sâ ,b̂ Eb̂ ,m̂ Sm̂ )=[q̂ ∗SEq̂ ∗⨂ĝ Eĝ ⨂q̂ SEq̂ −â Sâ q̂ ∗SEq̂ ∗⨂b̂ Eb̂ ⨂q̂ SEq̂ −m̂ Sm̂ ](27) (27) f ( q ^ E S q ^ , a ^ S a ^ , b ^ E b ^ , m ^ S m ^ ) = [ q ^ ∗ E S q ^ ∗ ⨂ g ^ E g ^ ⨂ q ^ E S q ^ − a ^ S a ^ q ^ ∗ E S q ^ ∗ ⨂ b ^ E b ^ ⨂ q ^ E S q ^ − m ^ S m ^ ]
其中 â Sâ a ^ S a ^ 表示重力矢量的测量值, m̂ Sm̂ m ^ S m ^ 表示磁力矢量的测量值。
bx≠0 b x ≠ 0 的情况下,四元数才有唯一解。
我们在每次采样时刻 t t 进行四元数迭代,可以求解向量观测姿态四元数 :
其中 Δt Δ t 表示采样间隔, α α 表示增强因子。
3. 融合算法
融合上文中计算出的角速度姿态四元数 qω,tSEqω,t q ω , t E S q ω , t 和向量观测姿态四元数 q∇,tSEq∇,t q ∇ , t E S q ∇ , t ,我们可以得到估计姿态四元数 qest,tSEqest,t q e s t , t E S q e s t , t :
其中 γt γ t 表示融合加权系数。
对于融合加权系数 γt γ t 而言,一种比较好的取值策略定义如下:取值 γt γ t 使得 q∇,tSEq∇,t q ∇ , t E S q ∇ , t 的收敛与 qω,tSEqω,t q ω , t E S q ω , t 的发散相当。
笔者不太明白这里的收敛和发散是否有具体的数学定义,个人将收敛理解为式 (28) ( 28 ) 中的减法运算,发散理解为式 (22) ( 22 ) 中的加法运算。
根据上述策略,可以推导出融合加权系数 γt γ t 的计算公式如下:
其中 β β 是由陀螺仪测量误差估算出来一个增益系数。
β β 在3.3节会有说明。
现在将 μt μ t 中的增强因子 α α 取一个非常大的值,那么向量观测姿态四元数 q∇,tSEq∇,t q ∇ , t E S q ∇ , t 可重写为:
融合加权系数 γt γ t 可重写为:
将式 (22)(33)(34) ( 22 ) ( 33 ) ( 34 ) 带入式 (30) ( 30 ) 可得:
理论上式 (35) ( 35 ) 第二项矢量权重应为 (1−βΔtμt) ( 1 − β Δ t μ t ) ,但由于 μt μ t 是一个非常大的值,为了简化运算这里取权重为 (1−0) ( 1 − 0 ) 。
其中 q˙est,tSEq˙est,t q ˙ e s t , t E S q ˙ e s t , t 表示估算姿态四元数导数, q̂ ˙ϵ,tSEq̂ ˙ϵ,t q ^ ˙ ϵ , t E S q ^ ˙ ϵ , t 表示由陀螺仪测量误差引入的单位四元数导数。
3.1 磁场畸变补偿
磁力计易受外部环境干扰,其测量值可能会存在偏差。假设 ĥ tEĥ t h ^ t E h ^ t 大地坐标系下 t t 时刻的地磁测量矢量,其数学表达如下:
其中 q̂ est,t−1SEq̂ est,t−1 q ^ e s t , t − 1 E S q ^ e s t , t − 1 表示 t−1 t − 1 时刻的姿态四元数, m̂ tSm̂ t m ^ t S m ^ t 表示载体坐标系下 t t 时刻的地磁测量值。我们使用下式对 时刻的地磁参考矢量进行补偿:
b̂ Eb̂ t=[0h2x+h2y‾‾‾‾‾‾‾√0hz](39) (39) b ^ E b ^ t = [ 0 h x 2 + h y 2 0 h z ]
这种方式可以补偿地磁矢量在倾斜角方向的畸变,使得地磁畸变仅影响姿态航向角。
3.2 陀螺仪偏置漂移补偿
由于温度和运动的影响,陀螺仪的偏置会存在漂移现象。Mahony的AHRS算法[2]中,陀螺仪偏置漂移的补偿利用误差积分消除。这里,我们采用相似的方法,其补偿公式如下:
ωSωϵ,t=2q̂ ∗est,t−1SEq̂ ∗est,t−1⨂q̂ ˙SEq̂ ˙ϵ,t(40) (40) ω S ω ϵ , t = 2 q ^ e s t , t − 1 ∗ E S q ^ e s t , t − 1 ∗ ⨂ q ^ ˙ E S q ^ ˙ ϵ , t
ωSωb,t=ζ∑ωϵ,tSωϵ,tΔt(41) (41) ω S ω b , t = ζ ∑ ω ϵ , t S ω ϵ , t Δ t
ωSωc,t=ωtSωt−ωb,tSωb,t(42) (42) ω S ω c , t = ω t S ω t − ω b , t S ω b , t
以重力矢量和地磁矢量解算的姿态为真实姿态 qreal,tSEqreal,t q r e a l , t E S q r e a l , t , q̂ ˙ϵ,tSEq̂ ˙ϵ,t q ^ ˙ ϵ , t E S q ^ ˙ ϵ , t 近似于真实姿态四元数 qreal,tSEqreal,t q r e a l , t E S q r e a l , t 与角速度姿态四元数 qω,t−1SEqω,t−1 q ω , t − 1 E S q ω , t − 1 之差,由此推导的 ωSωϵ,t ω S ω ϵ , t 可以视作真实角度 ωSωreal,t ω S ω r e a l , t 与测量角速度 ωSωt ω S ω t 之差,通过积分负反馈便可以消除补偿角速度 ωSωc,t ω S ω c , t 的静态误差(偏置漂移)。
3.3 滤波器增益系数
系数 β β 由陀螺仪零均值测量误差估算,零均值测量误差包括:传感器噪声,信号混叠,量化误差,校准误差,传感器失准,传感器轴非对称和一些频率响应特性。系数 ζ ζ 由陀螺仪非零均值测量误差估算,这种误差主要是指偏置漂移。
参考文献
[1] An efficient orientation filter for inertial and inertial/magnetic sensor arrays
[2] A complementary filter for attitude estimation of a fixed-wing UAV