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

grbl源码解析——速度前瞻（1）相关推荐

grbl源码解析——速度前瞻（2）
本文参考了两篇论文<基于运动控制卡的PC数控进给速度前瞻控制_刘青山>.<嵌入式数控系统速度前瞻规划算法研究_游达章> planner_recalculate if (!(bl ...
grbl源码解析——圆弧插补
uint8_tgc_execute_line(char *line)函数,对G02,G03代码进行解析,目的是求出圆心坐标. 接着调用mc_arc函数进行后续数据处理. // position == ...
Robot Arm 机械臂源码解析
Robot Arm 机械臂源码解析说明: Robot Arm是我复刻,也是玩的第一款机械臂.用的是三自由度的结构,你可以理解为了三个电机,三轴有自己的一些缺陷.相比于六轴机械臂而言因为结构的缺陷 ...
VVeboTableView 源码解析
原文链接:http://www.jianshu.com/p/78027a3a2c41 最近在看一些 iOS 性能优化的文章,我找到了 VVeboTableView 这个框架.严格来说这个不属于框架,而 ...
【特征匹配】ORB原理与源码解析
相关 : Fast原理与源码解析 Brief描述子原理与源码解析 Harris原理与源码解析 http://blog.csdn.net/luoshixian099/article/details/48 ...
EventBus3.0源码解析
本文主要介绍EventBus3.0的源码 EventBus是一个Android事件发布/订阅框架,通过解耦发布者和订阅者简化 Android 事件传递. EventBus使用简单,并将事件发布和 ...
Guava RateLimiter限流源码解析和实例应用
2019独角兽企业重金招聘Python工程师标准>>> 在开发高并发系统时有三把利器用来保护系统:缓存.降级和限流缓存缓存的目的是提升系统访问速度和增大系统处理容量降级降级是 ...
The Wide and Deep Learning Model（译文+Tensorlfow源码解析）原创 2017年11月03日 22:14:47 标签：深度学习 / 谷歌 / tensorf
The Wide and Deep Learning Model(译文+Tensorlfow源码解析) 原创 2017年11月03日 22:14:47 标签: 深度学习 / 谷歌 / tensorfl ...
TreeSet源码解析
TreeSet概述所有实现的接口: Serializable, Cloneable, Iterable<E>, Collection<E>, NavigableSet< ...

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

plan_buffer_line

junction_cos_theta求解

max_junction_speed_sqr求解

grbl源码解析——速度前瞻（1）相关推荐

最新文章

热门文章