插补算法这类东西平时藏在运动控制卡和伺服驱动器里很多人只管调用接口很少去想底层到底怎么走的一步。可真当你碰到一台设备需要自己写运动逻辑或者改造一台老机床、做实验平台又不想被厂商封闭的指令集绑死就会发现“能自己掌控的插补源码”是根救命稻草。这套用逐点比较法实现的直线圆弧插补算法支持XY、XZ、YZ三个平面切换同时内置加减速控制从设计上就没打算绑定具体硬件——通用PLC、单片机、嵌入式控制器只要有定时中断和两个方向可变的脉冲输出基本都能套上去。如果你也在做非标设备、小型数控或步进电机运动平台这篇文章值得看完。逐点比较法听起来有年头但它仍然是小成本运动控制方案里最稳的一类。它最大的特点就是“每走一步都算账”进给一个脉冲就判断一次当前位置和目标轨迹的偏差然后根据偏差决定下一步往哪个方向进给。这样一个个脉冲处理下来轨迹误差可以被控制在最小单位内。更关键的是这个方法只用到加减乘除和比较判断没有浮点运算依赖放到PLC里用整数算也毫无压力。下面我把原理、源码、参数计算和排错经验一次性拆开。1. 项目概述一份不依赖硬件平台的插补算法意味着什么1.1 这套源码到底能做什么标题里几个关键词拆开看其实非常直白逐点比较法核心插补算法直线和圆弧都靠它做偏差判别。直线圆弧插补支持两种最常见轨迹既可以走斜线也可以走圆。XY/XZ/YZ方向插补同一份算法可以映射到三个坐标平面。加减速控制不能一上来就最高速度发脉冲否则步进电机必丢步。适用于任何可编程控制器不依赖某个品牌PLC的特殊定位指令也不依赖专用运动控制芯片。我自己的用法是把这份算法写成一个函数块放在一个定时中断或周期任务里。每次进入任务就调用一次插补函数函数会告诉你这一步该往哪个物理轴发脉冲、方向是正还是负。外部只需要把返回的方向信号映射到轴I/O上即可。整个过程不需要浮点运算不需要数组之外的复杂数据结构资源占用极小。1.2 为什么不用现成的运动控制卡或伺服插补功能有人可能会问现在伺服驱动器都内置插补了为什么还要自己写我做项目时的体会是很多驱动器内置的插补只支持点位运动真正要三轴联动、自定义轨迹得买高配版本而且调试软件不透明出了轨迹问题只能摸黑排查。另外在非标设备里经常遇到“只需要两个轴走一个斜线、走一个圆不想为此引入整套运动控制器”的场景这时候一段纯软件插补代码就是最直接的工具。相比DDA插补法逐点比较法的优点是误差有界且不会随插补时间累计漂移。DDA走长的线段容易因为累加器位数不够出现速度波动逐点比较法则每一个进给周期都在用偏差拉回轨迹。缺点是速度控制没DDA那么灵活所以要额外做加减速。这也是标题里特意点名“带加减速”的原因。2. 核心算法原理解读偏差判别、坐标进给、新偏差计算、终点比较2.1 直线插补的四步运算以XY平面第一象限直线为例设终点为(xe, ye)当前动点坐标为(x, y)定义偏差F x * ye - y * xe这个F的几何意义是当前点相对理想直线的“带符号距离”。F等于0说明在线上大于0说明在线的上方小于0说明在下方。逐点比较法每步只做四件事偏差判别看F的符号。坐标进给F0时向X进给一步F0时向Y进给一步。新偏差计算用递推公式更新F避免每次都重新做乘除法。终点比较计数器减一判断插补是否完成。关键在第三步直接用加减法递推若向X进给一步F_new F_old - ye若向Y进给一步F_new F_old xe为什么能这么算因为x每加1F就增加1ye要从F里减掉ye让新位置重新对准偏差y每加1F就增加1xe所以给F补上xe。这个递推只用加减法在PLC的整数运算里效率非常高。多象限直线插补其实也好处理把xe、ye取绝对值偏差计算用绝对值进给方向由终点坐标的符号决定。比如终点在第二象限X仍然要往负方向走那就把方向变量置为-X但偏差递推公式不变。下文源码里会给这个通用做法。2.2 圆弧插补的偏差递推圆弧插补同样用偏差法只是偏差公式变成F x^2 y^2 - R^2也就是当前点到圆心的距离平方和半径平方的差。F0表示当前点在圆外或圆上F0表示在圆内。拿第一象限逆圆举例进给规则如下F0时向-X方向进给一步并更新F_new F_old - 2*x 1然后x减1F0时向Y方向进给一步并更新F_new F_old 2*y 1然后y加1这里的2*x1来自(x-1)^2展开x^2变成(x-1)^2后偏差减少2x-1。同样的道理y加1后偏差增加2y1。代码里我会用先更新坐标、再修正偏差的写法注意代数符号不能搞错这是圆弧插补最容易写错的地方。顺圆规则刚好反过来F0向-Y方向走F0向X方向走。实际工程中还有跨象限问题。我的处理策略是把任意起点、终点角度的圆弧按象限拆分或者通过轴对称镜像把其它象限映射到第一象限计算。源码里先给第一象限实现扩展时只需要做一张方向映射表把计算出的“虚拟方向”变成实际轴方向。2.3 三个平面一份代码虚拟轴U/V映射XY、XZ、YZ三个平面看起来要写三套算法其实完全没必要。思路是这样的插补算法只认识两个虚拟坐标轴叫它U轴和V轴运算逻辑一模一样。至于U轴到底对应物理X还是Y还是Z由平面选择参数决定。我用一个两列映射表搞定平面U轴虚拟第一轴V轴虚拟第二轴XYXYXZXZYZYZ代码里就是查这个表。插补算出来“U正向走一步”就查表找到实际物理轴然后往这个轴的方向引脚输出一个脉冲。这也是标题里“XY方向插补、XZ方向插补、YZ方向插补”能被一份源码统一的原因。2.4 梯形加减速的档位表和减速点判断逐点比较法每个插补周期走一步所以速度本质上由脉冲频率决定。如果一启动就按最高频率发脉冲步进电机会直接丢步或者机械机构会有很大冲击。梯形加减速是最简单也最实用的处理方式加速阶段频率线性上升匀速阶段保持最高频率减速阶段频率线性下降直到停止。实现上我不喜欢每次实时计算速度而是预生成一张频率档位表。比如从启动频率400Hz到最高频率2000Hz切成128档每档上升约12.5Hz。插补函数每走一步根据加减速状态决定当前用哪一档频率。把频率转为定时器装载值就是一个查表操作效率很高。减速点判断是梯形加减速的难点。不能等到只剩很少步数才开始减速否则来不及降下来。我的算法是在每次进给后检查剩余步数如果剩余步数小于或等于“从当前速度减速到启动频率所需要的步数”就切换到减速状态。这个减速步数可以在加速阶段同步估算出来因为加速和减速的斜率一样加速用了多少步减速至少也需要近似一样的步数。3. 实用源码实现加减速、三平面、直线圆弧一把梭3.1 数据结构定义先定义一个插补控制块所有状态都塞在里面方便移植typedef struct { int plane; /* 0XY, 1XZ, 2YZ */ int mode; /* 0直线, 1逆圆, 2顺圆 */ long xe, ye; /* 终点坐标圆弧模式下是相对圆心的终点坐标 */ long cx, cy; /* 圆弧圆心坐标 */ long px, py; /* 当前点坐标圆弧模式下是相对圆心的坐标 */ long F; /* 偏差值 */ long remain; /* 剩余步数仅直线用 */ int dir_u; /* U轴进给方向1正向 -1反向 0不走 */ int dir_v; /* V轴进给方向1正向 -1反向 0不走 */ } interp_t;为什么圆弧注意相对圆心因为偏差公式Fx^2y^2-R^2是相对圆心的把起点、终点、圆心做个坐标平移后续递推就统一了。这样圆弧函数不需要关心圆心在机床坐标系的哪个位置。加减速控制单独用一个状态块typedef struct { int state; /* 0停止 1加速 2匀速 3减速 */ int idx; /* 当前速度档位 */ long remain; /* 剩余步数 */ long dec_need; /* 从当前档降回启动档所需步数 */ } speed_t;3.2 直线插补核心函数下面是直线插补的完整函数支持四个象限void interp_line(interp_t *p) { long ax labs(p-xe); long ay labs(p-ye); if (p-F 0) { /* 沿U轴进给 */ if (p-xe 0) { p-px; p-dir_u 1; } else { p-px--; p-dir_u -1; } p-dir_v 0; p-F - ay; } else { /* 沿V轴进给 */ if (p-ye 0) { p-py; p-dir_v 1; } else { p-py--; p-dir_v -1; } p-dir_u 0; p-F ax; } p-remain--; }注意F的初始值是0remain的初始值是 |xe-xs| |ye-ys|。因为是逐点进给总步数就是X方向步数和Y方向步数的和。进给方向输出后外部调用代码要做的是把dir_u映射到plane对应的物理轴把dir_v映射到另一个物理轴然后触发脉冲输出。如果dir_u和dir_v都是0这一步就不发脉冲。3.3 圆弧插补核心函数圆弧函数我按逆圆、顺圆分别写递推公式用前面推导过的版本void interp_arc(interp_t *p) { if (p-mode 1) { /* 逆圆 */ if (p-F 0) { p-px--; p-F - 2 * (p-px 1) - 1; /* 等效于减掉2*px_old1 */ p-dir_u -1; p-dir_v 0; } else { p-py; p-F 2 * p-py - 1; /* 等效于加上2*py_old1 */ p-dir_u 0; p-dir_v 1; } } else { /* 顺圆 */ if (p-F 0) { p-py--; p-F - 2 * (p-py 1) - 1; p-dir_u 0; p-dir_v -1; } else { p-px; p-F 2 * p-px - 1; p-dir_u 1; p-dir_v 0; } } }圆弧的终点判别直接比较当前坐标和终点坐标都相等就停止。这里不用计数器是防止跨象限圆弧时|Δx||Δy|估步数出问题。我踩过一个坑递推公式写成“先改坐标再用更新后的坐标计算偏差”导致偏差越算越偏圆画成了螺线。上面代码先改坐标再用改动后的坐标值变形成正确公式这是验证过的照着抄不会错。3.4 加减速状态机实现加减速状态机核心代码如下void speed_update(speed_t *s) { switch (s-state) { case 1: /* 加速 */ if (s-idx 127) s-idx; if (s-remain s-dec_need) { s-state 3; if (s-idx 0) s-idx--; } else if (s-idx 127) { s-state 2; } break; case 2: /* 匀速 */ if (s-remain s-dec_need) { s-state 3; if (s-idx 0) s-idx--; } break; case 3: /* 减速 */ if (s-idx 0) s-idx--; else s-state 0; break; } }档位idx对应频率值我从一个预生成的speed_table里取。加减速对称所以dec_need在加速开始时基本固定加速阶段越早进入匀速减速需要的时间越长。我按“最高档对应的减速距离”来估算总步数减去当前已走步数一旦剩余步数小于最高档减速距离就强制减速。这是工程里常用的保守策略宁可用不满最高速度也不冒丢步风险。3.5 结构化文本ST版本的关键差异如果是写到PLC里不需要C语言那么底层的指针和结构体。直接用IEC 61131-3结构化文本思路是一样的区别在于用全局变量或函数块内部变量保存状态。以某品牌的普通PLC为例你只需要一个周期任务周期5ms或10ms每次执行一次插补更新。要注意的是PLC的周期任务必须优先于运动控制程序否则被其它程序卡住会导致脉冲间隔抖动。ST语言里没有labs这种函数自己写一个绝对值函数或者用条件选择即可。偏差变量用DINT类型因为坐标脉冲数动辄几千几万INT会溢出。圆弧偏差Fx^2y^2-R^2中平方后可能是百万级别的值DINT在32位下有正负21亿的范围工程上足够用了。4. 参数计算与移植要点从脉冲当量到速度档位4.1 脉冲当量与目标坐标换算写插补代码之前第一件事是把运动平台的机械参数换成脉冲数。假设一个常用测试台参数步进电机步距角1.8度200步/圈驱动器细分8丝杆导程5mm那么电机转一圈需要200*81600个脉冲丝杆移动5mm所以脉冲当量5/16000.003125mm。也就是说一个脉冲对应3.125微米。目标移动10mm对应的脉冲数N10/0.0031253200个脉冲。这个换算关系要提前做好插补算法只认脉冲数不认毫米。如果目标坐标用毫米就先把毫米值乘上脉冲当量的倒数转换成整数脉冲数再交给插补函数。换算出错轨迹等于白算。4.2 梯形加减速的具体计算假设启动频率400Hz最高频率2000Hz加速时间0.2秒。那么加速度a(2000-400)/0.28000Hz/s。加速阶段发出的脉冲数约等于平均频率乘时间(4002000)/2*0.2240个脉冲。减速阶段同样要240个脉冲。如果总脉冲数3200匀速阶段的脉冲数是3200-240-2402720个匀速时间2720/20001.36秒总运行时间约1.76秒。这个数字可以在界面参数里给操作员预估工时也可以用来验证实际运行时间是否吻合。实际加档时我建议把128档的速度表转换成“定时器周期”而不是频率因为很多MCU的定时器是递减计数装载值越大脉冲间隔越长。公式是timer_load timer_clock / freq - 1。比如定时器时钟1MHz2000Hz对应499400Hz对应2499。4.3 三平面切换的映射接口平面映射要单独封装一个函数不让插补核心直接操作物理轴。我在代码里这样处理int map_plane_axis(int plane, int uv) { if (plane 0) return uv 0 ? 0 : 1; /* XY: U-X V-Y */ if (plane 1) return uv 0 ? 0 : 2; /* XZ: U-X V-Z */ return uv 0 ? 1 : 2; /* YZ: U-Y V-Z */ }物理轴编号自己约定0X1Y2Z。输出脉冲前调用map_plane_axis得到物理轴编号再查轴方向寄存器和脉冲输出寄存器。这里有个细节值得注意切换平面时要先把当前所有轴的坐标状态保存好再初始化新的插补控制块。如果程序分别维护三套插补状态XYZ轴来回切换就会更灵活但代码量也翻倍。我的建议是先做单平面切换实际用到多平面联动时再扩展成三套状态块。4.4 软硬件接口定时中断与脉冲IO算法不挑硬件但硬件要满足一个条件能稳定产生脉冲。最理想的实现是硬件定时器中断里发脉冲。以单片机为例定时器每到达装载值就产生一次中断中断服务程序里判断该往哪个轴输出电平翻转。PLC的话要选带高速脉冲输出口的型号频率上限至少在50kHz以上否则加减速上限只有几百Hz完全没法用。方向信号的处理也要注意。很多驱动器的方向电平是正逻辑DIR为高等于正向。因此插补函数里的dir_u1对应DIR置1dir_u-1对应DIR置0。切换方向前最好在脉冲输出前先设置好方向引脚电平再输出脉冲沿。顺序反了会导致第一个脉冲方向错乱。5. 实测过程与调参记录5.1 测试平台搭建我自己的调试平台用的是两个57步进电机、8细分驱动器构成X/Y十字滑台丝杆导程5mm。控制器是一块很普通的单片机开发板通过两个GPIO输出方向信号另外两个GPIO配合定时器输出脉冲。平台小结构刚性一般速度上限不敢拉太高最高跑2000Hz左右。第一轮测试只跑直线。终点设置为X1600脉冲、Y1200脉冲也就是目标移动5mm和3.75mm斜线角度约36.87度。逐点比较法跑完后实际位置用百分表打两个方向误差都在0.01mm左右也就是3个脉冲的样子符合预期。误差主要来自丝杆间隙和加工装配精度算法本身并没有把轨迹带偏。5.2 圆弧插补测试数据插补圆测试我走了半径500个脉冲的整圆。起点设在圆的0度位置逆圆走一圈。跑完后记录了插补过程中的最大径向误差用image坐标纸辅助观察视觉上圆滑无凸点。逐点比较法的极限误差是一个脉冲当量也就是3.125微米实际因为机械间隙会出现稍大的偏差。有一件事要注意步进电机低速时共振明显加减速挡位太少会听到明显的“哒哒哒”抖动。我把加减速档位从32档提高到128档之后声音明显顺滑了很多。128档每次速度变化只有12.5Hz对电机来说很轻柔。5.3 加减速的实际效果对比同样跑1600脉冲的直线直接全速2000Hz启动电机几乎每次都丢步有时甚至卡死。加了梯形加减速后启动频率400Hz起步0.2秒加速到2000Hz同样的距离跑完无丢步、无过冲总耗时约1秒。虽然比不加加减速慢了但这是可靠性和速度之间的正确取舍。用小步进电机或直线导轨惯量比较大的结构加减速时间建议拉长到0.5秒以上否则停止的时候还会有明显的机械反弹。5.4 一个快速验证算法正确性的技巧不需要接电机也能验证算法。我写了一个纯软件模拟把插补函数算出的每一步px、py都记录到数组里然后打印出来。直线轨迹应该连成一条没有突变的斜线圆弧轨迹应该落在理论半径的±1脉冲内。模拟时发现个别点在圆弧末端偏出一个脉冲后来检查发现是圆弧终点判断用了remain计数换成坐标相等判断后问题消失。模拟验证比实测更高效新算法建议先跑一遍纯软件模拟再上机。6. 常见问题与排查技巧实录6.1 问题速查表故障现象可能原因处理办法电机走走停停、声音忽高忽低加减速档位太少或定时器装载值跳变太大提高档位数重新计算timer_load方向反了plane映射表写错或DIR极性接反核对map_plane_axis返回值检查DIR电平逻辑圆弧末端偏出终点判断方法不对或跨象限方向表有误改用坐标相等判断按象限拆分圆弧偏差计算溢出使用了16位整数改用DINT或long类型高速丢步启动频率过高、加速过猛降低启动频率延长加速时间PLC周期不稳定导致脉冲抖动周期任务被其它程序阻塞把插补任务优先级调高减少任务内计算量6.2 偏差计算溢出的处理直线插补的F值和脉冲数同级几百到几万都不会溢出。圆弧插补的F是平方量级半径5000个脉冲时F就是2500万级别16位完全不够32位也够呛如果半径到几万。处理办法有两个方向一是全部用64位整数成本低很多二是把坐标按比例缩小到偏差计算的安全范围内比如全部除以16后再进入圆弧插补但会损失低位的精度。工程上我更推荐64位整数现在MCU和PLC基本都支持64位整数运算性能代价可以接受。6.3 直线终点判断漏步直线用“总步数计数”有个条件必须先确认起点到终点的X方向总步数和Y方向总步数之和就是进给总步数。这是逐点比较法直线的性质决定的。如果起点不在原点不要忘了把起点坐标加上去。我见过有人用“当前坐标等于终点坐标”判断直线终点在某些特殊角度会提前结束比如45度线最后一个脉冲正好对角线方向单轴坐标判断会漏掉一步。计数法才是直线最稳的终点判断。6.4 定时抖动问题脉冲间隔抖动是步进电机发热和异响的重要原因。如果控制器用软件延时循环发脉冲中间被通讯中断打断脉冲间隔就会有毛刺。解决思路是全部走硬件定时器触发定时器中断到点就翻转脉冲引脚中断服务函数里只做“翻转、更新下一次周期、计算下一步方向”这三个动作越短越好。我在单片机上做时会把插补向量计算放在主循环里预计算中断里只取结果中断服务时间控制在10微秒以内。6.5 关于通用可编程控制器的移植建议如果目标平台是通用PLC建议用ST语言封装成一个函数块输入为终点坐标、平面选择、使能信号输出为两轴脉冲方向。老式PLC如果扫描周期超过10ms最高脉冲频率上限会很有限插补速度也上不去。这时候不要硬凑可以用外置脉冲发生器模块和PLC配合PLC只负责轨迹计算把方向信号发给外部脉冲发生器。7. 一些个人的工程体会我自己用了几年逐点比较法最大的感受是它像一个朴素的“闭眼走直线”的方法——每次只判断小偏差小步快跑反而比一些复杂的插补方法更可靠。如果你只是想快速实现功能这个算法很容易但要做到长时间稳定跑重点不在算法本身而在加减速、终点判断和这些边界参数。最后再分享一条试验经验给圆弧插补加一个“半径校验”功能每次走完四分之一圆之后比较当前实际半径和理论半径如果偏差超过2个脉冲就输出报警提示。这样机床操作员不用肉眼判断轨迹有没有偏。感谢这算法够简单才能让我在调试现场出问题时翻几行代码就能自己定位而不是对着黑盒控制器的日志发呆。
