1.概念

不管是做自动化设备还是机器人运动学，都离不开对电机的控制，根据实际场景有各种各样的运动控制算法，而直线运动就是其中一种控制方式，今天就跟大家分享一个直线插补运动算法的原理，而代码的实现，则采用STM32单片机；

插补的概念源自数值分析数学中插值的意思，它是一类在离散的已知数据点范围内构造新数据点的方法，这类方法可以用在机器人运动关节上，也大量应用在自动化数控设备上，

比如在数控机床加工过程中，在理论上刀具的运动轨迹应该十分精准的沿着被加工工件的轮廓，同时满足对加工对象的精度要求。 但是真正加工的工件轮廓可能是各种形状，有一些简单的直线段和圆弧，还有一些复杂曲线。直接生成复杂曲线的运动轨迹会耗费大量的计算资源， 因此在实际应用中通常使用简单的线型去拟合复杂曲线，同时采用一系列微小的直线段去逼近直线和圆弧线型，以满足加工精度的要求，

比如直线段只提供起点和终点在加工系统中的坐标，圆弧则会提供圆心、起点和终点的坐标， 以及圆弧的方向。一般数控机床的刀具运动轨迹是由X、Y两个方向的运动合成的，本身并不能非常严格的按照理论曲线运动，只知道这么一些线段参数无法精准的完成加工任务， 需要一种方法能把已知点中间所有微小直线段的坐标点全都计算出来，从而形成符合精度要求的刀具运动轨迹，这种计算方法就叫插补；

插补算法法所要解决的就是根据加工速度的要求，在给定的数据点坐标之间，连续计算出若干中间点的坐标值。 而这些中间点的坐标值以一定的精度逼近理论的轨迹。由于计算每个中间点所消耗的时间直接影响数控系统的控制速度，坐标值的计算精度又影响数控系统的控制精度， 所以插补算法是整个数控系统的控制核心。

2.插补算法介绍

插补属于一类方法，根据不同的条件可以有很多种实现方法，由于我们是使用步进电机作为控制源，所以根据脉冲信号输出方式，在这里介绍一种脉冲增量插补法；

所谓的脉冲增量，就是用一个个脉冲输出的方式，驱动电机实现运动，每次一个脉冲信号，X轴或者Y轴就是移动一个脉冲量，比较适合大多数普通电机控制，通过几个简单的加减法就可以实现，缺点就是运动速度，会因为每次计算时间受到一定的限制，优点就是非常适合以步进电机为驱动装置的开环控制系统中；

而脉冲增量插补算法，又分为很多不同的实现方式，在这里跟大家介绍三种常见的

2.1.逐点比较法：逐点比较法最开始被称为区域判别法，又称代数运算法或醉步式近似法。是一种逐点计算、判别偏差并修正逼近理论轨迹的方法。 逐点比较法的基本思想就是在刀具按理论轨迹运动加工工件轮廓的时候，不断比较刀具与工件轮廓之间的相对位置， 并根据比较结果决定下一步的进给方向，使刀具向减小误差的方向移动。

2.2.数字积分法：数字积分法又称数字微分分析法DDA，简称积分器。这种算法是在数字积分器的基础上建立起来的一种插补算法， 可以较为方便的实现一次、二次曲线的插补。具有运算速度快、脉冲分配均匀、易于实现多坐标联动及描绘平面各种函数曲线的特点，应用比较广泛。

2.3.Bresenham（布雷森汉姆）算法：这种算法本来是计算机图形学里的一种用来快速画直线段和圆的绘图算法，由于算法非常成熟并且十分高效，目前也逐渐被引入到数控系统中， 通常作为直线插补算法或者多轴联动算法使用。

而这期文章，重点就是跟大家介绍逐点比较法的实现和原理

3.逐点比较法原理

逐点比较法是以阶梯折现来逼近直线段和圆弧等曲线。 每完成一次进给都需要以下4个步骤：

3.1.偏差判别：判断当前加工点和理论加工图形之间的相对位置，决定下一步X、Y轴的运动方向；

3.2.坐标进给：根据得到的偏差，控制指定坐标轴进给（移动）一步，逼近理论图形，减小误差；

3.3.偏差计算：计算新的加工点与理论加工图像间的偏差大小，作为下一步判别的依据；

3.4.终点判别：判断是否到达加工终点，如果到达终点则停止插补，如过没有到达终点，则回到第一个步骤， 不断重复整个过程，直到到达轨迹终点。

4.偏差判断

如下图所示，一根直线OE，O为原点，E为目标点，P为动点，

根据上图信息，使用两点直线方程求OE的方程为：

之后，设F为动点P与直线OE的偏差，把F和动点P代入公式得：

上面等式中可以分析出，F的符号能够反映动点P和直线OE的位置偏差情况。

若F= 0，表示动点P在直线OE内；

若F> 0，表示动点P在直线OE的上方；

若F< 0，表示动点P在直线OE的下方。

5.位置变化和计算

当F = 0时，动点P在直线内，可向+X方向进给一步，也可向+Y方向进给一步，通常规定向+X方向进给；

当F > 0时，动点P在直线上方，应该向+X方向进给一步；

当F < 0时，动点P在直线下方，应该向+Y方向进给一步；

如图所示：

坐标每次的前进，都会得到一个新的动点，之后计算新的动点和理论轨迹之间的偏差值，

从前面的讨论中我们知道了偏差值Fi的计算公式， 可以通过公式直接求出Fi。虽然现在的各种控制器基本可以轻松的做乘法运算，但是为了追求更高的运行效率，我们把当前的偏差计算公式做一点小小的优化， 将其变为递推公式，即设法找到相邻两个加工动点偏差值间的关系。

假设当Fi> 0时，加工动点向+X方向进给一步，生成一个新的动点Pi+1，坐标是(Xi+1, Yi+1)， 则新动点的偏差值Fi+1计算公式为：

又因为动点Pi+1的坐标可由P点表示：

所以将由P点表示的Pi+1坐标代入Fi+1式中，可得：

最后得出的这个公式便是逐点比较法的第一象限直线插补偏差计算的递推公式，从式中可以看出，偏差Fi+1的计算只跟上一步进给的偏差Fi和终点坐标值有关， 且只有加法运算，比原始公式更简单快速。

同理可得，当Fi< 0，加工动点向+Y方向进给一步后的新偏差值递推公式：

常用的终点判别方法有三种，终点坐标法、投影法和总步长法。

终点坐标法。在启动插补之前，先定义X、Y两个方向的步长计数器，分别记录终点坐标在X、Y两个方向上的值。开始插补后当X、Y方向每进给一步， 就在相应的计数器中减1，直到两个计数器的值都减为0时，刀具抵达终点，停止插补。

投影法。在插补前，先比较出终点坐标的X、Y值中较大的一个，然后以较大的数值作为计数器的值，当对应的轴有进给时，计数器减1，直到计数器为0。 相当于终点坐标向值较大的轴做投影，所以叫投影法。

总步长法，即插补前，将终点坐标的X、Y值求和，得到一个总步长计数器，开始插补后，无论哪个轴进给一步，总步长计数器都减1，直到计数器等于0，停止插补。

以上三种终点判别的方法，全部使用坐标的绝对值进行计算。

6.STM32代码设计

变量初始化：

/* 坐标轴枚举 */typedef enum{x_axis = 0U,y_axis}Axis_TypeDef;/* 直线插补参数结构体 */typedef struct{__IO uint32_t endpoint_x;           //终点坐标X__IO uint32_t endpoint_y;           //终点坐标Y__IO uint32_t endpoint_pulse;       //到达终点位置需要的脉冲数__IO uint32_t active_axis;          //当前运动的轴__IO int32_t deviation;             //偏差参数}LinearInterpolation_TypeDef;

运动的初始化：

/*** @brief 放在移动之前初始化*    @note         无* @parameter xPulse x轴需要发送的脉冲总量；* @parameter yPulse y轴需要发送的脉冲总量；* @retval -*/
void move_init(int xPulse,int yPulse)
{interpolation_para.deviation = 0;interpolation_para.active_axis = x_axis;interpolation_para.deviation -= yPulse;interpolation_para.endpoint_y = yPulse;interpolation_para.endpoint_x = xPulse;interpolation_para.endpoint_pulse = yPulse+xPulse;__HAL_TIM_SET_COMPARE(&armX.htim, armX.tim_channel, armX.speed / 2); // 移动__HAL_TIM_SET_COMPARE(&armY.htim, armY.tim_channel, 0); // 停止移动，把占空比设置为0，表示停止HAL_TIM_PWM_Start_IT(&armX.htim, armX.tim_channel);//x轴启动HAL_TIM_PWM_Start_IT(&armY.htim, armY.tim_channel);//y轴启动
}

具体算法实现，放在定时器中断函数中调用（HAL_TIM_PeriodElapsedCallback()里面调用）：

/*** @brief 直线绘制， 逐点比较法来计算下一个脉冲*    @note         无* @retval 返回0表示没有执行完成，返回1表示执行完成*/
void move_draw_line()
{uint32_t last_axis = 0;if(interpolation_para.endpoint_pulse <= 0){//结束直线运动/* 关闭定时器 */servo_stop(&armX);servo_stop(&armY);return;}/* 记录上一步的进给活动轴 */last_axis = interpolation_para.active_axis;/* 根据上一步的偏差，判断的进给方向，并计算下一步的偏差 */if(interpolation_para.deviation >= 0){/* 偏差>0，在直线上方，进给X轴，计算偏差 */interpolation_para.active_axis = x_axis;interpolation_para.deviation -= interpolation_para.endpoint_y;}else{/* 偏差<0，在直线下方，进给Y轴，计算偏差 */interpolation_para.active_axis = y_axis;interpolation_para.deviation += interpolation_para.endpoint_x;}/* 下一步的活动轴与上一步的不一致时，需要换轴 */if(last_axis != interpolation_para.active_axis){if(interpolation_para.active_axis == x_axis){__HAL_TIM_SET_COMPARE(&armX.htim, armX.tim_channel, armX.speed / 2); // 移动__HAL_TIM_SET_COMPARE(&armY.htim, armY.tim_channel, 0); // 停止移动}if(interpolation_para.active_axis == y_axis){__HAL_TIM_SET_COMPARE(&armY.htim, armY.tim_channel, armY.speed / 2); // 移动__HAL_TIM_SET_COMPARE(&armX.htim, armX.tim_channel, 0); // x停止移动}}interpolation_para.endpoint_pulse--;return;
}