【运动传感器】Madgwick算法（上）

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

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

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

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

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

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

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

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

ω=ω x i+ω y j+ω z k 

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

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

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

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

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

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

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

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

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

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

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

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

第一方面，由于 R t   是 R 0   经过变换 q  得到的，故：

R t =q×R 0 ×q −1          (1) 

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

R t  ˙ =q ˙ ×R 0 ×q −1 +q×R 0 ×q −1  ˙          (2) 

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

q −1 ×R t =R 0 ×q −1  

R t ×q=q×R 0  

代入式(2)：

R t  ˙ =q ˙ ×q −1 ×R t +R t ×q×q −1  ˙          (3) 

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

q×q −1 =[q 0 ,q 1 ,q 2 ,q 3 ]×[q 0 ,−q 1 ,−q 2 ,−q 3 ]=[q 2 0 +q 2 1 +q 2 2 +q 2 3 ,0,0,0]=[1,0,0,0] 

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

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

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

把此结果代入式(3)：

R t  ˙ =q ˙ ×q −1 ×R t −R t ×q ˙ ×q −1          (3) 

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

Scale(q ˙ ×q −1 )=q 0  ˙ q 0 +q 1  ˙ q 1 +q 2  ˙ q 2 +q 3  ˙ q 3 =12 ddt (q 2 0 +q 2 1 +q 2 2 +q 2 3 )=0 

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

R t  ˙ =2q ˙ ×q −1 ×R t  

回顾：四元数 q=q 0 +q 1 i+q 2 j+q 3 k  。由一个标量 q 0   ，和一个矢量 q 1 i+q 2 j+q 3 k  组成。

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

R t  ˙ =ω ¯ ×R t  

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

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

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

ω ¯ =q×ω×q −1  

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

y A =q∙x A  

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

q ∗ ∙y A =q ∗ ∙q∙x A =y B  

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

代入(4)式，得到：

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

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

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

q t =q t−1 +q ˙ Δt 

结论：

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

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

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

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

设 d E =[0,de x ,de y ,de z ]  为世界坐标系下的矢量坐标。设 q  为传感器姿态。根据前节引理，使用 q −1   可以把 d E   变换为传感器坐标系下的矢量坐标 d S =[0,ds x ,ds y ,ds z ]  ：

d S =q −1 ×d E ×q 

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

(Δq×q) −1 ×d E ×(Δq×q)=q −1 ×(Δq −1 ×d E ×Δq)×q 

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

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

f(q)=||q −1 ×d E ×q−d S || 2 =e(q) T e(q) 

使用高斯牛顿法求解：

q k+1 =q k −μ∇f||∇f||  

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

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

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

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

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

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

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

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

加速度计

d E =[0,0,0,1]  , d S =[0,a x ,a y ,a z ]  。有：

e(q)=[2(q 1 q 3 −q 0 q 2 )−a x  2(q 0 q 1 +q 2 q 3 )−a y  2(12 −q 2 1 −q 2 2 )−a z  ] T  

J(q)=⎡ ⎣ ⎢ −2q 2 2q 1 0 2q 3 2q 0 −4q 1  −2q 0 2q 3 −4q 2  2q 1 2q 2 0 ⎤ ⎦ ⎥  

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

磁力计

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

不失一般性地，设 d E =[0,b x ,0,b z ]  ， d S =[0,m x ,m y ,m z ]  。

e(q) 1 =2b x (0.5−q 2 2 −q 2 3 )+2b z (q 1 q 3 −q 0 q 2 )−m x  

e(q) 2 =2b x (q 1 q 2 −q 0 q 3 )+2b z (q 0 q 1 +q 2 q 3 )−m y  

e(q) 3 =2b x (q 0 q 2 +q 1 q 3 )+2b z (0.5−q 2 1 −q 2 2 )−m z  

J(q)=⎡ ⎣ ⎢ −2b z q 2 −2b x q 3 +2b z q 1 2b x q 2  2b z q 3 2b x q 2 +2b z q 0 2b x q 3 −4b z q 1  −4b x q 2 −2b z q 0 2b x q 1 +2b z q 3 2b x q 0 −4b z q 2  −4b x q 3 +2b z q 1 −2b x q 0 +2b z q 2 2b x q 1  ⎤ ⎦ ⎥  

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

结论：

q t+1 =q t −μ t J(q)e(q)||J(q)e(q)||  

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