T-CPU 路径插补功能

n

1 路径插补功能简介

1.1 基本概念
插补的概念源于数控机床。在数控机床中，刀具不能严格地按照要求加工的曲线(直线)运动，只能用折线轨迹逼近所要加工的曲线。机床数控系统依照一定方法确定刀具运动轨迹的过程，叫做插补。也可以说，已知曲线上的某些数据，按照某种算法计算已知点之间的中间点的方法，或者称为“数据点的密化”。插补的动作过程：在每个插补周期（极短时间，一般为毫秒级）内，根据指令、进给速度计算出一个微小直线段的数据，刀具沿着微小直线段运动，经过若干个插补周期后，刀具从起点运动到终点，完成轮廓的加工。
路径运动最初来自于机器人和CNC领域，用机器人编程语言或G-Code编程。它是指在多维空间中，通过一组轴的协作动作，各轴之间无主从之分，它们按照设定的动态响应特性，实现路径对象从起点到终点的指定的路径轨迹运动。
路径插补产生路径的运行轨迹，计算插补周期内的路径插补点，并通过机械运动系统转换获得对应插补周期内插补点的各路径轴设定值。
隶属于机械运动系统的单独轴在S7 technology中被指定为路径轴，路径轴通过路径对象执行路径运动。参考图1 路径轴与路径对象。n

图1 路径轴与路径对象n

1.2 S7-Technology 路径功能特点n

S7-Technology 路径插补功能概述：n

> 从S7-Technology V4.2开始
> 允许进行3轴插补操作
> 路径插补可以通过直线、圆弧、多项式表示实现
> Move Path命令可以组成连续运动
> 支持多种机械运动学模型
> 可以与外部位置值同步，实现传送带跟踪功能

S7-Technology 路径插补的技术特点：n

> 所有的路径轴都相互同步移动
> 所有的路径轴都同时到达目标位置
> 路径轨迹的移动，将会始终是以一个固定的合成速度进行 (如果动态
特性限制没有被超越)
> 最低速度性能的轴，决定了整个轨迹的最高动态特性

路径差补可以执行最多3轴之间的2D或者3D的线性、圆弧或者多项式插补，路径差补工艺对象(TO) ，适用于机械运动学控制范畴，一个共同的系统中，可以存在多个机械运动学控制结构。同步于路径轴的“同步轴”，仍然可以实现同步控制，例如，旋转，凸轮开关，测量功能。通过图形化编辑器，可以简便地设置机械运动学控制系统的参数；通过动态特性轮廓窗口，可以轻松定义路径的动态特性；通过轨迹点表格，可以轻松定义路径差补，计划目的地路径。另外还可以定义保护防撞区域和实现传送带位置的精确跟踪。n

1.3 机械运动系统的选择
T-CPU 所实现运动学，等同于人们过去所熟悉机械运动学。可以将它们分为如下两种不同的类型。参考图2 在 T-CPU 中集成的机械运动学。n

图2 在 T-CPU 中集成的机械运动学n

不同的机械运动系统可以实现TCP (Tool Center Point，工具中心点或机械运动端点)相同的路径运动功能。尽管在某些情况下，不同的机械运动系统可实现相同的路径运动，但是，如果机械运动系统选择不合理，将有可能无法完成指定的路径功能。所以，必须根据实际的工艺需求选择合适的机械运动系统，并在工厂布局中考虑该机构的的合理安装位置。参考图3 不同机械系统的转化。n

图3 不同机械系统的转化n

2 路径插补的实现方法

2.1 运动学模型简介
常用的运动学模型请参考图 4 运动学模型。n

图 4 运动学模型n

下面介绍一些常用的运动学模型。直角坐标机器人 (英文名：Cartesian coordinate robot)，大型的直角坐标机器人也称桁架机器人或龙门式机器人，由多个运动自由度建成空间直角关系的、多用途的操作机器。工作的行为方式主要是通过完成沿着X、Y、Z轴上的线性运动。因末端操作工具的不同，直角坐标机器人可以非常方便的用作各种自动化设备，完成如焊接、搬运、上下料、包装、码垛、拆垛、检测、探伤、分类、装配、贴标、喷码、打码、（软仿型）喷涂等一系列工作。参考图5 直角坐标机器人。n

图5 直角坐标机器人n

SCARA 机器人（Selective Compliance Assembly Robot Arm）是一种圆柱坐标型的特殊工业机器人。有3个旋转关节，其轴线相互平行，在平面内进行定位，另一个关节是移动关节，用于完成机械末端在垂直平面的运动。SCARA机器人在x,y方向上具有良好的顺从性、灵活性，而在Z轴方向具有良好的刚度，此特性特别适合于装配工作。SCARA机器人广泛应用于塑料工业、汽车工业、电子产品工业、药品工业和食品工业等领域。它的主要职能是搬取零件和装配工作。 参考图6 SCARA 机器人。n

图6 SCARA 机器人n

铰链型机械臂：有很高的自由度，可以多至5~6轴，适合于几乎任何轨迹或角度的工作，可以自由编程，完成全自动化的工作, 提高生产效率；可以代替很多不适合人力完成、有害身体健康的复杂工作，比如，汽车外壳点焊。参考图 7 铰链型机械臂。n

图7铰链型机械臂n

Delta 3D机器人：外形酷似一只蜘蛛，这种先进的几何结构赋予了它们质量轻，强度大，轻便灵活，节省空间，高速，敏捷；适用于高速分拣。参考图8 Delta 3D机器人。n

图8 Delta 3D机器人n

2.2 路径插补实现方法
首先需要做轴的定义，选择“Path interploation”。参考图 9 路径轴的定义。n

图9 路径轴的定义n

然后双击“Insert path object”插入路径对象。路径对象是路径插补及与路径插补相关的其他任务的客体，路径对象也包括机械运动系统转换。参考图10 插入路径对象。n

图10 插入路径对象n

路径对象生成后，需要执行轴的关联。路径轴与其他路径轴一起，通过路径对象执行路径运动。参考图11 路径轴的关联。在该图中，路径轴Axis_1、Axis_2、 Axis_3同属于路径对象Path_object_1。n

图11 路径轴的关联n

路径轴关联后，需要选择对应的机械模型，通过组态，T-CPU自动完成目标坐标系与机器坐标系的转换。参考图12 机械模型的选择。工艺组态完成后，会自动生成各个轴以及路径对象的工艺DB，参考图13 工艺DB。然后就可以调用相应的功能块进行程序的编写工作。n

图12机械模型的选择n

图13 工艺DBn

2.3 坐标系的转换
坐标系是描述路径及任何位置信息的必要条件。在路径插补功能中，涉及三个坐标系的转换。分别是基础坐标系、机器坐标系和目标坐标系。基础坐标系描述了机械运动系统在空间的安装视图，参考图14的红色标示；机器坐标系描述了路径轴的位置视图，参考图14的黑色标示；目标坐标系描述了路径运动的物体或工件的本地视图，参考图14的蓝色视图。目标坐标系能与运动的传送带轴同步运动。路径对象的运动命令的位置信息在目标坐标系中或在基础坐标系中指定。n

图14 三种坐标系n

在T-CPU 的路径对象中，可以通过 Offset (偏移量)来设定基本坐标系的位置，该偏移量的计算是从运动学对象的零点起始，直到基本坐标系(BCS)的原点为止。如图15 坐标系转换示例，X轴偏移值Offset X = 250+50 = 300，Y轴偏移值 Offset Y= -550+50 = -500，Z轴偏移值 Offset Z = -1100+50+200 = -850。计算完成坐标系偏移值后，需要在组态中填入这些偏移值，参考图16 偏移量设定。n

图15 坐标系转换示例n

图16 偏移量设定n

在坐标系转换中，不仅要考虑到坐标系本身的偏移，还要考虑具体的机械结构和机械臂的初始位置。以SCARA系统为例，有两个臂长L1, L2，需要在组态中填写这两个长度。参考图17 SCARA 臂长设定。在初始位置，机械臂相对于零点位置和机械臂之间可能有角度上的偏移，参考图18 机械臂角度偏移，需要在组态中去设置这个偏移角度，参考图19 机械臂角度偏移设置。n

图17 SCARA 臂长设定n

图18 机械臂角度偏移n

图19 机械臂角度偏移设置n

3 路径插补功能块的使用
路径插补功能需要通过路径插补功能块来编程实现。具体的功能块请参考表1 路径插补功能块。

表1 路径插补功能块n

3.1 MovePath 功能
按照预先定义好的路径差补轨迹点表格，实现路径轨迹控制。可以在三维空间预先定义若干个点，按照这些路径点去做插补运动，可以实现常用的拾取 / 堆放工艺。参考图20 MovePath 轨迹。在轨迹的每一个段落内，都可以实现速度变化；在轨迹控制中，可以定义位置停止点，支持M 功能输出，路径表最多支持240个位置点。n

图20 MovePath 轨迹n

首先需要使用FB489 “MC_PathSelect “ 做路径轨迹的选择，然后调用功能块FB488”MC_MovePath” 做路径插补运动。参考图21 路径选择与运动。必须首先调用“MC_PathSelect” 工艺功能选择插补点数据，同时定义路径段，数值为1-5，数值写入输入参数Segment，然后将其装入到T-CPU 的Technology内核中。使用该Technology内核可存储最大5条路径段。工艺功能“MC_MovePath” 允许一个路径对象沿着一条路径段运动的同时，通过工艺功能“MC_PathSelect”准备好其他路径段。
可以使用 UDT 121创建路径点DB “PathData”， “PathData” 使用指针形式 ANY-Pointer，每一个路径点占用 22 Byte，每个路径表最多由240个路径点组成。参考图22 生成路径表。n

图21 路径选择与运动n

图22 生成路径表n

在路径表中，Point[x].X、Point[x].Y、Point[x].Z 分别指X,Y,Z坐标，Point[x].CornerDistance表示直线在插补点之间的圆弧转换和带冲击限制的多项式转换的角距离(REAL)，根据PathDescription (3或11)选择转换模式。对于小于0.0的值，路径对象的运动在相关停止点被中断。参考图23 插补点转换。Point[x].Override表示速度替换值/速度校正值(REAL) ，取值范围从0.0%到200%。参考图24 速度校正。对于沿着路径段的双向运动，速度曲线是一致的。Point[x].M_Function表示用户自定义功能码(INT) ，对应MC_MovePath的输出参数Code输出值。创建路径点DB “PathData”在程序中的调用请参考图25 路径表调用。n

图23 插补点转换n

图24 速度校正n

图25 路径表调用n

按照图25 所示的路径表，执行“MC_MovePath”后，三个轴同时到达（600，400，200）的坐标位置，参考图26 MovePath trace 曲线。n

图26 MovePath trace 曲线n

3.2 直线插补、圆弧插补和多项式插补
T-CPU 可以通过功能块实现空间三维的直线插补、圆弧插补和多项式插补，相关功能块有FB484 “MC_MoveLinearAbsolute”，FB485 “MC_MoveLinearRelative”，FB 486 “MC_MoveCircularAbsolute”，FB 487 “MC_MoveCircularRelative”，FB 490 “MC_MovePolynomAbsolute”，FB 491”MC_MovePolynomRelative”。另外还有一个圆周运动
能块FB 496 “MC_MoveCircles”。参考图27 路径插补基本功能。n

图27 路径插补基本功能n

3.2.1 直线插补
FB484 “MC_MoveLinearAbsolute”工艺功能使路径对象沿着一条线性路径移动到一个绝
对目标位置。目标位置在三维空间指定。命令中的输入参数AxesGroup 写入路径对象的工艺DB 块号；Position 写入目标位置的绝对坐标地址，该地址的数据结构由”S7-Tech”库中的UDT42 创建；Velocity , Jerk, Acceleration和 Deceleration允许指定路径对象的动态值；根据输入参数CoordSystem，可以指定参照基础坐标系或目标坐标系；输入参数BufferMode允许指定是否用新的运动取代当前运动，或将新的运动添加在当前运动之后，或将新的运动与当前运动混合再附加在当前运动之后；输入参数TransitionMode允许指定运动混合的类型；输入参数TransitionParameter指定混合类型的相关参数(转弯半径)。参考图28 FB484。n

图28 FB484n

其中，BufferMode取值有0、1、2，TransitionMode取值有0、3、10、11，具体的运动轨迹可以参考图29 BufferMode与TransitionMode的作用。n

图29 BufferMode与TransitionMode的作用n

在该图中，对于模式 ①，立即执行新的运动指令，有停顿；对于模式 ②，等待旧指令完成后执行新指令，有停顿；对于模式 ③，立即执行新的指令，通过指定的转弯半径切换，无停顿；对于模式 ④，等待旧指令完成后，通过指定的转弯半径执行新指令，无停顿；对于模式 ⑤，立即执行新的指令，无停顿，拐点冲击较大；对于模式 ⑥，等待旧指令完成后执行新指令，无停顿，拐点冲击较大；对于模式 ⑦，立即执行新的指令，通过多项式曲线切换，无停顿，减小拐点冲击；对于模式 ⑧，等待旧指令完成后，通过多项式曲线切换到新指令，无停顿，减小拐点冲击。
如果将目标位置坐标定义为（100，100，50），参考图30 目标位置，则触发FB484 后，各轴同时到达（100，100，50）的目标位置，参考图31 绝对线性运动轨迹。
、n

图30 目标位置n

图31 绝对线性运动trace曲线n

FB485 “MC_MoveLinearRelative” 工艺功能使路径对象沿着一条线性路径移动一段相对距离。该距离在三维空间指定，由输入参数Distance 写入，该变量的数据结构由”S7-Tech”库中的UDT42 创建；运动命令中的输入参数Velocity , Jerk, Acceleration和 Deceleration允许指定路径对象的动态值；根据输入参数CoordSystem，可以指定参照基础坐标系或目标坐标系；输入参数BufferMode允许指定是否用新的运动取代当前运动，或将新的运动添加在当前运动之后，或将新的运动与当前运动混合再附加在当前运动之后；输入参数TransitionMode指定混合类型的相关参数(转弯半径)。参考图32 FB485。n

图32 FB485n

举例说明BufferMode和TransitionMode 对运动轨迹的影响。参考图33 两次触发FB485，首先触发FB485进行线性相对运动，在目标位置还未到达时，触发另外一个FB485进行线性相对运动，时序图参考图34 线性相对运动时序图。由于FB485 BufferMode和TransitionMode均设置为0，则根据图29 可得出，第二次FB485触发后，轴将减速，向之前设定位置的运动停止下来，并立即执行新的运动指令，最终到达新的目标位置。运动轨迹参考图 35 线性运动轨迹。n

图33 两次触发FB485n

图34 线性相对运动时序图n

图35 线性运动轨迹n

3.2.2 圆弧插补
FB486“MC_MoveCircularAbsolute”工艺功能使路径对象沿着一条圆弧路径移动到一个绝对目标位置。目标位置在三维空间指定。参考图36 FB486，输入参数AxesGroup 写入路径对象的工艺DB 块号，AuxPoint、Endpoint 由UDT42 创建的DB块来指定；Velocity 指定路径对象的运行速度，Acceleration 指定运行对象的加速度，Deceleration 指定运行对象的减速度，Jerk指定运行对象的加加速。圆弧路径可由输入参数CircMode定义，CircMode为 0 时，用起点、中间点和终点定义圆弧路径，参考图37 CircMode为 0；CircMode为 1时，用起点、圆心点和终点定义圆弧路径，参考图38 CircMode为 1。通过PathChoice 来指定沿长弧运行或者沿短弧运行。n

图36 FB486n

图37 CircMode为 0n

图38 CircMode为 1n

BufferMode 的值为0，则替代当前的运动，值为1则附加当前的运动，值为2 则覆盖当前的运动，默认值为0。在图39 中，路径对象先做线性绝对运动，在运动过程中，触发绝对圆弧插补，则当前的线性运动运动中止，轴减速停止然后执行圆弧插补运动。时序图参考图40圆弧插补时序图，运行轨迹参考图41绝对圆弧插补轨迹。n

图39 绝对圆弧插补n

图40 圆弧插补时序图n

图41 绝对圆弧插补轨迹n

例如，将中间点坐标设置为（100，80，60），终点坐标设置为（200，160，120），需要建立两个DB块，DB块的数据类型选择UDT42，将X, Y, Z坐标填入对应的DB块中，参考图42 坐标点定义。在功能块FB486 中去调用这两个功能块，参考图43 调用坐标点。n

图42 坐标点定义n

图43 调用坐标点n

正确执行该功能块后，可用trace功能查看各轴的位置曲线，参考图44，三个轴在同一时刻分别到达各自坐标位置（200，160，120）。n

图44 圆弧插补trace 曲线n

FB487 “MC_MoveCircularRelative”工艺功能使路径对象沿着一条圆弧路径移动到一个相对于起点的目标位置。目标位置在三维空间指定，参考图45 FB487。Velocity 指定路径对象的运行速度，Acceleration指定运行对象的加速度，Deceleration指定运行对象的减速度，Jerk指定运行对象的加加速。CircMode 指定圆弧路径模式，值为0时，可以通过定义起始点、相对中间点和相对终点来定义圆弧；值为1时，通过定义起始点、相对圆心和相对终点来定义圆弧，通过PathChoice 来指定沿长弧运行或者沿短弧运行。n

图45 FB487n

在图46 中，路径对象先做线性绝对运动，在运动过程中，触发相对圆弧插补，BufferMode 为1，则立即执行相对圆弧插补运动，CircMode为1，则按照起始点、相对圆心、相对终点来定义圆弧，PathChoice为1，则运动沿长弧段执行。时序图参考图47相对圆弧插补时序图，运行轨迹参考图48相对圆弧插补轨迹。n

图46 相对圆弧插补n

图47相对圆弧插补时序图n

图48 相对圆弧插补轨迹n

FB496 “MC_MoveCircles”工艺功能使路径对象沿着一条圆弧路径移动。参考图49 FB496。该功能允许执行大于360°的路径运动，输入参数PathPlane设定在X/Y平面、Y/Z平面或Z/X平面中执行圆弧运动。Velocity 指定路径对象的运行速度；Acceleration 指定运行对象的加速度；Deceleration指定运行对象的减速度；Jerk 指定运行对象的加加速；Arc 指定圆弧的行程角度；Direction 指定圆弧角度的旋转方向。参考图50 圆弧插补轨迹。n

图49 FB496n

图50 圆弧插补轨迹n

3.2.3 多项式插补
FB490 “MC_MovePolynomAbosulte” 工艺功能使路径对象沿着一条多项式路径移动到一个绝对目标位置。目标位置在三维空间指定。参考图51 FB490。Velocity 指定路径对象的运行速度；Acceleration 指定运行对象的加速度；Deceleration 指定运行对象的减速度；Jerk 指定运行对象的加加速；PolynomialMode 定义了如何使用输入参数PolynomData上的多项式数据；PolynomData 表示多项式插补的相关数据。运动轨迹参考图52 多项式插补。n

图51 FB490n

图52多项式插补n

PolynomialMode=0 时，空间曲线由多项式来确定，可以由最大为五次方的多项式定义空间曲线。
参考图53 五次方多项式。n

图53 五次方多项式n

其中：“q”在0~1之间变化，q=0时得到起点，q=1时得到终点。当q=0时(起点)，可得到关于系数a0的公式；当q=1时(终点)，可得到关于系数a1的公式。参考图54 。n

图54 系数a1的公式n

所以在PolynomData中，只需要定义系数a2 ~ a5的值；PolynomData数据由UDT41创建的DB块提供。参考图55 UDT41定义系数。n

图55 UDT41定义系数n

PolynomialMode为1或者2时，多项式由起点和终点的切向量和法向量定义。所以在PolynomData中，只需要定义相应坐标上的切向量和曲率向量。PolynomData数据由UDT41创建的DB块提供。为了得到一个合适的多项式曲线，通常需要使用CAD等数学软件计算公式所需要的系数和向量。参考图56 多项式由切向量和法向量定义。n

图56 多项式由切向量和法向量定义n

FB491“MC_MovePolynomRelative”工艺功能使路径对象沿着一条多项式路径移动到一个相对目标位置。目标位置在三维空间指定。Velocity 指定路径对象的运行速度；Acceleration指定运行对象的加速度；Deceleration指定运行对象的减速度；Jerk指定运行对象的加加速； PolynomialMode定义了如何使用输入参数PolynomData上的多项式数据；PolynomData 定义多项式插补的相关数据。具体使用参考FB490。功能块参考图57 FB491。n

图57 FB491n

单个路径命令的路径运动可组合起来形成一条没有中间停止点的完整路径。用单个功能块指令实现路径运动时，要正确输入每条指令的参数，特别是前后指令之间的BufferMode和TransitionMode。 参考图58单个功能块指令组合实现路径差补。n

图58单个功能块指令组合实现路径差补n

3.3 安全区域检测
不同的检测区域可以被用户自定义，并且这些区域可以在启动运行和运行过程中被检测。可以检测产品区域是否与禁止区域发生碰撞，如果碰撞将要发生，则运动会被停止在区域边缘。同样也可以用来检测工件是否到达指定的报告区域内。参考图59 安全区域检测，控制对象被限定在蓝色或者绿色的区域内。n

图59 安全区域检测n

FB492 “MC_ZoneCheck”工艺功能允许用户在定义的区域内监测机械运动机构的运动。区域由起点及在X/Y/Z方向上的大小定义，起点定义区域相对于坐标原点的位置，在X/Y/Z方向上的大小定义区域的宽度、深度和高度。参考图60 FB492。n

图60 FB492n

使用FB492 可以定义多个区域来检测机械运动。ZoneData的数据结构根据S7-Tech库中的UDT44创建；同时必须调用UDT43。区域的坐标在基本坐标系中定义，每个区域被定义为立方体。各区域可通过变量Active使能或禁止。其中产品区域最多可以定义5个，工作区域可定义1个，运动极限区域可定义1个，禁止区域最多可定义5个，报告区域最多可以定义5个。各种区域的图示可参考图61区域定义。通过UDT44并调用UDT43来创建区域数据，参考图62 Zonedata。n

图61 区域定义n

图62 Zonedatan

3.4 路径插补附加功能块
FB482 “MC_GroupInterrupt” 中断当前路径对象的运动命令，使当前的路径运动暂停下来；FB 483 “MC_GroupContinue” 可以继续执行由FB482中断的路径命令。如图63 所示，FB482将之前的相对运动中断，而触发FB483 则可以继续执行之前的相对运动。FB 481 “MC_GroupStop” 中止当前激活的路径运动，并使涉及到的所有轴停止下来。n

图63 路径插补附加功能块n

4 KSC 运动仿真中心
路径插补功能目前可以用仿真的方式来实现。Kinematics Simulation Center (KSC，运动仿真中心)是一套软件解决方案，可用于在Windows PC机上仿真T-CPU中选择的机械运动系统。用户能在KSC中快速方便地选择所使用的机械运动系统。运行KSC软件的计算机与T-CPU站用以太网连接，T-CPU必须配备以太网通信模块CP343-1。Windows PC上必须有OpenGL 3D图形库的显卡。连接方式参考图64 KSC连接方式。n

图64 KSC连接方式n

KSC仅单向接收来自T- CPU的数据。KSC驱动FB从T-CPU路径对象中获取数据，然后通过CP343-1 传递给电脑里的KSC仿真软件。参考图65 数据传递。需要在T-CPU中调用KSC 驱动功能块FB300，下载链接请参考58260820；仿真软件的下载链接请参考 51339107。驱动功能块以项目的形式提供下载，该项目下载完成后，使用Step7软件打开，项目名为KSC-S7-Driver。参考图66 FB300 项目。n

图65 数据传递n

图66 FB300 项目n

打开该项目后，会看到很多功能块，参考图67 驱动功能块，然后将全部功能块拷贝到自己的项目中。n

图67 驱动功能块n

打开”Netpro”，建立一个TCP的连接，并记录下参数ID和LADDR，参考图68 建立连接。n

图68 建立连接

在远端IP 中填入电脑的IP 地址，两端的端口号定义为4711，参考图 69 设置IP 地址与端口号。

图 69 设置IP 地址与端口号

在OB65中调用FB300，参考图70 调用FB300。在”pathaxis”管脚填写轴的位置，最多可填四个轴；CfgConID 填写之前组态的ID 号W#16#1，CfgConLAddr 填写之前LADDR W#16#420。

图70调用FB300

双击KSC仿真软件，可得到图71的仿真画面。在软件的操作画面中点击Start Listening，参考图72 软件操作画面，启动软件检测，则仿真画面会按照程序中的设定进行相关的动作显示。

图71 仿真画面

图72 软件操作画面

关键词
T-CPU 路径插补