刚体运动学-四元数插值

前言

之前对写了一篇关于刚体运动学相关知识博客：刚体运动学——欧拉角、四元数、旋转矩阵，本篇博客就举例来说明，如何在运动捕捉数据中进行四元数插值。

国际惯例，参考博客：

探讨：向量（方向）之间的插值-四元数法VS.旋转矩阵法的性能比较

书籍《3D数学基础：图形与游戏开发》

插值理论

问题：3D空间中，在等长度的两个交角为$\theta$的向量$V_1(x_1,y_1,z_1)$和$V_2(x_2,y_2,z_2)$。

实例：行星绕太阳转动，找到旋转过程的两个位置$p_1,p_2$，现在模拟从$p_1$到$p_2$的过程。

思路：

1 .一般线性插值

线性插值方法：

这里可以看出，插值的部分就是向量$V_3$.下面来证明$V_3$与$t$的关系$V_2-V_1$得到$V_1$指向$V_2$的向量，再乘以$t$就是$V_1$指向$V_3$的向量了，最后加上向量$V_1$就是向量$V_3$了，公式为：

$$v(t) = v_1 + t*(v_2-v_1)（0\leq t\leq1)$$
【注：可以看出一般线性插值长度变化了，不满足要求，用球面线性插值就不会变化】

2.一般球面线性插值

将插值结果放大一个放大系数$k(t)$，使其长度放大到$|v_1|$或者$|v_2|$(简单的说就是保持长度不变)。

$$v(t) = k(t)(v_1 + t(v_2-v_1))$$
其中 $k(t) =\frac{|v_1|}{|v(t)|}=\frac{|v_1|}{|v_1+t*(v_2-v_1)|}$.

这样，插值向量$v(t)$的端点就会沿着$v_1$，$v_2$端点构成的圆弧进行，$v_1$和$v_2$是等长的，圆弧实际位于$v_1和$$v_2$构成的曲面上的一段，所以又叫球面线性插值。

这个插值解决了3D空间中旋转的插值，在关键帧动画中可以用来计算两个关键帧之间的动画。但是，由于它的插值不是等角速度的，而是变速的，所以如果用来实现案例中的效果的话还需进一步处理。

【注】一般球面线性插值$v(t)$与$v_1$的夹角$\theta(t)$不是t的线性函数。

证明过程如下(我滴妈呀，我的字好丑o(╯□╰)o)：

3.改进的球面线性插值，有两种方法：

1> 四元数工具
变换方法：

构造四元数$q(\cos \theta，\sin\theta *v_1’)，r(\cos \theta，\sin \theta *v_2’)$($v_1’，v_2’$为单位$v_1,v_2$向量)，以及参数$t(0\leq t\leq1)$,则构造四元数变换：

• 四元数$s(w,v’)=r*(q-1)t*q$即为球面线性插值变换。其中，s的虚部$v_1’$和$v_2’$间的插值向量，乘以长度$\sqrt{x^2+y^2+z^2}$，即得到$v_1,v_2$间插值向量$v$

• 另一种变换形式是对四元数进行插值变换
  

  $$s(w,v')=a*q+b*r$$
  其中$a=\frac{\sin(\alpha(1-t))}{\sin\alpha},b=\frac{\\sin(\alpha t)}{\sin\alpha},\cos\alpha=x_1*x_2+y_1*y_2+w_1*w_2$

  S的虚部$v'$即为$v_1'$和$v_2'$间的插值向量，乘以长度$\sqrt{x^2+y^2+z^2}$，即得$v_1,v_2$间插值向量$v$

2> 利用旋转矩阵

变换方法：$v=v1*Trot$

其中，$Trot$即绕任意轴旋转的矩阵变换矩阵，因为$v_1$到$v_2$间的插值可以看成是$v_1$绕垂直于$v_1,v_2$组成的平面的向量的旋转，所以实际上是绕轴旋转的问题，不过相应参数变成 $\theta=t*\theta$，轴$q(q1,q2,q3)$变成向量$\frac{v_1\times v_2}{|v_1\times v_2|}=\frac{y_1z_2-z_1y_2,z_1x_2-x_1z_2,x_1y_2-y_1x_2}{\sin\theta}$

四元数插值

第一种插值方法

四元数比较重要的一个用途就是球面线性插值(Spherical Linear Interpolation)，可以在两个四元数之间平滑插值。

插值步骤：

① 计算两个值的差：$q_0$到$q_1$的角位移由$\Delta q=q_0^{-1}q_1$给出

② 计算差的一部分，四元数求幂可以做到，差的一部分由$\Delta q_t$给出

③ 在开始值上加上差的一部分，用四元数乘法组合角位移$q_0\Delta q_t$

这样就可以得到slerp公式:

$$slerp(q_0,q_1,t)=q_0(q_0^{-1}q_1)^t$$
看看 matlab中的函数实现：

function y = slerp(q1, q2, t)
% The third parameter, t, gives the 'distance' along the 'arc' between the
% quaternions, 0 representing q1 and 1 representing q2. If q1 and q2 are
% unit pure quaternions, the interpolation is along a great circle of the
% sphere between the points represented by q1 and q2. If q1 and q2 are unit
% full quaternions, the interpolation is along the 'arc' on the 4-sphere:
% this means the result is a quaternion which represents a rotation
% intermediate between the two rotations represented by q1 and q2. If the
% first two parameters are not unit quaternions, then there is also
% interpolation in modulus.

error(nargchk(3, 3, nargin)), error(nargoutchk(0, 1, nargout))

if ~isnumeric(t) || ~isreal(t)
    error('Third parameter must be real and numeric.');
end

if any(any(t < 0.0)) || any(any(t > 1.0))
    error('Third parameter must have values between 0 and 1 inclusive.');
end

if ~(all(size(q1) == size(q2)) || isscalar(q1) || isscalar(q2))
    error(['First two parameters cannot be of different sizes unless' ...
          ' one is a scalar.']);
end

if ~isscalar(t)    
    if ~(all(size(q1) == size(t)) || all(size(q2) == size(t)) || ...
         (isscalar(q1) && isscalar(q2)) ...
        )
        error(['Third parameter cannot be an array unless' ...
               ' the first two are scalars, or it has the'...
               ' same size as one of the first two parameters.']);
    end
end

y = q1 .* (q1.^-1 .* q2).^t;

然后使用此函数尝试在运动捕捉数据中进行插值：

%方法一：matlab自带函数slerp
clear
clc
close all
addpath(genpath('.'))

%读取两个运动数据skel,A,B
load sample.mat
% skelPlayDataA(skel,[A;B])
%将欧拉角转换为四元数
quatA=joint_euler2quat(skel,A);
quatB=joint_euler2quat(skel,B);
%执行四元数插值，插20帧
internum=20;
temp_quat=zeros(31,4);%31个关节，每个关节一个四元数
newMotion=zeros(internum,62);%20帧，每帧62维
for i=1:internum
    t=i/internum;
    %对于角度采用四元数插值
    for j=1:size(quatA,1)
        temp_quat(j,:)=slerp(quatA(j,:),quatB(j,:),t);        
    end
    temp_quat(find(isnan(temp_quat)))=0;
    temp_quat=real(temp_quat);
    newMotion(i,:)=joint_quat2euler(temp_quat);
    %对于位置采用线性插值
    posA=A(1,1:3);posB=B(1,1:3);
    newMotion(i,1:3)=(1-t)*posA+t*posB;
end
newMotion(find(isnan(newMotion)))=0;
skelPlayDataA(skel,[A;newMotion;B])

结果

第二种插值方法

Slerp的思想就是沿着$4D$球面上连接两个四元数的弧插值。

先看平面上的两个$2D$向量$v_0$和$v_1$都是单位向量，我们需要计算$v_t$它是沿着$v_0$到$v_1$弧的平滑插值。设$w$是$v_0$到$v_t$弧所截的角，那么$v_t$就是$v_1$沿弧旋转$tw$的结果。

需要考虑两点问题：一是四元数$q$和$-q$代表同一方位，但是作为slerp的参数时，可能有不一样的结果，是因为$4D$球面不是欧式空间的直接扩展，而这种现象在$2D 3D$空间是不会发生的。解决方法是选择$q_0$和$q_1$的符号使得点乘$q_0\cdot q_1$的结果是非负。第二就是如果$q_0$和$q_1$非常接近，$\sin\theta$会非常小，这时除法会出现问题，解决方法是此时采用线性插值。

在论文《从运动捕获数据中提取关键帧》也有介绍到这种四元数插值方法，这里直接贴过来，有兴趣去看看论文：

若$q_1=[w_1,x_1,y_1,z_1]$和$q_2=[w_2,x_2,y_2,z_2]$为两个单位四元数，它们之间的球面线性插值为

$$slerp(q_1,q_2;t)=\frac{\sin(1-t)\theta}{\sin\theta}q_1+\frac{\sin t\theta}{\sin\theta}q_2$$
其中 $\theta=\arccos(w_1w_2+x_1x_2+y_1y_2+z_1z_2)$

直接撸代码：

function [ q3 ] = jointslerp( q1, q2, t )
%SLERP quaternion slerp
%   computes the slerp of value t between quaternions q1 and q2
%https://gist.github.com/simonlynen/5349167
q1 = q1 ./ norm(q1);
q2 = q2 ./ norm(q2);

one = 1.0 - eps;
d = q1'*q2;
absD = abs(d);

if(absD >= one)
    scale0 = 1 - t;
    scale1 = t;
else
    % theta is the angle between the 2 quaternions
    theta = acos(absD);
    sinTheta = sin(theta);

    scale0 = sin( ( 1.0 - t ) * theta) / sinTheta;
    scale1 = sin( ( t * theta) ) / sinTheta;
end
if(d < 0)
    scale1 = -scale1;
end

q3 = scale0 * q1 + scale1 * q2;
q3 = q3 ./ norm(q3);
end

同样使用此算法对运动捕捉数据进行插值：

%第二个插值方法
clear
clc
close all
addpath(genpath('.'))

%读取两个运动数据skel,A,B
load sample.mat
% skelPlayDataA(skel,[A;B])
%将欧拉角转换为四元数
quatA=joint_euler2quat(skel,A);
quatB=joint_euler2quat(skel,B);
%执行四元数插值，插20帧
internum=20;
temp_quat=zeros(31,4);%31个关节，每个关节一个四元数
newMotion=zeros(internum,62);%20帧，每帧62维
for i=1:internum
    t=i/internum;
    %对于角度采用四元数插值
    for j=1:size(quatA,1)
        temp_quat(j,:)=jointslerp(quatA(j,:)',quatB(j,:)',t);        
    end
    newMotion(i,:)=joint_quat2euler(temp_quat);
    %对于位置采用线性插值
    posA=A(1,1:3);posB=B(1,1:3);
    newMotion(i,1:3)=(1-t)*posA+t*posB;
end
newMotion(find(isnan(newMotion)))=0;
skelPlayDataA(skel,[A;newMotion;B])

结果：

后记

其实之前写过类似博客，但是不是用markdown写的，排版真的好丑，我就把它们删掉，写到此博客了。代码连接：链接：https://pan.baidu.com/s/1uLadyPL8yPlQWdPpLSWVrw 密码：asph

代码也可以到我个人的CSDN上传空间去找。