❶ 機器人手眼標定細節 AX=XB
前文介紹了機器人手眼標定的原理和具體實現的公式：
文中提到需要解決AX=XB的問題，並給出2種解決方案。其中第一種有較大缺陷，不多說，這篇主要講講第二種四元數解AX=XB的原理。很巧妙的思路，沒有什麼太復雜的計算和數學理論。
全網鋪天蓋地四元數的介紹，這里不多說。旋轉矩陣轉換四元數公式如下：
這里引用的是ABB機器人手冊中的公式，原版手冊下載地址如下：
Technical reference manual - RAPID Instructions, Functions and Data types RW 7
這篇文章需要用到的四元數性質很少，就兩點，一個是四元數的2-norm是1，另一個是四元數「乘法」計算。
如果我有一個四元數（這篇文章的四元數都會是4行1列的列向量）：
[公式]
s是四元數的標量部分，[公式] 是向量部分。
2-Norm等於1
有如下性質：
[公式]
這里的norm指的是2-norm，也就是向量每個元素的平方和開根號。
「乘法」
如果我有兩個旋轉矩陣相乘，表示連續2個旋轉：[公式]
這組旋轉對應的四元數表示為：[公式] ，計算方法為：
[公式]
注意，[公式] 指的是點乘，也就是內積，得到的結果是一個標量。
用Matlab驗證：
向量計算經常出現點乘與叉乘，比如四元數乘法。而推導公式過程中希望運算符統一，所以都會轉換成矩陣乘法的形式。
點乘
向量點乘可以表示為[公式] ，但是在Matlab中輸入a*b（或者Numpy中用a@b），會報錯告訴你矩陣尺寸不對，這是因為Matlab認為這是在做矩陣相乘。把向量點乘轉換成矩陣乘法也很簡單：
[公式]
叉乘
叉乘計算的是和兩個向量同時垂直的一個向量，方向由右手法則決定，因此這兩個向量順序反一下，結果的方向也要反一下：
[公式]
叉乘同樣也可以轉換成矩陣乘法形式：
[公式]
那個a構成的反對稱矩陣，一般記作[公式] 或者 [公式]
[公式]
這里關於奇異值分解不多說，詳細的可以去看另一篇文章：
這里只關心SVD分解出的3個矩陣的特性：
[公式]
[公式] 都是正交矩陣，det是1或者-1，而且他的逆就是他的轉置 。
[公式]， [公式]
更直觀來說，相當於4維空間里的旋轉/翻轉矩陣。
旋轉/翻轉矩陣不改變向量長度！
假設q是一個4*1的向量，則有：
[公式]
[公式]
另一個矩陣：
[公式]
其中對角線元素從大到小排列：
[公式]
這里主要解決的問題是[公式]
用四元數表示：
[公式]
四元數乘法展開：
[公式]
左右移動，整理一下：
[公式]
最後得到：
[公式]
把式子中的點乘，叉乘全部換成矩陣乘法：
式子中：
[公式]
替換得到：
[公式]
進一步合並，最終得到：
[公式]
可以發現，兩部分都有[公式] 和 [公式] ，把這兩個式子寫成一個大矩陣（最基礎的線性代數，二元一次方程組）：
[公式]
其中第一個矩陣記為M:
[公式]
其中第二個矩陣，[公式] ，也就是我們想要求的四元數，滿足：
[公式]
把這兩個代入等式，同時滿足四元數的模等於1，那前面這個等式可以寫成：
[公式]
好了，那現在的就變成了一個最小化的問題了：
[公式]
前面說過SVD，這里需要用到這個：
[公式]
[公式]
[公式] 的結果是一個向量，而 [公式] 並不會改變向量的長度（2-norm），可以直接忽略。
[公式]
另外，[公式] 是四元數，他的2-norm是1， [公式] 不改變向量的長度（2-norm）。
用向量y來替代[公式] ： [公式]
則有：
[公式]
並且[公式]
這樣前面那個最小化問題[公式]
可以寫成這個形式：
[公式]
展開看一下：
[公式]
[公式]
[公式]
由於[公式] ，如果我想最小化 [公式] ，肯定需要讓 [公式] 盡可能小，把所有的數都放在 [公式] 上，同時要保證向量 [公式] 的2-norm是1，答案很明顯：
[公式]
前面有個式子：[公式] ，把 [公式] 的值代進去：
[公式]
前面說SVD那一段時提過，[公式] ，所以 [公式] ，用這個性質移項：
[公式]
[公式]
如果把V寫成列向量的形式，結果就很清晰了：
[公式]
所以這就是最終結果。
"Talk is cheap. Show me the code." - Linus Torvalds