1. 直线插补基础

任务目标

实现两轴平面直线插补：给定起点、终点、最大速度与加速度，按梯形速度规划生成从起点到终点的一系列插补点，输出各轴位置序列。插补周期固定为1ms，位置序列可直接用于后续伺服控制或运动轨迹可视化。

实现方案

采用时间分割法：将直线轨迹按插补周期离散化，每个周期内计算当前进给速度（梯形加速-匀速-减速），再按速度方向分解到X/Y轴。核心数据结构为LinePlanner，负责规划总位移、加速段/减速段长度及实时位置计算。

关键完整代码

import math

class LinePlanner:
    def __init__(self, x0, y0, x1, y1, vmax, acc, dt=0.001):
        self.x0, self.y0 = x0, y0
        self.dx = x1 - x0
        self.dy = y1 - y0
        self.L = math.hypot(self.dx, self.dy)
        if self.L == 0:
            raise ValueError("起点与终点重合")
        self.vmax = vmax
        self.acc = acc
        self.dt = dt
        # 单位方向向量
        self.ux = self.dx / self.L
        self.uy = self.dy / self.L
        # 梯形规划参数
        self._plan_profile()

    def _plan_profile(self):
        # 加速到vmax所需距离
        d_acc = self.vmax * self.vmax / (2 * self.acc)
        if 2 * d_acc <= self.L:
            # 能达到最大速度：加速-匀速-减速
            self.v_peak = self.vmax
            self.d_acc = d_acc
            self.d_dec = d_acc
            self.d_const = self.L - 2 * d_acc
            self.t_acc = self.vmax / self.acc
            self.t_const = self.d_const / self.vmax
        else:
            # 无法达到最大速度：仅加速-减速
            self.v_peak = math.sqrt(self.acc * self.L)
            self.d_acc = self.L / 2
            self.d_dec = self.L / 2
            self.t_acc = self.v_peak / self.acc
            self.t_const = 0.0
        self.t_total = 2 * self.t_acc + self.t_const

    def get_position(self, t):
        """返回t时刻的轴位置(x, y)"""
        if t <= 0:
            return (self.x0, self.y0)
        if t >= self.t_total:
            return (self.x0 + self.dx, self.y0 + self.dy)
        # 计算当前时刻的直线位移s
        if t < self.t_acc:
            s = 0.5 * self.acc * t * t
        elif t < self.t_acc + self.t_const:
            s = self.d_acc + self.v_peak * (t - self.t_acc)
        else:
            t_dec = t - self.t_acc - self.t_const
            s = self.d_acc + self.d_const + self.v_peak * t_dec - 0.5 * self.acc * t_dec * t_dec
        return (self.x0 + s * self.ux, self.y0 + s * self.uy)

    def generate_sequence(self):
        """生成完整位置序列，返回[(x, y), ...]"""
        n = int(math.ceil(self.t_total / self.dt)) + 1
        return [self.get_position(i * self.dt) for i in range(n)]

# 使用示例：从(0,0)到(100,50)，最大速度200mm/s，加速度1000mm/s²
if __name__ == "__main__":
    planner = LinePlanner(0, 0, 100, 50, vmax=200, acc=1000)
    seq = planner.generate_sequence()
    print(f"插补点数: {len(seq)}")
    print(f"总耗时: {planner.t_total:.3f}s")
    print(f"终点: ({seq[-1][0]:.3f}, {seq[-1][1]:.3f})")
    # 验证终点误差
    err = math.hypot(seq[-1][0]-100, seq[-1][1]-50)
    print(f"终点误差: {err:.6f}mm")

运行与验证

1. 运行方式：将代码保存为line_interp.py，执行python line_interp.py。
2. 预期输出：插补点数约为573（总耗时约0.572s），终点坐标接近(100.000, 50.000)，终点误差小于0.001mm。
3. 通过条件：终点误差小于0.01mm；相邻插补点间距不超过vmax*dt=0.2mm；速度曲线呈梯形（可通过绘制位移-时间曲线观察斜率变化）。

关键说明

• 梯形规划中，若总位移不足以加速到最大速度，则自动降速为三角形速度曲线（仅加速-减速），代码已处理该边界情况。
• 位置序列按固定插补周期dt=1ms输出，实际运动控制中每个周期取一个点作为轴指令位置。
• 本实现为纯数学规划，不包含伺服闭环或物理电机模型，输出为理想轨迹位置序列。