KR101581096B1

KR101581096B1 - 다관절형 로봇의 제어 장치, 제어 방법 및 제어 프로그램을 기록한 컴퓨터 판독가능한 기록 매체

Info

Publication number: KR101581096B1
Application number: KR1020137023668A
Authority: KR
Inventors: 츠토무 오네; 도시히코 니시무라
Original assignee: 가부시키가이샤 고베 세이코쇼
Priority date: 2011-03-08
Filing date: 2012-03-02
Publication date: 2015-12-29
Also published as: JP5726052B2; CN103429398A; JP2012196753A; EP2684649A1; CN103429398B; KR20130122970A; US20130338827A1; EP2684649A4; US9199374B2; WO2012121125A1; EP2684649B1

Abstract

속도 억제 처리에 있어서, 손목 선단의 위치 및 손목 선단의 이동 속도와 축의 허용 속도를 유지하고 또한 원하는 좌표계에 있어서의 자세 각도의 특정 성분의 변동을 억제한다. 교시 경로 상의 보간점에 있어서의 위치, 자세 및 각 축의 각도를 산출하면서 움직이는 다관절 로봇에 있어서, 손목축의 속도가 허용 한도를 초과하는지를 판별하여, 허용 한도를 초과하는 경우에, 속도를 허용 한도 내에 한 상기 손목축의 각도의 복수의 후보를 산출하고, 복수의 상기 후보 중에서 용접선 좌표계의 자세각의 특정 성분의 변동이 가장 억제된 후보를 선택하여 상기 손목축의 각도로 하고, 이에 대응하는 다른 축의 각도를 다시 산출하여 다음의 보간점의 각도로 하여, 다관절 로봇을 구동시키는 것을 특징으로 한다.

Description

다관절형 로봇의 제어 장치, 제어 방법 및 제어 프로그램을 기록한 컴퓨터 판독가능한 기록 매체{CONTROL DEVICE, CONTROL METHOD AND COMPUTER-READABLE RECORDING MEDIUM STRORING CONTROL PROGRAM FOR ARTICULATED ROBOT}

본 발명은 다관절형 로봇의 제어 장치, 제어 방법 및 제어 프로그램에 관한 것으로, 특히 손목축의 각 구동축의 속도가 허용 범위를 초과하는 경우에, 손목의 자세를 유지하면서 각 구동축의 속도를 허용 범위 내로 유지하는 기술에 관한 것이다.

여기에, 도 23에 도시한 바와 같은 다관절형 로봇의 일종인 6축 매니퓰레이터에 있어서, 베이스에 장착된 아암축을 제어하는 관절축을 베이스로부터 차례로 J1축, J2축, J3축으로 하고, 손목축을 제어하는 관절축을 아암 부분부터 손목의 선단을 향해 차례로 J4축, J5축, J6축으로 한다. 이 경우, J1축 내지 J6축의 각 관절각 θ₁ 내지 θ₆이 부여되었을 때, 순운동학 문제의 해를 구함으로써, 직교 좌표계에서의 엔드 이펙터(손끝의 선단 부분에 장착한 용접기 등의 효과기)의 위치의 XYZ 좌표와, 엔드 이펙터의 자세각 α, β, γ(또한, 자세각 α, β, γ는 오일러 각이나 롤 피치 요 각 등으로 나타냄)를 구할 수 있다. 또한 반대로, 엔드 이펙터의 위치의 XYZ 좌표와 자세각 α, β, γ가 부여되었을 때, 역운동학 문제의 해를 구함으로써, J1축 내지 J6축의 각 관절각 θ₁ 내지 θ₆을 구할 수 있다.

그런데, 도 24에 도시한 바와 같이, J5축의 관절각 θ₅가 0도 부근에 있어서, 엔드 이펙터를 일정 속도로 움직이게 하는 경우에, J4축의 관절각 θ₄와 J6축의 관절각 θ₆의 각도가 급격하게 변화하기 때문에, J4축과 J6축을 고속으로 회전시켜야만 한다. 그러나, 매니퓰레이터의 각 관절축은 회전 속도의 상한값을 초과할 수 없어, 무리하게 상한값을 초과하여 동작시킨 경우에는, 엔드 이펙터의 경로가 예정 경로로부터 벗어나 버리거나, 엔드 이펙터가 진동해 버리는 문제가 발생한다. 이와 같이 다관절 매니퓰레이터에 있어서, 엔드 이펙터의 위치나 자세각을 약간 움직이게 한 것만으로, 매우 큰 회전 속도가 요구되는 관절축이 발생하여, 동작 불능이 되는 경우가 있다. 이러한 동작 불가능이 발생하는 경우에는, 각 관절축의 속도와 엔드 이펙터의 이동 경로의 정밀도의 양립이 곤란하여, 이동 경로의 정밀도를 확보하기 위해서 각 관절축의 회전 속도가 요구되는 속도보다도 저하해 버리거나, 안전 장치가 작동해서 매니퓰레이터의 동작이 정지해 버리는 문제가 있다.

따라서, 어떤 관절축의 속도가 허용 범위를 초과하는 경우에, 각 손목축에 급격한 변화를 발생시키지 않고 제한 내의 속도로 유지한 채 동작시키는 방법이, 특허문헌 1 내지 3에 개시되어 있다.

일본 특허 공개 소62-162109호 공보 일본 특허 공개 평6-324730호 공보 일본 특허 공개 제2003-300183호 공보

그러나, 어떤 관절축의 속도가 허용 범위를 초과하는 경우에, 각 손목축에 급격한 변화를 발생시키지 않고 제한 내의 속도로 유지한 채 동작시켜서, 예정 경로를 통과시키려고 하는 경우에는, 엔드 이펙터에 요구된 (1) 위치 (2) 이동 속도 (3) 자세 각도의 3 요소를 동시에 만족시킬 수 없는 것이 일반적으로 알려져 있다. 그때문에, 일반적인 매니퓰레이터에서는, (1) 내지 (3)의 요소 중에서 한개나 두개의 요소만을 만족하도록 매니퓰레이터를 동작시키는 제어가 이루어지고 있다.

예를 들어, 특허문헌 1 및 특허문헌 2에는, 엔드 이펙터의 (1)의 위치와 (3)의 자세 각도를 유지하는 방법이 기재되어 있지만, (2)의 이동 속도를 유지할 수 없어 이동 속도가 느려지기 때문에, 엔드 이펙터에 용접기를 구비한 것 같은 경우에는, 원래 요구되는 시간보다도 긴 시간에 걸쳐 작업선을 통과해 버려, 다른 부분의 용접 시간보다도 길어져서, 강도가 저하하는 등의 문제가 있다. 또한, 이동 속도가 느려져 용접 시간이 길어진 개소가 필요 이상으로 부풀어 올라서, 다음 공정(예를 들어 2층째의 용접 공정 등)에 영향을 미칠 우려도 있다.

또한, 특허문헌 3에는, 엔드 이펙터의 이동 속도 또는 위치 중 어느 한쪽을 유지하는 방법이 기재되어 있지만, 이동 속도를 유지하는 경우에는 위치를 유지할 수 없어, 엔드 이펙터에 용접기나 도장기를 구비한 것 같은 경우에는, 원래 예정된 작업선으로부터 벗어나서, 용접이나 도장이 필요한 장소에 효과를 미치게 할 수 없는 문제가 있다.

따라서, 본 발명은 상기 사정을 감안하여 이루어진 것으로, 그 목적으로 하는 바는, 손목축을 소정의 허용량을 초과하여 급격하게 변화시킬 필요가 있는 경우에, 용접이나 도장 작업에 특히 필요한 손목 선단의 위치 및 손목 선단의 이동 속도와 손목축의 허용 속도를 유지하고, 또한 목적에 따른 작업 좌표계에 있어서의 자세 각도 중 적어도 하나의 특정 성분의 변동을 억제하도록 한 다관절형 로봇의 제어 장치, 제어 방법 및 제어 프로그램을 제공하는 데 있다.

상기 목적을 달성하기 위해서 본 발명은, 작업 대상물의 작업선에 따라서 이동시키는 작업부를 선단에 갖고, 그 작업부에 연결되어 상기 작업부의 자세를 변화시키는 3개의 구동축을 구비한 제1 관절 구동계와, 상기 제1 관절 구동계에 연결되어 상기 제1 관절 구동계의 위치를 변화시키는 적어도 3개의 구동축을 구비한 제2 관절 구동계를 갖는 다관절형 로봇을 제어하는 다관절형 로봇의 제어 장치에 적용되는 것으로서, 하기의 (11) 내지 (16)을 구비하여 이루어지는 것을 특징으로 하여 구성된다.

(11) 상기 다관절형 로봇의 베이스 좌표계에 있어서 미리 교시된 상기 작업부의 작업 개시 위치 및 그때의 자세와 작업 종료 위치 및 그때의 자세를 연결하는 교시 경로를 보간하는 복수의 보간점에 있어서의 상기 작업부의 위치 및 그때의 자세를 나타내는 데이터를 산출하는 보간 데이터 산출 수단.

(12) 상기 보간 데이터 산출 수단에 의해 산출된 데이터로부터 역운동학 문제의 해를 구함으로써, 상기 보간점에 있어서의 상기 작업부의 위치 및 그때의 자세를 나타내는 상기 제1 관절 구동계 및 상기 제2 관절 구동계를 포함하는 모든 구동축의 각도를 산출하는 각도 산출 수단.

(13) 상기 각도 산출 수단에 의해 산출된 다음의 상기 보간점과 현재의 상기 보간점에 있어서의 상기 제1 관절 구동계의 상기 각 구동축의 각도의 차분에 기초하여 다음의 상기 보간점에 상기 작업부의 자세를 움직이게 할 때의 상기 제1 관절 구동계에 있어서의 양단의 2개의 구동축의 속도를 산출하는 속도 산출 수단.

(14) 상기 보간 데이터 산출 수단에 의해 산출된 데이터를, 상기 작업부의 이동 방향의 축, 상기 축에 수직인 방향의 축, 상기 2개의 축에 수직인 축을 포함하는 작업 좌표계에 있어서의 상기 작업부의 자세를 나타내는 자세 데이터로 변환하는 자세 데이터 변환 수단.

(15) 상기 속도 산출 수단에 의해 산출된 상기 제1 관절 구동계에 있어서의 양단의 2개의 구동축의 한쪽 또는 양쪽의 속도가 미리 정해진 허용 범위 외인 경우에, 상기 작업부의 이동 속도를 변경하지 않고, 또한 상기 자세 데이터 변환 수단에 의해 변환된 다음의 상기 보간점에 있어서의 상기 작업부의 자세 데이터에 대해서 한개 또는 두개의 특정 성분의 변동을 억제하면서, 상기 제1 관절 구동계에 있어서의 양단의 2개의 구동축의 속도가 상기 허용 범위 내로 되는 상기 제1 관절 구동계의 구동축 각각의 각도를 다시 산출하고, 상기 산출된 상기 제1 관절 구동계의 구동축 각각의 각도와 상기 보간 데이터 산출 수단에 의해 산출된 상기 작업부의 위치에 기초하여 상기 제2 관절 구동계의 구동축 각각의 각도를 다시 산출하는 재산출 수단.

(16) 상기 속도 산출 수단에 의해 산출된 상기 제1 관절 구동계에 있어서의 양단의 2개의 구동축의 양쪽의 속도가 상기 허용 범위 내인 경우에는, 상기 각도 산출 수단에 의해 산출된 상기 구동축 각각의 각도에 기초하여 상기 다관절형 로봇을 구동시키고, 상기 속도 산출 수단에 의해 산출된 상기 제1 관절 구동계에 있어서의 양단의 2개의 구동축의 한쪽 또는 양쪽의 속도가 상기 허용 범위 외인 경우에는, 상기 재산출 수단에 의해 산출된 상기 구동축 각각의 각도에 기초하여 상기 다관절형 로봇을 구동시키는 구동 지시 수단.

본 발명에 따르면, 상기 제1 관절 구동계의 구동축을 상기 허용 범위를 초과하여 급격하게 변화시킬 필요가 있는 경우에도, 용접이나 도장 작업에 특히 필요한 상기 작업부의 위치 및 손목 선단의 이동 속도와 상기 제1 관절 구동계의 구동축의 허용 속도를 유지하고, 또한 목적에 따른 작업 좌표계에 있어서의 자세 각도 중 적어도 하나의 특정 성분의 변동을 억제할 수 있다. 또한, 상기 작업 개시 위치로부터 상기 작업 종료 위치까지의 사이에 존재하는 특이점 영역이나 특이점 회피 경로 등을 산출하는 경우에는, 사전 계산에 있어서의 과대한 부하에 대응할 수 있는 연산 장치(CPU)를 설치할 필요가 있어 비용 상승 요인으로 되지만, 본 발명에 따르면, 현재의 상태와 다음의 상태에 따라서 그때마다 구동이 제어되면 되기 때문에, 연산 장치에 순간적으로 요구되는 처리 부하가 경감되고 있어 저비용화를 도모할 수 있다.

예를 들어, 상기 재산출 수단이, 현재의 상기 보간점에 있어서의 상기 제1 관절 구동계의 구동축 각각의 각도에 기초하여, 상기 구동축 각각의 각도가 상기 각도 산출 수단에 의해 산출된 다음의 상기 보간점에 있어서의 각도를 참조로 추종하고, 또한 상기 제1 관절 구동계에 있어서의 양단의 2개의 구동축의 다음의 상기 보간점에 있어서의 속도가 상기 허용 범위 내로 되는 상기 구동축 각각의 각도의 후보를 복수 산출하는 각도 후보 산출 수단과, 상기 각도 후보 산출 수단에 의해 산출된 각도의 후보 각각을 채용했을 때의 다음의 상기 보간점의 상기 작업부의 자세를 상기 작업 좌표계로 나타내는 자세 데이터를 각각 산출하는 자세 데이터 산출 수단과, 상기 자세 데이터 산출 수단에 의해 산출된 복수의 자세 데이터 중, 상기 자세 데이터 변환 수단에 의해 변환된 다음의 상기 보간점에 있어서의 상기 작업부의 자세 데이터에 대해서 상기 특정 성분의 변동이 가장 억제된 자세 데이터를 선택하고, 상기 자세 데이터에 대응하는 각도를 상기 구동축 각각의 각도로서 선택하는 각도 선택 수단을 포함하여 이루어지는 것이 생각된다.

이에 의해, 상기 작업부의 위치·자세를 움직이게 했을 때에, 상기 작업부의 작업 목적에 따른 적절한 특정 성분의 변동을 억제할 수 있다.

그런데, 상기 제1 관절 구동계의 구동축의 각도를 ±180°로 나타냈을 때, 상기 각도 산출 수단은, 예를 들어 상기 제1 관절 구동계의 중앙에 위치하는 구동축의 다음의 상기 보간점에 있어서의 각도가 상기 작업 개시 위치에 있어서의 상기 구동축의 각도와 동일 부호가 되는 역운동학 문제의 해를 산출한다. 이에 의해, 상기 제1 관절 구동계의 중앙에 위치하는 구동축의 각도가 0°를 통과하지 않도록 제어하여 특이점을 회피할 수 있다.

또한, 상기 각도 산출 수단은, 상기 속도 산출 수단에 의해 산출된 상기 제1 관절 구동계에 있어서의 양단의 2개의 구동축의 양쪽의 속도가 상기 허용 범위 내인 경우에는, 상기 제1 관절 구동계의 중앙에 위치하는 구동축의 다음의 상기 보간점에 있어서의 각도가 상기 작업 개시 위치에 있어서의 상기 구동축의 각도와 동일 부호가 되는 역운동학 문제의 해를 산출하고, 상기 속도 산출 수단에 의해 산출된 상기 제1 관절 구동계에 있어서의 양단의 2개의 구동축의 한쪽 또는 양쪽의 속도가 상기 허용 범위 외인 경우에는, 상기 제1 관절 구동계의 중앙에 위치하는 구동축의 다음의 상기 보간점에 있어서의 각도가 상기 작업 개시 위치에 있어서의 상기 구동축의 각도와 다른 부호가 되는 역운동학 문제의 해를 산출하는 것이어도 된다.

또한, 이들을 조합한 구성도 생각된다. 즉, 상기 각도 산출 수단이, 상기 작업 개시 위치 및 상기 작업 종료 위치에 있어서의 상기 제1 관절 구동계의 중앙의 구동축의 각도가 동일 부호인 경우에는, 상기 제1 관절 구동계의 중앙에 위치하는 구동축의 다음의 상기 보간점에 있어서의 각도가 상기 작업 개시 위치에 있어서의 상기 구동축의 각도와 동일 부호가 되는 역운동학 문제의 해를 산출하고, 상기 작업 개시 위치 및 상기 작업 종료 위치에 있어서의 상기 제1 관절 구동계의 중앙의 구동축의 각도가 다른 부호인 경우에 있어서, 상기 속도 산출 수단에 의해 산출된 상기 제1 관절 구동계에 있어서의 양단의 2개의 구동축의 양쪽의 속도가 상기 허용 범위 내인 경우에는, 상기 제1 관절 구동계의 중앙에 위치하는 구동축의 다음의 상기 보간점에 있어서의 각도가 상기 작업 개시 위치에 있어서의 상기 구동축의 각도와 동일 부호가 되는 역운동학 문제의 해를 산출하고, 상기 속도 산출 수단에 의해 산출된 상기 제1 관절 구동계에 있어서의 양단의 2개의 구동축의 한쪽 또는 양쪽의 속도가 상기 허용 범위 외인 경우에는, 상기 제1 관절 구동계의 중앙에 위치하는 구동축의 다음의 상기 보간점에 있어서의 각도가 상기 작업 개시 위치에 있어서의 상기 구동축의 각도와 다른 부호가 되는 역운동학 문제의 해를 산출하는 것이 바람직하다. 이에 의해, 상기 작업 종료 위치에 있어서의 상기 제1 관절 구동계의 중앙의 구동축의 각도를 교시된 대로 도착시킬 수 있다.

또한, 상기 재산출 수단은, 상기 자세 데이터 변환 수단에 의해 변환된 상기 작업부의 자세 데이터 중 미리 정해진 가중치 부여가 큰 상기 특정 성분으로부터 차례로 변동의 억제량이 커지도록 상기 구동축의 각도를 다시 산출하는 것이 생각된다. 이에 의해, 상기 가중치 부여를 적절히 설정해 둠으로써, 상기 특정 성분의 변동의 억제량을 적절히 조정하는 것이 가능해진다.

본원 발명의 보다 구체적인 구성으로서는, 상기 작업부가 토치이고, 상기 자세 데이터가 상기 작업부의 이동 방향의 축을 X축, 상기 X축과 중력 방향의 외적에 의해 나타나는 축을 Y축, 상기 X축과 상기 Y축의 외적에 의해 나타나는 방향의 축을 Z축, 상기 X축의 축 둘레의 회전각을 토치 경사각, 상기 Y축의 축 둘레의 회전각을 토치 전진각, 상기 Z축의 축 둘레의 회전각을 토치 회전각으로 하는 작업 좌표계로 나타나는 것으로서, 상기 특정 성분이, 상기 토치 경사각, 상기 토치 전진각, 상기 토치 회전각 중 어느 하나 또는 둘인 것이 생각된다. 도 25에 도시한 바와 같이, 상기 손목 선단부에 구비된 상기 작업부의 자세에 대해서, 직교 좌표계 Σbase 상에서 나타나는 손목 선단 위치 XYZ와 손목 선단 자세 αβγ로부터, 상기 작업부가 이동 방향의 축을 X축으로 하는 용접선 좌표계 Σline(작업 좌표계) 상에서 나타냄으로써, 상기 작업부의 자세를 도 26 내지 도 28에 도시한 바와 같이 각 축에 대한 기울기 성분으로서 나타낼 수 있다. 변환된 용접선 좌표계 Σline의 좌표축 둘레의 회전각은, 도 26, 도 27에 도시한 바와 같이 Xline축 둘레의 회전각을 우나사 방향을 정방향으로 해서 상기 토치 경사각(Rx)이라 하고, 도 26, 도 28에 도시한 바와 같이 Yline축 둘레의 회전각을 우나사 방향을 정방향으로 해서 상기 토치 전진각(Ry)이라 하고, Zline축 둘레의 회전각을 우나사 방향을 정방향으로 해서 상기 토치 회전각(Rz)이라 한다.

이때, 상기 작업부가, 선단에 상기 작업 대상물에 대해서 작업 상의 효과를 주는 하나의 작업점을 갖는 싱글 토치인 경우에는, 상기 특정 성분이, 상기 토치 경사각 및 상기 토치 전진각인 것이 생각된다. 이에 의해, 작업점이 하나인 싱글 토치의 작업에 중요한 상기 토치 경사각과 상기 토치 전진각의 변동을 억제할 수 있다.

한편, 상기 작업부가, 선단에 상기 작업 대상물에 대해서 작업 상의 효과를 주는 2개의 작업점을 갖는 탠덤 토치인 경우에는, 상기 특정 성분이, 상기 토치 경사각 및 상기 토치 회전각인 것이 생각된다. 이에 의해, 작업점이 둘인 탠덤 토치의 작업에 중요한 상기 토치 경사각과 상기 토치 회전각의 변동을 억제할 수 있다.

또한, 상기 구동 지시 수단이, 상기 속도 허용 판별 수단에 의해 상기 허용 범위 외의 구동축이 있다고 판별된 후에는 상기 재산출 수단에 의해 산출된 상기 구동축 각각의 각도에 기초하여 상기 다관절형 로봇을 구동시키고, 그 후, 상기 각도 산출 수단에 의해 산출된 상기 제1 관절 구동계의 각도와 상기 재산출 수단에 의해 산출된 상기 제1 관절 구동계의 각도의 차분이 소정값 이하가 된 것을 조건으로, 상기 재산출 수단에 의해 산출된 상기 구동축 각각의 각도 대신에 상기 각도 산출 수단에 의해 산출되는 상기 구동축 각각의 각도에 기초하여 상기 다관절형 로봇을 구동시키는 것이 생각된다.

이때, 상기 구동 지시 수단은, 상기 재산출 수단에 의해 산출된 상기 구동축 각각의 각도에 기초하는 상기 다관절 로봇의 구동 제어로부터 상기 각도 산출 수단에 의해 산출된 상기 구동축 각각의 각도에 기초하는 상기 다관절 로봇의 구동 제어로 전환할 때에, 상기 구동축 각각의 각도를 소정의 변동 범위 내에서 서서히 변동시키는 것인 것이 생각된다. 이에 의해, 상기 작업 대상물의 작업선에 따라서 이동하는 상기 작업선이 진동하여 작업선에 따라서 이동하지 않게 되는 것을 방지할 수 있다. 예를 들어, 상기 재산출 수단에 의해 산출된 각도와 상기 다관절 로봇의 구동 제어로부터 상기 각도 산출 수단에 의해 산출된 각도의 중간의 각도인 것이 생각된다.

또한, 상기 속도 산출 수단이, 속도 대신에 가속도를 산출하는 것이며, 상기 재산출 수단 및 상기 구동 지시 수단이, 속도 대신에 가속도에 기초하여 판별하는 것이어도 된다.

또한, 본 발명은 상기 다관절형 로봇의 제어 장치에서 실행되는 각 공정을 실행하는 다관절형 로봇의 제어 방법의 발명, 혹은 그 각 공정을 컴퓨터에 실행시키는 다관절형 로봇의 제어 프로그램의 발명으로서 파악할 수 있다.

본 발명에 따르면, 손목 선단을 약간 움직이게 한 것만으로, 손목축이 소정의 허용량을 초과하여 급격하게 변화하는 경우라도, 용접이나 도장 작업에 특히 필요한 손목 선단의 위치 및 손목 선단의 이동 속도와 손목축의 허용 속도를 유지하고, 또한 목적에 따른 작업 좌표계에 있어서의 자세 각도 중 적어도 하나의 특정 성분의 변동을 억제하는 것이 가능해진다. 또한, 상기 작업 개시 위치로부터 상기 작업 종료 위치까지의 사이에 존재하는 특이점 영역이나 특이점 회피 경로 등을 산출하는 경우에는, 사전 계산에 있어서의 과대한 부하에 대응할 수 있는 연산 장치(CPU)를 설치할 필요가 있어 비용 상승 요인으로 되지만, 본 발명에 따르면, 현재의 상태와 다음의 상태에 따라서 그때마다 구동이 제어되면 되기 때문에, 연산 장치에 순간적으로 요구되는 처리 부하가 경감되고 있어 저비용화를 도모할 수 있다.

도 1은 본 발명의 실시 형태에 따른 다관절형 로봇의 제어 방법의 일례에 의해 제어되는 다관절형 로봇(X)의 개략 구성을 나타내는 블록도.
도 2는 다관절형 로봇의 제어 방법의 제어 전체 처리의 일례를 나타내는 플로우차트.
도 3은 보간점의 각 회전축의 속도·각도 산출 처리의 일례를 나타내는 플로우차트.
도 4는 속도 억제 필요성 판별 처리의 일례를 나타내는 플로우차트.
도 5는 속도 억제 처리의 일례를 나타내는 플로우차트.
도 6은 속도 억제 처리로부터의 복귀 처리의 일례를 나타내는 플로우차트.
도 7은 보간점의 경로의 일례를 나타내는 도면.
도 8은 원래 경로와 예외 경로의 J6축의 각도의 변화의 일례를 나타내는 도면(A) 및 원래 경로의 보간점과 예외 경로의 보간점의 일례를 나타내는 도면(B).
도 9는 특이점을 회피하는 경로의 제1 관절 구동축의 각도 변화의 일례를 나타내는 도표.
도 10은 J4축의 속도의 변화의 일례를 나타내는 도면.
도 11은 등속 동작 시에 있어서의 용접선 좌표계의 각 자세각의 변동의 일례를 나타내는 도면.
도 12는 본 발명의 제어 방법에 있어서의 용접선 좌표계의 각 자세각의 변동의 일례를 나타내는 도면.
도 13은 제2 관절 구동축의 다른 예를 나타내는 도면.
도 14는 다관절형 로봇의 제어 방법의 제어 전체 처리의 다른 예를 나타내는 플로우차트.
도 15는 특이점을 경유하는 경로의 제1 관절 구동축의 각도 변화의 일례를 나타내는 도표.
도 16은 본 실시예 1에 따른 제어 방법을 채용한 경우의 제1 관절 구동축의 각도 변화의 일례를 나타내는 도표.
도 17은 등속 동작 시에 있어서의 용접선 좌표계의 각 자세각의 변동의 일례를 나타내는 도면.
도 18은 본 실시예 1에 따른 제어 방법을 채용한 경우의 용접선 좌표계의 각 자세각의 변동의 일례를 나타내는 도면.
도 19는 다관절형 로봇의 제어 방법의 제어 전체 처리의 다른 예를 나타내는 플로우차트.
도 20은 실시예 3에 따른 2개의 작업점을 갖는 엔드 이펙터의 일례를 나타내는 도면.
도 21은 2개의 작업점을 갖는 엔드 이펙터의 다관절형 로봇의 특이점을 회피하는 경로의 용접선 좌표계의 각 자세각의 변동의 일례를 나타내는 도면.
도 22는 실시예 4에 따른 다관절형 로봇의 제어 방법의 일례에 의해 제어되는 다관절형 로봇(X2)의 개략 구성을 나타내는 블록도.
도 23은 일반적인 6축의 매니퓰레이터의 개략 구성을 도시하는 도면.
도 24는 일반적인 6축의 매니퓰레이터가 특이점을 경유하는 경로에 있어서의 제1 관절 구동축의 각도 변화의 일례를 나타내는 도표.
도 25는 일반적인 6축의 매니퓰레이터의 엔드 이펙터의 위치와 자세를 나타내는 좌표계를 용접선 좌표계로 변환하는 경우의 일례를 나타내는 도면.
도 26은 용접선 좌표계에 있어서의 각 축마다의 기울기 성분의 일례를 나타내는 도면.
도 27은 용접선 좌표계에 있어서의 토치 경사각의 일례를 나타내는 도면.
도 28은 용접선 좌표계에 있어서의 토치 전진각과 토치 회전각의 일례를 나타내는 도면.
도 29는 실시예 5에 따른 컴퓨터 시스템(Y)의 개략 구성을 도시하는 블록도.
도 30은 특이점을 회피 중인 것을 통지하는 장치를 설명하기 위한 도면.

이하 첨부 도면을 참조하면서, 본 발명의 실시 형태에 대해서 설명하고, 본 발명의 이해에 제공한다. 또한, 이하의 실시 형태는, 본 발명을 구체화한 일례로서, 본 발명의 기술적 범위를 한정하는 성격의 것은 아니다.

우선, 도 1에 도시되는 개략 구성도를 참조하면서, 본 발명의 실시 형태에 따른 다관절형 로봇(X)의 구성에 대해서 설명한다.

다관절형 로봇(X)은 제어부(10), 조작부(21), 매니퓰레이터 본체(30) 등을 구비하고 있다. 또한, 다관절형 로봇(X)은, 도 1에 도시하는 구성 요소 이외에도, 일반적인 다관절형 로봇이 구비하는 다른 구성 요소도 구비하고 있지만, 여기에서는 기재를 생략하고 있다. 제어부(10)는 주제어부(11), 기억부(12), 구동 지시부(13)를 구비하고 있으며, 본 발명에 따른 다관절형 로봇의 제어 장치의 일례이다.

매니퓰레이터 본체(30)는 사람의 팔에 상당하는 J1축(31), J2축(32), J3축(33)을 갖는 제2 관절 구동계와, 사람의 손목에 상당하는 J4축(34), J5축(35), J6축(36)을 갖는 제1 관절 구동계와, 작업 대상물에 효과를 발휘하는 엔드 이펙터(39)를 구비하며, 조작된 내용에 따라서 구동하고, 사람의 상완이 행하는 것과 마찬가지 작업을 하는 로봇이다. 또한, 상기 매니퓰레이터 본체(30)는, 다관절형 로봇의 일례이다.

J1축(31) 내지 J6축(36)은 전기 모터 등에 의해 구성되며, 구동 지시부(13)로부터의 지시에 의해 정부의 방향으로 회전 구동하고, 복수의 축의 회전 구동이 연관됨으로써, 사람의 손목이나 팔의 움직임을 실현한다. 특히, 상기 제2 관절 구동계는, 상기 제1 관절 구동계의 구동축의 동작에 상관없이, 상기 다관절형 로봇의 베이스 좌표계 Σbase에 있어서의 상기 엔드 이펙터(39)의 위치(X, Y, Z)를 자유롭게 만들어 낼 수 있다.

엔드 이펙터(39)는 매니퓰레이터 본체(30)의 선단(J6축(36)의 끝)에 장착된 효과기이며, 1개의 작업점에 의해 작업을 하는 용접기(토치), 도장기, 공구, 포획기, 센서 등이다. 또한, 상기 엔드 이펙터(39)는 작업부의 일례이다.

또한, 1개의 작업점을 구비하는 토치를 싱글 토치라고 한다.

조작부(21)는 유저에 의해 조작되는 시트 키나 조작 버튼, 조작 레버 등에 의해 구성되며, 유저에 의한 조작 입력을 접수하는 입력 인터페이스이다. 예를 들어, 조작부(21)는 상기 엔드 이펙터(39)의 작업 개시 위치 및 자세와 작업 종료 위치 및 자세와 이것을 연결하는 경로의 작업 경로나 작업 시간, 목적에 따른 용접선 좌표계 Σline(작업 좌표계)에 있어서의 엔드 이펙터(39)의 자세 각도의 각 성분의 변동을 억제하기 위한 가중치 부여 정보, 이들의 내용이 기재된 프로그램 등의 입력을 접수하여 제어부(10)에 출력한다. 즉, 조작부(21)는 엔드 이펙터(39)의 작업 개시 위치 및 자세, 작업 종료 위치 및 자세를 교시하기 위한 수단이며, 상기 가중치 부여 정보를 설정하기 위한 수단이다.

기억부(12)는, 예를 들어 하드 디스크나, DRAM 등에 의해 구성된 휘발성 메모리이며, 엔드 이펙터(39)의 작업 개시 위치 및 자세와 작업 종료 위치 및 자세와 이것을 연결하는 경로의 작업 경로나 작업 시간, 엔드 이펙터(39)의 자세 각도의 각 성분의 변동을 억제하기 위한 가중치 부여 정보, 후술하는 주제어부(11)에 의해 산출된 보간점(특이점을 경유하는 경로와, 특이점을 회피하는 경로의 양쪽)에 있어서의 상기 J1축(31) 내지 J6축(36)의 각도와 속도(경우에 따라서는 더욱 가속도) 등을 기억하는 것이다.

구동 지시부(13)는 주제어부(11)로부터의 제어 지시에 따라서 샘플링 주기마다 기억부(12)에 기억된 다음의 보간점(특이점을 경유하는 경로 또는 특이점을 회피하는 경로의 한쪽)에 엔드 이펙터(39)를 움직이게 하도록 상기 J1축(31) 내지 J6축(36)의 각도 또는 속도의 정보를 판독하여, 매니퓰레이터 본체(30)에 출력한다. 또한, 주제어부(11) 및 구동 지시부(13)가 구동 지시 공정을 실행하기 위한 구동 지시 수단의 일례이다.

주제어부(11)는 CPU, ROM 등을 구비하고, CPU는 당해 다관절형 로봇(X)의 각종 제어 및 연산을 행하는 연산 수단이며, ROM은 CPU에 의해 실행되는 제어 프로그램이나 연산 프로그램, CPU가 그들 프로그램을 실행할 때에 참조하는 데이터 등이 기억되는 메모리이다.

이어서, 도 2 내지 도 6의 플로우차트를 참조하여, 다관절형 로봇(X)의 주제어부(11)가 중심으로 되어, 매니퓰레이터 본체(30)를 작동시키는 처리 수순의 일례에 대해서 설명한다. 또한, 이하에 나타내는 S11, S12, …는 처리 수순(스텝)의 식별 부호를 나타낸다.

우선, 도 2을 참조하여, 일반적인 매니퓰레이터 본체(30)의 제어와, 본 발명의 일 실시 형태에 따른 다관절형 로봇(X)의 제어의 처리 수순의 차이에 대해서 설명한다.

일반적인 매니퓰레이터 본체(30)의 제어는, 후술하는 바와 같이 주제어부(11)에 작동 궤적을 교시하는 교시 공정(스텝 S11)을 실시하고, 더 교시된 작동 궤적 상의 보간점의 위치·자세에 작동시키기 위해서 각 구동축의 각도를 산출·기억하는 공정(스텝 S12)과, 산출된 각도를 판독하여 작동을 지시하는 공정(스텝 S14)과, 작업 종료 위치·자세에 도달했는지 여부를 판별하는 공정(스텝 S15)을 실시하여, 매니퓰레이터 본체(30)를 현재의 보간점의 위치·자세로부터 다음의 보간점의 위치·자세로 작동시키고, 이후, 소정의 샘플링 기간마다 스텝 S12, S14, S15를 반복함으로써 작동 궤적을 따라 작업 개시 위치·자세로부터 작업 종료 위치·자세까지 매니퓰레이터 본체(30)를 작동시킨다. 또한, 주제어부(11)는 매니퓰레이터 본체(30)의 엔드 이펙터(39)를 미리 설정된 일정 속도로 이동시킴으로써, 상기 엔드 이펙터(39)에 의한 작업을 실행한다. 단, 일정 속도로 이동시키는 경우에는, 제1 관절 구동계의 J5축(35)이 0° 근방을 통과할 때(특이점)에, J4축(34)이나 J6축(36)의 속도가 미리 정해진 허용 범위를 초과할 우려가 있다.

따라서, 본 실시 형태에 따른 다관절형 로봇(X)의 제어는, 상기 일반적인 매니퓰레이터 본체(30)의 제어에 필요한 공정에 더하여, 후술하는 바와 같이 제1 관절 구동계의 J4축(34), J6축(36) 중 적어도 한쪽의 속도가 허용 범위를 초과하기 때문에 속도 억제 처리가 필요한지 여부를 판별하는 공정(스텝 S13)과, 속도 억제 처리가 필요한 경우에(스텝 S13; 예), 특이점을 회피한 작동 궤적 상의 보간점에 작동시키기 위해서 각 구동축의 각도를 다시 산출·기억하는 속도 억제 공정(스텝 S20)을 포함하여, 특이점을 회피하여 매니퓰레이터 본체(30)를 현재의 보간점의 위치·자세로부터 재산출된 다음의 보간점의 위치·자세로 작동시키는 것이다.

계속해서, 도 2를 참조하여, 주제어부(11)에 의해 실행되는 다관절형 로봇(X)의 제어 전체 처리에 대해서 설명하고, 각 처리의 상세에 대해서는 후술한다.

최초에, 이용자가, 로봇의 작동 궤적을, 조작부(21)에 의해 포인트 투 포인트로 교시한다. 즉, 조작부(21)에 의해 각 교시 포인트에 이동시키고, 상기 엔드 이펙터(39)의 작업 위치 및 자세를 제어부(10)의 기억부(12)에 기억시킨다(스텝 S11). 또한, 스텝 S11이 교시 공정의 일례이다.

주제어부(11)는 상기 교시된 포인트간을 소정의 샘플링 주기마다(예를 들어, 1/16초, 1/32초 등) 분할·보간하고, 엔드 이펙터(39)를 다음의 보간점으로 움직이게 하기 위한 J1축(31) 내지 J6축(36)의 각도와 제1 관절 구동계의 J4축(34) 내지 J6축(36)의 속도(경우에 따라서는 또한 가속도)를 산출하여 기억부(12)에 기억시킨다(스텝 S12). 여기에, J1축(31) 내지 J6축(36)의 각도는 ±180°로 나타난다. 물론, 각도의 지정 방법은 ±180°에 한하지 않고, 예를 들어 0°∼360° 등이어도 된다. 또한, 이 스텝 S12의 상세에 대해서는 후술한다.

그리고, 주제어부(11)는 상기 스텝 S12에서 산출된 엔드 이펙터(39)를 다음의 보간점으로 움직이게 하기 위한 제1 관절 구동계의 J4축(34) 및 J6축(36)의 한쪽 또는 양쪽의 속도나 가속도가 허용 범위 내인지 외인지(이후, 속도 억제 처리의 필요·불필요라고 함) 또는 이미 허용 범위 외의 구동축이 있다고 판별된 후의 회피 처리 중(이후, 속도 억제 처리 중이라고 함)인지를 판별한다(스텝 S13). 또한, 이 스텝 S13의 상세에 대해서는 후술한다.

여기서, 속도 억제 처리가 필요 또는 속도 억제 처리 중이라고 판별되면(스텝 S13; 예), 주제어부(11)가 엔드 이펙터(39)의 이동 속도를 변경하지 않고, 또한 상기 스텝 S12에 의해 산출된 다음의 보간점에 있어서의 엔드 이펙터(39)의 자세 데이터에 대해서 한개 또는 두개의 특정 성분의 변동을 억제하여 속도나 가속도가 상기 허용 범위 내로 되도록 상기 J4축(34) 및 J6축(36)의 한쪽 또는 양쪽의 각도를 다시 산출하고, 이에 대응하는 다른 축의 각도를 재산출하여 기억부(12)에 기억시키는 속도 억제 처리를 행한다(스텝 S20). 또한, 이 스텝 S20의 상기 설명에서는, 속도 억제 처리의 필요·불필요를 판별하는 기준으로서, 각 축의 속도나 가속도를 문제로 했지만, 상기 스텝 S20의 구체예에 따른 후술하는 설명에서는, 속도에 대해서 판별하는 경우를 예로 들어 설명한다.

한편, 속도 억제 처리가 불필요하여 속도 억제 처리 중이 아니라고 판별되거나(스텝 S13; 아니오), 속도 억제 처리(스텝 S20)가 이루어지면, 주제어부(11)의 지시에 의해 구동 지시부(13)가, 엔드 이펙터(39)를 다음의 보간점으로 움직이게 하기 위한 J1축(31) 내지 J6축(36)의 각도를 기억부(12)로부터 판독하여, 매니퓰레이터 본체(30)의 각 구동축의 액추에이터에 출력한다(스텝 S14). 이때, 속도 억제 처리가 불필요하여 속도 억제 처리 중이 아니라고 판별된 경우에는, 상기 스텝 S12에서 산출된 다음의 보간점으로 움직이게 하기 위한 J1축(31) 내지 J6축(36)의 각도에 기초하여 매니퓰레이터 본체(30)가 제어되고, J4축(34)이나 J6축(36)의 속도가 허용 범위 외로 되어 속도 억제 처리가 실행된 경우에는, 상기 스텝 S20에서 산출된 다음의 보간점으로 움직이게 하기 위한 J1축(31) 내지 J6축(36)의 각도에 기초하여 매니퓰레이터 본체(30)가 제어된다. 여기에, 이러한 스텝 S14가 구동 지시 공정의 일례이며, 이러한 처리를 실행할 때의 주제어부(11) 및 구동 지지부(13)가 구동 지시 수단에 상당한다.

계속해서, 주제어부(11)는 이동 후의 엔드 이펙터(39)의 위치 및 자세가, 작업부 종료 위치 및 자세에 도달했는지를 판별한다(스텝 S15). 작업부 종료 위치 및 자세에 도달해 있지 않은 경우에는(스텝 S15; 아니오), 소정의 샘플링 주기마다 상술한 스텝 S12 이하의 공정을 재차 실시하고, 작업부 종료 위치 및 자세에 도달한 경우에는(스텝 S15; 예), 처리를 종료한다.

이와 같이, 주제어부(11)는 소정의 샘플링 주기마다 엔드 이펙터(39)를 현재 위치 및 자세로부터 다음의 보간점으로 움직이게 하기 위한 J1축(31) 내지 J6축(36)의 각도를 산출하여, 다관절형 로봇(X)을 동작시킨다.

이어서, 도 3을 참조하여, 도 2의 스텝 S12에 해당하는 다관절형 로봇(X)의 현재의 보간점으로부터 다음의 보간점으로 이동하기 위한 J1축(31) 내지 J6축(36)의 각도와 제1 관절 구동계의 J4축(34) 내지 J6축(36)의 속도를 산출하는 처리 수순의 일례에 대해서 설명한다.

우선, 주제어부(11)는 상기 스텝 S11에 있어서 미리 교시된 작업 개시 위치 및 자세 P₀와 작업 종료 위치 및 자세 P_n 사이를 n(샘플링 주기마다에 기초하는, 임의의 값)개의 점에서 보간한 엔드 이펙터(39)의 위치 및 자세를 산출한다. 이 경우, 1 샘플링 주기마다의 이동량은, ΔP=(P_n-P₀)/n={ΔX=(X_n-X₀)/n, ΔY=(Y_n-Y₀)/n, ΔZ=(Z_n-Z₀)/n, Δα=(α_n-α₀)/n, Δβ=(β_n-β₀)/n, Δγ=(γ_n-γ₀)/n}에 의해 나타낼 수 있다. 또한, 상기 보간점 각각은, 상기 다관절형 로봇(X)의 베이스 좌표계 Σbase에 의해 나타나는 것이다.

주제어부(11)는, 도 7에 도시한 바와 같이, 현재 위치의 보간점이 P_i인 경우에, 다음의 보간점 P_i ₊₁의 위치 및 자세를 P_i ₊₁=P₀+ΔP×(i+1)이나, P_i ₊₁=Pi+ΔP에 의해 산출한다(스텝 S121). 또한, 이 스텝 S121이 보간 데이터 산출 공정의 일례이며, 이러한 처리를 실행할 때의 주제어부(11)가 보간 데이터 산출 수단에 상당한다.

계속해서, 주제어부(11)는 다음의 보간점 P_i ₊₁의 위치 및 자세(X_i ₊₁, Y_i ₊₁, Z_i+1, α_i+1, β_i+1, _i+1)로부터 역운동학 문제의 해를 구함으로써, 각 J1축(31) 내지 J6축(36)의 각도(θ₁, θ₂, θ₃, θ₄, θ₅, θ₆)를 산출하고, 기억부(12)에 기억시킨다(스텝 S122). 이때, J4축(34) 내지 J6축(36)에 대해서 역운동학 문제의 2개의 해가 얻어지지만, 여기에서는, 주제어부(11)는 그 2개의 해 중 제1 관절 구동계의 J5축(35)의 각도가, 교시된 상기 엔드 이펙터(39)의 작업 개시 위치에 있어서의 J5축(35)의 각도와 동일 부호가 되는 역운동학 문제의 해를 채용한다. 이에 의해, J5축(35)의 각도가 0°를 통과하지 않고 추이하기 때문에 특이점의 통과를 회피할 수 있다.

또한, 주제어부(11)는 후술하는 속도 억제 처리를 실행하는 경우에는, 특이점을 경유하는 경로의 보간점의 각 J1축(31) 내지 J6축(36)의 각도뿐만 아니라, 특이점을 회피하는 경로의 보간점의 각 J1축(31) 내지 J6축(36)의 각도도 각각 산출하여 기억부(12)에 기억시켜서, 후술하는 복귀 처리(도 5의 스텝 S310 및 도 6의 플로우차트)를 할 때의 비교 대상으로 한다.

여기에, 스텝 S122가 각도 산출 공정의 일례이며, 이러한 처리를 실행할 때의 주제어부(11)가 각도 산출 수단에 상당한다.

이어서, 주제어부(11)는 산출된 다음의 보간점 P_i ₊₁의 각 J4축(34) 내지 J6축(36)의 각도로부터, 기억부(12)에 기억된 현재의 보간점 P_i의 각 J4축(34) 내지 J6축(36)의 각도를 차감하고, 제1 관절 구동계의 J4축(34) 내지 J6축(36)의 속도를 산출하여 기억부(12)에 기억시킨다(스텝 S123). 또한, 후술하는 바와 같이, 속도 억제 처리(스텝 S20)를 실행하는 경우에는, 특이점을 경유하는 계의 보간점의 제1 관절 구동계의 J4축(34) 내지 J6축(36)의 속도를 산출하여 기억부(12)에 기억시키고, 후술하는 스텝 S131에 있어서 속도의 허용 범위를 산출하거나, 스텝 S211 등에 있어서 다음의 보간점의 후보로 되는 속도(필요에 따라 가속도)를 산출하기 위해서 사용한다.

여기에, 스텝 S123이 속도 산출 공정의 일례이며, 이러한 처리를 실행할 때의 주제어부(11)가 속도 산출 수단에 상당한다.

이어서, 도 4를 참조하여, 도 2의 스텝 S13에 해당하는 속도 억제 필요성 판별 처리 수순의 일례에 대해서 설명한다.

우선, 주제어부(11)는 엔드 이펙터(39)의 위치 및 자세를 현재의 보간점 Pi로부터 다음의 보간점 P_i ₊₁로 움직이게 할 때의 제1 관절 구동계의 J4축(34), J6축(36)의 속도의 허용 범위를 산출한다(스텝 S131). 예를 들어, 현재의 보간점 Pi의 속도의 값에 소정의 값이나 소정의 비율을 가감산하여 얻어지는 값을 상한·하한으로 해서, 속도의 허용 범위를 산출한다. 또한, 이 각 축의 속도의 허용 범위는, 엔드 이펙터(39)의 작동 조건(예를 들어, 용접할 때의 온도, 도장할 때의 도장막의 두께 등의 조건)이나 다관절형 로봇이 동작할 때에 진동 등의 이상 동작을 일으키지 않기 위한 조건 등, 작업 대상물의 품질에 영향을 미치지 않는 범위 내에서 미리 정해 두어도 된다. 또한, J4축(34), J6축(36)마다 개별의 허용 범위를 설정해도 된다.

이어서, 주제어부(11)는 제1 관절 구동계의 다음의 보간점에 있어서의 J4축(34) 및 J6축(36)의 한쪽 또는 양쪽의 속도가 상기 허용 범위를 초과하고 있거나, 혹은 속도 억제 처리 중을 나타내는 플래그가 세트되어 있는지 여부를 판별한다(스텝 S132).

여기서, 제1 관절 구동계의 다음의 보간점에 있어서의 J4축(34) 및 J6축(36)의 양쪽이 상기 허용 범위 내이며, 또한 속도 억제 처리 중이 아닌 경우에는(스텝 S132; 아니오), 주제어부(11)의 지시에 의해 구동 지시부(13)는 기억부(12)로부터 상기 스텝 S12에서 산출된 각 J1축(31) 내지 J6축(36)의 각도를 판독하여 매니퓰레이터 본체(30)에 출력한다(스텝 S14). 한편, 허용 범위 외의 축이 있거나, 또는 속도 억제 처리 중인 경우에는(스텝 S132; 예), 주제어부(11)는 이하에 설명하는 속도 억제 처리(스텝 S20)를 실행한다.

이하, 도 5를 참조하여, 도 2의 스텝 S20에 해당하는 다관절형 로봇(X)의 속도 억제 처리의 구체예에 대해서 설명한다.

또한, 이하에서는, 도 24에 나타낸 J5축(35)이 0도 부근에 있고, J4축(34)이나 J6축(36)의 속도가 허용 범위를 초과할 때에 당해 속도 억제 처리를 실행하는 경우를 예로 들어 설명하지만, 다른 이유에 의해 J4축(34)이나 J6축(36)의 속도가 허용 범위를 초과하는 경우에도 마찬가지이다. 따라서, 당해 속도 억제 처리는, 제1 관절 구동계의 J4축(34), J6축(36) 중 어느 하나 또는 2개의 구동축의 속도가 허용 범위를 초과하는 경우에 실시된다. 또한, 최초에 다음의 보간점에 대해서 허용 범위 외의 축이 발견되었을 때에, 현재의 보간점에서의 속도는 허용 범위 내이다.

우선, 주제어부(11)는 다음의 보간점에 대해서 속도가 상기 허용 범위 외인 구동축이 J4축(34) 및 J6축(36) 중 어느 하나 또는 양쪽이거나, 혹은 이미 속도 억제 처리 중인 플래그가 세트되어 있는 경우에는, 상기 속도 억제 처리가 J4축(34) 및 J6축(36) 중 어느 한쪽 또는 양쪽에 대해서 실행되어 있는지를 판별하여, 처리를 분기한다(스텝 S201). 허용 범위 내인지 외인지는, 상기한 바와 같이 속도가 허용 범위의 상한·하한의 사이에 있는지 여부에 따라 판별된다. 여기서, J4축(34) 및 J6축(36)의 양쪽이 속도의 허용 범위 외가 아니고, 또한 속도 억제 처리 중인 플래그가 세트되어 있지 않으면(스텝 S201;그 외), 주제어부(11)는 당해 속도 억제 처리를 종료한다.

한편, J6축(36)의 속도가 허용 범위 외인 경우에는 처리가 스텝 S211로 이행하고, J4축(34)의 속도가 허용 범위 외인 경우에는 처리가 스텝 S221로 이행하고, J4축(34) 및 J6축(36)의 양쪽의 속도가 허용 범위 외인 경우에는 처리가 스텝 S231로 이행한다.

(J6축(36)의 속도 억제 처리 스텝 S211 내지 S217)

J6축(36)의 속도가 허용 범위 외이거나 또는 J6축(36)의 속도 억제 처리 중을 나타내는 플래그가 세트되어 있는 경우(스텝 S201; J6축이 범위 외), 주제어부(11)는 현재의 보간점에 있어서의 J6축(36)의 각도에 기초하여, 상기 J6축(36)의 각도가 상기 스텝 S12에서 산출된 다음의 보간점에 있어서의 각도에 추종하고, 또한 J6축(36)의 다음의 보간점에 있어서의 속도가 상기 허용 범위 내로 되는 각도의 후보를 복수 산출한다(스텝 S211).

예를 들어, 주제어부(11)는 이하의 (21) 내지 (23)의 수순에 의해, J6축(36)의 속도의 후보를 산출한다.

(21) 우선, 하기 수학식 1에 의해 억제 기준 속도 V6b를 산출한다. 여기에, θ_now는 다음의 보간점에 있어서의 J6축(36)의 각도, θ_old는 현재의 보간점에 있어서의 J6축(36)의 각도, E는 미리 설정된 계수(예를 들어 0.1)이다.

(22) 이어서, 억제 기준 속도 V6b가 미리 정해진 속도 리미터 V_max 이상인지 여부를 판단하여, 상기 속도 리미터 V_max 이상이라고 판단한 경우에는, 억제 기준 속도 V6b를 전회의 속도 θ_old로 설정한다. 상기 속도 리미터 V_max는, 예를 들어 상기 허용 범위 내의 속도의 최대값이다.

(23) 그리고, 억제 기준 속도 V6b와 소정의 값 D(사전에 정해진 허용 범위 내의 가속도를 유지하고 있는 것을 알 수 있는 속도의 변화분의 절대값 등)에 의해 3개의 속도의 후보(Vθ_6a=V_6b-D, Vθ_6b=V_6b, Vθ_6c=V_6b+D)를 산출한다. 상기 소정의 값 D는, 예를 들어 상기 속도 리미터 V_max의 10% 정도의 값이다. 또한, Vθ_6a가 상기 속도 리미터 V_max보다 큰 경우에는, Vθ_6a=V_max라 한다.

그 후, 주제어부(11)는 현재의 보간점에 있어서의 J6축(36)의 각도 θ6으로부터 상기 속도의 각 후보(Vθ_6a, Vθ_6b, Vθ_6c)에 대응하는 다음의 보간점에 있어서의 3개의 J6축(36)의 각도의 후보(θ_6a, θ_6b, θ_6c)를 산출한다. 즉, 다음의 보간점으로서, 적어도 허용 범위를 초과하지 않는 억제 기준 속도 V6b 또는 현재의 보간점에 있어서의 속도를 계속한 각도(당연히, 허용 범위 내임)와, 그에 의해 약간의 양[±D(속도의 변화분의 절대값에 대응)]만큼 떨어진 각도가 산출된다. 여기에, 이러한 스텝 S211의 처리가 각도 후보 산출 공정의 일례이며, 이러한 처리를 실행할 때의 주제어부(11)가 각도 후보 산출 수단에 상당한다.

이어서, 주제어부(11)는 산출된 다음의 보간점에 있어서의 J6축(36)의 각도의 각 후보에 대응하는 다른 구동축 J1축(31) 내지 J5축(35)의 각도를 산출한다(스텝 S212). 예를 들어, 이하의 (1) 내지 (3)에 도시한 바와 같이, J6축(36)의 각도의 후보(θ_6a, θ_6b, θ_6c)마다 상기 스텝 S122에 있어서 산출된 다음의 보간점 P_i ₊₁의 엔드 이펙터(39)의 위치(X_i ₊₁, Y_i ₊₁, Z_i ₊₁)와, 제1 관절 구동계의 J4축(34), J5축(35)의 각도(θ₄, θ₅)를 사용하여, 제1 관절 구동계 및 제2 관절 구동계의 J1축(31) 내지 J6축(36)의 각도(θ₁, θ₂, θ₃, θ₄, θ₅, θ₆)를 재산출한다.

(1) 후보 θ_6a에 대해서, [X_i ₊₁, Y_i ₊₁, Z_i ₊₁, θ₄, θ₅, θ_6a]로부터 [θ_1a, θ_2a, θ_3a, θ₄, θ₅, θ_6a]를 산출한다.

(2) 후보 θ_6b에 대해서, [X_i ₊₁, Y_i ₊₁, Z_i ₊₁, θ₄, θ₅, θ_6b]로부터 [θ_1b, θ_2b, θ_3b, θ₄, θ₅, θ_6b]를 산출한다.

(3) 후보 θ_6c에 대해서, [X_i ₊₁, Y_i ₊₁, Z_i ₊₁, θ₄, θ₅, θ_6c]로부터 [θ_1c, θ_2c, θ_3c, θ₄, θ₅, θ_6c]를 산출한다.

이에 의해, 나중에 설명하는 도 8에 도시한 바와 같이, 속도가 허용 범위를 초과하지 않는 다음의 보간점의 후보 P'_i ₊₁이 계산되고, 이 다음의 보간점의 후보 P'_i+1을 다음의 보간점이라고 간주함으로써, J6축(36)의 속도가 허용 범위를 초과하지 않고, 원래의 보간점 P_i ₊₁(특이점)을 회피한 예외 경로(도 8의 (B) 참조)를 얻을 수 있다.

이와 같이 해서 J6축(36)의 속도를 허용 범위 내로 유지한 다음의 보간점에 있어서의 3개의 각도의 후보(θ_6a, θ_6b, θ_6c)가 얻어지면, 다음의 보간점으로서 어느 후보를 선택할지의 판단이 행해진다. 여기까지의 계산에서는, 제1 관절 구동계의 J4축(34) 내지 J6축(36)의 각도(θ₄, θ₅, θ₆)가 문제로 되어 있지만, 최종적으로는 작업의 종류에 따라서 중요시되는 엔드 이펙터(39)의 자세에 초점을 맞춰서 후보가 선택되어야만 한다. 따라서, 상기 J1축(31) 내지 J6축(36)의 각도로부터 엔드 이펙터(39)의 자세를 나타내는 데이터로의 변환이 필요하다.

그리고, 다음의 보간점으로서 어느 후보를 선택할지의 판단을 행하기 위해서, 주제어부(11)는 산출된 후보마다 다음의 보간점에 있어서의 용접선 좌표계 Σline의 엔드 이펙터(39)의 자세를 나타내는 데이터의 각 성분[토치 경사각(Rx), 토치 전진각(Ry), 토치 회전각(Rz)]을 각각 산출한다(스텝 S213). 또한, 이 스텝 S213이, 자세 데이터 산출 공정의 일례이며, 이러한 처리를 실행할 때의 주제어부(11)가 자세 데이터 산출 수단에 상당한다. 여기에, 상기 용접선 좌표계 Σline은, 도 25에 도시한 바와 같이, 엔드 이펙터(39)가 이동하는 방향(진행 방향)의 축을 X축이라 하고, 상기 X축과 중력 방향의 외적(X축×중력 방향)에 의해 나타나는 축을 Y축이라 하고, 상기 X축과 상기 Y축의 외적(X축×Y축)에 의해 나타나는 방향의 축을 Z축이라 하는 작업 좌표계이며, 도 26 내지 도 28에 도시한 바와 같이, 상기 X축의 축 둘레의 회전각이 토치 경사각(Rx)으로 나타나고, 상기 Y축의 축 둘레의 회전각이 토치 전진각(Ry)으로 나타나고, 상기 Z축의 축 둘레의 회전각이 토치 회전각(Rz)으로 나타난다.

예를 들어, 상기 (1) 내지 (3)의 후보 θ_6a 내지 θ_6c에 대해서, 이하의 (1')∼(3')에 의해 엔드 이펙터(39)의 자세를 나타내는 데이터의 각 성분을 산출한다.

(1') [θ_1a, θ_2a, θ_3a, θ₄, θ₅, θ_6a]로부터 [R_xa, R_ya, R_za]를 산출한다.

(2') [θ_1b, θ_2b, θ_3b, θ₄, θ₅, θ_6b]로부터 [R_xb, R_yb, R_zb]를 산출한다.

(3') [θ_1c, θ_2c, θ_3c, θ₄, θ₅, θ_6c]로부터 [R_xc, R_yc, R_zc]를 산출한다.

이어서, 주제어부(11)는 상기 스텝 S121에서 산출된 원래 경로 상의 다음의 보간점 P_i ₊₁에 있어서의 엔드 이펙터(39)의 위치 및 자세를 직교 좌표계 Σbase에 의해 나타낸 (X_i ₊₁, Y_i ₊₁, Z_i ₊₁, α_i+1, β_i+1, γ_i+1)을, 엔드 이펙터(39)가 이동하는 방향의 축을 X축으로 하는 용접선 좌표계 Σline[토치 경사각(Rx), 토치 전진각(Ry), 토치 회전각(Rz)]에 의해 나타내기 위해서 데이터를 변환한다(스텝 S214). 또한, 이 스텝 S214가, 자세 데이터 변환 공정의 일례이며, 이러한 처리를 실행할 때의 주제어부(11)가 자세 데이터 변환 수단에 상당한다.

또한, 직교 좌표계 Σbase로부터 용접선 좌표계 Σline으로 변환하는 식을 하기의 수학식 2)에 나타낸다.

여기서, αβγ로부터 RxRyRz를 구하기 위해서는,

그리고, 원래 J6축(36)의 속도가 허용 범위 외였으므로, 궤도를 유지하면서 J6축(36)의 속도를 억제하기 위해서는, J6축(36) 이외의 J1축(31) 내지 J5축(35) 중에서 여분으로 회전해도 되는 축을 선정하고, 그 축을 여분으로 회전시킴으로써, J6축(36)의 초과 속도에 대응하는 변동량을 억제해야만 한다. 그러나, 궤도를 유지하면서 J1축(31) 내지 J5축(35)의 어느 축을 여분으로 회전시키면 좋을지는, 각도 θ1 내지 θ6에 의해 나타나는 영역에서는 판단할 수 없다. 이에 반해, 엔드 이펙터(39)의 작업의 종류와 밀접하게 관계되는 용접선 좌표계 Σline에 있어서의 각 성분(Rx, Ry, Rz)에 의해 나타나는 영역에서는 판단할 수 있다.

따라서, 도 5에 도시한 바와 같이, 주제어부(11)는 상기 스텝 S213에 있어서 변환된 용접선 좌표계 Σline에 있어서의 엔드 이펙터(39)의 자세를 나타내는 데이터의 각 성분[토치 경사각(Rx), 토치 전진각(Ry), 토치 회전각(Rz)] 중에서 변동을 억제해야 할 성분을 나타내는 정보(가중치 부여 정보)를 취득한다(스텝 S215). 또한, 주제어부(11)는 상기 조작부(21)로부터의 입력에 의해 어느 성분의 가중치 부여를 크게 할지를 임의로 설정해도 된다. 예를 들어, 이 실시 형태에서는, 엔드 이팩터(39)는 작업점이 1개의 용접 토치이고, 용접 토치를 사용한 용접 작업의 성질 상, 토치의 작업 대상물(재료)에 대한 각도나 토치의 진행 속도는 중요하지만, 토치의 축 둘레의 회전은 중요하지 않다. 그때문에, 이 실시 형태에서는, 상기 가중치 부여 정보가, 토치 경사각(Rx), 토치 전진각(Ry)을 가중치 부여가 큰(억제해야 할) 성분으로 하고, 토치 회전각(Rz)을 가중치 부여가 작은(억제하지 않은) 성분으로 해서 미리 설정되어 있는 것으로 한다. 이 가중치 부여 정보는, 후술하는 스텝 S216의 변동을 억제한 후보를 선택하는 평가식의 각 성분에 대응하는 계수로서 사용된다.

이어서, 주제어부(11)는 산출된 다음의 보간점에 있어서의 복수의 후보 중에서 전술한 스텝 S215에 의해 취득된 억제하기 위한 가중치 부여가 큰 특정 성분[토치 경사각(Rx), 토치 전진각(Ry), 토치 회전각(Rz) 중 어느 하나 또는 둘]의 변동이 가장 적은 후보를 선택한다(스텝 S216). 예를 들어, 토치 경사각과, 토치 전진각의 변동을 억제하고, 토치 회전각의 변동을 허용하는 경우, 가중 계수 A=1, B=1, C=0이라고 하고(즉 특정 성분은 토치 경사각 및 토치 전진각), 상기 (1')∼(3')의 후보에 대해서 하기의 (1") 내지 (3")의 연산을 한다. 또한, Rx, Ry, Rz는 원래 경로 상에 있어서의 다음의 보간점의 토치 경사각, 토치 전진각, 토치 회전각이다.

상기 (1") 내지 (3")에 있어서, 변동을 억제하는 성분에 가중치 부여를 하고, 상기 스텝 S214에서 산출된 원래 경로 상의 다음의 보간점의 엔드 이펙터(39)의 자세각과, 상기 스텝 S213에서 산출된 예외 경로 상의 다음의 보간점의 각 후보의 엔드 이펙터(39)의 자세각의 차 Fa 내지 Fc가 구해지므로, 그 Fa 내지 Fc 중에서 가장 값이 작은(변동이 작은) 것을 선택하고, 선택된 후보의 구동축 J1축(31) 내지 J6축(36)의 각도를 특이점을 회피하는 동작 시의 각 축의 각도로서 채용한다. 여기에, 이러한 스텝 S216의 처리가 각도 선택 공정의 일례이며, 이러한 처리를 실행할 때의 주제어부(11)가 각도 선택 수단에 상당한다. 또한, 선택된 후보의 구동축 J1축(31) 내지 J6축(36)의 각도와 J6축(36)의 속도를 기억부(12)에 보존한다. 이에 의해, 엔드 이펙터(39)의 자세 데이터 중 미리 정해진 가중치 부여가 큰 성분으로부터 차례로 변동의 억제량이 커지도록 구동축 J1축(31) 내지 J6축(36)의 각도가 다시 산출된다. 예를 들어, 가중 계수 A=0.2, B=0, C=1인 경우, 토치 회전각(Rz)의 변동이 가장 억제되고, 토치 전진각(Ry)의 변동이 가장 커진다.

이어서, 주제어부(11)는 J6축(36)의 속도 억제 처리 중인 플래그가 세트되어 있지 않으면, 플래그를 세트해서 J6축(36)에 대한 속도 억제 처리 중인 것을 나타낸다(스텝 S217).

또한, 스텝 S211 내지 S213, S216, S217이, 재산출 공정의 일례이며, 이러한 처리를 실행할 때의 주제어부(11)가 재산출 수단에 상당한다.

상술한 처리(스텝 S211 내지 S217)를 실행함으로써, 도 8의 (A), 도 8의 (B)에 도시한 바와 같이, 현재의 보간점의 위치 Pi로부터 특이점을 경유하는 원래 경로 상의 다음의 보간점 Pi+1이 아니고, 특이점을 회피하는 예외 경로 상의 다음의 보간점 P'i+1이 발견된다.

이 연산(스텝 S201, S211 내지 S217)을 다음의 보간점이 원래 경로의 보간점이 될 때까지(스텝 S201; 그 외) 반복하여, 특이점을 회피하는 예외 경로를 산출한다. 단, 상기 제어부(10)에서는, 이 산출 처리가 사전에 통합하여 실행되는 것이 아니라, 현재의 상태와 다음의 상태에 따라서 그때마다 주제어부(11)에 의해 실행되기 때문에, 상기 주제어부(11)에 순간적으로 요구되는 처리 부하가 경감되어, 저비용화를 도모할 수 있다.

또한, 다음의 보간점의 각도의 후보(θ_6a, θ_6b, θ_6c) 중에는, 원래 허용 범위 내로 유지할 수 있는 상기 억제 기준 속도 V6b 또는 현재의 보간점의 J6축(36)의 각도가 포함되어 있기 때문에, 채용된 어느 하나의 후보에 의해 다음의 보간점의 J6축(36)의 속도를 허용 범위 내로 유지할 수 있다.

그 후에 주제어부(11)가 실행하는 J6축(36)의 속도 억제 처리로부터의 복귀 처리(스텝 S310)의 상세에 대해서는 후술한다.

(J4축(34)의 속도 억제 처리 스텝 S221 내지 S227)

J4축(34)의 속도가 허용 범위 외이거나 또는 J4축(34)의 속도 억제 처리 중을 나타내는 플래그가 세트되어 있는 경우(스텝 S201; J4축이 범위 외), 주제어부(11)는 전술한 J6축(36)의 속도와 각도의 후보를 산출하는 경우와 마찬가지로, 허용 범위 내에 수용되는 복수의 J4축(34)의 각도의 후보를 산출한다(스텝 S221). 즉, J4축(34)의 억제 기준 속도 V4b가 산출되어, 3개의 속도의 후보(Vθ_4a=V_4b-G, Vθ_4b=V_4b, Vθ_4c=V_4b+G)가 산출되고, 3개의 각도의 후보(θ_4a, θ_4b, θ_4c)가 산출된다. 또한, 소정의 값 G는 사전에 정해진 허용 범위 내의 가속도를 유지하고 있는 것을 알 수 있는 속도의 변화분의 절대값 등이다. 여기에, 이러한 스텝 S221의 처리가 각도 후보 산출 공정의 일례이다.

이하의 스텝 S222 내지 S227, S320의 처리는, 전술한 J6축(36)의 스텝 S212 내지 S217, S310의 처리와 마찬가지의 처리를 실행하기 때문에, 설명을 생략한다.

(J4축(34)·J6축(36)의 속도 억제 처리 스텝 S231 내지 S237)

J4축(34) 및 J6축(36) 양쪽의 속도가 허용 범위 외이거나 또는 J4축 및 J6축(36) 양쪽의 속도 억제 처리 중을 나타내는 플래그가 세트되어 있는 경우(스텝 S201; J4축·J6축이 범위 외), 주제어부(11)는 허용 범위 내에 수용되는 1개 또는 복수의 J4축(34)과 J6축(36)의 양쪽의 속도와 각도의 후보를 산출한다(스텝 S231).

예를 들어, 상기 스텝 S231에서는, 상기 스텝 S211과 마찬가지로 J6축(36)의 속도의 후보를 3개 산출하고, 상기 스텝 S221과 마찬가지로 J4축(34)의 속도의 후보를 3개 산출한 후, J4축(34)의 후보 3개와 J6축(36)의 후보 3개를 조합해서 9개의 후보를 산출하는 것이 생각된다. 즉, 주제어부(11)는 J6축(36)의 상기 억제 기준 속도 V6b와 상기 소정의 값 D로부터 3개의 속도의 후보(Vθ_6a=V_6b-D, Vθ_6b=V_6b, Vθ_6a=V_6b+D)를 생성하고, J4축(34)의 상기 억제 기준 속도 V4b와 소정의 값 G로부터 3개의 속도의 후보(Vθ_4a=V_4b-G, Vθ_4b=V_4b, Vθ_4a=V_4b+G)를 생성한다. 또한, 주제어부(11)는 현재의 보간점에 있어서의 J6축(36)의 각도 θ₆으로부터 상기 속도의 각 후보에 대응하는 다음의 보간점에 있어서의 3개의 J6축(36)의 각도의 후보(θ_6a, θ_6b, θ_6c)를 산출하고, 현재의 보간점에 있어서의 J4축(34)의 각도 θ₄로부터 상기 속도의 각 후보에 대응하는 다음의 보간점에 있어서의 3개의 J4축(34)의 각도의 후보(θ_4a, θ_4b, θ_4c)를 산출한다. 그리고, 이들 J4축(34)의 후보와 J6축(36)의 후보를 조합하고, (θ_4a, θ_6a), (θ_4a, θ_6b), (θ_4a, θ_6c), (θ_4b, θ_6a), (θ_4b, θ_6b), (θ_4b, θ_6c), (θ_4c, θ_6a), (θ_4c, θ_6b), (θ_4c, θ_6c)의 9개의 후보를 산출한다. 즉, 다음의 보간점에 있어서의 J4축(34)·J6축(36) 각각에 대해서, 허용 범위를 초과하고 있지 않은 현재의 보간점에 있어서의 속도를 계속한 각도와, 그에 의해 약간의 양만큼 이격된 각도가 산출된다. 여기에, 이러한 스텝 S231의 처리가 각도 후보 산출 공정의 일례이다.

이어서, 주제어부(11)는 산출된 다음의 보간점에 있어서의 J4축(34), J6축(36)의 각도의 각 후보에 대응하는 다른 구동축 J1축(31) 내지 J3축(33), J5축(35)의 각도를 산출한다(스텝 S232). 예를 들어, J4축(34)·J6축(36)의 각도의 후보(θ_4a, θ_6a) 내지 (θ_4c, θ_6c)의 조마다 상기 스텝 S122에 있어서 산출된 다음의 보간점 P_i ₊₁의 엔드 이펙터(39)의 위치(X_i ₊₁, Y_i ₊₁, Z_i ₊₁)와, 제1 관절 구동계의 J5축(35)의 각도(θ₅)를 사용해서 제2 관절 구동계의 J1축(31) 내지 J3축(33)의 각도(θ₁, θ₂, θ₃)를 재산출한다.

(1) 후보(θ_4a, θ_6a)에 대해서, [X_i ₊₁, Y_i ₊₁, Z_i ₊₁, θ_4a, θ₅, θ_6a]로부터 [θ_1aa, θ₂ _aa, θ₃ _aa, θ_4a, θ₅, θ_6a]를 산출한다.

(2) 후보(θ_4a, θ_6b)에 대해서, [X_i ₊₁, Y_i ₊₁, Z_i ₊₁, θ_4a, θ₅, θ_6b]로부터 [θ_1ab, θ2_ab, θ3_ab, θ_4a, θ₅, θ_6b]를 산출한다.

(3) 후보(θ_4a, θ_6c)에 대해서, [X_i ₊₁, Y_i ₊₁, Z_i ₊₁, θ_4a, θ₅, θ_6c]로부터 [θ_1ac, θ₂ _ac, θ₃ _ac, θ_4a, θ₅, θ_6c]를 산출한다.

(4) 후보(θ_4b, θ_6a)에 대해서, [X_i ₊₁, Y_i ₊₁, Z_i ₊₁, θ_4b, θ₅, θ_6a]로부터 [θ_1ba, θ₂ _ba, θ₃ _ba, θ_4b, θ₅, θ_6a]를 산출한다.

(5) 후보(θ_4b, θ_6b)에 대해서, [X_i ₊₁, Y_i ₊₁, Z_i ₊₁, θ_4b, θ₅, θ-_6b]로부터 [θ₁ _bb, θ₂ _bb, θ₃ _bb, θ_4b, θ₅, θ_6b]를 산출한다.

(6) 후보(θ_4b, θ_6c)에 대해서, [X_i ₊₁, Y_i ₊₁, Z_i ₊₁, θ_4b, θ₅, θ_6c]로부터 [θ_1bc, θ₂ _bc, θ₃ _bc, θ_4b, θ₅, θ_6c]를 산출한다.

(7) 후보(θ_4c, θ_6a)에 대해서, [X_i ₊₁, Y_i ₊₁, Z_i ₊₁, θ_4c, θ₅, θ_6a]로부터 [θ_1ca, θ₂ _ca, θ₃ _ca, θ_4c, θ₅, θ_6a]를 산출한다.

(8) 후보(θ_4c, θ_6b)에 대해서, [X_i ₊₁, Y_i ₊₁, Z_i ₊₁, θ_4c, θ₅, θ_6b]로부터 [θ_1cb, θ₂ _cb, θ₃ _cb, θ_4c, θ₅, θ_6b]를 산출한다.

(9) 후보(θ_4c, θ_6c)에 대해서, [X_i ₊₁, Y_i ₊₁, Z_i ₊₁, θ_4c, θ₅, θ_6c]로부터 [θ_1cc, θ₂ _cc, θ₃ _cc, θ_4c, θ₅, θ_6c]를 산출한다.

이에 의해, 속도가 허용 범위를 초과하지 않는 다음의 보간점의 후보 P'_i ₊₁이 계산되고, 이 다음의 보간점의 후보 P'_i ₊₁을 다음의 보간점으로 간주함으로써, 허용 범위를 초과하지 않는 다음의 보간점 Pi+1(특이점)을 회피하는 예외 경로를 얻을 수 있다.

이어서, 엔드 이펙터(39)의 자세에 초점을 맞춰서 후보를 선택하기 위해, J1축(31) 내지 J6축(36)의 각도의 후보를 엔드 이펙터(39)의 자세를 나타내는 데이터로 변환한다.

구체적으로, 주제어부(11)는 산출된 다음의 보간점에 있어서의 상기 용접선 좌표계의 엔드 이펙터(39)의 자세를 나타내는 데이터의 각 성분[토치 경사각(Rx), 토치 전진각(Ry), 토치 회전각(Rz)]을 산출한다(스텝 S233).

예를 들어, 상기 (1) 내지 (9)의 각도의 후보에 대해서, 이하의 (1')∼(9')에 의해 엔드 이펙터(39)의 자세를 나타내는 데이터의 각 성분을 산출한다.

(1')[θ₁ _aa _, θ₂ _aa, θ₃ _aa, θ_4a, θ₅, θ_6a]로부터 [R_xaa, R_yaa, R_zaa]를 산출한다.

(2')[θ₁ _ab, θ₂ _ab, θ₃ _ab, θ_4a, θ₅, θ_6b]로부터 [R_xab, R_yab, R_zab]를 산출한다.

(3')[θ₁ _ac, θ₂ _ac, θ₃ _ac, θ_4a, θ₅, θ_6c]로부터 [R_xac, R_yac, R_zac]를 산출한다.

(4')[θ₁ _ba, θ₂ _ba, θ₃ _ba, θ_4b, θ₅, θ_6a]로부터 [R_xba, R_yba, R_zba]를 산출한다.

(5')[θ₁ _bb, θ₂ _bb, θ₃ _bb, θ_4b, θ₅, θ_6b]로부터 [R_xbb, R_ybb, R_zbb]를 산출한다.

(6')[θ₁ _bc, θ₂ _bc, θ₃ _bc, θ_4b, θ₅, θ_6c]로부터 [R_xbc, R_ybc, R_zbc]를 산출한다.

(7')[θ₁ _ca, θ₂ _ca, θ₃ _ca, θ_4c, θ₅, θ_6a]로부터 [R_xca, R_yca, R_zca]를 산출한다.

(8')[θ₁ _cb, θ₂ _cb, θ₃ _cb, θ_4c, θ₅, θ_6b]로부터 [R_xcb, R_ycb, R_zcb]를 산출한다.

(9')[θ₁ _cc, θ₂ _cc, θ₃ _cc, θ_4c, θ₅, θ_6c]로부터 [R_xcc, R_ycc, R_zcc]를 산출한다.

그리고, 주제어부(11)는 상기 스텝 S234에서 산출된 다음의 보간점 P_i ₊₁에 있어서의 상기 용접선 좌표계의 엔드 이펙터(39)의 자세를 나타내는 데이터의 각 성분[토치 경사각(Rx), 토치 전진각(Ry), 토치 회전각(Rz)]을 산출한다(스텝 S234). 또한, 이 스텝 S234가, 자세 데이터 변환 공정의 일례이며, 이러한 처리를 실행할 때의 주제어부(11)가 자세 데이터 변환 수단에 상당한다.

이어서, 주제어부(11)는 상기 스텝 S233에 있어서 변환된 용접선 좌표계 Σline에 있어서의 엔드 이펙터(39)의 자세를 나타내는 데이터의 각 성분[토치 경사각(Rx), 토치 전진각(Ry), 토치 회전각(Rz)] 중에서 변동을 억제해야 할 성분을 나타내는 정보(가중치 부여 정보)를 취득한다(스텝 S235).

또한, 주제어부(11)는 산출된 다음의 보간점에 있어서의 복수의 후보 중에서 전술한 스텝 S235에 의해 취득된 억제하기 위한 가중치 부여가 큰 특정 성분[토치 경사각(Rx), 토치 전진각(Ry), 토치 회전각(Rz) 중 어느 하나]의 변동이 가장 적은 후보를 선택한다(스텝 S236). 예를 들어, 토치 경사각과, 토치 전진각의 변동을 억제하고, 토치 회전각의 변동을 허용하는 경우, 가중 계수 A=1, B=1, C=0으로 하여, 상기 (1')∼(9')의 후보에 대해서 하기의 (1") 내지 (9")의 연산을 한다.

상기 (1") 내지 (9")에 있어서, 변동을 억제하는 성분에 가중치 부여를 하고, 상기 스텝 S234에서 산출된 원래 경로의 다음의 보간점의 엔드 이펙터(39)의 자세각과, 상기 스텝 S233에서 산출된 예외 경로의 다음의 보간점의 각 후보의 엔드 이펙터(39)의 자세각의 차 F_aa 내지 F_cc가 구해지므로, 그 차 F_aa 내지 F_cc 중에서 가장 값이 작은(변동이 작은) 것을 선택하고, 선택된 후보의 구동축 J1축(31) 내지 J6축(36)의 각도를 특이점을 회피하는 동작 시의 각 축의 각도로 한다. 여기에, 이러한 스텝 S236의 처리가 각도 선택 공정의 일례이며, 이러한 처리를 실행할 때의 주제어부(11)가 각도 선택 수단에 상당한다. 또한, 선택된 후보의 구동축 J1축(31) 내지 J6축(36)의 각도와 J4축(34)·J6축(36)의 속도를 기억부(12)에 보존한다.

그 후, 주제어부(11)는 J4축(34)·J6축(36)의 속도 억제 처리 중인 플래그가 세트되어 있지 않으면, 플래그를 세트해서 J4축(34)·J6축(36)에 대한 속도 억제 처리 중인 것을 나타낸다(스텝 S237).

상술한 처리(스텝 S231 내지 S237)를 함으로써, 현재의 보간점의 위치 P_i로부터 특이점을 경유하는 원래 경로 상의 다음의 보간점 P_i ₊₁이 아니고, 특이점을 회피하는 예외 경로 상의 다음의 보간점 P'_i ₊₁이 발견된다.

주제어부(11)가 실행하는 J4축(34)·J6축(36)의 속도 억제 처리로부터의 복귀 처리(스텝 S330)의 상세에 대해서는 후술한다.

이어서, 도 6을 참조하여, 도 5의 스텝 S310에 해당하는 다관절형 로봇(X)의 속도 억제 처리로부터의 복귀 처리의 수순의 일례에 대해서 설명한다.

주제어부(11)는, 도 8의 (B)에 도시된 바와 같이 특이점 구간 P_i 내지 P_i ₊₁을 회피한 예외 경로 상의 다음의 보간점 P'_i ₊₁로부터, 특이점을 경유하는 원래 경로 상의 더욱 다음의 보간점 P_i ₊₂와 특이점을 회피한 예외 경로 상의 더욱 다음의 보간점 P'_i ₊₂의 각도의 차분을 계산하여, 그 차가 소정의 범위를 초과하지 않은 경우에는 P_i ₊₃으로 진행해서 이 처리를 종료하지만, 초과하는 경우에는 P'_i ₊₃으로 진행하고, 차분이 소정의 범위를 초과하고 있는 한 이 처리를 반복한다. 드디어 차분이 소정의 범위 내에 들어가는 다음의 보간점이 발견된 경우에(이 예에서는, P'_i ₊₄ 내지 P_i ₊₅의 경우)는, 보간점 P'_i ₊₄로부터 보간점 P_i ₊₅로 진행하여 이 처리를 종료한다. 따라서, 특이점을 회피하는 경로 상의 현재의 보간점 P'_i ₊₄로부터 특이점을 경유하는 경로 상의 다음의 보간점 P_i ₊₅로 복귀하는 경우를 중심으로 설명한다.

(J6축(36)의 속도 억제 처리로부터의 복귀 처리)

우선, 주제어부(11)는 상기 스텝 S122에 의해 산출된 특이점을 경유한 계의 다음의 보간점 P_i ₊₅의 J6축(36)의 각도 θ_6(i+5)와_,상기 스텝 S214에 의해 선택된 특이점을 회피한 계의 다음의 보간점 P'_i ₊₅의 J6축(36)의 각도 θ'_6(i+5)를 취득한다(스텝 S311).

이어서, 주제어부(11)는 특이점을 회피한 계의 J6축(36)의 각도 θ'_6(i+5)와 특이점을 경유한 계의 J6축(36)의 각도 θ_6(i+5)의 차분의 값이 소정의 범위 이내인지를 판별한다(스텝 S312). 또한, 상기 소정의 범위는 J6축(36)의 각도 θ'_6(i+5)를 각도 θ_6(i+5)로 치환하여 엔드 이펙터(39)를 작동시킨 경우에, 일반적으로는 진동 등의 동작 상의 문제가 발생하지 않는다고 생각되는 범위 내에서 정해진 각도차의 값으로 한다. 즉, 회피한 경로 상의 보간점이, 특이점을 경유한 원래 경로 상의 보간점이라고 간주할 수 있는 범위 내로 복귀했다고 할 수 있을 정도로 충분히 근접한 것인지 여부를 판단한다.

따라서, 차분의 값이 소정의 범위를 초과하는 경우(스텝 S312; 아니오), 주제어부(11)는 스텝 S310의 복귀 처리를 종료하고, 스텝 S14로 진행한다(도 2 참조). 차분의 값이 소정의 범위 이내인 경우(스텝 S312; 예), 주제어부(11)는 J6축(36)의 각도 θ'_6(i+5)를 각도 θ_6(i+5)로 치환할 때의 속도를 산출한다(스텝 S313). 여기에서는, 특이점을 경유한 다음의 보간점의 J6축(36)의 각도 θ_6(i+5)와 특이점을 회피한 경로의 보간점의 J6축(36)의 각도 θ'_6(i+4)의 차분에 기초하여, J6축(36)의 속도를 산출한다.

그리고, 주제어부(11)는 J6축(36)의 속도의 허용 범위를 산출한다(스텝 S314). 예를 들어, 특이점을 회피한 경로의 보간점의 J6축(36)의 속도의 값에 소정의 값이나 소정의 비율을 가감산하여 얻어지는 값을 상한·하한으로 해서, 속도의 허용 범위를 산출한다. 또한, 이 각 축의 속도의 허용 범위는, 엔드 이펙터(39)의 작동 조건(예를 들어, 용접할 때의 온도, 도장할 때의 도장막의 두께 등의 조건)이나 다관절형 로봇이 동작할 때에 진동 등의 이상 동작을 일으키지 않기 위한 조건 등, 작업 대상물의 품질에 영향을 미치지 않는 범위 내에서 미리 정해 두어도 된다.

계속해서, 주제어부(11)는 J6축(36)의 속도가 소정의 허용 범위 내인지 여부를 판별한다(스텝 S315). 또한, 이 허용 범위는 엔드 이펙터(39)를 움직이게 한 경우에 진동 등의 동작 상의 문제가 발생하지 않는 범위에서 미리 정해진 값으로 한다.

여기서, J6축(36)의 속도가 소정의 허용 범위 내인 경우(스텝 S315; 예), 주제어부(11)는 다음의 보간점을 P'_i ₊₅로부터 P_i ₊₅로 변경하여, J6축(36)의 각도를 θ_6(i+5)라 하고, 이에 대응하는 다른 모든 구동축 J1축(31) 내지 J5축(35)의 각도를 산출한다(스텝 S316). 주제어부(11)는 산출된 모든 구동축 J1축(31) 내지 J6축(36)의 각도를 기억부(12)에 기억시킨다. 또한, 주제어부(11)는 J6축(36)의 속도 억제 처리 중을 나타내는 플래그를 내린다(스텝 S317).

한편, J6축(36)의 속도가 소정의 허용 범위 외인 경우(스텝 S315; 아니오), 주제어부(11)는 각도 θ_6(i+5)와 각도 θ'_6(i+5)의 중간의 각도 θ"_6(i+5)를 J6축(36)의 각도로 하고, 이 J6축(36)의 각도 θ"_6(i+5)를 포함하는 보간점을 다음의 보간점 P"_i+5로 하고(스텝 S318), 이에 대응하는 다른 모든 구동축 J1축(31) 내지 J5축(35)의 각도를 산출한다(스텝 S319). 즉, 주제어부(11)는 J6축(36)의 각도 θ'_6(i+5)를 각도 θ₆ _(i+5)로 치환할 때에, 상기 구동축 J6축(36)의 각도를 θ'_6(i+5)로부터 θ_6(i+5)로 급격하게 변화시키지 않고, 소정의 변동 범위 내에서 서서히 변동시킨다.

또한 주제어부(11)는 산출된 모든 구동축 J1축(31) 내지 J6축(36)의 각도를 기억부(12)에 기억시킨다.

[J4축(34)의 속도 억제 처리로부터의 복귀 처리]

스텝 S320에 해당하는 J4축(34)의 속도 억제 처리로부터의 복귀 처리는, 상술한 J6축(36)의 속도 억제 처리로부터의 복귀 처리(스텝 S311 내지 S319)와 마찬가지의 처리를 하기 때문에 설명을 생략한다.

[J4축(34)·J6축(36)의 속도 억제 처리로부터의 복귀 처리]

스텝 S330에 해당하는 J4축(34)·J6축(36)의 속도 억제 처리로부터의 복귀 처리는, 각각 J4축(34)의 복귀 처리와 J6축(36)의 복귀 처리의 양쪽을 함께 실시하는 것 이외에는 J6축(36)의 속도 억제 처리로부터의 복귀 처리와 마찬가지이므로 설명을 생략한다.

이상으로 나타낸 제어 방법에 의해, 다관절형 로봇(X)의 주제어부(11)가 중심으로 되어, 변동을 억제하기 위한 가중치 부여가 큰 특정 성분[토치 경사각(Rx), 토치 전진각(Ry), 토치 회전각(Rz) 중 어느 하나 또는 둘]의 변동을 억제하면서 특이점을 회피해서 매니퓰레이터 본체(30)에 비치된 엔드 이펙터(39)의 위치 및 자세를 작동시킴으로써, 본래의 교시 포인트에 기초하는 보간점을 크게 벗어나지 않고, 또한 엔드 이펙터(39)의 이동 속도를 변경하지 않고 모든 축의 속도가 허용 범위 내에 들어가는 작업 궤도를 얻을 수 있다.

예를 들어, 도 24에 도시한 바와 같이, J5축(35)이 0도 부근에서, J4축(34)과 J6축(36)의 각도가 급격하게 변화하는(즉 속도가 허용 범위 외가 되는) 특이점이 있는 경우에, 상술한 속도 억제 처리를 실행함으로써, 토치 경사각(Rx), 토치 전진각(Ry)의 변동을 억제하면서, 도 9에 도시한 바와 같이, J4축(34)과 J6축(36)의 각도의 변동을 완만하게 하여, J4축(34)과 J6축(36)의 속도를 허용 범위 내의 속도로 할 수 있다.

또한, 도 10에 도시한 바와 같이, 속도 제한을 하지 않는 경우(일점쇄선)에는 특이점 부근에서 J4축(34)의 속도가 급격하게 변화하지만, 단순히 속도 제한을 한 경우(실선)와 본 발명의 제어 방법을 실시한 경우(파선)는 속도가 허용 범위 내로 억제된다.

단, 도 11에 도시한 바와 같이 단순히 속도 제한을 한 경우에는, 토치 전진각이나 토치 경사각의 자세 변동각이 크게 변동해 버린다.

그러나, 도 12에 도시한 바와 같이, 본 발명의 제어 방법을 실시하면, 가중치 부여가 작은(속도를 억제할 필요가 없는) 토치 회전각은 크게 변동하지만, 가중치 부여가 큰 토치 경사각과 토치 전진각의 자세 변동각의 변동을 억제할 수 있다.

또한, 도 13의 (A)에 도시한 바와 같이, J1축(31)이 상하 방향으로 구동하고, J2축(32)과 J3축(33)의 구동하는 방향이 동일한 방향이어도 좋고, 또한 도 13의 (B)에 도시한 바와 같이 J1축(31), J2축(32)이 좌우 방향으로 구동하고, J3축(33)이 상하 방향으로 구동하는 것이어도 좋다. 또한, 구동축이 상하 방향으로 구동하는 경우에는, 각 구동축을 상하시키기 위한 모터가 회전하는 각도의 차분 뿐만 아니라, 각 구동축이 상하하는 이동 거리의 차분을 바탕으로 속도를 산출해도 된다.

또한, 상기 설명에서는, 속도가 허용 범위를 초과하는 경우에, 다른 축의 변동량을 증가시킴으로써 상기 축의 속도를 억제하는 방법에 대해서 설명했지만, 상기 설명에서 얻어진 속도의 변화량(가속도)이, 소정의 허용 범위를 초과하는 경우에, 상기 축의 변동량을 억제하여, 가속도가 허용 범위에 들어가도록 하는 각 축의 각도의 데이터를 산출해도 된다. 수단으로서는, 속도를 그 변화량인 가속도로 치환하여 계산하면 된다.

예를 들어, 상기 스텝 S123에 있어서, 주제어부(11)가 속도 대신에 가속도를 산출하고, 상기 스텝 S131에 있어서, 주제어부(11)가 가속도의 허용 범위를 산출하고, 상기 스텝 S132에 있어서, 주제어부(11)가 스텝 S123에 의해 산출된 가속도가, 스텝 S131에 의해 산출된 가속도의 허용 범위 외인지를 판별한다. 그리고, 허용 범위 외인 경우에는, 가속도가 상기 허용 범위에 들어가도록 하는 각 축의 각도의 데이터를 산출한다. 이때 가속도의 후보의 산출 시에 사용하는 상기 소정의 값 D나 상기 소정의 값 G는 사전에 정해진 허용 범위 내의 가가속도를 유지하고 있는 것을 알 수 있는 가속도의 변화분의 절대값 등이다. 또한, 속도를 가속도로 치환하고, 가속도를 가가속도(jerk)로 치환하는 것 뿐이므로, 여기에서는 그 상세한 설명을 생략한다.

실시예 1

본 실시예 1에서는, 상기 실시 형태와 다른 다관절형 로봇(X)의 제어 방법에 대해서 설명한다.

상기 실시 형태에서 설명한 방법에 의해 다관절형 로봇(X)을 제어하면, 상기 J5축(35)의 각도의 부호가 작업 개시 위치 및 작업 종료 위치에서 다른 부호가 되는 경우에도, 상기 J5축(35)의 각도가 0°를 통과하지 않도록 제어되기 때문에, 상기 작업 종료 위치에 있어서의 상기 J5축(35)의 각도가 본래의 목표값과 다르다. 이 경우에는, 예를 들어 도 15에 도시한 바와 같이, 나머지 J4축(34)이나 J6축(36)의 각도가 급격하게 변화할 우려나 동작 한계 각도(예를 들어 ±180°)를 초과할 우려가 있다.

또한, 다관절형 로봇(X)에 있어서, 하나의 자세 데이터로부터 구해지는 역운동학 문제의 해는 2개 존재하고, 예를 들어 도 15에 도시한 바와 같이, J6축(36)의 각도의 변화량이 급격하게 증감하는 경우에는, 상기 J6축(36)의 별해가 상기 J6축(36)의 현재의 각도에 가까워지게 된다. 따라서, 본 실시예 1에서는, 역운동학 문제에 의해 구해지는 2개의 해를 이용함으로써, J4축(34)이나 J6축(36)의 급격한 변화를 방지하기 위한 구성에 대해서 설명한다.

여기에, 도 14는 다관절형 로봇(X)의 다른 제어 방법을 설명하기 위한 플로우차트이다. 또한, 상기 실시 형태에서 설명한 도 2의 처리 내용과 마찬가지의 처리 수순에 대해서는 동일한 부호를 붙이고 그 상세한 설명을 생략한다.

우선, 본 실시예 1에 관한 구성에서도, 상기 스텝 S13에서 속도 억제 처리가 필요하다고 판단될 때까지의 동안은(S13의 아니오측), 주제어부(11)가 엔드 이펙터(39)를 다음의 보간점으로 움직이게 하기 위한 J1축(31) 내지 J6축(36)의 각도로서, J5축(35)의 각도의 부호가 상기 작업 개시 위치에 있어서의 상기 J5축(35)의 각도와 동일 부호로 되는 역운동학 문제의 해를 산출한다(S12). 또한, 스텝 S12에서는, 그 산출된 J4축(34) 내지 J6축(36)에 대응하는 속도도 산출된다.

또한, 계속되는 스텝 S14에 있어서, 주제어부(11)는 구동 지시부(13)를 제어함으로써, 엔드 이펙터(39)를 다음의 보간점으로 움직이게 하기 위한 J1축(31) 내지 J6축(36)의 각도로서, J5축(35)의 각도의 부호가 상기 작업 개시 위치에 있어서의 상기 J5축(35)의 각도와 동일 부호로 되는 해를 기억부(12)로부터 판독하여, 매니퓰레이터 본체(30)의 각 구동축의 액추에이터에 출력시킨다. 이에 의해, J1축(31) 내지 J6축(36)은 엔드 이펙터(39)가 작업 종료 위치에 있어서의 위치 및 자세로 이동할 때까지(S15의 예측), J5축(35)의 각도의 부호가 상기 작업 개시 위치에 있어서의 상기 J5축(35)의 각도와 동일 부호로 되는 해를 목표값으로 해서 동작하게 된다(S15의 아니오측).

(스텝 S411 내지 S412)

한편, 상기 스텝 S13에서 속도 억제 처리가 필요하다고 판단된 경우(S13의 예측), 주제어부(11)는 계속되는 스텝 S411에 있어서, 상기 스텝 S12와 마찬가지로, 다음의 보간점에 있어서의 J1축(31) 내지 J6축(36)의 각도와 J4축(34) 내지 J6축(36)의 속도를 산출한다. 또한, 스텝 S411에서는, 그 산출된 J4축(34) 내지 J6축(36)에 대응하는 속도도 산출된다.

단, 이때, 주제어부(11)는 엔드 이펙터(39)를 다음의 보간점으로 움직이게 하기 위한 J1축(31) 내지 J6축(36)의 각도로서 구해지는 역운동학 문제의 2개의 해 중, J5축(35)의 각도의 부호가 상기 작업 개시 위치에 있어서의 상기 J5축(35)의 각도와 다른 부호로 되는 역운동학 문제의 해(별해)를 산출한다. 여기에, 역운동학 문제의 2개의 해로서 구해지는 J4축(34), J5축(35), J6축(36)의 각도를 θ41, θ51, θ61과 θ41', θ51', θ61'로 하면, 그 값에는, θ41-θ41'=±π, θ51+θ51'=0, θ61-θ61'=±π의 관계가 있다. 예를 들어, (θ41, θ51, θ61)=(90°, 75°, 0°)일 때, (θ41', θ51', θ61')=(-90°, -75°, 180°)가 된다.

이와 같이, 본 실시예 1에 있어서, 주제어부(11)는 J4축(34)이나 J6축(36)의 속도가 상기 허용 범위 내인 경우에는, J5축(35)의 다음의 보간점에 있어서의 각도가 작업 개시 위치에 있어서의 상기 J5축(35)의 각도와 동일 부호가 되는 역운동학 문제의 해를 채용하고, 상기 허용 범위 외인 경우에는, J5축(35)의 다음의 보간점에 있어서의 각도가 작업 개시 위치에 있어서의 상기 J5축(35)의 각도와 다른 부호가 되는 역운동학 문제의 해를 채용한다. 여기에, 이러한 처리가 각도 산출 공정의 일례이며, 이러한 처리를 실행할 때의 주제어부(11)가 각도 산출 수단에 상당한다.

그리고, 계속되는 스텝 S412에 있어서, 주제어부(11)는 상기 스텝 S13과 마찬가지로, 상기 스텝 S412에서 산출된 J4축(34), J6축(36)의 속도가 허용 범위 외이거나, J4축(34), J6축(36)에 대한 속도 억제 처리가 실행 중인지 등, 상기 속도 억제 처리의 필요성을 판단한다.

여기서, 속도 억제 처리가 필요하다고 판단되면(S412의 예측), 처리는 스텝 S413으로 이행하고, 속도 억제 처리가 필요하지 않다고 판단되면(S412의 아니오측), 처리는 스텝 S414로 이행한다.

(스텝 S413)

상기 스텝 S413에서는, 도 5에 도시한 상기 스텝 S20과 마찬가지로, J4축(34) 및 J6축(36) 중 어느 한쪽 또는 양쪽의 속도를 억제하기 위한 속도 억제 처리가 실행된다. 단, 이때, 주제어부(11)는 엔드 이펙터(39)를 다음의 보간점으로 움직이게 하기 위한 J1축(31) 내지 J6축(36)의 각도로서 구해지는 역운동학 문제의 2개의 해 중, J5축(35)의 각도의 부호가 상기 작업 개시 위치에 있어서의 상기 J5축(35)의 각도와 다른 부호가 되는 해를 채용하기 때문에, J1축(31) 내지 J6축(36)은 그 해를 목표값으로 해서 동작하게 된다. 또한, 상기 스텝 S413에 있어서의 처리 내용은, J4축(34)이나 J6축(36)의 각도가 다른 것 뿐이며, 그 외에는 상기 스텝 S20과 마찬가지이기 때문에 그 설명을 생략한다.

(스텝 S414 내지 S415)

그리고, 스텝 S414에서는, 상기 스텝 S14와 마찬가지로, 주제어부(11)가 구동 지시부(13)를 제어함으로써, 엔드 이펙터(39)를 다음의 보간점으로 움직이게 하기 위한 J1축(31) 내지 J6축(36)의 각도가 기억부(12)로부터 판독되어, 매니퓰레이터 본체(30)의 각 구동축의 액추에이터에 출력된다. 이때, J1축(31) 내지 J6축(36)의 각도는, 상기 스텝 S411 또는 상기 스텝 S413에 있어서, J5축(35)의 각도의 부호가 상기 작업 개시 위치에 있어서의 상기 J5축(35)의 각도와 다른 부호가 되는 해를 목표값으로 해서 산출된 것이다. 따라서, J5축(35)의 각도는 0°를 통과하여 천이하는 것이 된다.

그 후, J1축(31) 내지 J6축(36)은, 엔드 이펙터(39)가 작업 종료 위치에 있어서의 위치 및 자세로 이동할 때까지(S415의 예측), J5축(35)의 각도의 부호가 상기 작업 개시 위치에 있어서의 상기 J5축(35)의 각도와 다른 부호가 되는 해를 목표값으로 해서 동작하게 된다(S415의 아니오측).

여기에, 도 15는 J5축(35)의 각도의 부호가 작업 개시 위치 및 작업 종료 위치에서 다른 경우(플러스로부터 마이너스, 마이너스로부터 플러스로 변화하는 경우), 즉 0°를 통과하는 경우에, 종래의 특이점을 경유하는 경로의 J4축 J5축 J6축의 동작 결과의 일례를 나타내고 있다. 도 15에 도시하는 예에서는, J5축(35)의 각도가 0°를 통과하지 않도록 제어되고, 작업 종료 위치에 있어서의 J5축(35)의 각도가 플러스로 되어 있기 때문에, 다른 J4축(34)이나 J6축(36)의 각도의 변화량이 커지고 있는 것을 알 수 있다.

한편, 도 16은 J5축(35)의 각도의 부호가 작업 개시 위치 및 작업 종료 위치에서 다른 경우에, 본 실시예 1에 따른 다관절형 로봇(X)의 제어 방법을 채용한 결과의 일례를 나타내고 있다. 도 16에 나타내는 예에서는, J5축(35)에 0°를 통과시킴으로써, 다른 J4축(34)이나 J6축(36)의 각도의 변화량이 억제되고 있는 것을 알 수 있다.

또한, 도 17은 본 실시예 1에 따른 다관절형 로봇(X)의 제어 방법을 채용하지 않고 등속 동작시킨 경우의 엔드 이펙터(39)의 자세 데이터의 변화를 나타내고 있고, 도 18은 본 실시예 1에 따른 제어 방법을 채용한 경우의 엔드 이펙터(39)의 자세 데이터의 변화를 나타내고 있다. 또한, 도 17, 도 18에 있어서 종축은 본래의 보간점에 있어서의 엔드 이펙터(39)의 자세 데이터에 대한 변동량, 횡축은 시간을 나타내고 있다.

도 17에 도시한 바와 같이, 본 실시예 1에 따른 제어 방법을 채용하지 않는 경우에는, 경사각 및 전진각의 변동이 크지만, 도 18에 도시한 바와 같이, 본 실시예 1에 따른 제어 방법을 채용하여 경사각 및 전진각의 가중치 부여(변동의 억제량의 크기)를 크게 설정해 두면, 용접에 불필요한 회전각의 정밀도를 희생으로 해서 경사각 및 전진각의 변동을 억제할 수 있다.

실시예 2

또한, 상기 실시 형태에서 설명한 바와 같이, 다관절형 로봇(X)에 있어서, 상기 작업 개시 위치로부터 상기 작업 종료 위치의 사이에서 J5축(35)의 각도가 0° 근방으로 되는 특이점을 통과하지 않도록 제어하는 경우에는, 상기 작업 개시 위치 및 상기 작업 종료 위치에 있어서의 J5축(35)의 각도의 부호가 다른 부호인 경우에, 상기 작업 종료 위치에 있어서의 J5축(35)의 각도가 원하는 각도와 달라져 버리게 된다.

한편, 상기 실시예 1에서 설명한 바와 같이, 다관절형 로봇(X)에 있어서, 상기 작업 개시 위치로부터 상기 작업 종료 위치 사이에서 J5축(35)의 각도가 0° 근방을 통과하도록 제어하는 경우에는, 상기 작업 개시 위치 및 상기 작업 종료 위치에 있어서의 J5축(35)의 각도의 부호가 동일 부호인 경우에, 상기 작업 종료 위치에 있어서의 J5축(35)의 각도가 원하는 각도와 달라져 버리게 된다.

이들의 경우에는, 예를 들어 다관절형 로봇(X)의 J1축(31) 내지 J6축(36)이나 엔드 이펙터(39) 등에 접속된 배선이 얽히는 것이나, 그 후의 조작에 의해 J4축(34) 내지 J6축(36)의 구동 한계점을 초과할 우려가 있는 것 등이 문제가 된다.

따라서, 상기 작업 개시 위치 및 상기 작업 종료 위치에 있어서의 상기 제1 관절 구동계의 J5축(35)의 부호가 동일 부호인지 다른 부호인지에 따라, 상기 실시 형태에 따른 제어 방법과 상기 실시예 1에 따른 제어 방법을 전환하여 실행하는 것이 생각된다.

여기에, 도 19는 본 실시예 2에 따른 다관절형 로봇(X)의 제어 방법을 설명하기 위한 플로우차트이다. 또한, 상기 실시 형태 및 상기 실시예 1과 마찬가지의 처리 수순에 대해서는 동일한 부호를 붙이고 그 설명을 생략한다.

구체적으로, 주제어부(11)는 상기 스텝 S13의 예측과 상기 스텝 S20 및 상기 스텝 S411 사이에 있어서, 상기 작업 개시 위치 및 상기 작업 종료 위치에 있어서의 상기 제1 관절 구동계의 J5축(35)의 각도의 부호가 동일 부호인지 다른 부호인지를 판단하기 위한 처리(스텝 S511의 예측)을 실행한다.

그리고, 주제어부(11)는 상기 작업 개시 위치 및 상기 작업 종료 위치에 있어서의 상기 제1 관절 구동계의 J5축(35)의 각도의 부호가 동일 부호인 경우에는(스텝 S511의 예측), 상기 스텝 S20으로 이행한다. 이 경우에는, 상기 제1 관절 구동계의 J5축(35)의 각도의 부호가 상기 작업 개시 위치에 있어서의 각도와 동일 부호인 역운동학 문제의 해가 채용된다.

한편, 상기 작업 개시 위치 및 상기 작업 종료 위치에 있어서의 상기 제1 관절 구동계의 J5축(35)의 각도의 부호가 다른 부호인 경우(스텝 S511의 아니오측), 주제어부(11)는 처리를 스텝 S411로 이행시킨다. 이 경우에는, 상기 제1 관절 구동계의 J5축(35)의 각도의 부호가 상기 작업 개시 위치에 있어서의 각도와 다른 부호인 역운동학 문제의 해가 채용된다. 즉, 상기 작업 개시 위치 및 상기 작업 종료 위치에 있어서의 상기 제1 관절 구동계의 J5축(35)의 각도의 부호가 다른 부호인 경우에는, J5축(35)이 0° 근방에 근접하여 J4축(34)이나 J6축(36)의 속도가 허용 범위 내인 동안에는, 상기 제1 관절 구동계의 J5축(35)의 각도의 부호가 상기 작업 개시 위치에 있어서의 각도와 동일 부호인 역운동학 문제의 해가 채용되고, 상기 J5축(35)이 0° 근방에 근접하여 J4축(34)이나 J6축(36)의 속도가 허용 범위 외로 된 경우에는, 상기 제1 관절 구동계의 J5축(35)의 각도의 부호가 상기 작업 개시 위치에 있어서의 각도와 다른 부호인 역운동학 문제의 해가 채용된다. 이러한 처리가 각도 산출 공정의 일례이며, 이러한 처리를 실행할 때의 주제어부(11)가 각도 산출 수단에 상당한다.

이에 의해, 교시된 작업 종료 위치에 있어서의 J5축(35)의 각도의 부호가 그 교시한 바의 부호와 일치하게 되기 때문에, 예를 들어 다관절형 로봇(X)의 J1축(31) 내지 J6축(36)이나 엔드 이펙터(39) 등에 접속된 배선의 얽힘이나, 그 후의 조작에 의해 J4축(34) 내지 J6축(36)의 구동 한계점을 초과하는 우려 등을 방지할 수 있다.

실시예 3

또한, 상기 설명에서는, 다관절형 로봇(X)의 엔드 이펙터(39)의 작업점이 1개인 경우에 대해서 설명했지만, 도 20에 도시한 바와 같이, 2개의 작업점을 갖는 엔드 이펙터(39A)를 사용한 다관절형 로봇(X1)을 제어해도 된다.

다관절형 로봇(X1)은, 엔드 이펙터(39A)를 구비하고, 엔드 이펙터(39) 이외의 다른 구성은 다관절형 로봇(X)과 마찬가지이다. 또한, 상기 다관절형 로봇(X1)의 제어는, 상기 다관절형 로봇(X)의 제어 처리와는, 도 5에 도시한 스텝 S215, S216(스텝 S225, S226 및 S235, S236을 포함)의 처리 이외에는 마찬가지의 처리를 한다. 또한, 이하의 설명에서는, 다관절형 로봇(X1)과 다관절형 로봇(X)의 동일한 구성 및 동일한 처리는 설명을 생략한다.

우선, 도 1에 도시한 다관절형 로봇(X)과의 구성이 다른 부분에 대해서 설명한다.

엔드 이펙터(39A)는, 매니퓰레이터 본체(30)의 선단(J6축(36)의 끝)에 장착되고, 작업 대상물에 대해서 효과를 주는 2개의 작업점을 갖는 효과기이며, 작업점으로서 용접기(토치), 도장기, 공구, 포획기, 센서 등이 있다. 또한, 2개의 작업점을 구비하는 토치를 탠덤 토치라고 한다.

이어서, 도 5의 플로우차트를 참조하여, 본 발명의 실시 형태와 처리가 다른 속도 억제 처리의 스텝 S215, S216의 처리를 중심으로 설명한다.

우선, 주제어부(11)는 스텝 S201을 실행한다. 여기에서는, J6축(36)의 속도가 허용 범위 외이거나 또는 J6축(36)의 속도 억제 처리 중을 나타내는 플래그가 세트되어 있는 경우(스텝 S201; J6축이 범위 외)에 해당하고 있는 것으로 한다. 또한 스텝 S211 내지 스텝 S214를 실행한다.

이어서, 상기 스텝 S215에 있어서, 주제어부(11)는 상기 스텝 S213에 있어서 변환된 용접선 좌표계 Σline에 있어서의 엔드 이펙터(39A)의 자세를 나타내는 데이터의 각 성분[토치 경사각(Rx), 토치 전진각(Ry), 토치 회전각(Rz)] 중에서 변동을 억제해야 할 성분의 정보(가중치 부여 정보)를 취득한다. 그때, 이 실시예 1에서는, 엔드 이펙터(39A)는 작업점이 2개인 용접 토치이고, 용접 토치를 사용한 용접 작업의 성질 상, 토치의 축 둘레의 회전각과 토치의 진행 속도는 중요하다. 그때문에, 이 실시예 1에서는 표준 설정으로서, 토치 회전각(Rz)을 가중치 부여가 큰(억제해야 하는) 성분으로 하고, 토치 전진각(Ry)을 가중치 부여가 작은(억제하지 않는) 성분으로 해서 미리 설정된다.

이어서, 상기 스텝 S216에 있어서, 주제어부(11)는 산출된 다음의 보간점에 있어서의 복수의 후보 중에서 전술한 스텝 S215에 의해 취득된 억제하기 위한 가중치 부여가 큰 특정 성분[토치 경사각(Rx), 토치 전진각(Ry), 토치 회전각(Rz) 중 어느 하나 또는 둘]의 변동이 가장 적은 후보를 선택한다. 예를 들어, 토치 회전각(Rz)의 변동을 억제하고, 토치 전진각(Ry)의 변동을 허용하는 경우, 가중 계수 A=0.2, B=0, C=1이나, 가중 계수 A=0, B=0, C=1(즉 특정 성분은, 토치 경사각 및 토치 회전각, 혹은 토치 회전각임)로 하고, 상기 스텝 S213에 의해 산출된 각 후보 (1')∼(3')에 대해서 하기의 (1") 내지 (3")의 연산을 한다.

(1") 내지 (3")에 있어서, 특이점을 경유하는 경로 상의 다음의 보간점의 엔드 이펙터(39A)의 자세각과, 특이점을 회피하는 경로 상의 다음의 보간점의 각 후보의 엔드 이펙터(39A)의 자세각의 차가 구해지므로, Fa 내지 Fc 중에서 가장 값이 작은(변동이 작은) 것을 선택하고, 선택된 후보의 구동축 J1축(31) 내지 J6축(36)의 각도를 특이점을 회피하는 동작 시의 각 축의 각도로 한다.

계속해서, 주제어부(11)는 스텝 S217 이후의 처리를 한다. 또한, 실시 형태와 마찬가지로, 속도가 허용 범위 외라고 판별된 각 구동축에 대응하여, 스텝 S211 내지 S216 대신에 스텝 S221 내지 S226 혹은 스텝 S231 내지 S236을 실행한다.

이상으로 나타낸 제어 방법에 의해, 다관절형 로봇(X1)의 주제어부(11)가 중심으로 되어, 억제하기 위한 가중치 부여가 큰 토치 경사각(Rx), 토치 회전각(Rz)의 변동을 억제하면서 특이점을 회피해서 매니퓰레이터 본체(30)에 비치된 엔드 이펙터(39A)의 위치 및 자세를 작동시키고, 본래의 교시 포인트에 기초하는 보간점을 크게 벗어나지 않고, 또한 엔드 이펙터(39A)의 이동 속도를 변경하지 않고 모든 축의 속도가 허용 범위 내에 들어가는 작업 궤도를 얻을 수 있다.

또한, 도 21에 도시한 바와 같이 실시예 1의 제어 방법을 실시하면, 억제하기 위한 가중치 부여가 작은 토치 전진각(Ry)은 크게 변동하지만, 억제하기 위한 가중치 부여가 큰 회전각(Rz)의 자세 변동각을 억제할 수 있다.

실시예 4

상기 설명에서는, 다관절형 로봇(X)의 제2 관절 구동계의 구동축이 J1축(31) 내지 J3축(33)의 3개의 축을 구비한 매니퓰레이터 본체(30)의 제어 방법에 대해서 설명했지만, 제2 관절 구동계의 구동축이 4 이상의 매니퓰레이터여도 된다. 또한, 이하의 설명에서는, 다관절형 로봇(X)과 다관절형 로봇(X2)의 동일한 구성 및 동일한 처리는 설명을 생략한다.

우선, 도 1에 도시한 다관절형 로봇(X)과 도 22에 도시하는 다관절형 로봇(X2)의 구성이 다른 부분에 대해서 설명한다.

도 22에 도시하는 다관절형 로봇(X1)은, 제어부(10), 조작부(21), 매니퓰레이터 본체(30A) 등을 구비하고 있다. 매니퓰레이터 본체(30A)는 제1 관절 구동계의 3개의 구동축의 J4축(34), J5축(35), J6축(36)과, 제2 관절 구동계의 4개의 구동축의 J1축(31), J2축(32), J3축(33), J7축(37)을 구비한다.

J7축(37)은 J1축(31) 내지 J6축(36)과 마찬가지로, 전기 모터 등에 의해 구성되어, 구동 지시부(13)로부터의 지시에 의해 정부의 방향으로 회전 구동하고, 복수의 축의 회전 구동이 연관됨으로써, 사람의 손목이나 팔의 움직임을 실현한다. 그 중에서도 상기 J7축(37)을 포함하는 제2 관절 구동계의 4개의 구동축은, 사람의 팔에 상당하는 동작을 행하여, 엔드 이펙터(39)의 위치를 이동한다.

이어서, 도 2 내지 도 6의 플로우차트를 참조하여, 본 발명의 실시 형태와 처리가 다른 부분에 대해서 설명한다.

실시 형태에 있어서는, 보간점의 위치 및 자세 Pi(X_i, Y_i, Z_i, α_i, β_i, γ_i)로부터 역운동학 문제의 해로서 각 J1축(31) 내지 J6축(36)의 각도(θ₁, θ₂, θ₃, θ₄, θ₅, θ₆)를 산출하고, 보간점에 있어서의 각 J1축(31) 내지 J6축(36)의 각도(θ₁, θ₂, θ₃, θ₄, θ₅, θ₆)로부터 순운동학 문제의 해로서 위치 및 자세 Pi(X_i, Y_i, Z_i, α_i, β_i, γ_i)를 산출했지만, 실시예 4에서는 보간점의 위치 및 자세 Pi(X_i, Y_i, Z_i, α_i, β_i, γ_i)로부터 역운동학 문제의 해로서 각 J1축(31) 내지 J7축(37)의 각도(θ₁, θ₂, θ₃, θ₄, θ₅, θ₆, θ₇)를 산출하고, 보간점에 있어서의 각 J1축(31) 내지 J7축(37)의 각도(θ₁, θ₂, θ₃, θ₄, θ₅, θ₆, θ₇)로부터 순운동학 문제의 해로서 위치 및 자세 Pi(X_i, Y_i, Z_i, α_i, β_i, γ_i)를 산출하도록, 각도의 변수의 수가 6인지 7인지의 점이 다르다.

상기 스텝 S122에 있어서, 주제어부(11)는 역운동학 문제의 해를 구함으로써, 다음의 보간점의 위치 및 자세 P_i ₊₁(X_i ₊₁, Y_i ₊₁, Z_i ₊₁, α_i+1, β_i+1, γ_i+1)로부터 각 J1축(31) 내지 J7축(37)의 각도(θ₁, θ₂, θ₃, θ₄, θ₅, θ₆, θ₇)를 산출하여, 기억부(12)에 기억시킨다.

이어서, 상기 스텝 S123 이후와 마찬가지의 처리를 실행한다. 여기에서는, 속도 억제 처리가 필요하다고 판별되고(S13; 예), J6축(36)의 속도가 허용 범위 외이거나 또는 J6축(36)의 속도 억제 처리 중을 나타내는 플래그가 세트되어 있는 경우(스텝 S201; J6축이 범위 외)에 해당하고 있는 것으로 한다. 스텝 S211에 있어서, J6축(36)의 각도의 후보가 산출된 것으로 한다.

이어서, 상기 스텝 S212에 있어서, 주제어부(11)는 산출된 다음의 보간점에 있어서의 J6축(36)의 각도의 각 후보에 대응하는 다른 구동축 J1축(31) 내지 J5축(35), J7축(37)의 각도를 산출한다. 예를 들어, J6축(36)의 각도의 후보(θ_6a, θ_6b, θ_6c)마다 상기 스텝 S122에 있어서 산출된 다음의 보간점 P_i ₊₁의 엔드 이펙터(39)의 위치(X_i ₊₁, Y_i ₊₁, Z_i ₊₁)와, 제1 관절 구동계의 J4축(34), J5축(35)의 각도(θ₄, θ₅)로 사용해서, 제2 관절 구동계의 J1축(31) 내지 J3축(33), J7축(37)의 각도(θ₁, θ₂, θ₃, θ₇)를 재산출한다.

(1) 후보 θ_6a에 대해서, [X_i ₊₁, Y_i ₊₁, Z_i ₊₁, θ₄, θ₅, θ_6a]로부터 [θ_1a, θ_2a, θ_3a, θ₄, θ₅, θ_6a, θ₇]을 산출한다.

(2) 후보 θ_6b에 대해서, [X_i ₊₁, Y_i ₊₁, Z_i ₊₁, θ₄, θ₅, θ_6b]로부터 [θ_1b, θ_2b, θ_3b, θ₄, θ₅, θ_6b, θ₇]을 산출한다.

(3) 후보 θ_6c에 대해서, [X_i ₊₁, Y_i ₊₁, Z_i ₊₁, θ₄, θ₅, θ_6c]로부터 [θ_1c, θ_2c, θ_3c, θ₄, θ₅, θ_6c, θ₇]을 산출한다.

이어서, 상기 스텝 S213에 있어서, 주제어부(11)는 산출된 특이점을 회피하는 계의 다음의 보간점에 있어서의 상기 용접선 좌표계의 엔드 이펙터(39)의 자세를 나타내는 데이터의 각 성분[토치 경사각(Rx), 토치 전진각(Ry), 토치 회전각(Rz)]을 산출한다.

예를 들어, 상기 (1) 내지 (3)의 후보 θ_6a 내지 θ_6c에 대해서,

(1') [θ_1a, θ_2a, θ_3a, θ₄, θ₅, θ_6a, θ₇]로부터 [R_xa, R_ya, R_za]를 산출한다.

(2') [θ_1b, θ_2b, θ_3b, θ₄, θ₅, θ_6b, θ₇]로부터 [R_xb, R_yb, R_zb]를 산출한다.

(3') [θ_1c, θ_2c, θ_3c, θ₄, θ₅, θ_6c, θ₇]로부터 [R_xc, R_yc, R_zc]를 산출한다.

이어서, 스텝 S214에 있어서, 주제어부(11)는 산출된 특이점을 경유하는 계의 다음의 보간점 P_i ₊₁에 있어서의 상기 용접선 좌표계의 엔드 이펙터(39)의 자세를 나타내는 데이터의 각 성분[토치 경사각(Rx), 토치 전진각(Ry), 토치 회전각(Rz)]을 산출한다.

계속해서, 상기 스텝 S215 이후의 처리를 실행한다. 또한, 실시 형태와 마찬가지로, 속도가 허용 범위 외라고 판별된 각 구동축에 대응하여, 스텝 S211 내지 S216 대신에 스텝 S221 내지 S226 혹은 스텝 S231 내지 S236을 실행한다.

이상에 나타낸 제어 방법에 의해, 제2 관절 구동계의 축이 4축 있는 매니퓰레이터 본체(30A)를 구비한 다관절형 로봇(X2)의 주제어부(11)가 중심으로 되어, 억제하기 위한 가중치 부여가 큰 특정 성분[토치 경사각(Rx), 토치 전진각(Ry), 토치 회전각(Rz) 중 어느 하나]의 변동을 억제하면서 특이점을 회피하여 엔드 이펙터(39)의 위치 및 자세를 작동시키고, 본래의 교시 포인트에 기초하는 보간점을 크게 벗어나지 않고, 또한 엔드 이펙터(39)의 이동 속도를 변경하지 않고 모든 축의 속도가 허용 범위 내에 들어가는 작업 궤도를 얻을 수 있다.

이와 같이 실시예 4의 제어 방법은, 보간점 Pi에 있어서의 위치 및 자세(X_i, Y_i, Z_i, α_i, β_i, γ_i)로부터 제1 관절 구동계의 각 축의 각도(θ₄, θ₅, θ₆)를 역운동학 문제의 해로서 얻어지고, 또한, 보간점 Pi에 있어서의 위치 및 제1 관절 구동계의 각 축의 각도(X_i, Y_i, Z_i, θ₄, θ₅, θ₆)로부터 제2 관절 구동계의 각 축의 각도(θ₁, θ₂, θ₃, θ₇)를 역운동학 문제의 해로서 얻어지는 다수 관절축을 구비한 매니퓰레이터의 제어에 유용하다.

그 때문에, 상술한 6축, 7축의 매니퓰레이터를 구비한 다관절형 로봇에 한하지 않고, 그 이상의 축을 구비한 다관절형 로봇에도 적용할 수 있다.

실시예 5

또한, 다관절형 로봇(X)의 제어부(10)는, 전용의 시스템에 의하지 않고, 도 29에 도시하는 네트워크에 접속된 통상의 컴퓨터 시스템(Y)을 사용해서 실현 가능하다. 도 29의 컴퓨터 시스템(Y)은, 예를 들어 주제어부(11), 기억부(12), 외부 기억부(103), 입출력부(104), 표시부(105), 송신부(106), 내부 버스(109)을 구비하고 있다. 주제어부(11)는 CPU(Central Processing Unit)를 포함하고, 기억부(12)는 RAM(Random Access Memory) 등을 포함한다. 주제어부(11)는, 기억부(12)에 기억되어 있는 프로그램을 실행하고, 다관절형 로봇(X)의 제어 장치인 제어부(10)를 실현한다.

예를 들어, 상기한 처리를 실행하기 위한 프로그램을, 컴퓨터가 판독 가능한 기록 매체(플렉시블 디스크, CD-ROM, DVD-ROM 등)에 저장하여 배포하고, 당해 프로그램을 컴퓨터에 인스톨함으로써, 상기한 처리를 실행하는 다관절형 로봇(X)의 제어부(10)를 구성해도 된다. 또한, 인터넷 등의 통신 네트워크 상의 서버 장치가 갖는 기억 장치에 당해 프로그램을 저장해 두고, 통상의 컴퓨터 시스템이 다운로드 등 함으로써 다관절형 로봇(X)의 제어부(10)를 구성해도 된다.

이상, 본 발명의 실시 형태에 대해서 설명했지만, 설계 상의 형편이나 그 밖의 요인에 의해 필요해지는 여러가지 수정이나 조합은, 청구항에 기재되어 있는 발명이나 발명의 실시 형태에 기재되어 있는 구체예에 대응하는 발명의 범위에 포함된다고 이해되어야 한다.

상술한 실시 형태에서는, 예를 들어 제1 관절 구동계의 J4축(34)이나 J6축(36)의 한쪽 또는 양쪽의 속도나 가속도가 허용 범위 외로 되지 않도록, J5축(35)의 특이점을 회피한다고 하는 특이점 회피의 처리를 행하고 있다. 여기서, 특이점 회피의 처리를 행하고 있는 것을 외부에 통지하는 수단을 다관절형 로봇에 설치해도 된다.

구체적으로는, 도 5에 도시하는 바와 같이 특이점 회피를 위한 처리로서 속도 억제 처리를 행하고 있을 때, 도 30에 도시한 바와 같이, 특이점 회피의 처리를 행하고 있는 것을 나타내는 「특이점 회피 중」을, 조작부(21)(교시 펜던트)의 표시 화면(M)에 표시한다. 특이점 회피의 처리를 행하고 있는 것을 나타내는 표시는, 「특이점 회피 중」에 한정되지 않고, 특이점 회피의 처리가 행해지고 있는 것을 알 수 있는 표현이면 어떤 표현이어도 상관없다.

이와 같이, 특이점 회피 중인 것을 표시함으로써, 매니퓰레이터 본체(30)가 특이점 회피 중인 상태(회피 용접 상태)인지, 특이점 회피 중이 아닌 상태(통상 용접 상태)인지를 오퍼레이터가 용접 중에 파악할 수 있다. 그때문에, 예를 들어 작업 대상물(재료)의 용접 후에 품질 검사를 행하는 경우, 회피 용접 상태에서 용접한 개소를 주시하여 검사할 수 있고, 이 검사 결과에 기초하여, 회피 용접 상태에서 용접을 행했을 때의 용접 조건을 적절하게 변경할 수 있다. 또한, 예를 들어 회피 용접 상태에 있어서의 매니퓰레이터 본체(30)의 전체의 움직임을, 오퍼레이터가 육안 등으로 감시할 수도 있고, 이 감시에 기초하여, 다음 용접 작업 등에서, 다시, 매니퓰레이터 본체(30)를 움직이게 할 때에, 매니퓰레이터 본체(30)의 전체의 동작을 다시 볼 수도 있다.

또한, 「특이점 회피 중」인 표시 외에, 처리 프로그램을 알 수 있도록 처리 프로그램의 내용(예를 들어, 재생 중 프로그램의 번호)을 표시 화면(M)에 표시하거나, 처리 프로그램을 실행하고 있는 시간(재생 시간), 엔드 이펙터(39)의 움직임(예를 들어, 직선 이동)을 표시 화면(M)에 표시해도 된다. 또한, 미리 교시된 토치의 정보 (예를 들어, 토치 경사각, 토치 전진각, 토치 회전각 등)와, 현재의 토치의 정보를 표시 화면(M)에 표시해도 된다.

이에 의하면, 「특이점 회피 중」에 있어서의 매니퓰레이터 본체(30)의 자세한 상태를 파악할 수 있다.

상술한 예에서는, 특이점 회피의 처리를 행하고 있는 것을 표시 화면(M)에 표시시키고 있지만, 이 대신에, 특이점 회피를 행하고 있는 것을 스피커 등에 의해 소리로 알려도 되고, 램프 등의 점등이나 점멸 등 광으로 알려도 된다. 또한, 특이점 회피를 행하고 있는 것은, 다관절형 로봇의 교시 펜던트(21)와는 다른 외부의 표시 장치에 표시해도 된다.

또한, 「특이점 회피 중」인 시간을 제어부(10)의 기억부(12) 등에 기억해도 된다. 그리고, 예를 들어 용접 종료 후에 조작부(21)(교시 펜던트)의 표시 화면(M)에 특이점 회피 중인 시간 이력을 표시하고, 오퍼레이터 등이 특이점 회피 중인 시간 이력을 확인할 수 있도록 해도 된다.

X, X1, X2 : 다관절형 로봇
10 : 제어부
11 : 주제어부
12 : 기억부
13 : 구동 지시부
21 : 조작부
30, 30A : 매니퓰레이터 본체
31 : J1축
32 : J2축
33 : J3축
34 : J4축
35 : J5축
36 : J6축
37 : J7축
39, 39A : 엔드 이펙터

Claims

작업 대상물의 작업선에 따라서 이동시키는 작업부를 선단에 갖고, 그 작업부에 연결되어 상기 작업부의 자세를 변화시키는 3개의 구동축을 구비한 제1 관절 구동계와, 상기 제1 관절 구동계에 연결되어 상기 제1 관절 구동계의 위치를 변화시키는 적어도 3개의 구동축을 구비한 제2 관절 구동계를 갖는 다관절형 로봇을 제어하는 다관절형 로봇의 제어 장치로서,
상기 다관절형 로봇의 베이스 좌표계에 있어서 미리 교시된 상기 작업부의 작업 개시 위치 및 그때의 자세와 작업 종료 위치 및 그때의 자세를 연결하는 교시 경로를 보간하는 복수의 보간점에 있어서의 상기 작업부의 위치 및 그때의 자세를 나타내는 데이터를 산출하는 보간 데이터 산출 수단과,
상기 보간 데이터 산출 수단에 의해 산출된 데이터로부터 역운동학 문제의 해를 구함으로써, 상기 보간점에 있어서의 상기 작업부의 위치 및 그때의 자세를 나타내는 상기 제1 관절 구동계 및 상기 제2 관절 구동계를 포함하는 모든 구동축의 각도를 산출하는 각도 산출 수단과,
상기 각도 산출 수단에 의해 산출된 다음의 상기 보간점과 현재의 상기 보간점에 있어서의 상기 제1 관절 구동계의 상기 각 구동축의 각도의 차분에 기초하여 다음의 상기 보간점에 상기 작업부의 자세를 움직이게 할 때의 상기 제1 관절 구동계에 있어서의 양단의 2개의 구동축의 속도를 산출하는 속도 산출 수단과,
상기 보간 데이터 산출 수단에 의해 산출된 데이터를, 상기 작업부의 이동 방향의 축, 상기 축에 수직인 방향의 축, 상기 2개의 축에 수직인 축을 포함하는 작업 좌표계에 있어서의 상기 작업부의 자세를 나타내는 자세 데이터로 변환하는 자세 데이터 변환 수단과,
상기 속도 산출 수단에 의해 산출된 상기 제1 관절 구동계에 있어서의 양단의 2개의 구동축의 한쪽 또는 양쪽의 속도가 미리 정해진 허용 범위 외인 경우에, 상기 작업부의 이동 속도를 변경하지 않고, 또한 상기 자세 데이터 변환 수단에 의해 변환된 다음의 상기 보간점에 있어서의 상기 작업부의 자세 데이터에 대해서 한개 또는 두개의 특정 성분의 변동을 억제하면서 상기 제1 관절 구동계에 있어서의 양단의 2개의 구동축의 속도가 상기 허용 범위 내로 되는 상기 제1 관절 구동계의 구동축 각각의 각도를 다시 산출하고, 상기 산출된 상기 제1 관절 구동계의 구동축 각각의 각도와 상기 보간 데이터 산출 수단에 의해 산출된 상기 작업부의 위치에 기초하여 상기 제2 관절 구동계의 구동축 각각의 각도를 다시 산출하는 재산출 수단과,
상기 속도 산출 수단에 의해 산출된 상기 제1 관절 구동계에 있어서의 양단의 2개의 구동축의 양쪽의 속도가 상기 허용 범위 내인 경우에는, 상기 각도 산출 수단에 의해 산출된 상기 구동축 각각의 각도에 기초하여 상기 다관절형 로봇을 구동시키고, 상기 속도 산출 수단에 의해 산출된 상기 제1 관절 구동계에 있어서의 양단의 2개의 구동축의 한쪽 또는 양쪽의 속도가 상기 허용 범위 외인 경우에는, 상기 재산출 수단에 의해 산출된 상기 구동축 각각의 각도에 기초하여 상기 다관절형 로봇을 구동시키는 구동 지시 수단
을 구비하여 이루어지고,
상기 재산출 수단이,
현재의 상기 보간점에 있어서의 상기 제1 관절 구동계의 구동축 각각의 각도에 기초하여, 상기 구동축 각각의 각도가 상기 각도 산출 수단에 의해 산출된 다음의 상기 보간점에 있어서의 각도를 참조로 추종하고, 또한 상기 제1 관절 구동계에 있어서의 양단의 2개의 구동축의 다음의 상기 보간점에 있어서의 속도가 상기 허용 범위 내로 되는 상기 구동축 각각의 각도의 후보를 복수 산출하는 각도 후보 산출 수단과,
상기 각도 후보 산출 수단에 의해 산출된 각도의 후보 각각을 채용했을 때의 다음의 상기 보간점의 상기 작업부의 자세를 상기 작업 좌표계로 나타내는 자세 데이터를 각각 산출하는 자세 데이터 산출 수단과,
상기 자세 데이터 산출 수단에 의해 산출된 복수의 자세 데이터 중, 상기 자세 데이터 변환 수단에 의해 변환된 다음의 상기 보간점에 있어서의 상기 작업부의 자세 데이터에 대해서 상기 특정 성분의 변동이 가장 억제된 자세 데이터를 선택하고, 상기 자세 데이터에 대응하는 각도를 상기 구동축 각각의 각도로서 선택하는 각도 선택 수단
을 포함하여 이루어지는 것을 특징으로 하는, 다관절형 로봇의 제어 장치.
삭제
제1항에 있어서,
상기 제1 관절 구동계의 구동축의 각도를 ±180°로 나타냈을 때,
상기 각도 산출 수단이, 상기 제1 관절 구동계의 중앙에 위치하는 구동축의 다음의 상기 보간점에 있어서의 각도가 상기 작업 개시 위치에 있어서의 상기 구동축의 각도와 동일 부호가 되는 역운동학 문제의 해를 산출하는 것인, 다관절형 로봇의 제어 장치.
제1항에 있어서,
상기 제1 관절 구동계의 구동축의 각도를 ±180°로 나타냈을 때,
상기 각도 산출 수단이, 상기 속도 산출 수단에 의해 산출된 상기 제1 관절 구동계에 있어서의 양단의 2개의 구동축의 양쪽의 속도가 상기 허용 범위 내인 경우에는, 상기 제1 관절 구동계의 중앙에 위치하는 구동축의 다음의 상기 보간점에 있어서의 각도가 상기 작업 개시 위치에 있어서의 상기 구동축의 각도와 동일 부호가 되는 역운동학 문제의 해를 산출하고,
상기 속도 산출 수단에 의해 산출된 상기 제1 관절 구동계에 있어서의 양단의 2개의 구동축의 한쪽 또는 양쪽의 속도가 상기 허용 범위 외인 경우에는, 상기 제1 관절 구동계의 중앙에 위치하는 구동축의 다음의 상기 보간점에 있어서의 각도가 상기 작업 개시 위치에 있어서의 상기 구동축의 각도와 다른 부호가 되는 역운동학 문제의 해를 산출하는 것인, 다관절형 로봇의 제어 장치.
제1항에 있어서,
상기 제1 관절 구동계의 구동축의 각도를 ±180°로 나타냈을 때,
상기 각도 산출 수단이,
상기 작업 개시 위치 및 상기 작업 종료 위치에 있어서의 상기 제1 관절 구동계의 중앙의 구동축의 각도가 동일 부호인 경우에는, 상기 제1 관절 구동계의 중앙에 위치하는 구동축의 다음의 상기 보간점에 있어서의 각도가 상기 작업 개시 위치에 있어서의 상기 구동축의 각도와 동일 부호가 되는 역운동학 문제의 해를 산출하고,
상기 작업 개시 위치 및 상기 작업 종료 위치에 있어서의 상기 제1 관절 구동계의 중앙의 구동축의 각도가 다른 부호인 경우에 있어서, 상기 속도 산출 수단에 의해 산출된 상기 제1 관절 구동계에 있어서의 양단의 2개의 구동축의 양쪽의 속도가 상기 허용 범위 내인 경우에는, 상기 제1 관절 구동계의 중앙에 위치하는 구동축의 다음의 상기 보간점에 있어서의 각도가 상기 작업 개시 위치에 있어서의 상기 구동축의 각도와 동일 부호가 되는 역운동학 문제의 해를 산출하고, 상기 속도 산출 수단에 의해 산출된 상기 제1 관절 구동계에 있어서의 양단의 2개의 구동축의 한쪽 또는 양쪽의 속도가 상기 허용 범위 외인 경우에는, 상기 제1 관절 구동계의 중앙에 위치하는 구동축의 다음의 상기 보간점에 있어서의 각도가 상기 작업 개시 위치에 있어서의 상기 구동축의 각도와 다른 부호가 되는 역운동학 문제의 해를 산출하는 것인, 다관절형 로봇의 제어 장치.
제1항 및 제3항 내지 제5항 중 어느 한 항에 있어서,
상기 재산출 수단이, 상기 자세 데이터 변환 수단에 의해 변환된 상기 작업부의 자세 데이터 중 미리 정해진 가중치 부여가 큰 상기 특정 성분으로부터 차례로 변동의 억제량이 커지도록 상기 구동축의 각도를 다시 산출하는 것인, 다관절형 로봇의 제어 장치.
제1항 및 제3항 내지 제5항 중 어느 한 항에 있어서,
상기 작업부가 토치이고,
상기 자세 데이터가, 상기 작업부의 이동 방향의 축을 X축, 상기 X축과 중력 방향의 외적에 의해 나타나는 축을 Y축, 상기 X축과 상기 Y축의 외적에 의해 나타나는 방향의 축을 Z축, 상기 X축의 축 둘레의 회전각을 토치 경사각, 상기 Y축의 축 둘레의 회전각을 토치 전진각, 상기 Z축의 축 둘레의 회전각을 토치 회전각으로 하는 작업 좌표계로 나타나는 것으로서,
상기 특정 성분이, 상기 토치 경사각, 상기 토치 전진각, 상기 토치 회전각 중 어느 하나 또는 둘인, 다관절형 로봇의 제어 장치.
제7항에 있어서,
상기 작업부가, 선단에 상기 작업 대상물에 대해서 작업 상의 효과를 주는 하나의 작업점을 갖는 싱글 토치이고,
상기 특정 성분이, 상기 토치 경사각 및 상기 토치 전진각인, 다관절형 로봇의 제어 장치.
제7항에 있어서,
상기 작업부가, 선단에 상기 작업 대상물에 대해서 작업 상의 효과를 주는 2개의 작업점을 갖는 탠덤 토치이고,
상기 특정 성분이, 상기 토치 경사각 및 상기 토치 회전각인, 다관절형 로봇의 제어 장치.
제1항 및 제3항 내지 제5항 중 어느 한 항에 있어서,
상기 구동 지시 수단이, 상기 속도 허용 판별 수단에 의해 상기 허용 범위 외의 구동축이 있다고 판별된 후에는 상기 재산출 수단에 의해 산출된 상기 구동축 각각의 각도에 기초하여 상기 다관절형 로봇을 구동시키고, 그 후, 상기 각도 산출 수단에 의해 산출된 상기 제1 관절 구동계의 각도와 상기 재산출 수단에 의해 산출된 상기 제1 관절 구동계의 각도의 차분이 소정값 이하가 된 것을 조건으로, 상기 재산출 수단에 의해 산출된 상기 구동축 각각의 각도 대신에 상기 각도 산출 수단에 의해 산출되는 상기 구동축 각각의 각도에 기초하여 상기 다관절형 로봇을 구동시키는 것인, 다관절형 로봇의 제어 장치.
제10항에 있어서,
상기 구동 지시 수단이, 상기 재산출 수단에 의해 산출된 상기 구동축 각각의 각도에 기초하는 상기 다관절 로봇의 구동 제어로부터 상기 각도 산출 수단에 의해 산출된 상기 구동축 각각의 각도에 기초하는 상기 다관절 로봇의 구동 제어로 전환할 때에, 상기 구동축 각각의 각도를 소정의 변동 범위 내에서 서서히 변동시키는 것인, 다관절형 로봇의 제어 장치.
제1항 및 제3항 내지 제5항 중 어느 한 항에 있어서,
상기 속도 산출 수단이, 속도 대신에 가속도를 산출하는 것이며, 상기 재산출 수단 및 상기 구동 지시 수단이, 속도 대신에 가속도에 기초하여 판별하는 것인, 다관절형 로봇의 제어 장치.
작업 대상물의 작업선에 따라서 이동시키는 작업부를 선단에 갖고, 그 작업부에 연결되어 상기 작업부의 자세를 변화시키는 3개의 구동축을 구비한 제1 관절 구동계와, 상기 제1 관절 구동계에 연결되어 상기 제1 관절 구동계의 위치를 변화시키는 적어도 3개의 구동축을 구비한 제2 관절 구동계를 갖는 다관절형 로봇을 제어하는 다관절형 로봇의 제어 방법으로서,
상기 다관절형 로봇의 베이스 좌표계에 있어서 미리 교시된 상기 작업부의 작업 개시 위치 및 그때의 자세와 작업 종료 위치 및 그때의 자세를 연결하는 교시 경로를 보간하는 복수의 보간점에 있어서의 상기 작업부의 위치 및 그때의 자세를 나타내는 데이터를 산출하는 보간 데이터 산출 공정과,
상기 보간 데이터 산출 공정에 의해 산출된 데이터로부터 역운동학 문제의 해를 구함으로써, 상기 보간점에 있어서의 상기 작업부의 위치 및 그때의 자세를 나타내는 상기 제1 관절 구동계 및 상기 제2 관절 구동계를 포함하는 모든 구동축의 각도를 산출하는 각도 산출 공정과,
상기 각도 산출 공정에 의해 산출된 다음의 상기 보간점과 현재의 상기 보간점에 있어서의 상기 제1 관절 구동계의 상기 각 구동축의 각도의 차분에 기초하여 다음의 상기 보간점에 상기 작업부의 자세를 움직이게 할 때의 상기 제1 관절 구동계에 있어서의 양단의 2개의 구동축의 속도를 산출하는 속도 산출 공정과,
상기 보간 데이터 산출 공정에 의해 산출된 데이터를, 상기 작업부의 이동 방향의 축, 상기 축에 수직인 방향의 축, 상기 2개의 축에 수직인 축을 포함하는 작업 좌표계에 있어서의 상기 작업부의 자세를 나타내는 자세 데이터로 변환하는 자세 데이터 변환 공정과,
상기 속도 산출 공정에 의해 산출된 상기 제1 관절 구동계에 있어서의 양단의 2개의 구동축의 한쪽 또는 양쪽의 속도가 미리 정해진 허용 범위 외인 경우에, 상기 작업부의 이동 속도를 변경하지 않고, 또한 상기 자세 데이터 변환 공정에 의해 변환된 다음의 상기 보간점에 있어서의 상기 작업부의 자세 데이터에 대해서 한개 또는 두개의 특정 성분의 변동을 억제하면서 상기 제1 관절 구동계에 있어서의 양단의 2개의 구동축의 속도가 상기 허용 범위 내로 되는 상기 제1 관절 구동계의 구동축 각각의 각도를 다시 산출하고, 상기 산출된 상기 제1 관절 구동계의 구동축 각각의 각도와 상기 보간 데이터 산출 공정에 의해 산출된 상기 작업부의 위치에 기초하여 상기 제2 관절 구동계의 구동축 각각의 각도를 다시 산출하는 재산출 공정과,
상기 속도 산출 공정에 의해 산출된 상기 제1 관절 구동계에 있어서의 양단의 2개의 구동축의 양쪽의 속도가 상기 허용 범위 내인 경우에는, 상기 각도 산출 공정에 의해 산출된 상기 구동축 각각의 각도에 기초하여 상기 다관절형 로봇을 구동시키고, 상기 속도 산출 공정에 의해 산출된 상기 제1 관절 구동계에 있어서의 양단의 2개의 구동축의 한쪽 또는 양쪽의 속도가 상기 허용 범위 외인 경우에는, 상기 재산출 공정에 의해 산출된 상기 구동축 각각의 각도에 기초하여 상기 다관절형 로봇을 구동시키는 구동 지시 공정
을 실행하고,
상기 재산출 공정이,
현재의 상기 보간점에 있어서의 상기 제1 관절 구동계의 구동축 각각의 각도에 기초하여, 상기 구동축 각각의 각도가 상기 각도 산출 공정에 의해 산출된 다음의 상기 보간점에 있어서의 각도를 참조로 추종하고, 또한 상기 제1 관절 구동계에 있어서의 양단의 2개의 구동축의 다음의 상기 보간점에 있어서의 속도가 상기 허용 범위 내로 되는 상기 구동축 각각의 각도의 후보를 복수 산출하는 각도 후보 산출 공정과,
상기 각도 후보 산출 공정에 의해 산출된 각도의 후보 각각을 채용했을 때의 다음의 상기 보간점의 상기 작업부의 자세를 상기 작업 좌표계로 나타내는 자세 데이터를 각각 산출하는 자세 데이터 산출 공정과,
상기 자세 데이터 산출 공정에 의해 산출된 복수의 자세 데이터 중, 상기 자세 데이터 변환 공정에 의해 변환된 다음의 상기 보간점에 있어서의 상기 작업부의 자세 데이터에 대해서 상기 특정 성분의 변동이 가장 억제된 자세 데이터를 선택하고, 상기 자세 데이터에 대응하는 각도를 상기 구동축 각각의 각도로서 선택하는 각도 선택 공정
을 포함하여 이루어지는 것을 특징으로 하는, 다관절형 로봇의 제어 방법.
작업 대상물의 작업선에 따라서 이동시키는 작업부를 선단에 갖고, 그 작업부에 연결되어 상기 작업부의 자세를 변화시키는 3개의 구동축을 구비한 제1 관절 구동계와, 상기 제1 관절 구동계에 연결되어 상기 제1 관절 구동계의 위치를 변화시키는 적어도 3개의 구동축을 구비한 제2 관절 구동계를 갖는 다관절형 로봇을 제어하는 컴퓨터에 의해 실행되는 다관절형 로봇의 제어 프로그램을 기록한 컴퓨터 판독가능한 기록 매체로서,
상기 다관절형 로봇의 베이스 좌표계에 있어서 미리 교시된 상기 작업부의 작업 개시 위치 및 그때의 자세와 작업 종료 위치 및 그때의 자세를 연결하는 교시 경로를 보간하는 복수의 보간점에 있어서의 상기 작업부의 위치 및 그때의 자세를 나타내는 데이터를 산출하는 보간 데이터 산출 공정과,
상기 보간 데이터 산출 공정에 의해 산출된 데이터로부터 역운동학 문제의 해를 구함으로써, 상기 보간점에 있어서의 상기 작업부의 위치 및 그때의 자세를 나타내는 상기 제1 관절 구동계 및 상기 제2 관절 구동계를 포함하는 모든 구동축의 각도를 산출하는 각도 산출 공정과,
상기 각도 산출 공정에 의해 산출된 다음의 상기 보간점과 현재의 상기 보간점에 있어서의 상기 제1 관절 구동계의 상기 각 구동축의 각도의 차분에 기초하여 다음의 상기 보간점에 상기 작업부의 자세를 움직이게 할 때의 상기 제1 관절 구동계에 있어서의 양단의 2개의 구동축의 속도를 산출하는 속도 산출 공정과,
상기 보간 데이터 산출 공정에 의해 산출된 데이터를, 상기 작업부의 이동 방향의 축, 상기 축에 수직인 방향의 축, 상기 2개의 축에 수직인 축을 포함하는 작업 좌표계에 있어서의 상기 작업부의 자세를 나타내는 자세 데이터로 변환하는 자세 데이터 변환 공정과,
상기 속도 산출 공정에 의해 산출된 상기 제1 관절 구동계에 있어서의 양단의 2개의 구동축의 한쪽 또는 양쪽의 속도가 미리 정해진 허용 범위 외인 경우에, 상기 작업부의 이동 속도를 변경하지 않고, 또한 상기 자세 데이터 변환 공정에 의해 변환된 다음의 상기 보간점에 있어서의 상기 작업부의 자세 데이터에 대해서 한개 또는 두개의 특정 성분의 변동을 억제하면서 상기 제1 관절 구동계에 있어서의 양단의 2개의 구동축의 속도가 상기 허용 범위 내로 되는 상기 제1 관절 구동계의 구동축 각각의 각도를 다시 산출하고, 상기 산출된 상기 제1 관절 구동계의 구동축 각각의 각도와 상기 보간 데이터 산출 공정에 의해 산출된 상기 작업부의 위치에 기초하여 상기 제2 관절 구동계의 구동축 각각의 각도를 다시 산출하는 재산출 공정과,
상기 속도 산출 공정에 의해 산출된 상기 제1 관절 구동계에 있어서의 양단의 2개의 구동축의 양쪽의 속도가 상기 허용 범위 내인 경우에는, 상기 각도 산출 공정에 의해 산출된 상기 구동축 각각의 각도에 기초하여 상기 다관절형 로봇을 구동시키고, 상기 속도 산출 공정에 의해 산출된 상기 제1 관절 구동계에 있어서의 양단의 2개의 구동축의 한쪽 또는 양쪽의 속도가 상기 허용 범위 외인 경우에는, 상기 재산출 공정에 의해 산출된 상기 구동축 각각의 각도에 기초하여 상기 다관절형 로봇을 구동시키는 구동 지시 공정
을 컴퓨터에 실행시키고,
상기 재산출 공정이,
현재의 상기 보간점에 있어서의 상기 제1 관절 구동계의 구동축 각각의 각도에 기초하여, 상기 구동축 각각의 각도가 상기 각도 산출 공정에 의해 산출된 다음의 상기 보간점에 있어서의 각도를 참조로 추종하고, 또한 상기 제1 관절 구동계에 있어서의 양단의 2개의 구동축의 다음의 상기 보간점에 있어서의 속도가 상기 허용 범위 내로 되는 상기 구동축 각각의 각도의 후보를 복수 산출하는 각도 후보 산출 공정과,
상기 각도 후보 산출 공정에 의해 산출된 각도의 후보 각각을 채용했을 때의 다음의 상기 보간점의 상기 작업부의 자세를 상기 작업 좌표계로 나타내는 자세 데이터를 각각 산출하는 자세 데이터 산출 공정과,
상기 자세 데이터 산출 공정에 의해 산출된 복수의 자세 데이터 중, 상기 자세 데이터 변환 공정에 의해 변환된 다음의 상기 보간점에 있어서의 상기 작업부의 자세 데이터에 대해서 상기 특정 성분의 변동이 가장 억제된 자세 데이터를 선택하고, 상기 자세 데이터에 대응하는 각도를 상기 구동축 각각의 각도로서 선택하는 각도 선택 공정
을 포함하여 이루어지는 것을 특징으로 하는, 다관절형 로봇의 제어 프로그램을 기록한 컴퓨터 판독가능한 기록 매체.