KR20170094351A

KR20170094351A - 암형 로봇의 장애물 자동 회피방법 및 제어장치

Info

Publication number: KR20170094351A
Application number: KR1020177019053A
Authority: KR
Inventors: 아츠시 나카야; 마사야 요시다; 쇼헤이 마세; 히로키 타카하시
Original assignee: 카와사키 주코교 카부시키 카이샤
Priority date: 2014-12-25
Filing date: 2014-12-25
Publication date: 2017-08-17
Also published as: CN107000223B; JP6378783B2; US20170348856A1; KR101941147B1; TWI579669B; US10350756B2; JPWO2016103297A1; CN107000223A; WO2016103297A1; TW201624161A

Abstract

본 발명은, 로봇(100)을 기하모델(M)로서 표현하는 기하모델표현단계(S40), 초기궤도상의 소정의 점에 대응하는 로봇의 가상자세(Pp)를 산출하는 가상자세산출단계(S30), 가상자세에 있어서의 기하모델이 진입금지영역과 간섭하는지 아닌지를 판정하는 간섭판정단계(S50), 간섭한다고 판정한 경우, 가상반발력에 의해 기하모델의 간섭부분이 진입금지영역에서 동작영역으로 밀려난 상태의 자세를 산출하고, 산출한 자세를 경유자세로서 결정하는 경유자세결정단계(S70)과, 현재자세에서 경유자세를 경유하여 최종자세로 변화하는 경우에 있어서의 핸드의 궤도를 갱신궤도(Tb)로서 결정하는 갱신궤도결정단계(S80)과, 최신의 경유자세결정단계에서 결정된 경유자세를 초기궤도결정단계에 있어서의 현재자세라고 가정해서 초기궤도결정단계에서 갱신궤도결정단계까지의 단계를 반복해서 실행하는 단계를 포함하는 로봇의 장애물 자동회피방법을 제공한다.

Description

암형 로봇의 장애물 자동 회피방법 및 제어장치{AUTOMATIC OBSTRUCTION AVOIDANCE METHOD AND CONTROL DEVICE FOR ARM-TYPE ROBOT}

본 발명은, 암형(arm-type) 로봇의 장애물 자동 회피방법 및 제어장치에 관한 것이다.

종래기술로부터 정지한 로봇을 장애물과의 간섭을 자동적으로 회피해서, 소정의 자세에 도달시키는 방법이 알려져있다.

특허문헌 1에는, 경로를 소행해서 작업원점까지 복귀하는 방법이 개시되어 있다. 구체적으로는, 이동 커맨드를 포함하여 기술된 제어프로그램을 순차 실행하는 것에 의해 소망의 경로에서 일련의 동작을 행하는 로봇을 정지위치에서 작업원점으로 복귀시키는 제어방법이고, 이미 실행된 제어 프로그램을 순차 역실행할 때에 즈음해서, 각 이동 커맨드를 이 하나 전의 이동 커맨드의 위치 인수를 이용해서 실행한다.

특허문헌 2에는, 이동경로의 바깥에서 로봇이 이상정지한 경우라도 원점으로 복귀할 수 있는 로봇이 개시되어 있다. 구체적으로는, 로봇이 동작하는 동작 에어리어를 적어도 포함하는 에어리어맵(area map)을, 소정의 영역을 갖는 에어리어 블록마다 구획하는 에어리어 블록 작성공정과, 구획된 에어리어 블록마다 로봇의 복귀방향을 설정하는 방향 설정공정을, 구비한다. 이것에 의해, 장애물과의 간섭을 회피할 수 있다.

특개평7-28520호 공보 특개2009-90383호 공보

특허문헌 1에 기재된 방법에서는, 이동 경로의 바깥에서 로봇이 이상정지한 경우에는, 작업원점으로 복귀할 수 없다. 한편, 특허문헌 2의 방법에서는 그것이 가능하다. 그러나, 특허문헌 2에 기재된 방법은, 미리 에어리어 블록마다 로봇의 복귀방향을 설정할 필요가 있으며, 설정작업을 행하는 작업자의 작업량이 증대된다고 하는 문제가 있었다. 또한, 복귀방향의 결정도 작업자의 경험칙에 의한 판단을 요하며, 작업자의 확보가 곤란하다고 하는 문제가 있었다. 게다가, 에어리어 블록마다 로봇의 복귀방향을 설정한 데이터는 데이터양이 방대하게 된다고 하는 문제가 있었다. 바꾸어 말하면, 로봇의 복귀방향에 관해서 복잡한 설정이 필요하다고 하는 문제가 있었다.

본 발명은, 상기 과제를 감안해서 이루어진 것이며, 로봇이 어느 위치에서 정지해도, 단순함 설정으로 안전하게 소정의 자세에 도달시킬 수 있는 로봇의 장애물 자동회피방법 및 제어장치를 제공하는 것을 목적으로 한다.

상기 과제를 해결하기 위해, 본 발명의 어떤 태양(態樣)에 관계되는 로봇의 장애물 자동회피방법은, 1 이상의 링크(link)가 관절에 의해 연결되고, 선단부(先端部)에 핸드(hand)가 설치된 로봇암(robot arm)을 구비하는 암형 로봇의 장애물 자동회피방법으로서, 상기 로봇을, 기하형상(幾何形狀)을 갖도록 모델링(modeling)해서 기하모델로서 표현하는 기하모델 표현단계(step)와, 상기 기하모델이 진입해서는 안 되는 진입 금지영역과, 이 진입 금지영역에 의해 규정되어, 상기 기하모델이 동작하는 동작영역을 설정하는 영역 설정단계와, 상기 로봇의 최종자세를 결정하는 최종자세 결정단계와, 상기 로봇이 현재자세에서 상기 최종자세를 향하여 변화하는 경우에 있어서의 상기 핸드의 초기궤도를 결정하는 초기궤도 결정단계와, 상기 초기궤도상의 소정의 점에 대응하는 상기 로봇의 가상자세(假想姿勢)를 산출하는 가상자세 산출단계와, 상기 가상자세에 있어서의 상기 기하모델이 상기 진입 금지영역과 간섭하는지 아닌지를 판정하는 간섭 판정단계와, 상기 간섭 판정단계에 있어서 간섭하지 아니한다고 판정한 경우, 상기 가상자세를 경유자세(經由姿勢)로서 결정하고, 간섭한다고 판정한 경우, 상기 진입 금지영역의 간섭부분과 상기 기하모델의 간섭부분이 상대적으로 반발하는 반발력을 가상적으로 발생시키고, 가상 반발력에 의해 상기 기하모델의 간섭부분이 상기 진입 금지영역에서 상기 동작영역으로 밀려난 상태의 자세를 산출하고, 이 산출한 자세를 경유자세로서 결정하는 경유자세 결정단계와, 상기 현재자세에서 상기 경유자세를 경유해서 상기 최종자세로 변화하는 경우에 있어서의 상기 핸드의 궤도를 갱신궤도로서 결정하는 갱신궤도 결정단계와, 최신의 경유자세 결정단계에서 결정된 경유자세를 상기 초기궤도 결정단계에 있어서의 상기 현재자세라고 가정해서 상기 초기궤도 결정단계, 상기 가상자세 산출단계, 상기 간섭 측정단계, 상기 경유자세 결정단계, 및 상기 갱신궤도 결정단계를 반복해서 실행하는 단계를, 포함하도록 구성되어 있다.

상기 구성에 의하면, 로봇이 현재자세에서 최종자세로 향하여 변화하는 경우에 있어서 핸드의 초기궤도상의 소정의 점에 대응하는 로봇의 가상자세를 산출하고, 가상자세에 있어서의 로봇의 기하모델이 진입 금지영역(장애물)과 간섭하는지 아니지를 판정한다. 그리고, 간섭하지 아니한다고 판정한 경우에는 가상자세를, 현재자세에서 최종자세로 향하여 변화하는 경우에 경유하는 경유자세로서 결정하고, 간섭한다고 판정한 경우에는, 가상 반발력에 의해 기하모델의 간섭부분이 진입 금지영역에서 동작영역으로 밀려난 상태의 자세를 산출하고, 이 자세를 경유자세로서 결정한다. 따라서, 로봇의 기하모델이 진입 금지영역과 간섭하는 경우에는, 진입 금지영역에 간섭하지 아니하는 경유자세가 주어지므로, 장애물과의 충돌이 회피된다. 다만, 이 경우에는 당초의 초기궤도로부터 벗어난 것이므로 최종자세에 접근하는 것은 담보되지 않는다. 한편, 로봇의 기하모델이 진입 금지영역과 간섭하지 않는 경우에는, 초기궤도 상의 점에 대응하는 경유자세가 주어지므로, 최종자세에 접근한다. 그리고, 상기 구성에 의하면, 새롭게 주어진 경유자세를 현재자세로서 이 순서를 반복하므로, 로봇의 기하모델이 진입 금지영역과 간섭하지 않는 순서가 출현할 때마다, 이 순서에 있어서의 초기궤도상의 점에 대응하는 경유자세가 주어져 최종자세에 접근한다. 따라서, 시행착오를 반복하는 것에 의해, 최종적으로, 로봇이, 장애물과의 충돌을 회피하면서 현재자세에서 최종자세로 변화하는 경우에 있어서의 핸드의 갱신궤도를 얻을 수 있다.

그 결과, 로봇의 기하모델, 진입금지영역, 및 동작영역의 설정이라는 단순한 설정으로, 로봇이 어느 위치에 정지하여도 장애물과의 간섭을 자동적으로 회피해서, 안전하게 로봇을 소정의 자세(예를 들면, 퇴피(退避)자세)에 도달시킬 수 있다.

상기 최종자세 결정단계는, 기지(旣知)의 궤도 상에 설정되는 상기 로봇의 복수의 최종후보자세 중에서, 상기 로봇의 현재자세와 상기 최종후보자세와의 상기 각 관절의 각도변위의 절대치의 총합이 최소값인 최종후보자세를 상기 최종자세로서 선택하여, 결정해도 좋다.

이 구성에 의하면, 로봇의 현재자세에 가까운 자세를 최종자세로서 선택할 수 있다. 따라서, 최종자세에 도달하기까지에 요하는 시간을 단축할 수 있다.

상기 경유자세 결정단계는, 상기 가상반발력에 의해 어떤 가상자세에 있어서의 상기 로봇의 상기 각 관절의 회전축 주위에 생기는 토크(torque)를 산출하고, 게다가 이 토크의 영향에 의해 변화하는 상기 로봇의 소정시간 경과 후의 가상자세를 산출하는 연산을 상기 가상자세를 기점(起点)으로 해서 반복하는 것에 의해 상기 로봇의 가상자세의 경시(經時)변화를 산출하고, 상기 로봇의 가상자세의 경시변화가 수렴했을 때의 상기 로봇의 가상자세에 근거해서 상기 경유자세를 결정해도 좋다.

이 구성에 의하면, 로봇과 진입 금지영역과의 간섭을 회피한 경유자세를 적절하게 제공할 수 있다.

상기 가상반발력은, 상기 기하모델이 진입 금지영역에 진입한 거리에 비례해서 커지도록 구성되어 있어도 좋다.

이 구성에 의하면, 로봇의 기하모델이 진입 금지영역에 깊이 진입한 경우라도, 신속히 진입 금지영역의 바깥으로 밀어낼 수 있다.

상기 로봇의 자세가 상기 최종자세에 접근하도록 경시변화 하지 않고, 정류상태에 빠져있는지 아닌지를 판정하는 정류(停留)상태 판정단계와, 상기 정류상태 판정단계에서 정류상태에 빠져있다고 판정한 경우, 상기 로봇의 상기 가상 반발력에 의해 밀려난 상태의 자세 이외의 경유후보자세를 생성하는 경유후보자세 생성단계와, 상기 경유후보자세가 상기 최종자세에 접근하는 자세인지 아닌지를 판정하는 제1판정단계와, 상기 제1판정단계에서, 상기 경유후보자세가 상기 최종자세에 접근하는 방향으로 변화하는 자세라고 판정한 경우, 이 경유후보자세에 근거해서 상기 경유자세를 결정하는 제1결정단계를 가지고 있어도 좋다.

이 구성에 의하면, 예를 들면, 반발력에 의해 되돌리는 방향이 초기궤도의 방향과 일치한 경우에는, 동작영역에 되돌려진 자세가 초기궤도상의 점에 대응하므로, 진입 금지영역에 대한 기하모델의 간섭이 반복되어 정류상태에 빠져서, 로봇이 최종자세로 접근하지 않는다. 그러나, 상기 구성에 의하면, 초기궤도상에 없는 점에 대응하는 경유자세가 얻어지므로, 이와 같은 정류상태에 빠진 경우라도, 최종자세에 접근하는 다른 경유자세를 경유해서, 최종자세에 도달할 수 있다.

상기 제1판정단계에서, 상기 경유후보자세가 상기 최종후보에서 멀어지는 방향으로 변화하는 자세라고 판정한 경우, 확률치에 근거해서 상기 경유후보자세를 선택하는지 아닌지를 판정하는 제2판정단계와, 상기 제2판정단계에서 선택한다고 판정되면, 그 경유후보자세에 근거해서 상기 경유자세를 결정하는 제2결정단계를 가지고 있어도 좋다.

이 구성에 의하면, 최종자세에 접근하는 다른 자세를 생성할 수 없는 경우라도, 다른 경유자세를 경유해서 최종자세에 도달할 수 있는지 아닌지를 시도할 수 있다. 이것에 의해, 현재의 최종자세에 도달할 수 없는 상태를 매우 적절하게 벗어날 수 있다.

상기 과제를 해결하기 위해, 본 발명의 어떤 태양(態樣)에 관계되는 암형 로봇의 제어장치는, 1 이상의 링크가 관절에 의해 연결되고, 선단부에 핸드가 설치된 로봇암을 구비하는 암형로봇의 제어장치로서, 상기 로봇을, 기하형상을 갖도록 모델링 해서 기하모델로서 표현하는 기하모델 표현부와, 상기 기하모델이 진입해서는 안 되는 진입 금지영역과, 이 진입 금지영역에 의해 규정되어, 상기 기하모델이 동작하는 동작영역을 설정하는 영역 설정부와, 상기 로봇의 최종자세를 결정하는 최종자세 결정부와, 상기 로봇이 현재자세에서 상기 최종자세를 향하여 변화하는 경우에 있어서의 상기 핸드의 초기궤도를 결정하는 초기궤도 결정부와, 상기 초기궤도상의 소정의 점에 대응하는 상기 로봇의 가상자세를 산출하는 가상자세 산출부와, 상기 가상자세에 있어서의 상기 기하모델이 상기 진입 금지영역과 간섭하는지 아닌지를 판정하는 간섭판정부와, 경유자세를 결정하는 경유자세 결정부와, 갱신궤도 결정부를 구비하고, 상기 경유자세 결정부는, 상기 간섭 결정부가 간섭하지 않는다고 판정한 경우, 상기 가상자세를 경유자세로서 결정하고, 간섭한다고 판정한 경우, 상기 진입 금지영역의 간섭부분과 상기 기하모델의 간섭부분이 상대적으로 반발하는 반발력을 가상적으로 발생시켜서, 가상 반발력에 의해 상기 기하모델의 간섭부분이 상기 진입 금지영역에서 상기 동작영역으로 밀려난 상태의 자세를 산출하고, 이 산출한 자세를 경유자세로서 결정하고, 상기 갱신궤도 결정부는, 상기 현재자세에서 상기 경유자세를 경유해서 상기 최종자세로 변화하는 경우에 있어서의 상기 핸드의 궤도를 갱신궤도로서 결정하고, 상기 경유자세 결정부가 결정한 최신의 경유자세를 상기 초기궤도 결정부의 현재자세라고 가정해서, 상기 초기궤도 결정부, 상기 가상자세 산출부, 상기 간섭판정부, 상기 경유자세 결정부, 및 상기 갱신궤도 결정부가 반복하여 처리하도록 구성되어 있다.

상기 구성에 의하면, 로봇이 현재자세에서 최종자세를 향하여 변화하는 경우에 있어서의 핸드의 초기궤도상의 소정의 점에 대응하는 로봇의 가상자세를 산출하고, 가상자세에 있어서의 로봇의 기하모델이 진입금지영역(장애물)과 간섭하는지 아닌지를 판정한다. 그리고, 간섭하지 않는다고 판정한 경우에는 가상자세를, 현재자세에서 최종자세를 향하여 변화하는 경우에 경유하는 경유자세로서 결정하고, 간섭한다고 판정한 경우에는, 가상반발력에 의해 기하모델의 간섭부분이 진입금지영역에서 동작영역으로 밀려난 상태의 자세를 산출하고, 이 자세를 경유자세로서 결정한다. 따라서, 로봇의 기하모델이 진입 금지영역과 간섭하는 경우에는, 진입금지영역에 간섭하지 않는 경유자세가 주어지므로, 장애물과의 충돌이 회피된다. 다만, 이 경우에는 당초의 초기궤도에서 벗어나기 때문에 최종자세에 접근하는 일은 담보되지 않는다. 한편, 로봇의 기하모델이 진입금지영역과 간섭하지 않는 경우에는, 초기궤도상의 점에 대응하는 경유자세가 주어지므로, 최종자세에 접근한다. 그리고, 상기 구성에 의하면, 새롭게 주어진 경유자세를 현재자세로서 이 순서를 반복하므로, 로봇의 기하모델이 진입금지영역과 간섭하지 않는 순서가 출현할 때마다, 이 순서에 있어서의 초기궤도상의 점에 대응하는 경유자세가 주어져서 최종자세에 접근한다. 따라서, 시행착오를 반복하는 것에 의해, 최종적으로, 로봇이, 장애물과의 충돌을 회피하면서 현재자세에서 최종자세로 변화하는 경우에 있어서의 핸드의 갱신궤도를 얻을 수 있다.

본 발명은, 단순한 설정으로, 로봇이 어느 위치에서 정지해도 장애물과의 간섭을 자동으로 회피해서, 안전하게 로봇을 소정의 자세에 도달시킬 수 있다고 하는 효과를 나타낸다.

도1은 본 발명의 실시형태1에 관계되는 로봇을 구비하는 설비의 구성예를 보여주는 평면도이다.
도2는 도1의 로봇의 제어계통의 구성예를 개략적으로 보여주는 블록도이다.
도3는 도1의 로봇의 최종자세의 선택방법을 보여주는 설명도이다.
도4는 도1의 로봇의 경유자세의 산출방법을 보여주는 플로우 차트이다.
도5a는 도1의 로봇의 경유자세의 산출방법을 보여주는 설명도이다.
도5b는 도1의 로봇의 경유자세의 산출방법을 보여주는 설명도이다.
도5c는 도1의 로봇의 경유자세의 산출방법을 보여주는 설명도이다.
도6는 도1의 로봇의 동작예를 보여주는 플로우 차트이다.
도7a는 도1의 로봇의 동작예를 보여주는 도면이다.
도7b는 도1의 로봇의 동작예를 보여주는 도면이다.
도7c는 도1의 로봇의 동작예를 보여주는 도면이다.
도7d는 도1의 로봇의 동작예를 보여주는 도면이다.
도7e는 도1의 로봇의 동작예를 보여주는 도면이다.
도7f는 도1의 로봇의 동작예를 보여주는 도면이다.
도8는 본 발명의 실시형태 2에 관계되는 로봇의 동작예를 보여주는 플로우 차트이다.
도9는 도8의 로봇의 처리에서 이용하는 천이확률(遷移確率)을 보여주는 그래프이다.
도10a는 도8의 로봇의 시뮬레이션 조건을 보여주는 도면이다.
도10b는 도8의 로봇의 시뮬레이션 조건을 보여주는 도면이다.
도10c는 도8의 로봇의 시뮬레이션 조건을 보여주는 도면이다.

이하, 본 발명의 실시형태에 대해서, 도면을 참조하면서 설명한다. 더구나, 본 발명의 실시형태에 의해 본 발명이 한정되는 것이 아니다. 또한, 이하에서는, 모든 도면을 통해서, 동일 또는 상당하는 요소에는 동일한 참조부호를 붙이고, 그 중복되는 설명을 생략한다.

(실시형태 1)

도1은, 본 발명의 실시형태1에 관계되는 로봇(100)을 구비하는 설비(110)의 구성예를 보여주는 평면도이다.

설비(110)는, 예를 들면 기판의 처리를 행하는 설비이고, 내부에 로봇(100)이 설치되어, 예를 들면, 챔버(chamber, 113)를 둘러싸는 벽(111)과, 챔버 내부에서 벽(111)에서 내측으로 돌출하고 있는 벽(112)을 갖는다. 이들 벽(111) 및 벽(112)은, 로봇(100)의 동작에 있어서의 장애물을 구성한다.

[로봇의 구성]

로봇(100)은, 예를 들면 반도체 웨이퍼(wafer), 글라스(glass) 웨이퍼 등의 기판을 반송(搬送)하는 로봇이다.

도1에 나타낸 바와 같이, 로봇(100)은, 1 이상의 링크가 관절에 의해 연결되고, 선단부에 핸드가 설치된 로봇암을 구비하는 암형의 로봇이다. 더욱 구체적으로는, 로봇(100)은, 수평 다관절로봇이고, 기대(基臺)(1)와, 제1암(2)과, 제2암(3)과, 핸드(4)를 구비하고, 이들이 직쇄상(直鎖狀)으로 연결되어 있다. 본 실시형태에서, 제1암(2), 제2암(3), 핸드(4)가 로봇암을 구성한다. 또한, 로봇(100)은, 로봇(100)의 동작을 제어하는 제어기(5)(도2참조)를 구비한다.

기대(基臺)(1)는, 예를 들면, 중공(中空)의 원통형 부재이다. 기대(1)의 내부에는, 서보모터를 포함하는 제1암구동부(12)(도2참조)가 배설(配設)되어 있다. 제1암구동부(12)는, 후술하는 제1관절축(2a)을 회동(回動)시키도록 구성되어 있다.

제1암(2)은, 예를 들면, 중공(中空)의 판형(板形) 부재이며, 평면시(平面視)에서 대략 단책형(短冊形)으로 형성되어 있다. 제1암(2)은, 제1암(2)의 기단부(基端部)의 저면(底面)에서 하방(下方)으로 돌출되도록 제1관절축(2a)이 형성되어 있다. 그리고, 제1관절축(2a)은, z방향으로 연장되는 회동축선(回動軸線)(L1)을 중심으로 회동 가능하게 기대(1)에 부착되고, 이 부분이 제1관절을 구성하고 있다. 따라서, 제1암(2)은, xy평면에서 회동하도록 구성되어 있다. 더구나, 본 실시형태에 있어서, 회동축선(L1)이 xy평면상에 있어서의 기준점(O)을 구성한다.

제1암(2)의 내부에는, 서보모터를 포함하는 제2암구동부(13)(도2참조)가 배설되어 있다. 제2암구동부(13)는, 후술하는 제2관절축(3a)을 회동시키도록 구성되어 있다.

그리고, 제1암(2)의 기대(1)에 대한 회동축선(L1) 주위의 상대적인 각도위치(q1)(도3참조)는, 제1암구동부(12)의 서보모터의 엔코더에 의해 검출된다. 이 엔코더가 제1암 각도위치 검출부(15)(도2참조)를 구성한다.

더구나, 제1암(2)은 1번째의 링크라고 말하기도 하고, 또 제1관절축(2a)을 1축이라고 말하기도 한다.

제2암(3)은, 예를 들면, 중공(中空)의 판형(板形) 부재이고, 평면시(平面視)에서 대략 단책형으로 형성되어 있다. 제2암(3)은, 제2암(3)의 기단부(基端部)의 저면(底面)에서 하방(下方)으로 돌출되도록 제2관절축(3a)이 설치되어 있다. 그리고, 제2관절축(3a)은, 회동축선(L1)과 평행하게 연장되는 회동축선(L2)을 중심으로 회동 가능하게 제1암(2)에 부착되어, 이 부분이 제2관절을 구성하고 있다. 따라서, 제2암(3)은, xy평면상을 회동하도록 구성되어 있다.

제2암(3)의 내부에는, 서보모터를 포함하는 핸드구동부(14)(도2참조)가 배설(配設)되어 있다. 핸드구동부(14)는, 후술하는 제3관절축(4a)을 회동시키도록 구성되어 있다.

그리고, 제2암(3)의 제1암(2)에 대한 회동축선(L2) 주위의 상대적인 각도위치(q2)(도3참조)는, 제2암구동부(13)의 서보모터의 엔코더에 의해 검출된다. 이 엔코더가 제2암 각도위치 검출부(16)(도2참조)를 구성한다.

더구나, 제2암(3)은 2번째의 링크라고 말하기도 하며, 또 제2관절축(3a)을 2축이라고 말하기도 한다.

핸드(4)는, 기판(基板)을 유지(保持)하는 것이고, 평판형으로 형성되어 있다. 핸드(4)는, 그 기단부의 저면에서 하방으로 돌출되도록 형성된 제3관절축(4a)을 가지고 있다. 그리고, 제3관절축(4a)은, 회동축선(L1, L2)과 평행하게 연장되는 회동축선(L3)을 중심으로 회동 가능하게 제2암(3)에 부착되어, 이 부분이 제3관절을 구성하고 있다. 따라서, 핸드(4)는, xy평면에서 회동하도록 구성되어 있다.

그리고, 핸드(4)의 제2암(3)에 대한 회동축선(L3)의 상대적인 각도위치(q3)(도3참조)는, 핸드구동부(14)의 서보모터의 엔코더에 의해 검출된다. 이 엔코더가 핸드 각도위치 검출부(17, 도2참조)를 구성한다.

더구나, 핸드(4)는 3번째의 링크라고 말하기도 하며, 또 제3관절축(4a)을 3축이라고 말하기도 한다.

[제어부]

도2는, 로봇(100)의 제어계통의 구성예를 개략적으로 보여주는 블록도이다.

로봇(100)이 구비하는 제어기(5)는, 예를 들면, CPU 등의 연산기를 갖는 제어부(21)와, ROM 및 RAM 등의 메모리를 갖는 기억부(22)를 구비하고 있다. 제어기(5)는, 집중제어하는 단독의 제어기로 구성되어 있어도 좋고, 서로 협동해서 분산제어하는 복수의 제어기로 구성되어도 좋다. 제어부(21)는, 제1암 제어부(31), 제2암 제어부(32), 핸드제어부(33), 최종자세 결정부(34), 초기궤도 결정부(35), 가상자세 산출부(36), 기하모델 표현부(37), 영역설정부(40), 간섭판정부(38), 경유자세 결정부(39), 갱신궤도 결정부(41)를 포함한다. 이들의 기능부(31~41)는, 기억부(22)에 격납된 소정의 제어 프로그램을 제어부(21)가 실행하는 것에 의해 실현되는 기능블록(기능모듈)이다.

제1암 제어부(31)는, 제1암 구동부(12)를 제어하고, xy평면에서 회동축선(L1) 주위로 제1암(2)을 회동시킨다.

제2암 제어부(32)는, 제2암 구동부(13)를 제어하고, xy평면에서 회동축선(L2) 주위로 제2암(3)을 회동시킨다.

핸드제어부(33)는, 핸드구동부(14)를 제어하고, xy평면에서 회동축선(L3) 주위로 핸드(4)를 회동시킨다.

또한, 본 실시형태에서는, 예를 들면, 제어부(21)의 주제어부(도시하지 않음)가, 로봇(100)의 제1암 각도위치 검출부(15), 제2암 각도위치 검출부(16), 및 핸드 각도위치 검출부(17)가 각각 검출하는 각도위치와, 최종자세에 근거해서, 제1암제어부(31), 제2암제어부(32), 및 핸드제어부(33)로 최종각도위치를 각각 출력한다. 제1암제어부(31), 제2암제어부(32), 및 핸드제어부(33)는, 각각, 대응하는 제어대상물(제1관절축(2a), 제2관절축(3a), 및 제3관절축(4a))의 각도위치가 최종자세에 대응하는 최종 각도위치가 되도록, 대응하는 각도위치 검출부(15, 16, 17)가 검출하는 회동축의 각도위치에 근거해서, 대응하는 제1암구동부(12), 제2암구동부(13), 및 핸드구동부(14)를 각각 피드백 제어한다.

기하모델 표현부(37)는, 로봇(100)을, 기하형상을 갖도록 모델링해서 기하모델(M)로서 표현한다.

본 실시형태에서, 기하모델 표현부(37)는, 제1암(2), 제2암(3), 및 핸드(4)를 기하모델(M1, M2, 및 M3)로 표현한다.

제1암(2)의 기하모델(M1)은, 도1에 나타낸 바와 같이, 예를 들면, 타원형(長圓形)의 모델이다. 기하모델(M1)은, 제1암(2)을 xy평면에 투영한 도형의 외측에서, 이 도형과 이격되도록 표현된다.

제2암(2)의 기하모델(M2)은, 예를 들면, 타원형의 모델이다. 기하모델(M2)는, 제2암(3)을 xy평면에 투영한 도형의 외측에서, 이 도형과 이격되도록 표현된다.

핸드(4)의 기하모델(M3)은, 예를 들면, 수적형(水滴型)의 모델이다. 기하모델(M3)는, 기판(基板)을 유지(保持)하는 핸드(4)를 xy평면에 투영한 도형의 외측에서, 이 도형과 이격되도록(離間) 규정된다.

도1 및 도2에 나타낸 바와 같이, 영역설정부(40)는, 기하모델(M1~M3)이 진입해서는 아니 되는 진입금지영역(S1)과, 진입금지영역(S1)에 의해 규정(획정)되고, 기하모델(M1~M3)이 동작하는 동작영역(S2)을 설정한다.

본 실시형태에서, 진입금지영역(S1) 및 동작영역(S2)은, 기억부(22)에 격납되어 있고, 영역설정부(40)가 기억부(22)로부터 진입금지영역(S1) 및 동작영역(S2)을 읽어내는 것에 의해, 진입금지영역(S1) 및 동작영역(S2)을 설정한다.

또한, 진입금지영역(S1)은, 설비(110)의 챔버(113)의 내벽면(벽(111) 및 벽(112)의 벽면)보다도 내측의 영역을 포함하도록 설정된다. 또한, 진입금지영역(S1)과 동작영역(S2)의 경계를 따라 연장되는 가상선이 경계선(B)을 구성한다.

최종자세 결정부(34)는, 로봇(100)의 최종자세(Pt)를 결정한다.

본 실시형태에서, 최종자세 결정부(34)는, 기억부(22)에 기억되어 있는 복수의 최종후보자세(Ptc)에서 하나의 최종자세(Pt)를 선택하여, 결정한다.

최종후보자세(Ptc)는, 진입금지영역(S1)과 간섭하지 아니하는 기지(旣知)의 궤도상에 설정되는 자세이다. 따라서, 로봇(100)은, 어느 한쪽의 자세에 도달할 수 있으면, 이후는 기지(旣知)의 궤도를 더듬어, 진입금지영역(S1)과 간섭하지 않고 동작할 수 있다.

도3은, 로봇(100)의 최종자세(Pt)의 선택방법을 보여주는 설명도이다.

도3에 나타낸 바와 같이, 최종자세결정부(34)가 복수의 최종후보자세(Ptc)(도3에서는 그 중의 하나를 보여준다)에서 하나의 최종자세(Pt)를 선택할 때는, 우선, 현재자세(Ps)와 최종후보자세(Ptc)의 각 관절축의 관절각도 변위의 절대치 총합(E)을 이하의 식에 근거해서 산출한다.

[수1]

… (1)

단,

n 은, 축의 수

q _i 는, i축의 목표자세에 있어서의 관절각도

q _di 는, i축의 목표후보자세에 있어서의 관절각도

다음에, E가 최소치인 최종후보자세 Ptc를 선택하고, 이것을 최종자세 Pt로서 결정한다.

초기궤도결정부(35)는, 로봇(100)이 현재자세(Ps)에서부터 최종자세 결정부(34)에서 결정된 최종자세(Pt)를 향하여 변화하는 경우에 있어서의 핸드(4)의 초기궤도(Ta)를 결정한다. 본 실시형태에 있어서, 초기궤도(Ta)는, 현재자세(Ps) 및 최종자세(Pt)가 주어지는 것에 의해 일의적으로 결정되는 궤도이다.

가상자세산출부(36)는, 초기궤도(Ta)상의 소정의 점에 있어서의 로봇(100)의 가상자세(Pp)를 산출한다.

간섭측정부(38)는, 가상자세(Pp)에 있어서의 로봇(100)의 기하모델(M1~M3)이 진입금지영역(S1)과 간섭하는지 아닌지를 각각 판정한다.

경유자세결정부(39)는, 현재자세(Ps)에서 최종자세(Pt)를 향하여 변화하는 경우에 경유하는 경유자세(Pv)를 산출한다.

더욱더, 경유자세결정부(39)는, 간섭판정부(38)가 로봇(100)의 기하모델(M1~M3)과 진입 금지영역(S1)이 전부 간섭하지 않는다고 판정한 경우, 가상자세(Pp)를 경유자세(Pv)로서 결정한다.

한편, 경유자세결정부(39)는, 기하모델(M1~M3) 중 적어도 하나의 기하모델과 진입금지영역(S1)이 간섭한다고 판정한 경우, 진입금지영역(S1)의 간섭부분과 기하모델의 간섭부분이 상대적으로 반발하는 반발력(fi)을 가상적으로 발생시켜, 가상반발력(fi)에 의해 기하모델의 간섭부분이 진입금지영역(S1)에서 동작영역(S2)으로 밀려난 상태의 자세를 산출하고, 이 산출한 자세를 경유자세(Pv)로서 결정한다.

도4는, 로봇(100)의 경유자세(Pv)의 산출방법을 보여주는 플로우 차트이다.

도5a~5c는, 로봇(100)의 경유자세(Pv)의 산출방법을 보여주는 설명도이다.

경유자세(Pv)를 산출하기 위해 경유자세결정부(39)가 행하는 처리에 대해서 이하에 상세하게 설명한다. 더욱이, 본 실시형태에서, 로봇(100)은, 3개의 링크를 갖는 로봇이지만, 이하에서는 n개의 링크를 갖는 로봇으로서 일반화한 경우의 사고방식을 보여준다.

우선, 로봇(100)의 기하모델이 진입금지영역(S1)에 진입한 거리 di를 결정한다(단계S71). 거리 di의 산출은, 가상자세(Pp)에 있어서의 각 기하모델과 진입금지영역(S1)의 간섭형태에 따라서 행한다.

예를 들면, 도5a에 나타낸 경우, 우선, 기하모델(Mi)의 간섭부분(도5a에서 박묵(薄墨)을 붙인 부분)에서 경계선 B와의 거리가 최대가 되는 기하모델(Mi)상의 점 P(x, y)를 산출한다. 그리고, 경계선 B의 법선방향에 있어서, 점 P와 경계선 B와의 거리를 거리 di로 설정한다.

또한, 도5b에 나타낸 경우, 우선, 기하모델(Mi)의 간섭부분(도5b에서 박묵(薄墨)을 붙인 부분)에서 경계선 B와의 거리가 최대가 되는 기하모델(Mi)상의 점 P(x, y)를 산출한다. 그리고, 점 P와 경계선 B의 정점(頂点) Q와의 거리를 거리 di로 설정한다.

더욱, 도5c에 나타낸 경우, 우선, 기하모델(Mi)의 간섭부분(도5c에서 박묵(薄墨)을 붙인 부분)에서 경계선 B의 정점(頂点) Q에서 기하모델과 경계선 B와의 한쪽의 교점 R을 향하는 쪽에 위치하는, 경계선 B와의 거리가 최대가 되는 기하모델(Mi)상의 점 P1(x, y)를 산출한다. 그리고, 선분 QR의 법선 방향에서, 점 P1과 경계선 B와의 거리를 거리 di1로 설정한다.

또한, 기하모델(Mi)의 간섭부분에서, 경계선 B의 정점 Q에서 기하모델과 경계선 B와의 다른 쪽의 교점 R'을 향하는 쪽에 위치하는, 경계선 B와의 거리가 최대가 되는 기하모델상의 점 P2(x, y)를 산출한다. 그리고, 선분 QR'의 법선 방향에 있어서, 점 P2와 경계선 B와의 거리를 거리 di2로 설정한다.

이와 같이, 거리 di의 산출은, 가상자세(Pp)에 있어서의 기하모델과 진입금지영역(S1)과의 간섭형태에 따르도록 구성되지만, 상기의 구성에 한정되는 것은 아니다.

다음에, 결정한 거리 di에 근거하고, 이하의 식 (2)에 근거해서 가상반발력 fi를 각각의 링크에 대해서 산출한다(단계 S72).

[수2]

…(2)

단,

i 는, 링크번호

k 는, 임의의 가상강성계수(假想剛性係數)

d _i 는, 진입금지경계선 B 에서의 진입거리

또한, 도5c에 나타낸 경우는, 결정한 거리 di1 및 di2에 근거하고, 식(2)에 근거해서 각각 가상반발력 fi1 및 fi2를 산출하고 나서, 가상반발력 fi1 및 fi2를 합성하여, 가상반발력 fi를 산출한다.

즉, 진입금지영역(S1)과 간섭하고 있다고 판정된 i번째의 링크에 대해서, 기하모델이 금지영역(S2)에 진입한 거리에 비례하는 가상반발력 fi를 가상적으로 발생시킨다. 이것에 의해, 기하모델이 진입금지영역(S1)에 깊이 진입한 경우는, 강하게 밀어내도록 되어 있다. 따라서, 기하모델이 밀려날 때까지 걸리는 시간을 단축할 수 있다.

한편, 진입금지영역(S1)과 간섭하고 있지 않다고 판정된 i번째의 링크에는, 가상반발력 fi를 발생시키지 않도록 구성되어 있다.

다음에, 가상반발력 fi에 의해 생기는 로봇(100)의 각축 주위에 생기는 가상의 토크 τ(torque)를 산출한다(단계S73).

[수3]

…(3)

단,

τ_i 는, i 번째의 축에 생기는 토크

는, i 번째의 (1≤i≤n)의 링크길이

w_n은, i<n 번째의 가상반발력 f_i의 영향이 i번째의 링크에서 떨어져 있는 만큼 작아지도록 설정한 가중계수(加重係數)

(예 : w_n=1, w_n _-1=1/2,…, w₁=(1/2)^n-1)

는, xy평면에서 진입금지경계선 B의 법선방향과 x축이 이루는 각도

는, xy평면에서 i번째(1≤i≤n)의 링크와 x축이 이루는 각도

즉, 경계선 B와 간섭하고 있다고 판정한 i번째의 링크에서 발생하는 가상반발력 fi는, i번째의 링크만이 아니라, i<n번째의 링크에도 영향을 주도록 구성되어 있다. 이것에 의해, i번째의 링크에 가상반발력 fi를 발생시킨 점이 이 링크의 사점(死點)인 경우라도, 이 가상반발력 fi는 다른 링크에 영향을 주어 토크를 발생시킨다. 따라서, 이 토크에 의해 다른 링크가 회동하고, 가상자세 Pp와는 서로 다른 경유자세 Pv를 생성할 수 있다.

또한, 각도 ψi의 산출은, 각 기하모델과 진입금지영역 S1과의 간섭형태에 따라 행한다.

예를 들면, 도5a에 나타낸 경우, 각도 ψi는, xy평면에서 경계선 B의 법선방향과 x축이 이루는 각도이다.

또한, 도5b에 나타낸 경우, 각도 ψi는, 선분 PQ와 x축이 이루는 각도이다.

더욱이, 도5c에 나타낸 경우, 각도 ψi는, 상기 가상반발력의 합성벡터와 x축이 이루는 각도이다.

다음에, 상기 가상토크 τi의 영향에 의해 생기는, 로봇(100)의 가상자세의 경시변화(經時變化)를 산출한다.

[수4]

… (4)

단,

q[0] 는, 현재자세

τ[0] 는, (3)식에 의해 산출

α, β 는, 로봇이 원하는 거동이 되도록 설정한 상수

여기에서 q(·)는, q 위에 부호 ·(도트)가 한 개 붙여진 기호로 한다.

그리고, (2)식과 (3)식을 반복해서 계산하는 것에 의해, τ[k]와 q(·)[k]가 0에 수렴했을 때의 q[k]를 산출한다. 이 q[k]가 제1암(2), 제2암(3), 및 핸드(4)가 진입금지영역(S1)으로 밀려난 자세이다.

그리고, q[k]에 근거해서, 가상자세 Pp를 설정한다(단계 S74).

회피궤도결정부(41)는, 현재자세 Ps에서 경유자세 Pv를 경유해서 최종자세 Pt로 변화하는 경우에 있어서의 핸드(4)의 궤도를 갱신궤도 Tb로서 결정한다.

[동작예]

다음에, 로봇(100)의 동작예를 설명한다.

도6은, 본 발명의 실시형태에 있어서의 로봇(100)의 동작예를 보여주는 플로우 차트이다. 도7a~도7f는, 본 발명의 실시형태에 있어서의 로봇(100)의 동작예를 보여주는 도면이다.

우선, 도7a에 나타낸 바와 같이, 제어부(21)의 최종자세 결정부(34)는, 로봇(100)의 최종자세 Pt를 결정한다(최종자세결정단계)(단계S10). 본 실시형태에서, 제어부(21)의 최종자세결정부(34)는, 4개의 최종후보자세 Ptc1~Ptc4에 대해서 E의 값을 구하고, E의 값이 최소가 되는 최종후보자세 Ptc4를 최종자세 Pt로서 선택한다. 이와 같이, 현재자세 Ps에 가까운 자세를 최종자세 Pt로서 선택하도록 구성되고 있으므로, 현재자세 Ps에서 최종자세 Pt에 도달하기까지 요하는 각 축(軸)의 변위를 적게 할 수 있고, 도달하기까지 걸리는 평균시간을 단축할 수 있다.

다음에, 도7b에 나타낸 바와 같이, 제어부(21)의 초기궤도결정부(35)는, 로봇(100)이 현재자세 Ps에서 최종자세 Pt를 향하여 변화하는 초기궤도 Ta를 결정한다(초기궤도결정단계)(단계S20).

다음에, 도7c에 나타낸 바와 같이, 제어부(21)의 가상자세산출부(36)는, 초기궤도 Ta에 있어서 현재자세 Ps의 근방(近傍)이고, 현재자세 Ps에서 최종자세 Pt를 향하는 쪽(측)에 있어서 소정의 점에서의 로봇(100)의 가상자세 Pp를 산출한다(가상자세산출단계)(단계S30). 여기에서, 근방이란, 현재자세에서 각 축(軸)에 미소(微小)한 변경을 가해서 얻어지는 자세를 말한다.

다음에, 제어부(21)의 영역설정부(40)는, 진입금지영역(S1) 및 동작영역(S2)을 기억부(22)에서 읽어 들여서, 설정한다(영역설정단계)(단계S35).

다음에, 제어부(21)의 기하모델표현부(37)는, 가상자세 Pp에 있는 로봇(100)의 제1암(2), 제2암(3), 및 핸드(4)를 모델링 해서, 각각 기하모델 M1, M2, 및 M3로 표현한다(기하모델표현단계)(단계S40).

기하모델 M1~M3는, 로봇(100)의 제1암(2), 제2암(3), 및 핸드(4)를 간략화 하여 나타낸 모델이므로, 후술하는 간섭의 판정에 걸리는 계산량을 삭감할 수 있고, 간섭의 판정을 신속하게 행할 수 있다.

또한, 상술한 대로, 기하모델 M1에서 M3의 가상선에 의해 그려진 도형의 외주(外周)는, 기판을 유지(保持)하는 핸드(4)를 xy평면에 투영한 도형의 외측에서, 이 도형과 이격되도록 규정(획정)된다. 즉, 기하모델은, 제1암(2), 제2암(3), 및 핸드(4)보다도 두툼하게 표현되므로, 후술하는 단계(S50)에서 제어부(21)의 간섭판정부(38)가 기하모델과 진입 금지영역(S1)이 간섭하고 있다고 판정한 경우라도, 로봇(100)과 설비(110)가 간섭하지 않도록 할 수 있으며, 실제로 로봇(100) 또는 로봇(100)이 유지(保持)하는 기판이 설비(110)에 접촉하는 것을 방지할 수 있다.

또한, 진입금지영역(S1)은, 설비(110)의 챔버(113)의 내벽면보다도 내측의 영역을 포함하도록 설정되므로, 실제로 로봇(100) 또는 로봇(100)이 유지(保持)하는 기판이 설비(110)에 접촉하는 것을 보다 더 확실하게 방지할 수 있다.

다음에, 제어부(21)의 간섭판정부(38)는, 가상자세 Pp에 있어서의 기하모델 M1~M3가 진입금지영역(S1)과 간섭하는지 아닌지를 각각 판정한다(간섭판정단계)(단계S50). 즉, 기하모델과 진입금지영역(S1)이 간섭하고 있다는 것은, 기하모델의 경계선과 경계선 B가 교차하고 있는 상태라는 것이다.

더욱이, 본 실시형태에서, 기하모델표현부(37)는 간섭판정부(38)에 의한 간섭판정 직전의 단계에서 기하모델의 생성을 행하고 있지만, 이것에 한정되는 것이 아니고, 간섭판정부(38)에 의한 간섭판정보다 전(前)의 임의의 단계에서 실행해도 좋다.

마찬가지로, 영역설정부(40)에 의한 진입금지영역(S1) 및 진입금지영역(S2)의 설정도 간섭판정부(38)에 의한 간섭판정보다 전(前)의 임의의 단계에서 실행해도 좋다.

그리고, 어느 것도 간섭하지 않는다고 판정한 경우(단계 S50에서 No), 제어부(21)의 경유자세결정부(39)는, 가상자세 Pp를 경우자세 Pv로서 결정한다(경우자세 결정단계)(단계S60).

따라서, 로봇(100)의 기하모델이 진입금지영역(S1)과 간섭하지 않는 경우에는, 초기궤도 Ta상의 점에 대응하는 경우자세가 주어지므로, 최종자세 Pt에 접근한다.

한편, 도7d에 나타낸 바와 같이, 단계 S50에서, 제어부(21)의 간섭판정부(38)가 기하모델과 경계선 B가 간섭한다고 판정한 경우(단계S50에서 Yes), 제어부(21)의 경유자세결정부(39)는, 도7e에 나타낸 바와 같이, 기하모델의 간섭부분이 진입금지영역(S1)에서 동작영역(S2)으로 밀려난 상태의 자세를 산출하고, 이 산출한 자세를 경유자세 Pv로서 결정한다(경유자세 결정단계)(단계S70).

따라서, 로봇(100)의 기하모델이 진입금지영역(S1)과 간섭하는 경우에는, 진입금지영역(S1)에 간섭하지 않는 경우자세 Pv가 주어지므로 장애물과의 충돌이 회피된다.

더욱이, 상기 가상자세를 경유자세 Pv로 결정할 때에, 다시, 현재자세에서 상기 밀려난 상태의 자세에 이르는 궤도가 진입금지영역(S1)과 간섭하는지 아닌지를 판정해도 좋다.

단계 S60 또는 단계 S70에서 경유자세 Pv가 결정되면, 다음에, 제어부(21)의 갱신궤도결정부(41)는, 현재자세 Ps에서 경유자세 Pv를 경유해서 최종자세 Pt로 변화하는 경우에 있어서의 핸드(4)의 궤도를 갱신궤도 Tb로서 결정한다(갱신궤도결정단계)(단계 S80).

다음에, 제어부(21)는, 경우자세 Pv를 새로운 현재자세 Ps로서 결정함과 동시에, 갱신궤도 Tb를 새로운 초기궤도 Ta로서 결정(가정)한다(단계 S90).

다음에, 제어부(21)의 가상자세산출부(36)는, 초기궤도 Ta에 있어서 현재자세 Ps의 근방이고, 현재자세 Ps에서 최종자세 Pt를 향하는 쪽에서 소정의 점(点)에 있어서의 새로운 가상자세 Pp를 산출한다(단계 S100).

다음에, 단계 S100에서 산출한 가상자세 Pp가 최종자세 Pt와 일치하고 있는지 아닌지를 판정한다. 그리고, 일치하고 있지 않다고 판정하면(단계S110에서 No)은, 반복해서 상기 단계 S40~단계 S110를 실행한다. 즉, 단계 S40~단계 S110는, 현재자세 Ps가 최종자세 Pt와 일치할 때까지 실행된다.

그리고, 단계 S100에서 산출한 가상자세 Pp가 최종자세 Pt와 일치하면(단계 S110에서 Yes), 처리를 종료한다. 이것에 의해, 도7f에 나타낸 바와 같이, 진입금지영역(S1)과의 간섭을 회피한 최종적인 갱신궤도 Tb가 생성된다.

더욱이, 본 실시형태에서, 로봇(100)은, 상기 처리가 종료한 뒤에, 최종적인 갱신궤도를 따라 로봇(100)을 현재자세 Ps에서 최종자세 Pt를 향하여 실제로 움직이도록 구성되어 있다. 즉, 단계 S90에서 경유자세(Pv)를 새로운 현재자세 Ps로서 재설정했을 때에, 이 현재자세 Ps를 향하여, 제어부(21)가 제1암(2), 제2암(3), 핸드(4)를 움직이지 않고, 결정한 갱신궤도 Tb의 이력을 기억부(22)에 기억하도록 구성되어 있다. 그러나, 이것에 한정되는 것은 아니고, 단계 S90에서 경유자세 Pv를 새로운 현재자세 Ps로서 결정할 때마다, 이 현재자세 Ps를 향하여, 제어부(21)가 제1암(2), 제2암(3), 및 핸드(4)를 실제로 움직여도 좋다.

이상 설명했던 것처럼, 본 발명의 로봇(100)은, 로봇(100)이 현재자세Ps에서 최종자세 Pt를 향하여 변화하는 경우에서 핸드(4)의 초기궤도 Ta상의 소정의 점에 대응하는 로봇(100)의 가상자세 Pp를 산출하고, 가상자세 Pp에 있어서의 로봇(100)의 기하모델 M1~M3가 장애물에 대응해서 규정(획정)되는 진입 금지영역(S1)과 간섭하는지 아닌지를 판정한다. 그리고, 간섭하지 않는다고 판정한 경우에는 가상자세 Pp를, 현재자세 Ps에서 최종자세 Pt를 향하여 변화하는 경우에 경유하는 경유자세 Pv로서 결정하고, 간섭한다고 판정한 경우에는, 가상반발력 fi에 의해 기하모델의 간섭부분이 진입금지영역(S1)에서 동작영역(S2)으로 밀려난 상태의 자세를 산출하고, 이 자세를 경유자세 Pv로서 결정한다. 따라서, 로봇(100)의 기하모델이 진입금지영역(S1)과 간섭하는 경우에는, 진입금지영역(S1)에 간섭하지 않는 경유자세 Pv가 주어지므로, 장애물과의 충돌이 회피된다.

한편, 로봇(100)의 기하모델이 진입금지영역(S1)과 간섭하지 않는 경우에는, 초기궤도 Ta상의 점에 대응하는 경유자세 Pv가 주어지므로, 최종자세 Pt에 접근한다. 그리고, 상기 구성에 의하면, 새롭게 주어진 경유자세 Pv를 현재자세 Ps로서 이 순서를 반복하므로, 로봇(100)의 기하모델이 진입금지영역(S1)과 간섭하지 않는 순서가 출현할 때마다, 이 순서에 있어서의 초기궤도 Ta상의 점에 대응하는 경유자세 Pv가 주어져 최종자세 Pt에 접근한다. 따라서, 시행착오를 반복하는 것에 의해, 최종적으로, 로봇(100)이, 장애물과의 충돌을 회피하면서 현재자세 Ps에서 최종자세 Pt로 변화하는 경우에 있어서의 핸드(4)의 갱신궤도 Tb를 얻을 수 있다.

그 결과, 로봇의 기하모델, 진입금지영역, 및 동작영역의 설정이라는 단순한 설정으로, 로봇이 어느 위치에서 정지해도 장애물과의 간섭을 자동적으로 회피해서, 안전하게 로봇을 소정의 자세에 도달시킬 수 있다.

(실시형태 2)

이하에서는, 실시형태 2의 구성, 동작에 대해서, 실시형태 1과의 상위점(相違点)을 중심으로 설명한다.

상기 실시형태 1에서, 초기궤도 Ta는, 현재자세 Ps 및 최종자세 Pt가 주어지는 것에 의해 일의적(一義的)으로 정해지는 궤도이다. 또한, 단계 S70에서 결정되는 경유자세 Pv는, 가상자세 Pp에 있어서의 각 기하모델과 경계선 B와의 위치관계에서 일의적으로 정해지는 자세이다. 따라서, 실시형태 1에서는, 예를 들면, 반발력에 의해 되돌리는 방향이 초기궤도 Ta의 방향과 일치하는 경우에는, 가상반발력 fi에 의해 밀려난 상태의 자세에 근거해서 결정된 갱신궤도 Tb가 최종자세 Pt에 접근하지 않는 경우가 있다.

본 실시형태는, 갱신궤도 Tb가 최종자세 Pt에 접근하지 않는 경우를 고려한 실시형태다.

도8은, 본 발명의 실시형태 2에 관계되는 로봇(100)의 동작예를 보여주는 플로우 차트이다.

본 실시형태에서, 단계 S70의 경유자세의 결정은 이하와 같이 실행된다.

우선, 정류상태에 빠져있는지 아닌지를 판정한다(정류상태판정단계)(단계S271). 본 실시형태에서, 정류상태에 빠져있는지 아닌지의 판정은, 가상자세 Pp와 최종자세 Pt 와의 각 관절축의 관절각도 변위의 절대치의 총합 E를 이하의 식(5)에 근거해서 산출한다.

[수5]

… (5)

단,

n 은, 축의 수

q _ri 는, i축의 가상자세 Pp에 있어서의 관절각도

q _gi 는, i축의 목표자세 Pt에 있어서의 관절각도

그리고, E의 값이 감소하고 있으면, 최종자세에 접근하고 있다고 판정한다. 한편, E의 값이 변화하지 않거나, 또는 증가하고 있으면, 최종자세 Pt에 접근하고 있지 않거나, 또는 최종자세 Pt에서 멀어지고 있다고 판정한다. 그리고, 소정의 시간간격에서, E의 값은 변화하지 않거나, 또는 증가하고 있는 상태가 지속하고 있으면, 정류상태에 빠져 있다고 판정한다.

그리고, 단계 S271에서, 제어부(21)가 정류상태에 빠져있지 않다고 판정하면, 단계 S71~S74를 실행한다. 단계 S71~S74는, 상기 실시형태 1의 단계 S71~S74와 마찬가지이므로, 그 설명을 생략한다.

한편, 단계 S271에서, 제어부(21)가 정류상태에 빠져있다고 판정하면, 제어부(21)의 경유자세결정부(39)는, 현재자세 Ps의 근방에서 가상반발력 fi에 의해 밀려난 상태의 자세 이외의 경유후보자세 Pvc를 산출한다(경유후보자세생성단계)(단계S272). 본 실시형태에서, 경유후보자세 Pvc는, 랜덤(random)하게 생성된 자세이다. 그러나, 이것에 한정되는 것은 아니다. 이것을 대신해서, 예를 들면, 현재자세 Ps의 주위의 상황에 따라서 생성해도 좋다.

다음에, 경유후보자세 Pvc가 현재자세 Ps보다도 최종자세 Pt에 접근하는자세인지 아닌지를 판정한다(제1판정단계)(단계 S273). 본 실시형태에서, 경유후보자세 Pvc가 최종자세 Pt에 접근하는 자세인지 아닌지의 판단은, 이하의 식(6)에 근거해서 산출한 E의 값과, 이하의 식(7)에 근거해서 산출한 E의 값을 비교하는 것에 의해 행한다.

[수6]

… (6)

… (7)

… (8)

단,

n 은, 축의 수

q _gi 는, i축의 목표자세 Pt에 있어서의 관절각도

q _ri 는, i축의 경유후보자세 Pvc에 있어서의 관절각도

q _i 는, i축의 현재자세 Ps에 있어서의 관절각도

즉, 상기 식(6)은, 경유후보자세 Pvc와 최종자세 Pt와의 각 관절축의 관절각도 변위의 절대치의 총합 E(qr)를 구하는 것이다. 또한, 상기 식(7)은, 현재자세 Ps와 최종자세 Pt와의 각 관절축의 관절각도 변위의 절대치의 총합 E(qr)를 구하는 것이다. 그리고, 상기 식(8)은, E(qr)과 E(q) 와의 값을 비교하는 식이다.

그리고, 예를 들면, 상기 식(8)에서, △≤0 이면, 즉, 경유후보자세 Pvc에 관계되는 E(qr)의 값이 현재자세 Ps의 E(q)의 값과 동일 또는 작으면, 최종자세 Pt에 접근하는 자세라고 판정한다. 한편, △>0이면, 즉, 경유후보자세 Pvc에 관계되는 E(qr)의 값이 현재자세 Ps의 E(q)의 값보다도 크면 최종자세 Pt에 접근하는 자세가 아니라고 판정한다.

그리고, 제어부(21)의 경유자세결정부(39)가 최종자세 Pt에 접근하는 자세라고 판정하면(단계 S273에서 Yes), 경우후보자세 Pvc에 근거해서 경우자세 Pv를 결정한다(제1결정단계)(단계 S274). 다시, 현재자세에서 이 경유자세 Pv에 이르는 궤도가 진입금지영역(S1)과 간섭하는지 아닌지를 판정한다.

한편, 제어부(21)의 경유자세결정부(39)가 최종자세 Pt에 접근하는 자세가 아니라고 판정하면(단계 S273에서 No), 다시 경유후보자세 Pvc를 경유자세 Pv로 선택하는지 아닌지를 결정한다(제2판정단계)(단계 S275).

이 선택은, 확률값을 이용해서 행한다. 본 실시형태에서, 이 선택은, 경유후보자세 Pvc의 평가치에 따른 천이확률 P(

,t)를 이하의 식(9)에 의해 산출하고, 이 확률에 따라서, 경유후보자세 Pvc를 경유자세 Pv로 선택하는지 아닌지를 결정한다. 또한, 이 천이확률의 그래프를 도면의 a로 나타내는 선으로서 예시한다.

[수7]

… (9)

단,

t 는, 파라미터

△는, (8)식에서 산출

따라서, 천이확률 P(△,t)는 △가 0에 접근하는 만큼, 즉, 경유후보자세 Pvc가 현재자세 Ps보다도 최종자세 Pt로부터 괴리하는(멀어지는) 정도가 적을수록, 경유후보자세 Pvc가 경유자세 Pv로서 선택되는 가능성이 높아지고(1에 근접함), 경유후보자세 Pvc가 현재자세 Ps보다도 최종자세 Pt로부터 괴리하는 정도가 클수록, 경유후보자세 Pvc가 경유자세 Pv로서 선택되는 가능성이 낮아진다(0에 접근함).

그리고, 경유후보자세 Pvc가 경유자세 Pv로서 선택되면, 경유후보자세 Pvc에 근거해서 경유자세 Pv를 결정한다(제2결정단계)(단계 S276).

그리고, 제어부(21)는, 상기 실시형태와 마찬가지로 단계 S80 이후의 처리를 실행한다.

더욱이, 경유후보자세 Pvc가 현재자세 Ps보다도 최종자세 Pt에서 괴리하는 정도가 적은 자세에는, 정류상태에서 빠져나올 수 있는 자세가 발견되기 어려운 경우가 있다. 이와 같은 경우는, 도9의 그래프에서 b로 나타낸 선에서 보여주는 바와 같이, 정류상태에 빠진 때에 상기 (9)식의 파라미터 t의 값을 크게 하는 것에 의해, 현재자세 Ps보다도 최종자세 Pt에서 괴리하는 정도가 비교적 큰 경유후보자세 Pvc에서도 선택되기 쉽도록 조정해도 좋다.

또한, 현재자세 Ps가 최종자세 Pt에 접근함에 따라서, 최종자세 Pt에서 괴리하는 자세를 선택할 확률을 낮추는 것이 바람직하다. 따라서, 현재자세 Ps가 최종자세 Pt에 접근함에 따라서, 도9의 그래프에서 b로 나타낸 선에서 보여주는 바와 같이, 파라미터 t의 값을 작게 하는 것에 의해, 최종자세 Pt에서 괴리하는 자세를 선택하는 확률을 낮추어도 좋다.

이와 같이, 본 실시형태에서, 로봇(100)은, 예를 들면 반발력 fi에 의해 밀어내는 방향이 초기궤도 Ta의 방향과 일치하는 경우에는, 동작영역(S2)으로 밀려난 자세가 초기궤도 Ta상의 점에 대응하고 있으므로, 진입금지영역(S1)에 대한 기하모델의 간섭이 반복되어서 정류상태에 빠지고, 로봇(100)이 최종자세 Pt에 접근하지 않는다(최종자세에 도달할 수 없다). 이와 같은 경우에, 초기궤도 Ta상에 없는 점에 대응하는 경유자세 Pv를 얻을 수 있고, 이 자세에 근거해서 갱신궤도 Tb를 결정하고, 최종자세 Pt에 접근하는 다른 경우자세 Pv를 경유해서 최종자세 Pt에 도달할 수 있다.

또한, 다른 최종자세 Pt에 접근하는 자세를 산출할 수 없는 경우라도, 다른 자세를 경유해서 최종자세 Pt에 도달할 수 있는지 아닌지를 시도해 볼 수 있다. 이것에 의해, 현재의 최종자세 Pt에 도달할 수 없는 상태를 매우 적절하게 벗어날 수 있다. 그리고, 최종자세 Pt와 경유후보자세 Pvc와의 괴리도(乖離度)에 비례해서 저하(低下)하는 확률치에 근거해서 경유후보자세를 선택하는지 아닌지를 판정하고 있으므로, 현재자세 Ps와의 괴리도가 작은 경유후보자세 Pvc가 경유자세 Pv로서 선택되는 가능성이 높아진다. 이것에 의해, 보다 적절하게 현재의 최종자세에 도달할 수 없는 상태를 매우 적절하게 벗어날 수 있다.

도10a 밑 도10b는, 본 실시형태에 있어서의 로봇(100)의 시뮬레이션 조건을 보여주는 도면이다. 도 10a에 나타낸 바와 같이, 로봇(100)의 현재자세 Ps, 진입금지영역 S1, 동작영역 S2, 기하모델 M을 설정했다. 또한, 도10b에 나타낸 바와 같이, 로봇(100)의 최종자세 Pt를 설정했다. 또한, 최종자세 Pt에 있어서의 핸드의 교시점(敎示点)은, [x, y]=[0, 450]이다.

도10c는, 로봇(100)의 시뮬레이션 결과를 보여주는 도면이고, 핸드의 궤도를 보여주는 도면이다. 도10c에 나타낸 바와 같이, 로봇(100)은, 현재자세 Ps에서 최종자세 Pt로 도달하는 것을 알 수 있다.

<변형예>

상기 실시형태에서는, 로봇(100)을 수평 다관절 로봇으로 구성하여, 기하모델을 xy평면에서 표현하고, z축방향에 있어서의 간섭의 유무판정은 행하고 있지 않지만 이것에 한정되는 것은 아니다. 이를 대신해서, 로봇을 수직 다관절로 구성하고, 기하모델을 입체적으로 표현하여, 간섭의 판정을 3차원 공간에서 행해도 좋다.

또한, 상기 실시형태에서는, 가상자세 산출부(36)는, 로봇(100)의 가상자세 Pp를 산출하고 있었지만, 이를 대신해서, 로봇(100)의 기하모델의 가상자세 Pp를 산출해도 좋다.

상기 설명에서, 당업자(통상의 지식을 가진 자)에게는, 본 발명에 대한 많은 개량이나 다른 실시형태가 명백한 것이다. 따라서, 상기 설명은, 예시로서만 해석되어야 하며, 본 발명을 실행하는 최선(最善)의 태양(態樣)을 당업자에게 교시(敎示)하는 목적으로 제공된 것이다. 본 발명의 정신을 일탈(逸脫)하는 일 없이, 그 구조 및/또는 기능의 상세한 사항을 실질적으로 변경할 수 있다.

[산업상 이용가능성]

본건 발명은, 산업용 로봇에 적용할 수 있다.

B : 경계선
L1 : 회동국선(回動曲線)
L2 : 회동곡선
L3 : 회동곡선
M1 : 기하모델(幾何MODEL)
M2 : 기하모델
M3 : 기하모델
Pp : 가상자세(假想姿勢)
Ps : 현재자세
Pt : 최종자세
Ptc : 최종후보자세(最終候補姿勢)
Pv : 경유자세(經由姿勢)
Pvc : 경유후보자세
S1 : 진입금지영역
S2 : 동작영역
Ta : 초기궤도
Tb : 갱신궤도(更新軌道)
1 : 기대(基臺)
2 : 제1암(FIRST ARM)
2a : 제1관절축(第一關節軸)
3 : 제2암
3a : 제2관절축
4 : 핸드(HAND)
4a : 제3관절축
5 : 제어기(制御器)
12 : 제1암구동부(第一ARM驅動部)
13 : 제2암구동부
14 : 핸드구동부
15 : 제1암각도위치검출부(第一ARM角度位置檢出部)
16 : 제2암각도위치검출부
17 : 핸드각도위치검출부
21 : 제어부
22 : 기억부(記憶部)
31 : 제1암제어부
32 : 제2암제어부
33 : 핸드제어부
34 : 최종자세결정부
35 : 초기궤도결정부
36 : 가상자세산출부
37 : 기하모델표현부
38 : 간섭판정부(干涉判定部)
39 : 경유자세결정부
40 : 영역설정부
41 : 갱신궤도결정부
100 : 로봇(ROBOT)
110 : 설비(設備)

Claims

1 이상의 링크가 관절에 의해 연결되고, 선단부에 핸드가 설치된 로봇암을 구비하는 암형 로봇의 장애물 자동회피방법으로서,
상기 로봇을, 기하형상을 갖도록 모델링해서 기하모델로서 표현하는 기하모델 표현단계와,
상기 기하모델이 진입해서는 안되는 진입 금지영역과, 상기 진입 금지영역에 의해 규정되고, 상기 기하모델이 동작하는 동작영역을 설정하는 영역 설정단계와,
상기 로봇의 최종자세를 결정하는 최종자세 결정단계와,
상기 로봇이 현재자세로부터 상기 최종자세를 향하여 변화하는 경우에서의 상기 핸드의 초기궤도를 결정하는 초기궤도 결정단계와,
상기 초기궤도상의 소정의 점에 대응하는 상기 로봇의 가상자세를 산출하는 가상자세 산출단계와,
상기 가상자세에서의 상기 기하모델이 상기 진입 금지영역과 간섭하는지 아닌지를 판정하는 간섭 판정단계와,
상기 간섭 측정단계에 있어서 간섭하지 아니한다고 판정한 경우, 상기 가상자세를 경유자세로서 결정하고, 간섭한다고 판정한 경우, 상기 진입 금지영역의 간섭부분과 상기 기하모델의 간섭부분이 상대적으로 반발하는 반발력을 가상적으로 발생시키고, 가상 반발력에 의해 상기 기하모델의 간섭부분이 상기 진입 금지영역으로부터 상기 동작영역으로 밀려난 상태의 자세를 산출하고, 상기 산출한 자세를 경유자세로서 결정하는 경유자세 결정단계와,
상기 현재자세로부터 상기 경유자세를 경유해서 상기 최종자세로 변화하는 경우에서의 상기 핸드의 궤도를 갱신궤도로서 결정하는 갱신궤도 결정단계와,
최신의 경유자세 결정단계에서 결정된 경유자세를 상기 초기궤도 결정단계에서의 상기 현재자세라고 가정해서 상기 초기궤도 결정단계, 상기 가상자세 산출단계, 상기 간섭 판정단계, 상기 경유자세 결정단계, 및 상기 갱신궤도 결정단계를 반복해서 실행하는 단계를 포함하는 암형 로봇의 장애물 자동회피방법.

제1항에 있어서,
상기 최종자세 결정단계는, 기지(旣知)의 궤도상에 설정되는 상기 로봇의 복수의 최종후보자세 중에서, 상기 로봇의 현재자세와 상기 최종후보자세와의 상기 각 관절의 각도변위의 절대치의 총합이 최소값인 최종후보자세를 상기 최종자세로서 선택하여 결정하는 암형 로봇의 장애물 자동회피방법.

제1항 또는 제2항에 있어서,
상기 가상 반발력에 의해 소정의 가상자세에서의 상기 로봇의 상기 각 관절의 회전축 주위에 생기는 토크를 산출하고, 상기 토크의 영향에 의해 변화하는 상기 로봇의 소정시간 경과 후의 가상자세를 산출하는 연산을 상기 가상자세를 기점으로 해서 반복하는 것에 의해 상기 로봇의 가상자세의 경시변화를 산출하고, 상기 로봇의 가상자세의 경시변화가 수렴했을 때의 상기 로봇의 가상자세에 근거해서 상기 경유자세를 결정하는 암형 로봇의 장애물 자동회피방법.

제1항 내지 제3항 중의 어느 한 항에 있어서,
상기 가상 반발력은, 상기 기하모델이 진입 금지영역에 진입한 거리에 비례해서 커지도록 구성되어 있는 암형 로봇의 장애물 자동회피방법.

제1항 내지 제4항 중의 어느 한 항에 있어서,
상기 로봇의 자세가 상기 최종자세에 접근하도록 경시변화 하지 않고, 정류상태에 빠져있는지 여부를 판정하는 정류상태 판정단계와,
상기 정류상태 판정단계에서 정류상태에 빠져있다고 판정한 경우, 상기 로봇의 상기 가상 반발력에 의해 밀려난 상태의 자세 이외의 경유후보자세를 생성하는 경유후보자세 생성단계와,
상기 경유후보자세가 상기 최종자세에 가까워지는 자세인지 여부를 판정하는 제1판정단계와,
상기 제1판정단계에서, 상기 경유후보자세가 상기 최종자세에 가까워지는 방향으로 변화하는 자세라고 판정한 경우, 상기 경유후보자세에 근거해서 상기 경유자세를 결정하는 제1결정단계를 갖는, 암형 로봇의 장애물 자동회피방법.

제5항에 있어서,
상기 제1판정단계에서, 상기 경유후보자세가 상기 최종자세로부터 멀어지는 방향으로 변화하는 자세라고 판정한 경우, 확률치에 근거해서 상기 경유후보자세를 선택할지 여부를 판정하는 제2판정단계와,
상기 제2판정단계에서 선택한다고 판정되면, 상기 경유후보자세에 근거해서 상기 경유자세를 결정하는 제2결정단계를 갖는, 암형 로봇의 장애물 자동회피방법.

1 이상의 링크가 관절에 의해 연결되고, 선단부에 핸드가 설치된 로봇암을 구비하는 암형로봇의 제어장치로서,
상기 로봇을, 기하형상을 갖도록 모델링해서 기하모델로서 표현하는 기하모델 표현부와,
상기 기하모델이 진입해서는 안 되는 진입 금지영역과, 상기 진입 금지영역에 의해 규정되고, 상기 기하모델이 동작하는 동작영역을 설정하는 영역설정부와,
상기 로봇의 최종자세를 결정하는 최종자세결정부와,
상기 로봇이 현재자세로부터 상기 최종자세를 향하여 변화하는 경우에서의 상기 핸드의 초기궤도를 결정하는 초기궤도결정부와,
상기 초기궤도상의 소정의 점에 대응하는 상기 로봇의 가상자세를 산출하는 가상자세산출부와,
상기 가상자세에서의 상기 기하모델이 상기 진입 금지영역과 간섭하는지 여부를 판정하는 간섭판정부와,
경유자세를 결정하는 경유자세결정부와,
갱신궤도결정부를 구비하고,
상기 경유자세결정부는, 상기 간섭판정부가 간섭하지 않는다고 판정한 경우, 상기 가상자세를 경유자세로서 결정하고, 간섭한다고 판정한 경우, 상기 진입 금지영역의 간섭부분과 상기 기하모델의 간섭부분이 상대적으로 반발하는 반발력을 가상적으로 발생시켜서, 가상반발력에 의해 상기 기하모델의 간섭부분이 상기 진입 금지영역으로부터 상기 동작영역으로 밀려난 상태의 자세를 산출하고, 상기 산출한 자세를 경유자세로서 결정하고,
상기 갱신궤도 결정부는, 상기 현재자세로부터 상기 경유자세를 경유해서 상기 최종자세로 변화하는 경우에서의 상기 핸드의 궤도를 갱신궤도로서 결정하고,
상기 경유자세 결정부가 결정한 최신의 경유자세를 상기 초기궤도결정부의 현재자세라고 가정해서, 상기 초기궤도결정부, 상기 가상자세산출부, 상기 간섭판정부, 상기 경유자세결정부, 및 상기 갱신궤도결정부가 반복하여 처리하는, 암형로봇의 제어장치.