- 四元數的求負是對每個分量變負就可以了，即$\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?