2. 三轴空间直线插补

任务目标

实现三轴(X/Y/Z)空间直线插补器：输入起点、终点与进给速度，输出各轴同步运动的位置序列，保证三轴同时到达终点。通过不同方向组合的测试用例验证插补正确性。

实现原理

采用时间分割插补法：将空间直线按插补周期等分为N段，每周期各轴增量 = 该轴总位移 / N。由于各轴每周期增量比例恒定，三轴严格同步。总步数N由最长轴决定，确保小位移轴不因取整丢失终点。

关键完整代码

import numpy as np

class LinearInterpolator3D:
    def __init__(self, feed_rate=100.0, period=0.01):
        """
        feed_rate: 合成进给速度 (mm/s)
        period: 插补周期 (s)
        """
        self.feed = feed_rate
        self.T = period

    def plan(self, start, end):
        """
        规划三轴直线插补
        参数: start, end = (x, y, z) 元组/列表
        返回: list of (x, y, z) 位置点，含起点不含终点
        """
        s = np.array(start, dtype=float)
        e = np.array(end, dtype=float)
        delta = e - s
        dist = np.linalg.norm(delta)

        if dist < 1e-9:
            return []  # 起点终点重合

        # 总插补周期数（向上取整保证覆盖全程）
        total_time = dist / self.feed
        n = int(np.ceil(total_time / self.T))
        n = max(n, 1)  # 至少1步

        # 每周期各轴增量
        step = delta / n

        points = []
        current = s.copy()
        for _ in range(n):
            current = current + step
            points.append(tuple(current))
        return points

    def verify_sync(self, points, start, end, tol=1e-6):
        """验证终点精度与同步性"""
        if not points:
            return False, "无插补点"
        last = np.array(points[-1])
        target = np.array(end)
        err = np.linalg.norm(last - target)
        if err > tol:
            return False, f"终点误差 {err:.2e} > {tol}"

        # 同步性：各轴位移比例应全程一致
        s = np.array(start)
        total_disp = target - s
        ratios = []
        for p in points:
            disp = np.array(p) - s
            # 各轴已完成比例
            r = [disp[i]/total_disp[i] if abs(total_disp[i])>1e-9 else 1.0
                 for i in range(3)]
            ratios.append(r)
        # 检查每点三轴比例最大偏差
        max_dev = max(max(abs(r[0]-r[1]), abs(r[0]-r[2]), abs(r[1]-r[2]))
                      for r in ratios)
        if max_dev > 1e-6:
            return False, f"同步偏差 {max_dev:.2e}"
        return True, f"通过 (终点误差 {err:.2e}, 同步偏差 {max_dev:.2e})"

# ===== 测试不同方向组合 =====
if __name__ == "__main__":
    interp = LinearInterpolator3D(feed_rate=100.0, period=0.01)

    cases = [
        ((0,0,0), (100,0,0)),      # 纯X轴
        ((0,0,0), (0,80,0)),       # 纯Y轴
        ((0,0,0), (0,0,60)),       # 纯Z轴
        ((0,0,0), (60,80,0)),      # XY平面
        ((0,0,0), (60,80,100)),    # 空间对角线
        ((10,20,30), (70,-40,90)), # 负方向组合
        ((0,0,0), (-50,-60,-70)),  # 全负方向
    ]

    for i, (s, e) in enumerate(cases):
        pts = interp.plan(s, e)
        ok, msg = interp.verify_sync(pts, s, e)
        print(f"用例{i+1}: {s} -> {e}")
        print(f"  插补点数: {len(pts)}, 验证: {msg}")

运行与验证

运行方式：直接执行 python linear_interp_3d.py（需NumPy）。

预期输出：7个用例全部打印“验证: 通过”，且终点误差与同步偏差均小于1e-6。例如用例5（空间对角线）输出类似：

用例5: (0,0,0) -> (60,80,100)
  插补点数: 13, 验证: 通过 (终点误差 0.00e+00, 同步偏差 0.00e+00)

判定标准：所有用例验证函数返回True。若任一用例终点误差>1e-6或同步偏差>1e-6，则插补实现有误。

关键说明

• 插补点数N = ceil(距离/(速度×周期))，距离为空间直线长度，保证合成速度恒定。
• 每周期增量 = 总位移/N，浮点累加可能引入微小误差，验证容差设为1e-6。
• 同步性验证原理：任意时刻各轴已完成位移与总位移之比应相等（比例相同即同步）。
• 负方向组合用例验证了增量方向正确性，纯单轴用例验证了退化情况。