单位四元数多姿态插值（squad）

squad是Shoemake在1987年提出的一种比贝塞尔曲线更高效的近似算法，之前想运用它进行机械臂末端的多姿态平滑插值，但是上网搜关于squad姿态插补的算法，没找着，所以自己写了个C语言版本的，还没测试，仅供参考吧。

四元数的相关基础知识还有slerp、squad等插值方法这里就不赘述了，相关的知识推荐去看一下Krasjet*的《四元数与三维旋转》，想更深入可以去看《Quaternions, Interpolation and Animation》。

如果我们有一个四元数序列 $q_{0}$ , $q_{1}$ , . . . , $q_{n}$ ，那么插补公式有：

式中， $q_{i}$ 、 $q_{i+1}$ 、 $s_{i}$ 、 $s_{i+1}$ 都是单位四元数，t的范围是0~1；第二条式子是求控制点的。细心的同学可能会发现，如果要求 $s_{0}$ 、 $s_{n}$ ，那就要有 $q_{-1}$ 和 $q_{n+1}$ ，这时有两种解决方法，第一种是令 $q_{-1}$ = $q_{0}$ ， $q_{n+1}$ = $q_{n}$ ，第二种是令 $s_{0}$ = $q_{0}$ ， $s_{n}$ = $q_{n}$ ，两种方法都可以，对整段插补曲线的影响较小。需要注意的是：假设你有 $q_{0}$ 、 $q_{1}$ 、 $q_{2}$ 三个姿态，那么分为 $q_{0}$ ~ $q_{1}$ ， $q_{1}$ ~ $q_{2}$ 两段squad插补。与slerp插补不同的是，使用squad能保持这两段可以平滑过渡，而slerp会发生角速度突变。

然后是公式中四元数的对数和指数运算，这里引用《Quaternions, Interpolation and Animation》中的，红色框中可能是你需要注意的。

代码：

math_robot.c

/**************************** @file       math_robot.c* @brief      四元数相关运算* @author     vanvan* @date       2020-4-20* @version    ****************************/
#include "math_robot.h"//四元数乘法
double Quat_Mupltipy(QUAT *Q1, QUAT *Q2)
{return (Q1->s*Q2->s + Q1->v[0]*Q2->v[0] + Q1->v[1]*Q2->v[1] + Q1->v[2]*Q2->v[2]);
}//四元数标量乘法
QUAT Quat_Smupltipy(QUAT *Q, double scalar)
{QUAT q;q.s = Q->s*scalar;q.v[0] = Q->v[0]*scalar;q.v[1] = Q->v[1]*scalar;q.v[2] = Q->v[2]*scalar;return q;
}//四元数共轭
QUAT Quat_Conj(QUAT *Q)
{QUAT q;q.s = Q->s;q.v[0] = -Q->v[0];q.v[1] = -Q->v[1];q.v[2] = -Q->v[2];return q;
}//两四元数点积
double Quat_Dot(QUAT *q1, QUAT *q2)
{return (sqrt(q1->s*q2->s+q1->v[0]*q2->v[0]+q1->v[1]*q2->v[1]+q1->v[2]*q2->v[2]));
}//四元数取反
QUAT Quat_Reverse(QUAT *Q)
{QUAT q;q.s = -Q->s;q.v[0] = -Q->v[0];q.v[1] = -Q->v[1];q.v[2] = -Q->v[2];return q;
}//四元数对数运算
QUAT Quat_Log(QUAT *q)
{//四元数求对数// log(q)=[0, θv]double sina = sqrt(q->v[0]*q->v[0]+q->v[1]*q->v[1]+q->v[2]*q->v[2]);double cosa = q->s;double theta = atan2(sina,cosa);QUAT Q;//当sina很小时，不能作分子，用theta代替sin(theta)if(cosa > 0.9995){Q = *q;}else{ Q = Quat_Smupltipy(q, theta/sina);}Q.s=0;return Q;
}//四元数指数运算
QUAT Quat_Exp(QUAT *q)
{//exp(q)=[cosθ,sinθv]//求四元数的指数double theta = sqrt(q->v[0]*q->v[0]+q->v[1]*q->v[1]+q->v[2]*q->v[2]);double cosa = COS(theta);QUAT Q;//当sina很小时，不能作分子，用theta代替sin(theta)if(cosa > 0.9995){Q = *q;}else{Q = Quat_Smupltipy(q, sin(theta)/theta);}Q.s = cosa;return Q;
}//四元数相乘
QUAT Quat_Product(QUAT *q1, QUAT *q2)
{QUAT Q;Q.s    = q1->s*q2->s    - q1->v[0]*q2->v[0] - q1->v[1]*q2->v[1] - q1->v[2]*q2->v[2] ;Q.v[0] = q1->v[0]*q2->s + q1->s*q2->v[0]    - q1->v[2]*q2->v[1] + q1->v[1]*q2->v[2] ;Q.v[1] = q1->v[1]*q2->s + q1->v[2]*q2->v[0] + q1->s*q2->v[1]    - q1->v[0]*q2->v[2] ;Q.v[2] = q1->v[2]*q2->s - q1->v[1]*q2->v[0] + q1->v[0]*q2->v[1] + q1->s*q2->v[2] ;return Q;
}//四元数相加
QUAT Quat_Add(QUAT *q1, QUAT *q2)
{QUAT Q;Q.s    = q1->s+q2->s;Q.v[0] = q1->v[0]+q2->v[0];Q.v[1] = q1->v[1]+q2->v[1];Q.v[2] = q1->v[2]+q2->v[2];return Q;
}//计算控制点，形参n是控制点数，跟姿态个数相同
void GetCtlPoint(QUAT Qn[], int n, QUAT Sn[])
{Sn[0] = Qn[0];        //第一个控制点和最后一个控制点无法由公式获取Sn[n-1] = Qn[n-1];    //因此设置为与第一个插值点和最后一个插值点相同，对整个曲线影响不大QUAT qi,qi_m1,qi_a1,qi_conj;QUAT m0,m1,m0_log,m1_log,m_log_sum,k,k_exp;u8 i = 0;for(i = 1; i < n-1; i++){qi = Qn[i];qi_m1 = Qn[i-1];qi_a1 = Qn[i+1];if(Quat_Dot(&qi, &qi_m1)<0)  qi_m1 = Quat_Reverse(&qi_m1);if(Quat_Dot(&qi, &qi_a1)<0)  qi_a1 = Quat_Reverse(&qi_a1);qi_conj = Quat_Conj(&qi);m0 = Quat_Product(&qi_conj, &qi_m1);m1 = Quat_Product(&qi_conj, &qi_a1);//k = -((log(m0)+log(m1))/4);m0_log = Quat_Log(&m0);m1_log = Quat_Log(&m1);m_log_sum = Quat_Add(&m0_log,&m1_log);k = Quat_Smupltipy(&m_log_sum, -1/4);k_exp = Quat_Exp(&k);Sn[i] = Quat_Product(&qi,&k_exp);}
}//四元数球面线性插值
QUAT Slerp_Inter(QUAT *Qs, QUAT *Qe, float lambda)
{float cosa = Qs->s*Qe->s + Qs->v[0]*Qe->v[0] + Qs->v[1]*Qe->v[1] + Qs->v[2]*Qe->v[2];float k0, k1;QUAT Qt;// If the dot product is negative, the quaternions have opposite handed-ness and slerp won't take// the shorter path. Fix by reversing one quaternion.//q与-q实际上对应的是同一个旋转（double cover），为了得到最短路径，//插补之前应该判断两个四元数的角度，钝角则反转其中一个四元数if(cosa < 0){cosa = -cosa;Qe->s = -Qe->s;Qe->v[0] = -Qe->v[0];Qe->v[1] = -Qe->v[1];Qe->v[2] = -Qe->v[2];}// If the inputs are too close for comfort, linearly interpolate //这里使用的是Lerp，使用Nlerp可能误差更小if(cosa > 0.9995f){k0 = 1.0f - lambda;k1 = lambda;}else{float sina = sqrt(1.0f - cosa*cosa);float a = atan2(sina, cosa);k0 = sin((1.0f - lambda)*a) / sina;k1 = sin(lambda*a) / sina;}Qt.s      = Qs->s*k0 + Qe->s*k1;Qt.v[0] = Qs->v[0]*k0 + Qe->v[0]*k1;Qt.v[1] = Qs->v[1]*k0 + Qe->v[1]*k1;Qt.v[2] = Qs->v[2]*k0 + Qe->v[2]*k1;return Qt;
}//squad姿态插值
QUAT Squad(QUAT *Qi, QUAT *Si, QUAT *Si_1, QUAT *Qi_1, double t)
{QUAT k1 = Slerp_Inter(Qi,Qi_1,t);QUAT k2 = Slerp_Inter(Si,Si_1,t);return Slerp_Inter(&k1,&k2, 2*t*(1-t));
}

math_robot.h

#ifndef _MATH_ROBOT_H_
#define _MATH_ROBOT_H_typedef struct Quat{float s;         //scalarfloat v[3];       //vector
}QUAT;QUAT Slerp_Inter(QUAT *Qs, QUAT *Qe, float lambda);
QUAT Squad(QUAT *Qi, QUAT *Si, QUAT *Si_1, QUAT *Qi_1, double t);
void GetCtlPoint(QUAT Qn[], int n, QUAT Sn[4]);#endif

代码仅供参考，如果有错误，请告诉我一下，Thanks!

参考文献：

《四元数与三维旋转》

《Quaternions, Interpolation and Animation》

文章分享给你们：

链接：https://pan.baidu.com/s/1Wuv25T7J0OYCdFPW_QAzqw
提取码：169y