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

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

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

    如果我们有一个四元数序列 , , . . . , ，那么插补公式有：

    式中，、、、都是单位四元数，t的范围是0~1；第二条式子是求控制点的。细心的同学可能会发现，如果要求、，那就要有和，这时有两种解决方法，第一种是令=，=，第二种是令=，=，两种方法都可以，对整段插补曲线的影响较小。需要注意的是：假设你有、、三个姿态，那么分为~，~两段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;         //scalar
	float 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