热门标签
热门文章
- 1微服务、容器、云原生、Kubernetes、SOA、PaaS平台、Devops 之间的关系_paas是微服务吗
- 2【无标题】【Atlas 200DK】YOLOv3和YOLOv5部署_atlas200dk部署yolov5
- 3Python闭包：深入解析与使用场景_python闭包的应用场景
- 4PX4装机教程（四）F4飞控刷写PX4_px4教程
- 5【大道至简】机器学习算法之隐马尔科夫模型(Hidden Markov Model, HMM)详解（3）---预测问题：维特比算法（Viterbi Algorithm）详解及Python代码实现_【大道至简】机器学习算法之隐马尔科夫模型(hidden markov model, hmm)详解(2
- 6探索年入千万裂变营销模式的具体实施方法与学习要点
- 7Git使用教程（很详细）_git 操作流程
- 8IP第一次综合实验
- 9AtCoder Beginner Contest 363 B
- 10走出迷茫，在技术上持续精进
当前位置: article > 正文
库卡机器人编程之干涉区间(转载)_机器人干涉区
作者：Guff_9hys | 2024-07-25 08:04:45
赞
踩
机器人干涉区
前言
本文转载自公众号安德鲁机器人，欢迎前去关注此大牛。
原文中代码存在部分错误已在本文中更正。
一、干涉的定义
一.
机器人在作业过程中，或者在运行过程中，两个或两个以上的机器人同时占有同一区间位置而发生冲突叫干涉。因为干涉可能引起的后果有：
机器人的部件损坏
机器人程序混乱
加工中的工件损坏
周边其他设备设施的损坏
二、干涉区间
同一工位的机器人，在工作过程中，需要进入到同一个区域，但在进入的先后次序无严格的限定，一台机器人(Master)具有绝对优先的权利，即该机器人首先进入干涉区，作业完成之后另一台机器人(Slave)才可以进入该干涉区工作。任意一台机器人先进入(Master)，在工艺上都允许（除了影响运行时间外），允许使用干涉区信号对控制机器人运行，防止机器人之间碰撞。对于有严格的工艺时序的干涉，采用互锁信号来控制。这种干涉区信号设置方法如图所示。
⑴如图中，Rob2机器人为Master,Rob1机器人为Slave. Rob1和Rob2机器人在进入干涉区之前首先分别对彼此进行初始化（开启干涉区监控并释放干涉区）。
⑵机器人进入干涉区三种情形：
1、Rob1机器人进入干涉区前,发送从机欲进入干涉区的请求信号，并检测Rob2作为主机是否发送欲进入干涉区的请求信号。如果主机Rob2进入干涉区的请求信号被激活，无论此时Rob2进入或则即将进入干涉区，此时Rob1都将停止运动并等待Rob2完成运动退出干涉区。
2、如果Rob1和Rob2同时发出欲进入干涉区的请求信号，Rob2机器人优先进入，此时Rob1都将停止运动并等待Rob2完成运动退出干涉区。
3、如果Rob1作为从机已进入干涉区时，Rob2若想进入干涉区，Rob2必须停止运动并等待Rob1完成运动退出干涉区。
⑶待进入干涉区的机器人必须等待已进入干涉区的机器人完成运动退出干涉区后方可进入。
三、干涉区间互锁编程思路
创建名为Interlock的程序主要适用于存在有关联的机器人组之间互锁，该程序包含源代码SRC文件和数据文件DAT两个文件：
1.Interlock.dat数据文件说明：
①枚举类型互锁指令E_LOCKTYPE：
GLOBAL ENUM E_LOCKTYPE ENTER_LOCK_M,ENTER_LOCK_S,LEAVE_LOCK,CYC_CONTROL,RESET_LOCK
- 1
②结构体型互锁信号S_LOCKSIG：
GLOBAL STRUC S_LOCKSIG INT ROB_IN,INT ROB_OUT
- 1
③用于诊断的当前机器人激活干涉区号zLockNumber：
GLOBAL INT zLockNumber=3
- 1
④干涉区监控的中断及循环旗帜的定义：
GLOBAL CONST INT czCollInterrupt=1 ;定义干涉区监控的中断等级
GLOBAL CONST INT czCycCollision=1 ;定义干涉区监控的循环旗帜号
- 1
- 2
⑤信号安全检测时间，与SPS扫描周期：
GLOBAL CONST REAL crDeadtime=0.200000003 ;信号安全检测时间，与SPS扫描周期
- 1
⑥干涉区互锁信号：
SIGNAL giRobotInterlock $IN[89] TO $IN[104] ;许可当前机器人进入干涉区的输入信号组 SIGNAL goRobotInterlock $OUT[89] TO $OUT[104] ;当前机器人进入干涉区请求的输出信号组 DECL S_LOCKSIG TABLE_LOCK[16] ;定义机器人干涉区的互锁表 TABLE_LOCK[1]={ROB_IN 89,ROB_OUT 89} ;机器人干涉区的互锁1 TABLE_LOCK[2]={ROB_IN 90,ROB_OUT 90} ;机器人干涉区的互锁2 TABLE_LOCK[3]={ROB_IN 91,ROB_OUT 91} ;机器人干涉区的互锁3 TABLE_LOCK[4]={ROB_IN 92,ROB_OUT 92} ;机器人干涉区的互锁4 TABLE_LOCK[5]={ROB_IN 93,ROB_OUT 93} ;机器人干涉区的互锁5 TABLE_LOCK[6]={ROB_IN 94,ROB_OUT 94} ;机器人干涉区的互锁6 TABLE_LOCK[7]={ROB_IN 95,ROB_OUT 95} ;机器人干涉区的互锁7 TABLE_LOCK[8]={ROB_IN 96,ROB_OUT 96} ;机器人干涉区的互锁8 TABLE_LOCK[9]={ROB_IN 97,ROB_OUT 97} ;机器人干涉区的互锁9 TABLE_LOCK[10]={ROB_IN 98,ROB_OUT 98} ;机器人干涉区的互锁10 TABLE_LOCK[11]={ROB_IN 99,ROB_OUT 99} ;机器人干涉区的互锁11 TABLE_LOCK[12]={ROB_IN 100,ROB_OUT 100} ;机器人干涉区的互锁12 TABLE_LOCK[13]={ROB_IN 101,ROB_OUT 101} ;机器人干涉区的互锁13 TABLE_LOCK[14]={ROB_IN 102,ROB_OUT 102} ;机器人干涉区的互锁14 TABLE_LOCK[15]={ROB_IN 103,ROB_OUT 103} ;机器人干涉区的互锁15 TABLE_LOCK[16]={ROB_IN 104,ROB_OUT 104} ;机器人干涉区的互锁16
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
2.Interlock.src源程序文件说明：
①Interlock源程序包含以下子程序：
②互锁程序ANTI_COLLISION(eiLockType:IN,ziLockNum:IN)：
四、源码展示
DEF Interlock( ) END ;-------------Interlock Active---------------------- GLOBAL DEF ANTI_COLLISION(eiLockType:IN, ziLockNum:IN) DECL E_LOCKTYPE eiLockType DECL INT ziLockNum DECL BOOL nbOK CONTINUE IF VARSTATE("ziLockNum")<>#INITIALIZED THEN MsgQuit("Lock area not initialized") HALT ELSE IF ziLockNum>16 OR ziLockNum<1 THEN MsgQuit("Lock area msut be 1 to 16!") HALT ENDIF ENDIF SWITCH eiLockType ; --- Enter Collision Zone as Master --- CASE #ENTER_LOCK_M WAIT SEC 0 INTERRUPT DISABLE czCollInterrupt ; Disable Monitoring IF ($IN[TABLE_LOCK[ziLockNum].ROB_IN]) THEN $OUT[TABLE_LOCK[ziLockNum].ROB_OUT]=FALSE ELSE nbOK=FALSE MSG_ANTICOLL(ziLockNum) REPEAT IF ($IN[TABLE_LOCK[ziLockNum].ROB_IN]) THEN nbOK=TRUE ELSE nbOK=FALSE ENDIF WAIT SEC 0.01 ; Sampling 100 ms UNTIL (nbOK==TRUE) $OUT[TABLE_LOCK[ziLockNum].ROB_OUT]=FALSE ENDIF INTERRUPT ENABLE czCollInterrupt ; Enable Monitoring ; --- Enter Collsion Zone as Slave --- CASE #ENTER_LOCK_S WAIT SEC 0 INTERRUPT DISABLE czCollInterrupt ; Disable Monitoring nbOK=FALSE MSG_ANTICOLL(ziLockNum) REPEAT IF ($IN[TABLE_LOCK[ziLockNum].ROB_IN]) THEN $OUT[TABLE_LOCK[ziLockNum].ROB_OUT]=FALSE WAIT SEC crDeadtime IF ($IN[TABLE_LOCK[ziLockNum].ROB_IN]) THEN nbOK=TRUE ELSE nbOK=FALSE $OUT[TABLE_LOCK[ziLockNum].ROB_OUT]=TRUE ENDIF ENDIF WAIT SEC 0.01 ; Sampling 100 ms UNTIL (nbOK==TRUE) $OUT[TABLE_LOCK[ziLockNum].ROB_OUT]=FALSE INTERRUPT ENABLE czCollInterrupt ; Enable Monitoring ; --- Leave Collision Zone --- CASE #LEAVE_LOCK CONTINUE IF NOT $ON_PATH THEN WAIT FOR $ON_PATH ENDIF TRIGGER WHEN DISTANCE=1 DELAY=0 DO $OUT[TABLE_LOCK[ziLockNum].ROB_OUT]=TRUE ; --- Initialize Collision Zone --- CASE #RESET_LOCK WAIT SEC 0 WAIT FOR $ON_PATH $OUT[TABLE_LOCK[ziLockNum].ROB_OUT]=TRUE ; --- Prepare for permanent Monitoring--- CASE #CYC_CONTROL GLOBAL INTERRUPT DECL czCollInterrupt WHEN NOT $CYCFLAG[czCycCollision] DO STOP_ROB( ) INTERRUPT OFF czCollInterrupt $CYCFLAG[czCycCollision]=TRUE $CYCFLAG[czCycCollision]=(((goRobotInterlock B_EXOR 'B1111111111111111') B_AND giRobotInterlock) == (goRobotInterlock B_EXOR 'B1111111111111111')) INTERRUPT ON czCollInterrupt INTERRUPT ENABLE czCollInterrupt ; --- WARNING: Program Failure Interlocks --- DEFAULT HALT MsgNotify("Unknown Collision Setting!Please Switch Ti Mode!") WAIT FOR $T1 ENDSWITCH END GLOBAL DEF STOP_ROB ( ) ; *** Stop Movement by Interlock Monitoring*** BRAKE setmsg (99,3,0) WAIT FOR $CYCFLAG[czCycCollision] END GLOBAL DEF Msg_Anticoll(ziLockNum :IN) DECL INT ziLockNum DECL REAL nrTimer nrTimer=0.0 zLockNumber=ziLockNum ;Message Output setmsg (ziLockNum,1,nzTimer) $TIMER_STOP[20]=TRUE $TIMER[20]=0 $TIMER_STOP[20]=FALSE REPEAT WAIT sec .01 UNTIL ($IN[TABLE_LOCK[ziLockNum].ROB_IN])==TRUE $TIMER_STOP[20]=TRUE nrTimer=$TIMER[20] nrTimer=nrTimer/1000.0 setmsg (ziLockNum,2,nrTimer) END GLOBAL DEF SetMsg(ziLockNum:IN,ziMsg:IN,riPar:IN ) DECL KrlMsg_T Msg DECL KrlMsgPar_T Par[3] DECL KrlMsgOpt_T Opt DECL INT nzHandle DECL INT ziLockNum, ziMsg DECL REAL riPar ;Select Msssage Text--------------------- SWITCH ziMsg CASE 1 ;Interlock is Active Msg = {Modul[] "Interlock", Nr 007, Msg_txt[] "Interlock Area %1 is Active!"} CASE 2 ;Interlock was active Msg = {Modul[] "Interlock", Nr 008, Msg_txt[] "Interlock Area %1 was active %2[s] ago!"} CASE 3 ;Interlock Cycflag has triggered Msg = {Modul[] "Interlock", Nr 009, Msg_txt[] "Collision Monitoring has Triggered!"} ENDSWITCH ;Configuration of message parameter -------- Par[1] = {PAR_TYPE #VALUE, PAR_INT 0} Par[1].PAR_INT = ziLockArea Par[2] = {PAR_TYPE #VALUE, PAR_REAL 00.0} Par[2].PAR_REAL = riPar ;Message Options ------------------------------------ Opt = {Vl_STOP FALSE, CLEAR_P_RESET TRUE, CLEAR_P_SAW FALSE, LOG_TO_DB TRUE} ;Message generation--------------------------- nzHandle = SET_KRLMSG (#NOTIFY, Msg, Par[], Opt) END
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
- 43
- 44
- 45
- 46
- 47
- 48
- 49
- 50
- 51
- 52
- 53
- 54
- 55
- 56
- 57
- 58
- 59
- 60
- 61
- 62
- 63
- 64
- 65
- 66
- 67
- 68
- 69
- 70
- 71
- 72
- 73
- 74
- 75
- 76
- 77
- 78
- 79
- 80
- 81
- 82
- 83
- 84
- 85
- 86
- 87
- 88
- 89
- 90
- 91
- 92
- 93
- 94
- 95
- 96
- 97
- 98
- 99
- 100
- 101
- 102
- 103
- 104
- 105
- 106
- 107
- 108
- 109
- 110
- 111
- 112
- 113
- 114
- 115
- 116
- 117
- 118
- 119
- 120
- 121
- 122
- 123
- 124
- 125
- 126
- 127
- 128
- 129
- 130
- 131
- 132
- 133
- 134
- 135
- 136
- 137
- 138
- 139
- 140
- 141
- 142
- 143
代码如下（示例）：
import numpy as np
import pandas as pd
import matplotlib.pyplot as plt
import seaborn as sns
import warnings
warnings.filterwarnings('ignore')
import ssl
ssl._create_default_https_context = ssl._create_unverified_context
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
五、互锁程序的使用
1.初始化干涉监控：
ANTI_COLLISION(#CYC_CONTROL,0 )
- 1
2.重置并释放干涉区信号-依据自己需求：
ANTI_COLLISION(#RESET_LOCK,1)
ANTI_COLLISION(#RESET_LOCK,2)
ANTI_COLLISION(#RESET_LOCK,3)
ANTI_COLLISION(#RESET_LOCK,4)
ANTI_COLLISION(#RESET_LOCK,5)
ANTI_COLLISION(#RESET_LOCK,6)
ANTI_COLLISION(#RESET_LOCK,7)
ANTI_COLLISION(#RESET_LOCK,8)
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
3.进入干涉区：
-作为主机器人进入：
ANTI_COLLISION(#ENTER_LOCK_M,3)
ANTI_COLLISION(#ENTER_LOCK_M,4) ；三台机器人同时进入该区域
ANTI_COLLISION(#ENTER_LOCK_M,5)
- 1
- 2
- 3
-作为从机器人进入：
ANTI_COLLISION(#ENTER_LOCK_S,3)
ANTI_COLLISION(#ENTER_LOCK_S,4) ；三台机器人同时进入该区域
ANTI_COLLISION(#ENTER_LOCK_S,5)
- 1
- 2
- 3
复杂的互锁建议最好由PLC来统一处理；
4.退出干涉区：
ANTI_COLLISION(#LEAVE_LOCK,3)
- 1
- 2
声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/Guff_9hys/article/detail/879137
推荐阅读
相关标签
