工业机器人多机协同位置轨迹速度预处理方法技术

本发明专利技术的目的在于提供一种工业机器人多机协同位置轨迹速度预处理方法，在PC上设置包含有多个机器人子线程的多机控制系统，每个机器人子线程都设有：控制指令存储区、译码模块、轨迹数据缓冲区、协同数据缓冲区、速度预处理模块、速度信息存储模块、运动控制模块、粗插补数据缓冲区、以及数据处理模块；每个机器人子线程都有与之对应的硬件层可编程I/O接口卡用以连接一台机器人；机器人子线程与机器人子线程之间，子线程与各自的可编程I/O接口卡之间，通过协同指令实现多机协同工作。通过协同指令实现多机协同工作。通过协同指令实现多机协同工作。

【技术实现步骤摘要】
工业机器人多机协同位置轨迹速度预处理方法


[0001]本专利技术属于工业机器人运动控制
，具体涉及一种工业机器人多机协同位置轨迹速度预处理方法。

技术介绍

[0002]在计算机技术、网络技术等新技术的支持下，机器人技术发展迅猛，基于PC平台，采用“PC+”的控制模式进行相关机器人控制系统的研发具有较大优势，为合理利用计算机资源，可基于“PC机+可编程I/O接口”的控制模式开发相应的多机器人控制系统，为确保多机器人之间能协同进行作业，需要保证底层控制算法可以合理的协调运动轨迹控制指令与协同指令。

技术实现思路

[0003]本专利技术从速度规划出发，提出一种基于“PC机+可编程I/O接口”的工业机器人多机协同位置轨迹速度预处理方法。实现多机系统每台机器人在兼顾运动轨迹控制指令和多机协同指令的条件下，以允许的最大速度完成多轨迹段的连续运行。
[0004]其主要方案包括译码模块从控制指令存储区提取出控制指令，并根据指令的不同，将控制指令信息存入轨迹数据缓冲区或协同数据缓冲区。开始进行速度预处理时，速度预处理模块使用倒序法从轨迹数据缓冲区中提取出各轨迹段的轨迹数据，并设定该轨迹段对应的速度信息，设定完成后将每条轨迹段的速度信息存入速度信息存储模块。使用倒序法完成第一次速度预处理后，速度预处理模块按照速度信息存储顺序从速度信息存储模块中取出每段轨迹段的速度信息，依次完成速度微调，并将速度信息存入速度信息存储模块，实现速度信息的第二次预处理。预处理完成后，运动控制模块从速度信息存储模块中取出每段轨迹段的速度信息，并根据每段轨迹段速度信息中对应的指令类型不同，输出相应的粗插补数据到粗插补数据缓冲区，数据处理模块从粗插补数据缓冲区中依次取出每段轨迹段的粗插补数据，结合协同数据缓冲区中协同指令的协同数据完成多机协同控制。
[0005]其包括译码模块对运动指令和协同指令的译码，并将指令信息以结构体的形式存入轨迹段缓冲区，并初始化速度信息数组vel_arr[N]，速度预处理模块从轨迹段缓冲区反向读取指令，完成第一次速度预处理，并将数据存入vel_arr[N]，之后正向读取vel_arr[N]中每一段轨迹段速度信息，完成速度微调，实现在保证加减速距离足够并且兼顾多机协同指令的条件下，坚持时间最优原则，以允许的最大速度完成多轨迹段的连续运行。
[0006]本专利技术解决其技术问题具体采用的技术方案是：
[0007]一种工业机器人多机协同位置轨迹速度预处理方法，其特征在于：
[0008]在PC上设置包含有多个机器人子线程的多机控制系统，每个机器人子线程都设有：控制指令存储区、译码模块、轨迹数据缓冲区、协同数据缓冲区、速度预处理模块、速度信息存储模块、运动控制模块、粗插补数据缓冲区、以及数据处理模块；每个机器人子线程都有与之对应的硬件层可编程I/O接口卡用以连接一台机器人；机器人子线程与机器人子
线程之间，子线程与各自的可编程I/O接口卡之间，通过协同指令实现多机协同工作：
[0009]机器人子线程开始运行时，所述译码模块从控制指令存储区提取出控制指令，并根据指令的不同，将控制指令信息存入轨迹数据缓冲区或协同数据缓冲区；
[0010]开始进行速度预处理时，为实现在兼顾运动轨迹控制指令与多机协同指令的条件下，以允许的最大速度完成多轨迹段的连续运行，所述速度预处理模块使用倒序法，即：使用与轨迹数据存储顺序相反的顺序从轨迹数据缓冲区中提取出轨迹段的轨迹数据，依次往前预读，根据每段轨迹段对应的指令类型不同，初步设定该轨迹段对应的速度信息，并判断该轨迹段前后轨迹段对应的指令类型，根据指令类型再次设定该轨迹段的速度信息；设定完成由速度预处理模块将速度信息存入速度信息存储模块；
[0011]使用倒序法完成第一次速度预处理后，所述速度预处理模块按照速度信息存储顺序从速度信息存储模块中取出每段轨迹段的速度信息，依次完成速度微调，并将速度信息存入速度信息存储模块，实现速度信息的第二次预处理；
[0012]完成预处理后，所述运动控制模块按照速度信息存储顺序从速度信息存储模块中取出每段轨迹段的速度信息，并根据每段轨迹段速度信息中对应的指令类型不同，输出相应的粗插补数据到粗插补数据缓冲区，数据处理模块按照粗插补数据存储顺序从粗插补数据缓冲区中依次取出每段轨迹段的粗插补数据，结合协同数据缓冲区中协同指令的协同数据完成多机协同控制。
[0013]其实现在保证加减速距离足够并且兼顾多机协同指令的条件下，坚持时间最优原则，以允许的最大速度完成多轨迹段的连续运行。
[0014]多机控制系统在PC机软件层包含有多个机器人子线程，每个机器人子线程都包含有上述模块、缓冲区，每个机器人子线程都有与之对应的硬件层可编程I/O接口卡，每个机器人子线程可称为一台机器人。机器人子线程与机器人子线程之间，子线程与各自的可编程I/O接口卡之间，通过协同指令实现多机协同工作。
[0015]进一步地，在完成各轨迹段速度信息的预处理后，运动控制模块按照速度信息存储顺序从速度信息存储模块中取出每段轨迹段的速度信息，依次完成运动规划，根据每段轨迹段速度信息中对应的指令类型不同：
[0016]如果是运动轨迹控制指令，则对其进行运动规划，并输出相应的粗插补数据到粗插补数据缓冲区，粗插补数据包括：轨迹段编号、指令类型、机器人各关节轴脉冲数；
[0017]如果是协同指令，不进行运动规划，直接输出粗插补数据，包括轨迹段编号、指令类型、机器人各关节轴脉冲数，其中各关节轴脉冲数为0；数据处理模块按照粗插补数据存储顺序从粗插补数据缓冲区中依次取出每段轨迹段的粗插补数据，如果粗插补数据对应的指令类型是运动轨迹控制指令，则将粗插补数据中的各关节脉冲数发送至可编程I/O接口卡控制机器人运动，如粗插补数据对应的指令类型是协同指令，则在协同数据缓冲区中进行索引，取出缓冲区中协同指令编号与该轨迹段粗插补数据中的轨迹段编号相同的协同指令，根据指令类型不同：
[0018]如果是SET指令，则发送相应的信号编号各对应的机器人子线程，
[0019]如果是WAIT指令则等待相应的机器人子线程的协同信号的到来。
[0020]进一步地，多机系统中每台机器人的控制程序由开始指令NOP、运动轨迹控制指令、协同指令和结束指令END构成；
[0021]其中所述运动轨迹控制指令包括：MOVL和MOVC指令；单条MOVL指令代表一段直线轨迹段，每条MOVL指令都包含有直线轨迹的末端点位置矢量数据P
E
、轨迹段内允许最大速度V
m
；每条MOVC指令都包含有圆弧轨迹的末端点位置矢量数据P
E
、轨迹段内允许最大速度V
m
，两条MOVC指令代表一段圆弧轨迹段；
[0022]所述协同指令包括SET和WAIT指令；SET指令用于发送协同信号，每条SET指令都包含有接收协同信号的机器人子线程编号r_num及其相应的信号编号s_num；WAIT指令用于等待协同信号，每条WAI本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种工业机器人多机协同位置轨迹速度预处理方法，其特征在于：在PC上设置包含有多个机器人子线程的多机控制系统，每个机器人子线程都设有：控制指令存储区、译码模块、轨迹数据缓冲区、协同数据缓冲区、速度预处理模块、速度信息存储模块、运动控制模块、粗插补数据缓冲区、以及数据处理模块；每个机器人子线程都有与之对应的硬件层可编程I/O接口卡用以连接一台机器人；机器人子线程与机器人子线程之间，子线程与各自的可编程I/O接口卡之间，通过协同指令实现多机协同工作：机器人子线程开始运行时，所述译码模块从控制指令存储区提取出控制指令，并根据指令的不同，将控制指令信息存入轨迹数据缓冲区或协同数据缓冲区；开始进行速度预处理时，为实现在兼顾运动轨迹控制指令与多机协同指令的条件下，以允许的最大速度完成多轨迹段的连续运行，所述速度预处理模块使用倒序法，即：使用与轨迹数据存储顺序相反的顺序从轨迹数据缓冲区中提取出轨迹段的轨迹数据，依次往前预读，根据每段轨迹段对应的指令类型不同，初步设定该轨迹段对应的速度信息，并判断该轨迹段前后轨迹段对应的指令类型，根据指令类型再次设定该轨迹段的速度信息；设定完成由速度预处理模块将速度信息存入速度信息存储模块；使用倒序法完成第一次速度预处理后，所述速度预处理模块按照速度信息存储顺序从速度信息存储模块中取出每段轨迹段的速度信息，依次完成速度微调，并将速度信息存入速度信息存储模块，实现速度信息的第二次预处理；完成预处理后，所述运动控制模块按照速度信息存储顺序从速度信息存储模块中取出每段轨迹段的速度信息，并根据每段轨迹段速度信息中对应的指令类型不同，输出相应的粗插补数据到粗插补数据缓冲区，数据处理模块按照粗插补数据存储顺序从粗插补数据缓冲区中依次取出每段轨迹段的粗插补数据，结合协同数据缓冲区中协同指令的协同数据完成多机协同控制。2.根据权利要求1所述的工业机器人多机协同位置轨迹速度预处理方法，其特征在于：完成各轨迹段速度信息的预处理后，运动控制模块按照速度信息存储顺序从速度信息存储模块中取出每段轨迹段的速度信息，依次完成运动规划，根据每段轨迹段速度信息中对应的指令类型不同：如果是运动轨迹控制指令，则对其进行运动规划，并输出相应的粗插补数据到粗插补数据缓冲区，粗插补数据包括：轨迹段编号、指令类型、机器人各关节轴脉冲数；如果是协同指令，不进行运动规划，直接输出粗插补数据，包括轨迹段编号、指令类型、机器人各关节轴脉冲数，其中各关节轴脉冲数为0；数据处理模块按照粗插补数据存储顺序从粗插补数据缓冲区中依次取出每段轨迹段的粗插补数据，如果粗插补数据对应的指令类型是运动轨迹控制指令，则将粗插补数据中的各关节脉冲数发送至可编程I/O接口卡控制机器人运动，如粗插补数据对应的指令类型是协同指令，则在协同数据缓冲区中进行索引，取出缓冲区中协同指令编号与该轨迹段粗插补数据中的轨迹段编号相同的协同指令，根据指令类型不同：如果是SET指令，则发送相应的信号编号各对应的机器人子线程，如果是WAIT指令则等待相应的机器人子线程的协同信号的到来。3.根据权利要求1所述的工业机器人多机协同位置轨迹速度预处理方法，其特征在于：多机系统中每台机器人的控制程序由开始指令NOP、运动轨迹控制指令、协同指令和结
束指令END构成；其中所述运动轨迹控制指令包括：MOVL和MOVC指令；单条MOVL指令代表一段直线轨迹段，每条MOVL指令都包含有直线轨迹的末端点位置矢量数据P
E
、轨迹段内允许最大速度V
m
；每条MOVC指令都包含有圆弧轨迹的末端点位置矢量数据P
E
、轨迹段内允许最大速度V
m
，两条MOVC指令代表一段圆弧轨迹段；所述协同指令包括SET和WAIT指令；SET指令用于发送协同信号，每条SET指令都包含有接收协同信号的机器人子线程编号r_num及其相应的信号编号s_num；WAIT指令用于等待协同信号，每条WAIT指令都包含有触发协同信号的机器人子线程编号r_num及其相应的信号编号s_num；在执行SET和WAIT指令时，机器人处于停止状态；所述轨迹数据缓冲区包含有每段轨迹段的轨迹数据变量，包括：轨迹段编号、指令类型、初始位置点矢量数据P
Start
，中间位置点矢量数据P
Middle
，末端位置点矢量数据P
End
，轨迹段内允许最大速度V
Max
；协同数据缓冲区包含有每条协同指令的协同数据变量，包括：协同指令编号，机器人子线程编号R_num，信号编号S_num；粗插补数据缓冲区包含有每段轨迹段的粗插补数据变量，包括：轨迹段编号、指令类型、机器人各关节轴脉冲数；所述速度信息存储模块存储着每段轨迹段的速度信息，速度信息包含的变量如下：轨迹段编号、指令类型、初始位置点矢量数据P
Start
，中间位置点矢量数据P
Middle
，末端位置点矢量数据P
End
，初始2次Besizer过渡曲线的控制点矢量数据P
0s
、P
1s
、P
2s
及相应的节点参数变量t
s
，初始过渡弧长变量L
s
，初始过渡总弧长变量L
z
，中间段弧长变量L
r
，末端2次Besizer过渡曲线的控制点矢量数据P
0e
、P
1e
、P
2e
及相应节点参数变量t
e
，末端过渡弧长L
e
，以及初速度变量V
s
，初速度上下界变量V
min
,V
max
，末速度变量V
e
，用vel_arr[i]代表段轨迹段i的速度信息。4.根据权利要求3所述的工业机器人多机协同位置轨迹速度预处理方法，其特征在于：译码开始时，译码模块从控制指令存储区提取出控制指令，并根据指令的不同，将控制指令信息存入轨迹数据缓冲区或协同数据缓冲区：如果指令是NOP指令，将该指令包含的该机器人子线程控制程序第一个位置点矢量数据P
S
赋值给全局临时位置矢量P
temp
；如果是MOVL指令，则设置该轨迹段的轨迹数据，包括：设定轨迹段编号，设定指令类型为MOVL，将P
temp
赋值给轨迹数据中的初始位置点矢量数据P
Start
，轨迹数据中的中间位置点矢量数据P
Middle
赋值为0，将MOVL指令包含的P
E
赋值给轨迹数据中的末端位置点矢量数据P
End
和临时位置矢量P
temp
，将MOVL指令包含的V
m
赋值给轨迹数据中的轨迹段内允许最大速度V
Max
，将该轨迹段的轨迹数据存入轨迹数据缓冲区；如果是MOVC指令，需要再提取一条指令，下一条指令也是MOVC，因为两条MOVC指令共三个点可唯一确定一段圆弧轨迹段，设置该轨迹段的轨迹数据，包括：设定轨迹段编号，设定指令类型为MOVC，将P
temp
赋值给轨迹数据中的初始位置点矢量数据P
Start
，将第一条MOVC中的P
E
赋值给轨迹数据中的中间位置点矢量数据P
Middle
，将第二条MOVC中的P
E
赋值给轨迹数据中的末端位置点矢量数据P
End
和临时位置矢量P
temp
，将第二条MOVC指令包含的V
m
赋值给轨迹数据中的轨迹段内允许最大速度V
Max
，将该轨迹段的轨迹数据存入轨迹数据缓冲区；如果是SET指令，设置该轨迹段的轨迹数据，包括：设定轨迹段编号，设定指令类型为SET，其他的P
Start
、P
Middle
、P
End
、V
Max
全部赋值为0，并设置该协同指令的协同数据，将该轨迹段的轨迹段编号赋值给协同数据中的协同指令编号，将SET中包含的r_num赋值给协同数据中
的机器人子线程编号R_num，将SET中包含的s_num赋值给协同数据中的信号编号S_num，将轨迹数据和协同数据分别送入轨迹数据缓冲区和协同数据缓冲区；根据以上的译码方式，完成对所有控制指令的译码。5.根据权利要求4所述的工业机器人多机协同位置轨迹速度预处理方法，其特征在于：第一次速度预处理时，速度预处理模块使用倒序法，即：使用与轨迹数据存储顺序相反的顺序从轨迹数据缓冲区中提取出轨迹段的轨迹数据，依次往前预读并进行速度信息设定；其中，速度信息设定方法如下：假设当前取出的是轨迹段i，需要判断轨迹段i对应的指令类型，如果指令类型是协同指令，则将轨迹段i包含的轨迹数据一一对应的赋值给轨迹段i的速度信息vel_arr[i]中包含的轨迹段编号、指令类型，vel_arr[i]中的其他变量全部赋值为0，赋值完成后由速度预处理模块将该轨迹段的速度信息vel_arr[i]存入速度信息存储模块；如果是运动轨迹控制指令，则初步设定该轨迹段的速度信息vel_arr[i]，将轨迹段i包含的轨迹数据一一对应的赋值给vel_arr[i]中包含的轨迹段编号、指令类型、初始位置点矢量数据P
Start
，中间位置点矢量数据P
Middle
，末端位置点矢量数据P
End
；初步完成设定之后需要判断该轨迹段后面和前面轨迹段对应的指令类型，根据后面轨迹段i+1和前面轨迹段轨迹段i
‑
1对应的指令类型不同设定轨迹段i对应的速度信息vel_arr[i]中的末端速度信息和初始速度信息；根据后面轨迹段i+1对应的指令类型设置速度信息vel_arr[i]中的末端速度信息：如果轨迹段i+1对应的指令类型是协同指令或者轨迹段i+1不存在，则轨迹段i的vel_arr[i]中末端速度信息的设定方法如下：将轨迹段i的速度信息vel_arr[i]中包含的末端2次Besizer过渡曲线的控制点矢量数据及相应节点参数变量t
e
，末端过渡弧长L
e
以及末速度变量V
e
全部赋值为0；如果轨迹段i+1对应的指令类型是运动轨迹控制指令，则读取轨迹段i+1对应的速度信息vel_arr[i+1]，提取vel_arr[i+1]中的初始速度信息如下：初始2次Besizer过渡曲线的控制点矢量数据P
0s
、P
1s
、P
2s
及相应的节点参数t
s
，初始过渡弧长L
s
，初始过渡总弧长L
z
以及初速度V
s
，将vel_arr[i+1]的矢量速度P
0s
、P
1s
、P
2s
、节点参数t
s
、弧长差L
z
‑
L
s
，初速度V
s
一一赋值给vel_arr[i]中包含的末端速度信息；根据前面轨迹段i
‑
1对应的指令类型设置设置速度信息vel_arr[i]中的初始速度信息：如果轨迹段i
‑
1对应的指令类型是协同指令或者轨迹段i
‑
1不存在，则轨迹段i的vel_arr[i]中初始速度信息的设定方法如下：将轨迹段i的速度信息vel_arr[i]中包含的几个初始变量：初始2次Besizer过渡曲线的控制点矢量数据据P
0s
、P
1s
、P
2s
及相应的节点参数变量t
s
，初始过渡弧长变量L
s
，初始过渡总弧长变量L
z
、初速度上下界变量V
min
,V
max
以及初速度变量V
s
全部赋值为0，并判断轨迹段i对应的运动轨迹控制指令类型，如果是MOVL指令，通过该轨迹段的初始位置点矢量数据P
Start
和末端位置点矢量数据P
End
两点计算直线轨迹段的长度line，并将line
‑
L
e
‑
L
s
的值赋值给vel_arr[i]中的中间段弧长变量L
r
，其中L
e
...

【专利技术属性】
技术研发人员：林述温，陈伟鹏，
申请(专利权)人：福州大学，
类型：发明
国别省市：