逐点比较法实现三平面直线圆弧插补：带加减速的通用源码

插补算法这类东西，平时藏在运动控制卡和伺服驱动器里，很多人只管调用接口，很少去想底层到底怎么走的一步。可真当你碰到一台设备需要自己写运动逻辑，或者改造一台老机床、做实验平台，又不想被厂商封闭的指令集绑死，就会发现“能自己掌控的插补源码”是根救命稻草。这套用逐点比较法实现的直线圆弧插补算法，支持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说明在下方。逐点比较法每步只做四件事：

1. 偏差判别：看F的符号。
2. 坐标进给：F>=0时向+X进给一步，F<0时向+Y进给一步。
3. 新偏差计算：用递推公式更新F，避免每次都重新做乘除法。
4. 终点比较：计数器减一，判断插补是否完成。

关键在第三步，直接用加减法递推：

• 若向+X进给一步：F_new = F_old - ye
• 若向+Y进给一步：F_new = F_old + xe

为什么能这么算？因为x每加1，F就增加1ye，要从F里减掉ye让新位置重新对准偏差；y每加1，F就增加1xe，所以给F补上xe。这个递推只用加减法，在PLC的整数运算里效率非常高。

多象限直线插补其实也好处理：把xe、ye取绝对值，偏差计算用绝对值，进给方向由终点坐标的符号决定。比如终点在第二象限，X仍然要往负方向走，那就把方向变量置为-X，但偏差递推公式不变。下文源码里会给这个通用做法。

2.2 圆弧插补的偏差递推

圆弧插补同样用偏差法，只是偏差公式变成：

F = x^2 + y^2 - R^2

也就是当前点到圆心的距离平方和半径平方的差。F>=0表示当前点在圆外或圆上，F<0表示在圆内。

拿第一象限逆圆举例，进给规则如下：

• F>=0时，向-X方向进给一步，并更新：F_new = F_old - 2*x + 1，然后x减1
• F<0时，向+Y方向进给一步，并更新：F_new = F_old + 2*y + 1，然后y加1

这里的2*x+1来自(x-1)^2展开：x^2变成(x-1)^2后，偏差减少2x-1。同样的道理，y加1后偏差增加2y+1。代码里我会用先更新坐标、再修正偏差的写法，注意代数符号不能搞错，这是圆弧插补最容易写错的地方。

顺圆规则刚好反过来：F>=0向-Y方向走，F<0向+X方向走。实际工程中还有跨象限问题。我的处理策略是把任意起点、终点角度的圆弧按象限拆分，或者通过轴对称镜像把其它象限映射到第一象限计算。源码里先给第一象限实现，扩展时只需要做一张方向映射表，把计算出的“虚拟方向”变成实际轴方向。

2.3 三个平面一份代码：虚拟轴U/V映射

XY、XZ、YZ三个平面看起来要写三套算法，其实完全没必要。思路是这样的：插补算法只认识两个虚拟坐标轴，叫它U轴和V轴，运算逻辑一模一样。至于U轴到底对应物理X还是Y还是Z，由平面选择参数决定。

我用一个两列映射表搞定：

代码里就是查这个表。插补算出来“U正向走一步”，就查表找到实际物理轴，然后往这个轴的方向引脚输出一个脉冲。这也是标题里“XY方向插补、XZ方向插补、YZ方向插补”能被一份源码统一的原因。

2.4 梯形加减速的档位表和减速点判断

逐点比较法每个插补周期走一步，所以速度本质上由脉冲频率决定。如果一启动就按最高频率发脉冲，步进电机会直接丢步，或者机械机构会有很大冲击。梯形加减速是最简单也最实用的处理方式：加速阶段频率线性上升，匀速阶段保持最高频率，减速阶段频率线性下降，直到停止。

实现上我不喜欢每次实时计算速度，而是预生成一张频率档位表。比如从启动频率400Hz到最高频率2000Hz，切成128档，每档上升约12.5Hz。插补函数每走一步，根据加减速状态决定当前用哪一档频率。把频率转为定时器装载值，就是一个查表操作，效率很高。

减速点判断是梯形加减速的难点。不能等到只剩很少步数才开始减速，否则来不及降下来。我的算法是在每次进给后检查剩余步数：如果剩余步数小于或等于“从当前速度减速到启动频率所需要的步数”，就切换到减速状态。这个减速步数可以在加速阶段同步估算出来，因为加速和减速的斜率一样，加速用了多少步，减速至少也需要近似一样的步数。

3. 实用源码实现：加减速、三平面、直线圆弧一把梭

3.1 数据结构定义

先定义一个插补控制块，所有状态都塞在里面，方便移植：

typedef struct { int plane; /* 0=XY, 1=XZ, 2=YZ */ 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;

为什么圆弧注意相对圆心？因为偏差公式F=x^2+y^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的初始值是0，remain的初始值是 |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_old+1 */ p->dir_u = -1; p->dir_v = 0; } else { p->py++; p->F += 2 * p->py - 1; /* 等效于加上2*py_old+1 */ 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会溢出。圆弧偏差F=x^2+y^2-R^2中平方后可能是百万级别的值，DINT在32位下有正负21亿的范围，工程上足够用了。

4. 参数计算与移植要点：从脉冲当量到速度档位

4.1 脉冲当量与目标坐标换算

写插补代码之前，第一件事是把运动平台的机械参数换成脉冲数。假设一个常用测试台参数：

• 步进电机步距角1.8度，200步/圈
• 驱动器细分8
• 丝杆导程5mm

那么电机转一圈需要200*8=1600个脉冲，丝杆移动5mm，所以脉冲当量=5/1600=0.003125mm。也就是说，一个脉冲对应3.125微米。目标移动10mm，对应的脉冲数N=10/0.003125=3200个脉冲。

这个换算关系要提前做好，插补算法只认脉冲数，不认毫米。如果目标坐标用毫米，就先把毫米值乘上脉冲当量的倒数，转换成整数脉冲数再交给插补函数。换算出错，轨迹等于白算。

4.2 梯形加减速的具体计算

假设启动频率400Hz，最高频率2000Hz，加速时间0.2秒。那么加速度a=(2000-400)/0.2=8000Hz/s。加速阶段发出的脉冲数约等于平均频率乘时间：(400+2000)/2*0.2=240个脉冲。减速阶段同样要240个脉冲。

如果总脉冲数3200，匀速阶段的脉冲数是3200-240-240=2720个，匀速时间2720/2000=1.36秒，总运行时间约1.76秒。这个数字可以在界面参数里给操作员预估工时，也可以用来验证实际运行时间是否吻合。

实际加档时，我建议把128档的速度表转换成“定时器周期”而不是频率，因为很多MCU的定时器是递减计数，装载值越大，脉冲间隔越长。公式是timer_load = timer_clock / freq - 1。比如定时器时钟1MHz，2000Hz对应499，400Hz对应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 */ }

物理轴编号自己约定，0=X，1=Y，2=Z。输出脉冲前调用map_plane_axis得到物理轴编号，再查轴方向寄存器和脉冲输出寄存器。

这里有个细节值得注意：切换平面时，要先把当前所有轴的坐标状态保存好，再初始化新的插补控制块。如果程序分别维护三套插补状态，XYZ轴来回切换就会更灵活，但代码量也翻倍。我的建议是先做单平面切换，实际用到多平面联动时再扩展成三套状态块。

4.4 软硬件接口：定时中断与脉冲IO

算法不挑硬件，但硬件要满足一个条件：能稳定产生脉冲。最理想的实现是硬件定时器中断里发脉冲。以单片机为例，定时器每到达装载值就产生一次中断，中断服务程序里判断该往哪个轴输出电平翻转。PLC的话，要选带高速脉冲输出口的型号，频率上限至少在50kHz以上，否则加减速上限只有几百Hz，完全没法用。

方向信号的处理也要注意。很多驱动器的方向电平是正逻辑，DIR为高等于正向。因此插补函数里的dir_u=1对应DIR置1，dir_u=-1对应DIR置0。切换方向前，最好在脉冲输出前先设置好方向引脚电平，再输出脉冲沿。顺序反了会导致第一个脉冲方向错乱。

5. 实测过程与调参记录

5.1 测试平台搭建

我自己的调试平台用的是两个57步进电机、8细分驱动器，构成X/Y十字滑台，丝杆导程5mm。控制器是一块很普通的单片机开发板，通过两个GPIO输出方向信号，另外两个GPIO配合定时器输出脉冲。平台小，结构刚性一般，速度上限不敢拉太高，最高跑2000Hz左右。

第一轮测试只跑直线。终点设置为X=1600脉冲、Y=1200脉冲，也就是目标移动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 问题速查表

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个脉冲，就输出报警提示。这样机床操作员不用肉眼判断轨迹有没有偏。感谢这算法够简单，才能让我在调试现场出问题时，翻几行代码就能自己定位，而不是对着黑盒控制器的日志发呆。