技术心得：方向余弦矩阵(DCM)简介

方向余弦矩阵(DCM)简介

——定向运动学简介——

1 前言

这篇文章是翻译Starlino_DCM_Tutorial.pdf而来，

链接为：各位看官可以对照着原文看，翻译不尽人意之处，请各位轻拍!

这篇文章主要是介绍无人机方向余弦矩阵相关的知识，另外增加了定向运动学的主题。文章先通过一些理论介绍，然后结合一些实际的例子展开讨论。该文章的算法是通过融合陀螺仪和加速度计数据，利用方向余弦矩阵(DCM、Direction Consine Matrix)的方法，以估计设备在空间中的方位。

符号说明：向量以粗体文本标记 - 例如，“v”是向量，“v”是标量。

2 DCM矩阵

通常来说，定向运动学处理的问题为计算本体坐标系相对全局坐标系的相对方位。我们将本体坐标系定义为Oxyz，另外一个全局坐标系定义为OXYZ，两坐标系的具有相同的原点O，如图 1所示。本体坐标系的x、y、z轴对应的单位向量分别为i，j，k；全局坐标系X、Y、Z轴对应的单位向量分别为I，J，K。

图1坐标系定义

因此，通过定义，全局坐标系下的单位向量I，J，K表示形式可写为：

IG = {1,0,0} T, JG={0,1,0} T , KG = {0,0,1} T

相应的，本体坐标系下的单位向量i，j，k表示形式可写为：

iB = {1,0,0} T, jB={0,1,0} T , kB = {0,0,1} T

现在让我们看看是否可以在全局坐标系下表示向量i，j，k。让我们以向量i为例，写出它在全局坐标系下的坐标：

iG = {ixG , iyG , izG} T

作为示例，让我们分析X坐标轴的分量ixG，它的值为i向量投影到全局坐标系下X轴上的长度。

ixG = |i| cos(X,i) = cos(I,i)

其中，|i|为单位向量i的范数(长度)，cos(I,i)为向量I与i形成的夹角余弦，由于I与i均为单位向量，上式又可写成：

ixG = cos(I,i) = |I||i| cos(I,i) = I.i

其中，I.i为I与i的标量(点)乘积，为了计算标量积I.i。标量积在哪个坐标系中测量这些矢量并不重要,只要他们在同一个系统中表示，因为旋转不改变矢量的夹角。因此：I.i = IB.iB = IG.iG = cos(IB.iB) = cos(IG.iG) ，为了表示简便，本文一下部分将忽略I.i , J.j , K.k以及 cos(I,i), cos(J,j), cos(K,k)的上标。类似地，我们可以得出iyG = J.i , izG=K.i 。

所以现在我们可以将全局坐标系中的向量i写为：iG= { I.i, J.i, K.i}T

此外，类似的可以得出jG= { I.j, J.j, K.j} T , kG= { I.k, J.k, K.k} T

现在本体坐标系下的 i, j, k在全局坐标系下有一套完整的表示，我们可以将这些向量组成一个方便的矩阵：

Eq.1. 1

该矩阵为方向余弦矩阵，很明显，它是由本体坐标系和全局坐标系单位向量所有可能组合的角度的余弦组成。

类似的，在本体坐标系中表示全局坐标系统的单位向量 IB, JB, KB本质上是对称的，并且可以简单的将I, J, K和 i, j, k交换实现，其结果是：

IB= { I.i, I.j, I.k}T , JB= { J.i, J.j, J.k}T , KB= { K.i, K.j, K.k}T

组成矩阵的形式为：

Eq.1. 2

现在很容易注意到DCMB = (DCMG)T或者DCMG = (DCMB)T，换句话说，两个矩阵是可以相互转换，我们将会在后面使用这一重要属性。

另外，我们可以发现DCMB. DCMG = (DCMG)T .DCMG = DCMB. (DCMB)T = I3 ，其中I3是一个33的单位矩阵，换句话说DCM是正交的Eq.1. 3

其中， iGT. iG = | iG|| iG|cos(0) = 1 iGT. jG = 0

DCM矩阵（通常也称为旋转矩阵）在定向运动学中具有很高的重要性，因为它定义了一个坐标系相对于另一个坐标系的旋转。如果我们知道任意向量在本体坐标系中的坐标，那么久可以利用DCM矩阵确定它在全局坐标系中的坐标，反之亦然。

现在假设在本体坐标系中的向量rB= { rxB, ryB, rzB} T,让我们通过已知的旋转矩阵 DCMG确定其在全局坐标系中的坐标rG = { rxG , ryG , rzG } T。

先从第一个坐标分量rxG开始，rxG = | rG| cos(IG,rG) ，

根据定义，旋转是这样的变换，其不改变向量的尺度，并且不改变经过相同旋转的两个向量之间的角度，因此如果//代码效果参考：http://hnjlyzjd.com/hw/wz_24840.html

rxG = | rG| cos(IG,rG) = | IB || rB| cos(IB,rB) = IB. rB = IB. { rxB, ryB, rzB} T

上式代入IB= { I.i, I.j, I.k}T

rxG = IB. rB = { I.i, I.j, I.k}T . { rxB, ryB, rzB} T = rxB I.i + ryB I.j + rzB I.k

以相同的方式，可以表示出：

ryG = rxB J.i + ryB J.j + rzB J.k

rzG = rxB K.i + ryB K.j + rzB K.k

最后，让我们以一个更紧凑的矩阵形式写：

Eq.1. 4

因此，DCM矩阵可以用于将在一个坐标系B中表示的任意向量rB转换为旋转的坐标系G。

相同的逻辑，可以证明逆逻辑可以表示为：

Eq.1. 5

最后：

DCMB rG = DCMB DCMG rB = DCMGT DCMG rB = I3 rB = rB

3 角速度

到目前为止，我们已经有一个方法来定义一个坐标系相对于另一坐标系的方向旋转矩阵，DCM矩阵，它允许我们使用Eq.1.4和Eq.1.5容易地来回转换全局和本体坐标系。在本节中，我们将旋转矩阵作为时间的函数进行分析，这将有助于我们建立更新基于角速率的DCM矩阵的规则。

假设任意一旋转向量r，并将其关于时间t的关系定义为 r(t)。现在考虑很小的时间间隔dt，并做出如下符号：r = r (t) , r’= r (t+dt) 和dr = r’ – r:

图2 r(t)

假设非常小的时间间隔内dt→0，矢量r以单位矢量u为旋转轴旋转了角度dθ到r’， //代码效果参考：http://hnjlyzjd.com/hw/wz_24838.html

u = (r x r’) / |r x r’| = (r x r’) / (|r|| r’|sin(dθ)) = (r x r’) / (|r|2 sin(dθ)) Eq.2. 1

因为旋转并不会改变一个矢量的长度，所以有| r’| = |r|。

矢量r的线速度向量可以定义为：

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

请注意，由于我们的dt→0因此向量r和dr之间的角度（我们称之为α）可以从由r，r’和dr构成的等腰三角形中找到。

α = (π – 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 x r 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。这些旋转矢量可以用角速率矢量表征：

wx = wx i = {wx , 0 , 0 }T , wy = wy j = { 0 , wy , 0}T , wz = wz k = { 0 , 0, wz }T

其中， i,j,k 分别对应着本地坐标系各轴的方向向量。每个轴的旋转将会产生一定的线性位移，线性位移可表示为：

dr1 = dt v1 = dt (wx x r) ; dr2 = dt v2 = dt (wy x r) ; dr3 = dt v3 = dt (wz x r)

将三个线性位移相加可得：

dr = dr1 + dr2 + dr3 = dt (wx x r + wy x r + wz x r) = dt (wx + wy + wz) x r

有此产生的线速度可表示为：

v=dr/dt=(wx + wy + wz) x r = w x r这里引入w = wx+wy+wz= {wx , wy , wz }

上式类似于式Eq.2.6, 并且表明由角旋转矢量wx , wy , wz表征的轴x，y，z的三个小旋转的组合等效于由角旋转矢量w = wx + wy + wz = {wx , wy , wz }。请注意，我们强调这些都是小旋转，因为一般来说，当你结合大旋转，旋转的顺序执行变得重要，你不能简单地得出上述结论。我们的主要假设是，让我们通过使用Eq.2.6从线性位移到旋转的dt非常小，因此旋转dθ和线性位移dr也很小。实际上，这意味着陀螺仪查询间隔dt越大，我们的累积误差越大，我们稍后将处理这个误差。

5 基于6DOF或9DOF IMU传感器的DCM滤波算法

在本文的上下文中，6DOF设备是由3轴陀螺仪和3轴加速度计组成的IMU设备。9DOF设备是3轴陀螺仪，3轴加速度计和3轴磁力计的IMU设备。现在将基于右手定则的全局坐标系固定于地球，向量I指向北，向量 K指向天，向量J指向西。

图3全局坐标系定义

另外，定义连接到我们的IMU设备的本体坐标系如下：

图4本体坐标系定义

我们已经确立了陀螺仪可以测量角速度矢量的事实。让我们看看加速度计和磁力计的测量结果将如何进入我们的模型。

加速度计是能感应重力的设备，重力矢量指向地心，与KB矢量相反。如果加速度计的三轴输出为A = {Ax , Ay , Az } ，假设无外力引起的加速度，并且误差已得到纠正，我们就可以确定KB= –A。磁强计类似于加速度计，可以感应北向磁场指向，犹如加速度计输出并不完美并且时常需要纠正和初始校准。如果纠正后的3轴磁场输出为M = {Mx , My , Mz }，根据我们的假设的模型 IB指向北，因此有 IB = M。确认IB和KB后便可计算出JB = KB x IB。

因此根据加速度计和磁强计的输出便能给出DCM矩阵，表示为 DCMB或者DCMG。

DCMG = DCMBT = 【IB, JB, KB】T

到目前为止，你可能会问自己，如果加速度计和磁力计在任何时间点能给出DCM矩阵，为什么我们需要陀螺仪？陀螺仪实际上是一个比加速度计和磁力计更精确的设备，它用于“微调”加速度计和磁力计返回的DCM矩阵。

单靠陀螺仪并不能得到设备的方向，即不知道北在哪里和天的指向，但这些可以使用加速度计和磁力计确定，而是如果我们知道在时间t的设备的指向，表示为DCM矩阵DCM(t)，我们可以使用陀螺仪找到更精确的指向DCM(t + dt)。如果直接从加速度计和磁力计估计读数，其在形式上包含大量的噪声，包括外部（非重力）惯性力（即加速度）或不是由地球磁场引起的磁力。

基于上述事实，需要寻求一种能够组合三种设备的输出数据的算法，用于最有效地估计设备在全局坐标系下的方位，下面我们将直接介绍该算法。

将使用到的DCM如下所示：

如果直接读取DCM的行，我们将得到向量 IB, JB, KB，我们将主要讨论向量KB (可以通过加速度计估计)和向量IB (可以通过磁力计估计)，向量JB 可由 JB = KB x IB简单计算获得。

假设 t0时刻我们知道本体坐标系下指向天的矢量，记为 KB0，假设此时陀螺仪的输出我们也确定，记为 w = {wx , wy , wz }。经过一小段时间后，我们想知道指向天定矢量的位置，假设记为KB1G。通过Eq.2.6我们可以得出：

KB1G ≈ KB0 + dt v = KB0 + dt (wg x KB0) = KB0 + ( dθg x KB0)

其中， dθg = dt wg

显然，估计KB的另外一种方法是通过加速度计输出计算，我们这里记为KB1A 。现实情况是，通过陀螺仪输出估计与通过加速度输出估计的值将会不一致。

现在，我们可以用反向的方式，估计角速度 wa或角位移dθa -=dt wa，通过加速度计的输出KB1A- 以及Eq.2.5：

w-a = KB0 x va / | KB0|2-其中va = (KB1A- – KB0) / dt

其中va = (KB1A- – KB0) / dt, 几乎等于为KB0的线速度，并且| KB0|2-- = 1。

从而可得出：

dθa -= dt wa = KB0 x (KB1A- – KB0)

估计由KB1A和 KB1G组成的KB1方法的基本想法首先是将 dθ看作为dθa与 dθg的加权平均：dθ = (sa dθa + sg dθg) / (sa + sg-)，稍后将讨论权重，它们很快地被实验确定，以便实现期望的响应和噪声抑制。

然后，对KB1的推导类似于如何计算KB1G：

KB1 ≈ KB0 + ( dθ x KB0)

同样， dθ 可以用来以相同的方式计算DCM矩阵的其他元素：

IB1 ≈ IB0 + ( dθ x IB0)

JB1 ≈ JB0 + ( dθ x JB0)

三个向量彼此附接，并且均与由相同小的时间间隔 dt产生的角位移dθ叉乘转化而来。简而言之，这就是我们通过先前时刻 t--0估计的矩阵DCM0，推导出 t1时刻的DCM1 算法。它是以相同的小时间间隔dt的递归应用，并且可以在任何时间递推出DCM矩阵。该DCM矩阵不会随着时间漂移太多，因为它被加速度计固定在指定的绝对位置；同时，不会由于外部加速度计太大噪声儿产生大漂移，因为DCM矩阵又使用了速率脱落的数据进行更新。

到目前为止，我们并没有提到一个关于磁力计的词，其中一个原因是它并不是在所有的IMU单元中都有，上面的算法我们可以不使用它，但是该方法的结果指向会有一定的漂移，或许可以通过引入一个虚拟的指北磁力计，以保障模型的稳定性。

下面将会讲如何集成磁力计的输出到我们的算法中，事实证明，过程非常的简单，因为磁力计类似于加速度计，唯一的区别是磁力计估计的不是天的指向 KB，而是估计指北向量 IB。遵循我们对加速度计所做的相同的逻辑，我们可以根据更新的磁力计读数确定角位移为：

dθm -= dt wm = IB0 x (IB1M- – IB0)

将上式并入加权平均公式可得：

dθ = (sa dθa + sg dθg + sm dθm) / (sa + sg +- sm)

同样，可通过dθ 求得: DCM1-

IB1 ≈ IB0 + (dθ x IB0)

KB1 ≈ KB0 + (dθ x KB0)

JB1 ≈ JB0 + (dθ x JB0)

实际上，在将 KB1和IB1再次校正为垂直单位矢量之后，我们将计算JB1 = KB1 x IB1，注意，我们的所有逻辑都是近似的，并且取决于dt是否足够小，dt越大，误差越大。

因此，如果向量IB0, JB0, KB0形成了有效的DCM矩阵，换句话说，它们彼此正交并且是单位向量，我们不能对关于 IB1, JB1, KB1的计算公式得出保证向量的正交性或长度，但是如果dt很小，我们不会得到太大的误差，我们需要做的是在每次迭代之后对它们进行校正化。

首先，让我们看看如何确保两个向量再次正交。让我们考虑“几乎正交”的两个单位矢量a和b，换句话说，这两个矢量之间的角度接近90°，但不是精确的90°。我们要寻找一个与a正交并且在由向量a和b形成的平面中的向量b'，这一向量很容易找到，如图 5所示。首先通过叉乘的规则我们找到向量 c = a x b，c肯定垂直于a与b形成的平面。然后通过叉乘c与a得到b’ = c x a，因此，b’是我们正在寻找的与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

在上面的场景中，我们认为矢量a是固定的，并且我们推导出了与a正交的校正矢量b'。我们可以考虑相对称的问题——通过固定的矢量b，并找到经校正的向量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'是正交的，但是我们提出了直观的推理，即如果我们应用上述校正变换，a'和b'之间的角度将接近90°。

现在回到DCM矩阵的更新，我们在将DCM矩阵重新引入下一个循环之前应用以下纠正措施：

Err = ( IB1 . JB1 ) / 2

IB1’ = IB1 – Err JB1

JB1’ = JB1 – Err * IB1

IB1’’ = Normalize【IB1’】

JB1’’ = Normalize【JB1’】

KB1’’ = IB1’’ x JB1’’

其中，Normalize【a】 = a / |a|

因此，最后我们校正的矩阵DCM1 可以从IB1’’, JB1’’, KB1’’的组合得到。