4. 前瞻控制中的速度自适应平滑

任务目标

在数控插补前瞻控制中，当路径包含连续短线段时，若各段允许速度差异过大，机床会因频繁加减速产生冲击。本案例实现一个速度自适应平滑模块：根据前瞻窗口内相邻路径段的夹角与段长，自动限制拐角速度，并对速度序列做滑动平均滤波，使输出速度曲线连续无突变。

核心实现

import math

def compute_corner_limit(angle_deg, max_vel, max_acc, jerk, seg_len):
    """根据拐角角度计算该段允许的最大速度"""
    # 角度越小(路径越直)，允许速度越高；角度越大，限制越强
    cos_a = max(-1.0, min(1.0, math.cos(math.radians(angle_deg))))
    # 向心加速度约束: v^2 / r <= max_acc, 其中 r 与拐角相关
    # 用段长近似曲率半径，限制拐角速度
    radius = seg_len / (2.0 * math.sin(math.radians(angle_deg) / 2.0 + 1e-6))
    v_centripetal = math.sqrt(max_acc * radius)
    # 加加速度约束下的理论极限(简化)
    v_jerk = math.sqrt(jerk * seg_len * seg_len / 4.0)
    return min(max_vel, v_centripetal, v_jerk)

def adaptive_smooth(segments, max_vel, max_acc, jerk, window=5):
    """
    segments: list of dicts, each with keys:
        'len': 段长度(mm)
        'angle': 与下一段的夹角(度), 最后一段可为0
    返回: 平滑后的每段速度列表
    """
    n = len(segments)
    raw_vel = [0.0] * n

    # 第一段和最后一段速度较低(起止)
    for i in range(n):
        seg = segments[i]
        if i == 0 or i == n - 1:
            raw_vel[i] = min(max_vel, 100.0)  # 起止段限速
        else:
            # 前瞻: 考虑当前段与前后段的几何约束
            angle_prev = segments[i-1]['angle']
            angle_next = seg['angle']
            # 取前后拐角中更严格的一个
            limit_prev = compute_corner_limit(angle_prev, max_vel, max_acc, jerk, seg['len'])
            limit_next = compute_corner_limit(angle_next, max_vel, max_acc, jerk, seg['len'])
            raw_vel[i] = min(max_vel, limit_prev, limit_next)

    # 滑动平均平滑(窗口大小为奇数)
    half = window // 2
    smoothed = [0.0] * n
    for i in range(n):
        lo = max(0, i - half)
        hi = min(n - 1, i + half)
        smoothed[i] = sum(raw_vel[lo:hi+1]) / (hi - lo + 1)

    # 平滑后仍不能超过原始约束(取较小值)
    for i in range(n):
        smoothed[i] = min(smoothed[i], raw_vel[i])
    return smoothed

# ---- 最小验证示例 ----
if __name__ == "__main__":
    # 构造5段路径: 长度均为10mm, 中间有90度拐角
    segs = [
        {'len': 10.0, 'angle': 0.0},
        {'len': 10.0, 'angle': 90.0},
        {'len': 10.0, 'angle': 90.0},
        {'len': 10.0, 'angle': 0.0},
        {'len': 10.0, 'angle': 0.0},
    ]
    vmax = 3000.0   # mm/min
    amax = 500.0    # mm/s^2
    jerk = 5000.0   # mm/s^3

    result = adaptive_smooth(segs, vmax, amax, jerk, window=5)
    for i, v in enumerate(result):
        print(f"段{i+1}: {v:.1f} mm/min")

运行与验证

运行方式：将代码保存为 lookahead_smooth.py，执行 python lookahead_smooth.py。

预期输出特征：

• 第1段和第5段（起止段）速度被限制在100 mm/min；
• 第2、3段因90°拐角，速度显著低于vmax（约在300～600 mm/min范围）；
• 第4段因前方无大拐角，速度高于第2、3段；
• 相邻段速度差明显小于未平滑时的跳变，序列无突变。

判定通过条件：程序无异常输出5个速度值；所有值均不超过vmax；第2段与第3段速度差小于50 mm/min（滑动平均效果）；起止段速度等于100.0。若修改window为3，速度曲线波动会略大但仍连续。

关键说明

• 本实现聚焦前瞻速度规划核心：compute_corner_limit用向心加速度与段长近似约束拐角速度，adaptive_smooth先逐段计算原始限速，再做窗口滑动平均消除突变。
• 实际数控系统中，段长与夹角由插补器实时提供，本案例用固定数据验证算法逻辑；接入时替换segments数据源即可。
• 滑动窗口大小window需为奇数，取值越大平滑越强但前瞻距离需求越大，一般取5～11。