利用四元数计算两个坐标系之间的平滑过渡

四个月前刚研究了四元数，这周正好就在项目中用上了。看来深入钻研相关的知识点还是有莫大的好处的。

项目中的应用场景是，已知一个起始的用户坐标系UCS1，和一个终止的用户坐标系UCS2，如何求出一系列的中间坐标系，使得它们在UCS1和UCS2直接平滑的过渡？

（UCS1在这里是机器人手部的起始位置，UCS2是手部的目标位置，因为目前我只使用简单的反向动力学算法来求机器人的关节位置，所以需要计算初始和终止位置的过渡坐标系。将来用上运动规划的模块，这个就不再需要了。不过，这仍然是一个具有普适意义的好题目。例如UCS1可以看成是摄像机的初始坐标系，UCS2是摄像机的终止坐标系，我们可以求一个平滑的相机过渡路径。）

 

最简单的做法，坐标系的原点可以逐步的平移过去。那坐标系的旋转如何逐步过渡呢？答案是使用四元数的插值(interpolation)。

 

从 前文得知，坐标系的旋转矩阵表达可以和四元数表达互相转换。所以我们有如下解决问题的伪代码：

 1 UCS GetInterpolationMatrix(UCS& ucs1, UCS& ucs2, double t)
 2 {
 3     Matrix m1 = UCS1.GetMatrix();
 4     Matrix m2 = UCS2.GetMatrix();
 5     Vector p1 = m1.GetT();
 6     Vector p2 = m2.GetT();
 7     Quaternion q1 = m1.GetQ();
 8     Quaternion q2 = m2.GetQ();
 9     
10     Matrix m;
11     Vector     p = p1 * (1 - t) + p2 * t;
12     Quaternion q = q1 * (1 - t) + q2 * t;
13     m.SetT(p);
14     m.SetQ(q);  
15     
16     return UCS(m);          
17 }

这个函数接受两个待插值UCS和一个数值t，将t在0和1之间变化，就可以得到一系列过渡的UCS。

值得注意的是，两个四元数的插值还有很多种方法，本文只是展示了最简单并且可用的一个方法。

转载于:https://www.cnblogs.com/kaige/p/use_quaternion_interpolation_for_coordinate_system_smooth_transitioning.html