1. 两轴直线插补基础

任务目标

实现两轴(X/Y)联动直线插补：给定起点(0,0)与终点(100,75)，以恒定合成速度运动，输出各插补周期内的位置指令，验证轨迹精度(轮廓误差)与速度平滑性(速度波动率)。

核心实现

import math

class LinearInterpolator:
    def __init__(self, x0, y0, x1, y1, v, ts):
        """
        x0,y0: 起点坐标(mm)
        x1,y1: 终点坐标(mm)
        v: 合成速度(mm/s)
        ts: 插补周期(s)
        """
        self.x0, self.y0 = x0, y0
        self.x1, self.y1 = x1, y1
        self.v = v
        self.ts = ts

        # 计算总位移与单位方向向量
        dx = x1 - x0
        dy = y1 - y0
        self.total_len = math.hypot(dx, dy)
        if self.total_len == 0:
            raise ValueError("起点与终点重合")
        self.ux = dx / self.total_len   # X方向单位向量
        self.uy = dy / self.total_len   # Y方向单位向量

        # 每个插补周期的步长
        self.step = v * ts

        # 计算总插补步数(向上取整保证到达终点)
        self.total_steps = math.ceil(self.total_len / self.step)

    def generate(self):
        """生成插补点序列(含起点与终点)"""
        points = [(self.x0, self.y0)]
        for i in range(1, self.total_steps + 1):
            # 当前步的弧长距离(最后一步截断到终点)
            dist = min(i * self.step, self.total_len)
            x = self.x0 + self.ux * dist
            y = self.y0 + self.uy * dist
            points.append((round(x, 6), round(y, 6)))
        return points

# ========== 验证 ==========
if __name__ == "__main__":
    # 参数设置
    interp = LinearInterpolator(0, 0, 100, 75, v=50, ts=0.002)

    pts = interp.generate()
    n = len(pts)

    # 1. 终点精度验证
    end_x, end_y = pts[-1]
    pos_err = math.hypot(end_x - 100, end_y - 75)
    print(f"插补点数: {n}")
    print(f"终点坐标: ({end_x}, {end_y})")
    print(f"终点位置误差: {pos_err:.6f} mm")

    # 2. 轨迹精度验证(最大轮廓误差)
    # 理论直线: 3x - 4y = 0 (由(0,0)->(100,75)化简)
    max_dev = 0.0
    for (px, py) in pts:
        dev = abs(3*px - 4*py) / 5.0   # 点到直线距离
        max_dev = max(max_dev, dev)
    print(f"最大轮廓误差: {max_dev:.6f} mm")

    # 3. 速度平滑性验证(相邻点间实际速度)
    speeds = []
    for i in range(1, n):
        dx = pts[i][0] - pts[i-1][0]
        dy = pts[i][1] - pts[i-1][1]
        seg_len = math.hypot(dx, dy)
        speeds.append(seg_len / interp.ts)

    v_max = max(speeds)
    v_min = min(speeds)
    v_avg = sum(speeds) / len(speeds)
    ripple = (v_max - v_min) / v_avg * 100
    print(f"速度范围: {v_min:.3f} ~ {v_max:.3f} mm/s")
    print(f"速度波动率: {ripple:.2f}%")

运行与验证

运行方式：直接执行 python linear_interp.py（需Python 3.6+，无第三方依赖）。

通过条件：

• 终点位置误差 < 0.001 mm（由浮点舍入决定，实际为0）
• 最大轮廓误差 < 0.001 mm（插补点严格位于理论直线上）
• 速度波动率 < 1%（除最后一步因截断减速外，中间各步速度恒定）

最后一步因总步长取整导致实际步长缩短，速度会低于设定值，这是离散化截断的正常现象。若需严格恒速，可调整速度或插补周期使总位移恰好为步长整数倍。

关键说明：插补点全部位于理论直线上，因此轮廓误差理论上为0；速度波动仅来源于最后一步的截断效应。实际运动控制系统中，插补周期通常与伺服驱动周期同步，本实现可直接替换为实时任务循环调用。