原英文地址:dcm_tutorial

　　感觉这篇文章还是很有学习价值的,所以就抽出了一些时间对本文进行的翻译.下面这个好多人用的算法就是一种DCM 滤波器.

1. //======================================================================================
2. // IMU.c
3. // S.O.H. Madgwick
4. // 25th September 2010
5. //======================================================================================
6. // Description:
7. //
8. // Quaternion implementation of the 'DCM filter' [Mayhony et al].

　　 这个地址可以下载到原代码.

/*======================================正文===========================================*/

介绍:

　　这篇文章是继IMU Guide 后,涵盖一些方向运动(orientation kinematics )的理论,然后我会围绕Arduino和一个六*度IMU传感器提出一个实际的例子（ acc_gyro_6dof )。本文是目的是创建一种算法融合加速度计和陀螺仪从而可以进行空间中方向的估计.像这样的算法已经在第三部分"IMU Guide" 提及.代码部分可以参考"Using a 5DOF IMU" 简称为 "简单卡尔曼滤波". 在这篇文章中我们将用另外一种方法,利用DCM（方向余弦矩阵）.对于不不熟悉MEMS 传感器的读者 ,建议先阅读一下IMU Guide 的  Part1 和Part2 .对于实验部分,建议先购买Arduino 电路板 和 acc_gyro_ddof 传感器.

准备:

这本不需要太高深的数学知识,只需要找一本好的矩阵方面的书. 如果你想温固一下你的知识,可以阅读下面的文章:

直角坐标系 :http://en.wikipedia.org/wiki/Cartesian_coordinate_system Rotation

旋转- http://en.wikipedia.org/wiki/Rotation_%28mathematics%29 Vector scalar product

向量叉乘- http://en.wikipedia.org/wiki/Dot_product Vector cross product

矩阵乘- http://en.wikipedia.org/wiki/Cross_product Matrix Multiplication

块矩阵- http://en.wikipedia.org/wiki/Matrix_multiplication Block Matrix

块矩阵转置- http://en.wikipedia.org/wiki/Block_matrix Transpose Matrix

三重积- http://en.wikipedia.org/wiki/Transpose Triple Product - http://en.wikipedia.org/wiki/Triple_product

标记:

　　向量标记以粗体文字 - 例如“V”是一个向量和“V”是一个标量（如果你不能区分,可以回头看看这里）。(本文使用右手坐标系)

第一部分. DCM 矩阵

一般来说 orientation kinematics 是指计算物品相对全局坐标系的一个方向,体坐标系我们用 Oxyz表示,全局坐标系我们用OXYZ表示.两个坐标系O 点重合(图1)定义i,j,k 为体坐标系中x,y,z轴的相互垂直的单位向量, I,J,K为全局坐标系(global frame )OXYZ 中的单位向量.

由上面的定义可知:

全局坐标系,

　　I^G = {1,0,0} ^T, J^G={0,1,0} ^T , K^G = {0,0,1} ^T

体坐标系:

　　i^B = {1,0,0} ^T, j^B={0,1,0} ^T , k^B = {0,0,1} ^T

向量 i,j,k  在全局坐标系中的表示如下:

　　i^G = {i_x^G , i_y^G , i_z^G} ^T

下面我们对来分析一下 向量 i 在全局坐标系中 X 轴上的投影长度 .  表示为

　　i_x^G = |i| cos(X,i) = cos(I,i)

|i|的值为1 ,cos(I,i)为两个单位向量的余弦值.所以这个公式可以写成:

　　i_x^G = cos(I,i) = |I||i| cos(I,i) = I.i

我们知道向量之间的夹角与所在的坐标系无关,所以我们可以得到

　　I.i = I^B.i^B = I^G.i^G = cos(I^B.i^B) = cos(I^G.i^G)

同样的我们可以得出:

　　i_y^G = J.i , i_z^G=K.i  

所以向量 i 在全局坐标系中的表示形式为:

　　i^G= { I.i, J.i, K.i}^T

以此类推我们可以得到:

　　j^G= { I.j, J.j, K.j} ^T , k^G= { I.k, J.k, K.k} ^T.

最终我们可以得到下面这个"方向余弦矩阵":

　　   (Eq. 1.1)

如果上面这些看懂的话,我们不难得出:

　　I^B= { I.i, I.j, I.k}^T , J^B= { J.i, J.j, J.k}^T , K^B= { K.i, K.j, K.k}^T

　　

观察上面两个矩阵我们很容易可以看出,DCM^B = (DCM^G)^T or DCM^G = (DCM^B)^{T 利用这个属性,我们做下面的推导}.

　　DCM^B. DCM^G = (DCM^G)^T .DCM^G = DCM^B. (DCM^B)^T = I₃  ,这里I₃  是3x3单位矩阵,所以DCM矩阵是正交的.如下图:

(Eq. 1.3)

DCM 矩阵在方向运动(orientation kinematics )有着重要的应用,它可以定义坐标系之间的旋转.下面讲解一下如何使用DCM矩阵,我们定义一个向量r:

在体坐标系中表示   r^B= { r_x^B, r_y^B, r_z^B} ^T 在全局坐标中表示 r^G = { r_x^G , r_y^G , r_z^G } ^T .这里取出

r_x^G = | r^G| cos(I^G,r^G) = | I^B || r^B| cos(I^B,r^B) = I^B. r^B = I^B. { r_x^B, r_y^B, r_z^B} ^{T  ,} 因为 I^B= { I.i, I.j, I.k}^T  所以得出如下公式:

　　r_x^G = I^B. r^B = { I.i, I.j, I.k}^T . { r_x^B, r_y^B, r_z^B} ^T = r_x^B I.i + r_y^B I.j + r_z^B I.k

同理可得  :

　　r_y^G = r_x^B J.i + r_y^B J.j + r_z^B J.k 
　　r_z^G = r_x^B K.i + r_y^B K.j + r_z^B K.k

最终可以得到:

　　   (Eq. 1.4)

从上面这个等式我们可以看出DCM 矩阵可以用于把向量 r 的坐标从B 坐标系转为G 坐标系的坐标.同样我们可以得出下面这个等式:

　　  (Eq. 1.5)

第二部分 .Angular Velocity

　　这里首先定义一个*向量(非零)r,在t时间为表示为r(t) 经过dt时间之后为 r’= r (t+dt) ,其中 dr = r’ – r .

　　

向量u是单位向量与  r x r’ 方向一致:

　　u = (r x r’) / |r x r’| = (r x r’) / (|r|| r’|sin(dθ)) = (r x r’) / (|r|² sin(dθ)) (Eq. 2.1)

定义向量v:

　　 v = dr / dt = ( r’ – r) / dt (Eq. 2.2)    .

在进行下面分析之前,这里我们假设 dt 趋近于 0 ,所以dθ → 0 ,则  v ⊥ r    (Eq. 2.21)    

这里用向量w 表示角速率,所以可以得到下面这个公式:

　　w = (dθ/dt ) u (Eq. 2.3)

结合公式  (Eq. 2.3) 和 (Eq. 2.1):

　　w = (dθ/dt ) u = (dθ/dt ) (r x r’) / (|r|² sin(dθ))

因为 dθ趋近于0 所以sin(dθ) ≈ dθ   进一步对上式化简

　　w = (r x r’) / (|r|² dt)           (Eq. 2.4)

又因  r’ = r + dr , dr/dt = v , r x r = 0   所以w = (r x (r + dr)) / (|r|² dt) = (r x r + r x dr)) / (|r|² dt) = r x (dr/dt) / |r|²

最终 :

　　w = r x v / |r|²­ (Eq. 2.5)    

利用上面这个公式 ,我们可以知道向量v的情况下去求角速度(w) ,另外有一个公式与这个对应,这里不做证明.

　　v = w x r ­ (Eq. 2.6)   

第三部分.   陀螺仪和角速度向量

　　三轴陀螺仪可以分别测出在三个轴上的旋转角速度,这里用 w_x , w_y , w_z 来表示陀螺仪的三个方向的输出值, 单位 rad/s. 同时我们应该知道 dθ_x = w_xdt ,dθ_y = w_ydt ,dθ_z = w_zdt  . 我们用向量的形式表示旋转 w_x = w_x i = {w_x , 0 , 0 }^T , w_y = w_y j = { 0 , w_y , 0 }^T , w_z = w_z k = { 0 , 0, w_z }^T   ( i,j,k  分别为体坐标系中x,y,z 轴上的方向向量).物体的旋转会产生线位移(linear displacement) ,可以用公式表示如下:

　　dr₁ = dt v₁ = dt (w_x x r) ; dr₂ = dt v₂ = dt (w_y x r) ; dr₃ = dt v₃ = dt (w_z x r) .

合成这三个向量,  dr = dr₁ + dr₂ + dr₃ = dt (w_x x r + w_y x r + w_z x r) = dt (w_x + w_y + w_z) x r 

所以等效线速度可以表示为:

　　v = dr/dt = (w_x + w_y + w_z) x r = w x r   这里 　w = w_x + w_y + w_z = {w_x , w_y , w_z }  ,需要强调一点这是在小角的情况下,否则不能像这样简单的合成.

第四部分. DCM 互补滤波器在6DOF或9DOF IMU 传感器上的应用

　　6DOF IMU模块指的是3轴陀螺仪和3轴加速度计.9DOF 模块指的是3轴陀螺仪3轴加速度计和3轴磁阻传感器.这里把全局右手坐标系与地理坐标不系联系起来(Earth's frame)  I向量指向北, K 指向"天", J 向量指向东.如下图:

这里首先定义IMU设备中的坐标系,如下图:

　　　　

　　　　　　图4

　我们已经知道陀螺仪可以测得角速度向量,下面介绍加速度计和磁阻传感器如何所测量值加入到模块中来.假设加速度计测得的值为 A = {A_x , Ay , A_z },  今K^B = -A(与传感器位置有关). 磁阻传感器与加速度计类似,用 M = {M_x , My , M_z }表示测得值,因此I^B = M.知道了I^B 和 K^B 则允许计算 J^B = K^B x I^B(都是单位向量).

因此通过加速度计和磁阻传感器我们可以得到DCM矩阵, 表示为DCM^B 或 DCM^G.

　　DCM^G = DCM^BT = [I^B, J^B, K^B]^T

任何位于体坐标系中的向量利用DCM矩阵可以转化到全局坐标系中来.比如说飞机的机头朝向位于体坐标系统中用r^B = {1,0,0}表示,我们便可以计算得出在全局坐标中的航向,如下面这个公式:(Eq. 1.4)

　　r^G = DCM^G r^B

　　简要介绍一下加速度计和磁阻,陀螺仪传感器的一些特性.陀螺仪瞬时的测量比较准确,但是有漂移,这里加计和磁阻起到观测者的作用.同时陀螺仪不知道哪里是"北""天"所以多个传感器可以融合起来得到准确的空间方位.下面进行介绍如何使用DCM 计算方位.

每一行来看,我们可以得到三个向量 I^B, J^B, K^B.我们会经常用到这个向量K^B (这个可以用加速度计测量得到),向量I^B(可以于磁阻传感器测得).向量J^B 可以简单计算得到J^B = K^B x I^B (单位向量,且正交).

我们定义K 向量在t₀ 时刻 在体坐标系内的值为K^B_0. 陀螺仪的输出值为w = {w_x , w_y, w_z }.利用陀螺仪我们想知道经过一个小的时间间隔 dt 的 K^B_{1G .}我们使用公式 (Eq. 2.6):

　　K^B_1G ≈ K^B₀ + dt v = K^B₀ + dt (w_g x K^B₀) = K^B₀ + ( dθ_g x K^B₀)

其中 dθ_g = dt w_g.

很明显K^B 也可以直接从加速度计测得,测得的值为K^B_1A .这里测得的角速度我们用 w_a  表示,角位移 θ_{a ­}= dt w_a , 使用公式(Eq. 2.5):

　　w­_a = K^B₀ x v_a / | K^B₀|²

其中 v_a = (K^B_1A­ - K^B₀) / dt ,因为K^B₀为单位向量,所以推出:

　　dθ_{a ­}= dt w_a = K^B₀ x (K^B_1A­ - K^B₀)

读到这里大家也可能已经明白,我们是想融合 K^B_1A  K^B_1G 得到一个新的K^B₁ ,所以首先需要整合dθ ,公式如下:

　　dθ = (s_a dθ_a + s_g dθ_g) / (s_a + s_g­),    s_a s_g  为权重.

这里我们得到一个新的K^B_{1  :}

　　K^B₁ ≈ K^B₀ + ( dθ x K^B₀)   ,dθ 为融合之后的.

类似的我们可以计算:　

　　I^B₁ ≈ I^B₀ + ( dθ x I^B₀)

　　J^B₁ ≈ J^B₀ + ( dθ x J^B₀)

到这里我们完成了从0时刻经过dt 时间到1时刻的DCM矩阵更新.这个矩阵不那么容易受外部加速度的影响,因为有陀螺仪参与更新.到目前我们还没有提及如何使用磁阻传感器.因为我们这里用的是6dof imu传感器,这里介绍如何使用虚拟磁阻,它是一直指向北部.

下面整合磁阻到我们的算法中来.过程与加速度计相似,这里用向量I^B 表示.

　　dθ_{m ­}= dt w_m = I^B₀ x (I^B_1M­ - I^B₀)

融合:

　　dθ = (s_a dθ_a + s_g dθ_g + s_m dθ_m) / (s_a + s_g +_­ s_m)

到这里我们采用相同的方法更新DCM 矩阵

　　I^B₁ ≈ I^B₀ + ( dθ x I^B₀) , K^B₁ ≈ K^B₀ + ( dθ x K^B₀) and J^B₁ ≈ J^B₀ + ( dθ x J^B₀)

在实际运用中,我们会用J^B₁ = K^B₁ x I^B₁ ,在重新校正 K^B₁ I^B₁ 垂直之后.强调一点,所有的计算都是在小的dt 基础之上的,否则将会出现错误.在每次进行矩阵更新时,我们不能保证是单位正交矩阵,所以每次更新都需要校正.

　　首先我们先了解如果让两个向量重新正交.如下图:

图5

这里向量 a 和 b 接近正交,向量 c为 其叉积.向量 b’为向量a和向量c的叉积. 根据叉积的定义,所以b’及为b校正后的向量.根据矢量三重积进一步推导:

　　b’ = c x a = (a x b) x a = -a (a.b) + b(a.a) = b – a (a.b) = b + d , where d = – a (a.b)   (方案1, a 固定 b 被校正)

这里向量d  称之为修正量.向量d  平行于向量a ,其方向取决于a 与 b 的夹角.

上面我们讨论的是固定向量a ,去寻找向量b’ .我们也可以反过来考虑问题,如下面公式.

　　a’ = a – b (b.a) = a – b (a.b) = a + e, where e = - b (a.b) (方案2, b 固定 a 被校正)

所以我们综合考虑,

　　a’ = a – b (a.b) / 2 (方案3, a 和b 都需要校正)
　　b’ = b – a (a.b) / 2

图6

这里为了减少处理器计算量,我们提前计算出  Err = (a.b)/2 ,然后再有这个值去校正两个向量.

　　a’ = a – Err * b
　　b’ = b – Err * a

需要注意一点,在方案三这里没有证明a’ 和 b’ 是正交的,只是进行了直观的推理更接近于90度.

现在我们回到更新DCM 矩阵这个问题上来 ,我们如下的方法进行矩阵更新:

　　Err = ( I^B₁ . J^B₁ ) / 2

　　I^B₁^’ = I^B₁ – Err * J^B₁ 
　　J^B₁^’ = J^B₁ – Err * I^B₁ 
　　I^B₁^’’ = Normalize[I^B₁^’]
　　J^B₁^’’ = Normalize[J^B₁^’]
　　K^B₁^’’ = I^B₁^’’ x J^B₁^’’

然后像这样[a] = a / |a|进行向量的单位化,重复上面的过程,我们得到DCM_{2 ,} DCM_{3 ,}  ....DCM _n .

其原作者的代码  https://code.google.com/p/picquadcontroller/source/browse/trunk/imu.h

参考:

1. Theory of Applied Robotics: Kinematics, Dynamics, and Control (Reza N. Jazar)

2. Linear Algebra and Its Applications (David C. Lay)

3. Fundamentals of Matrix Computations (David S. Watkins)

4. Direction Cosine Matrix IMU: Theory (W Premerlani)

有翻译不对的地方希望大家给予反馈,我们及时更新.