运动规划——多姿态插值

多姿态插补用于多个连续的姿态，此处采用Squad插值，贝塞尔相关参考深入理解贝塞尔曲线

图上$B_1,A_2$为$s_i,s_{i+1}$

插补核心公式如下
$$\begin{gathered} Squad(q_i,q_{i+1}.s_i,s_{i+1},h)=Slerp(Slerp(q_i,q_{i+1},h),Slerp(s_i,s_{i+1},h),2h(1-h)) \\ {s}_i=q_i\exp (-\frac{\log (q_i^{-1}{q}_{i+1})+\log (q_i^{-1}{q}_{i-1})}{4}) \end{gathered}$$

插补一系列点$q_{i-2},q_{i-1},q_i,q_{i+1}$时，两两分组进行插补，在插补前需要生成中间控制点$s_i$,它可以确保姿态及其导数在插补点$q$处连续，$s_i$的生成需要$q_{i-1},q_i,q_{i+1}$，若插补点组为边缘点，可以直接让$q_{i-1}=q_i$或$q_{i+1}=q_i$，$h$为插补细分$[0,1]$。

一、中间点s的确定

Squad在控制点的姿态上是平滑的
$$\begin{gathered} \begin{array}{rl}\mathrm{Squad}\left(q_{i-1},q_i,s_{i-1},s_i,1\right)& =\mathrm{Slerp}\left(\mathrm{Slerp}\left(q_{i-1},q_i,1\right),\mathrm{Slerp}\left(s_{i-1},s_i,1\right),0\right)\\ & =\mathrm{Slerp}\left(q_i,s_i,0\right)\\ & =q_i\end{array} \\ \begin{array}{rl}\mathrm{Squad}\left(q_i,q_{i+1},s_i,s_{i+1},0\right)& =\mathrm{Slerp}\left(\mathrm{Slerp}\left(q_i,q_{i+1},0\right),\mathrm{Slerp}\left(s_i,s_{i+1},0\right),0\right)\\ & =\mathrm{Slerp}\left(q_i,s_i,0\right)\\ & =q_i\end{array} \end{gathered}$$

因此从速度平滑上找到中间点

$$\begin{gathered} \begin{array}{rl}\frac{d}{dt}\mathrm{Squad}\left(q_{i-1},q_i,s_{i-1},s_i,1\right)& =\mathrm{Slerp}\left(q_{i-1},q_i,1\right)\log \left(q_{i-1}^{\ast }{q}_i\right)+\\ & \mathrm{Slerp}\left(q_{i-1},q_i,1\right)\frac{d}{dt}\left(g_{i-1}(h{)}^{2h(1-h)}\right)(1)\\ =& q_i\log \left(q_{i-1}^{\ast }{q}_i\right)+q_i\left[0,-2{\theta }_{g_{i-1}}(1){\mathbf{v}}_{g_{i-1}}(1)\right]\\ =& q_i\left(\log \left(q_{i-1}^{\ast }{q}_i\right)-2\log \left(\left[\cos \left({\theta }_{g_{i-1}}(1)\right),\sin \left({\theta }_{g_{i-1}}(1)\right){\mathbf{v}}_{g_{i-1}}(1)\right]\right)\right)\\ =& q_i\left(\log \left(q_{i-1}^{\ast }{q}_i\right)-2\log \left(g_{i-1}(1)\right)\right)\\ =& q_i\left(\log \left(q_{i-1}^{\ast }{q}_i\right)-2\log \left(q_i^{\ast }{s}_i\right)\right)\end{array} \\ \begin{array}{rl}\frac{d}{dt}\mathrm{Squad}\left(q_i,q_{i+1},s_i,s_{i+1},0\right)& =\mathrm{Slerp}\left(q_i,q_{i+1},0\right)\log \left(q_i^{\ast }{q}_{i+1}\right)+\\ & \mathrm{Slerp}\left(q_i,q_{i+1},0\right)\frac{d}{dt}\left(g_i(h{)}^{2h(1-h)}\right)(0)\\ =& q_i\log \left(q_i^{\ast }{q}_{i+1}\right)+q_i\left[0,2{\theta }_{g_i}(0){\mathbf{v}}_{g_i}(0)\right]\\ =& q_i\left(\log \left(q_i^{\ast }{q}_{i+1}\right)+2\log \left(\left[\cos \left({\theta }_{g_i}(0)\right),\sin \left({\theta }_{g_i}(0)\right){\mathbf{v}}_{g_i}(0)\right]\right)\right)\\ =& q_i\left(\log \left(q_i^{\ast }{q}_{i+1}\right)+2\log \left(g_i(0)\right)\right)\\ =& q_i\left(\log \left(q_i^{\ast }{q}_{i+1}\right)+2\log \left(q_i^{\ast }{s}_i\right)\right)\end{array} \end{gathered}$$

$s_i$需要满足两式相等，因此

$$\begin{array}{rl}4\log \left(q_i^{\ast }{s}_i\right)& =\log \left(q_{i-1}^{\ast }{q}_i\right)-\log \left(q_i^{\ast }{q}_{i+1}\right)\\ q_i^{\ast }{s}_i& =\exp \left(\frac{\log \left(q_{i-1}^{\ast }{q}_i\right)-\log \left(q_i^{\ast }{q}_{i+1}\right)}{4}\right)\\ s_i& =q_i\exp \left(\frac{\log \left(q_{i-1}^{\ast }{q}_i\right)-\log \left(q_i^{\ast }{q}_{i+1}\right)}{4}\right)\end{array}$$

其中因为$q$为单位四元数，因此$q^{\ast }=q^{-1},\log (q^{\ast })=-\log (q)$

$$\begin{array}{rl}{s}_i& =q_i\exp \left(-\frac{\log \left(q_i^{\ast }{q}_{i-1}\right)+\log \left(q_i^{\ast }{q}_{i+1}\right)}{4}\right)\\ & =q_i\exp \left(-\frac{\log \left(q_i^{-1}{q}_{i-1}\right)+\log \left(q_i^{-1}{q}_{i+1}\right)}{4}\right)\end{array}$$

程序实现

function [qa] = get_intermediate_control_point(j,q)
% 插补中间点
% 若点为起点和终点，则直接返回当前值
L = size(q,2);
if(j==1)
    qa =  q(:,1)';
    return;
elseif(j==L)
    qa =  q(:,L)';
    return;
else
    qji=quatinv(q(:,j)');
    qiqm1=quatmultiply(qji,q(:,j-1)');
    qiqp1=quatmultiply(qji,q(:,j+1)');
    ang_vel =-((quatlog(qiqp1)+quatlog(qiqm1))/4); 
    
    qa = quatmultiply(q(:,j)',quatexp(ang_vel));
    %qa = quatnormalize(qa);
end
end

二、插补间距的确定

当插补姿态为一系列姿态时，全局插补分割仍沿用[0,1]分割，但需要映射到不同组姿态内（在进行具体插补时，依然以两个姿态一组进行插补，但调用函数时是整个系列的全局分割）。

程序实现

function alpha = eval_alpha(s,i,L)
% 将s[0,1]映射到q1-q4的范围内，判断s在哪两个q之间，然后在计算s在对应区间的值，
% 如L=4,s=0.7,则k=3.1,插值应该在q3,q4之间(3.1-4.1)，插值为0.1,
% 若i不在范围内，直接返回0不计算
k = s*(L-1)+1;

if((i>=k)&&(i

三、slerp插值

slerp标准公式
$$Slerp(t;q_0,q_1)=\frac{q_0\sin [(1-t)\theta ]+q_1\sin (t\theta )}{\sin \theta }(t\in [0,1])$$
对于单位四元数，该公式可写为
$$Slerp(t;q_0,q_1)=q_0(q_0^{-1}{q}_1{)}^t$$
其中单位四元数可写为
$$q=\cos (\theta )+\hat{u}\sin (\theta )$$

$\hat{u}$为单位三维矢量，$\hat{u}\hat{u}=-1$
$$\begin{gathered} \exp (\hat{u}\theta )=\cos (\theta )+\hat{u}\sin (\theta ) \\ {q}^t=(\cos (\theta )+\hat{u}\sin (\theta ){)}^t=\exp (\hat{u}\theta t) \\ \log (q)=\log (\cos (\theta )+\hat{u}\sin (\theta ))=log(\exp (\hat{u}\theta ))=\hat{u}\theta \end{gathered}$$
因此
$$(q_0^{-1}{q}_1{)}^t=\exp (\hat{u}\theta t)=\exp (\log (q_0^{-1}{q}_1)\ast t)$$

Slerp插值可写为
$$Slerp(t;q_0,q_1)=q_0\exp (\log (q_0^{-1}{q}_1)\ast t)$$

程序实现

q1_inv    = quatinv(q1);
q1_inv_q2 = quatmultiply(q1_inv,q2);
omega     = quatlog(q1_inv_q2);
q_out 	  = quatmultiply(q1,quatexp(omega*t));

四、实现效果

function [val] =  quat_squad(q,s)

L = size(q,2);
val = q(:,1)';

% 若点积小于0进行取负，确保两姿态之间取最短路径
for j=2:L
    C = dot(q(:,j-1),q(:,j));
    if(C<0)
        q(:,j) = -q(:,j);
    end 
end

% 若时间在0,1则直接返回起点和终点
if s==0 
    val=q(:,1);
    return;
elseif s==1
    val=q(:,end);
    return;
end

for j =2:L
    % 全局细分映射到局部细分
    alpha=eval_alpha(s,j,L); 
    t= alpha;
    
    if(alpha>0)
        EPS = 1e-9;

        % 计算两个姿态的点积，结果范围[-1,1]
        C = dot(q(:,j-1),q(:,j));

        if ((1 - C) <= EPS) 
            % 姿态之间过于接近采用线性插补
            val=q(:,j-1)'*(1-s)+q(:,j)'*s; 
            val = quatnormalize(val);
	    return;
		end

        if((1 + C) <= EPS)
            % 当姿态夹角接近180，无最短路径，结果不确定
            % 将角度旋转90
            qtemp(1) = q(4,j); qtemp(2) = -q(3,j); qtemp(3)= q(2,j); qtemp(4) = -q(1,j);
            q(:,j) = qtemp';
        end

        % 计算中间点
        qa = get_intermediate_control_point(j-1,q);
        qap1 = get_intermediate_control_point(j,q);

        % 插补
        qtemp1 = do_slerp(q(:,j-1)', q(:,j)', t);
        qtemp2 = do_slerp(qa, qap1, t);
        squad = do_slerp(qtemp1, qtemp2, 2*t*(1-t));
        val = squad;val = quatnormalize(val);
	return;
    end

end

end

function [qa] = get_intermediate_control_point(j,q)
% 插补中间点
% 若点为起点和终点，则直接返回当前值
L = size(q,2);
if(j==1)
    qa =  q(:,1)';
    return;
elseif(j==L)
    qa =  q(:,L)';
    return;
else
    qji=quatinv(q(:,j)');
    qiqm1=quatmultiply(qji,q(:,j-1)');
    qiqp1=quatmultiply(qji,q(:,j+1)');
    ang_vel =-((quatlog(qiqp1)+quatlog(qiqm1))/4); 
    
    qa = quatmultiply(q(:,j)',quatexp(ang_vel));
    %qa = quatnormalize(qa);
end
end

function alpha = eval_alpha(s,i,L)
% 全局细分到局部细分
k = s*(L-1)+1;

if((i>=k)&&(i q2
    q2 =qtemp;
end
% 插补
q1_inv    = quatinv(q1);
q1_inv_q2 = quatmultiply(q1_inv,q2);
omega     = quatlog(q1_inv_q2);
q_out = quatmultiply(q1,quatexp(omega*t));
%q_out = quatnormalize(q_out);
end

参考

Quaternions, Interpolation and Animation-6.2 Interpolation over a series of rotations: Heuristic approach

Eberly, David. “Quaternion algebra and calculus.” Magic Software, Inc 21 (2002).