四元数插值：Slerp

Candice_jy

已于 2024-08-08 23:54:18 修改

阅读量776

点赞数 5

文章标签：机器学习人工智能计算机视觉 opencv python

于 2024-08-08 23:52:37 首次发布

本文链接：https://blog.youkuaiyun.com/qq_52039107/article/details/141038304

版权

已知

time1时刻的旋转四元数q1，平移向量t1

time2时刻的旋转四元数q2，平移向量t2

目标时刻time_target

目的

通过插值求解目标时刻的旋转四元数q_target和平移向量t_target

平移向量的求解

直接使用线性插值

$\alpha = \frac{time\_target-time1}{time2-time1}$

$t\_target = \alpha*(t2-t1)$

旋转四元数的求解

1. Lerp

简单的向量插值

$v_t=Lerp(v_0,v_1,\alpha)=v_0+\alpha(v_1-v_0)$

应用到单位四元数上

$q_t=Lerp(q_0,q_1,\alpha)=q_0+\alpha(q_1-q_0)$

2. Nlerp

这样插值算出的 $q_t$ 并不是单位四元数，因此除以它的模长转换为单位四元数

$q_t'=\frac{q_t}{||q_t||}$

3. Slerp

Nlerp插值存在的问题：当需要插值的弧比较大时，弦上的线性插值与对应的弧插值相差较大

如图，五个t值将整个弧和弦分割成了四个部分，虽然弦上的四段是等长的，但四个弧长完全不相等。

解决方法（Slerp）：对角度线性插值

如下图，如果 $v_0$ 和 $v_1$ 之间的角度为 $\theta$ ，则 $v_t$ 与 $v_0$ 之间的角度为 $\alpha\theta$

如下图，可以将vt分解到“ v0” 和 “v1正交于v0的向量部分对应的单位向量norm_v1_orthoganal_v0”

$v_t=|v_{tx}|v_0+|v_{ty}|norm\_v_1\_orthoganal\_v_0$

其中

$|v_{tx}|=v_tcos\alpha\theta=cos\alpha\theta$

$|v_{ty}|=v_tsin\alpha\theta=sin\alpha\theta$

v1正交于v0的向量部分v1_orth_v0如下图

等于v1减去"v1在v0上的投影"

！！！！最终结果！！！！

$v1\_proj\_v_0=v1*cos\theta=v1*\frac{v1*v0}{|v1||v0|}=v1*(v1*v0)$

$v1\_orth\_v0=v1-v1\_proj\_v0$

$norm\_v1\_orthoganal\_v0=\frac{v1\_orth\_v0}{|v1\_orth\_v0|}$

$v_t=v_0*cos\alpha\theta+norm\_v_1\_orthogonal\_v_0*sin\alpha\theta$

4. 最短插值路径问题

已知：两个不同的单位四元数q和-q对应的同一旋转

如下图情况所示，当两向量夹角大于90度时，对q0到-q1进行插值的旋转变化量会更小，路径更短

因此，在对两个四元数进行插值之前，需要先检测q0与q1之间是否为钝角，即它们的点积是否为负数。如果q0·q1<0，那么我们反转其中的一个四元数，比如令q1=-q1，进而保证插值路径最短。

完整代码

注意：list之间是不可以直接相减的，需要转换为narray

list_example = np.array(list_example)

import numpy as np
from scipy.spatial.transform import Rotation as R

def slerp(q1, q2, alpha):
    # 确保 q1 和 q2 是 numpy 数组
    q1 = np.array(q1)
    q2 = np.array(q2)
    
    # 计算四元数的内积
    dot_product = np.dot(q1, q2)
    
    # 为确保插值路径的最短性，如果内积为负，则反转 q2
    if dot_product < 0.0:
        q2 = -q2
        dot_product = -dot_product
    
    # 限制内积范围在 [0, 1] 内
    dot_product = np.clip(dot_product, -1.0, 1.0)
    
    # 如果内积接近1，直接线性插值
    if dot_product > 0.9995:
        result = q1 + alpha * (q2 - q1)
        result /= np.linalg.norm(result)  # 除以范数得到为单位向量
        return result
    
    # 计算插值角度和系数
    theta_0 = np.arccos(dot_product)  # theta_0 是 q1 和 q2 之间的角度
    theta = theta_0 * alpha  # theta 是插值角度
    
    q2_proj_q1 = q1 * dot_product
    q2_orthogonal_q1 = q2 - q2_proj_q1
    q2_orthogonal_q1 /= np.linalg.norm(q2_orthogonal_q1)
    
    return q1 * np.cos(theta) + q2_orthogonal_q1 * np.sin(theta)

def interpolate_pose(q1, t1, q2, t2, t_target, t_time1, t_time2):
    # 计算时间插值系数
    alpha = (t_target - t_time1) / (t_time2 - t_time1)
    
    # 确保 t1 和 t2 是 numpy 数组
    t1 = np.array(t1)
    t2 = np.array(t2)
    
    # 平移向量插值
    t_interpolated = t1 + alpha * (t2 - t1)
    
    # 将旋转矩阵转换为四元数
    # q1 = R.from_matrix(R1).as_quat()
    # q2 = R.from_matrix(R2).as_quat()
    
    # 手动实现 Slerp（球面线性插值）
    q_interpolated = slerp(q1, q2, alpha)
    
    # 将插值后的四元数转换回旋转矩阵
    # R_interpolated = R.from_quat(q_interpolated).as_matrix()
    
    return q_interpolated, t_interpolated

参考链接：3D数学笔记——四元数的插值_四元数插值-优快云博客