【传感器融合姿态估计】Madgwick算法（上）

Madgwick算法能够综合多种传感器参数得到传感器的姿态。传感器可以采用以下两种配置：
1. IMU：包含三轴线加速度计，测量物体坐标系下的三轴线加速度；三轴角速度计，测量物体坐标系下的三轴欧拉角变化率。
2. MARG或AHRS：除了前述两类，还包含三轴磁力计，测量地球磁力线在物体坐标系下投影。

以下从原始传感器参数逐步推导出物体姿态。在推导过程中，大量使用四元数(quaternion)来表示旋转和姿态，不熟悉的同学可以参看这篇文章。

本文分上下两篇，上篇讲解各个传感器独立结果，下篇讲解融合方法、对误差的处理，以及标定实验。
本篇公式较多，可以直接关注各分段结尾处的“结论”。

欧拉角变化率和角速度（gyro →ω ）

问题：已知欧拉角变化率，求角速度。

二维空间中的角速度 ω 是一个伪标量（pseudoscale）。其大小为单位时间转动过的弧度，其方向垂直于所在平面符合，和旋转方向符合右手定则，换言之，其方向为旋转轴。
三维空间中的角速度 ω 是一个伪矢量（pseudovector），其大小为单位时间转动过的弧度，其方向和旋转方向符合右手定则。

用矢量表示角速度时，可以直接使用矢量加法表示角速度的叠加。
绕 x 轴旋转的角速度为 ωxi ，其中 i 表示旋转轴 [1,0,0] ；类似地，绕 y,z 轴旋转的角速度为 ωyj,ωzk 。

总体角速度和欧拉角变化率具有以下关系：

ω=ωxi+ωyj+ωzk

其中， i,j,k 为三个轴单位矢量。

完整的证明较为繁琐，可以定性地考察一下这个结论。
考虑绕 x 轴的旋转，该旋转发生在 yz 平面内，旋转矢量和 yz 平面垂直，即和 x 轴平行。
ω 在 x 轴投影为 ωx ，就是绕 x 轴的旋转速度。

角速度 ω 的旋转轴（归一化长度）为：

[ωx,ωy,ωz]ω2x+ω2y+ω2z−−−−−−−−−−−√

旋转速度（单位时间旋转的角度）为：

ω2x+ω2y+ω2z−−−−−−−−−−−√

此部分证明原文可以参看维基百科角速度词条。

结论： ω=[0,ωx,ωy,ωz] 。

角速度和姿态四元数（ ω→q ）

问题：已知传感器坐标系下角速度和当前姿态四元数，求姿态四元数变化率，进而求姿态四元数。

首先考虑一个稍简单的情况，已知世界坐标系下角速度 ω¯ ，以及当前姿态四元数 q ，求 q˙ 。（上标一点表示对时间的一次微分）。

考虑空间中任意一点 R0=[0,x0,y0,z0] ，当前时刻的位置为 Rt ，以下通过两个方面来写出当前线速度 Rt˙ 的表达。

第一方面，由于 Rt 是 R0 经过变换 q 得到的，故：

Rt=q×R0×q−1         (1)

对时间 t 求导，注意只有 q 是 t 的函数， R0 常量：

Rt˙=q˙×R0×q−1+q×R0×q−1˙         (2)

根据式(1)（此处存疑，叉乘不满足结合律？），有：

q−1×Rt=R0×q−1

Rt×q=q×R0

代入式(2)：

Rt˙=q˙×q−1×Rt+Rt×q×q−1˙         (3)

考察 q 和其反变换 q−1 的叉乘：

q×q−1=[q0,q1,q2,q3]×[q0,−q1,−q2,−q3]=[q20+q21+q22+q23,0,0,0]=[1,0,0,0]

叉乘结果是个常数矢量，所以其对于时间的导数是0：

q˙×q−1+q×q−1˙=0

q˙×q−1=−q×q−1˙

把此结果代入式(3)：

Rt˙=q˙×q−1×Rt−Rt×q˙×q−1         (3)

考察 q˙×q−1 的标量部分（即四元数第一分量）：

Scale(q˙×q−1)=q0˙q0+q1˙q1+q2˙q2+q3˙q3=12ddt(q20+q21+q22+q23)=0

说明 q˙×q−1 是一个纯矢量。由于矢量叉乘满足反交换律： a×b=−b×a 。(3)式变成：

Rt˙=2q˙×q−1×Rt

回顾：四元数 q=q0+q1i+q2j+q3k 。由一个标量 q0 ，和一个矢量 q1i+q2j+q3k 组成。

第二方面，线速度= 角速度 × 弧长：

Rt˙=ω¯×Rt

综合第一、第二方面的结论，要想让两式对于任意 R0 都相等，有：

2q˙×q−1=ω¯→q˙=12ω¯×q           (4)

注意此处 ω¯ 表示世界坐标系下的角速度。它可以通过传感器坐标系下角速度 ω 经过变换 q 得到：

ω¯=q×ω×q−1

引理：将原始坐标系A施加变换 q 获得坐标系B。则对于同一点，A到B的坐标变换为 q−1 。
证明：为书写简便，用矩阵相乘形式表示变换，例如 q∙xA 。
设有两个点 x,y ，他们在两个坐标系中的坐标相同： xA=yB 。 由于两点相对各自坐标系位置相同，可以认为 y 是 x 经过变换 q 得到的：

yA=q∙xA

这个变换结果是在A坐标系下，设A到B的坐标变换矩阵为 q∗ :

q∗∙yA=q∗∙q∙xA=yB

所以得到坐标变换矩阵： q∗=q−1

代入(4)式，得到：

2q˙×q−1=ω¯→q˙=12q×ω

此部分证明原文可以参看euclideanspace网站。

进一步，可以得到一段时间内四元数的变换：

qt=qt−1+q˙Δt

结论：

qt=qt−1+12qt×ωtΔt

加速度计、磁力计和姿态四元数（acce, mag →q ）

问题：已知加速度计、磁力计读数，求姿态四元数。

重力矢量和磁场矢量在世界坐标系下的三个分量根据物理常识已知。
加速度计测量重力矢量在传感器坐标系下的三个分量（静止状态下）。
磁力计测量磁场矢量在传感器坐标系下的三个分量。

设 dE=[0,dex,dey,dez] 为世界坐标系下的矢量坐标。设 q 为传感器姿态。根据前节引理，使用 q−1 可以把 dE 变换为传感器坐标系下的矢量坐标 dS=[0,dsx,dsy,dsz] ：

dS=q−1×dE×q

满足这个方程的解不唯一。设 q 为方程的一个解，考虑一个以 dE 为轴的旋转 Δq 。两者的复合变换为 Δq×q 。

(Δq×q)−1×dE×(Δq×q)=q−1×(Δq−1×dE×Δq)×q

向量绕自身旋转不会发生变化，所以括号内部分等于 dE ，上式仍然等于 dS 。即：绕轴的旋转不会改变传感器的示数。

为了获得一个确定的解，需要从优化的角度来求解。找到 q ，最小化以下标量误差：

f(q)=||q−1×dE×q−dS||2=e(q)Te(q)

使用高斯牛顿法求解：

qk+1=qk−μ∇f||∇f||

其中 μ 为步长， ∇ 为微分算子，表示标量 f 对于矢量 q 的变化率。其和误差矢量 e(q) 的关系如下：

∇f(q)=J(q)e(q)

其中 e(q) 尺寸为 4×1 。 J(q) 尺寸为 4×4 ， Jij=∂(ei)/∂qj 。都可以写出解析表达式。

为节约时间，每个采样间隔只进行一次迭代：

qt+1=qt−μt∇f||∇f||

其中步长和当前时刻误差项的二阶导有关， α 为固定参数， Δt 为采样间隔：

μt=α||q˙||Δt

采样间隔越大，变化速率越快，步长应该越大。

加速度计

dE=[0,0,0,1] , dS=[0,ax,ay,az] 。有：

e(q)=[2(q1q3−q0q2)−ax2(q0q1+q2q3)−ay2(12−q21−q22)−az]T

J(q)=⎡⎣⎢−2q22q102q32q0−4q1−2q02q3−4q22q12q20⎤⎦⎥

为书写简便，省去了 e,J 中为0的部分。
使用加速度计估计的姿态只在加速度为0条件下准确。但后续融合算法中，加速度计和磁力计只起到修正的作用，影响不大。

磁力计

粗略来说，地球磁力线处于经线和垂直轴构成的平面内，在东西方向投影为0。磁力线大小(Intensity)和方向(Inclination)随地理位置变化，在两极最大，赤道最小；在北极为90°（朝下），在赤道为0°，在南极为-90°（朝上）。具体数值可以根据当地经纬度查询维基百科。

不失一般性地，设 dE=[0,bx,0,bz] ， dS=[0,mx,my,mz] 。

e(q)1=2bx(0.5−q22−q23)+2bz(q1q3−q0q2)−mx

e(q)2=2bx(q1q2−q0q3)+2bz(q0q1+q2q3)−my

e(q)3=2bx(q0q2+q1q3)+2bz(0.5−q21−q22)−mz

J(q)=⎡⎣⎢−2bzq2−2bxq3+2bzq12bxq22bzq32bxq2+2bzq02bxq3−4bzq1−4bxq2−2bzq02bxq1+2bzq32bxq0−4bzq2−4bxq3+2bzq1−2bxq0+2bzq22bxq1⎤⎦⎥

这部分证明原文参看Madgwick内部报告。

结论：

qt+1=qt−μtJ(q)e(q)||J(q)e(q)||

至此，使用角速度计、加速度计、磁力计可以分别计算出当前姿态。在下篇中，会介绍它们的融合方法，对误差的处理，以及标定实验。