KR20000008600A - 로봇 위치서보시스템의 충돌감시장치 - Google Patents

Abstract

본 발명은 로봇 위치서보시스템의 충돌감시장치에 관한 것으로, 목표속도산출부를 통해 상위제어부로부터 출력된 위치명령이 미분되어 목표속도가 구해지고 목표토오크산출부를 통해 상기 목표속도에 미리 설정된 모터와 로봇의 관성모멘트의 합이 곱해진 다음 미분되어 목표토오크가 구해지며 목표전류산출부를 통해 상기 목표토오크가 명령토오크상수로 나누어져 목표전류가 구해지며 전류편차산출부를 통해 상기 목표전류와 실제 모터에 인가되는 전류명령과의 전류편차가 구해져 이 전류편차가 비교부를 통해 미리 설정된 전류편차와 비교되어 그 비교결과에 따라 상기 비교부로부터 상기 상위제어부로 충돌신호가 출력되며 상기 비교부로부터 충돌신호가 입력되면 상기 상위제어부로부터 구동중지명령이 출력되어 모터의 구동이 중지되고 로봇의 동작이 즉시로 정지한다.

여기서, 위치명령과 모터와 로봇의 관성모멘트의 합의 설정값과 명령토오크상수 및 전류편차의 설정치 등 미리 설정된 값을 이용해 로봇의 충돌여부를 판단하기 때문에, 로봇의 충돌상태가 시간지연 없이 즉각적으로 감지되어 로봇을 신속하게 정지시킬 수 있으므로, 로봇이 방해물에 충돌된 이후 모터의 계속적인 구동에 의해 로봇 및 모터가 파손되는 사고는 물론, 로봇이 정상적인 위치이동경로로부터 일탈되어 오동작되는 사고를 미연에 방지할 수 있는 효과가 있다.

Description

로봇 위치서보시스템의 충돌감시장치

본 발명은 로봇 위치서보시스템에 관한 것으로, 보다 상세하게는 로봇이 방해물에 충돌하는 경우에 이를 신속하게 감지하여 로봇의 동작을 중단시킬 수 있는 로봇 위치서보시스템의 충돌감시장치에 관한 것이다.

일반적으로, 로봇은 모터의 구동에 의해 발생되는 동력에 의해 방향전환 등의 위치이동이 수행되는데 이러한 로봇의 위치를 조정하는 종래의 로봇 위치서보시스템은, 도 1에 도시된 바와 같이, 상위제어기(1)로부터 출력된 위치명령(θr)을 양(+)의 입력으로 하고 모터 회전축의 각위치(θm)를 음(-)의 입력으로 하여 가산을 행하는 가산부(10)와, 이 가산부(10)의 출력값에 위치오차상수(Kp)를 곱해 속도명령(ω)을 구하는 속도산출부(20)와, 이 속도산출부(20)로부터 출력된 속도명령(ω)을 양(+)의 입력으로 하고 모터의 속도(ωm)를 음(-)의 입력으로 하여 가산을 행하는 가산부(30)를 포함한다.

이와 더불어 상기 종래의 로봇 위치서보시스템은, 상기 가산부(30)의 출력값을 " "(K1; 속도 P 이득, K2; 속도 Z 이득)의 적분블럭에 통과시켜 전류명령(I)을 출력하는 속도제어부(40)와, 이 속도제어부(40)로부터 인가되는 전류명령(I)에 상응하게 도시생략된 전류공급수단으로부터 공급되는 전류에 토오크상수(Kt)가 곱해져 토오크(τm)가 발생되는 모터의 토오크발생부(50)와, 이 토오크발생부(50)에 의해 발생된 토오크(τm) 및 외란에 의한 역토오크(τd)가 각각 양(+)의 입력으로 되어 가산이 행해지는 토오크가산부(60)를 포함한다.

또한, 상기 종래의 로봇 위치서보시스템은, 상기 토오크가산부(60)의 출력토오크를 입력으로 한 " "(J: 모터와 로봇의 관성 모멘트의 합)의 적분요소에 의해 속도(ωm)가 발생되는 모터의 속도발생부(70)와, 이 속도발생부(70)에 의해 발생되는 속도(ωm)를 입력으로 한 " "의 적분요소에 의해 모터 회전축의 각위치(θm)가 이동되는 모터의 위치이동부(80)를 포함하여 구성된다.

이와 같은 종래의 위치서보시스템은, 외란이 없는 경우에는 토오크발생부(50)에 의해 발생된 정상치의 토오크(τm)에 의해 로봇이 구동되지만 방해물과 로봇이 충돌하면 역토오크(τd)가 발생되어 로봇 및 모터가 파손됨은 물론, 정상적인 이동경로에서 일탈되어 오동작을 수행하게 된다.

상기한 바와 같이 외란에 의한 로봇의 파손 및 오동작을 방지하도록 구성된 종래의 외란보상기를 가진 모터의 위치서보시스템은 도2에 도시된 바와 같으며, 이 외란보상기를 가진 모터의 위치서보시스템의 기본구성은 도 1과 동일하므로 동일한 부분에 대해서는 동일한 참조부호를 부여하고 중복된 설명은 생략하기로 한다.

도 2에 있어서, 외란보상기(100)는 속도제어부(40)로부터 출력된 전류명령(I)에 상응하는 토오크와 모터의 속도발생부(70)에 의해 발생된 모터의 실제속도(ωm)에 상응하는 토오크를 비교하여 그 비교결과에 따라 외란을 보상하도록 모터의 토오크발생부(50)로 입력되는 전류명령(I)을 조절하도록 된 것이다.

상기한 바와 같은 외란보상기(100)는, 속도제어부(40)로부터 출력된 전류명령(I) 및 후술될 전류보상치산출부(160)에서 구해진 전류보상치(Icom)를 각각 양(+)의 입력으로 한 가산을 행하여 보상된 전류명령(Icr)을 구하고, 이 보상된 전류명령(Icr)을 모터의 토오크발생부(50) 및 후술될 명령토오크산출부(120)로 입력하는 가산부(110)를 포함한다.

이와 더불어 상기 외란보상기(100)는, 상기 가산부(110)로부터의 보상된 전류명령(Icr)에 미리 설정된 명령토오크상수(Ktn)를 곱해 명령토오크(τ)를 구하는 명령토오크산출부(120)와, 속도발생부(70)에 의해 발생된 모터의 속도(ωm)를 "JmS"(Jm; 모터의 관성 모멘트)의 미분블럭에 통과시켜 모터의 실제 토오크(τm)를 구하는 모터토오크산출부(130) 및, 상기 명령토오크산출부(120)에서 구해진 명령토오크(τ)를 양(+)의 입력으로 하고 상기 모터토오크산출부(130)에서 구해진 모터의 실제 토오크(τm)를 음(-)의 입력으로 한 가산을 행하여 토오크편차(τΔ)를 구하는 가산부(140)를 포함한다.

또한, 상기 외란보상기(100)는, 상기 가산부(140)에서 구해진 토오크편차(τΔ)를 " "(g; 주파수상수)의 적분블럭을 통해 필터링하여 소정값이상의 토오크편차(τΔ)만을 출력하는 필터부(150) 및, 이 필터부(150)로부터 출력된 토오크편차(τΔ)를 미리 설정된 명령토오크상수(Ktn)로 나누어 전류보상치(Icom)를 구하는 전류보상치산출부(160)로 구성된다.

그러나, 상술한 외란보상기는 명령토오크산출부와 모터토오크산출부의 출력값을 연산하여 구해진 토오크편차를 필터부를 통해 필터링시키는 동안에 시간지연이 발생되기 때문에, 로봇이 방해물에 충돌하는 경우에 로봇을 즉각적으로 정지시키지 못함에 따라 로봇 및 모터의 파손과 오작동을 방지하지 못하는 문제점이 있었다.

따라서, 본 발명은 상기한 바와 같은 종래기술의 문제점을 해결하기 위해 안출한 것으로, 미리 설정된 값인 위치명령으로부터 목표전류를 도출해내어 이를 실제 모터의 전류명령과 연산하여 전류편차를 구하는 신속한 연산과정을 통해 로봇의 충돌여부를 판단함으로써, 로봇이 방해물에 충돌하는 경우 이를 즉각적으로 감지하여 로봇을 정지시킬 수 있는 로봇 위치서보시스템의 충돌감시장치를 제공하는 데에 그 목적이 있다.

상기 목적을 달성하기 위한 본 발명에 따른 로봇 위치서보시스템의 충돌감시장치는, 상위제어부로부터의 위치명령에 따른 전류명령에 따라 구동되는 모터에 의해 로봇이 위치이동되는 로봇 위치서보시스템에 있어서, 소정의 연산과정을 통해 상기 위치명령으로부터 목표전류치를 구하는 목표전류치산출수단과, 상기 목표전류치와 상기 전류명령을 연산하여 전류편차를 구하는 전류편차산출부 및, 상기 전류편차를 미리 설정된 전류편차와 비교하여 그 비교결과에 따라 충돌신호를 출력하는 비교부로 구성된 것을 특징으로 한다.

도 1은 종래의 통상적인 로봇 위치서보시스템의 제어블록도,

도 2는 종래의 외란보상형 로봇 위치서보시스템의 제어블록도,

도 3은 본 발명의 바람직한 실시예에 의한 로봇 위치서보시스템의 충돌감시장치를 도시한 제어블록도.

<도면의 주요 부분에 대한 부호의 설명>

200: 상위제어부 300: 충돌감지수단

310: 목표속도산출부 320: 목표토오크산출부

330: 목표전류산출부 340: 전류편차산출부

350: 비교부

이하, 본 발명의 바람직한 실시예를 첨부된 도면을 참조하여 상세히 설명하기로 한다.

도 3은 본 발명의 바람직한 실시예에 의한 로봇 위치서보시스템의 충돌감시장치를 도시한 제어블록도로서, 본 발명에 따른 로봇 위치서보시스템의 충돌감시장치는, 상위제어부(200)와, 복수개의 가산부(210, 230)와, 속도산출부(220)와, 속도제어부(240)와, 모터의 토오크발생부(250)와, 토오크가산부(260)와, 모터의 속도발생부(270)와, 모터의 위치이동부(280) 및, 충돌감지수단(300)으로 구성된다.

도 3에 있어서, 상기 상위제어부(200)는 키입력 및 내장된 프로그램에 따라 로봇의 위치를 제어하기 위한 위치명령(θr)을 출력하는 한편, 상기 충돌감지수단(300)으로부터 충돌신호가 입력되면 모터의 회전축 각위치(θm)를 피드백받아 로봇을 정지시키기 위한 위치명령(θr)을 출력한다.

그리고, 상기 가산부(210)는 상술한 상위제어기(200)로부터 출력된 위치명령(θr)을 양(+)의 입력으로 하고 모터 회전축의 각위치(θm)를 음(-)의 입력으로 하여 가산을 행하며, 속도산출부(220)는 상기 가산부(210)의 출력값에 위치오차상수(Kp)를 곱해 속도명령(ω)을 구한다.

이와 더불어 상기 가산부(230)는 상기 속도산출부(220)로부터 출력된 속도명령(ω)을 양(+)의 입력으로 하고 모터의 속도(ωm)를 음(-)의 입력으로 하여 가산을 행하며, 상기 속도제어부(240)는 상기 가산부(30)의 출력값을 " "(K1; 속도 P 이득, K2; 속도 Z 이득)의 적분블럭에 통과시켜 모터를 구동시키기 위한 전류명령(I)을 구한다.

그리고, 모터의 토오크발생부(250)는 상기 속도제어부(240)로부터 인가되는 전류명령(I)에 의해 도시생략된 전류공급수단으로부터 공급되는 전류에 토오크상수(Kt)가 곱해져 토오크(τm)가 발생되는 모터의 소정 부분이며, 상기 토오크가산부(260)는 상기 토오크발생부(250)에 의해 발생된 토오크(τm) 및 외란에 의한 역토오크(τd)가 각각 양(+)의 입력으로 되어 가산이 행해지는 로봇과 모터의 소정부분이다.

또한, 상기 모터의 속도발생부(270)는 상기 토오크가산부(260)의 출력토오크를 입력으로 한 " "(J: 모터와 로봇의 관성 모멘트 합)의 적분요소에 의해 속도(ωm)가 발생되는 모터의 소정부분이며, 모터의 위치이동부(280)는 상기 속도발생부(270)에 의해 발생되는 속도(ωm)를 입력으로 한 " "의 적분요소에 의해 모터 회전축의 각위치(θm)가 이동되는 모터의 소정부분이다.

그리고, 충돌감지수단(300)은 로봇의 충돌여부를 판단하여 그 판단결과에 따라 충돌신호를 상술한 상위제어부(200)로 입력하기 위한 것으로, 목표속도산출부(310)와 목표토오크산출부(320)와 목표전류산출부(330)와 전류편차산출부(340) 및 비교부(350)로 구성된다.

상기 충돌감지수단(300)에 있어서, 상기 목표속도산출부(310)는 상기 상위제어부(200)로부터 출력된 위치명령(θr)을 단일미분블럭에 통과시켜 목표속도(ωm*)를 구하며, 상기 목표토오크산출부(320)는 상기 목표속도산출부(310)에서 구해진 목표속도(ωm*)를 "JnS"(Jn; 모터와 로봇의 관성모멘트 합에 대한 설정치)의 미분블럭에 통과시켜 목표토오크(τm*)를 구한다.

이와 더불어, 상기 목표전류산출부(330)는 상기 목표토오크산출부(320)에서 구해진 목표토오크(τm*)를 미리 설정된 명령토오크상수(Ktn)로 나누어 목표전류(I*)를 구하며, 전류편차산출부(340)는 상기 목표전류산출부(330)에서 구해진 목표전류(I*)를 양(+)의 입력으로 하고 상기 속도제어부(240)로부터 출력된 전류명령(I)을 음(-)의 입력으로 한 가산을 행하여 전류편차(y)를 구한다.

그리고, 비교부(350)는 상기 전류편차산출부(340)에서 구해진 전류편차(y)를 미리 설정된 전류편차(y1)와 비교하여 비교결과 전류편차(y)가 미리 설정된 전류편차(y1)의 이상이면 충돌신호를 출력하여 상술한 상위제어부(200)로 입력한다.

상기한 바와 같이 구성된 본 발명의 동작예를 도 3을 참조하여 상세히 설명하기로 한다.

먼저, 로봇의 동작을 지시하는 키신호가 상위제어부(200)로 입력되면, 상위제어부(200)는 내장된 프로그램에 따라 로봇을 이동시키도록 위치명령(θr)을 출력한다.

상기 상위제어부(200)로부터 출력된 위치명령(θr)이 양(+)의 입력이 되고 피드백된 모터 회전축의 각위치(θm)가 음(-)의 입력이 되어 가산부(210)를 통해 가산되어 출력되며, 이 가산부(210)의 출력값이 속도산출부(220)에서 위치오차상수(Kp)와 곱해져 속도명령(ω)이 구해진다.

상기 속도산출부(220)에서 구해진 속도명령(ω)이 양(+)의 입력이 되고 피드백된 모터의 속도(ωm)가 음(-)의 입력이 되어 가산부(230)를 통해 가산되어 출력되며, 이 가산부(230)의 출력값이 속도제어부(240)에 의해 " "(K1; 속도 P 이득, K2; 속도 Z 이득)의 적분블럭에 통과되어 전류명령(I)이 구해진다.

이때, 모터의 토오크발생부(250)에서는 상기 속도제어부(240)에서 구해진 전류명령(I)에 의해 도시생략된 전류공급수단으로부터 모터로 공급되는 전류치에 토오크상수(Kt)가 곱해져 토오크(τm)가 발생된다.

이와 더불어, 토오크가산부(260)를 통해 상기 모터의 토오크발생부(250)에서 발생된 토오크(τm)에 대해 로봇이 충돌될 경우 발생되는 역토오크(τd)가 가산되는데, 로봇이 충돌되지 않은 경우 토오크가산부(260)로부터 역토오크(τd)가 가산되지 않는 정상치의 토오크(τm)가 모터의 속도발생부(270)로 입력되며 이 토오크(τm)와 " "(J: 모터와 로봇의 관성 모멘트 합)의 적분요소에 의해 속도발생부(270)에서 속도(ωm)가 발생된다.

그리고, 모터의 위치이동부(280)에서는 상기 속도발생부(270)에서 발생된 속도(ωm)와 " "의 적분요소에 의해 모터 회전축의 각위치(θm)가 이동된다.

이와 동시에, 상위제어부(200)로부터 출력된 위치명령(θr)이 충돌감지수단(300)의 목표속도산출부(310)에 의해 단일미분블럭을 통과하여 목표속도(ωm*)가 구해지며, 이 목표속도산출부(310)에서 구해진 목표속도(ωm*)가 목표토오크산출부(320)에 의해 "JnS"(Jn; 모터와 로봇의 관성모멘트 합에 대한 설정치)의 미분블럭을 통과하여 목표토오크(τm*)가 구해진다.

이와 같이 구해진 목표토오크(τm*)가 목표전류산출부(330)를 통해 미리 설정된 명령토오크상수(Ktn)로 나누어져 목표전류(I*)가 구해지며, 이와 같이 구해진 목표전류(I*)가 양(+)의 입력이 되고 상술한 속도제어부(240)로부터 출력된 전류명령(I)이 음(-)의 입력이 되어 전류편차산출부(340)에서 가산이 행해져 전류편차(y)가 구해진다.

그리고, 비교부(350)를 통해 상기 전류편차산출부(340)에서 구해진 전류편차(y)가 미리 설정된 전류편차(y1)와 비교되는데, 로봇이 방해물에 충돌되지 않고 정상적으로 이동되고 있는 상태에서는 목표전류치(I*)와 실제의 전류명령(I) 간에 차이가 발생되지 않으므로 산출된 전류편차(y)가 미리 설정된 전류편차(y1) 범위의 미만이되어 비교부(350)로부터 충돌신호가 출력되지 않는다.

그러나, 로봇이 방해물에 충돌되어 역토오크(τd)가 발생되면 토오크가산부(260)를 통해 정상치의 토오크(τm)에 상기 역토오크(τd)가 가산되어 토오크에 편차가 발생되고 이와 같이 편차가 발생된 토오크가 속도발생부(270)로 입력됨에 따라 모터의 속도(ωm)에 편차가 발생된다.

상기와 같이 편차가 발생된 모터의 속도(ωm)가 가산부(230)로 피드백되어 속도산출부(220)로부터 출력된 속도명령(ω*)에 가산되어 속도제어부(240)로 입력됨에 따라 속도제어부(240)로부터 출력되는 전류명령(I)에 편차가 발생된다.

이와 같이 편차가 발생된 전류명령(I)이 전류편차산출부(340)로 입력됨에 따라 목표전류산출부(330)로부터 출력되어 비교부(350)로 입력되는 전류편차(y)가 미리 설정된 전류편차(y1)의 이상이되고 이에 따라 비교부(350)로부터 충돌신호가 출력된다.

상기 비교부(350)로부터 출력된 충돌신호는 상위제어부(200)로 입력되고, 상기 비교부(350)로부터 입력된 충돌신호에 따라 상위제어부(200)는 피드백된 모터 회전축의 각위치(θm)와 동일한 위치로 로봇을 위치시키기 위한 위치명령(θm) 즉, 로봇의 구동중지명령을 출력한다.

여기서, 모터 회전축의 각위치(θm)와 동일한 위치명령(θr)이 양(+)의 입력이 되고 피드백된 모터 회전축의 각위치(θm)가 음(-)의 입력이 됨에 따라 가산부(210)로부터 "0"의 위치명령이 출력되어 속도산출부(220)로 입력되고, 상기 가산부(210)로부터 "0"의 위치명령이 입력됨에 따라 속도산출부(220)로부터 "0"의 속도명령이 속도제어부(240)로 입력된다.

이때, 가산부(230)에서 상기 속도산출부(220)로부터의 "0"의 속도명령이 음(-)의 값으로 입력되는 모터의 속도(ωm)와 가산되는 데, 현재 로봇이 방해물에 충돌하여 정지상태이므로 모터의 속도(ωm)가 "0"이 되어 가산부(230)로부터 "0"의 값이 출력되고 이 값이 속도제어부(240)로 입력된다.

상기 가산부(230)로부터 "0"의 속도명령이 입력됨에 따라 속도제어부(240)로부터 "0"의 전류명령이 출력되고, 이 "0"의 전류명령에 의해 모터로 전류가 공급되지 않아 모터의 구동이 중단됨과 더불어 로봇이 정지한다.

상술한 바와 같이 본 발명은, 상위제어부(200)의 위치명령(θr)과 모터와 로봇의 관성모멘트 합의 설정값(Jn)과 명령토오크상수(Ktn) 및 설정된 전류편차(y1) 등 미리 설정된 값을 이용해 로봇의 충돌여부를 판단하기 때문에, 로봇의 충돌상태가 시간지연 없이 즉각적으로 감지되어 로봇을 신속하게 정지시킬 수 있으므로, 로봇이 방해물에 충돌된 이후 모터의 계속적인 구동에 의해 역토오크가 증가되어 로봇이 파손되는 사고가 방지될 수 있다.

따라서, 본 발명을 이용하면, 미리 설정된 값인 위치명령으로부터 목표전류를 도출해내어 이를 실제 모터의 전류명령과 연산하여 전류편차를 구하는 신속한 연산과정을 통해 로봇의 충돌여부를 판단하여 모터의 구동을 즉각적으로 정지시킬 수 있므로, 로봇이 방해물에 충돌된 이후 모터의 계속적인 구동에 의해 로봇 및 모터가 파손되는 사고는 물론, 로봇이 정상적인 위치이동경로로부터 일탈되어 오동작되는 사고를 미연에 방지할 수 있는 효과가 있다.

Claims (2)

1. 상위제어부로부터의 위치명령에 따른 전류명령에 따라 구동되는 모터에 의해 로봇이 위치이동되는 로봇 위치서보시스템에 있어서,

   소정의 연산과정을 통해 상기 위치명령으로부터 목표전류치를 구하는 목표전류치산출수단과,

   상기 목표전류치와 상기 전류명령을 연산하여 전류편차를 구하는 전류편차산출부 및,

   상기 전류편차를 미리 설정된 전류편차와 비교하여 그 비교결과에 따라 충돌신호를 출력하는 비교부로 구성된 것을 특징으로 하는 로봇 위치서보시스템의 충돌감시장치.
2. 제 1 항에 있어서, 상기 목표전류치산출수단은,

   상기 위치명령을 미분하여 목표속도를 구하는 목표속도산출부와,

   상기 목표속도에 미리 설정된 모터 및 로봇의 관성 모멘트의 합을 곱한 다음 미분하여 목표토오크를 구하는 목표토오크산출부 및,

   상기 목표토오크를 미리 설정된 명령토오크상수로 나누어 목표전류치를 구하는 목표전류산출부로 구성된 것을 특징으로 하는 로봇 위치서보시스템의 충돌감시장치.