ZMC600E运动控制器直线和圆弧插补算法详解

导读

想深入了解ZMC600E运动控制器的插补算法吗？空间直线插补适用于精确定位，圆弧插补提供平滑曲线过渡。合理应用这些算法，能提升多轴协同能力，保障工作精度与可靠性，让工业自动化更高效、更智能。

ZMC600E是广州致远电子股份有限公司开发的最新一代高性能智能总线型控制器，是面向工厂智能化时代的机器人控制器。ZMC600E采用多核异构应用处理器为核心，其内核包括2个64位的ArmCortex-A53核，主频1.0GHz；4个Cortex-R5F内核，主频800MHz。同时板载1GB DDR4、4GB eMMC以及32KB FRAM。

ZMC600E EtherCAT主站控制器为了满足不同的自动化应用需求，在硬件接口上面，精心设计了1路EtherCAT主站接口，ms 周期任务抖动±5μs 以内；3 路以太网接口，其中1路是千兆网口；2路RS485；2路CAN；32个用户I/O，其中16路DI，16路DO，输入支持最多2路正交编码或两路脉冲计数；1个USB3.0接口；支持TF卡；系统电源采用高稳定隔离电源，支持掉电检测；提供多种程序加密手段，保护用户应用软件知识产权。

ZMC600E 示意图及接口图如下所示：

图1 产品示意图

图2 ZMC600E接口图

ZMC600E EtherCAT主站控制器为设备制造商提供多轴数、多IO点数控制的新一代可靠智能的运动控制解决方案，提供空间直线、圆弧插补算法，可以广泛应用于注塑行业、冲压行业、车床行业、搬运码垛、关节机器人、喷涂、玻璃机、压铸机、包装设备、3C设备、锂电池设备、纺织、流水线工作站、非标自动化装备、特种机床等高端设备应用。

 插补的概念

插补是数控机床中的一项关键技术，它通过特定的算法精确确定刀具或机械部件的运动轨迹。这一过程的核心目标是根据预定的运动要求，实时计算出各个坐标轴的运动指令，驱动执行部件协调地运动，确保机械部件能够沿着理想的轨迹和速度精确移动。插补算法的基本原理涉及数字计算，它不断地生成各轴的进给指令，使得在运动过程中各轴能够同步且协调地运动，从而确保设备能够精确地完成预定的路径。

插补通常涉及至少两个轴的协同工作。首先，通过建立坐标系，将运动轴映射到相应的坐标系中。然后，运动控制器依据插补算法来控制各个轴的运动，实现所需的轨迹。在常见的插补类型中，直线插补用于实现两个点之间的直线运动，而圆弧插补则用于实现沿圆弧轨迹的运动。这两种插补都依赖于算法的实时计算，以保证轨迹运动的精度和平稳性。简而言之，插补是数控系统中确保机械部件按照预定轨迹精确、高效运动的一种技术。

 直线插补

以二维空间简化说明，如图1所示，直线插补运动：由起始点处沿X 方向走一小段（给一个脉冲当量轴走一段固定距离），发现终点在实际轮廓的下方，则下一条线段沿 Y 方向走一小段，此时如果线段终点还在实际轮廓下方，则继续沿 Y 方向走一小段，直到在实际轮廓上方以后，再向 X 方向走一小段，依次循环类推，直到到达轮廓终点为止。空间直线适用于任意维空间的直线，插补方法也是一样的道理。

图3 直线插补示意图
 圆弧插补

圆弧插补与直线插补类似，都是通过给定起止点及相关参数，计算出一系列逼近实际圆弧的离散点。这些点构成插补轨迹，控制各个轴沿此轨迹运动，最终完成圆弧路径的加工。空间圆弧插补既可以在二维平面进行，也可以是三维空间的协同运动。

以二轴为例简化说明，如图2所示，圆的插补运动：将通过一定的算法，逐步计算出圆弧的离散点，并控制两个轴按这些点进行协调运动，最终加工出目标圆弧路径。这一过程能够实现精确的曲线轨迹控制，确保机械部件沿指定的圆弧轨迹平稳移动。

图4 圆弧插补

空间圆弧默认为三维，构造圆弧时，方向逆时针为正、除起始点和终点外，辅助参数输入分为三种：

图5 空间圆弧构造

辅助点为圆弧上的点，其优势在于，路径方向唯一，且实际辅助点可以通过示教获得。

辅助点为圆心点，正常情况下是有两个方向的解，因此需要额外指定方向。但与起点、终点的夹角不能是180度，否则有无数解；缺点在于圆心处于障碍物区间时无法通过示教获得。

辅助点为与与原点形成的向量为平面法向量，向量长度为半径坐标，方向根据右手原则。但已知半径下，圆心在它们的中垂线上，同样有两个解。算法会选择起点和终点之间距离较短的那个圆，即总的运动角度最多为180度。缺点是圆弧角度必须小于或等于180°。

 插补算法API

ZMC600E 运动控制器提供了直线和圆弧插补 API 接口，方便用户进行插补运动的设置和调整。

创建直线轨迹规划

创建圆弧轨迹规划

删除轨迹规划的句柄

设置起始位置

设置终点位置

开启轨迹规划

获取当前轨迹规划经过的总距离

获取某个经过距离点的规划信息

 示例1. 直线插补示例：起始点{0, 10, 20}，终点{ 10, 0, -20}代码如下：

#include #include #include #include#include "trajectory.h"int main(){ double startPos[3] = { 0, 10, 20 }; double endPos[3] = { 10, 0, -20 };
trajectory_handle handle = trajectory_new_line(3); trajectory_set_start_pos(handle, startPos); trajectory_set_end_pos(handle, endPos); trajectory_set_plan(handle);
double dist = trajectory_get_plan_distance(handle); for(double d = 0; d <= dist; d+=0.1) { double pass_dist[3]; trajectory_get_plan_slice(handle, d, pass_dist); std::cout << "pass_dist :" << pass_dist[0] << "," << pass_dist[1] << "," << pass_dist[2] << endl; } trajectory_delete(handle); return 0;}

执行结果如图下图所示：

图6 直线插补轨迹

2.圆弧插补示例：起始点{-10,0,10}，终点{ 10, 0, 5 }, 经过圆弧点{0, 10, 7.5}

代码如下：

#include #include #include #include#include "trajectory.h"int main(){ double startPt[3]= { -10, 0, 10 }; double auxPt[3] = { 0, 10, 7.5 }; double endPt[3] = { 10, 0, 5 };
trajectory_handle handle = trajectory_new_arc(0, auxPt, 0); trajectory_set_start_pos(handle, startPt); trajectory_set_end_pos(handle, endPt); trajectory_set_plan(handle);
double dist = trajectory_get_plan_distance(handle); for (double d = 0; d <= dist; d+=0.1) { double pass_dist[3]; trajectory_get_plan_slice(handle, d, pass_dist); std::cout << "pass_dist :" << pass_dist[0] << "," << pass_dist[1] << "," << pass_dist[2] << endl; } trajectory_delete(handle); return 0;}

执行结果如图下图所示：

图7 空间圆弧插补示例

ZMC600E运动控制器 配备了直线和圆弧插补算法，这些算法能够灵活应对各种应用需求，确保轨迹的精确性。掌握这些运动插补技术，有助于提高设备的控制精度和工作效率，为工业生产带来显著的效益提升。