如何实现带终点指定速度约束的半球面A-B轨迹生成（基于SLERP）

SLERP是匀速球面旋转插值，其终点速度方向固定（垂直于A和B的旋转轴），无法直接满足自定义的终点速度约束。我们可以使用四元数Squad插值（三次球面插值）来实现带终点速度约束的轨迹生成，核心思路是通过切线控制点调整轨迹的导数方向。

关键步骤说明 #

1. 速度向量预处理：球面上任意点的速度必须与该点的位置向量垂直（位置向量长度固定，导数与自身点积为0），因此需要先将输入的速度向量投影到B点的切平面并归一化。
2. 四元数Squad插值：Squad插值通过四个控制点（起点、终点、两个切线控制点）生成平滑的球面曲线，我们可以通过终点的目标速度方向推导对应的切线控制点，确保轨迹在B点的速度方向符合要求。

完整实现代码 #

import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D

# 四元数操作工具函数
def quat_mult(q1, q2):
    w1, x1, y1, z1 = q1
    w2, x2, y2, z2 = q2
    return np.array([
        w1*w2 - x1*x2 - y1*y2 - z1*z2,
        w1*x2 + x1*w2 + y1*z2 - z1*y2,
        w1*y2 - x1*z2 + y1*w2 + z1*x2,
        w1*z2 + x1*y2 - y1*x2 + z1*w2
    ])

def quat_conj(q):
    return np.array([q[0], -q[1], -q[2], -q[3]])

def quat_norm(q):
    return q / np.linalg.norm(q)

def quat_exp(q):
    # 仅支持纯虚四元数的指数运算
    w, x, y, z = q
    if w != 0:
        raise ValueError("Only pure imaginary quaternions are supported for exp here")
    theta = np.linalg.norm([x, y, z])
    if np.isclose(theta, 0):
        return np.array([1, 0, 0, 0])
    s = np.sin(theta / 2)
    c = np.cos(theta / 2)
    return np.array([c, x*s/theta, y*s/theta, z*s/theta])

def quat_slerp(q0, q1, t):
    dot = np.dot(q0, q1)
    if dot < 0:
        q1 = -q1
        dot = -dot
    dot = np.clip(dot, -1.0, 1.0)
    omega = np.arccos(dot)
    if np.isclose(omega, 0):
        return q0
    s0 = np.sin((1 - t)*omega)
    s1 = np.sin(t*omega)
    return quat_norm(s0*q0 + s1*q1)

def quat_squad(q0, q1, a0, a1, t):
    q_t0 = quat_slerp(q0, q1, t)
    q_t1 = quat_slerp(a0, a1, t)
    return quat_slerp(q_t0, q_t1, 2*t*(1 - t))

def vec_to_quat(v):
    # 3D向量转纯虚四元数
    return np.array([0, v[0], v[1], v[2]])

def quat_to_vec(q):
    # 纯虚四元数转3D向量
    return q[1:]

def project_to_tangent_plane(v, point):
    # 将向量投影到球面点的切平面并归一化方向
    point = point / np.linalg.norm(point)
    tangent_v = v - np.dot(v, point) * point
    return tangent_v / np.linalg.norm(tangent_v) if not np.isclose(np.linalg.norm(tangent_v), 0) else np.zeros(3)

def constrained_spherical_trajectory(A, B, target_v_end, num_points=10):
    # 归一化起点和终点
    A = A / np.linalg.norm(A)
    B = B / np.linalg.norm(B)
    
    # 预处理目标速度：投影到B的切平面
    v_end = project_to_tangent_plane(target_v_end, B)
    
    # 转换为四元数表示
    qA = vec_to_quat(A)
    qB = vec_to_quat(B)
    
    # 计算角速度向量：v_end = omega × B → omega = B × v_end
    omega = np.cross(B, v_end)
    omega_quat = vec_to_quat(omega)
    
    # 计算Squad的切线控制点
    alpha = 0.5  # 切线强度参数，可调整
    a1_quat = quat_mult(qB, quat_exp(-0.5 * alpha * omega_quat))
    a0_quat = quat_mult(qA, quat_exp(0.5 * alpha * vec_to_quat(np.cross(A, B))))
    
    # 生成轨迹点
    trajectory = []
    for t in np.linspace(0, 1, num_points):
        q_t = quat_squad(qA, qB, a0_quat, a1_quat, t)
        trajectory.append(quat_to_vec(q_t))
    
    return np.array(trajectory)

# 示例使用
A_cartesian = np.array([1, 0, 0])  # 起点A（单位球）
B_cartesian = np.array([0, 1, 0])  # 终点B（单位球）
target_v_end = np.array([0, 0, 1])  # 终点目标速度方向

num_points = 20
trajectory = constrained_spherical_trajectory(A_cartesian, B_cartesian, target_v_end, num_points)

# 可视化
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ax.plot(trajectory[:, 0], trajectory[:, 1], trajectory[:, 2], 'bo-', label="Constrained Trajectory")
ax.scatter(*A_cartesian, color='red', label='Start (A)')
ax.scatter(*B_cartesian, color='green', label='End (B)')

# 绘制目标速度方向
ax.quiver(B_cartesian[0], B_cartesian[1], B_cartesian[2], 
          target_v_end[0], target_v_end[1], target_v_end[2], 
          color='orange', label='Target Velocity')

# 绘制球面
u, v = np.mgrid[0:2 * np.pi:20j, 0:np.pi:10j]
x = np.cos(u) * np.sin(v)
y = np.sin(u) * np.sin(v)
z = np.cos(v)
ax.plot_wireframe(x, y, z, color='gray', alpha=0.3)

ax.set_xlabel("X")
ax.set_ylabel("Y")
ax.set_zlabel("Z")
ax.legend()
plt.show()

# 验证终点速度方向（数值求导）
delta_t = 1/(num_points-1)
end_velocity = (trajectory[-1] - trajectory[-2])/delta_t
end_velocity = end_velocity / np.linalg.norm(end_velocity)
print("实际终点速度方向：", end_velocity)
print("目标速度方向：", project_to_tangent_plane(target_v_end, B_cartesian))

代码解释 #

• 四元数操作：四元数是处理球面旋转的高效工具，通过四元数的乘法、指数、SLERP和Squad插值实现平滑的球面轨迹。
• 切线控制点计算：通过对终点B对应的四元数进行微小旋转生成切线控制点a1_quat，旋转方向由目标速度推导的角速度向量决定，确保轨迹在终点的速度方向符合要求。
• 速度验证：通过数值求导可以验证轨迹终点的速度方向与目标方向一致。

内容的提问来源于stack exchange，提问作者Pratham