❶ 机器人手眼标定细节 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