📚 课程目录第2章五轴圆弧插补实现

2. 五轴圆弧插补实现

任务目标

在五轴空间内实现圆弧插补：给定空间三点（起点、中间点、终点）及起点/终点姿态，计算圆心与半径，生成圆弧路径点，并在路径点上进行姿态线性插值，输出可供后续运动控制使用的插补点序列（位置+姿态）。

核心实现

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

def compute_arc_center(p1, p2, p3):
    """由空间三点计算圆心与半径（三点不共线）"""
    v1 = p2 - p1
    v2 = p3 - p1
    n = np.cross(v1, v2)
    if np.linalg.norm(n) < 1e-10:
        raise ValueError("三点共线，无法构成圆弧")
    # 圆心在过p1p2中点且垂直于v1的平面上，也在过p1p3中点且垂直于v2的平面上
    A = np.array([2*v1, 2*v2])
    b = np.array([np.dot(v1, p1+p2), np.dot(v2, p1+p3)])
    # 最小二乘解（三点确定唯一圆心）
    center, _, _, _ = np.linalg.lstsq(A, b, rcond=None)
    radius = np.linalg.norm(p1 - center)
    return center, radius

def generate_arc_points(p1, p2, p3, q_start, q_end, num_points=50):
    """生成圆弧路径点（位置+姿态四元数）"""
    center, radius = compute_arc_center(p1, p2, p3)
    # 建立圆弧平面坐标系
    v1 = p1 - center
    v2 = p3 - center
    n = np.cross(v1, v2)
    n = n / np.linalg.norm(n)
    # 计算总圆心角（取短弧方向）
    theta1 = np.arctan2(np.dot(np.cross(v1, v2), n), np.dot(v1, v2))
    if theta1 < 0:
        theta1 += 2 * np.pi
    # 生成角度序列
    angles = np.linspace(0, theta1, num_points)
    # 旋转矩阵：绕n轴旋转angle
    rot_vec = n * angles[:, np.newaxis]  # (N,3)
    R = Rotation.from_rotvec(rot_vec).as_matrix()  # (N,3,3)
    # 计算路径点位置
    points = center + np.einsum('nij,j->ni', R, v1)
    # 姿态插值（四元数slerp）
    quat_start = Rotation.from_quat(q_start)
    quat_end = Rotation.from_quat(q_end)
    t = np.linspace(0, 1, num_points)
    quats = (quat_start * (quat_end * quat_start.inv()) ** t[:, np.newaxis]).as_quat()
    return points, quats, center, radius, theta1

# ===== 最小验证示例 =====
if __name__ == "__main__":
    # 定义三点（单位圆上取三点，便于验证）
    p1 = np.array([1.0, 0.0, 0.0])
    p2 = np.array([0.0, 1.0, 0.0])
    p3 = np.array([-1.0, 0.0, 0.0])
    # 起点/终点姿态（四元数，w,x,y,z）
    q_start = [1.0, 0.0, 0.0, 0.0]
    q_end = [0.0, 0.0, 1.0, 0.0]  # 绕z轴转180度

    pts, quats, c, r, ang = generate_arc_points(p1, p2, p3, q_start, q_end, num_points=10)
    print("圆心:", c)
    print("半径:", r)
    print("总圆心角(rad):", ang)
    print("首点位置:", pts[0], "末点位置:", pts[-1])
    print("首点姿态:", quats[0], "末点姿态:", quats[-1])
    # 验证：所有点到圆心距离应等于半径
    dists = np.linalg.norm(pts - c, axis=1)
    print("最大半径误差:", np.max(np.abs(dists - r)))

运行与验证

依赖安装：

pip install numpy scipy

运行方式：将上述代码保存为 arc_interp_5axis.py，执行 python arc_interp_5axis.py。

通过条件：

输出圆心接近 [0,0,0]，半径接近 1.0（数值误差 < 1e-6）；
总圆心角接近 π（180°短弧）；
首点位置等于 p1，末点位置等于 p3；
最大半径误差 < 1e-6，证明所有生成点均在圆弧上；
姿态首末值与输入四元数一致，中间点姿态平滑过渡。

关键说明

圆心计算采用最小二乘求解线性方程组，适用于三点确定的唯一圆弧；若三点共线则抛出异常。
圆弧方向取从 p1 经 p2 到 p3 的短弧方向（圆心角 < 2π），通过叉积法向量的符号确定旋转方向。
姿态插值使用四元数球面线性插值（slerp），避免欧拉角插值的万向锁问题，输出为 (w,x,y,z) 格式四元数。
本实现输出位置与姿态序列，可直接作为五轴运动控制器的插补点输入；如需速度规划或加减速控制，可在后续环节对 num_points 对应的时间戳进行处理。