3. 空间直线插补

任务目标

在三维空间中，给定起点 \(P_0(x_0, y_0, z_0)\) 与终点 \(P_1(x_1, y_1, z_1)\)，按指定插补周期 \(T_s\) 和进给速度 \(v\)，计算各轴增量，输出从起点到终点的插补点序列。本案例实现位置前馈型直线插补，不涉及姿态插补与加减速规划。

实现原理

空间直线插补的核心是将总位移矢量 \(\vec{D} = P_1 - P_0\) 按插补周期离散化。设插补周期为 \(T_s\)（秒），进给速度为 \(v\)（mm/s），则每个周期的步长为 \(\Delta L = v \cdot T_s\)。总插补步数 \(N = \lceil |\vec{D}| / \Delta L \rceil\)，各轴增量为 \(\Delta x = D_x / N\)，\(\Delta y = D_y / N\)，\(\Delta z = D_z / N\)。第 \(k\) 个插补点坐标为：

\[ P_k = P_0 + k \cdot (\Delta x, \Delta y, \Delta z), \quad k = 1, 2, \dots, N \]

最后一轴增量按 \(N\) 均分，保证终点精确落在 \(P_1\) 上（因 \(N\) 取整后，实际步长略小于或等于 \(\Delta L\)，但终点位置精确）。

代码实现

<code>import numpy as np

def spatial_line_interpolation(p0, p1, v, Ts):
    """
    空间直线插补
    :param p0: 起点 [x0, y0, z0] (mm)
    :param p1: 终点 [x1, y1, z1] (mm)
    :param v:  进给速度 (mm/s)
    :param Ts: 插补周期 (s)
    :return:   N×3 插补点数组（含起点，不含终点；终点由最后一步到达）
    """
    p0 = np.array(p0, dtype=float)
    p1 = np.array(p1, dtype=float)
    D = p1 - p0
    dist = np.linalg.norm(D)
    if dist < 1e-9:
        return p0.reshape(1, 3)  # 起终点重合

    step_len = v * Ts
    N = int(np.ceil(dist / step_len))  # 总步数
    delta = D / N                      # 各轴增量

    # 生成插补点序列：k=1..N，共N个点，第N个点即终点
    k = np.arange(1, N + 1).reshape(-1, 1)
    points = p0 + k * delta
    return points

# 示例：起点(0,0,0)，终点(30,40,50)，速度10mm/s，周期0.1s
if __name__ == "__main__":
    P0 = [0, 0, 0]
    P1 = [30, 40, 50]
    v = 10.0
    Ts = 0.1

    pts = spatial_line_interpolation(P0, P1, v, Ts)
    print(f"插补点数: {len(pts)}")
    print("前3个插补点:")
    print(pts[:3])
    print("最后一个插补点(应为终点):")
    print(pts[-1])
    print("终点误差:", np.linalg.norm(pts[-1] - np.array(P1)))

运行与验证

运行方式：需安装 NumPy（pip install numpy）。将代码保存为 line_interp.py，执行 python line_interp.py。

预期输出：总距离 \(\sqrt{30^2+40^2+50^2}=70.0\) mm，步长 \(10 \times 0.1 = 1.0\) mm，步数 \(N=70\)。输出70个插补点，前3个点应为 \((0.4286, 0.5714, 0.7143)\)、\((0.8571, 1.1429, 1.4286)\)、\((1.2857, 1.7143, 2.1429)\)（四舍五入到4位小数），最后一个点精确为 \((30, 40, 50)\)，终点误差为0。

验证方法：检查输出点数是否为70；验证相邻点间距均为1.0mm（因均分后实际步长 = 70/70 = 1.0）；验证所有点共线，即任意连续三点构成的向量方向一致；验证终点与输入终点完全一致。

关键点说明

• 步数 \(N\) 使用向上取整，确保实际步长不超过理论步长，且终点精确可达。
• 各轴增量由总位移除以步数得到，而非直接使用理论步长分解，避免累积误差。
• 若需输出包含起点的完整轨迹，可在返回数组前插入 p0；当前实现返回从第一个插补点开始的序列，便于直接作为运动控制指令。