2. 两轴直线插补

任务目标

编写一个两轴直线插补程序，输入起点坐标 (x0, y0) 与终点坐标 (x1, y1)，输出从起点到终点路径上的一系列插补点。通过本案例掌握直线插补的核心原理：将直线运动离散化为等间距的坐标点序列，供后续运动控制使用。

直线插补原理

直线插补的核心是参数化直线方程。设起点为 P0(x0, y0)，终点为 P1(x1, y1)，则直线上任意一点可表示为：

P(t) = P0 + t × (P1 − P0)，其中 t ∈ [0, 1]

将 t 按插补步数 N 等分，即可得到 N+1 个插补点。步长由插补周期与进给速度决定：若插补周期为 T（秒），进给速度为 F（单位/秒），则每步移动距离 d = F × T，总步数 N = ceil(L / d)，其中 L 为直线总长度。

完整实现代码

import math

def linear_interpolation(x0, y0, x1, y1, feed_rate, period):
    """
    两轴直线插补
    参数:
        x0, y0: 起点坐标
        x1, y1: 终点坐标
        feed_rate: 进给速度（单位/秒）
        period: 插补周期（秒）
    返回:
        插补点列表 [(x, y), ...]，包含起点和终点
    """
    # 计算直线长度
    dx = x1 - x0
    dy = y1 - y0
    length = math.hypot(dx, dy)
    
    if length == 0:
        return [(x0, y0)]
    
    # 计算步长与步数
    step_dist = feed_rate * period
    steps = max(1, math.ceil(length / step_dist))
    
    # 生成插补点
    points = []
    for i in range(steps + 1):
        t = i / steps
        x = x0 + dx * t
        y = y0 + dy * t
        points.append((round(x, 4), round(y, 4)))
    
    return points

# 示例：从 (0, 0) 到 (10, 5)，进给速度 2 单位/秒，插补周期 0.5 秒
if __name__ == "__main__":
    result = linear_interpolation(0, 0, 10, 5, 2, 0.5)
    for pt in result:
        print(pt)

运行方式

将上述代码保存为 linear_interp.py，在命令行执行：

python linear_interp.py

验证方法

程序输出应为从 (0,0) 到 (10,5) 的插补点序列。直线总长度 L = √(10² + 5²) ≈ 11.18，步长 d = 2 × 0.5 = 1，步数 N = ceil(11.18 / 1) = 12。因此应输出 13 个点，首点为 (0,0)，末点为 (10,5)，中间点均匀分布在直线上。

通过条件：

• 输出点数 = 13
• 第一个点为 (0,0)，最后一个点为 (10,5)
• 任意相邻两点间距 ≈ 1.0（允许浮点舍入误差）
• 所有点满足直线方程：y = 0.5x（即 y/x = 0.5）

预期输出（前5个点）

(0, 0)
(0.8333, 0.4167)
(1.6667, 0.8333)
(2.5, 1.25)
(3.3333, 1.6667)
...

验证时可将输出点绘制在坐标图上，观察是否形成一条从原点到 (10,5) 的直线段。若需实际运动控制，将返回的坐标点依次发送给两轴伺服驱动器即可。