四個月前剛研究了四元數，這周正好就在項目中用上了。看來深入鑽研相關的知識點還是有莫大的好處的。
項目中的應用場景是，已知一個起始的用戶坐標系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。
值得注意的是，兩個四元數的插值還有很多種方法，本文只是展示了最簡單並且可用的一個方法。