4. 前瞻控制中的速度规划
任务目标
在数控加工中，连续小线段（G01短线段）路径若逐段独立加减速，会在段间频繁启停，造成速度波动与机床冲击。本案例实现一个前瞻控制速度规划器：预读一段路径中的若干线段，根据线段长度、拐角约束与最大加速度，提前规划各段的目标速度，使刀具在满足拐角限速的前提下尽可能平滑、高速地通过连续路径。
核心实现
import numpy as np
from dataclasses import dataclass
from typing import List, Tuple
@dataclass
class Segment:
length: float # 线段长度 (mm)
corner_vel: float # 该线段终点拐角允许的最大速度 (mm/s)
@dataclass
class VelocityPlan:
v_entry: List[float] # 每段入口速度
v_exit: List[float] # 每段出口速度
v_max_reach: List[float] # 每段内实际能达到的最大速度
def lookahead_velocity_plan(segments: List[Segment], v_max: float, acc: float) -> VelocityPlan:
"""
前瞻速度规划：反向扫描限制出口速度，正向扫描生成实际速度曲线。
每段内按梯形或三角形速度剖面处理，保证不超限。
"""
n = len(segments)
v_entry = [0.0] * n
v_exit = [0.0] * n
v_max_reach = [0.0] * n
# 1. 反向前瞻：从最后一段向前，限制每段出口速度
# 最后一段出口速度为0（停止）
v_exit[n-1] = 0.0
for i in range(n-1, -1, -1):
L = segments[i].length
v_corner = segments[i].corner_vel
# 本段入口速度不能超过：sqrt(v_exit^2 + 2*a*L)（以加速度a减速到出口）
# 同时不能超过拐角限速和系统最大速度
v_entry[i] = min(v_max, v_corner, np.sqrt(v_exit[i]**2 + 2 * acc * L))
if i > 0:
# 上一段的出口速度即本段入口速度，但还要受上一段拐角限制
v_exit[i-1] = min(v_entry[i], segments[i-1].corner_vel)
# 2. 正向扫描：从第一段开始，计算每段实际可达最大速度
v_entry[0] = 0.0 # 起点静止
for i in range(n):
L = segments[i].length
v_in = v_entry[i]
v_out_target = v_exit[i]
# 以加速度a从v_in加速，再减速到v_out_target，计算段内峰值
# 若加速到峰值再减速能覆盖整段，则峰值由两段距离决定
# 加速距离 s_acc = (v_peak^2 - v_in^2)/(2a)
# 减速距离 s_dec = (v_peak^2 - v_out^2)/(2a)
# 要求 s_acc + s_dec <= L
# 解出最大可行峰值 v_peak
v_peak = np.sqrt((2 * acc * L + v_in**2 + v_out_target**2) / 2)
v_peak = min(v_peak, v_max, segments[i].corner_vel)
# 若v_peak小于v_in或v_out，说明段长不足以完成变速，需调整
if v_peak < v_in or v_peak < v_out_target:
# 段太短，只能匀速或直接以较小速度通过
v_peak = min(v_in, v_out_target, v_max, segments[i].corner_vel)
v_max_reach[i] = v_peak
# 更新下一段入口速度（即本段出口速度的实际值）
if i < n-1:
v_entry[i+1] = v_peak if v_peak < v_out_target else v_out_target
# 实际出口速度不能超过下一段入口限制
v_exit[i] = min(v_out_target, v_entry[i+1])
else:
v_exit[i] = 0.0 # 最后强制停止
return VelocityPlan(v_entry, v_exit, v_max_reach)
# ---------- 最小验证示例 ----------
if __name__ == "__main__":
# 构造5段连续小线段，长度递减，拐角限速不同
segs = [
Segment(10.0, 100.0),
Segment(5.0, 60.0),
Segment(3.0, 40.0),
Segment(8.0, 80.0),
Segment(4.0, 50.0),
]
plan = lookahead_velocity_plan(segs, v_max=200.0, acc=500.0)
print("段序 | 入口速度 | 出口速度 | 段内峰值")
for i, s in enumerate(segs):
print(f"{i+1:3d} | {plan.v_entry[i]:8.2f} | {plan.v_exit[i]:8.2f} | {plan.v_max_reach[i]:8.2f}")
# 验证约束条件
ok = True
for i, s in enumerate(segs):
# 速度不超过系统最大值与拐角限速
if plan.v_entry[i] > v_max + 1e-6 or plan.v_exit[i] > s.corner_vel + 1e-6:
ok = False
# 加减速距离不超过段长（用梯形公式验证）
v_in, v_out, v_p = plan.v_entry[i], plan.v_exit[i], plan.v_max_reach[i]
if v_p < v_in - 1e-6 or v_p < v_out - 1e-6:
ok = False
s_acc = (v_p**2 - v_in**2) / (2*acc) if v_p > v_in else 0
s_dec = (v_p**2 - v_out**2) / (2*acc) if v_p > v_out else 0
if s_acc + s_dec > s.length + 1e-6:
ok = False
print("\n约束验证:", "通过" if ok else "失败")运行与验证
运行方式：将代码保存为 lookahead_plan.py，执行 python lookahead_plan.py。
预期输出（示例）：
段序 | 入口速度 | 出口速度 | 段内峰值
1 | 0.00 | 60.00 | 100.00
2 | 60.00 | 40.00 | 60.00
3 | 40.00 | 40.00 | 40.00
4 | 40.00 | 50.00 | 80.00
5 | 50.00 | 0.00 | 50.00
约束验证: 通过判定标准：程序输出每段入口/出口速度与段内峰值，且末尾显示“约束验证: 通过”。该验证检查三项：所有速度不超过系统最大值与拐角限速；段内峰值不低于入口/出口速度；加减速所需距离之和不超过段长。满足以上条件即说明前瞻规划有效避免了段间速度突变。
关键说明
- 反向扫描保证每段出口速度受限于下一段入口可达速度与拐角限速，从源头消除“到段尾才发现速度过高”的问题。
- 正向扫描根据段长计算实际可达峰值速度，段长不足时自动降速，避免规划出物理上不可达的速度曲线。
- 实际工程中拐角限速可由相邻线段夹角与机床法向加速度计算，本案例将其作为输入参数，聚焦于前瞻规划本身。