机器人 / 系统课时
规划速度与加速度约束下的轨迹
规划速度与加速度约束下的轨迹
本课把已确定的机械臂 TCP 路径换算成带时间的运动轨迹。以起点、终点速度均为零的直线移动为例,根据路径长度、速度上限与加速度上限计算各阶段时长,并检查短行程为何达不到设定速度。
学习目标
- 根据路径长度、最大速度与加速度上限,判断应采用梯形还是三角形速度曲线。
- 计算加速、匀速、减速时间,并求出指定时刻的路径位移和速度。
- 用程序核对轨迹终点,识别 TCP 限值与六个关节实际限值之间的区别。
明确路径与运动边界
先确定空间路径,再沿路径分配时间。用 表示从起点开始的累计路程,用 表示沿路径的标量速度;对固定姿态的直线移动,TCP 位置可由起点、单位方向向量和 重建。曲线路径的 应取实际弧长,不能直接使用首尾两点的直线距离。
本例采用对称加减速、起止速度为零、加速度大小恒定的理想模型。下列参数仅供离线仿真计算,不代表某型号机器人的额定能力。
| 参数 | 含义 | 示例值 |
|---|---|---|
| TCP 路径长度 | 1.20 m | |
| 沿路径的速度上限 | 0.60 m/s | |
| 沿路径的加速度上限 | 0.40 m/s² | |
| 起点/终点速度 | 两端需停稳 | 0 m/s |
如果机器人系统以毫米为单位录入,对应值是 1200 mm、600 mm/s、400 mm/s²,不能把米和毫米直接混算。下面求出的只是这一运动段的理论时间,不包括夹爪动作、PLC 等待和停顿。
用临界距离选择速度曲线
从静止以 加速到 ,需要的单程加速距离为 。终点还要对称减速,因此达到最大速度所需的临界路径长度为:
代入本例, m。对于 1.20 m 的路径,长度超过 0.90 m,可以加速到限速并维持一段匀速;若路径只有 0.50 m,则必须提前减速,速度随时间呈三角形。恰好等于临界长度时,匀速时间为零。

速度—时间曲线下的面积等于实际行程,斜线斜率的绝对值等于加速度。不能把 直接当作短行程一定能达到的速度。
计算各阶段时间与位置
当 时,峰值速度就是 ,加速时间为 ,匀速时间为 ,减速时间同样为 ,总时间为 。
本例中,加速用 1.50 s 走过 0.45 m,匀速用 0.50 s 走过 0.30 m,减速再用 1.50 s 走过 0.45 m,因此 s。到 2.00 s 时,TCP 已走过 m。
当 时,匀速段消失,峰值速度应重新计算为 ,,。例如 0.50 m 的行程,峰值仅为 0.447 m/s,总时间约 2.236 s。
计算任意时刻的路径位置,可按所属阶段套用下面的分段公式;其中 ,减速段的局部时间为 。
| 阶段 | 路程 | 速度 |
|---|---|---|
| 加速, | ||
| 匀速, | ||
| 减速, |
用 Python 核对轨迹计算
使用 Python 3.12 标准库运行下面的完整示例。它对 1.20 m 和 0.50 m 两种行程选择正确的速度曲线,并在阶段交界处检查累计路程和速度。
from math import sqrt
def plan(length: float, vmax: float, amax: float):
if min(length, vmax, amax) <= 0:
raise ValueError("路径长度、速度和加速度上限必须大于零")
critical = vmax * vmax / amax # 达到限速后再减至零所需的距离
if length > critical:
v_peak, t_acc = vmax, vmax / amax
t_cruise = (length - critical) / vmax
kind = "梯形"
else:
v_peak = sqrt(amax * length) # 短行程只达到较低的峰值速度
t_acc, t_cruise = v_peak / amax, 0.0
kind = "三角形"
return kind, v_peak, t_acc, t_cruise
def sample(t: float, v_peak: float, t_acc: float, t_cruise: float, amax: float):
total = 2 * t_acc + t_cruise
if not 0 <= t <= total:
raise ValueError("采样时间超出轨迹范围")
if t <= t_acc:
return 0.5 * amax * t * t, amax * t
s_acc = 0.5 * amax * t_acc * t_acc
if t <= t_acc + t_cruise:
return s_acc + v_peak * (t - t_acc), v_peak
tau = t - t_acc - t_cruise
s = s_acc + v_peak * t_cruise + v_peak * tau - 0.5 * amax * tau * tau
v = max(0.0, v_peak - amax * tau)
return s, v
for length in (1.20, 0.50):
kind, vp, ta, tc = plan(length, vmax=0.60, amax=0.40)
total = 2 * ta + tc
print(f"L={length:.2f}m 模式={kind} 加速={ta:.3f}s 匀速={tc:.3f}s 总时长={total:.3f}s 峰值速度={vp:.3f}m/s")
times = [0.0, ta, ta + tc, total] if tc > 0 else [0.0, ta, total]
for t in times:
s, v = sample(t, vp, ta, tc, 0.40)
print(f"t={t:.3f}s s={s:.3f}m v={v:.3f}m/s")预期输出如下(这里的时刻均由给定参数确定,不是设备时钟):
L=1.20m 模式=梯形 加速=1.500s 匀速=0.500s 总时长=3.500s 峰值速度=0.600m/s
t=0.000s s=0.000m v=0.000m/s
t=1.500s s=0.450m v=0.600m/s
t=2.000s s=0.750m v=0.600m/s
t=3.500s s=1.200m v=0.000m/s
L=0.50m 模式=三角形 加速=1.118s 匀速=0.000s 总时长=2.236s 峰值速度=0.447m/s
t=0.000s s=0.000m v=0.000m/s
t=1.118s s=0.250m v=0.447m/s
t=2.236s s=0.500m v=0.000m/s重点检查两个终点:长行程应在 3.500 s 时到达 1.200 m,短行程应在约 2.236 s 时到达 0.500 m,且两者的末速度都为零。若程序输出的终点位置不等于 ,先检查加速段距离是否被重复计算,以及减速段的局部时间是否正确。
验证关节限值和实际工位约束
TCP 的线速度、线加速度满足设定值,并不代表六个关节的角速度与角加速度都合规。沿规划路径以 10 ms 为间隔求解连续的关节位置,再对相邻采样点做差分,分别核对各轴的速度、加速度上限;同时检查关节限位、奇异位形、工具姿态及夹具与障碍物的净空。
例如离线计算发现某关节峰值速度为 132°/s、限值为 120°/s;峰值加速度为 216°/s²、限值为 180°/s²。若保持相同路径并整体拉长时间,所需倍率至少为 ,原先 3.50 s 的轨迹至少应调整到 3.85 s,并重新运行全部约束检查。
仿真验收时,把轨迹采样位置与终点目标对照,确认起止速度为零、全程关节限值未超标、工具没有碰撞,再在符合现场安全规程的条件下进行低速实际验证。