向量的方向余弦公式_方向余弦矩阵(DCM)简介
方向余弦矩阵(DCM)简介
——定向运动学简介——
1 前言
这篇文章是翻译Starlino_DCM_Tutorial.pdf而来，
链接为：http://www.starlino.com/dcm_tutorial.html，请 dear readers 点击链接后参考原文内容。如果您发现翻译上存在一些不太理想的方面，请各位多多包涵！
本文主要阐述了无人机方向余弦矩阵的相关知识，并补充了姿态运动学的内容。首先从理论层面进行阐述，并结合实例分析展开讨论。具体而言, 该算法通过综合采集陀螺仪与加速度计的数据信息, 利用方向余弦矩阵(DCM、Direction Consine Matrix)这一数学工具, 从而确定设备在空间中的位置与姿态。
符号说明：向量以粗体文本标记 - 例如，“v”是向量，“v”是标量。
2 DCM矩阵
在研究领域中，定向运动学主要研究内容是计算本体坐标系相对于全局坐标系的相对方位。我们采用本体坐标系定义为Oxyz的方式，并采用另一个全局坐标系定义为OXYZ的形式。两者均以相同原点O为基础。其中本体坐标的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\cdot i代表向量I与向量i之间的点积。\ 用于计算该点积的目的在于确定两向量之间的夹角关系。\ 由于点积的结果不受所选坐标系的影响（只要两向量位于同一系统中），因此可以通过不同的坐标系来计算同一个点积。\ 进而可得：
I\cdot i = I_B \cdot I_B' = I_G \cdot I_G'
即：
I\cdot i = \cos(I_B, I_B') = \cos(I_G, I_G')
为简化起见，在后续讨论中暂且略去上角标的标注。\ In a similar manner, 我们也可以得到：
i_y^G = J\cdot i,\quad i_z^G=K\cdot 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)转置或者DCMG等于(DCMB)转置。简而言之,这两个矩阵之间是可以相互转换的,并且在后续内容中我们将充分利用这一关键特性。
此外，在计算过程中我们发现DCMB \cdot DCMG = (DCMG)^T \cdot DCMG其中单位矩阵I_3具有3\times 3的维度进一步说明行列变换矩阵DCM满足正交条件
Eq.1.3
其中， iGT.iG = |iG||iG|cos(0) = 1 iGT.jG = 0
DCM矩阵（亦称旋转矩阵）在定向运动学领域扮演着关键角色，在此框架下我们能够描述一个空间直角坐标系相对于另一个空间直角坐标系之间的旋转变换关系。当已知任意向量在其本体空间直角坐标系下的分量时，则可通过应用DCM矩阵来计算出该向量在其全局空间直角坐标系下的分量；反过来也是如此。
我们在此假定向量\mathbf{r}_B，在本体坐标系中的分量为\{\dot{r}_x^B, \dot{r}_y^B, \dot{r}_z^B\}^\top。基于现有的方向余弦矩阵DCMG，计算得到其对应于全局坐标系的位置矢量\mathbf{r}_G = \{\dot{r}_x^G, \dot{r}_y^G, \dot{r}_z^G\}^\top。
先从第一个坐标分量rxG开始，rxG = |rG| cos(IG,rG) ，
由定义可知，在旋转变换下不会有任何向量长度发生变更，并且任意两个经同样角度旋转变换后的向量间夹角也不会发生变化；即若在同一旋转变换下表示若干个不同坐标系中的向量，则这些向量间的范数与夹角均会保持一致：||r_G|| = ||r_B||, ||I_G|| = ||I_B|| = 1 并且 cos(I_G, r_G) = cos(I_B, r_B) 。基于此特性即可推导出：
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 = 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中的任意向量rB实现与之对应的旋转坐標系G之间的变换。
相同的逻辑，可以证明逆逻辑可以表示为：
Eq.1.5
最后：
DCMBrG = DCMB DCMGrB = DCMGT DCMGrB = I3rB= rB
3 角速度
至此为止, 我们已确定一种坐标系相对于另一坐标系的方向旋转矩阵的方法, 即DCM（方向余弦矩阵）, 这一方法可使我们方便地相互转换全局与本体坐标系统的信息, 如文中所示于方程组(1-4)与(1-5)所展示。在此一节中, 我们将该旋转矩阵表示为时间函数来进行研究, 这对于构建基于角速度的更新型DCM矩阵规则具有重要意义
对于任一旋转向量r，我们可用符号表示其随时间t的变化关系为r(t)。接下来考虑一个微小的时间间隔dt，并规定以下符号表示：
- 当前时刻的向量值为r(t)
- 时间推进dt后的向量值记作r'(t)
- 向量增量用\Delta{r}表示，则有\Delta{r} = r'(t + dt) - r(t)
图2r(t)
假设在极小的时间间隔内Δt趋近于零，在极小的时间间隔内Δt趋近于零，在极小的时间间隔内Δt趋近于零，在极小的时间间隔内Δt趋近于零，在极小的时间间隔内Δt趋近于零，在极小的时间间隔内Δt趋近于零，在极小的时间间隔内Δt趋近于零，在极小的时间间隔内Δt趋近于零，在极小的时间间隔内Δt趋近于零，在极小的时间间隔内Δt趋近于零
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 \rightarrow 0\ 时（观察），在\ r\ 及其导数\ r'\ 与微分增量\ dr\ 所形成的等腰三角形中（可观察到），可以确定出向量\ r\ 与其微分向量\ dr\ 之间的夹角（我们称之为α）。
α = (π – dθ) / 2 ，由于 dθ→0，所以α→π/2，同时v⊥r。
为了便于理解角运动学中的基本概念, 我们可以引入角速率矢量ω, 其用于描述角度θ的变化速度及其所在的轴线. 具体来说, 角速率矢量ω被定义为: ω = \frac{dθ}{dt}.
w= (dθ/dt ) uEq.2.3
将Eq.2.1代入上式可得：
w= (dθ/dt ) u= (dθ/dt ) (r xr’) / (|r|2 sin(dθ))
当dθ→0时，代入上式可得：
w= (r xr’) / (|r|2 dt) Eq.2.4
将r’ = r + dr代入上式，又由于dr/dt = v , rx r = 0，我们可推导出：
w= (r x (r + dr)) / (|r|2 dt)
= (r x r + rx dr)) / (|r|2 dt)
= rx (dr/dt) / |r|2
w = r x v/ |r|2 Eq.2.5
该段详细阐述了基于已知线速度v演算角速度的方法，在应用矢量运算中的叉乘性质时较为简便。
v=wxrEq.2.6
4 陀螺仪及角速度矢量
三轴MEMS陀螺仪是一种能够感知运动方向并提供运动状态反馈的信息传感器。当我们将该装置应用于本体坐标系时，在分析一类全局坐标系（如地球坐标系）中的矢量时（例如指向天空方向的向量K或指向北方方向的向量I），对于设备内部的位置观察者而言这些向量将围绕着设备中心发生旋转。其中wx wy wz分别表示沿x轴 y轴和z轴方向上的陀螺仪角速度输出 单位为rad/s。当采用均匀的时间间隔dt进行查询时 在此时间段内 地球相对于陀螺仪绕x轴转动的角度增量为dθ_x = wx·dt 绕y轴转动的角度增量为dθ_y = wy·dt 以及绕z轴转动的角度增量为dθ_z = wz·dt 这些旋转矢量可以用角速率矢量来表征：
wx= wxi= {wx, 0, 0}T, wy= wyj= { 0, wy, 0}T , wz= wzk= { 0, 0, wz}T
在其中，在本地坐标系中定义三个正交单位向量i, j, k分别代表各轴方向的位置关系。各个轴的旋转将导致一定的线性位移变化情况出现，并且其线性位移可通过以下公式表示：
dr1 = dt v1= dt (wxxr) ; dr2 = dt v2= dt (wyxr) ; dr3 = dt v3= dt (wzxr)
将三个线性位移相加可得：
dr= dr1+ dr2+ dr3= dt (wxxr+ wyxr+wzxr) = dt (wx+ wy+wz) xr
有此产生的线速度可表示为：
v=dr/dt=(wx+ wy+wz) xr= wxr这里引入w = wx+wy+wz= {wx, wy, wz}
与类似Eq.2.6的形式相似，并且表明由角旋转矢量wx、wy、wz分别对应轴x、y、z的小旋转组合等价于一个总角旋转矢量w= wx + wy + wz = {wx, wy, wz}。特别注意的是，在这种情况下我们假设所有的小旋转都是独立进行的操作。然而，在实际应用中如果将大角度的旋转变换叠加在一起时（即大旋转变换），则需要考虑变换顺序的问题才能保证结论的有效性）。实际上，在陀螺仪查询间隔dt较大的情况下（即dr的变化也相应增大），我们的线性位移近似方法可能导致较大的累积误差。因此在后续分析中我们将详细探讨这一误差来源及其对系统性能的影响。
5 基于6DOF或9DOF IMU传感器的DCM滤波算法
在本文的研究场景中，我们选择右手定则建立全局坐标系，并将其固定于地球表面。其中：
- 向量I指向上方，
- 向量K指向天体方向，
- 向量J指向西方。
六自由度IMU装置由3轴陀螺仪、3轴加速度计组成；
而九自由度IMU装置则增加了3轴磁力计，
从而实现了更高的定位精度和运动感知能力。
图3全局坐标系定义
另外，定义连接到我们的IMU设备的本体坐标系如下：
图4本体坐标系定义
我们已经证实陀螺仪能够准确测定角速度矢量这一事实。让我们考察一下加速度计和磁力计所测得的数据如何被纳入我们的模型中。
加速度计是一种能够感知重力的设备，在地球引力作用下其读数方向与本地水平面垂直。当三轴测得的数据值分别为A_x, A_y, A_z时，在理想条件下（即无外部干扰因素并且系统误差已校正），我们能够确定该装置测得的地心引力场强度值K_B等于负A值（即K_B = -A）。同样地，在类似设计下使用的磁强计功能类似于加速度计测量装置，在这种情况下它能够检测北向磁场方向的变化趋势（此处假设测得的数据并未完全失准）。当经过校正后得到的数据值为M_x, M_y, M_z时，则根据模型设定可知指示磁场方向I_B指向北方（IB = M）。通过上述测量数据即可计算出交变磁场方向J_B = K_B × I_B的结果向量。
基于加速度计和磁强计的数据计算出相应的DCM矩阵值，并将其表示为DCMB或DCMG。
DCMG = DCMBT = [IB,JB,KB]T
到现在为止，在大多数情况下人们会质疑：即使当加速度计与磁场传感器同步捕捉到DCM矩阵时（即方向余弦矩阵），为什么要引入这样的精密传感器呢？陀卡诺（Tachometer）本质上是一种比加速度计与磁场传感器更为精准的技术手段。它可以用来校准来自加速度计与磁场传感器的数据。
仅凭三轴角速度传感器（陀螺仪）无法确定设备的方向信息，即无法准确判定地理北极方向和天体指向的位置。相反地，这些方向信息可以通过结合加速度计和磁力计的数据来实现精确确定。若已知在时间t时设备的指向可表示为DCM矩阵DCM(t)，则可通过借助于陀螺仪推导出更精确的时间更新后的姿态矩阵DCM(t + dt)。若直接通过加速度计和磁力计获取测量值，则其形式上包含了大量的噪声数据：包括非引力加速度效应（即加速度）或由地球磁场以外因素产生的干扰磁场效应。
基于前述事实，在研究过程中我们有必要设计一种具备将三种设备的输出数据进行整合的能力，并以最为精确的方式估计其位置。
将使用到的DCM如下所示：
当我们直接获取DCM的每一行时，则能获得三个重要的向量：IB、JB和KB。重点分析向量KB（可借助加速度计估算）以及向量IB（通常基于磁场传感器测量）。其中JB可通过计算KB与IB的叉积来轻松获得。
在t0时间点时，在本体坐标系中存在一个指向天空的方向向量KB₀。已知此时陀螺仪的测量结果明确给出ω={ω_x, ω_y, ω_z}。在经过一段时间后，在全球坐标系中我们希望确定该固定方向向量的新位置KB₁^G。根据Eq.2.6进行推导
KB1G ≈ KB0 + dtv= KB0 + dt (wgxKB0) = KB0 + ( dθgxKB0)
其中， dθg= dt wg
基于上述分析，在研究如何估算KB时发现了一种新的方法。为此，我们引入了另一种用于估计KB的方法并命名为KB1A。实际情况表明，在实际应用中采用陀螺仪输出所得的结果与采用加速度计输出所得的结果之间存在显著差异。
在此基础上, 我们可以通过逆向的方法计算出物体的转动惯量 J 和转速 ω, 并结合加速度计测得的数据 KB1A 以及式 (2.5) 进行求解
wa= 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θa与dθg的加权算术平均来表示dθ。随后探讨权重这一参数如何影响整体结果，并详细说明其在实验过程中的确定步骤以期达到预期的响应特性和降噪效果。
然后，对KB1的推导类似于如何计算KB1G：
KB1 ≈ KB0 + ( dθxKB0)
同样， dθ可以用来以相同的方式计算DCM矩阵的其他元素：
IB1 ≈ IB0 + ( dθxIB0)
JB1 ≈ JB0 + ( dθxJB0)
三个向量彼此相邻，并均与由相同的极小时间间隔dt所引起的角位移dθ进行叉乘转换生成。简单来说，则是基于先前时刻t₀估算得到的DCM₀矩阵用于计算t₁时刻的DCM₁算法。该算法基于相同的极小时间间隔dt进行递归计算，并可以在任何时间段内连续地推导出所需的所有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θxIB0)
KB1 ≈ KB0 + (dθxKB0)
JB1≈ JB0 + (dθxJB0)
经过对KB₁和IB₁的重新调整使其成为垂直单位矢量后随后计算得到JB₁=KB₁×IB₁需要注意的是我们所采用的方法均为近似处理其结果受到时间步长dt大小的影响当dt增大时误差会随之增大
鉴于此，在向量IB₀、JB₀和KB₀构成了有效的DCM矩阵的情况下（即它们彼此正交且长度为单位长度），我们无法确保IB₁、JB₁和KB₁计算所得结果的正交性或长度特性；然而，在时间步长dt足够小时（即当时间步长dt足够小的时候），所引入的误差相对较小；因此，在每次迭代后都需要对其进行归一化处理。
首先，请考虑"近似正交"的两个单位向量a和b，在它们之间的夹角趋近于90度但并非精确达到此角度。我们需要找到一个与a正交的新向量b'（位于由a和b张成的平面内），这一目标可以通过几何方法轻松实现（图5）。按照叉乘规则计算得到向量c = a × b，则该向量c必然垂直于由a和b所确定的平面。随后再通过叉乘计算得到新的校正向量b' = c × a（即从c出发再次进行叉乘运算），这将是我们所需的那个与原基底向量a正交的新基底组成部分
图5向量正交化
通过三角定律的公式，我们可以推导出：
b’ = c x a = (a x b) x a = –a (a.b) + b(a.a) = b – a (a.b) = b+d
在上述情境下，在固定矢量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矩阵再次引入下一个循环之前实施以下纠正措施：
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’’的组合得到。