GRBL运动控制算法（四）加减速运算

前言

在数控系统和运动控制领域，GRBL 作为一款高效、轻量化的开源固件，因其卓越的性能和简洁的架构被广泛应用于各类嵌入式运动控制场景。GRBL加减速算法的实现尤为关键——它直接决定了运动控制的精度、效率与设备稳定性。

本文深入解析加减速运算的核心机制，速度规划帮助读者掌握动态速度调整与实时控制的关键技术。

一.GRBL加减速速度计算

1 .加减速流程

/* --------------------------------------------------------------------------------- 
         根据入口和出口速度计算新规划块的速度曲线，
         或者如果规划器已更新它，则重新计算部分完成的规划块的曲线。
         对于命令强制减速（如进给保持），覆盖规划器速度并减速到目标出口速度。
      */
      prep.mm_complete = 0.0; // 默认速度曲线在块末端0.0mm处完成
      float inv_2_accel = 0.5/pl_block->acceleration;
      if (sys.state & (STATE_HOLD|STATE_MOTION_CANCEL|STATE_SAFETY_DOOR)) { // [强制减速到零速度]
        // 计算进给保持进行中的速度曲线参数。此曲线覆盖规划块曲线，强制减速到零速。
        prep.ramp_type = RAMP_DECEL;
        // 计算相对于块末端的减速距离
        float decel_dist = pl_block->millimeters - inv_2_accel*pl_block->entry_speed_sqr;
        if (decel_dist < 0.0) {
          // 整个规划块都在减速。进给保持的结束不在此块中。
          prep.exit_speed = sqrt(pl_block->entry_speed_sqr-2*pl_block->acceleration*pl_block->millimeters);
        } else {
          prep.mm_complete = decel_dist; // 进给保持的结束
          prep.exit_speed = 0.0;
        }
      } else { // [正常操作]
        // 计算或重新计算预装规划块的速度曲线参数
        prep.ramp_type = RAMP_ACCEL; // 初始化为加速斜坡
        prep.accelerate_until = pl_block->millimeters; 
        prep.exit_speed = plan_get_exec_block_exit_speed();   
        float exit_speed_sqr = prep.exit_speed*prep.exit_speed;
        float intersect_distance =
                0.5*(pl_block->millimeters+inv_2_accel*(pl_block->entry_speed_sqr-exit_speed_sqr));
        if (intersect_distance > 0.0) {
          if (intersect_distance < pl_block->millimeters) { // 梯形或三角形类型
            // 注意：对于加速-匀速和仅匀速类型，以下计算将为0.0
            prep.decelerate_after = inv_2_accel*(pl_block->nominal_speed_sqr-exit_speed_sqr);
            if (prep.decelerate_after < intersect_distance) { // 梯形类型
              prep.maximum_speed = sqrt(pl_block->nominal_speed_sqr);
              if (pl_block->entry_speed_sqr == pl_block->nominal_speed_sqr) { 
                // 匀速-减速或仅匀速类型
                prep.ramp_type = RAMP_CRUISE;
              } else {
                // 全梯形或加速-匀速类型
                prep.accelerate_until -= inv_2_accel*(pl_block->nominal_speed_sqr-pl_block->entry_speed_sqr); 
              }
            } else { // 三角形类型
              prep.accelerate_until = intersect_distance;
              prep.decelerate_after = intersect_distance;
              prep.maximum_speed = sqrt(2.0*pl_block->acceleration*intersect_distance+exit_speed_sqr);
            }          
          } else { // 仅减速类型
            prep.ramp_type = RAMP_DECEL;
            prep.maximum_speed = prep.current_speed;
          }
        } else { // 仅加速类型
          prep.accelerate_until = 0.0;
          prep.maximum_speed = prep.exit_speed;
        }
      }  
    }

用于根据入口和出口速度计算运动块的速度曲线，处理正常运动和强制减速两种情况。

变量定义:

整体流程：

强制减速流程：

正常运动流程：

2 .强制减速计算

速度-时间关系图：

判断减速到零所需距离：

$$\begin{array}{rl}{S}_{\text{d}}& =S-\frac{v_0^2}{2a}\end{array}$$

出口速度（当减速距离不足时，即 Sd<0）：

$$\begin{array}{rl}{V}_{\text{e}}& =\sqrt{V_0^2-2aS}\end{array}$$

出口速度（当减速距离足够时,即 Sd>0）：

$$\begin{array}{rl}{S}_{\text{c}}& =S_{\text{d}}\\ V_{\text{e}}& =0\end{array}$$

3 .交点距离计算

速度-时间关系图：

速度-位移关系：

$$\begin{array}{rl}{V}_{\text{max}}^2-V_0^2& =2a{S}_{\text{1}}\\ V_{\text{max}}^2-V_e^2& =2a{S}_{\text{2}}\\ S_{\text{1}}+S_{\text{2}}& =S\end{array}$$

联立(5-7)可得 Vmax：

$$\begin{array}{rl}{V}_{\text{max}}^2& =\frac{2aS+V_0^2+V_e^2}{2}\end{array}$$

交点距离 S_intersect：

$$\begin{array}{rl}{S}_{\text{intersect}}& =\frac{V_{\text{max}}^2-V_e^2}{2a}=\frac{2aS+V_0^2-V_e^2}{4a}=\frac{1}{2}\left(S+\frac{V_0^2-V_e^2}{2}\right)\end{array}$$

或者

$$\begin{array}{rl}{S}_{\text{intersect}}& =\frac{V_{\text{max}}^2-V_0^2}{2a}=\frac{2aS-V_0^2+V_e^2}{4a}=\frac{1}{2}\left(S+\frac{V_e^2-V_0^2}{2}\right)\end{array}$$

4 .仅加速类型

特点：整个运动块都在加速，没有减速阶段

速度-时间关系图：

判断条件：

$$\begin{array}{rl}{S}_{\text{intersect}}& <=0\end{array}$$

参数设置：

$$\begin{array}{rl}{S}_{\text{a}}& =0\\ V_{\text{max}}& =V_e\end{array}$$

5 .仅减速类型

特点：整个运动块都在减速，没有加速阶段

速度-时间关系图：

判断条件：

$$\begin{array}{rl}{S}_{\text{intersect}}& >=S\end{array}$$

参数设置：

$$\begin{array}{rl}{V}_{\text{max}}& =V_m\end{array}$$

6 .三角形类型

特点：先加速到最大速度后立即减速，没有匀速阶段

速度-时间关系图：

判断条件：

$$\begin{array}{rl}0<S_{\text{intersect}}& <S\\ S_e>S_{\text{intersect}}& \end{array}$$

参数设置：

$$\begin{array}{rl}{S}_{\text{a}}& =S_{\text{intersect}}\\ S_{\text{e}}& =S_{\text{intersect}}\\ V_{\text{max}}& =\sqrt{2a{S}_{\text{intersect}}+V_e^2}\end{array}$$

7 .梯形类型

特点：全梯形：有加速、匀速和减速阶段；加速-匀速：加速后直接进入匀速；匀速-减速：从匀速开始减速；仅匀速：整个块都是匀速运动；

速度-时间关系图：

判断条件：

$$\begin{array}{rl}0<S_{\text{intersect}}& <S\\ S_e<S_{\text{intersect}}& \end{array}$$

参数设置：

$$\begin{array}{rl}{S}_{\text{temp}}& =\frac{v_m^2-v_0^2}{2a}\\ V_{\text{max}}& =\sqrt{V_m^2}\\ S_{\text{a}}& =S-S_{\text{temp}}\end{array}$$

二.GRBL段生成计算

1 .段生成定义

    /*------------------------------------------------------------------------------------
        通过确定在段时间DT_SEGMENT内行进的总距离来计算这个新段的平均速度。
        以下代码首先尝试基于当前斜坡条件创建一个完整的段。如果段时间在斜坡状态变化处终止时不完整，
        代码将继续循环通过进展中的斜坡状态以填充剩余的段执行时间。然而，如果
        一个不完整的段在速度曲线末端终止，尽管执行时间被截断小于DT_SEGMENT，
        该段仍被视为完成。
        速度曲线总是假设通过斜坡序列进展：
        加速斜坡、匀速状态和减速斜坡。每个斜坡的行进距离可以从零到块的长度不等。
        速度曲线可以在规划块末端结束（典型）或在强制减速（如进给保持）的块中间结束。
    */
    float dt_max = DT_SEGMENT; // 最大段时间
    float dt = 0.0; // 初始化段时间
    float time_var = dt_max; // 时间工作变量
    float mm_var; // 毫米距离工作变量
    float speed_var; // 速度工作变量   
    float mm_remaining = pl_block->millimeters; // 从块末端开始的新段距离
    float minimum_mm = mm_remaining-prep.req_mm_increment; // 保证至少一步
    if (minimum_mm < 0.0) { minimum_mm = 0.0; }

    do {
      switch (prep.ramp_type) {
        case RAMP_ACCEL: 
          // 注意：加速斜坡仅在第一个do-while循环中计算
          speed_var = pl_block->acceleration*time_var;
          mm_remaining -= time_var*(prep.current_speed + 0.5*speed_var);
          if (mm_remaining < prep.accelerate_until) { // 加速斜坡结束
            // 加速-匀速、加速-减速斜坡连接点或块结束
            mm_remaining = prep.accelerate_until; // 注意：在EOB处为0.0
            time_var = 2.0*(pl_block->millimeters-mm_remaining)/(prep.current_speed+prep.maximum_speed);
            if (mm_remaining == prep.decelerate_after) { prep.ramp_type = RAMP_DECEL; }
            else { prep.ramp_type = RAMP_CRUISE; }
            prep.current_speed = prep.maximum_speed;
          } else { // 仅加速
            prep.current_speed += speed_var;
          }
          break;
        case RAMP_CRUISE: 
          // 注意：mm_var用于保留不完整段time_var计算的最后一个mm_remaining
          // 注意：如果maximum_speed*time_var值太低，舍入可能导致mm_var不改变。
          //  为了防止这种情况，只需在规划器中强制执行最小速度阈值。
          mm_var = mm_remaining - prep.maximum_speed*time_var;
          if (mm_var < prep.decelerate_after) { // 匀速结束
            // 匀速-减速连接点或块结束
            time_var = (mm_remaining - prep.decelerate_after)/prep.maximum_speed;
            mm_remaining = prep.decelerate_after; // 注意：在EOB处为0.0
            prep.ramp_type = RAMP_DECEL;
          } else { // 仅匀速
            mm_remaining = mm_var; 
          } 
          break;
        default: // case RAMP_DECEL:
          // 注意：mm_var用作杂项工作变量，以防止在接近零速时出错
          speed_var = pl_block->acceleration*time_var; // 用作速度增量（毫米/分钟）
          if (prep.current_speed > speed_var) { // 检查是否达到或低于零速
            // 计算从段末端到块末端的距离
            mm_var = mm_remaining - time_var*(prep.current_speed - 0.5*speed_var); // (毫米)
            if (mm_var > prep.mm_complete) { // 仅减速
              mm_remaining = mm_var;
              prep.current_speed -= speed_var;
              break; // 段完成。退出switch-case语句。继续do-while循环。
            }
          } // 块结束或强制减速结束
          time_var = 2.0*(mm_remaining-prep.mm_complete)/(prep.current_speed+prep.exit_speed);
          mm_remaining = prep.mm_complete; 
      }
      dt += time_var; // 将计算的斜坡时间添加到总段时间
      if (dt < dt_max) { time_var = dt_max - dt; } // **不完整** 在斜坡连接点
      else {
        if (mm_remaining > minimum_mm) { // 检查零步的非常慢的段
          // 增加段时间以确保段中至少一步。覆盖并循环
          // 通过距离计算直到minimum_mm或mm_complete
          dt_max += DT_SEGMENT;
          time_var = dt_max - dt;
        } else { 
          break; // **完成** 退出循环。段执行时间达到最大
        }
      }
    } while (mm_remaining > prep.mm_complete); // **完成** 退出循环。曲线完成

运动路径被划分为一系列“块（block）”，每个块代表一段直线路径。为了实现平滑运动，每个块被进一步划分为“段（segment）”，每个段执行时间固定：DT_SEGMENT。

变量定义:

2 .加速阶段

加速阶段流程图：

速度增量计算：

$$\begin{array}{r}\Delta v=a\cdot \Delta t\end{array}$$

距离计算公式（基于初始速度 + 加速度）：

$$\begin{array}{r}\Delta s=v_0\cdot \Delta t+\frac{1}{2}a\cdot (\Delta t{)}^2\end{array}$$

更新当前速度：

$$\begin{array}{r}{v}_{\text{new}}=v_{\text{old}}+\Delta v\end{array}$$

走完加速段（mm_remaining < accelerate_until）, 则重新计算时间段到这个连接点的时间：

$$\begin{array}{r}t=\frac{2\cdot \text{s}}{v_{\text{start}}+v_{\text{end}}}\end{array}$$

平均速度计算时间的公式：

$$\begin{array}{r}t=\frac{s}{\frac{v_1+v_2}{2}}=\frac{2s}{v_1+v_2}\end{array}$$

3 .匀速阶段

匀速阶段流程图：

匀速距离计算：

$$\begin{array}{r}\Delta s=v\cdot \Delta t\end{array}$$

判断是否走完匀速段：

若 mm_var < decelerate_after，说明匀速段结束，需要进入减速；否则，更新 mm_remaining 并继续。

4 .减速阶段

加速阶段流程图：

减速过程中的距离计算：

$$\begin{array}{r}\Delta s=v_0\cdot \Delta t-\frac{1}{2}a\cdot (\Delta t{)}^2\end{array}$$

减速后的速度为：

$$\begin{array}{r}{v}_{\text{new}}=v_{\text{old}}-a\cdot \Delta t\end{array}$$

若该段已达终点（即减速结束）：

$$\begin{array}{r}t=\frac{2\cdot s}{v_{\text{start}}+v_{\text{end}}}\end{array}$$