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