开源项目推荐：运动控制速度前瞻算法

一、什么是速度前瞻

Look-Ahead 技术又称为速度前瞻控制技术，目前实现此技术有两个基本思路：
1、进行路径段之间速度衔接；
2、进行大量微小线段参数曲线拟合。

Look-Ahead 技术考虑的主要因素是机床本身动力学特性限制和加工精度限制。机器人运动中为保证轨迹移动精度，插补算法要将运动轨迹离散成大量首尾衔接的微小线段。

一般，微小线段间的速度衔接分为以下几种：

1、微小线段间无衔接，减速到零，下段再重新启动。这种速度衔接方式，加减速就会频繁变化，大大降低了插补效率，同时会产生很大震动，形成很大的轨迹误差；

2、微小线段间不减速。这种方式插补效率很高，但运动路径遇到急转弯，很难保证插补轨迹精度；

3、微小线段间相切或方向角变化小于一定值按照不减速处理或者运动路径遇到急转弯设定最高限速。

这种方式可以通过预读加工路径进行速度规划（速度前瞻），在满足插补轨迹精度的同时保证插补效率。前瞻控制能够提前对运动轨迹进行分析和处理，发现高曲率点和尖锐拐角，然后对路径上的速度进行规划，找出减速点，保证加工精度的同时满足机器人的加减速特性，在保证速度最大化的同时实现速度的平滑过渡。

完成速度前瞻的关键就在于推导出相邻插补段的衔接速度。

如图，在加工轨迹上预先依次取 a、b、c、d、e、f 6个插补点，构成ab、bc、cd、de、ef 五段插补线段。计算相邻插补段之间的夹角，通过对比从中发现高曲率点。曲率越大，则线段间衔接时的允许速度越小。由图可以看出c、e为分界点，把ab、bc两段作为一个整体进行插补计算，把cd、de两段作为一个整体进行插补计算，这样根据衔接线段的曲率将整个插补轨迹上衔接速度变化不大的线段作为一个插补的整体，减少了加减速的变化，提高了加工效率。提前对运动轨迹进行分析和处理找到减速点c和e，先根据速度约束条件确定减速点两侧插补段衔接时的最大速度，保证插补精度的同时实现平滑过渡。

二、开源项目

1、LinuxCNC

LinuxCNC支持最多9轴运动控制，内置具有前瞻性的实时轨迹规划器，可实现轨迹误差限制、轴同步运动控制、自适应进给率控制等功能；

https://github.com/LinuxCNC/linuxcnc

2、grbl

用优化的 C 语言编写的开源、嵌入式、高性能 g 代码解析器和 CNC 铣削控制器，可在直接 Arduino 上运行

https://github.com/grbl/grbl

grbl圆弧插补算法和直线插补算法以及前瞻算法的学习笔记

https://blog.csdn.net/Archar_Saber/article/details/80475733

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

 https://blog.csdn.net/cln512/article/details/120748978

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

https://blog.csdn.net/cln512/article/details/120749226

3、Marlin

Marlin 是基于 Arduino 平台的 RepRap 3D 打印机的优化固件。Marlin1.0工程代码用C语言写的，阅读起来比较容易。Marlin1.0主要核心算法包括圆弧插补、速度前瞻、转角速度圆滑、梯形速度规划、Bresenham多轴插补。Marlin2.0工程相对于Marlin1.0工程程序用了更多C++的写法，2.0比1.0主要是增加了S形速度规划。

https://github.com/MarlinFirmware/Marlin

G代码解析都在queue.advance()函数中。G0、G1是直线插补命令，G3、G4是圆弧插补命令。

https://github.com/MarlinFirmware/Marlin/blob/2.0.x/Marlin/src/gcode/queue.cpp

源码路径中motion文件夹中G0_G1.cpp的G0_G1()就是解析G0、G1直线插补命令，G2_G3.cpp的G2_G3()就是解析圆弧插补命令。

https://github.com/MarlinFirmware/Marlin/tree/2.0.x/Marlin/src/gcode/motion

直线规划的实现函数在planner.cpp的Planner::buffer_line函数，

https://github.com/MarlinFirmware/Marlin/blob/2.0.x/Marlin/src/module/planner.cpp

buffer_line函数又调用buffer_segment函数，_buffer_steps首先调用_populate_block()函数生成新的规划block并进行填充，填充时调用了转角平滑算法来计算初速度，然后再调用recalculate()函数来执行速度前瞻算法和梯形轨迹规划算法。

贝塞尔拟合

https://github.com/MarlinFirmware/Marlin/blob/2.0.x/Marlin/src/module/planner_bezier.cpp

4、MRPT移动机器人软件库

移动机器人编程工具包(MRPT)。移动机器人编程工具包(MRPT)提供了面向移动机器人和计算机视觉研究人员的c++库。库包括SLAM解决方案、3D(6D)几何、SE(2)/SE(3)李群、点、地标、姿态和地图上的概率密度函数(pdf)、贝叶斯推理(卡尔曼滤波、粒子滤波)、图像处理、避障等。MRPT还提供了用于立体相机校准、数据集检查等的GUI应用程序。 

四元数插补算法：slerp

https://github.com/MRPT/mrpt

三、参考文献

1、连续微小线段速度前瞻规划算法

https://blog.csdn.net/Galaxy_Robot/article/details/109377361

开源cnc项目Marlin2.0运动控制部分代码理解 

https://blog.csdn.net/liuzhijun301/article/details/104477187

2、论文

王宇晗、肖凌剑等，“小线段高速加工速度衔接数学模型研究”，交通大学学报，2004.10，vol.37，EIP04378355344 

高速加工中速度前瞻控制新算法研究 任 锟 ,傅建中 ,陈子辰 (浙江大学 机械工程学系 ,浙江 杭州 3 10027