https://blog.csdn.net/u013745804/article/details/79274900

本著看書一定要寫出來才能理解的心態，我開設了這個專欄，目的有兩個：一是督促自己看書學習；二是與大家一起分享自己的讀書感悟，在大家的批評中成長。

      這篇博文我們主要介紹空間描述和變換，相信大家對於其重要性都表示認同。舉個例子，比如我們在做基於視覺的抓取時，不管是眼在手上（eye-in-hand），還是眼在手外（eye-to-hand），總是需要標定相機和執行器末端的關係的，那如何對這二者之間的關係進行表述呢？就是使用這裡將要介紹的空間描述和變換。說白了，就是把我們學過的線性代數的知識拿過來用一用，然後對其在空間中的含義進行理解，而不是單純的瞭解矩陣的數學運算。

      我們知道，在我們生活的三維空間中，對於剛體而言，存在著六個自由度，其中三個用來描述質心位置，而另外三個自由度用來描述整個剛體繞質心的旋轉。所以，我們下面先介紹位置描述，然後介紹姿態描述。

位置描述
      關於位置描述其實大家是比較熟悉的，比如說下面的圖例：

      在這裡，我們在座標系{A}下對向量進行表示：

      用上面的有序三元組對我們在空間中的一個質點的位置進行表示，這三個量是完全獨立的，因此有3個自由度。

姿態描述
      上面我們談到位置描述的時候，關心的物件是質點，因此它本身不需要對其姿態進行描述，故而沒有引入旋轉，但是當我們的研究物件是剛體的時候，則需要在剛體上固定一個座標系，用來表示剛體的旋轉了，這也就是我們引入姿態描述的原因。

      如上圖所示，我們關心機械臂末端夾持器末端中心的位姿（包括位置和姿態）在座標系{A}下的表示。所以我們以該點作為原點，並且選擇三個正交單位向量作為軸，建立一個座標系{B}，這個座標系本身就是我們所尋求的表示：

      這個矩陣就叫做旋轉矩陣，表示{B}相對於{A}的旋轉。其中的三個分量分別為{B}的三個主軸向量在{A}中的表示，因此我們可以將上面的式子寫為：

      這樣，我們就能夠清晰地理解這個矩陣的由來了，並且由於旋轉矩陣是利用{B}的三個主軸單位向量在{A}中的投影得到的，所以它有一些特殊性質，首先它是一個對稱矩陣：

      這個式子的話可以很容易從上一個式子彙總看出來，下面我們來看它的另一個性質，即“旋轉矩陣的逆等於它的轉置”：

      拿單位陣中的第一行第一列的1來說，它其實就是座標系{B}的X軸在座標系{A}中各軸上的投影構成的行向量乘以它構成的列向量，也就等於{B}的X軸本身的長度1，同理，我們可以得到旋轉矩陣的轉置乘以旋轉矩陣等於單位陣（可以從方向餘弦推導），所以說“旋轉矩陣的逆等於它的轉置”。

      可是旋轉矩陣明明有9個值，為什麼只佔了3個自由度呢？因為這些量之間是有約束方程的：每一列的平方和為1，不同列相互正交，這與旋轉矩陣的轉置乘以旋轉矩陣本身等於單位矩陣是一樣的。這麼一算，就有6個約束方程了，所以只有3個自由度，這一點我們後面將會詳細分析。

座標系的描述
      在機器人學中，位置和姿態經常成對出現，於是我們將此組合稱為座標系，比如座標系{B}在座標系{A}中的表示如下：

      值得注意的是，在John J.Craig的《Introduction to Robotics: Mechanics and Control》書中有這樣一句表述：“一個參考系可以用一個座標系相對於另一個座標系的關係來描述”。也就是說，在這裡，Craig將參考系虛化為了一種關係，這與我們平時說的參考系的理解其實不太一樣。“參考系包括位置和姿態兩個概念，大多數情況下被認為是這兩個概念的結合。位置可由一個參考系表示，這個參考系中的旋轉矩陣是單位陣，並且這個參考系中的位置向量確定了被描述點的位置。同樣，如果參考系中的位置向量是零向量，那麼它表示的就是姿態。”從這段話來看，其實將參考系理解為一個變換就好了，比如說如果{B}是由{A}平移得到的，那麼{B}的原點可以由旋轉矩陣為單位陣，平移向量為{A}到{B}的向量的這樣一個組合來表示。後面我們介紹完對映之後，大家將進一步理解這本書裡面的參考系的含義。

對映
      這一節主要介紹平移座標系的對映以及旋轉座標系的對映，其中平移座標系對映的概念相比大家都比較熟悉，所以我們就不仔細討論了：

      P相對於{A}的位置可以表示為：

      值得注意的是，我們已知P在{B}中的位置表示以及{B}相對於{A}的平移，想要計算的是P在{A}中的位置表示，能夠列出上式的前提是，{B}相對於{A}不存在旋轉。如果存在旋轉的話，我們需要了解旋轉座標系的對映：

      我們知道{A}的主軸X在{B}中的表示為：

      這個式子的含義是{A}的X軸上的“1”在{B}中各個軸上的投影，或者反過來說，{B}的各個軸上的“1”往{A}的X軸上的投影。我們將其轉置為行向量，然後左乘到上，中的各個分量為在{B}下的表示，所以就表示P點在{A}下X軸上的投影，也即，同理我們可以推出P點在{A}中Y軸、Z軸上的分量，從而得到：

      顯然，我們可以將其簡化為：

      這個對映將空間中的某個點P相對於{B}的描述轉換成了該點相對於{A}的描述。

關於一般座標系的描述
      在這裡，我們將討論下圖所示的一般情況：

      按照我們在平移對映中所說，如果要運用向量加法求得P在{A}中的表示，則前提是{B}相對於{A}沒有旋轉，而在圖2-7中，這一條件不再成立，所以我們首先需要繞{B}的原點旋轉，使之姿態與{A}一樣，記為座標系{C}。此時P點在{C}座標系下的表示為，而座標系{C}與座標系{A}的姿態一致，所以該表示等價於，於是我們可以得到下面的等式：

      為了更加簡潔的表示上面的式子，我們有：

      其中我們定義齊次變換矩陣為：

      注意該式僅僅只是用於簡化公式推導，但是在計算機程式中一般不用它來進行計算，因為這個變換將把時間消耗在1、0的乘法運算上，這並不是我們想看到的結果。

運算元
      到目前為止，我們討論的內容都集中於如何對一個向量在不同座標系間的表示進行變換上，反過來想，我們可以將這種操作看作是一個向量在固定座標系下的變換。比如說，假設座標系{B}由座標系{A}平移得到，那麼點P從座標系{B}變換到座標系{A}上可以理解為以{A}原點作為起點的向量，按照進行平移得到。同理，假設座標系{B}是由{A}旋轉k角度得到，那麼點P從座標系{B}變換到座標系{A}上可以理解為在座標系{A}中直接將向量旋轉k所得。就好比我們一開始{A}、{B}重合，然後將{A}旋轉-k得到{A'}，然後在{A'}中尋求P的表示，這與我們在{A}中直接將P對應的向量旋轉k是一樣的。

      不論是平移還是旋轉，首先假設初始時刻座標系{A}、{B}重合，然後將點P固定在{B}中，使之隨著{B}的變換而變換，此時，我們可以看見，座標系{B}的變換即是點P在{A}中的座標變換。

混合變換
      如下圖所示，已知，求：

      此時我們可以列出下面的式子：

      求得：

逆變換
      假設我們已知{B}相對於{A}的變換，要求{A}相對於{B}的變換，也即求矩陣的逆。一個很直接的辦法是直接對這個4*4的齊次變換矩陣求逆，另一種方法是利用我們上面說到的變換的性質求逆：

      直接寫出{A}相對於{B}的變換：

變換方程
      這一小節比較簡單，我們直接給出示例：

      由上圖可以列出等式如下：

      如果我們一直其中任意4個變換，便可以求出剩下的一個未知變換。

姿態的其他描述方法
      我們知道旋轉矩陣是單位正交陣，又因為其行列式為1，所以被稱為標準正交陣（非標準正交陣的行列式值為-1）。前面我們聊到旋轉矩陣中有9個量，但是卻只表示了3個自由度，是因為矩陣中的元素存在著6個約束：

      這裡我們假定旋轉矩陣R為：

      所以，旋轉矩陣只有3個自由度。另一種方法是使用正交矩陣的凱萊公式，對於任何正交陣R，都存在一個反對稱矩陣S（）滿足：

      由反對稱矩陣的定義知：反對稱矩陣主對角線上的元全為0，位於主對角線兩側對稱的元反號。所以，一個三維的反對稱矩陣可以由三個引數表示：

      從上面可以看出來，任何3*3的旋轉矩陣可以用3個參量確定。那我們能否找到一種表示方法，直接用三個參量表達姿態呢？大家是不是一下想起了諸如X-Y-Z固定角座標系和Z-Y-X尤拉角座標系？是的，這些表示方法被統一稱為角座標系表示法，共有24種。這類表示法均需要按照一定的順序進行三次繞主軸的旋轉，如果大家有了解過相關知識，或許知道還有一種表示法，只需要繞某一個軸旋轉一次即可，它就是等效軸表示法，這種表示法基於“任何方位都可以通過選擇適當的軸和角度得到”，下面我們將對這些知識點進行討論。

      首先介紹X-Y-Z固定角座標系，為什麼叫固定角座標系？“固定”指的是旋轉是繞著固定的參考系的各個主軸，比如：

      我們對{B}進行旋轉，每次都是繞著固定座標系{A}的軸進行旋轉的，我們常將繞著X、Y、Z的旋轉稱為pitch、yaw、roll，也即俯仰、偏航、翻滾。將三次旋轉利用旋轉矩陣寫出並相乘：

      乘積為：

      接著我們介紹Z-Y-X尤拉角座標系，在這種表示法中，我們每次旋轉都是繞著{B}的主軸進行旋轉，也就是說，並不是繞著固定座標系{A}的軸進行旋轉了：

      在這種表示下，有：

      關於這個式子應該怎麼理解呢？我們最終的目標是求出{B}相對於{A}的旋轉，對於固定角座標系，每次旋轉都是基於{A}的，因而是按照旋轉次序，依次左乘；對於尤拉角則是利用中間座標系變換，所以依次右乘。比如上面的Z-Y-X尤拉角，記{A}繞{A}的Z旋轉得到{B'}，然後{B'}繞{B'}的Y旋轉得到{B''}，最後{B''}繞{B''}的X旋轉得到{B}，所以，我們可以寫出：

      式中的3個旋轉角分別是繞著{A}的Z軸，{B'}的Y軸，{B‘’}的X軸旋轉（所以相對於我們的旋轉，是右乘，先乘以繞X的旋轉，再乘以繞Y的旋轉，最後才是繞Z的旋轉），因此我們可以得出結論：X-Y-Z固定角與Z-Y-X尤拉角在同樣的角度大小下，旋轉所得到的最終結果是一樣的，也就是說在這兩種表示下，{B}相對於{A}的姿態一致。注意，這並不是巧合，是因為固定角表示下是基本旋轉矩陣左乘，而尤拉角表示下是基本旋轉矩陣的右乘，而恰好X-Y-Z與Z-Y-X是相反的旋轉順序，所以最終的效果就一樣了。

      然後介紹一下等效軸角座標表示法，也即用一個單位向量加上一個旋轉角表示旋轉：

      因為我們的單位向量長度恆為1，所以實際上確定它只需要兩個引數，加上旋轉角，也即三個引數，正好確定旋轉所需要的3個自由度。當旋轉軸K為一般軸時，等效旋轉矩陣為：

      除了上面的固定角座標系表示法、尤拉角表示法和等效軸角座標系表示法之外，我們再介紹另一種姿態表示法，這種表示法通過四個數值來表示，稱為尤拉引數。

      由等效旋轉軸和等效旋轉角定義的尤拉引數為：

      我們將這四個變數平方相加，得到：

      也即，這四個引數不是獨立的，所以，仍然符合我們對於姿態的3自由度的認知。同時，由上面的式子可知，我們可以將一個姿態看作是四維空間中單位超球面上的一點，或者說，它是一個單位四元數。用這組引數表示的旋轉矩陣為：

      已知旋轉矩陣求尤拉引數為：

      本次介紹就到這裡，歡迎大家討論~