grbl源码解析——速度前瞻（1）

本文参考了两篇论文《基于运动控制卡的PC数控进给速度前瞻控制_刘青山》、《嵌入式数控系统速度前瞻规划算法研究_游达章》

plan_buffer_line

速度前瞻位于直线、圆弧插补之后，首先是通过plan_buffer_line(float *target, plan_line_data_t *pl_data)函数进行拐点处的速度计算。

uint8_t plan_buffer_line(float *target, plan_line_data_t *pl_data)
{
  // Prepare and initialize new block. Copy relevant pl_data for block execution.
  plan_block_t *block = &block_buffer[block_buffer_head];

block_buffer保存了线段前瞻数据，通过block指针进行数据操作。

for (idx=0; idx<N_AXIS; idx++) {
    // Calculate target position in absolute steps, number of steps for each axis, and determine max step events.
    // Also, compute individual axes distance for move and prep unit vector calculations.
    // NOTE: Computes true distance from converted step values.
    #ifdef COREXY
      if ( !(idx == A_MOTOR) && !(idx == B_MOTOR) ) {
        target_steps[idx] = lround(target[idx]*settings.steps_per_mm[idx]);
        block->steps[idx] = labs(target_steps[idx]-position_steps[idx]);
      }
      block->step_event_count = max(block->step_event_count, block->steps[idx]);
      if (idx == A_MOTOR) {
        delta_mm = (target_steps[X_AXIS]-position_steps[X_AXIS] + target_steps[Y_AXIS]-position_steps[Y_AXIS])/settings.steps_per_mm[idx];
      } else if (idx == B_MOTOR) {
        delta_mm = (target_steps[X_AXIS]-position_steps[X_AXIS] - target_steps[Y_AXIS]+position_steps[Y_AXIS])/settings.steps_per_mm[idx];
      } else {
        delta_mm = (target_steps[idx] - position_steps[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]-position_steps[idx]);
      block->step_event_count = max(block->step_event_count, block->steps[idx]);
      delta_mm = (target_steps[idx] - position_steps[idx])/settings.steps_per_mm[idx];
	  #endif
    unit_vec[idx] = delta_mm; // Store unit vector numerator

    // Set direction bits. Bit enabled always means direction is negative.
    if (delta_mm < 0.0 ) { block->direction_bits |= get_direction_pin_mask(idx); }
  }

block->steps[idx]是当前位置到目标位置之间的步数，block->step_event_count是所有轴方向中的最大步数。delta_mm将每个轴方向移动的步数换算成mm。

block->millimeters = convert_delta_vector_to_unit_vector(unit_vec);

float convert_delta_vector_to_unit_vector(float *vector)
{
  uint8_t idx;
  float magnitude = 0.0;
  for (idx=0; idx<N_AXIS; idx++) {
    if (vector[idx] != 0.0) {
      magnitude += vector[idx]*vector[idx];
    }
  }
  magnitude = sqrt(magnitude);
  float inv_magnitude = 1.0/magnitude;
  for (idx=0; idx<N_AXIS; idx++) { vector[idx] *= inv_magnitude; }
  return(magnitude);
}

假设机床有1、2、3（xyz）三个轴。由于计算采用的是相对位置，可以认为当前点的坐标为{0，0，0}，下一个点的坐标unit_vec[3]={x0,y0,z0}。
unit_vec[idx] = delta_mm通过convert_delta_vector_to_unit_vector函数，将会把unit_vec[idx]转换成单位向量。
block->millimeters为空间位移的长度。

block->acceleration = limit_value_by_axis_maximum(settings.acceleration, unit_vec);

#define SOME_LARGE_VALUE 1.0E+38
#define DEFAULT_X_ACCELERATION (10.0*60*60) // 10*60*60 mm/min^2 = 10 mm/sec^2
#define DEFAULT_Y_ACCELERATION (10.0*60*60) // 10*60*60 mm/min^2 = 10 mm/sec^2
#define DEFAULT_Z_ACCELERATION (10.0*60*60) // 10*60*60 mm/min^2 = 10 mm/sec^2
settings.acceleration[X_AXIS] = DEFAULT_X_ACCELERATION;
settings.acceleration[Y_AXIS] = DEFAULT_Y_ACCELERATION;
settings.acceleration[Z_AXIS] = DEFAULT_Z_ACCELERATION;

float limit_value_by_axis_maximum(float *max_value, float *unit_vec)
{
  uint8_t idx;
  float limit_value = SOME_LARGE_VALUE;
  for (idx=0; idx<N_AXIS; idx++) {
    if (unit_vec[idx] != 0) {  // Avoid divide by zero.
      limit_value = min(limit_value,fabs(max_value[idx]/unit_vec[idx]));
    }
  }
  return(limit_value);
}

max_value[idx]为每个轴的加速度，unit_vec[idx]为单位向量。

block->acceleration = Acc为空间向量的加速度（未知量），acc[i]为0、1、2…轴的加速度（已知量）。limit_value_by_axis_maximum函数求得空间向量的加速度。

block->rapid_rate = limit_value_by_axis_maximum(settings.max_rate, unit_vec);

#define DEFAULT_X_MAX_RATE 500.0 // mm/min
#define DEFAULT_Y_MAX_RATE 500.0 // mm/min
#define DEFAULT_Z_MAX_RATE 500.0 // mm/min
settings.max_rate[X_AXIS] = DEFAULT_X_MAX_RATE;
settings.max_rate[Y_AXIS] = DEFAULT_Y_MAX_RATE;
settings.max_rate[Z_AXIS] = DEFAULT_Z_MAX_RATE;

float limit_value_by_axis_maximum(float *max_value, float *unit_vec)
{
  uint8_t idx;
  float limit_value = SOME_LARGE_VALUE;
  for (idx=0; idx<N_AXIS; idx++) {
    if (unit_vec[idx] != 0) {  // Avoid divide by zero.
      limit_value = min(limit_value,fabs(max_value[idx]/unit_vec[idx]));
    }
  }
  return(limit_value);
}

block->rapid_rate（空间向量的速度）的求法与block->acceleration的求法相同，参考上一步。

junction_cos_theta求解

接下来讲解拐点速度的求解，首先看一下这张图。如果没有速度前瞻的话，每段线执行完，速度会变为0.前瞻的作用就是计算出每个block块的junction处的速度。

float junction_unit_vec[N_AXIS];
float junction_cos_theta = 0.0;
for (idx=0; idx<N_AXIS; idx++) {
    junction_cos_theta -= pl.previous_unit_vec[idx]*unit_vec[idx];
    junction_unit_vec[idx] = unit_vec[idx]-pl.previous_unit_vec[idx];
}

max_junction_speed_sqr求解

#define MINIMUM_JUNCTION_SPEED 0.0 // (mm/min)    
if (junction_cos_theta > 0.999999) {
      //  For a 0 degree acute junction, just set minimum junction speed.
      block->max_junction_speed_sqr = MINIMUM_JUNCTION_SPEED*MINIMUM_JUNCTION_SPEED;
    } else {
      if (junction_cos_theta < -0.999999) {
        // Junction is a straight line or 180 degrees. Junction speed is infinite.
        block->max_junction_speed_sqr = SOME_LARGE_VALUE;
      } else {
        convert_delta_vector_to_unit_vector(junction_unit_vec);//将junction_unit_vec[3]转换为单位向量
        float junction_acceleration = limit_value_by_axis_maximum(settings.acceleration, junction_unit_vec);//求解方法参考block->acceleration的求解过程
        float sin_theta_d2 = sqrt(0.5*(1.0-junction_cos_theta)); // Trig half angle identity. Always positive.
        block->max_junction_speed_sqr = max( MINIMUM_JUNCTION_SPEED*MINIMUM_JUNCTION_SPEED,
                       (junction_acceleration * settings.junction_deviation * sin_theta_d2)/(1.0-sin_theta_d2) );
      }
    }

构造与Ventry、Vexit相切的圆，v为圆的线速度，根据圆周运动公式v2=ar，可求得max_junction_speed_sqr。grbl用这种构造内切圆的巧妙方法，求解了拐点速度。
junction_cos_theta > 0.999999表示Ventry、Vexit间的夹角为0°。
偶然机会，我在论文中查到了与grbl中“拐角速度求解方法”对应的论文——《基于运动控制卡的PC数控进给速度前瞻控制_刘青山》、《嵌入式数控系统速度前瞻规划算法研究_游达章》