GRBL运动控制算法（三）速度前瞻

前言

在 CNC 加工和精密运动控制领域，运动规划的性能直接决定了加工效率、表面质量和设备寿命。GRBL 作为一款高效、轻量的开源运动控制固件，其核心算法之一——速度前瞻（Look-Ahead），在复杂路径的连续运动中发挥着关键作用。通过动态调整运动段的衔接速度，显著减少了不必要的加减速停顿，从而实现了更平滑、更高速的轨迹控制。

本文将从算法原理、实现机制剖析 GRBL1.1 的速度前瞻设计，帮助读者深入理解其如何平衡“速度”与“精度”，以及如何通过前瞻路径段实现最优加减速规划。

一.GRBL速度前瞻流程

1 .函数定义

#ifdef USE_LINE_NUMBERS   
  void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate, int32_t line_number) 
#else
  void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate) 
#endif

这是一个条件编译的函数定义，根据USE_LINE_NUMBERS宏是否定义来决定是否包含line_number参数

参数说明：

target: 目标位置的各轴坐标数组(单位:mm)

feed_rate: 进给速率

invert_feed_rate: 标志位，表示是否使用反时间模式(即时间倒数模式)

line_number:(可选)行号，用于调试或追踪

2 .流程图

以下是速度前瞻流程图

二.GRBL速度前瞻分析

1 .运动块初始化

  // 获取当前缓冲区头部的运动块指针
  // block_buffer是环形缓冲区，block_buffer_head指向当前要填充的块
  plan_block_t *block = &block_buffer[block_buffer_head];
  
  // 初始化运动块参数
  block->step_event_count = 0;       // 步进事件计数器清零，用于记录总步数
  block->millimeters = 0;            // 运动距离清零(mm)，将计算实际运动距离
  block->direction_bits = 0;         // 方向位清零，每个bit代表一个轴的运动方向(0正/1负)
  block->acceleration = SOME_LARGE_VALUE; // 初始化为最大值，后续会根据各轴加速度限制调整
  
  // 如果启用了行号功能，记录当前G代码行号(用于错误报告和调试)
  #ifdef USE_LINE_NUMBERS
    block->line_number = line_number; 
  #endif

初始化运动块各字段：

step_event_count: 步进事件计数器初始化为0

millimeters: 运动距离初始化为0

direction_bits: 方向位初始化为0(每位代表一个轴的运动方向)

acceleration: 初始化为一个大值，后续会根据限制缩小

line_number: (可选)存储行号

2 .运动参数计算准备

  int32_t target_steps[N_AXIS];      // 存储各轴目标位置的步数
  float unit_vec[N_AXIS];            // 单位方向向量(运动方向的单位向量)
  float delta_mm;                    // 各轴移动距离(mm)
  uint8_t idx;                       // 轴索引

  // COREXY机构特殊处理(如CoreXY、H-bot等并联机构)
  #ifdef COREXY
    // 计算A/B电机的目标步数 = 目标位置(mm) × 每毫米步数
    // 注意：A/B电机是虚拟轴，实际控制XY运动
    target_steps[A_MOTOR] = lround(target[A_MOTOR]*settings.steps_per_mm[A_MOTOR]);
    target_steps[B_MOTOR] = lround(target[B_MOTOR]*settings.steps_per_mm[B_MOTOR]);
    
    // CoreXY特殊步数计算：
    // A电机步数 = |(X轴步数差 + Y轴步数差)|
    // B电机步数 = |(X轴步数差 - Y轴步数差)|
    block->steps[A_MOTOR] = labs((target_steps[X_AXIS]-pl.position[X_AXIS]) + 
                             (target_steps[Y_AXIS]-pl.position[Y_AXIS]));
    block->steps[B_MOTOR] = labs((target_steps[X_AXIS]-pl.position[X_AXIS]) - 
                             (target_steps[Y_AXIS]-pl.position[Y_AXIS]));
  #endif

target_steps: 目标位置步数数组

unit_vec: 单位向量数组

delta_mm: 各轴运动距离(mm)

A/B电机的目标步数 = 目标位置(mm) × 每毫米步数

对于COREXY并联机构(如H-bot、CoreXY等)：

计算A、B电机的步数差(使用X、Y轴的位置差)

A电机步数 = |(ΔX + ΔY)|

B电机步数 = |(ΔX - ΔY)|

3 .各轴运动参数计算

  for (idx=0; idx<N_AXIS; idx++) {
    // 计算各轴步数和运动参数
    #ifdef COREXY
      // 对于非A/B电机轴(如Z轴)的正常计算
      if (!(idx == A_MOTOR) && !(idx == B_MOTOR)) {
        // 目标步数 = 目标位置(mm) × 每毫米步数，四舍五入取整
        target_steps[idx] = lround(target[idx]*settings.steps_per_mm[idx]);
        // 计算步数差绝对值
        block->steps[idx] = labs(target_steps[idx]-pl.position[idx]);
      }
      // 更新最大步数(决定运动总时间)
      block->step_event_count = max(block->step_event_count, block->steps[idx]);
      
      // CoreXY特殊距离计算：
      if (idx == A_MOTOR) {
        // A电机移动距离 = (ΔX + ΔY)/A电机每毫米步数
        delta_mm = (target_steps[X_AXIS]-pl.position[X_AXIS] + 
                   target_steps[Y_AXIS]-pl.position[Y_AXIS])/settings.steps_per_mm[idx];
      } else if (idx == B_MOTOR) {
        // B电机移动距离 = (ΔX - ΔY)/B电机每毫米步数
        delta_mm = (target_steps[X_AXIS]-pl.position[X_AXIS] - 
                   target_steps[Y_AXIS]+pl.position[Y_AXIS])/settings.steps_per_mm[idx];
      } else {
        // 其他轴移动距离 = 步数差/每毫米步数
        delta_mm = (target_steps[idx] - pl.position[idx])/settings.steps_per_mm[idx];
      }
    #else
      // 普通笛卡尔机构计算
      target_steps[idx] = lround(target[idx]*settings.steps_per_mm[idx]);
      block->steps[idx] = labs(target_steps[idx]-pl.position[idx]);
      block->step_event_count = max(block->step_event_count, block->steps[idx]);
      delta_mm = (target_steps[idx] - pl.position[idx])/settings.steps_per_mm[idx];
    #endif

    // 存储移动距离(单位向量分子部分，分母后续计算)
    unit_vec[idx] = delta_mm; 
    
    // 设置方向位(负方向时置位)
    // direction_bits是一个位域，每个bit代表一个轴方向
    if (delta_mm < 0 ) { 
      block->direction_bits |= get_direction_pin_mask(idx); 
    }
    
    // 累计距离平方(用于后续计算欧几里得距离)
    // 公式：distance² = Σ(Δx_i² + Δy_i² + Δz_i² + ...)
    block->millimeters += delta_mm * delta_mm;
  }

block->steps[idx]： 是当前位置到目标位置之间的步数差。

block->step_event_count：取各轴步数的最大值。

delta_mm： 每个轴方向移动的步数换算成mm。

CoreXY机构使用两个电机(A和B)协同控制XY运动，其运动学方程为：
$$\begin{array}{r}X=(A+B)/2\\ Y=(A-B)/2\end{array}$$

逆运动学为：
$$\begin{array}{r}A=X+Y\\ B=X-Y\end{array}$$

目标步数 = 目标位置(mm) × 每毫米步数，四舍五入取整：

$${\text{target\_steps}}_i=\text{round}(p_{\text{target},i\times s_i})$$

步数差 = |目标步数 - 当前位置步数|：

$$\Delta {\text{steps}}_i=|{\text{target\_steps}}_i-p_{\text{current},i}|$$

移动距离(mm) = 步数差 / 每毫米步数：

$$\Delta {\text{mm}}_i=\frac{\Delta {\text{steps}}_i}{s_i}$$

4 .运动总距离计算

  // 计算实际运动距离 = √(ΣΔx_i²)
  block->millimeters = sqrt(block->millimeters); 
  
  // 检查零长度运动(理论上不应发生)
  if (block->step_event_count == 0) { 
    return; // 如果总步数为0，直接返回
  } 

完成空间距离计算：

$ distance = \sqrt{(\Delta x_i^2+\Delta y_i^2+\Delta z_i^2)} $

检查零长度运动块(步数为0)，如果是则直接返回；

5 .进给速率处理

  if (feed_rate < 0) { 
    // 快速移动模式(G0)，使用最大速率
    feed_rate = SOME_LARGE_VALUE; 
  } else if (invert_feed_rate) { 
    // 反时间模式(G93)，feed_rate实际上是时间的倒数(1/min)
    // 需要转换为正常速率：速率 = 1/时间 × 距离
    feed_rate *= block->millimeters; 
  }
  
  // 确保不低于最小进给速率(防止步进脉冲间隔过大导致问题)
  if (feed_rate < MINIMUM_FEED_RATE) { 
    feed_rate = MINIMUM_FEED_RATE; 
  }

处理不同进给速率模式：

如果feed_rate < 0：设置为一个大值(表示快速移动)；

如果invert_feed_rate为真：使用反时间模式，feed_rate *= 运动距离；

确保进给速率不低于最小值MINIMUM_FEED_RATE，避免步进生成时的舍入问题

6 .路径转角计算

  float inverse_millimeters = 1.0/block->millimeters;  // 距离单位化计算
  float junction_cos_theta = 0;       // 路径转角余弦值
  
  for (idx=0; idx<N_AXIS; idx++) {
    if (unit_vec[idx] != 0) {  // 只处理有运动的轴
      // 计算单位向量：u_i = Δx_i / distance
      unit_vec[idx] *= inverse_millimeters;  
      
      // 限制进给速率不超过各轴最大速率
      // 原理：各轴分速度 = 总速度 × |u_i| ≤ 各轴最大速率
      // 所以：总速度 ≤ 各轴最大速率 / |u_i|
      feed_rate = min(feed_rate, settings.max_rate[idx]/fabs(unit_vec[idx]));
      
      // 限制加速度不超过各轴最大加速度
      block->acceleration = min(block->acceleration, 
                              settings.acceleration[idx]/fabs(unit_vec[idx]));

      // 计算路径转角余弦值(用于后续转角速度计算)
      // cosθ = -Σ(u_prev_i × u_curr_i)
      junction_cos_theta -= pl.previous_unit_vec[idx] * unit_vec[idx];
    }
  }

前一个向量的单位向量分量：

$$\begin{array}{rl}\text{previous\_unit\_vec}[0]& =\frac{x_0}{\sqrt{x_0^2+y_0^2+z_0^2}}\\ \text{previous\_unit\_vec}[1]& =\frac{y_0}{\sqrt{x_0^2+y_0^2+z_0^2}}\\ \text{previous\_unit\_vec}[2]& =\frac{z_0}{\sqrt{x_0^2+y_0^2+z_0^2}}\end{array}$$

当前向量的单位向量分量：

$$\begin{array}{rl}\text{unit\_vec}[0]& =\frac{x_1}{\sqrt{x_1^2+y_1^2+z_1^2}}\\ \text{unit\_vec}[1]& =\frac{y_1}{\sqrt{x_1^2+y_1^2+z_1^2}}\\ \text{unit\_vec}[2]& =\frac{z_1}{\sqrt{x_1^2+y_1^2+z_1^2}}\end{array}$$

向量夹角公式：

$$\cos ⟨\mathbf{a},\mathbf{b}⟩=\frac{\mathbf{a}\cdot \mathbf{b}}{|\mathbf{a}||\mathbf{b}|}$$

几何关系：

$$\begin{array}{rl}{\mathbf{v}}_{i-1}& =(x_0,y_0,z_0)\\ {\mathbf{v}}_i& =(x_1,y_1,z_1)\\ ⟨{\mathbf{v}}_{\mathbf{i}-1},{\mathbf{v}}_{\mathbf{i}}⟩& =180^{\circ }-\theta \end{array}$$

转角计算：

$$\begin{array}{rl}\text{junction\_cos\_theta}& =-\cos ⟨{\mathbf{v}}_{i-1},{\mathbf{v}}_i⟩\\ & =-\frac{x_0{x}_1+y_0{y}_1+z_0{z}_1}{\sqrt{x_0^2+y_0^2+z_0^2}\cdot \sqrt{x_1^2+y_1^2+z_1^2}}\\ & =-(\text{previous\_unit\_vec}\cdot \text{unit\_vec})\end{array}$$

7 .最大转角速度计算

  if (block_buffer_head == block_buffer_tail) {
    // 缓冲区为空时的初始化(从静止开始运动)
    block->entry_speed_sqr = 0.0;          // 入口速度平方为0
    block->max_junction_speed_sqr = 0.0;   // 最大转角速度平方为0
  } else {
    // 检查转角是否接近直线(cosθ≈1)
    if (junction_cos_theta > 0.999999) {
      // 接近直线运动，使用最小转角速度
      block->max_junction_speed_sqr = MINIMUM_JUNCTION_SPEED*MINIMUM_JUNCTION_SPEED;
    } else {
      // 避免数值问题导致除零
      junction_cos_theta = max(junction_cos_theta,-0.999999); 
      
      // 使用半角公式计算sin(θ/2) = √[0.5(1-cosθ)]
      float sin_theta_d2 = sqrt(0.5*(1.0-junction_cos_theta)); 
      
      // 基于向心加速度计算最大转角速度
      // 公式：v_max² = max(v_min², (a×d×sin(θ/2))/(1-sin(θ/2)))
      // 其中：a=加速度，d=转角偏差参数
      block->max_junction_speed_sqr = max(
        MINIMUM_JUNCTION_SPEED*MINIMUM_JUNCTION_SPEED,
        (block->acceleration * settings.junction_deviation * sin_theta_d2)/
        (1.0-sin_theta_d2)
      );
    }
  }

由二倍角公式：

$$\cos \theta =2{\cos }^2\frac{\theta }{2}-1=1-2{\sin }^2\frac{\theta }{2}$$

可得半角正弦公式：

$$\sin \frac{\theta }{2}=\sqrt{\frac{1-\cos \theta }{2}}$$

由几何关系：

$$\sin \frac{\theta }{2}=\frac{r}{r+d}$$

可得半径 r ：

$$r=\frac{d\cdot \sin \frac{\theta }{2}}{1-\sin \frac{\theta }{2}}$$

速度平方计算：

$$v^2=a\cdot r=a\cdot d\cdot \frac{\sin \frac{\theta }{2}}{1-\sin \frac{\theta }{2}}$$

最大连接速度：

$$\text{max\_junction\_speed\_sqr}=\max \left({\text{MINIMUM\_JUNCTION\_SPEED}}^2,v^2\right)$$

8 .运动参数存储

 // 存储标称速度平方(用于后续规划)
  block->nominal_speed_sqr = feed_rate * feed_rate; 
  
  // 计算最大入口速度(取转角速度、当前速度和前一个速度的最小值)
  block->max_entry_speed_sqr = min(
    block->max_junction_speed_sqr, 
    min(block->nominal_speed_sqr, pl.previous_nominal_speed_sqr)
  );
  
  // 更新前一路径信息(供下一条路径使用)
  memcpy(pl.previous_unit_vec, unit_vec, sizeof(unit_vec)); // 保存当前单位向量
  pl.previous_nominal_speed_sqr = block->nominal_speed_sqr; // 保存当前标称速度平方

存储标称速度平方；计算最大入口速度平方：取转角速度、当前标称速度和前一标称速度三者中的最小值

更新前一路径信息；复制当前单位向量到pl.previous_unit_vec；存储当前标称速度平方；

9 .系统状态更新

  // 更新规划器位置(记录目标位置的步数)
  memcpy(pl.position, target_steps, sizeof(target_steps)); 
  
  // 更新缓冲区索引
  block_buffer_head = next_buffer_head;          // 移动头指针到下一个位置
  next_buffer_head = plan_next_block_index(block_buffer_head); // 计算下下个位置
  // 调用规划器重新计算速度曲线等参数
  planner_recalculate();

更新缓冲区索引；block_buffer_head指向下一个缓冲区位置；调用planner_recalculate()重新计算规划；

三.GRBL速度前瞻运动规划

1 .运动规划初始化

  // 从缓冲区最后一个块开始反向计算（注意是环形缓冲区）
  uint8_t block_index = plan_prev_block_index(block_buffer_head);
  
  // 如果只有一个可规划块，直接返回（至少需要2个块才能规划）
  if (block_index == block_buffer_planned) { 
    return; 
  }
      

planner_recalculate()函数主要重新计算缓冲区中所有运动块的速度曲线（加速度/减速度），采用"反向规划-正向规划"双通道计算策略，确保运动平滑。

关键变量：

block_buffer_planned： 指向已规划完成的最后一个块

block_buffer_head： 指向最新添加的块

block_buffer_tail： 指向正在执行的块

2 .反向规划

  /* 从最后一个块开始反向计算最大允许入口速度 */
  
  float entry_speed_sqr;  // 临时存储入口速度平方值
  plan_block_t *next;     // 指向下一个块（反向计算时指向前一个块）
  plan_block_t *current = &block_buffer[block_index]; // 当前处理块

  /* 1 计算最后一个块的最大入口速度（出口速度为0） 
   * 公式：v_entry² = min(v_max_entry², 2*a*s) */
  current->entry_speed_sqr = min(current->max_entry_speed_sqr, 
                               2*current->acceleration*current->millimeters);
  
  /* 2 处理缓冲区中只有两个块的情况 */
  block_index = plan_prev_block_index(block_index);
  if (block_index == block_buffer_planned) { 
    // 如果第一个块是尾块（正在执行的块），更新参数
    if (block_index == block_buffer_tail) { 
      st_update_plan_block_parameters(); 
    }
  } 
  /* 3 处理三个及以上块的情况 */ 
  else { 
    while (block_index != block_buffer_planned) { 
      next = current;
      current = &block_buffer[block_index];
      block_index = plan_prev_block_index(block_index);

      // 如果下一个块是尾块，更新参数
      if (block_index == block_buffer_tail) { 
        st_update_plan_block_parameters(); 
      } 

      /* 4 计算当前块的最大入口速度
       * 公式：v_entry² = v_exit² + 2*a*s */
      if (current->entry_speed_sqr != current->max_entry_speed_sqr) {
        entry_speed_sqr = next->entry_speed_sqr + 2*current->acceleration*current->millimeters;
        
        // 限制不超过最大允许入口速度
        if (entry_speed_sqr < current->max_entry_speed_sqr) {
          current->entry_speed_sqr = entry_speed_sqr;
        } else {
          current->entry_speed_sqr = current->max_entry_speed_sqr;
        }
      }
    }
  }  

假设最后一个块的出口速度 exit_speed初始化为 0（因为运动最终要停止）；已知块加速度 a 和块运动距离 S，往前反推，计算各个块 entry_speed；

计算前一个块的最大允许入口速度公式：

$$v_{entry}^2=min(v_{max\_entry}^2,v_{exit}^2+2\ast a\ast S)$$

反向规划前瞻过程图：

3 .正向规划

  /* 从已规划块开始正向计算，优化加速度曲线 */
  
  next = &block_buffer[block_buffer_planned]; // 从已规划指针开始
  block_index = plan_next_block_index(block_buffer_planned); 
  
  while (block_index != block_buffer_head) {
    current = next;
    next = &block_buffer[block_index];
    
    /* 1 检查当前块是否可以加速 */
    if (current->entry_speed_sqr < next->entry_speed_sqr) {
      // 计算可能的出口速度：v_exit² = v_entry² + 2*a*s
      entry_speed_sqr = current->entry_speed_sqr + 2*current->acceleration*current->millimeters;
      
      /* 2 如果计算出的速度小于下一个块的当前入口速度 */
      if (entry_speed_sqr < next->entry_speed_sqr) {
        next->entry_speed_sqr = entry_speed_sqr; // 更新入口速度
        block_buffer_planned = block_index; // 移动最优规划指针
      }
    }
    
    /* 3 优化策略：当块以最大速度运行时提前终止计算 */
    if (next->entry_speed_sqr == next->max_entry_speed_sqr) { 
      block_buffer_planned = block_index; 
    }
    
    block_index = plan_next_block_index(block_index);
  } 

已知入口速度 entry_speed，检查当前块是否可以加速到下一个块的入口速度；如果可以，已知块加速度 a 和块运动距离 S，往后反推，计算各个块出口速度 exit_speed；如果计算出的出口速度 exit_speed小于下一个块的当前入口速度，则更新速度；

计算当前块的最大允许出口速度公式：

$$v_{exit}^2=v_{entry}^2+2\ast a\ast S$$

正向规划前瞻过程图：

四.反向规划与正向规划分析

1 .运动规划举例

设有 4 个块，初始参数如下表

块号	距离 S（mm）	加速度 a（mm/s²）	速度 v_max_entry（mm/s²）
1	5	200	80
2	10	200	60
3	20	200	100
4	15	200	80

2 .反向规划（Backward Pass）

块 4 计算（最后一块，出口速度必须为0）：

$$v_{4,entry}=min(80,\sqrt{0+2\times 200\times 15})=min(80,77.46)=77.46mm/s$$

块 3 计算（基于块 4 的入口速度）：

$$v_{3,entry}=min(100,\sqrt{77.46^2+2\times 200\times 20})=min(100,118.32)=100mm/s$$

块 2 计算（基于块 3 的入口速度）：

$$v_{2,entry}=min(60,\sqrt{100^2+2\times 200\times 10})=min(60,118.32)=60mm/s$$

块 1 计算（基于块 2 的入口速度）：

$$v_{1,entry}=min(80,\sqrt{60^2+2\times 200\times 5})=min(80,74.83)=74.83mm/s$$

反向规划结果：

块1：74.83 → 60

块2：60 → 100

块3：100 → 77.46

块4：77.46 → 0

3 .正向规划（Forward Pass）

计算块 1 的实际出口速度：

判断$74.83^2<60^2$

条件不成立！不执行任何操作；正向计算不会修改块 2 的入口速度，块 2 的入口速度 60

计算块 2 的实际出口速度：

$$v_{1,exit}=\sqrt{60^2+2\times 200\times 10}=87.2mm/s$$

块 1 的当前出口速度87.2 < 块2的当前入口速度100；需要调整块 3 入口速度从100降为87.2

计算块 3 的实际出口速度：

判断$87.2^2<77.46^2$

条件不成立！不执行任何操作；正向计算不会修改块 4 的入口速度，块 4 的入口速度 77.46

正向规划结果：

块1：74.83 → 60

块2：60 → 87.2

块3：87.2 → 77.46

块4：77.46 → 0

4 .总结

反向规划：确保所有块的减速能力（出口→入口）

正向规划：优化加速能力（入口→出口）

$$\{\begin{array}{ll}{v}_{i,entry}\le \sqrt{v_{i+1,exit}^2+2a_i{S}_i}& \text{(反向保证减速可行)}\\ v_{i,exit}\le \sqrt{v_{i,entry}^2+2a_i{S}_i}& \text{(正向优化加速能力)}\end{array}$$

很多人误以为正向计算会全面重新计算所有速度，实际上它只做单方向的加速优化。

反向规划是"安全基线"，正向规划是"性能优化"。

参考文章

参考优快云文章：开源项目推荐：运动控制速度前瞻算法(Look-Ahead)，连续小线段高速插补算法