12. 機器人正運動學---姿态描述之軸角（旋轉向量）

目錄

1. 引言

2. 軸角/旋轉向量

3. 羅德裡格斯公式

4. 軸角轉旋轉矩陣

5. 旋轉矩陣轉軸角

6. 軸角與旋轉矩陣轉換的C++實作

7. 總結

1. 引言

上一篇文章主要介紹了四元數與旋轉矩陣之間的轉換，這篇文章介紹旋轉矩陣與軸角/旋轉向量之間的關系。

2. 軸角/旋轉向量

軸角和旋轉向量本質上是一個東西，軸角用四個元素表達旋轉，其中的三個元素用來描述旋轉軸，另外一個元素描述旋轉的角度,如下所示：

其中機關向量

對應的是旋轉軸，

對應的是旋轉角度。旋轉向量與軸角相同，隻是旋轉向量用三個元素來描述旋轉，它把

角乘到了旋轉軸上，如下：

如果你還記得我們上一篇文章介紹的四元數，會發現姿态的軸角表示法與四元數十分神似。是的，他們确實很像，都是描述了繞某一個軸旋轉一個角度。你可能也聽說過這樣一個定理，任何姿态都可以通過繞某一個軸旋轉特定的角度得到。也就是說隻要兩個坐标系原點是重合的，那麼他們之間的姿态關系一定可以表達為繞某個軸旋轉一個角度。

3. 羅德裡格斯公式

講到軸角轉旋轉矩陣我覺得有必要介紹一下羅德裡格斯公式。現在假設有一個慣性坐标系{A}，一個運動坐标系{B}原點始終與{A}重合，坐标系{A}與{B}之間某一瞬間的旋轉矩陣為

。假設有一個質點

與坐标系{B}固連在一起，質點在坐标系{B}下的坐标為

，在這一瞬時，這個點在坐标系{A}下的坐标為

，根據旋轉矩陣的定義我們很容易确定

和

之間的關系如下式：

大學實體中我們知道

其實描述的是質點

在坐标系{A}下的位移。現在我希望求質點

相對于坐标系{A}的速度要怎麼求解呢？顯然位移的時間導數是速度，是以我們對上式求導得到以下關系。

所謂矩陣的導數就是對各個元素都求導。以上求導法則與函數乘法求導法則一緻，即：

回到正題，前面已經提到質點

與坐标系{B}是固連的，也就是說

是個常量，常量的導數是0，是以得到以下等式：

      （1）

大學實體中我們知道在一個慣性系中的質點運動滿足

，我們知道叉乘運算實際上可以轉化為矩陣運算：

上式中用

代表由

得到的反對稱矩陣。套用到前面的公式中得到：

    （2）

根據式（1）和式（2）我們很容易得到以下關系：

這個等式就很有意思了，不知道你是否還記得大學時學的關于e指數求導的知識，

，那麼

，按照這個方式去看上面的式子，你會發現他們在形式上完全相同，那麼這個旋轉矩陣的導數對應的原函數是不是也是一種e指數呢？答案是肯定的，以上微分方程的解如下：

為了讓答案更明顯，我們可以再進一步，把角速度歸一化，我們定義一個與角速度

方向一緻的機關矢量

，設角速度的大小為

，那麼你可以得到下面的關系

，同理

，令

，将這些關系代入以上方程，你會得到一個更清晰的表達：

以上就是旋轉矩陣的e指數表達，從他的指數項我們可以看出這個旋轉矩陣描述了繞

軸旋轉

得到的旋轉矩陣。接下來的問題是怎麼計算呢？看起來無從下手的樣子，這個時候需要再借助一點點級數展開的知識，e指數是有标準級數展開式的，如果你忘記了可以去網上查一下，我們無需關心這個展開是怎麼來的，用就可以了，e指數展開式應用于以上表達式可以得到：

下面我們研究一下

這個反對稱矩陣，分别計算一下這個反對稱矩陣的二次方和三次方，注意由于

是機關向量，是以元素平方和為1，根據這個原則我們可以計算得到以下關系：

有了這兩個關系我們了解到不管是多少次方我們總能用

和

來表達。用這個關系來化簡前面的

，我們多寫幾項找找規律：

是以我們看到所有這些項分成了兩類，一類是含有

的項，一類是含有

的項，整理一下：

你可以再去查一下正弦函數

和餘弦函數

的級數展開，會發現他們分别可以與括号中的表達式對應！是以我們得到最終的旋轉矩陣表達式：

這個等式就是著名的羅德裡格斯公式（Rodriguez formula），它描述的是繞任意軸

旋轉

對應的旋轉矩陣！

4. 軸角轉旋轉矩陣

前面介紹了羅德裡格斯公式，這裡軸角轉旋轉矩陣就很容易了，我們直接把軸和角度代入羅德裡格斯公式就可以得到旋轉矩陣。在這裡，旋轉軸為

，旋轉角度為

。代入羅德裡格斯公式（

是

的簡寫，

是

的簡寫）：

5. 旋轉矩陣轉軸角

旋轉矩陣轉軸角思路上和上一節介紹的旋轉矩陣轉四元數類似。我們還是先蹚個雷，上一節我們提到四元數以及它的相反四元數描述的是同一個旋轉。這個命題對于軸角也成立，繞

軸旋轉

角與繞

軸旋轉

角描述得也是同一個旋轉。這個問題其實很好解釋，我們可以從幾何的角度去思考，

和

在三維空間中是共線的，隻是方向相反，正好旋轉角度也相反，你可以想象他們其實是在沿着相同的方向旋轉，如下圖所示：

旋轉矩陣中比較特殊的是對角線元素（

代表旋轉矩陣的第i行第j列的元素）。把對角線元素相加如下：

是以

的求解就簡單了：

旋轉軸對應的元素就比較容易了，比如求

分量，觀察旋轉矩陣的

和

，将兩者作差然後除以

即可，另外兩個元素類似。結果如下：

接下來開始踩坑，我們知道反餘弦正常是有兩個解的，

，這裡我們隻取了一個解，為什麼呢？這就是我們前面提到的，一旦取

，那麼旋轉軸的每一個元素都含有一個

項，是以這些元素也全都加了一個負号，這樣得到的軸角正好是公式中給出的軸角的相反軸角，它們描述的是同一個旋轉，是以我們取一組就可以了。

繼續觀察求解公式發現其中存在分母項

，這個值是有可能為0的，當

時，

等于0（角度的取值範圍是

），這個軸角求解式出現表達式奇異的情況。當這種情況出現時我們需要讨論一下，将

代入到旋轉矩陣中如下：

好像沒什麼特别的，事實上，當

時，

可能是1或者-1，即

 取1或者-1，我們利用這一點來判斷實際的旋轉角度到底是

還是

。

當

時，旋轉矩陣如下：

這時我們找對角線元素最大值來求解對應的元素（這是為了減小舍入誤差帶來的影響），假設

最大，則我們先求

，對應的可以求出

，

。這裡有一點需要注意，當

時，旋轉軸是

和

将産生相同的結果，是以開根号我們取正值對應的一組解作為旋轉軸。

當

時，情況比較特殊，這時旋轉矩陣是機關陣，旋轉軸可以任意，我們給出一個預設的旋轉軸即可。

6. 軸角與旋轉矩陣轉換的C++實作

軸角轉旋轉矩陣十分簡單，直接把旋轉矩陣各個元素的公式代入即可，旋轉矩陣轉軸角由于需要分類讨論，邏輯稍微複雜一些，如下：

void Rotation::getAxialAngle(double &x, double &y, double &z,
                             double &theta) const {
  double epsilon = 1E-12;
  double v = (data[0] + data[4] + data[8] - 1.0f) / 2.0f;
  if (fabs(v) < 1 - epsilon) {
    theta = acos(v);
    x = 1 / (2 * sin(theta)) * (data[7] - data[5]);
    y = 1 / (2 * sin(theta)) * (data[2] - data[6]);
    z = 1 / (2 * sin(theta)) * (data[3] - data[1]);
  } else {
    if (v > 0.0f) {
      // \theta = 0, diagonal elements approaching 1
      theta = 0;
      x = 0;
      y = 0;
      z = 1;
    } else {
      // \theta = \pi
      // find maximum element in the diagonal elements
      theta = PI;
      if (data[0] >= data[4] && data[0] >= data[8]) {
        // calculate x first
        x = sqrt((data[0] + 1) / 2);
        y = data[1] / (2 * x);
        z = data[2] / (2 * x);
      } else if (data[4] >= data[0] && data[4] >= data[8]) {
        // calculate y first
        y = sqrt((data[4] + 1) / 2);
        x = data[3] / (2 * y);
        z = data[5] / (2 * y);
      } else {
        // calculate z first
        z = sqrt((data[8] + 1) / 2);
        x = data[6] / (2 * z);
        y = data[7] / (2 * z);
      }
    }
  }
}
           

代碼中的分類讨論就是我們介紹旋轉矩陣轉軸角時的特殊情況，當v的絕對值沒有趨向于1說明不存在表達式奇異的問題，是以直接計算。

當v的絕對值趨向于1我們就要判斷v是正的還是負的，如果是正的，那麼

，旋轉軸是任意的，我們預設取

軸。

如果v是負的，那麼

，為了避免舍入誤差帶來的影響，我們需要判斷一下

哪個絕對值比較大，我們先求絕對值最大的，另外兩個元素計算公式的分母中會包含這個絕對值最大的元素，這樣可以有效屏蔽舍入誤差的影響。

旋轉矩陣與軸角的轉換相關C++源代碼我已經上傳到github: https://github.com/hitgavin/rosws/tree/master/src/frames，感興趣可以參考一下。

7. 總結

這篇文章主要介紹了軸角與旋轉矩陣之間的轉換。姿态描述到這裡就告一段落了，事實上還有一些其他的描述方式，比如旋量等，這部分進階一點，後面有機會我們再介紹。實際上姿态描述和他們之間的轉換有很多開源庫都已經做了，比如Eigen，ROS等（感興趣的可以查閱一下），我們這裡隻是為了夯實基礎做了一些原理上的分析與實作，希望能夠幫助大家更好地了解姿态這個概念。

由于個人能力有限，所述内容難免存在疏漏，歡迎指出，歡迎讨論。