4. 五轴样条插补

任务目标

在五轴加工中，刀尖点路径与刀轴方向需同步平滑变化。本案例使用三次均匀B样条曲线对刀尖点位置序列进行插补，同时采用四元数球面线性插值（Slerp）对刀轴方向序列进行平滑过渡，生成连续且无突变的五轴插补路径。

核心实现

import numpy as np
from scipy.interpolate import splprep, splev

def bspline_interpolate(points, num_samples=100, smoothness=0):
    """
    对刀尖点序列进行B样条插补
    points: (N,3) 原始刀尖点
    返回: (num_samples,3) 平滑后的刀尖点路径
    """
    points = np.asarray(points, dtype=float)
    # 使用参数化B样条插值（s=0表示严格通过所有点）
    tck, u = splprep([points[:,0], points[:,1], points[:,2]], 
                     s=smoothness, k=3)
    u_new = np.linspace(0, 1, num_samples)
    x, y, z = splev(u_new, tck)
    return np.column_stack((x, y, z))

def quaternion_slerp(q_start, q_end, t):
    """四元数球面线性插值"""
    q_start = q_start / np.linalg.norm(q_start)
    q_end = q_end / np.linalg.norm(q_end)
    dot = np.clip(np.dot(q_start, q_end), -1.0, 1.0)
    
    if dot < 0:  # 确保最短路径
        q_end = -q_end
        dot = -dot
    
    if dot > 0.9995:  # 角度过小时线性插值
        result = q_start + t * (q_end - q_start)
        return result / np.linalg.norm(result)
    
    theta_0 = np.arccos(dot)
    theta = theta_0 * t
    sin_theta = np.sin(theta)
    sin_theta_0 = np.sin(theta_0)
    
    s0 = np.cos(theta) - dot * sin_theta / sin_theta_0
    s1 = sin_theta / sin_theta_0
    return s0 * q_start + s1 * q_end

def axis_to_quaternion(axis_vectors):
    """将刀轴方向向量转换为四元数（绕X轴旋转对齐）"""
    quats = []
    for v in axis_vectors:
        v = v / np.linalg.norm(v)
        # 将方向向量映射为旋转四元数（绕X轴旋转使Z轴对齐到v）
        z_axis = np.array([0, 0, 1.0])
        axis = np.cross(z_axis, v)
        axis_norm = np.linalg.norm(axis)
        if axis_norm < 1e-8:
            quats.append(np.array([1.0, 0, 0, 0]))  # 无旋转
        else:
            axis = axis / axis_norm
            angle = np.arccos(np.clip(np.dot(z_axis, v), -1, 1))
            q = np.append(np.cos(angle/2), np.sin(angle/2) * axis)
            quats.append(q)
    return np.array(quats)

def generate_five_axis_path(pos_points, axis_vectors, num_samples=100):
    """
    五轴样条插补主函数
    pos_points: (N,3) 刀尖点序列
    axis_vectors: (N,3) 刀轴方向序列
    返回: 平滑后的位置路径与刀轴方向
    """
    # 1. 位置B样条插补
    smooth_pos = bspline_interpolate(pos_points, num_samples)
    
    # 2. 刀轴方向四元数Slerp插补
    quats = axis_to_quaternion(axis_vectors)
    t_vals = np.linspace(0, 1, num_samples)
    smooth_axes = []
    
    # 将参数区间映射到原始点段
    n_segments = len(quats) - 1
    for t in t_vals:
        seg_pos = t * n_segments
        idx = min(int(seg_pos), n_segments - 1)
        local_t = seg_pos - idx
        q_interp = quaternion_slerp(quats[idx], quats[idx+1], local_t)
        # 四元数转方向向量
        v = np.array([
            2*(q_interp[1]*q_interp[3] + q_interp[0]*q_interp[2]),
            2*(q_interp[2]*q_interp[3] - q_interp[0]*q_interp[1]),
            q_interp[0]**2 - q_interp[1]**2 - q_interp[2]**2 + q_interp[3]**2
        ])
        smooth_axes.append(v / np.linalg.norm(v))
    
    return smooth_pos, np.array(smooth_axes)

# ========== 最小验证示例 ==========
if __name__ == "__main__":
    # 输入：5个刀尖点与对应刀轴方向
    pos_input = np.array([
        [0, 0, 0], [10, 5, 2], [20, 8, 5], 
        [30, 5, 3], [40, 0, 0]
    ])
    axis_input = np.array([
        [0, 0, 1], [0.2, 0.1, 0.98], [0.3, 0.2, 0.93],
        [0.2, 0.3, 0.93], [0, 0.5, 0.87]
    ])
    
    smooth_pos, smooth_axis = generate_five_axis_path(pos_input, axis_input, num_samples=50)
    
    # 验证1：插补点数量正确
    assert smooth_pos.shape == (50, 3), "位置插补点数量错误"
    assert smooth_axis.shape == (50, 3), "刀轴插补点数量错误"
    
    # 验证2：刀轴方向保持单位长度
    norms = np.linalg.norm(smooth_axis, axis=1)
    assert np.allclose(norms, 1.0, atol=1e-6), "刀轴方向未归一化"
    
    # 验证3：刀轴方向连续性（相邻点夹角不超过阈值）
    max_angle = 0
    for i in range(len(smooth_axis)-1):
        cos_angle = np.dot(smooth_axis[i], smooth_axis[i+1])
        angle = np.degrees(np.arccos(np.clip(cos_angle, -1, 1)))
        max_angle = max(max_angle, angle)
    print(f"最大相邻刀轴夹角: {max_angle:.4f}°")
    assert max_angle < 5.0, "刀轴方向变化过陡"
    
    # 验证4：首末点与输入一致（B样条插值通过所有点）
    assert np.allclose(smooth_pos[0], pos_input[0], atol=1e-3), "起点不匹配"
    assert np.allclose(smooth_pos[-1], pos_input[-1], atol=1e-3), "终点不匹配"
    
    print("验证通过：五轴样条插补路径生成成功")
    print(f"插补点数: {len(smooth_pos)}")
    print(f"位置范围: X[{smooth_pos[:,0].min():.1f}, {smooth_pos[:,0].max():.1f}]")

运行与验证

环境依赖：NumPy、SciPy（仅用于B样条插值）。

运行方式：

python five_axis_spline.py

通过条件：程序输出最大相邻刀轴夹角小于5°，且打印“验证通过”即表示插补路径满足平滑性要求。首末位置点与输入一致，证明B样条插值严格通过原始控制点。

关键说明

• 位置插补采用三次B样条（s=0强制插值通过所有输入点），保证刀尖路径的C²连续性。
• 刀轴方向使用四元数Slerp插值，避免欧拉角插值可能出现的万向锁问题，确保方向过渡自然无突变。
• 验证代码中的最大夹角阈值（5°）可根据实际加工需求调整；若需更平滑的刀轴变化，可增加插补采样点数。