用stc15w和旧光驱移植arduino写字机GRBL之七：前瞻算法1

XTXB

stc15w4k32s4和旧光驱移植arduino写字机GRBL连载：
用stc15w4k32s4和旧光驱移植arduino写字机GRBL之一：机架搭建
https://www.amobbs.com/thread-5701202-1-1.html

用stc15w4k32s4和旧光驱移植arduino写字机GRBL之二：PCB制作
https://www.amobbs.com/thread-5701573-1-1.html

用stc15w4k32s4和旧光驱移植arduino写字机GRBL之三：Bresenham算法
https://www.amobbs.com/thread-5701994-1-1.html

用stc15w4k32s4和旧光驱移植arduino写字机GRBL之四：FIFO算法
https://www.amobbs.com/thread-5702730-1-1.html

用stc15w4k32s4和旧光驱移植arduino写字机GRBL之五：G_Code
https://www.amobbs.com/thread-5702784-1-1.html

用stc15w4k32s4和旧光驱移植arduino写字机GRBL之六：运动控制motion_control
https://www.amobbs.com/thread-5703165-1-1.html

用stc15w4k32s4和旧光驱移植arduino写字机GRBL之七：前瞻算法1

进给速度对CNC加工过程中的加工时间，精度，震动有很大影响，因此在高速度高精度加工过程需要优化速度，举个例子，在图①加工路径ABCDEFGH中，咱们凭生产经验先大概分析一下：从A启动，开始加速,一直加速到最高速度，经过节点B，由于是直线，无需减速，到节点C，很明显，由于y轴有换向，必须先减速，过C点后再加速。从C到E的过程，经过D，方向改变不明显，所以经过D的速度改变方式可能跟C有所不同，从E到F的加速过程很短暂，达不到最高速度就面临减速，所以速度曲线呈三角形。从G开始，经历加速，到最大速度，随即减速到停止。

前瞻算法目的就是根据图①的加工路径（微小线段），提前计算，得出加工路径的坐标数据以及图②的时间速度曲线。


在GRBL0.8中，单个线段的运动数据定义在block_t中。线段数据共分个三部分，第一部分用于bresenham 算法，第二部分第三部分用于对速度的控制，
前瞻算法能提前计算6-8个线段的运动数据，（BLOCK_BUFFER_SIZE=18），定时器驱动步进电机的时候，只需简单调用这些运动数据，无需复杂的计算。
typedef struct {
// Fields used by the bresenham algorithm for tracing the line   第一部分，用于bresenham 算法①
uint8_t  direction_bits;            // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h)
                                        //轴运转方向
uint32_t steps_x, steps_y, steps_z; // Step count along each axis
                                        //轴运转步数
int32_t  step_event_count;          // The number of step events required to complete this block
                                        //总步数

// Fields used by the motion planner to manage acceleration   第二部分，用于mm/min速度②
// 这部分速度_speed单位为mm/min
float nominal_speed;               // The nominal speed for this block in mm/min  正常速度,mm/分
float entry_speed;                 // Entry speed at previous-current block junction in mm/min起始速度,mm/分
float max_entry_speed;             // Maximum allowable junction entry speed in mm/min最大转角速度,mm/分
float millimeters;                 // The total travel of this block in mm 总距离，mm
uint8_t recalculate_flag;           // Planner flag to recalculate trapezoids on entry junction  重新计算标记
uint8_t nominal_length_flag;        // Planner flag for nominal speed always reached
                                        //= true表示该线段速度有平台期,就是说可以加速到正常速度

  // Settings for the trapezoid generator    第三部分，用于梯形加速度③
// 这部分速度_rate单位为step_events/minute
uint32_t initial_rate;              // The step rate at start of block 开始速度，步/分
uint32_t final_rate;                // The step rate at end of block结束速度，步/分
int32_t rate_delta;                 // The steps/minute to add or subtract when changing speed (must be positive)
                                //在改变速度时需要要增加或减去的速度，加速度的斜率，步/分钟
uint32_t accelerate_until;          // The index of the step event on which to stop acceleration
                                        //加速段的步数
uint32_t decelerate_after;          // The index of the step event on which to start decelerating
                                        //减速段的步数=停止加速+平台期步数
uint32_t nominal_rate;  // The nominal step rate for this block in step_events/minute
                                //步/分，这个块的标称步进速率

} block_t;
前瞻算法主要执行函数是：
void plan_buffer_line(float x, float y, float z, float feed_rate, uint8_t invert_feed_rate)
各个变量的含义：
xyz：绝对坐标，单位是mm
invert_feed_rate：模式选择,是个逻辑值
        false=gc.motion_mode 速度以mm/minute进给。
        true=inverse_feed_rate_mode，G93，G94专用模式，这个命令比较适合圆弧的加工（以1/minute时间倒数进给）。
feed_rate：速度，在不同模式下速度含义不同，从单位上看：
        在gc.motion_mode下，mm/minute，在gc.feed_rate = settings.default_feed_rate中定义。
        在inverse_feed_rate_mode下，1/minute。

根据以上数据计算block_t各个成员，第一部分数据用于bresenham 算法，比较容易得到，重点探讨第二三部分：

第一部分计算比较简单：
      根据绝对坐标xyz计算各点的相对坐标，然后得出轴运转方向XYZ_DIRECTION_BIT，各轴步数steps_xyz，以及最大轴长step_event_count，供bresenham 算法调用。
第二部分主要计算线段长度及线段交点的速度：
GRBL0.8中有两种速度模式motion_mode和inverse_feed_rate_mode，
在前瞻算法下统一成一种mm/mintue，所以：
gc.motion_mode下：
nominal_speed=feed_rate  单位：mm/min
inverse_feed_rate_mode下，由于feed_rate=1/时间，所以：
nominal_speed=线段长度*feed_rate
有了以上，GRBL中的下面算式就好理解了：        nominal_rate 单位： step/min
nominal_rate = ceil(block->step_event_count * inverse_minute); // (step/min) Always > 0
                    
下面是加速度在最大轴向的分量：
block->rate_delta = ceil( block->step_event_count*inverse_millimeters *settings.acceleration / (60 * ACCELERATION_TICKS_PER_SECOND ));
ACCELERATION_TICKS_PER_SECOND 估计是比例因子，调节加速度的大小，
这个数值直接改变梯形加速度的斜率，默认单位是步/分，/60就是步/秒。

之前谈到由于两线段夹角不同，交点的速度亦不同，在GRBL中规定：
1，线段夹角小于18.195°（cos18.195°=0.95），以默认值通过。
        block->max_entry_speed = vmax_junction。
2，线段夹角为钝角大于161.805°（180-18.195°）时，两线段当直线通过。
3，夹角在18.195°到161.805°之间时，两线段交点速度算法如下：
为了求两线段BCD交点的速度，GRBL抽象出一个与两线段相切的圆，
如图③，定义了CS及圆的向心加速度a，以求圆弧运动速度代替交点速度。
(CS=h=settings.junction_deviation = 0.05mm)
(a=settings.acceleration=10 mm/min^2)       

根据两向量BC（x1,y1）,|BC|=S1,DC(x2,y2),|DC|=S2夹角余弦公式：
Cos∠BCD =Cosθ =(x1*x2+y1*y2+z1*z2)/(s1*s2)
对应在GRBL中：
cos_theta=-pl.previous_unit_vec[X_AXIS]*unit_vec[X_AXIS]
                   -pl.previous_unit_vec[Y_AXIS]*unit_vec[Y_AXIS]
                   - pl.previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ;
由向心加速度公式：a=v²*r，v=sqrt(a/r)
在直角△CPO，sin(θ/2)=OP/CO=r/(r+h)
        则r=h*sin(θ/2)/(1-sin(θ/2))
所以：v=sqrt(a/r)=sqrt(a/h*sin(θ/2)/(1-sin(θ/2)) )
在GRBL中是这样表达的：V=sqrt(settings.acceleration * settings.junction_deviation * sin_theta_d2/(1.0-sin_theta_d2))
所以两线段交点的最大速度为： block->max_entry_speed =V

由于线段长度不同，运动时加减速的距离时间亦不同，在图②的时间速度曲线中，有下面几种情形：
1，有加速，最高速度，没有减速，如A-B
2，有最高速，减速，没有加速，如B-C
3，有完整的加速减速最高速，如C-D
4，只有加速，如F-G
5，只有减速，如D-E
6，有加速，减速，没有最高速，如E-F
看起来是不是有点头大，前瞻算法第三部分就是把这些情形一一落实。

第三部分就是根据前面得出的交点速度，线段长度，计算节点速度（initial_rate;final_rate）及速度节点位置（accelerate_until;decelerate_after）
很容易发现BDG点的速度计算有些特别，图①中，根据经验，E点的速度会影响前一点D的速度，但在规划BC,CD段时由于无法知晓E的存在，D点的速度照直线规划应该为保持最大速度，但后面E点太近，就来不及减速了，所以在plan_buffer_line( )最后调用函数planner_recalculate( )进行向前和向后的复核，这是前瞻算法的核心所在，函数是：
static void planner_recalculate()
{     
  planner_reverse_pass();
  planner_forward_pass();
  planner_recalculate_trapezoids();//计算梯形加速曲线
}
这部分逻辑关系比较复杂，下一节再探讨。arduino GRBL0.8的源程序：

前瞻算法的学习感谢网友xufeixueren的无私奉献：https://blog.csdn.net/xufeixueren/article/details/79663068

hkjabcd

也谢谢楼主的无私奉献！！MARK!

lcmdw

学习，谢谢楼主

huangguimina4

楼主厉害了

LinuxTux.China

感谢，学习了！！

tear604922959

这都可以的啊

flash3g

LZ研究的很透切啊

jack_yu

牛！！谢谢楼主！

NM2012

谢谢分享

王涛

楼主你好
有一个问题要问楼主C F点对应轴步进电机要换向，速度应该先减到0后加速的。是不是图画错了还是我理解有误？如果这是合成的进给速度就是对的。如果是单轴的话就不对。

所谓的前瞻算法就是走一步看n步计算过弯时的速度，实际到每个轴就是计算我在第几步后就要开始减速了这样才能平稳的转弯，电机平稳过度减少冲击和失步。用示波器观察波形时，加速的时候步进电机脉冲频率变密减速时正好相反。与此同时加速的时单片机中断频率快减速时正好相反。
这里有一个矛盾就是速度快了中断频繁主循环来不及处理前瞻算法。现象就是电机会抖动。所以GRBL1.1版本有部分处理放到了中断中来计算。
现在成熟的嵌入式运动控制器都是ARM+FPGA架构或是ARM+ASIC架构,计算在ARM中完成脉冲输出在FPGA中实现硬件插补。速度可以做很高。

XTXB

王涛 发表于 2018-12-21 10:34
楼主你好
有一个问题要问楼主C F点对应轴步进电机要换向，速度应该先减到0后加速的。是不是图画错了还是我 ...

感谢大侠的指教！这个速度时间曲线只是个大概，就是个人理解，没有经过实际论证，不一定正确。菜鸟一个，纯粹对grbl感兴趣，就扒一扒源码，学习学习，也不是为了做啥产品。目前正在看加速度和定时器的关联这块代码。

armok.

帖子移动通知：
原分论坛：8051/STC32【已下线】
目标分论坛：51单片机
移动时间：0小时之后