四元数插值与均值（姿态平滑）
- 四元数的求负是对每个分量变负就可以了，即$\textbf{q}=(w,x,y,z)$，$-\textbf{q}=(-w,-x,-y,-z)$。但是q和-q表示的角位移是相同的，当旋转角为360°或其整数倍时，不会改变q的角位移，但是q的四个分量都变负了
- 四元数与标量相乘：$k \textbf{q}=k[w,x,y,z]=[kw ,kx , ky , kz]$
- 四元数的点乘:$\textbf{q}_1\cdot\textbf{q}_2=[w_1 \:\textbf{v}_1]\cdot[w_2 \:\textbf{v}_2]=w_1w_2+\textbf{v}_1\cdot \textbf{v}_2=w_1w_2+x_1x_2+y_1y_2+z_1z_2$。四元数点乘的绝对值越大，代表其方向越相近。
- 四元数乘法（叉乘）：$\textbf{p}=t+\vec{v}$、$\textbf{q}=a+\vec{u}$，则$\textbf{p}\textbf{q}=at-\vec{u}\cdot\vec{v}+a\vec{v}+t\vec{u}+\vec{u}\times\vec{v}$
- 四元数的模长（范数）：$\left \| \textbf{q} \right \|=\sqrt{x^2+y^2+z^2+w^2}=\sqrt{\textbf{q}\cdot\textbf{q}^*}$
- 四元数的共轭：四元数的共轭就是让四元数的向量部分取负，即若$\mathbf{q}=(w,x,y,z)$，则其共轭为$\mathbf{q}^*=(w,-x,-y,-z)$
- 四元数的逆：由于$\textbf{q}\textbf{q}^*=\left \| \textbf{q} \right \|^2$，因此$\textbf{q}\frac{\textbf{q}^*}{\left \| \textbf{q} \right \|^2}=1$。若四元数存在逆$\textbf{q}^{-1}$，则$\textbf{q}\textbf{q}^{-1}=\textbf{q}^{-1}\textbf{q}=1$，因此当$\left \| \textbf{q} \right \|\neq 0$时：$$\textbf{q}^{-1}=\frac{\textbf{q}^*}{\left \| \textbf{q} \right \|^2}$$对于单位四元数来说有$\textbf{q}^{-1}=\textbf{q}^*$
- 用四元数旋转矢量：对于矢量$\vec{v}=(x,y,z)$，给定一个单位四元数q，让$\vec{v}$旋转q。首先将$\vec{v}$改写成四元素的形式v = (0, x, y ,z)，接下来进行如下变换：$${\mathbf{v}}'= \textbf{q}\mathbf{v}\textbf{q}^{-1}$$
- 四元数的球面线性插值（Slerp）
对于一般线性插值来说：$\mathbf{r}=\mathbf{P}_0+(\mathbf{P}_1-\mathbf{P}_0)t$，其中t∈[0, 1]，代表了插值矢量r末端在弦P0-P1上的位置。假如t取值为1/4、2/4、3/4即将P0P1弦长均分为4等份，可以看出其对应的弧长并不相等。靠近中间位置的弧长较长，而靠近两段处的弧长较短，这就意味着当t匀速变化时，代表姿态矢量的角速度变化并不均匀：
To better solve the nonconstant rotation speed and normalization issues, we need an interpolation method known as spherical linear interpolation (usually abbreviated as slerp). Slerp is similar to linear interpolation except that instead of interpolating along a line, we’re interpolating along an arc on the surface of a sphere. Figure 10.21 shows the desired result. When using spherical interpolation at quarter intervals of t, we travel one-quarter of the arc length to get from orientation to orientation. We can also think of slerp as interpolating along the angle, or in this case, dividing the angle between the orientations into quarter intervals.
球面线性插值（Spherical linear interpolation，通常简称Slerp），是四元数的一种线性插值运算，主要用于在两个表示旋转的四元数之间平滑差值。下面来简单推导一下Slerp的公式。插值的一般公式可以写为：$\mathbf{r}=a(t)\mathbf{p}+b(t)\mathbf{q}$，现在要找到合适的a(t)和b(t)。注意单位向量p和q之间的夹角为θ，p和r之间的夹角为tθ，q和r之间的夹角为(1-t)θ：
将上面的公式两边点乘p可得$$\begin{align*}\mathbf{p}\cdot\mathbf{r}&=a(t)\mathbf{p}\cdot\mathbf{p}+b(t)\mathbf{p}\cdot\mathbf{q}\\\cos{t\theta}&=a(t)+b(t)\cos{\theta}\end{align*}$$
同样地，对公式两边点乘q可得$$\cos{[(1-t)\theta]}=a(t)\cos{\theta}+b(t)$$
两个方程可以解出两个未知量a(t)和b(t)：$$\begin{align*}a(t)&=\frac{\cos{t\theta}-\cos[{(1-t)\theta}]\cos{\theta}}{1-\cos^2{\theta}}\\b(t)&=\frac{\cos[{(1-t)\theta}]-\cos{t\theta}\cos{\theta}}{1-\cos^2{\theta}}\end{align*}$$
使用三角函数公式可以将其化简为：$$\begin{align*}a(t)&=\frac{\sin{[(1-t)\theta]}}{\sin{\theta}}\\b(t)&=\frac{\sin{t\theta}}{\sin{\theta}}\end{align*}$$
注意这个公式有2个问题，必须在实现过程中加以考虑：
- 如果四元数点积的结果是负值（夹角大于90°），那么后面的插值就会在4D球面上绕远路。为了解决这个问题，先测试点积的结果，当结果是负值时，将2个四元数的其中一个取反（并不会改变它代表的朝向）。而经过这一步操作，可以保证这个旋转走的是最短路径。
- 当p和q的夹角θ差非常小时会导致sinθ→0，这时除法可能会出现问题。为了避免这样的问题，当θ非常小时可以使用简单的线性插值代替（θ→0时，sinθ≈θ，因此方程退化为线性方程：slerp(p,q,t)=(1-t)p+tq）
具体实现可以参考下面的代码：
#include <iostream> #include <math.h> void slerp(float starting[4], float ending[4], float result[4], float t ) { float cosa = starting[0]*ending[0] + starting[1]*ending[1] + starting[2]*ending[2] + starting[3]*ending[3]; // 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. if ( cosa < 0.0f ) { ending[0] = -ending[0]; ending[1] = -ending[1]; ending[2] = -ending[2]; ending[3] = -ending[3]; cosa = -cosa; } float k0, k1; // If the inputs are too close for comfort, linearly interpolate if ( cosa > 0.9995f ) { k0 = 1.0f - t; k1 = t; } else { float sina = sqrt( 1.0f - cosa*cosa ); float a = atan2( sina, cosa ); k0 = sin((1.0f - t)*a) / sina; k1 = sin(t*a) / sina; } result[0] = starting[0]*k0 + ending[0]*k1; result[1] = starting[1]*k0 + ending[1]*k1; result[2] = starting[2]*k0 + ending[2]*k1; result[3] = starting[3]*k0 + ending[3]*k1; } int main() { float p[4] = {1,0,0,0}; float q[4] = {0.707,0,0,0.707}; float r[4] = {0}; slerp(p,q,r,0.333); std::cout<<r[0]<<","<<r[1]<<","<<r[2]<<","<<r[3]<<std::endl; slerp(p,q,r,0.667); std::cout<<r[0]<<","<<r[1]<<","<<r[2]<<","<<r[3]<<std::endl; return 0; }
下图中，X轴指向代表的姿态四元数为(1,0,0,0)，Y轴指向代表的姿态四元数为(0.707,0,0,0.707)，转换为Z-Y-X欧拉角为(0,0,90°)，即Y轴由X轴绕Z轴旋转90°得到。那么对这两个姿态进行插值，将其三等分（分别绕Z轴旋转30°和60°），根据欧拉角转四元数的公式得到r1=(0.966,0,0,0.259)，r2=(0.866,0,0,0.5)
上面代码输出的插值结果如下，可以看出与理论分析一致：
在DirectXMath Library中有XMQuaternionSlerp函数可以进行四元数的球面线性插值：
#include<iostream> #include <DirectXMath.h> using namespace DirectX; int _tmain(int argc, _TCHAR* argv[]) { XMVECTOR p = XMVectorSet(0, 0, 0, 1); // Unit quaternion to interpolate from. XMVECTOR q = XMVectorSet(0, 0, 0.707, 0.707); // Unit quaternion to interpolate to. XMVECTOR result = XMQuaternionSlerp(p, q, 0.333); // Interpolates between two unit quaternions, using spherical linear interpolation. std::cout << XMVectorGetW(result) << "," << XMVectorGetX(result) << "," << XMVectorGetY(result) << "," << XMVectorGetZ(result) << std::endl << std::endl; result = XMQuaternionSlerp(p, q, 0.667); std::cout << XMVectorGetW(result) << "," << XMVectorGetX(result) << "," << XMVectorGetY(result) << "," << XMVectorGetZ(result) << std::endl << std::endl; return 0; }
其结果与上面相同。
接下来在VREP中测试姿态插值，如下图所示将NAO机器人头部保持端正的姿态作为起始姿态，将转过一定角度的姿态作为终止姿态，在这两个姿态之间使用球面线性插值生成一系列中间姿态。编写MFC程序通过水平滑动条来控制参数t；C++客户端程序和VREP之间传送四元数数据可以使用SetFloatSignal/GetFloatSignal函数来设置信号（由于信号每次只能设置一个分量，不能处理数组，因此当数据量很大时可以采用simPackFloatTable/simUnpackFloatTable等函数进行处理，参考Exchanging data between MATLAB and VREP）
Child Script:
if (sim_call_type==sim_childscriptcall_initialization) then -- Put some initialization code here simExtRemoteApiStart(19999) handle0 = simGetObjectHandle('Head0') handle1 = simGetObjectHandle('Head1') local q0 = simGetObjectQuaternion(handle0,-1) local q1 = simGetObjectQuaternion(handle1,-1) simSetFloatSignal('q0_w', q0[4]) simSetFloatSignal('q0_x', q0[1]) simSetFloatSignal('q0_y', q0[2]) simSetFloatSignal('q0_z', q0[3]) simSetFloatSignal('r_w', q0[4]) simSetFloatSignal('r_x', q0[1]) simSetFloatSignal('r_y', q0[2]) simSetFloatSignal('r_z', q0[3]) simSetFloatSignal('q1_w', q1[4]) simSetFloatSignal('q1_x', q1[1]) simSetFloatSignal('q1_y', q1[2]) simSetFloatSignal('q1_z', q1[3]) end if (sim_call_type==sim_childscriptcall_actuation) then -- Put your main ACTUATION code here local r_w = simGetFloatSignal("r_w") local r_x = simGetFloatSignal("r_x") local r_y = simGetFloatSignal("r_y") local r_z = simGetFloatSignal("r_z") simSetObjectQuaternion(handle0,-1,{r_x,r_y,r_z,r_w}) end if (sim_call_type==sim_childscriptcall_sensing) then -- Put your main SENSING code here end if (sim_call_type==sim_childscriptcall_cleanup) then -- Put some restoration code here end
MFC部分代码：
CSlerpTestDlg::CSlerpTestDlg(CWnd* pParent /*=NULL*/) : CDialogEx(CSlerpTestDlg::IDD, pParent) { m_hIcon = AfxGetApp()->LoadIcon(IDR_MAINFRAME); clientID = simxStart((simxChar*)"127.0.0.1", 19999, true, true, 2000, 5); if (clientID != -1) {// Send some data to V-REP in a non-blocking fashion: simxAddStatusbarMessage(clientID, "Connected to V-REP!", simx_opmode_oneshot); Handle0 = 0; Handle1 = 0; simxGetObjectHandle(clientID, "Head0", &Handle0, simx_opmode_oneshot_wait); simxGetObjectHandle(clientID, "Head1", &Handle1, simx_opmode_oneshot_wait); } } // CSlerpTestDlg 消息处理程序 BOOL CSlerpTestDlg::OnInitDialog() { CDialogEx::OnInitDialog(); // 设置此对话框的图标。 当应用程序主窗口不是对话框时，框架将自动 // 执行此操作 SetIcon(m_hIcon, TRUE); // 设置大图标 SetIcon(m_hIcon, FALSE); // 设置小图标 // TODO: 在此添加额外的初始化代码 m_slider.SetRange(0, 100); //设置滑动范围 m_slider.SetPos(0); //设置滑块初始位置为0 float q0_w, q0_x, q0_y, q0_z; float q1_w, q1_x, q1_y, q1_z; simxGetFloatSignal(clientID, "q0_w", &q0_w, simx_opmode_oneshot_wait); simxGetFloatSignal(clientID, "q0_x", &q0_x, simx_opmode_oneshot_wait); simxGetFloatSignal(clientID, "q0_y", &q0_y, simx_opmode_oneshot_wait); simxGetFloatSignal(clientID, "q0_z", &q0_z, simx_opmode_oneshot_wait); simxGetFloatSignal(clientID, "q1_w", &q1_w, simx_opmode_oneshot_wait); simxGetFloatSignal(clientID, "q1_x", &q1_x, simx_opmode_oneshot_wait); simxGetFloatSignal(clientID, "q1_y", &q1_y, simx_opmode_oneshot_wait); simxGetFloatSignal(clientID, "q1_z", &q1_z, simx_opmode_oneshot_wait); q0 = XMVectorSet(q0_x, q0_y, q0_z, q0_w); // Unit quaternion to interpolate from. q1 = XMVectorSet(q1_x, q1_y, q1_z, q1_w); // Unit quaternion to interpolate to. return TRUE; } void CSlerpTestDlg::OnDestroy() { CDialogEx::OnDestroy(); // Close the connection to V-REP: simxFinish(clientID); } void CSlerpTestDlg::OnHScroll(UINT nSBCode, UINT nPos, CScrollBar* pScrollBar) { CSliderCtrl *pSlidCtrl = (CSliderCtrl*)GetDlgItem(IDC_SLIDER1); int pos = pSlidCtrl->GetPos(); //取得当前位置值 float t = (float)pos / 100.0; XMVECTOR r = XMQuaternionSlerp(q0, q1, t); // Interpolates between two unit quaternions, using spherical linear interpolation. float r_w, r_x, r_y, r_z; r_w = XMVectorGetW(r); r_x = XMVectorGetX(r); r_y = XMVectorGetY(r); r_z = XMVectorGetZ(r); simxSetFloatSignal(clientID, "r_w", r_w, simx_opmode_oneshot_wait); simxSetFloatSignal(clientID, "r_x", r_x, simx_opmode_oneshot_wait); simxSetFloatSignal(clientID, "r_y", r_y, simx_opmode_oneshot_wait); simxSetFloatSignal(clientID, "r_z", r_z, simx_opmode_oneshot_wait); CDialogEx::OnHScroll(nSBCode, nPos, pScrollBar); }
- 四元数的平均值
Kinect获取到的人体关节姿态数据的噪声比较大，如果直接用这些原始的四元数去控制模型会造成模型抖动，于是想到对四元数求均值来进行平滑滤波。对n个四元数q1、q2、...qn求其加权平均值（权值分别为w1、w2、...wn），如果采用下面这种最直观的公式会出现一些问题$$\bar{\mathbf q}_{bad}= (\sum_{i=1}^{n}w_i)^{-1}\sum_{i=1}^{n}w_i\mathbf{q}_i $$
比如对于四元数q和-q代表同一姿态，因此对于上面方程中任一qi改变其符号不应影响均值，但显然上面的公式并不具备这一性质。于是有人提出了其他方法来计算多个四元数之间的加权平均值：
Equations 12 and 13 in Markley et al.'s paper "Quaternion Averaging" (2007) define the weighted average of n quaterions qi with scalar weights wi：$$\bar{\mathbf q}=\arg\,\max_{\mathbf q\in\mathbb S^3}\mathbf q^T\mathbf M\mathbf q$$where$$\mathbf M=\sum_{i=1}^nw_i\mathbf q_i\mathbf q_i^T$$
That is, interpret the quaternions as vectors in $\mathbb R^4$, form the matrix M, and take its largest eigenvector. 即根据四元数和其对应的权值构造4×4矩阵M，M最大特征值对应的单位特征向量就是要求的加权平均值。
根据论文，求四元数加权平均值的MATLAB程序如下：
% Q is an Nx4 matrix of quaternions. weights is an Nx1 vector, a weight for each quaternion. % Qavg is the weightedaverage quaternion % Markley, F. Landis, Yang Cheng, John Lucas Crassidis, and Yaakov Oshman. % "Averaging quaternions." Journal of Guidance, Control, and Dynamics 30, % no. 4 (2007): 1193-1197. function [Qavg] = quatWAvgMarkley(Q, weights) % Form the symmetric accumulator matrix M = zeros(4, 4); n = size(Q, 1); % amount of quaternions wSum = 0; for i = 1:n q = Q(i,:)'; w_i = weights(i); M = M + w_i.*(q*q'); wSum = wSum + w_i; end % scale M = (1.0/wSum)*M; % The average quaternion is the eigenvector of M corresponding to the maximum eigenvalue. % Get the eigenvector corresponding to largest eigen value [Qavg, ~] = eigs(M, 1); end
下面测试一组数据，6个姿态分别表示绕Z轴旋转一定角度，求其平均姿态：
>> Q = [1,0,0,0; 0.999,0,0,0.044; 0.999,0,0,0.035; 1,0,0,0.026; 1,0,0,0.017; 1,0,0,0.009] % 0°,5°,4°,3°,2°,1° rotation about Z-axis
>> w = ones(6,1) / 6
>> quatWAvgMarkley(Q, w)
输出结果为(-0.9998, 0, 0, -0.0218)，代表的平均姿态为绕Z轴旋转2.498°
求矩阵特征值和特征向量有很多快速的实现方式，不过计算量依然比较大。而当要求均值的四元数之间比较接近时还是可以采用简化的算法，经比较判断后直接求4个分量的均值。 Unity3D论坛中给出了这种简化算法的代码。Note: this code will only work if the separate quaternions are relatively close to each other.
//Get an average (mean) from more then two quaternions (with two, slerp would be used). //Note: this only works if all the quaternions are relatively close together. //Usage: //-Cumulative is an external Vector4 which holds all the added x y z and w components. //-newRotation is the next rotation to be added to the average pool //-firstRotation is the first quaternion of the array to be averaged //-addAmount holds the total amount of quaternions which are currently added //This function returns the current average quaternion public static Quaternion AverageQuaternion(ref Vector4 cumulative, Quaternion newRotation, Quaternion firstRotation, int addAmount){ float w = 0.0f; float x = 0.0f; float y = 0.0f; float z = 0.0f; //Before we add the new rotation to the average (mean), we have to check whether the quaternion has to be inverted. Because //q and -q are the same rotation, but cannot be averaged, we have to make sure they are all the same. if(!Math3d.AreQuaternionsClose(newRotation, firstRotation)){ newRotation = Math3d.InverseSignQuaternion(newRotation); } //Average the values float addDet = 1f/(float)addAmount; cumulative.w += newRotation.w; w = cumulative.w * addDet; cumulative.x += newRotation.x; x = cumulative.x * addDet; cumulative.y += newRotation.y; y = cumulative.y * addDet; cumulative.z += newRotation.z; z = cumulative.z * addDet; //note: if speed is an issue, you can skip the normalization step return NormalizeQuaternion(x, y, z, w); } public static Quaternion NormalizeQuaternion(float x, float y, float z, float w){ float lengthD = 1.0f / (w*w + x*x + y*y + z*z); w *= lengthD; x *= lengthD; y *= lengthD; z *= lengthD; return new Quaternion(x, y, z, w); } //Changes the sign of the quaternion components. This is not the same as the inverse. public static Quaternion InverseSignQuaternion(Quaternion q){ return new Quaternion(-q.x, -q.y, -q.z, -q.w); } //Returns true if the two input quaternions are close to each other. This can //be used to check whether or not one of two quaternions which are supposed to //be very similar but has its component signs reversed (q has the same rotation as //-q) public static bool AreQuaternionsClose(Quaternion q1, Quaternion q2){ float dot = Quaternion.Dot(q1, q2); if(dot < 0.0f){ return false; } else{ return true; } }
参考：
Averaging Quaternions and Vectors
Maths - Quaternion Interpolation (SLERP)
Joint Orientation Smoothing [Unity C#]
“Average” of multiple quaternions?
Exchanging data between MATLAB and VREP
How would I apply an Exponential Moving Average to Quaternions?
