四元数插值与均值（姿态平滑）

四元数插值与均值（姿态平滑）

转自：.html

•  四元数的球面线性插值（Slerp）

　　对于一般线性插值来说：r=P0+(P1−P0)tr=P0+(P1−P0)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的公式。插值的一般公式可以写为：r=a(t)p+b(t)qr=a(t)p+b(t)q，现在要找到合适的a(t)和b(t)。注意单位向量p和q之间的夹角为θ，p和r之间的夹角为tθ，q和r之间的夹角为(1-t)θ：

　　注意这个公式有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 interpolateif ( 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;
}

• 四元数的平均值

 　　根据论文，求四元数加权平均值的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:nq = 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°

参考：

Rotations

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?