方向余弦矩陣(DCM)簡介

方向余弦矩陣(DCM)簡介

——定向運動學簡介——

1   前言

這篇文章是翻譯Starlino_DCM_Tutorial.pdf而來，

鏈接為：http://www.starlino.com/dcm_tutorial.html，各位看官可以對照着原文看，翻譯不盡人意之處，請各位輕拍!

 

這篇文章主要是介紹無人機方向余弦矩陣相關的知識，另外增加了定向運動學的主題。文章先通過一些理論介紹，然后結合一些實際的例子展開討論。該文章的算法是通過融合陀螺儀和加速度計數據，利用方向余弦矩陣(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表示形式可寫為：

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 = {i_x^G , i_y^G , i_z^G} ^T

作為示例，讓我們分析X坐標軸的分量i_x^G，它的值為i向量投影到全局坐標系下X軸上的長度。

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

其中，|i|為單位向量i的范數(長度)，cos(I,i)為向量I與i形成的夾角余弦，由於I與i均為單位向量，上式又可寫成：

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

其中，I.i為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.i , J.j , K.k以及 cos(I,i), cos(J,j), cos(K,k)的上標。類似地，我們可以得出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

現在本體坐標系下的 i, j, k在全局坐標系下有一套完整的表示，我們可以將這些向量組成一個方便的矩陣：

Eq.1. 1

       該矩陣為方向余弦矩陣，很明顯，它是由本體坐標系和全局坐標系單位向量所有可能組合的角度的余弦組成。

類似的，在本體坐標系中表示全局坐標系統的單位向量 I^B, J^B, K^B本質上是對稱的，並且可以簡單的將I, J, K和 i, j, k交換實現，其結果是：

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

現在很容易注意到DCM^B = (DCM^G)^T或者DCM^G = (DCM^B)^T，換句話說，兩個矩陣是可以相互轉換，我們將會在后面使用這一重要屬性。

另外，我們可以發現DCM^B. DCM^G = (DCM^G)^T .DCM^G = DCM^B. (DCM^B)^T = I₃ ，其中I₃是一個3*3的單位矩陣，換句話說DCM是正交的Eq.1. 3

 

其中， i^GT. i^G = | i^G|| i^G|cos(0) = 1    i^GT. j^G = 0

DCM矩陣（通常也稱為旋轉矩陣）在定向運動學中具有很高的重要性，因為它定義了一個坐標系相對於另一個坐標系的旋轉。如果我們知道任意向量在本體坐標系中的坐標，那么久可以利用DCM矩陣確定它在全局坐標系中的坐標，反之亦然。

現在假設在本體坐標系中的向量r^B= { r_x^B, r_y^B, r_z^B} ^T,讓我們通過已知的旋轉矩陣 DCM^G確定其在全局坐標系中的坐標r^G = { r_x^G , r_y^G , r_z^G } ^T。

先從第一個坐標分量r_x^G開始，r_x^G = | r^G| cos(I^G,r^G) ，

根據定義，旋轉是這樣的變換，其不改變向量的尺度，並且不改變經過相同旋轉的兩個向量之間的角度，因此如果我們在不同的旋轉坐標系中表示一些向量，則向量之間的范數和角度將不改變：| r^G| = | r^B| , | I^G| = | I^B| = 1 並且cos(I^G,r^G) = cos(I^B,r^B) 。根據這一屬性可得出：

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矩陣可以用於將在一個坐標系B中表示的任意向量r^B轉換為旋轉的坐標系G。

相同的邏輯，可以證明逆邏輯可以表示為：

 

 

  Eq.1. 5   

最后：

DCM^B r^G = DCM^B DCM^G r^B = DCM^GT DCM^G r^B = I₃ r^B = r^B

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’， u單位矢量垂直於由r和r’形成的平面，由於u為單位矢量，所以|u| = 1並且與 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

因為旋轉並不會改變一個矢量的長度，所以有| 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|² sin(dθ))

當dθ→0時，代入上式可得：

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推導角速度的過程，通過矢量的叉乘特性，很容易推導出：

v = w x r     Eq.2. 6

4   陀螺儀及角速度矢量

三軸MEMS陀螺儀是一個能感應裝置軸運動的裝置，如果我們將該裝置應用於本體坐標系中，然后分析一類全局坐標系(地球坐標系)的矢量，例如指向天的矢量K 或者指向北的矢量I，對於設備內的觀察者來說，這些向量將圍繞着設備中心旋轉。假設 w_x , w_y , w_z分別為軸 x, y , z的對應的陀螺儀的角速度速輸出，單位為rad/s。如果我們以規則的小時間間隔dt查詢陀螺儀，則陀螺儀輸出告訴我們的是在該時間間隔期間，地球圍繞陀螺儀的x軸旋轉角度為dθ_x = w_xdt，圍繞y軸旋轉角度為dθ_y = w_ydt並且關於z軸旋轉角度 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 分別對應着本地坐標系各軸的方向向量。每個軸的旋轉將會產生一定的線性位移，線性位移可表示為：

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 }

上式類似於式Eq.2.6, 並且表明由角旋轉矢量w_x , w_y , w_z表征的軸x，y，z的三個小旋轉的組合等效於由角旋轉矢量w = w_x + w_y + w_z = {w_x , w_y , w_z }。請注意，我們強調這些都是小旋轉，因為一般來說，當你結合大旋轉，旋轉的順序執行變得重要，你不能簡單地得出上述結論。我們的主要假設是，讓我們通過使用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本體坐標系定義

我們已經確立了陀螺儀可以測量角速度矢量的事實。讓我們看看加速度計和磁力計的測量結果將如何進入我們的模型。

加速度計是能感應重力的設備，重力矢量指向地心，與K^B矢量相反。如果加速度計的三軸輸出為A = {A_x , Ay , A_z } ，假設無外力引起的加速度，並且誤差已得到糾正，我們就可以確定K^B= –A。磁強計類似於加速度計，可以感應北向磁場指向，猶如加速度計輸出並不完美並且時常需要糾正和初始校准。如果糾正后的3軸磁場輸出為M = {M_x , My , M_z }，根據我們的假設的模型 I^B指向北，因此有 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矩陣，為什么我們需要陀螺儀？陀螺儀實際上是一個比加速度計和磁力計更精確的設備，它用於“微調”加速度計和磁力計返回的DCM矩陣。

單靠陀螺儀並不能得到設備的方向，即不知道北在哪里和天的指向，但這些可以使用加速度計和磁力計確定，而是如果我們知道在時間t的設備的指向，表示為DCM矩陣DCM(t)，我們可以使用陀螺儀找到更精確的指向DCM(t + dt)。如果直接從加速度計和磁力計估計讀數，其在形式上包含大量的噪聲，包括外部（非重力）慣性力（即加速度）或不是由地球磁場引起的磁力。

基於上述事實，需要尋求一種能夠組合三種設備的輸出數據的算法，用於最有效地估計設備在全局坐標系下的方位，下面我們將直接介紹該算法。

將使用到的DCM如下所示：

 

如果直接讀取DCM的行，我們將得到向量 I^B, J^B, K^B，我們將主要討論向量K^B (可以通過加速度計估計)和向量I^B (可以通過磁力計估計)，向量J^B 可由 J^B = K^B x I^B簡單計算獲得。

假設 t₀時刻我們知道本體坐標系下指向天的矢量，記為 K^B₀，假設此時陀螺儀的輸出我們也確定，記為 w = {w_x , w_y , w_z }。經過一小段時間后，我們想知道指向天定矢量的位置，假設記為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或角位移dθ_{a ­}=dt w_a，通過加速度計的輸出K^B_1A­ 以及Eq.2.5：

w­_a = K^B₀ x v_a / | K^B₀|²­其中v_a = (K^B_1A­ – K^B₀) / dt

其中v_a = (K^B_1A­ – K^B₀) / dt, 幾乎等於為K^B₀的線速度，並且| K^B₀|²­­ = 1。

從而可得出：

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

估計由K^B_1A和 K^B_1G組成的K^B₁方法的基本想法首先是將 dθ看作為dθ_a與 dθ_g的加權平均：dθ = (s_a dθ_a + s_g dθ_g) / (s_a + s_g­)，稍后將討論權重，它們很快地被實驗確定，以便實現期望的響應和噪聲抑制。

然后，對K^B₁的推導類似於如何計算K^B_1G：

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

同樣， dθ 可以用來以相同的方式計算DCM矩陣的其他元素：

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

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

三個向量彼此附接，並且均與由相同小的時間間隔 dt產生的角位移dθ叉乘轉化而來。簡而言之，這就是我們通過先前時刻 t­­₀估計的矩陣DCM₀，推導出 t₁時刻的DCM₁ 算法。它是以相同的小時間間隔dt的遞歸應用，並且可以在任何時間遞推出DCM矩陣。該DCM矩陣不會隨着時間漂移太多，因為它被加速度計固定在指定的絕對位置；同時，不會由於外部加速度計太大噪聲兒產生大漂移，因為DCM矩陣又使用了速率脫落的數據進行更新。

到目前為止，我們並沒有提到一個關於磁力計的詞，其中一個原因是它並不是在所有的IMU單元中都有，上面的算法我們可以不使用它，但是該方法的結果指向會有一定的漂移，或許可以通過引入一個虛擬的指北磁力計，以保障模型的穩定性。

下面將會講如何集成磁力計的輸出到我們的算法中，事實證明，過程非常的簡單，因為磁力計類似於加速度計，唯一的區別是磁力計估計的不是天的指向 K^B，而是估計指北向量 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)

同樣，可通過dθ 求得: DCM_1­

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

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

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

實際上，在將 K^B₁和I^B₁再次校正為垂直單位矢量之后，我們將計算J^B₁ = K^B₁ x I^B₁，注意，我們的所有邏輯都是近似的，並且取決於dt是否足夠小，dt越大，誤差越大。

因此，如果向量I^B₀, J^B₀, K^B₀形成了有效的DCM矩陣，換句話說，它們彼此正交並且是單位向量，我們不能對關於 I^B₁, J^B₁, K^B₁的計算公式得出保證向量的正交性或長度，但是如果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 = ( 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₁^’’

其中，Normalize[a] = a / |a|

因此，最后我們校正的矩陣DCM₁ 可以從I^B₁^’’, J^B₁^’’, K^B₁^’’的組合得到。

<完>