机器人 / 系统课时

规划速度与加速度约束下的轨迹

规划速度与加速度约束下的轨迹

本课把已确定的机械臂 TCP 路径换算成带时间的运动轨迹。以起点、终点速度均为零的直线移动为例,根据路径长度、速度上限与加速度上限计算各阶段时长,并检查短行程为何达不到设定速度。

学习目标

  • 根据路径长度、最大速度与加速度上限,判断应采用梯形还是三角形速度曲线。
  • 计算加速、匀速、减速时间,并求出指定时刻的路径位移和速度。
  • 用程序核对轨迹终点,识别 TCP 限值与六个关节实际限值之间的区别。

明确路径与运动边界

先确定空间路径,再沿路径分配时间。用 s(t)s(t) 表示从起点开始的累计路程,用 v(t)v(t) 表示沿路径的标量速度;对固定姿态的直线移动,TCP 位置可由起点、单位方向向量和 s(t)s(t) 重建。曲线路径的 LL 应取实际弧长,不能直接使用首尾两点的直线距离。

本例采用对称加减速、起止速度为零、加速度大小恒定的理想模型。下列参数仅供离线仿真计算,不代表某型号机器人的额定能力。

参数 含义 示例值
LL TCP 路径长度 1.20 m
vmaxv_{max} 沿路径的速度上限 0.60 m/s
amaxa_{max} 沿路径的加速度上限 0.40 m/s²
起点/终点速度 两端需停稳 0 m/s

如果机器人系统以毫米为单位录入,对应值是 1200 mm、600 mm/s、400 mm/s²,不能把米和毫米直接混算。下面求出的只是这一运动段的理论时间,不包括夹爪动作、PLC 等待和停顿。

用临界距离选择速度曲线

从静止以 amaxa_{max} 加速到 vmaxv_{max},需要的单程加速距离为 sa=vmax2/(2amax)s_a=v_{max}^2/(2a_{max})。终点还要对称减速,因此达到最大速度所需的临界路径长度为:

Lcrit=vmax2amaxL_{crit}=\frac{v_{max}^2}{a_{max}}

代入本例,Lcrit=0.602/0.40=0.90L_{crit}=0.60^2/0.40=0.90 m。对于 1.20 m 的路径,长度超过 0.90 m,可以加速到限速并维持一段匀速;若路径只有 0.50 m,则必须提前减速,速度随时间呈三角形。恰好等于临界长度时,匀速时间为零。

梯形与三角形速度时间曲线及各阶段的路程面积

速度—时间曲线下的面积等于实际行程,斜线斜率的绝对值等于加速度。不能把 vmaxv_{max} 直接当作短行程一定能达到的速度。

计算各阶段时间与位置

当 L>LcritL>L_{crit} 时,峰值速度就是 vmaxv_{max},加速时间为 ta=vmax/amaxt_a=v_{max}/a_{max},匀速时间为 tc=(L−Lcrit)/vmaxt_c=(L-L_{crit})/v_{max},减速时间同样为 tat_a,总时间为 T=2ta+tcT=2t_a+t_c。

本例中,加速用 1.50 s 走过 0.45 m,匀速用 0.50 s 走过 0.30 m,减速再用 1.50 s 走过 0.45 m,因此 T=3.50T=3.50 s。到 2.00 s 时,TCP 已走过 0.45+0.60×0.50=0.750.45+0.60\times0.50=0.75 m。

当 L≤LcritL\le L_{crit} 时,匀速段消失,峰值速度应重新计算为 vpeak=amaxLv_{peak}=\sqrt{a_{max}L},ta=vpeak/amaxt_a=v_{peak}/a_{max},T=2taT=2t_a。例如 0.50 m 的行程,峰值仅为 0.447 m/s,总时间约 2.236 s。

计算任意时刻的路径位置,可按所属阶段套用下面的分段公式;其中 sa=amaxta2/2s_a=a_{max}t_a^2/2,减速段的局部时间为 τ=t−ta−tc\tau=t-t_a-t_c。

阶段 路程 s(t)s(t) 速度 v(t)v(t)
加速,0≤t≤ta0\le t\le t_a amaxt2/2a_{max}t^2/2 amaxta_{max}t
匀速,ta<t≤ta+tct_a<t\le t_a+t_c sa+vpeak(t−ta)s_a+v_{peak}(t-t_a) vpeakv_{peak}
减速,ta+tc<t≤Tt_a+t_c<t\le T sa+vpeaktc+vpeakτ−amaxτ2/2s_a+v_{peak}t_c+v_{peak}\tau-a_{max}\tau^2/2 vpeak−amaxτv_{peak}-a_{max}\tau

用 Python 核对轨迹计算

使用 Python 3.12 标准库运行下面的完整示例。它对 1.20 m 和 0.50 m 两种行程选择正确的速度曲线,并在阶段交界处检查累计路程和速度。

python
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")

预期输出如下(这里的时刻均由给定参数确定,不是设备时钟):

text
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,且两者的末速度都为零。若程序输出的终点位置不等于 LL,先检查加速段距离是否被重复计算,以及减速段的局部时间是否正确。

验证关节限值和实际工位约束

TCP 的线速度、线加速度满足设定值,并不代表六个关节的角速度与角加速度都合规。沿规划路径以 10 ms 为间隔求解连续的关节位置,再对相邻采样点做差分,分别核对各轴的速度、加速度上限;同时检查关节限位、奇异位形、工具姿态及夹具与障碍物的净空。

例如离线计算发现某关节峰值速度为 132°/s、限值为 120°/s;峰值加速度为 216°/s²、限值为 180°/s²。若保持相同路径并整体拉长时间,所需倍率至少为 k=max⁡(132/120,216/180)=1.10k=\max(132/120,\sqrt{216/180})=1.10,原先 3.50 s 的轨迹至少应调整到 3.85 s,并重新运行全部约束检查。

仿真验收时,把轨迹采样位置与终点目标对照,确认起止速度为零、全程关节限值未超标、工具没有碰撞,再在符合现场安全规程的条件下进行低速实际验证。

按自己的节奏,确认真正理解。

目录与阅读设置

阅读设置

17

记录一个想法

规划速度与加速度约束下的轨迹

0 / 10000 · 笔记保存到账号。未提交的修改会在关闭、离开或刷新后丢失。

登录哪都通