库卡机器人编程之干涉区间(转载)

---

前言

本文转载自公众号安德鲁机器人，欢迎前去关注此大牛。
原文中代码存在部分错误已在本文中更正。

一、干涉的定义

一.
机器人在作业过程中，或者在运行过程中，两个或两个以上的机器人同时占有同一区间位置而发生冲突叫干涉。因为干涉可能引起的后果有：
机器人的部件损坏
机器人程序混乱
加工中的工件损坏
周边其他设备设施的损坏

二、干涉区间

同一工位的机器人，在工作过程中，需要进入到同一个区域，但在进入的先后次序无严格的限定，一台机器人(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

②结构体型互锁信号S_LOCKSIG：

GLOBAL STRUC S_LOCKSIG INT ROB_IN,INT ROB_OUT

③用于诊断的当前机器人激活干涉区号zLockNumber：

GLOBAL INT zLockNumber=3

④干涉区监控的中断及循环旗帜的定义：

GLOBAL CONST INT czCollInterrupt=1  ;定义干涉区监控的中断等级
GLOBAL CONST INT czCycCollision=1   ;定义干涉区监控的循环旗帜号

⑤信号安全检测时间，与SPS扫描周期：

GLOBAL CONST REAL crDeadtime=0.200000003 ;信号安全检测时间，与SPS扫描周期

⑥干涉区互锁信号：

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

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

代码如下（示例）：

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.初始化干涉监控：

ANTI_COLLISION(#CYC_CONTROL,0 )

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)

3.进入干涉区：
-作为主机器人进入：

ANTI_COLLISION(#ENTER_LOCK_M,3)
ANTI_COLLISION(#ENTER_LOCK_M,4)  ；三台机器人同时进入该区域
ANTI_COLLISION(#ENTER_LOCK_M,5)

-作为从机器人进入：

ANTI_COLLISION(#ENTER_LOCK_S,3)
ANTI_COLLISION(#ENTER_LOCK_S,4)  ；三台机器人同时进入该区域
ANTI_COLLISION(#ENTER_LOCK_S,5)

复杂的互锁建议最好由PLC来统一处理；

4.退出干涉区：

ANTI_COLLISION(#LEAVE_LOCK,3)