方向余弦矩阵(DCM)简介
方向余弦矩阵(DCM)简介
——定向运动学简介——
1 前言
这篇文章是翻译Starlino_DCM_Tutorial.pdf而来，
链接为：http://www.starlino.com/dcm_tutorial.html, dear readers can refer to the original text for understanding, and any dissatisfaction with the translation can be excused! Please be understanding!
本文主要阐述了无人机方向余弦矩阵的相关知识，并同时补充了定向运动学的相关内容。文章首先进行了理论阐述，并紧接着引入了实际案例进行详细探讨。该算法通过融合陀螺仪和加速度计的数据信息实现... 采用方向余弦矩阵（DCM, Direction Cosine Matrix）的方法... 旨在准确计算设备在空间中的方位位置...
符号说明： 向量以粗体文本 标记 - 例如，“v ”是向量，“v”是标量。
2 DCM矩阵
在定向运动学中，我们通常需要解决的问题是确定本体坐标系相对于全局坐标系的相对姿态。为此，在此研究中我们定义了本体坐标系Oxyz，并将其与全局坐标系OXYZ对应起来使用。两个坐标系统共用相同的原点O（如图1所示）。其中其x、y、z轴的方向分别由单位向量\textbf{i}, \textbf{j}, \textbf{k}表示；而全局X、Y、Z轴的方向则由单位向量\textbf{I}, \textbf{J}, \textbf{K}表示。
图1坐标系定义
因此，通过定义，全局坐标系下的单位向量I ，J ，K 表示形式可写为：
I G = {1,0,0}****T , J G={0,1,0}****T , K G = {0,0,1}****T
相应的，本体坐标系下的单位向量i ，j ，k 表示形式可写为：
i B = {1,0,0}****T , j B={0,1,0}****T , k B = {0,0,1}****T
我们现在考察是否可能用全局坐标系来表示这些基本向量 i, j, 和 k 。为了具体说明，请问向量 i 在这个全局坐标系中的具体位置与方向是怎样的？
i G = {ixG , iyG , izG}****T
作为示例，请我们来探讨ixG这一分量的作用吧？其在全局坐标系下的X轴方向上之投影大小即为其值i
ixG = |i | cos(X,i) = cos(I ,i)
其中主要涉及的是单位向量 i* 的长度计算。cos(I, i) 是由向量 I* 和 i* 形成的角度余弦。考虑到 I* 和 i* 都是单位向量
ixG = cos(I ,i) = |I ||i | cos(I ,i) = I.i
其中，
是
点积，
用于计算点积。
无论采用何种坐标系来衡量这些向量，
只要它们在同一系统内表示，
由于旋转不会影响向量之间的夹角。
由此可得：
\mathbf{I}\cdot\mathbf{i} = \mathbf{I}_B\cdot\mathbf{i}_B = \mathbf{I}_G\cdot\mathbf{i}_G = \cos(\mathbf{I}\cdot\mathbf{i}) = \cos(\mathbf{J}\cdot\mathbf{j})，
为了简化表达，
本文后续将忽略\mathbf{I}\cdot\mathbf{i},
\mathbf{J}\cdot\mathbf{j},
\mathbf{K}\cdot\mathbf{k}以及
\cos(\mathbf{I}, \boldsymbol{\imath}),
\cos(\boldsymbol{\jmath}, \boldsymbol{\jmath}),
\cos(\boldsymbol{k}, \boldsymbol{k})的上标。
同样地，
我们可以得到：
\mathrm{i}_y^G = \boldsymbol{\jmath}\cdot\mathrm{i},
\mathrm{i}_z^G = \boldsymbol{k}\cdot\mathrm{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, J_B, K_B来表征全局坐标的单位基向量，并且这些基向量之间具有对称性；通过方便地实现这些基向量之间的互换其作用效果就可以轻松地完成转换操作
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
组成矩阵的形式为：
**
**Eq.1.2
不难发现，在某些情况下有DCMB = (DCMG)^{T}或者DCMG = (DCMB)^{T}成立；换句话说就是说这两个矩阵之间存在相互转换的关系；这将是我们后续处理中的一个重要依据。
另外, 通过观察可以得出以下结论: 我们能够发现, DCMB 等于 DCMG 的转置乘以 DCMG, 即 DCMB = (DCMG)^T \cdot DCMG, 而 DCMG 则等于 DCMB. 换言之, D(MB)^T = I_3, 其中 I_3 是一个 3×3 的单位矩阵, 换言之, DCM 是正交矩阵
**Eq.1.3
其中， i GT.****i G = |****i G||****i G|cos(0) = 1 i GT.****j G = 0
DCM矩阵（通常也称为旋转矩阵）在其所属领域中扮演着至关重要的角色，在定向运动学中尤其突出其作用
假设将向量\mathbf{r}_B定义为\{\hat{r}_{xB}, \hat{r}_{yB}, \hat{r}_{zB}\}^T在本体坐标系中，请通过已知的旋转矩阵DCMG计算其在全局坐标系中的位置\mathbf{r}_G = \{\hat{r}_x G, \hat{r}_y G, \hat{r}_z G\}^T。
先从第一个坐标分量rxG开始，rxG = |****r G| cos(I G,r G) ，
按照定义，在数学中旋转被视为一种特定的线性变换。这种变换具有两个关键性质：它保持向量的方向和大小不变；同样地，在经过相同旋转变换后的位置上计算两个向量之间的夹角时也会得到相同的度数。无论在哪个旋转变换后的坐标系中进行表示时所计算出的各个向量间的距离与夹角都不会发生变化。基于以上性质我们可以推导出以下结论：对于任意给定的目标点G和参考点B来说，在旋转变换后的坐标系中的各个分量值满足以下关系式：
|r_G^G| = |r_B^B|, \quad |i_G^G| = |i_B^B| = 1
并且：
\cos(i_G^G, r_G^G) = \cos(i_B^B, r_B^B)
rxG = |****r G| cos(I G,r G) = |****I B ||****r B| cos(I B,r B) = I B.r B = I B. { rxB, ryB, rzB} T
上式代入I B= {I.i, I.j, I.k}T
rxG = I B.r B = {I.i, I.j, I.k}T.{**** rxB, ryB, rzB} T = rxBI.i + ryBI.j + rzBI.k
以相同的方式，可以表示出：
ryG = rxBJ.i + ryBJ.j + rzBJ.k
rzG = rxBK.i + ryBK.j + rzBK.k
最后，让我们以一个更紧凑的矩阵形式写：
**
**Eq.1.4
由于DCM矩阵的应用，在该坐标系B内表示的任意矢量r_B可被用来转换到与旋转相关的坐标系G。
相同的逻辑，可以证明逆逻辑可以表示为：
**
**Eq.1.5 ****
最后：
DCMBr G = DCMB DCMGr B = DCMGT DCMGr B = I3r B = r B
3 角速度
到目前为止，在坐标系转换领域中我们已经建立了一个方法来制定一个坐标系相对于另一坐标系的方向旋转矩阵，并命名为DCM（方向余弦矩阵）。这个DCM矩阵通过Eq.1.4和Eq.1.5提供了简便地在全局坐标系与本体坐标系之间来回转换的可能性。在本节中我们将深入研究旋转矩阵的时间依赖性并探讨如何基于角速度对DCM矩阵进行更新。
设任意一旋转向量为\mathbf{r}, 其关于时间t的变化关系定义为\mathbf{r}(t)。为了便于分析, 我们考虑微小的时间增量dt, 并在此基础上建立符号表示法: \mathbf{r}=\mathbf{r}(t), \mathbf{dr}=\mathbf{dr}(t+dt), 和微分位移\mathrm{d}\mathbf{ r}=\mathbf{\dot r}(t+dt)-\mathbf{\dot r}(t)
图2r(t)
在时间间隔dt趋近于零的情况下，在矢量空间中以单位矢量\mathbf{u}作为旋转轴使矢量\mathbf{r}转过微小角度dθ到达\mathbf{r'}的位置，在这种情况下单位矢量\mathbf{u}处于与由\mathbf{r}和\mathbf{r'}所构成平面正交的位置上；由于\mathbf{u}是单位矢量可知其模长|\mathbf{u}|=1，并且其方向与向量叉积\mathbf{r}\times\mathbf{r'}一致；经过一系列推导过程可得：
u =(r x r ’) / |r x r ’| =(r x r ’) / (|r ||r ’|sin(dθ)) = (r xr ’) / (|r |2 sin(dθ)) **Eq.2.**1
因为旋转并不会改变一个矢量的长度，所以有|****r ’| = |r |。
矢量r 的线速度向量可以定义为：
v = dr / dt = (****r ’ – r) / dt **Eq.2.**2
值得我们注意的是，在dt趋近于零的情况下，向量\mathbf{r}与d\mathbf{r}所成的角度（我们将其定义为α）可以通过分析由\mathbf{r}、\mathbf{r}'和d\mathbf{r}构成的等腰三角形来确定。
α = (π – dθ) / 2 ，由于 dθ→0，所以α→π/2，同时v ⊥r 。
通过以下方式描述角速率矢量：表征角度θ的变化率以及旋转轴的具体方向。
w = (dθ/dt ) **u****Eq.2.**3
将Eq.2.1 代入上式可得：
w = (dθ/dt ) u = (dθ/dt ) (r x****r ’) / (|r |2 sin(dθ))
当dθ→0时，代入上式可得：
w = (r x****r ’) / (|r |2 dt) **Eq.2.**4
将r ’ = r + dr 代入上式，又由于dr /dt = v , r x r = 0 ，我们可推导出：
w = (r x (r + dr)) / (|r |2 dt)
= (r x r + r x dr)) / (|r |2 dt)
= r x (dr /dt) / |r |2
w = r x v / |r |2 **Eq.2.**5
上式描述了从已知线速度 v 推导角速度的方法，并利用其运算性质进行推导相对较为直接。
v =w xr**Eq.2.**6
4 陀螺仪及角速度矢量
三轴MEMS陀罗仪是一种能够感知运动轴的装置。当我们将它应用于主体坐标系时，进而分析一种全局坐标系（如地球坐标系）中的矢量。例如，在地球指向天的方向上有一个矢量K，在北方向有一个矢量I，在设备内部观察者看来这些向量将围绕设备中心旋转。其中wx, wy, wz分别代表沿x轴、y轴和z轴的陀罗仪角速度输出值（单位：rad/s）。当我们按照固定的小时间间隔dt连续查询陀罗仪时，则可知在该时间段内地球绕着陀罗仪x轴旋转的角度为dθx = wxdt；绕着y轴旋转的角度为dθy = wydt；以及绕着z轴旋转的角度为dθz = wzdt。这些旋转矢量可用角速率向量来表示：
w x**** = wxi = {wx , 0 , 0 }T , w y = wyj = { 0 , wy , 0}T , w z = wz****k = { 0 , 0, wz }T
其中，在本地坐标系中，变量 i, j, k 分别代表各轴的方向向量。每个轴的旋转过程会产生相应的线性位移量；这些位移可以用数学公式来描述。
dr 1 = dt v 1**** = dt (w x**** xr) ; dr 2 = dt v 2**** = dt (w y**** xr) ; dr 3 = dt v 3**** = dt (w z**** x****r)
将三个线性位移相加可得：
dr = dr 1 + dr 2 + dr 3 = dt (w x**** xr + w y xr +w z xr) = dt (w x**** + w y**** +w z) xr
有此产生的线速度可表示为：
v =dr/ dt=(w x**** + w y**** +w z) xr = w xr 这里引入w = w x+w y+w z= {wx , wy , wz }
如上式所示类似式（2.6），并且揭示了由角速度矢量wx, wy, wz所表示的轴x、y、z三个微小转动组合等效于角速度矢量w = wx + wy + wz = {wx, wy, wz}。值得注意的是，在一般情况下结合大角度转动时转动顺序变得至关重要因此不能简单地得出上述结论。在此问题中我们主要假设由于我们采用式（2.6）从线性位移到微小角度转动的过程dt非常小时所以dθ与dr的变化也非常微小事实上陀螺仪查询间隔dt越大累积误差也会越大为此我们将后续工作集中在解决这一误差问题上
5 基于6DOF或9DOF IMU传感器的DCM滤波算法
在本文所讨论的情境中
图3全局坐标系定义
另外，定义连接到我们的IMU设备的本体坐标系如下：
图4本体坐标系定义
已证实陀螺仪可监测角速度矢量的基础数据。接下来，请评估加速度计与磁力计的数据输入方式。
加速度计是一种能够检测重力的装置；其重力矢量方向地心，并与基准矢量K_B方向相反。当无外力作用下且误差已修正的情况下，则可确定K_B = -A；磁强计类似于加速度计装置；其输出存在不准确性并且时常需要纠正和初始校准；当经过纠正后的3轴磁场输出为M = {Mx, My, Mz}时，则根据假设模型I_B指向北向，则有I_B = M；确认I_B及K_B后即可计算出J_B = K_B × I_B
因此基于加速度计和磁强计的输出结果即可生成DCM矩阵并将该矩阵分别表示为 DCMB 或 DCMG
DCMG = DCMBT = [I B,****J B,****K B]T
至此, 你可能会好奇：既然加速度计与磁力计在任何时间点都能输出DCM矩阵, 那么我们为什么要引入陀螺仪呢? 事实上, 陀螺仪本质上是一种超越加速度计与磁力计精度水平的先进设备, 它主要负责对加速度计与磁力计返回的结果进行精细校准
仅凭陀螺仪无法确定设备的具体方向；也就是说，在没有方位信息的情况下，
我们无法通过常规的方法找到北方及指南针的方向；
然而，
如果已知在时间t时设备的指向，
记作DCM矩阵DCM(t)，
那么通过利用陀螺仪的数据可以进一步提高方向精度；
具体而言，
如果仅仅通过加速度计和磁力计测得值来进行计算，
其在形式上受到多种干扰因素的影响；
其中包括外部（非重力）惯性力（即加速度）
以及并非由地球磁场所导致的真实磁场变化。
基于上述事实，在寻求一种能够整合三种设备输出数据的有效算法方面存在需求时，请注意我们将在接下来的内容中直接介绍该算法的具体设计与实现方式
将使用到的DCM如下所示：
如果我们直接读取DCM的行，则会得到三个向量：\mathbf{I}_B, \mathbf{J}_B 和 \mathbf{K}_B；其中重点讨论的是\mathbf{K}_B（可以通过安装在设备上的加速度计来估算）以及\mathbf{I}_B（可以通过安装在设备上的磁场传感器来估算），而\mathbf{J}_B则可通过简单的数学运算获得：\mathbf{J}_B = \mathbf{K}_B \times \mathbf{I}_B）。
在时间t0时，在本体坐标系中指向天空的方向可以用矢量K B0表示。与此同时，在该时刻陀螺仪的读数已知，并表示为向量w={wx, wy, wz}。经过一小段时间后，则关心指向天空的新位置，并将其标记为K B1G。通过式（2.6）可知：
K B1G ≈ K B0 + dtv = K B0 + dt (w g xK B0) = K B0 + ( dθ g xK B0)
其中， d**θ g **= dt w g
估算K B的另一种方法是通过加速度计输出来计算，在本研究中将其标记为 **K₁A **。在实际情况下，在使用陀螺仪和加速度计两种传感器进行数据采集时所得到的结果往往存在差异。
目前我们可以通过反方向的方法来估算物体的角速度 w_a 或者其角位移 dθ_a = dt * w_a。基于加速度计测得的数据 K_B1A 和 Eq.2.5 中的结果。
w a**** = K B0 x **v a **/ |K B0|2其中v a = (K B1A – K B0) / dt
其中v a = (K B1A – K B0) / dt, 几乎等于为K B0的线速度，并且|****K B0|2 = 1。
从而可得出：
dθ a =**** dt **w a **= K B0 x (K B1A – K B0)
该方法的核心概念基于将 dθ 视为 dθ a 和 dθ g 的加权平均：即 dθ 等于 sa 乘以 dθ a 加上 sg 乘以 dθ g 再除以 sa 加 sg；随后将探讨权重设置以实现预期响应与降噪效果
然后，对K B1的推导类似于如何计算K B1G：
K B1 ≈ K B0 + ( dθ x****K B0)
同样， dθ 可以用来以相同的方式计算DCM矩阵的其他元素：
I B1 ≈ I B0 + ( dθ x****I B0)
J B1 ≈ J B0 + ( dθ x****J B0)
这三个向量依次相连，并均源自同一微小时间间隔dt所导致的角位移dθ的叉积转换。
简单来说，
它基于相同的小时间间隔dt进行递归应用，
能够持续不断地计算出DCM矩阵。
该DCM矩阵难以随着时间发生较大漂移，
因为它被固定在指定绝对位置；
不容易受到外部噪声干扰而导致大幅漂移，
因为其利用掉了速率数据进行更新。
到目前为止，并没有提及关于磁力计的相关信息；其原因之一在于它并不存在于所有的IMU单元中。基于上述算法的设计理念，在不使用该设备的情况下依然能够有效运行；然而该方案可能会导致预测结果出现偏差，在这种情况下建议引入一个虚拟的指北磁力计以增强模型的稳定性
下面将会介绍如何将磁力计的输出集成到我们的算法中；事实上这一过程非常简便；这是因为尽管磁力计与加速度计在基本功能上相似但其主要区别在于磁力计并不是用于估计天向而是用于估计北向量 I 而 K 则表示的是指向天体方向；按照我们在处理加速度计时所采用的方法我们可以根据更新后的磁力计量测值来确定系统的角位移
dθ m =**** dt **w m **= I B0 x (I B1M – I B0)
将上式并入加权平均公式可得：
dθ = (sa dθ a +** sg dθ g +** sm dθ m) / (sa + sg + sm)
同样，可通过dθ 求得: DCM1
I B1 ≈ I B0 + (dθ x****I B0)
K B1 ≈ K B0 + (dθ x****K B0)
J B1 ≈ J B0 + (dθ x****J B0)
在经过对 K B₁ 和 I B₁ 的重新校准后（经过对 **K_B1 和 **I_B₁ 的重新校准后），随后计算得到
因此，在构建了一个有效的DCM矩阵的前提下，请注意这些基础矢量必须满足相互正交且为单位向量这一前提条件。基于此，在计算出 I B1,****J B1,****K B1的过程中我们无法保证这些矢量将保持原有的正交性和长度属性。不过值得强调的是当时间步长 dt 足够小时产生的误差是可以接受的范围之内因此我们采取的方法是在每次迭代后对其进行归一化处理以维持数值计算的稳定性
首先让我们探讨如何使两个向量重新达到正交状态。我们关注的是两个近似正交的单位矢量a和b值得注意的是这两个矢量之间的夹角虽接近90度但并非完全精确。我们的目标是找到一个与a垂直并且位于由a和b所形成的平面内的新向量记作b'这一过程相对容易执行（如图5所示）。首先根据叉乘规则计算出c = a × b这个结果必然垂直于包含a和b的平面。接着再计算c × a得到的结果就是我们所需的校正矢量它不仅与a垂直而且位于由a和b所张成的空间内。
图5向量正交化
通过三角定律的公式，我们可以推导出：
b’ = c x a = (a x b) x a = –a (a.b) + b(a.a) = b – a (a.b) = b +****d
在上述情况下, 我们假设矢量\mathbf{a}是恒定的, 并推导出与其正交的校正向量\mathbf{b'}. 针对这一问题进行探讨——固定矢量\mathbf{b}后, 则需要寻找经过修正后的向量\mathbf{a'}.
a’ = a – b (b.a) = a – b (a.b)
在第三种情况下, 我们希望同时对这两个向量进行校正, 我们认为这两个向量具有相同的偏差, 因此可以推断, 在前面所述的两种情况中, 每个对应的向量都应施以半校正。
a’ = a – b (a.b) / 2
b’ = b – a (a.b) / 2
__
图6向量半正交化
我们定义Err= (a.b)/2
可得出a’ = a – Err * b b’ = b – Err * a
请注意，在现有条件下我们无法证实其正交性；然而基于直觉进行推测的结果表明：如果采用上述校正变换，则a' 和b' 之间的角度将接近90°。
在完成上一步骤后，在完成当前循环中的更新操作后实施以下调整措施：
Err = ( I B1 . J B1 ) / 2
I B1’ = I B1 – Err * J B1
J B1’ = J B1 – Err * I B1
I B1’’ = Normalize[I B1’]
J B1’’ = Normalize[J B1’]
K B1’’ = I B1’’ x J B1’’
其中，Normalize[a] = a / |a |
因此，最后我们校正的矩阵DCM1 可以从I B1’’, J B1’’, K B1’’的组合得到。
<完>