1. 引言

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

2. 軸角/旋轉向量

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

$r=[x,y,z,\theta]$

其中單位向量 $\vec{n}=[x,y,z]$ 對應的是旋轉軸， $\theta$ 對應的是旋轉角度。旋轉向量與軸角相同，只是旋轉向量用三個元素來描述旋轉，它把 $\theta$ 角乘到了旋轉軸上，如下：

$r_v=[x*\theta,y*\theta,z*\theta]$

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

3. 羅德里格斯公式

講到軸角轉旋轉矩陣我覺得有必要介紹一下羅德里格斯公式。現在假設有一個慣性座標系{A}，一個運動座標系{B}原點始終與{A}重合，座標系{A}與{B}之間某一瞬間的旋轉矩陣爲。假設有一個質點與座標系{B}固連在一起，質點在座標系{B}下的座標爲，在這一瞬時，這個點在座標系{A}下的座標爲，根據旋轉矩陣的定義我們很容易確定和之間的關係如下式：

$p_{A}(t) =R(t) p_{B}$

大學物理中我們知道 $p_{A}(t)$ 其實描述的是質點在座標系{A}下的位移。現在我希望求質點相對於座標系{A}的速度要怎麼求解呢？顯然位移的時間導數是速度，因此我們對上式求導得到以下關係。

$\dot{p}_{A}(t)=\dot{R}(t)p_{B}+R(t)\dot{p}_{B}$

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

$\varphi (t)=f(t)g(t)$

$\dot{\varphi} (t)=\dot{f}(t)g(t)+f(t)\dot{g}(t)$

回到正題，前面已經提到質點與座標系{B}是固連的，也就是說 $p_{B}$ 是個常量，常量的導數是0，因此得到以下等式：

$\dot{p}_{A}(t)=\dot{R}(t)p_{B}$ （1）

大學物理中我們知道在一個慣性系中的質點運動滿足 $v=\dot{p}=\omega \times p$ ，我們知道叉乘運算實際上可以轉化爲矩陣運算：

$\omega \times p=\begin{bmatrix} 0& -\omega_{z} & \omega_{y} \\ \omega_{z}& 0& -\omega_{x} \\ -\omega_{y}& \omega_{x}& 0 \end{bmatrix}p=\hat{\omega}p$

上式中用 $\hat{\omega}$ 代表由 $\omega$ 得到的反對稱矩陣。套用到前面的公式中得到：

$\dot{p}_{A}(t)=\omega\times p_{A}(t)=\hat{\omega}p_{A}(t)=\hat{\omega}R(t)p_{B}$ （2）

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

$\dot{R}=\hat{\omega}R$

這個等式就很有意思了，不知道你是否還記得大學時學的關於e指數求導的知識， $f(x)=e^{ax}$ ，那麼 $\dot{f}(x)=a\cdot e^{ax}$ ，按照這個方式去看上面的式子，你會發現他們在形式上完全相同，那麼這個旋轉矩陣的導數對應的原函數是不是也是一種e指數呢？答案是肯定的，以上微分方程的解如下：

$R=e^{\hat{\omega}t}$

爲了讓答案更明顯，我們可以再進一步，把角速度歸一化，我們定義一個與角速度 $\omega$ 方向一致的單位矢量，設角速度的大小爲 $\dot{q}$ ，那麼你可以得到下面的關係 $\omega=a\dot{q}$ ，同理 $\hat{\omega}=\hat{a}\dot{q}$ ，令 $\dot{q}t=\theta$ ，將這些關係代入以上方程，你會得到一個更清晰的表達：

$R=e^{\hat{a}\theta}$

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

$R(\theta)=E+\hat{a}\theta+\frac{(\hat{a}\theta)^{2}}{2!}+\frac{(\hat{a}\theta)^{3}}{3!}+...$

下面我們研究一下 $\hat{a}$ 這個反對稱矩陣，分別計算一下這個反對稱矩陣的二次方和三次方，注意由於是單位向量，因此元素平方和爲1，根據這個原則我們可以計算得到以下關係：

$\hat{a}^{2}=\begin{bmatrix} x^2-1 & xy & xz\\ xy& y^2-1& yz\\ xz& yz& z^2-1 \end{bmatrix}$

$\hat{a}^{3}=\begin{bmatrix} 0 & z & -y\\ -z&0& x\\y& -x& 0 \end{bmatrix}=-\hat{a}$

有了這兩個關係我們瞭解到不管是多少次方我們總能用 $\hat{a}$ 和 $\hat{a}^{2}$ 來表達。用這個關係來化簡前面的 $R(\theta)$ ，我們多寫幾項找找規律：

$R(\theta)=E+\hat{a}\theta+\frac{\hat{a}^2\theta^{2}}{2!}-\frac{\hat{a}\theta^{3}}{3!}-\frac{\hat{a}^2\theta^{4}}{4!}+\frac{\hat{a}\theta^{5}}{5!}+...$

所以我們看到所有這些項分成了兩類，一類是含有 $\hat{a}$ 的項，一類是含有 $\hat{a}^{2}$ 的項，整理一下：

$R(\theta)=E+\hat{a}(\theta-\frac{\theta^{3}}{3!}+\frac{\theta^{5}}{5!}-...)+\hat{a}^{2}(\frac{\theta^{2}}{2!}-\frac{\theta^{4}}{4!}+...)$

你可以再去查一下正弦函數 $sin(\theta)$ 和餘弦函數 $cos(\theta)$ 的級數展開，會發現他們分別可以與括號中的表達式對應！因此我們得到最終的旋轉矩陣表達式：

$R(\theta)=e^{\hat{a}\theta}=E+\hat{a}sin\theta+\hat{a}^2(1-cos\theta)$

這個等式就是著名的羅德里格斯公式（Rodriguez formula），它描述的是繞任意軸旋轉 $\theta$ 對應的旋轉矩陣！

4. 軸角轉旋轉矩陣

前面介紹了羅德里格斯公式，這裏軸角轉旋轉矩陣就很容易了，我們直接把軸和角度代入羅德里格斯公式就可以得到旋轉矩陣。在這裏，旋轉軸爲，旋轉角度爲 $\theta$ 。代入羅德里格斯公式（ $s_\theta$ 是 $sin\theta$ 的簡寫， $c_\theta$ 是 $cos\theta$ 的簡寫）：

$R(\theta)=e^{\hat{a}\theta}=E+\hat{a}sin\theta+\hat{a}^2(1-cos\theta)=\begin{bmatrix} x^2(1-c_\theta)+c_\theta & xy(1-c_\theta)-zs_\theta & xz(1-c_\theta)+ys_\theta\\ xy(1-c_\theta)+zs_\theta & y^2(1-c_\theta)+c_\theta & yz(1-c_\theta)-xs_\theta\\ xz(1-c_\theta)-ys_\theta & yz(1-c_\theta)+xs_\theta & z^2(1-c_\theta)+c_\theta \end{bmatrix}$

5. 旋轉矩陣轉軸角

旋轉矩陣轉軸角思路上和上一節介紹的旋轉矩陣轉四元數類似。我們還是先蹚個雷，上一節我們提到四元數以及它的相反四元數描述的是同一個旋轉。這個命題對於軸角也成立，繞 $\vec{r}$ 軸旋轉 $\theta$ 角與繞 $-\vec{r}$ 軸旋轉 $-\theta$ 角描述得也是同一個旋轉。這個問題其實很好解釋，我們可以從幾何的角度去思考， $\vec{r}$ 和 $-\vec{r}$ 在三維空間中是共線的，只是方向相反，正好旋轉角度也相反，你可以想象他們其實是在沿着相同的方向旋轉，如下圖所示：

旋轉矩陣中比較特殊的是對角線元素（ $r_{ij}$ 代表旋轉矩陣的第i行第j列的元素）。把對角線元素相加如下：

$r_{11}+r_{22}+r_{33}=x^2+y^2+z^2-(x^2+y^2+z^2)c_\theta+3*c_\theta=1+2c_\theta$

所以 $\theta$ 的求解就簡單了：

$\theta=arccos(\frac{r_{11}+r_{22}+r_{33}-1}{2})$

旋轉軸對應的元素就比較容易了，比如求分量，觀察旋轉矩陣的 $r_{32}$ 和 $r_{23}$ ，將兩者作差然後除以 $2sin\theta$ 即可，另外兩個元素類似。結果如下：

$r=\frac{1}{2sin\theta}\begin{bmatrix} r_{32}-r_{23}\\ r_{13}-r_{31}\\ r_{21}-r_{12} \end{bmatrix}$

接下來開始踩坑，我們知道反餘弦正常是有兩個解的， $\pm \theta$ ，這裏我們只取了一個解，爲什麼呢？這就是我們前面提到的，一旦取 $-\theta$ ，那麼旋轉軸的每一個元素都含有一個 $\frac{1}{2sin\theta}$ 項，因此這些元素也全都加了一個負號，這樣得到的軸角正好是公式中給出的軸角的相反軸角，它們描述的是同一個旋轉，所以我們取一組就可以了。

繼續觀察求解公式發現其中存在分母項 $2sin\theta$ ，這個值是有可能爲0的，當 $\theta=0,\pi$ 時， $2sin\theta$ 等於0（角度的取值範圍是 $[0,\pi]$ ），這個軸角求解式出現表達式奇異的情況。當這種情況出現時我們需要討論一下，將 $sin\theta=0$ 代入到旋轉矩陣中如下：

$R(\theta)=\begin{bmatrix} x^2(1-c_\theta)+c_\theta & xy(1-c_\theta) & xz(1-c_\theta)\\ xy(1-c_\theta)& y^2(1-c_\theta)+c_\theta & yz(1-c_\theta)\\ xz(1-c_\theta) & yz(1-c_\theta) & z^2(1-c_\theta)+c_\theta \end{bmatrix}$

好像沒什麼特別的，事實上，當 $\theta=0,\pi$ 時， $cos\theta$ 可能是1或者-1，即 $\frac{r_{11}+r_{22}+r_{33}-1}{2}$ 取1或者-1，我們利用這一點來判斷實際的旋轉角度到底是還是 $\pi$ 。

當 $\theta=\pi$ 時，旋轉矩陣如下：

$R(\theta)=\begin{bmatrix} 2x^2-1 & 2xy & 2xz\\ 2xy & 2y^2-1 & 2yz\\ 2xz & 2yz & 2z^2-1 \end{bmatrix}$

這時我們找對角線元素最大值來求解對應的元素（這是爲了減小舍入誤差帶來的影響），假設 $r_{11}$ 最大，則我們先求 $x=\sqrt{(r_{11}+1)/2}$ ，對應的可以求出 $y=r_{12}/2x$ ， $z=r_{13}/2x$ 。這裏有一點需要注意，當 $\theta=\pi$ 時，旋轉軸是 $\vec{r}$ 和 $-\vec{r}$ 將產生相同的結果，因此開根號我們取正值對應的一組解作爲旋轉軸。

當 $\theta=0$ 時，情況比較特殊，這時旋轉矩陣是單位陣，旋轉軸可以任意，我們給出一個默認的旋轉軸即可。

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是正的還是負的，如果是正的，那麼 $\theta=0$ ，旋轉軸是任意的，我們默認取軸。

如果v是負的，那麼 $\theta=\pi$ ，爲了避免舍入誤差帶來的影響，我們需要判斷一下哪個絕對值比較大，我們先求絕對值最大的，另外兩個元素計算公式的分母中會包含這個絕對值最大的元素，這樣可以有效屏蔽舍入誤差的影響。

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

7. 總結

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

由於個人能力有限，所述內容難免存在疏漏，歡迎指出，歡迎討論。

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

1. 引言

2. 軸角/旋轉向量

3. 羅德里格斯公式

4. 軸角轉旋轉矩陣

5. 旋轉矩陣轉軸角

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

7. 總結

如何使用 JS 判斷用戶是否處於活躍狀態

通過HPA+CronHPA組合應對業務複雜彈性伸縮場景

❤️‍🔥 Solon Cloud Event 新的事務特性與應用

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

1. SimMechanics/Multibody入門

3. 機器人正運動學---座標系及其變換

9. 機器人正運動學---修改DH參數

1. 機器人簡介

https://yachay.unat.edu.pe/blog/index.php?comment_area=format_blog&comment_component=blog&comment_co

linux以太網驅動總結