CN104133424A - 一种基于可编程控制器的工业机器人直线插补方法 - Google Patents

Abstract

本发明涉及一种基于可编程控制器的工业机器人直线插补方法，该插补方法是根据所需的插补路径，按照等时间间隔插补的方式确定轮廓，然后根据工业机器人的逆运动学方程获取各关节在一个插补步长内的位移，进一步将其换算到各关节每走一步对应的脉冲数和脉冲频率，进而根据脉冲数和频率制作定位数据表，同步运行各关节，实现直线插补。本发明所提出的工业机器人直线插补方法，实现了工业机器人在运动过程中，插补轴数不受限制，插补比例系数是时变的；并且该直线插补方法对可编程控制器PLC硬件的要求较低，只需要提供运动控制最基本的高速脉冲通道以及数据表的运行方式，此外，还可以在任意结构形式的工业机器人上实现任意轴数的直线插补。

Description

一种基于可编程控制器的工业机器人直线插补方法

技术领域

本发明涉及工业机器人的插运方法，特别是一种基于可编程控制器的工业机器人直线插补方法。 

背景技术

工业机器人在搬运、码垛、焊接、包装等场合的应用越来越普遍，技术越来越成熟，但是目前通用型机器人的成本依然居高不下，因而迫切需要一些经济型的工业机器人，相比通用型的工业机器人，其功能相对单一，价格低廉。价格低决定了其不大可能选用基于PC架构的、高成本的控制器，此时选用通用的运动控制型可编程控制器PLC作为经济型工业机器人的控制器，就是一个很不错的选择。运动控制型可编程控制器PLC有专门的脉冲输出通道，可以通过对驱动器输出高速脉冲实现单轴的运动控制。通常情况下，当用户要求进行直线插补和圆弧甚至是曲线插补时，需要可编程控制器PLC提供对应的专用插补指令。但实际问题在于，对于绝大多数可编程控制器PLC，一般只提供单轴高速脉冲输出指令，较少的可编程控制器PLC机型提供直线插补指令。可编程控制器PLC自带的插补指令对使用场合有诸多限制，例如，插补轴数一般为两轴，插补只适合于关节均为直线运动的情况，即两轴间插补的脉冲比例固定不变。而对于既有移动关节又有转动关节的工业机器人，或者均为转动关节的工业机器人，如圆柱坐标、球坐标、链式坐标等形式机器人，这样的插补指令不能满足需求。此外对插补性能要求越高，可编程控制器PLC的价格就越高，这也是目前可编程控制器PLC存在的局限。 

对于如图1所示的XY直角坐标型工业机器人，其各关节均为移动关节，当需要在直线起点和终点之间实现直线运动时，X、Y两轴的脉冲频率比例是始终不变的，这种情况下的插补比较简单，可以通过带直线插补功能的可编程控制器PLC实现，但是带插补功能的可编程控制器PLC价格往往比不带插补功能的高出许多。此外，随着关节数的增加，可编程控制器PLC自带的插补功能也不能满足要求，多数可编程控制器PLC可实现的插补轴数仅为两轴。 

对于如图2所示的圆柱坐标型工业机器人，其既有移动关节又有转动关节，当需要实现在笛卡尔坐标系内的起点和终点间的直线运动时，在运动过程中的每个时刻，两个关节所需的脉冲频率却是一直在变化的，这种情况下，可编程控制器PLC自带的直线插补功能就无法满足要求。 

发明内容

本发明的目的在于提供一种基于可编程控制器的工业机器人直线插补方法，以克服现有可编程控制器PLC自身插补性能的局限性。 

为实现上述目的，本发明的技术方案是：一种基于可编程控制器的工业机器人直线插补方法，提供一工业机器人系统，该工业机器人系统包括至少M个关节，其中M为大于或等二的整数；每个关节均由任意实现圆周运动或直线运动的传动机构传动；由带高速脉冲输出通道且支持数据表运行方式的可编程控制器PLC控制所述每个关节的定位运动;其特征在于，按照以下步骤实现： 

S01：在笛卡尔坐标下，通过示教手段，根据工业机器人正运动学方程获得所述待插补直线运动的插补起点A及插补终点B的工具中心点TCP坐标，且将所述插补起点A的坐标表示为 ，将所述插补终点B的坐标表示为，同时得到所述插补起点A和所述插补终点B两点间的距离S：；

S02：将所述工具中心点TCP在所述笛卡尔坐标系下的运动速度表示为V，并得到插补运行的总时间T：；将所需的插补时间步长表示为，进而得到从所述插补起点A到所述插补终点B所需的理论插补步数：；由于存在机械臂的惯量以及高速脉冲输出口对初始脉冲频率的限制，在插补过程的首尾分别设置加速段和减速段，令加速段和减速段的时间相等，且均表示为，从而得到笛卡尔坐标系下的加速度a：，加减速段所需的插补步数：，实际插补步数应为：；其中，从第0步到第步为加速段，从第步到第步为恒速段，从第到第步为减速段；

S03：根据所述步骤S02中加减速特性相关参数计算每一插补点与所述插补起点A的距离：

，其中，所述插补点为第个插补点,也即插补过程第步插补；

S04：以所述插补起点A的坐标开始，在插补线段AB上的任一插补点在笛卡尔坐标系下的坐标，为，，，其中，，；

S05：确定每个关节的脉冲当量，其中m为关节编号,表示第m个关节，其中；并根据工业机器人逆运动学方程，分别求出每一插补点坐标对应的每个关节的角位移或线位移；且基于该位移量，求出每个关节对应的高速脉冲通道的目标脉冲数:或；将各高速脉冲通道中当前插补点的目标脉冲数与上一插补点的目标脉冲数的相减，得到各通道每插补一步需要输出的脉冲增量：；由于是等时间间隔插补，即每一步都是在内完成的，进一步得到每插补一步对应关节的脉冲频率F：；

S06：由于可编程控制器PLC在硬件上存在限制的最低脉冲频率，则以最低频率插补一步的最小脉冲量为：；若插补过程中单步的脉冲增量小于最小脉冲量，则对目标脉冲数进行补偿，即每次计算完脉冲增量，都与最小脉冲量进行比较，若出现满足的插补点，则对目标脉冲数进行补偿，补偿为最小脉冲量，并记录补偿次数；直到遇到的插补点，开始对目标脉冲数进行反向补偿，以抵消之前增加的脉冲数；

S07：每完成一个插补点，通过插补点指针i判断插补计算是否完成；若完成插补计算，跳转步骤S08，若未完成则跳转所述步骤S03，计算下一插补点；

S08：定位数据表包括控制代码、脉冲频率和目标脉冲数三部分，根据不同的可编程控制器PLC数据表指令，将对应计算出来的脉冲频率和目标脉冲值按照顺序提取出来，并确定控制代码，分别生成特定格式的定位数据表，以控制各高速脉冲通道按照各自的定位数据表同步输出高速脉冲，实现直线插补。

在本发明一实施例中，所述的基于可编程控制器的工业机器人直线插补方法适用于任意数量的可编程控制器PLC高速脉冲输出通道,并且适用于任意实现圆周运动或直线运动的传动机构。 

在本发明一实施例中，所述基于可编程控制器的工业机器人直线插补方法在插补运动开始之前已完成插补计算并生成定位数据表。 

相较于现有技术，本发明具有以下有益效果： 

1. 本发明所提出的工业机器人直线插补方法对可编程控制器PLC自身的插补特性没有任何要求，可编程控制器PLC只需要提供运动控制最基本的高速脉冲通道以及数据表的运行方式即可，可适用于任意数量的可编程控制器PLC高速脉冲输出通道，并且适用于任意实现圆周运动或直线运动的传动机构，极大地降低开发经济型工业机器人的硬件成本；

2. 本发明所提出的工业机器人直线插补方法是依据机器人的正、逆运动学方程，进行坐标与各关节位移的转化的，因而插补轴数不受限制，在任意结构形式的工业机器人上均可实现任意轴数的直线插补，可移植性好，具有极高的推广价值；

3. 本发明所提出的工业机器人直线插补方法是一种等时间间隔的插补，可编程控制器通过定时器即可实现等时间步长的控制，编程更容易；

4. 本发明所提出的工业机器人直线插补方法的精度取决于插补步长的大小，插补步长越小，其精度越高。

附图说明

图1为XY直角坐标型工业机器人的结构图。 

图2为圆柱坐标型工业机器人的结构图。 

图3为本发明中一种基于可编程控制器的工业机器人直线插补方法的流程图。 

注：1-轴1 

2-轴2。

具体实施方式

下面结合附图，对本发明的技术方案进行具体说明。 

本发明涉及一种基于可编程控制器的工业机器人直线插补方法，提供一工业机器人系统，该工业机器人系统包括至少M个关节，其中M为大于或等于二的整数；每个关节均由能任意实现圆周运动或直线运动的传动机构传动，包括驱动器、电机、减速机或者丝杠导轨；由带高速脉冲输出通道且支持数据表运行方式的可编程控制器PLC控制所述每个关节的定位运动;在本实施例中所采用的经济型工业机器人控制系统的机械臂本体为两轴圆柱坐标型结构，结构简图如图2所示，可编程控制器PLC选用松下可编程控制器PLC：FP-X C60T，可编程控制器PLC自带两路高速脉冲通道(100kHz)和两路中速通道(20kHz)，伺服电机和驱动器选用松下MSMD系列，其中轴1，即本实施例中的关节1，选用行星减速机进行减速，驱动机械臂做圆周运动，轴2，即本实施例中的关节2，伺服电机与滚珠丝杠直连，驱动机械臂末端做直线运动；其特征在于，如图3所示，按照以下步骤实现： 

S01：确定待插补直线端点的坐标及长度：在笛卡尔坐标下，通过示教手段，根据工业机器人正运动学方程获得所述待插补直线运动的插补起点A及插补终点B的工具中心点TCP坐标，且将所述插补起点A的坐标表示为，将所述插补终点B的坐标表示为，同时得到所述插补起点A和所述插补终点B两点间的距离S：；在本实施例中，在笛卡尔坐标下建立工业机器人正运动学方程，轴1相对原点的角位移为，轴2相对原点的线位移为，则工业机器人正运动学方程为：，通过示教获得插补起点A及插补终点B的工具中心点TCP坐标，取插补起点A点坐标为为，插补终点B点坐标为，则容易求得插补起点A、插补终点B两点间的距离S：。

S02：确定插补步数和加减速相关特性参数：将所述工具中心点TCP在所述笛卡尔坐标系下的运动速度表示为V，并得到插补运行的总时间T：；根据实际情况选择所需的插补时间步长，并表示为，进而得到从所述插补起点A到所述插补终点B所需的理论插补步数：；在本实施例中，将运动速度V设置为，插补的时间步长，则可得，；由于存在机械臂的惯量以及高速脉冲输出口对初始脉冲频率的限制，在插补过程的首尾分别设置加速段和减速段，这样,实际的插补步数应大于理论插补步数；令加速段和减速段的时间相等，且均表示为为，在本实施例中，取加减速时间；从而可以得到笛卡尔坐标系下的加速度a：，即在本实施例中加速度；加减速段所需的插补步数：，实际插补步数应为：，即在本实施例中，加减速段所需的插补步数，实际插补步数；其中，从第0步到第步为加速段，从第步到第步为恒速段，从第到第步为减速段，即在本实施例中从第1步到第4步为加速段，从第5步到第20步为恒速段，从第21步到第24步为减速段。 

S03：确定每一插补点与所述插补起点A之间的距离：根据所述步骤2中加减速特性相关参数计算每一插补点与所述插补起点A的距离： 

，其中，所述插补点为第个插补点,也即插补过程第步插补；在本实施例中，根据步骤S02中的计算结果可得：，具体计算结果请参见表1。

S04：确定每一插补点的坐标：将所述插补起点A的坐标与每一插补点距所述插补起点A的距离在对应方向上的分量叠加，得到每一插补点在笛卡尔坐标系下的坐标，即，，，其中，，；具体计算结果请参见表1。 

 表1 

 

注：的点表示插补起点。

S05：根据脉冲当量计算每一插补段的脉冲频率和目标脉冲数：根据传动机构的结构形式以及驱动器的相关参数确定每个关节的脉冲当量，其中m为关节编号,表示第m个关节，其中，在本实施例中，；在本实施例中，所述传动机构的结构形式包括作圆周运动的传动机构和作直线运动的传动机构，且对于作圆周运动的传动机构，对应驱动器的参数包括伺服驱动器上设置的电子齿轮比D、编码器脉冲数E、减速器的减速比R和减速器输出一圈对应末端的角度Q；对于作直线运动的传动机构，对应驱动器的参数包括伺服驱动器上设置的电子齿轮比D、编码器脉冲数E、减速器的减速比R和丝杠导程L；在本实施例中，轴1，即关节1，对应驱动器的参数设置为：，，，其对应的脉冲当量：；轴2，即关节2，对应驱动器的参数设置为：，，，其对应的脉冲当量：； 

当可编程控制器PLC输出一个定位控制脉冲时，所产生的定位控制移动的位移，对直线运动来说，是指移动的距离，即线位移，对圆周运动来说，是指其转动的角度，即角位移；根据工业机器人逆运动学方程，分别求出每一插补点坐标对应的每个关节的角位移或线位移；在本实施例中，根据工业机器人逆运动学方程，分别求出每一插补点坐标对应的每个关节位移和：；且基于该位移量，求出每个关节对应的高速脉冲通道的目标脉冲数:或，在本实施例中，对应目标脉冲数为：；将各高速脉冲通道中当前插补点的目标脉冲数与上一插补点的目标脉冲数的相减，得到各通道每插补一步需要输出的脉冲增量：，在本实施例中，对应脉冲增量分别为：；由于是等时间间隔插补，即每一步都是在内完成的，进一步得到每插补一步对应关节的脉冲频率F：，在本实施例中，对应的脉冲频率分别为：，具体计算结果请参见表2。

S06：对目标脉冲数补偿：由于可编程控制器PLC在硬件上存在限制的最低脉冲频率，则以最低频率插补一步的最小脉冲量为：；若插补过程中单步的脉冲增量小于最小脉冲量，则对目标脉冲数进行补偿，否则定位数据表无法正常执行，即每次计算完脉冲增量，都与最小脉冲量进行比较，若出现满足的插补点，则对目标脉冲数进行补偿，补偿为最小脉冲量，并记录补偿次数；直到遇到的插补点，开始对目标脉冲数进行反向补偿，以抵消之前增加的脉冲数；在本实施例中，对于松下PLC，当选择脉冲频率的范围为48Hz～100kHz时，最低脉冲频率，则以最低频率插补一步的最小脉冲量为：，本实施例中，虽然出现了一个脉冲增量绝对值小于最小脉冲量的插补点，但该点属于步骤S08中所述的特殊情况，故本实施例中暂不做相应的补偿。 

S07：判断插补运算是否完成：每完成一个插补点，通过插补点指针i判断插补计算是否完成；若完成插补计算，跳转步骤S08，若未完成则跳转所述步骤S03，计算下一插补点；在本实施例中，每执行完一个插补点，插补点指针；然后判断插补指针i是否大于实际插补步数24，即判断是否成立，当时，表示插补计算完成，跳转步骤S08，否则始终跳转步骤S03，计算下一插补点。 

S08：制作定位数据表：定位数据表包括控制代码、脉冲频率和目标脉冲数三部分，根据不同的可编程控制器PLC数据表指令，将对应计算出来的脉冲频率和目标脉冲值按照顺序提取出来，并确定控制代码，分别生成特定格式的定位数据表，以控制各高速脉冲通道按照各自的定位数据表同步输出高速脉冲，实现直线插补。在本实施例中，存在一个特殊的情况是：在执行特定的直线插补时，可能出现某一关节电机需要先正转再反转，或者先反转再正转的情况，而一般可编程控制器PLC的数据表指令无法在运行中更改旋转方向，因而在“换向”时，建立新的定位数据表，覆盖原数据表的后半段以保证直线插补的顺利执行； 

具体的，在本实施例中，轴1，即关节1，采用增量式，脉冲增量值均为正数，故采用加计数，默认选用48Hz～100kHz的脉冲频率范围，控制代码为16#1101，紧接着依次写入每插补一步的脉冲频率和脉冲增量，即形成关节1的定位数据表，结束定位数据表写入“0”即可；关节2采用绝对式，注意前半段的目标脉冲值顺序递减，而后半段的目标脉冲值顺序递增，即定位过程中，关节2长度先减小再增大，因而需要分段制作定位数据表，关节2的第一张数据表的控制代码为16#1110，第二张的控制代码为16#1111，先将每插补一步的脉冲频率和目标脉冲数依次写入到第一张定位数据表，在需要“换向”处，建立一个新的定位数据表，将“换向”后部分的脉冲频率和目标脉冲数依次写入到第二张数据表中，同时，将第一张数据表对应位置的脉冲频率和目标脉冲值清零，避免误动作，即形成关节2的定位数据表，结束定位数据表写入“0”即可；控制两个脉冲输出按照各自的定位数据表同步输出高速脉冲，即可实现直线插补。

 表2 

 

注：的点表示插补起点。

插补误差分析：插补误差是直线插补方法的一个重要指标,不同的插补方法，插补的位置误差大小也不一样。本实施例中的插补直线插补方法造成位置误差主要原因有： 

（1）理论上，本实施例中将每个插补点的目标值与坐标值一一对应，累积误差较小，但是数据运算过程中的四舍五入，将会不可避免地带来一定的位置误差；

（2）以折线替代插补的直线，因而插补时间步长的大小将影响插补误差值的大小；

（3）可编程控制器PLC硬件上限制的最低脉冲频率，当出现这种情况时，需要做脉冲数的补偿，以保证可编程控制器PLC脉冲输出通道正常输出高速脉冲，这种人为引入的补偿量将会引起各轴插补不同步，造成一定的插补误差。

误差补偿机制：在插补完成前，实时计算定位误差，判断误差是否处于合理范围内并及时修正；指定目标值时采用绝对式，尽量减少误差累积。 

以上是本发明的较佳实施例，凡依本发明技术方案所作的改变，所产生的功能作用未超出本发明技术方案的范围时，均属于本发明的保护范围。 

Claims (3)

1.一种基于可编程控制器的工业机器人直线插补方法，提供一工业机器人系统，该工业机器人系统包括至少M个关节，其中M为大于或等二的整数；每个关节均由任意实现圆周运动或直线运动的传动机构传动；由带高速脉冲输出通道且支持数据表运行方式的可编程控制器PLC控制所述每个关节的定位运动;其特征在于，按照以下步骤实现：

S01：在笛卡尔坐标下，通过示教手段，根据工业机器人正运动学方程获得所述待插补直线运动的插补起点A及插补终点B的工具中心点TCP坐标，且将所述插补起点A的坐标表示为                                                ，将所述插补终点B的坐标表示为，同时得到所述插补起点A和所述插补终点B两点间的距离S：；

S02：将所述工具中心点TCP在所述笛卡尔坐标系下的运动速度表示为V，并得到插补运行的总时间T：；将所需的插补时间步长表示为，进而得到从所述插补起点A到所述插补终点B所需的理论插补步数：；由于存在机械臂的惯量以及高速脉冲输出口对初始脉冲频率的限制，在插补过程的首尾分别设置加速段和减速段，令加速段和减速段的时间相等，且均表示为，从而得到笛卡尔坐标系下的加速度a：，加减速段所需的插补步数：，实际插补步数应为：；其中，从第0步到第步为加速段，从第步到第步为恒速段，从第到第步为减速段；

S03：根据所述步骤S02中加减速特性相关参数计算每一插补点与所述插补起点A的距离：

，其中，所述插补点为第个插补点,也即插补过程第步插补；

S04：以所述插补起点A的坐标开始，在插补线段AB上的任一插补点在笛卡尔坐标系下的坐标，为，，，其中，，；

S05：确定每个关节的脉冲当量，其中m为关节编号,表示第m个关节，其中；并根据工业机器人逆运动学方程，分别求出每一插补点坐标对应的每个关节的角位移或线位移；且基于该位移量，求出每个关节对应的高速脉冲通道的目标脉冲数:或；将各高速脉冲通道中当前插补点的目标脉冲数与上一插补点的目标脉冲数的相减，得到各通道每插补一步需要输出的脉冲增量：；由于是等时间间隔插补，即每一步都是在内完成的，进一步得到每插补一步对应关节的脉冲频率F：；

S06：由于可编程控制器PLC在硬件上存在限制的最低脉冲频率，则以最低频率插补一步的最小脉冲量为：；若插补过程中单步的脉冲增量小于最小脉冲量，则对目标脉冲数进行补偿，即每次计算完脉冲增量，都与最小脉冲量进行比较，若出现满足的插补点，则对目标脉冲数进行补偿，补偿为最小脉冲量，并记录补偿次数；直到遇到的插补点，开始对目标脉冲数进行反向补偿，以抵消之前增加的脉冲数；

S07：每完成一个插补点，通过插补点指针i判断插补计算是否完成；若完成插补计算，跳转步骤S08，若未完成则跳转所述步骤S03，计算下一插补点；

S08：定位数据表包括控制代码、脉冲频率和目标脉冲数三部分，根据不同的可编程控制器PLC数据表指令，将对应计算出来的脉冲频率和目标脉冲值按照顺序提取出来，并确定控制代码，分别生成特定格式的定位数据表，以控制各高速脉冲通道按照各自的定位数据表同步输出高速脉冲，实现直线插补。

2.根据权利要求1所述的一种基于可编程控制器的工业机器人直线插补方法，其特征在于：所述方法适用于任意数量的可编程控制器PLC高速脉冲输出通道,并且适用于任意实现圆周运动或直线运动的传动机构。

3.根据权利要求1所述的一种基于可编程控制器的工业机器人直线插补方法，其特征在于：所述方法在插补运动开始之前已完成插补计算并生成定位数据表。