空间中位姿矩阵的插补算法代码，旋转中采用四元数进行插补