空间直线插补三轴联动控制系统的设计

　　摘 要：为了进一步改善数控设备中三轴机械手数控系统的性能与可靠性，设计一种基于 STM32 单片机和步进电机的三轴联动控制控制系统。系统以 STM32F103ZET6 处理器为核心控制器，以步进电机和三轴机械手为执行机构，结合互相独立的硬件形成统一的整体;系统可根据设定的任务路径坐标，在三维空间中进行连续空间直线插补，控制三轴机械手末端执行器的运动轨迹，实现精确稳定的三轴联动控制。实验结果表明，系统定位精度高，响应速度快，且便于移植，在工业生产中用途广泛，可有效提高生产效率。

　　关键词：空间直线插补;三轴联动控制;STM32;三轴机械手;步进电机

　　周佳明; 张宏， 宜宾学院学报 发表时间：2021-11-18

　　计算机数字化控制(CNC)系统利用计算机，通过数字信号控制执行器的开关量和机械量，实现精准控制运动单元的速度、加速度、位置等物理量。肖苏华[1]采用领域为载体、模型为中心的方法开发计算机数控系统，针对数控领域进行特性分析，提出了一种面向 CNC 系统领域的 CNCVFC 元建模方法。近年来，随着 SOC 技术的普及，核心控制器向小型化的方向发展，低廉的嵌入式芯片出现井喷现象，嵌入式数控系统[2]得到快速发展，越来越多的学者对嵌入式数控系统的稳定性和可靠性开展研究。张敏[3]分析了嵌入式控制器的重要电路工作原理，设计了以 STM32 为核心控制器的电路原理图以及 PCB 图，制作出样机设备，并做了相关的可靠性试验，通过对数控系统的现状分析，确定了一种体积小、功能强大、成本低廉的插补系统的研发方向。目前，典型开放的嵌入式数控系统有 Grbl、Marlin、Sprinter，开发人员只需做好相关的硬件配置，便可实现系统的快速移植，并大大降低研发和生产成本。

　　另一方面，随着计算机技术和工业自动化的快速发展，三轴机械手[4]在数控机床[5] 、 3D 打印[6] 、医疗器械等高精度领域被广泛应用。由于机床的加工精度、加工质量和生产效率在很大程度上取决于数控驱动系统的性能和精度，因而要求数控机床的核心控制系统具有稳定的多轴联动控制性能[7] ，能根据指令信号精确地控制各轴步进电机的运动速度，并能按照任务坐标轨迹在三维空间中精确作业。这些工业生产的需求，对数字控制系统的稳定性和可靠性提出了更高的要求。基于上述背景，以实现稳定的三轴联动控制和任务轨迹跟踪为目标，本文设计了一种以 STM32 单片机为核心控制器的三轴联动运动控制系统，采用步进电机和三轴运动平台作为执行机构，能够根据任务坐标连续进行空间直线插补，控制末端执行器的运动轨迹。

　　1 系统结构与方案设计

　　1.1 系统组成

　　系统由 STM32F103ZET6 核心控制器、串口通信模块、光电隔离模块、步进电机驱动模块、按键控制、LED 指示模块和三轴机械手等组成，能够在空间坐标内实现精确定位，完成任务路径跟踪及相关作业。机械手为通用模块，通过更换机械手的末端执行器和上位机指令可以改变其用途和功能，实现自动分拣、3D 打印、跟踪焊接[8]等功能。

　　1.2 系统架构设计

　　STM32F103ZET6 核心控制器输出 PWM 脉冲信号控制步进电机，同时将任务路径与当前位置进行比较，经空间直线插补算法，输出进给信号控制三轴机械手末端执行器的运动轨迹。为了便于人工控制，除上位机串口外，通过机械按键可控制步进电机的启动和停止，LED 指示灯指示电机的运行状态。系统架构如图 1 所示。

　　1.3 程序流程设计

　　系统的程序流程如图 2 所示，进入主程序后，先对系统时钟、步进电机、串口、及相关外设进行初始化，然后进入主循环程序。在循环程序中，重复进行按键扫描，当检测到按键 1 被按下，使能步进电机和 PWM 脉冲输出，并点亮电机状态指示灯，开启定时器插补中断，在插补中断中判断当前位置和预定路径位置的偏差并给定相应脉冲信号，进行空间直线插补，直到执行机构走完任务路径，失能步进电机，电机状态指示灯熄灭;当检测到按键 2 按下，再次使能步进电机，使末端执行器回到初始起点位置，为设备的下一次使用做准备。

　　2 硬件平台设计

　　运动控制系统硬件电路主要包括 STM32 单片机控制模块、光耦隔离模块、电机驱动模块、编码器模块、电源模块、按键控制模块和串口通信模块。

　　2.1 STM32 核心控制器

　　三轴联动控制系统采用 STM32F103ZET6 作为运动控制器，它是一高性能 ARM Cotex-M3 内核的 32 位微控制器，主频 72 MHz，有 144 个引脚、112 个 IO、512 KB flash 和 64 KB SRAM，拥有强大的数字处理功能，具有运行速度快、功耗低的特点。其高级定时器和通用定时器可以输出精准高频的 PWM 信号，从而精确控制步进电机。同时，可以实现复杂的轨迹运算以及和外围设备交互、与上位机及存储芯片通信[9-10] 。 STM32F103ZET6 最小系统如图 3 所示。

　　2.2 光耦隔离模块

　　光电耦合器(简称光耦)是一种半导体光电子器件，通过电→光→电的转换传输电信号，一般用于隔离和保护电路。由于具有体积小、寿命长、抗干扰能力强、输出与输入隔离和单向传输信号等特点，被广泛应用于数字电路中。本文设计的运动控制系统采用光耦隔离电路对驱动步进电机的脉冲信号输出进行隔离，达到保护和隔离控制电路的目的[11] 。光电耦合电路如图 4 所示。

　　2.3 电机驱动模块

　　2.3.1 步进电机

　　步进电机是一种将电脉冲信号转换成角位移或直线位移的控制器件(如图 5)，具有快速启停的能力。在额定工作条件下，电机转速和旋转角位移仅与脉冲信号的频率和脉冲个数有关。接收到的脉冲数越多，电动机的转角就越大。脉冲频率越高，电机转速越快，但不能超过最高频率，否则转矩将迅速降低，电动机停止旋转[12] 。混合式步进电机兼具反应式和永磁式的优点，具有精度高、步距角小、输出转矩大、动态性能好等优点，在高精度控制领域得到广泛应用[13]。本文选用了三个 57 闭环混合式步进电机作为三轴机械手驱动电机。

　　2.3.2 步进电机驱动器

　　由于本系统要求的精度较高，步进电机的步距角无法满足系统的使用要求，因此需要使用步进电机驱动器对步进电机的步距角进行细分。本系统使用的步进电机驱动器实物图及其型号参数如图 6 所示，它是一种将控制器输出的脉冲信号转换成步进电机角位移的执行器件，其输出的角位移与输入脉冲数成比例，转速与脉冲频率成正比。通过控制脉冲频率，能够控制电机转速，从而实现调速和定位。

　　3 系统设计与规划

　　3.1 PWM 脉冲与速度设计

　　为了能够稳定驱动步进电机，需要用使用核心控制器 STM32 单片机输出一定频率的 PWM 脉冲，以控制步进电机的启停、步进方向及其旋转速度[14-15]。以本系统使用的三轴机械手为例，需要控制三个不同的步进电机。如果要求三个步进电机的速度相同，则只需使用同一个通用定时器或高级定时器的三个不同的输出通道即可。在本系统中，定时器选用高级定时器 TIM8，PWM 模式选用模式 2，已知 STM32F103ZET6 的高级定时器 TIM8 的时钟频率为 72 MHz，设 PWM 周期为T(μs)，则每隔时间T 产生一次插补中断，那么可以建立等式： ( 1)( 1) 72 psc arr T + + = 其中： psc 为预分频系数，arr 为计数器重装载值。

　　由上式可知，PWM 的周期与预分频系数 psc 和计数器重装载值arr 成正比，又因为频率是周期的倒数，则 PWM 的频率与预分频系数 psc 和计数器重装载值arr 成反比，一般来说，可以通过改变计数器重装载值arr 来控制 PWM 的脉冲频率，改变频率就可以改变步进电机旋转速度;但这个频率必须小于步进电机的启动频率，否则电机无法启动。

　　3.2 任务轨迹规划

　　在三轴机器人完成给定的任务之前，需要进行轨迹规划，轨迹规划时一般将连续的路径轨迹进行离散化，用离散点“内插”或“逼近”任务轨迹。当离散的数据点相对分散时，离散点之间的间隔较大，该间隔成为插补盲区，从而产生了实际轨迹和理论轨迹的误差。在提出的多项式中，对理想笛卡尔轨迹进行了均匀采样。采样点的数目被步进电机步距角大小和细分驱动器的精度限制，其精度越高，能够设置的采样点就越多[16-17] 。如图 7 所示，设在笛卡尔坐标系中，空间中的任意一点为 , , Px y z ( e ee)，只要设定一条路径曲线公式，便可通过采样获得多个离散的三维坐标点，再由通讯接口将离散点发送给核心控制器完成相应动作。

　　基于三维直角坐标空间的轨迹规划比较直观，通常能直观地看到末端执行器的运动轨迹，也能达到较高的精度。本文使用 MATLAB 模拟了一条笛卡尔坐标系下的光滑轨迹，如图 8 所示。

　　根据三轴机械手的运动学模型以及相关参数，可将空间中轨迹的离散坐标点转换成步进电机的旋转圈数及旋转角度，在 STM32 的软件程序中生成离散的三维位置信息坐标，作为目标终点信号控制各轴上的步进电机运动。

　　4 空间直线插补

　　插补概念源于数值分析数学中的插值，它是一种在已知离散点范围内构造新数据点的方法。插补指的是数控系统根据电机的特性，用微小折线段来近似替代连续曲线的过程。这种方法在数控系统及相关行业中得到广泛的应用，是数控机床控制系统的重要组成部分。数控系统中刀具的移动与步进或伺服电机的驱动相关，脉冲通常作为步进或伺服电机的驱动信号来源，因而在数控系统中的插补过程称为脉冲增量插补，常见脉冲增量插补算法有数字积分法、逐点比较法和Bresenham法等。为了使插补算法更具一般性，通常将曲线轨迹按照特定的规则或算法打断成若干段连续的微小直线段，而后采用直线插补算法完成插补过程[18-19] 。

　　4.1 逐点比较法

　　逐点比较法[20]是插补算法中应用最广泛的一种。它既可用作直线插补也可以作圆弧插补，其逼近误差小于一个脉冲当量，输出脉冲的速度变化小，输出脉冲均匀，调节方便且操作直观，因此它被广泛地应用于具有多坐标联动的数控机床[21]。本文采用逐点比较法对第一象限点的线性插补原理进行了分析，再过渡到任意象限的直线插补，分别给出计算过程和计算公式。逐点比较法的直线插补步骤如图 9 所示。

　　4.2 第一象限内的插补

　　4.2.1 偏差判别

　　偏差判别就是判断当前加工点和理论加工点之间的相对位置偏差，决定下一步 X 、 Y 轴的运动方向。如图 10 所示，建立平面直角坐标系，要加工的空间轨迹为线段OE ， OE 是以坐标原点O为起点、以 ( ,) EX Ye e 为终点的平面直线段，点 ( ,) PX Yi i 表示加工点位置。为使问题简化，设 Xe 、Ye、Ze 均在第一象限，设加工点与OE 的偏差为 Fi ，则： F XY XY i ei ie = − 若 0 Fi = ，表示动点 P 在直线OE 内;若 0 Fi > ，表示动点 P 在直线OE 的上方;若 0 Fi < ，表示动点 P 在直线OE 的下方。

　　4.2.2 坐标进给

　　坐标进给就是根据计算出的偏差，控制指定坐标轴进给一个脉冲，逼近理论值，减小误差。如图 11 所示，当 0 Fi = 时，动点 P 在直线内，可向+X 方向进给一步，也可向+Y 方向进给一步，此处规定向+X 方向进给;当 0 Fi > 时，动点+Y 在直线上方，应该向+X 方向进给一步;当 0 Fi < 时，动点 P 在直线下方，应该向+Y 方向进给一步。

　　4.2.3 偏差计算

　　当 0 Fi ≥ 时，加工动点向 +X 方向进给一步，生成一个新的动点 Pi+1 ，设其坐标为 1 1 ( ,) X Y i i + + ，则新动点的偏差值 Fi+1计算公式为： F XY X Y i ei i e + ++ 1 11 = − (1)又因为动点 Pi+1 的坐标可由 P 点表示： 1 1 X X i i + = + (2) Y Y i i +1 = (3)将公式(2)、(3)代入公式(1)中，经化简可得新的偏差递推公式： F FY i ie +1 = − (4)同理可得，当 0 Fi < ，加工动点向+Y 方向进给一步后的新偏差值递推公式为： F FX i ie +1 = + (5)

　　4.2.4 终点判别

　　终点判别就是要判断是否到达加工终点，如果到达终点则停止插补，如过没有到达终点则回到第一个步骤，不断重复整个过程，直到到达轨迹终点。常用的终点判别方法有投影法，终点坐标法和总步长法。本文选取总步长法做插补终点判别。

　　4.3 任意象限的插补

　　由第一象限直线插补公式可推导出其它任意象限的直线插补公式，偏差计算公式基本相同，利用坐标系的绝对值来计算误差值。任意象限的插补如图 12 所示，设 L1、L2、 L3、L4 分别表示第 1、2、3、4 象限的直线，则任意象限直线插补的 X 、Y 轴进给方向如表 1 所示。

　　4.4 空间插补原理

　　如图 13(a)所示，建立空间直角坐标系，设加工轨迹OE 是以坐标原点O为起点，以 ( ,, ) EX Y Z ee e 为终点的空间直线段。设、Ye、Ze 均为正值，将坐标系中的直线OE 分别作两个二维平面的投影，并以 X 轴为基轴建立两个平面坐标系 XOY 和 XOZ ，投影到二维平面 XOY 和 XOZ 后如图 13(b)、13(c)所示，三维空间中的插补就可以分解成二维平面的插补。

　　4.5 空间插补程序流程

　　空间直线插补算法其计算流程[22]如图 14 所示， Xe 、Ye、Ze 分别表示 X 、Y 、Z 三轴的终点坐标，Fi 、 i S 分别表示投影到 XOY 和 XOZ 平面的偏差，End 等于终点坐标值之和，即表示总步长，用于终点判别。

　　5 实验与结果

　　(1)实验操作过程

　　①搭建硬件平台，对各个模块进行接线。 ②将上位机编译的程序写入 STM32 单片机。 ③打开电源使系统上电，观察到指示灯点亮。 ④按下按键 1，观察三轴机械手是否按照指定路径运动。 ⑤按下按键 2，观察三轴机械手是否能够返回到初始位置。

　　(2)实验结果

　　按下按键 1 后，启动状态指示灯点亮，三轴机械手按照任务轨迹平稳运行，到达终点位置后电机停止，状态指示灯熄灭;按下按键 2 后，启动状态指示灯再次点亮，三轴机械平稳运行，到达初始位置后停止，状态指示灯熄灭;整个实验过程平稳可靠，达到设计的预期要求，实验结果如图 15 所示。

　　6 结语

　　本文设计的基于 STM32 单片机和空间直线插补的三轴联动控制系统能够实现对三轴机械手进行空间直线插补，能够精确稳定地控制末端执行器跟踪任务路径坐标，实现稳定可靠的三轴联动控制。该系统硬件电路实现简单，成本低，可靠性高，软件程序逻辑清晰且易于移植。实验结果表明，该系统具备可实现性，在工业生产中用途广泛，对提高工业生产效率具有重要意义，具有较强的实用价值。