KR101688344B1 - 다관절 로봇의 위빙 제어 장치 - Google Patents

Abstract

관절축을 구동하는 모터 자신의 동특성 및 다른 관절축의 동작에 의한 영향에 기인하는 위빙 동작의 오차의 발생을 억제하여, 고정밀도의 위빙 동작을 달성 가능한, 다관절 로봇의 위빙 제어 장치를 제공한다. 위빙 제어 장치(10)는 각 축의 목표 위치 신호를 연산하는 신호 연산부와, 목표 위치 신호에 대해 로우 패스 필터 처리된 목표 지령 신호를 연산하는 필터 연산부(400)와, 목표 지령 신호를 입력으로 하여 각 축을 구동하는 모터 제어부(2500)를 포함한다. 모터 제어부(2500)의 게인의 주파수 특성은 대략 플랫하게 구성되고, 목표 위치로부터 모터 출력 각도까지의 동특성은 필터에 근사된다. 위빙 신호 보정부(200)는 필터 특성에 기초하여 게인 보정된 목표 위치 신호를 연산한다.

Description

다관절 로봇의 위빙 제어 장치 {WEAVING CONTROL DEVICE OF MULTI-JOINT ROBOT}

본 발명은 예를 들어 아크 용접에 사용되는 다관절 로봇의 제어에 관한 것으로, 특히 높은 궤적 정밀도로 위빙 동작을 가능하게 하는 다관절 로봇의 제어에 관한 것이다.

아크 용접에 의해 복수의 모재의 용접을 행할 때에는, 용접 전극을 용접 방향으로 진행시키면서, 용접선의 좌우 방향으로 정현파 위빙 동작을 시키면서 용접하는 위빙 용접이 채용된다. 이 위빙 용접은, 종래부터, 용접 토치 자체를 좌우로 요동시키거나, 또는 용접 토치 자체를 중심으로 하여 좌우로 틸팅시킴으로써 행해진다. 이와 같은 위빙 용접을 다관절 로봇에 행하도록 하는 경우, 높은 궤적 정밀도가 요구된다.

위빙 동작에 대해, 이하와 같은 기술이 공지이다.

일본 특허 공개 2006-59037호 공보(특허문헌 1)는 미리 교시된 진폭으로 확실하고 또한 정확하게 위빙 동작시킬 수 있는 용접 로봇 제어 장치를 개시한다.

용접 로봇에 위빙 동작을 행하게 할 때, 지령된 위빙 주파수나 진폭에 기초하여, 각 관절의 모터의 각도 지령값이 생성되고, 그 지령값에 기초하여 위빙 동작이 제어된다. 단, 모터의 동특성(피드백계의 제어 시상수)에 의존하여, 모터는, 각도 지령값대로 동작할 수 없고, 결과적으로, 지령된 진폭으로 위빙할 수 없다. 따라서, 특허문헌 1에 개시된 기술은, 이하와 같이, 이 문제를 해결하고 있다. 먼저, 위빙 동작하지 않는 경우와 위빙 동작하는 경우의 각 관절의 각도 지령값의 차가, 위빙 신호로서 산출된다. 그리고, 모터의 피드백계의 제어 시상수와 위빙 주파수에 기초하여, 모터의 피드백계의 게인(출력 진폭/지령 진폭)이 산출되고, 이 게인의 역수를 위빙 신호에 곱함으로써 얻어진 값을 위빙 동작하지 않는 경우의 각 관절의 각도 지령값에 가산함으로써, 최종적인 각 관절의 각도 지령값이 산출된다.

또한, 일본 특허 공개 평 6-222817호 공보(특허문헌 2)는 위빙 용접이 행해지는 로봇에 적용되어 위빙 파형을 고정밀도로 그리도록 할 수 있는 기술을 개시한다.

특허문헌 2에 개시된 기술은, 모터의 동특성에 의존하여 위빙 진폭이 변동되는 문제점을 해결하기 위해, 로봇의 자세에 기초하여 결정되는 모터 구동계의 모델[전달 함수 G(s)]을 구하고, 위빙 동작 시의 각 축의 각도 지령값에 1/G(s)이 승산된 각도 지령값을 모터에 대해 입력한다.

일본 특허 공개 2006-59037호 공보 일본 특허 공개 평 6-222817호 공보

다관절 로봇의 각 축을 동작시키는 모터에, 위빙 동작과 같은 주기 신호가 지령값으로서 입력된 경우, 출력 각도는 지령값에 대해 진폭·위상이 변화된다고 하는 특성을 갖고 있다. 이로 인해, 로봇 선단 위치를 정확하게 움직이게 하기 위해서는, 이 변화 특성을 예측하여 각도 지령값을 보정할 필요가 있다. 또한, 이 진폭·위상의 변화에 관여하는 요인은, 모터 자신의 동특성(피드백계의 제어 시상수)과, 외부로부터의 외력(로봇의 경우에서는, 타축이 움직이는 것에 의한 영향)이 있다. 문제점은, 정리하면 이하와 같이 정리된다.

(1) 모터의 피드백계의 제어 시상수에 의해, 지령값에 대해 모터 각도의 진폭이 변화된다.

(2) 각 모터의 피드백계의 제어 시상수가 상이하기 때문에, 지령값에 대해 각 모터 각도의 위상이 상이하다.

(3) 타축의 동작 영향에 의해, 지령값에 대해 모터 각도의 진폭이 변화된다.

(4) 타축의 동작 영향에 의해, 지령값에 대해 각 모터 각도의 위상이 상이하다.

그러나, 상술한 특허문헌 1은 이들 문제점 중 (1)만을 해결한 기술이며, (2)∼(4)에 대해서는 전혀 고려되어 있지 않다.

또한, 상술한 이와 같은 문제점을 해결하기 위한 방법으로서, 각도 지령값을 보정하는 것(특허문헌 1도 이 방법을 채용)을 들 수 있다. 그러나, 보정을 행하기 위해서는, 각도 지령값에 대한 출력 각도의 변화 특성을 모델화하고, 어느 정도 변화될 것인지를 정확하게 예측할 필요가 있다. 보정하는 양이 커지거나, 모델이 복잡해지면, 당연히 오차도 증가한다. 이 오차를 저감시키기 위해서는, 애당초, 상술한 변화 특성에 의한 진폭·위상의 변화량을 최대한 작게 하기 위한 제어를 행할 필요도 있다.

또한, 특허문헌 2는 상술한 4개의 문제점 중 (1) 및 (2)를 해결한 기술이며, (3) 및 (4)에 대해서는 전혀 고려되어 있지 않다.

또한, 특허문헌 1에 개시된 기술과 동일한 바와 같이 특허문헌 2에 개시된 기술도, 모터의 특성을 모델화하고, 진폭·위상의 변화량을 예측하고, 각도 지령값을 보정함으로써 문제를 해결한다. 그로 인해, 근본적으로 보정량은 작아지지 않고, 보정하는 양이 커지거나, 모델이 복잡해지면 오차가 커진다.

즉, 이들 특허문헌 1, 2에 개시된 기술에서는, 상술한 4개의 문제점의 반드시 전부가 해결되는 것은 아니며, 높은 궤적 정밀도로의 위빙 동작이 실현되어 있지 않다.

본 발명은 상술한 문제점을 감안하여 이루어진 것이며, 복수 축을 구비한 다관절 로봇에 있어서, 다관절 로봇의 축을 동작시키는 모터 자신의 동특성[상술한 문제점 (1) 및 (2)]에 기인하는 위빙 동작의 오차 및 타축이 동작하는 것에 의한 영향[상술한 문제점 (3), (4)]에 기인하는 위빙 동작의 오차의 발생을 억제하여, 높은 궤적 정밀도로 위빙 동작을 가능하게 할 수 있는, 다관절 로봇의 위빙 제어 장치를 제공하는 것을 목적으로 한다.

상기 과제를 해결하기 위해, 본 발명에 관한 다관절 로봇의 위빙 제어 장치는, 이하의 기술적 수단을 강구하고 있다.

본 발명에 관한 다관절 로봇의 위빙 제어 장치는, 다관절 로봇에 설치된 툴이 원하는 위빙 궤적을 그리도록 복수의 관절축을 구동시킨다. 이 위빙 제어 장치는, 상기 위빙 궤적에 기초하여, 다관절 로봇의 각 축의 목표 위치 신호를 연산하는 신호 연산부와, 상기 연산된 목표 위치 신호에 대해, 다관절 로봇의 고유 진동 성분을 제거하기 위한 로우 패스 필터 처리를 행하고, 다관절 로봇의 각 축에 대해 필터 처리된 목표 지령 신호를 연산하는 필터 연산부와, 상기 필터 처리된 목표 지령 신호를 입력으로 하여, 다관절 로봇의 각 축을 구동하는 모터 제어부를 포함하여 구성된다. 상기 모터 제어부에 있어서의 게인의 주파수 특성이, 대략 플랫하게 구성되고, 상기 신호 연산부는, 게인 보정된 목표 위치 신호를 연산하는 것을 특징으로 한다.

바람직하게는, 상기 모터 제어부는, 위치 피드백부와 속도 피드백부와 속도 피드 포워드부를 포함하도록 구성할 수 있고, 상기 신호 연산부는, 상기 필터 연산부에 있어서의 필터 특성 및 상기 위빙 궤적에 기초하여, 게인 보정된 목표 위치 신호를 연산하도록 구성할 수 있다.

더욱 바람직하게는, 상기 로우 패스 필터 처리에서 사용하는 필터는 전체 축에서 마찬가지로 되도록 구성할 수 있고, 상기 신호 연산부는, 상기 필터 연산부의 필터 특성으로부터 얻어지는 위빙 주파수에 있어서의 필터 게인의 역수를 목표 위치 신호에 승산함으로써, 게인 보정된 목표 위치 신호를 연산하도록 구성할 수 있다.

더욱 바람직하게는, 상기 신호 연산부는, 상기 필터 연산부의 필터 특성 및 상기 필터 연산부의 필터 특성으로부터 얻어지는 위빙 주파수에 기초하여, 게인 보정 외에, 위상 보상된 목표 위치 신호를 연산하도록 구성할 수 있다.

더욱 바람직하게는, 상기 모터 제어부는, 자축이 가감속함으로써 자축에 작용하는 관성력, 중력 및 마찰력을 예측하고, 상기 속도 피드백으로부터 출력되는 토크 지령값에 추가하는 피드 포워드 제어부를 더 포함하도록 구성할 수 있다.

더욱 바람직하게는, 상기 모터 제어부는, 타축이 가감속함으로써 자축에 작용하는 관성력 및 원심 코리올리력을 예측하고, 상기 속도 피드백으로부터 출력되는 토크 지령값에 추가하는 피드 포워드 제어부를 더 포함하도록 구성할 수 있다.

본 발명에 관한 위빙 제어 장치를 사용함으로써, 복수 축을 구비한 다관절 로봇에 있어서, 다관절 로봇의 축을 동작시키는 모터 자신의 동특성에 기인하는 위빙 동작의 오차 및 타축이 동작하는 것에 의한 영향에 기인하는 위빙 동작의 오차가 발생하는 것을 억제하여, 높은 궤적 정밀도로의 위빙 동작이 가능하게 된다.

도 1은 본 발명의 실시 형태에 관한 위빙 제어 장치가 적용되는 다관절 로봇의 전체 구성을 도시하는 개략도이다.
도 2는 본 발명의 제1 실시 형태에 관한 위빙 제어 장치의 제어 블록도이다.
도 3은 도 2의 필터 연산부 및 모터 제어부의 상세한 제어 블록도이다.
도 4는 도 3에 도시한 제어 블록도의 간략판이다.
도 5는 도 4에 도시하는 제어 블록도에 의해 제어된 다관절 로봇의 출력 파형을 나타내는 도면이다.
도 6은 도 2에 도시한 제어 블록도의 간략판이다.
도 7은 도 6에 도시하는 제어 블록도에 의해 제어된 다관절 로봇의 출력 파형을 나타내는 도면(제1)이다.
도 8은 도 6에 도시하는 제어 블록도에 의해 제어된 다관절 로봇의 출력 파형을 나타내는 도면(제2)이다.
도 9는 도 6에 도시하는 제어 블록도에 의해 제어된 다관절 로봇의 출력 파형을 나타내는 도면(제3)이다.
도 10은 본 발명의 제2 실시 형태에 관한 위빙 제어 장치의 제어 블록도이다.
도 11은 2링크 로봇의 모식도이다.
도 12는 도 10에 도시하는 제어 블록도(단, 속도 FF만)에 의해 제어된 다관절 로봇의 출력 파형을 나타내는 도면이다.
도 13은 도 10에 도시하는 제어 블록도(속도 FF＋3종 FF)에 의해 제어된 다관절 로봇의 출력 파형을 나타내는 도면이다.
도 14는 도 10에 도시한 제어 블록도의 간략판이다.
도 15는 도 14에 도시하는 제어 블록도에 의해 제어된 다관절 로봇의 출력 파형을 나타내는 도면이다.
도 16은 본 발명의 제3 실시 형태에 관한 위빙 제어 장치의 제어 블록도이다.
도 17은 도 16에 도시하는 제어 블록도(속도 FF＋3종 FF)에 의해 제어된 다관절 로봇의 출력 파형을 나타내는 도면이다.
도 18은 도 16에 도시하는 제어 블록도(속도 FF＋5종 FF)에 의해 제어된 다관절 로봇의 출력 파형을 나타내는 도면이다.
도 19는 도 16에 도시한 제어 블록도의 간략판이다.
도 20은 도 19에 도시하는 제어 블록도에 의해 제어된 다관절 로봇의 출력 파형을 나타내는 도면이다.
도 21은 본 발명의 실시 형태의 변형예에 있어서의, 기본 위치 신호와 위빙 신호의 생성 방법을 도시하는 도면이다.
도 22는 본 발명의 실시 형태의 변형예에 관한 위빙 제어 장치의 제어 블록도이다.

이하, 본 발명의 실시 형태에 관한 다관절 로봇의 위빙 제어 장치를, 도면에 기초하여 상세하게 설명한다. 또한, 이하의 설명에서는, 동일한 부품에는 동일한 부호를 부여하고 있다. 그들의 명칭 및 기능도 동일하다. 따라서, 그들에 관한 상세한 설명은 반복하지 않는다.

<제1 실시 형태>

[전체 구성]

먼저, 제1 실시 형태에 관한 위빙 제어 장치가 적용되는 수직 다관절 로봇(이하, 간단히 다관절 로봇이라고 기재하는 경우가 있음)의 개요에 대해 설명한다.

도 1은 용접 토치를 틸팅 동작(위빙 동작)시키는 로봇의 일례이며, 본 실시 형태에 관한 위빙 제어 장치가 적용되는 다관절 로봇(1)의 개요를 도시하는 도면이다. 이 다관절 로봇(1)은 수직 다관절형이며, J1∼J6의 6관절을 구비한다. J6축의 선단에 용접 토치가 설치되고, 용접 토치로부터 송출되는 용접 와이어에 의해 아크 용접이 행해진다. 이 다관절 로봇(1)은 미리 정해진 용접 개시점과 용접 종료점 사이를 용접 작업 구간으로 하여, 용접 개시점과 용접 종료점을 연결하는 용접선 방향으로 이동하면서, 용접 와이어를 미리 정해진 진폭 및 주파수로 틸팅시키는 동작(위빙 동작)을 행하도록 세트되어 있다.

이와 같은 다관절 로봇(1)은 도시한 다관절 로봇(1)의 본체 외에, 교시 펜던트를 구비함과 함께 각 축을 서보 제어하는 제어 장치(서보 제어부)와, 상위 컴퓨터(상위 CPU)를 포함한다. 이들 제어 장치 및 상위 컴퓨터에 의해, 제1 형태에 관한 위빙 제어 장치(10)가 실현되어 있다.

제어 장치(서보 제어부)는 다관절 로봇(1)에 설치된 용접 토치를, 미리 교시한 프로그램에 따라서, 상술한 용접선을 따라 이동시켜 위빙 동작하도록 제어한다. 교시 프로그램은, 제어 장치에 접속된 교시 펜던트를 사용하여 작성되는 경우나, 상위 컴퓨터를 이용한 오프라인 교시 시스템을 사용하여 작성되는 경우가 있다. 어느 쪽의 경우에도, 교시 프로그램은, 실제의 동작 전에 미리 작성된다.

[제어 블럭]

도 2는 도 1의 다관절 로봇(1)을 제어하는 위빙 제어 장치(10)의 제어 블록도를 도시한다. 이 위빙 제어 장치(10)는 다관절 로봇(1)에 설치된 툴(여기서는 용접 토치)에 원하는 동작(여기서는 위빙 동작)을 행하도록 복수의 관절축을 구동시킨다. 도 2는 복수 어느 관절축(도 1의 예시에서는 6축) 중 1개의 관절축에 대응하고 있다.

이 위빙 제어 장치(10)는 위빙 궤적에 기초하여, 다관절 로봇(1)의 각 축의 목표 위치 신호를 연산하는 신호 연산부[위빙 신호 생성부(100), 위빙 신호 보정부(200), 기본 위치 신호 생성부(300)]를 포함한다. 또한, 위빙 제어 장치(10)는 연산된 목표 위치 신호에 대해, 다관절 로봇(1)의 고유 진동 성분을 제거하기 위한 로우 패스 필터 처리를 행하고, 다관절 로봇(1)의 각 축에 대해 필터 처리된 목표 지령 신호를 연산하는 필터 연산부(400)를 포함한다. 또한, 위빙 제어 장치(10)는 필터 처리된 목표 지령 신호를 입력으로 하여, 다관절 로봇(1)의 각 축을 구동하는 모터 제어부(500)를 포함한다. 여기서, 모터 제어부(500)는 위치 피드백부(510)와, 속도 피드백부와, 속도 피드 포워드부(520)를 포함한다. 모터 제어부(500)에 있어서의 게인의 주파수 특성은 대략 플랫하게 구성되고, 신호 연산부의 위빙 신호 보정부(200)는 게인 보정된 목표 위치 신호를 연산하는 것이 특징이다. 여기서, 「대략 플랫」은, 모터 제어부(500)에 있어서의 게인의 주파수 특성이, 양호한 위빙 동작에 영향을 미치지 않는 정도로, 플랫하게 구성되어 있는 것을 의미한다. 바꿔 말하면, 모터 제어부(500)의 특성을 보드 선도로 나타낸 경우, 보드 선도 상의 게인 곡선이 평탄하게 되어 있거나, 또는 양호한 위빙 동작에 영향을 미치지 않는 정도로 평탄하게 되어 있는 것을 의미한다.

또한, 이하에 있어서, 제어 블럭에 있어서의 요소의 특성은, (동특성이라는 기재가 없어도)모두 동특성이다. 또한, FB의 기재는 피드백을 의미하고, FF의 기재는 피드 포워드를 의미한다. 이하에, 더욱 상세하게 각 블럭에 대해 설명한다.

기본 위치 신호 생성부(300)는 위빙 동작하지 않는 경우의 다관절 로봇(1)의 선단 궤적을 그리기 위한 다관절 로봇(1)의 각 축의 각도 위치로서, 기본 위치 신호를 생성한다.

위빙 신호 생성부(100)는 위빙 동작하는 경우의 다관절 로봇(1)의 선단 궤적을 그리기 위한 다관절 로봇(1)의 각 축의 각도 위치로부터, 기본 위치 신호 생성부(300)에서 생성된 각도 위치를 뺀 각도 위치로서, 위빙 신호를 생성한다. 이 각도 위치는, 기본적으로, 지정되는 위빙 주파수의 주기 신호로 된다.

위빙 신호 보정부(200)는 위빙 신호 생성부(100)에서 생성된 위빙 신호를 보정하고, 위빙 보정 신호를 생성한다. 제1 실시 형태에 있어서는, 위빙 신호 보정부(200)에 있어서의 보정이, 필터 연산부(400)에서 사용하는 필터에 기초하여 행해지는 것이 특징이다. 이 보정 방법의 상세에 대해서는, 후술한다.

필터 연산부(400)는 기본 위치 신호와 위빙 보정 신호를 더한 목표 위치 신호에 대해, 다관절 로봇(1)의 고유 진동 성분을 제거하기 위한 로우 패스 필터 처리를 행하고, 필터 후 목표 위치 신호를 생성한다. 여기서 생성되는 필터 후 목표 위치 신호는, 모터 제어부(500)에서의 제어 방법에 따라, 각도 위치 외에, 각속도, 각가속도를 포함한다. 필터 처리 방법의 상세에 대해서는, 후술한다.

모터 제어부(500)는 필터 후 목표 위치 신호에 기초하여, 모터(550)를 제어한다. 가장 일반적인 모터 제어 방법으로서, 도 2의 상세도인 도 3에 도시한 바와 같이, 인코더(560)의 검출 각도에 기초한 위치(피드백) 제어부(510), 속도(피드백) 제어부(530), 전류 제어부(540)에 의한 제어가 행해진다. 제1 실시 형태에 있어서는, 이들 외에, 필터 후 목표 속도 신호에 속도 피드 포워드 게인을 승산함으로써 속도 피드 포워드 제어를 행하는 속도 피드 포워드부(520)가 추가되어 있는 것이 특징이다. 모터 제어 방법의 상세에 대해서는, 후술한다.

제1 실시 형태에 관한 위빙 제어 장치(10)는 모터 제어부(500)에서의 제어 방법에 속도 피드 포워드 제어를 추가함으로써, 모터(550)의 동특성에 의한 지연을 저감시킨다. 또한, 제1 실시 형태에 관한 위빙 제어 장치(10)는 목표 위치 신호로부터 인코더에서 검출되는 현재 위치 신호까지의 동특성을 필터 연산부(400)에서 사용되는 필터로 근사시킬 수 있는 것을 특징으로 한다.

그리고, 목표 위치 신호로부터 현재 위치 신호까지의 동특성을 임의 설계한 필터에 근사시킬 수 있는 것을 이용하여, 이 필터 특성을 고려하여 위빙 신호를 보정하고, 위빙 보정 신호를 생성하는 것을 특징으로 한다.

이에 의해, 다관절 로봇(1)의 각 축에서의 모터(550)의 동특성에 의한 지연에 의해 흐트러지는 위빙 파형을, 설정한 필터에 기초하여 보정하는 것만으로, 양호한 위빙 파형을 얻을 수 있다.

[필터 처리 방법 및 모터 제어 방법]

여기서는, 도 3 또는 도 4에 도시하는, 제1 실시 형태에 관한 위빙 제어 장치(10)의 필터 연산부(400) 및 모터 제어부(500)에 대해 설명한다.

여기서, T는 필터 시상수, Kxp는 위치 제어부(510)의 비례 게인, Kvp와 Kvi는 속도 제어부(530)의 비례 게인과 적분 게인, Kvf는 속도 피드 포워드부(520)의 피드 포워드 게인, s는 라플라스 연산자를 나타낸다.

또한, T, Kxp, Kvp, Kvi는, 다관절 로봇(1)의 동특성에 따라 설정되고, Kvf는 0∼1의 값으로 설정된다. 또한, 여기서는, 필터 연산부(400)가 1차의 로우 패스 필터를 사용하여, 필터 후 목표 위치 신호로서, 각도 위치, 각속도를 계산한다. 그리고, 각도 위치, 각속도에 기초하여, 도 3에 도시하는 모터 제어가 행해진다. 또한, 필터 연산부(400)의 로우 패스 필터는, 필요에 따라 2차나 3차의 필터를 사용해도 된다.

도 2에서 도시한 제어 방법에 의해 모터(550)의 동특성에 의한 지연을 저감시키고, 도 3, 4에 도시한 바와 같이, 목표 위치 신호 θ로부터 현재 위치 신호 θr까지의 특성을, 필터 연산부(400)에서 사용되는 1차의 로우 패스 필터[F(s)＝1/(T·s＋1)]로 근사시킬 수 있다. 이것을, 도 4를 참조하여 설명한다.

도 5는 목표 위치 신호 θ를 어느 정현파 신호로 한 경우의 각 출력 파형을 나타내고 있다. 도 5 중, 실선이 목표 위치 신호, 파선(broken line)이 필터 후 목표 위치 신호를 나타낸다. 또한, 일점쇄선이 속도 피드 포워드 제어를 사용하였을 때의 모터(550)의 현재 위치 신호, 점선(dotted line)이 속도 피드 포워드 제어를 사용하고 있지 않을 때의 모터(550)의 현재 위치 신호를 나타낸다. 이것을 보면, 속도 피드 포워드 제어 사용 시의 현재 위치 신호는 필터 후 목표 위치 신호에 가깝고, 목표 위치 신호 θ로부터 현재 위치 신호 θr까지의 특성이 필터 특성에 근사되어 있는 것을 알 수 있다. 또한, 한편, 속도 피드 포워드 제어를 사용하고 있지 않을 때의 현재 위치 신호는, 크게 진폭이 감소하고 있고, 보정을 행할 때에는 큰 보정을 행할 필요가 있는 것을 알 수 있다.

[위빙 신호 보정 방법]

상술한 필터 처리 방법 및 모터 제어 방법을 사용함으로써, 목표 위치 신호 θ로부터 현재 위치 신호 θr까지의 동특성을, 임의로 설계한 필터의 전달 함수로 근사시킬 수 있다. 위빙 신호 보정부(200)는 이것을 이용하여, 위빙 신호를 보정한다. 이하, 2가지의 보정 방법(게인 보정 방법, 게인 위상 보정 방법)을 설명한다.

·게인 보정 방법

이 게인 보정 방법에서는, 위빙 신호 생성부(100)에서 생성된 위빙 신호(위빙 주파수는 ω[㎐])에 필터 연산부(400)에서 사용되는 필터 F(s)의 주파수 ω에서의 게인(필터 게인)의 역수를 승산함으로써, 위빙 보정 신호가 생성된다. 이때의 위빙 보정 신호의 산출식은, 이하의 수학식 1로 나타내어진다.

이에 의해, 모터(550)에 대해 주기 신호를 입력하였을 때의 진폭 변화를 보정할 수 있다. 또한, 여기서의 보정은, 모터(550)의 동특성에 의존하지 않고, 임의 설계한 필터에만 기초하고 있다.

단, 여기서는 위상 변화에 대해서는 보정이 행해지지 않기 때문에, 각 축의 필터 연산부(400)에서 상이한 필터를 설정하면, 각 축의 위상 변화량이 상이하여, 양호한 위빙 파형이 얻어지지 않는다. 그로 인해, 이 게인 보정 방법을 사용할 때는, 필터 연산부(400)에서 사용하는 필터를 다관절 로봇(1)의 전체 축에서 동일한 것으로 하고, 전체 축의 위상 변화량을 동등하게 하는 것이 바람직하다.

도 7은 이와 같은 게인 보정 방법(보정 방법 1)을 사용하여, 제어 블럭(도 6)을 사용한 경우의 출력 파형을 나타낸다.

도 7은 보정 방법 1에 의한 출력 파형과, 비교를 위해 종래 기술 1(특허문헌 1에 의한 기술, 이하에서는 종래 기술 1이라고 기재)에 의한 보정을 실시하였을 때의 출력 파형을 나타내고 있다. 실선이 위빙 신호를 나타내고, 일점쇄선이 보정 방법 1에 의한 모터(550)의 현재 위치 신호를 나타내고, 점선이 종래 기술 1에 의한 모터(550)의 현재 위치 신호를 나타낸다. 또한, 여기서는 단일 축만이 구동되는 것으로 하고 있기 때문에, 타축이 동작하는 것에 의한 영향은 나와 있지 않다.

이것을 보면, 어느 쪽의 모터(550)의 현재 위치 신호의 진폭도 위빙 신호의 진폭에 가깝고, 적절하게 보정되어 있는 것을 알 수 있다. 단, 종래 기술 1에 의한 보정은, 모터 모델을 완전히 알고 있다고 가정하여 행해지고 있다.

도 8은 이와 같은 종래 기술 1에 있어서, 모터 모델이 어긋났을 때의 결과를 나타낸다. 이 도 8을 보면, 종래 기술 1에 의한 모터(550)의 현재 위치 신호의 진폭은 크게 어긋나 있는 것을 알 수 있다. 즉, 종래 기술 1에 의한 보정에서 모터 모델이 어긋나면, 바람직한 제어를 실현할 수 없는 것을 알 수 있다.

·게인 위상 보정 방법

이 게인 위상 보정 방법에서는, 위빙 신호 생성부(100)에서 생성된 위빙 신호(위빙 주파수는 ω[㎐])에, 필터 연산부(400)에서 사용되는 필터 F(s) 및 위빙 주파수에 기초하여 위빙 신호의 진폭 변화 및 위상 지연을 추측한 보상을 행한다. 예를 들어, 위상 지연 φ는 이하의 수학식 2로, 진폭 변화 A는 이하의 수학식 3으로 추측된다. 이들 값에 기초하여, 위상 전진 보상 및 게인 보정이 행해진다.

이에 의해, 모터(550)에 대해 주기 신호를 입력하였을 때의 진폭 변화 및 위상 변화를 보정할 수 있다.

도 9는 위빙 신호(실선)와, 이와 같은 게인 위상 보정 방법(보정 방법 2)을 사용하여 제어 블럭(도 6)을 사용한 경우의 출력 파형(일점쇄선)을 나타낸다. 보정 방법 2는 보정 방법 1과는 달리, 위상의 지연도 보상한다. 이로 인해, 도 9를 보면, 보정 방법 1보다도, 위빙 신호와, 보정 방법 2에 의한 모터(550)의 현재 위치 신호의 위상의 어긋남이 적은 것을 알 수 있다.

이상과 같이, 제1 실시 형태에 관한 위빙 제어 장치에 의하면, 모터 제어부에 있어서 속도 피드 포워드 제어를 실시함으로써, 목표 위치로부터 모터 출력 각도까지의 특성을 로우 패스 필터 처리에서 사용하는 필터에 근사시킬 수 있다. 이 필터에 기초하여, 목표 위치의 진폭의 보정이 행해지므로, 다관절 로봇에 있어서, 다관절 로봇의 축을 동작시키는 모터 자신의 동특성에 기인한 위빙 동작의 오차의 발생이 억제된다. 이에 의해, 높은 궤적 정밀도로 위빙 등의 동작을 가능하게 할 수 있다. 또한, 각 축의 위상의 어긋남은 전체 축 동일한 필터를 사용함으로써 해결하고 있다. 그 결과, 종래 기술에서는, 제어 장치도 포함한 모터의 모델화를 행하여 모델에 기초하여 목표 위치의 보정을 행하고 있었기 때문에, 보정에 걸리는 계산량이 많아, 모델의 오차가 큰 경우에는 잘 보정할 수 없다고 하는 문제가 있었다. 제1 실시 형태에 관한 위빙 제어 장치에 의하면, 이와 같은 문제점을 해결할 수 있게 되었다.

<제2 실시 형태>

이하, 본 발명의 제2 실시 형태에 관한 위빙 제어 장치(20)에 대해 설명한다. 또한, 본 실시 형태에 관한 위빙 제어 장치는, 상술한 제1 실시 형태에 관한 위빙 제어 장치(10)와는, 모터 제어부의 제어 특성의 부여하는 방법에 있어서 상이하다. 그 이외의 구성은, 제1 실시 형태와 동일하므로, 상술한 설명과 중복되는 부분에 대해서는 여기서는 반복하지 않는다.

상술한 제1 실시 형태에 있어서는, 모터 제어부(500)에 속도 피드 포워드부(520)를 설치함으로써, 목표 위치 신호로부터 현재 위치 신호까지의 동특성을 임의 설계한 필터에 근사시킬 수 있도록 하였다. 그러나, 이 근사 정밀도는 충분하지 않은 경우가 있다.

이것은, 다음과 같은 이유에 의한다고 생각된다. 즉, 다관절 로봇(1)의 각 관절은, 다관절 로봇(1)의 자세에 따라 변화되는 관성력 또는/및 중력의 영향, 속도 등에 의존하고 있는 마찰력의 영향을 받고 있다. 그로 인해, 속도 피드 포워드 제어만으로는, 목표 위치 신호로부터 현재 위치 신호까지의 동특성을 충분히 임의 설계한 필터에 근사시킬 수 없다.

제2 실시 형태에 관한 위빙 제어 장치(20)는 목표 위치 신호로부터 현재 위치 신호까지의 동특성을, 임의 설계한 필터에 의해 엄밀하게 근사시킬 수 있도록 하기 위한 제어를 추가한다. 이와 같이 하면, 임의 설계한 필터에 기초하여 위빙 신호의 보정을 행하는 것만으로 위빙 정밀도를 더욱 향상시킬 수 있게 된다.

도 10은, 제2 실시 형태에 관한 위빙 제어 장치(20)의 제어 블록도이다. 도 10의 제어 블록도도, 도 2와 마찬가지로, 다관절 로봇(1)의 복수 어느 관절축 중 1개의 관절축에 대응하고 있다.

위빙 제어 장치(20)는 모터 제어부(500) 대신에 모터 제어부(1500)를 구비하는 점, 모터 제어부(1500)에 타축의 정보를 부여하는 타축 목표 위치 신호 출력부(600)를 더 구비하는 점에 있어서, 도 2에 도시한 위빙 제어 장치(10)와 상이하다.

모터 제어부(1500)는 위빙 제어 장치(10)의 모터 제어부(500)의 구성 외에, 관성항 피드 포워드부(1510)와, 중력항 피드 포워드부(1520)와, 마찰항 피드 포워드부(1530)를 구비한다. 타축 목표 위치 신호 출력부(600)로부터 출력되는, 필터 후의 목표 위치 신호(타축)는 관성항 피드 포워드부(1510), 중력항 피드 포워드부(1520) 및 마찰항 피드 포워드부(1530)에 입력된다. 이들 3종류의 피드 포워드부에서는, 필터 후의 목표 위치 신호(타축)에 기초하여 다관절 로봇(1)의 자세를 알 수 있기 때문에, 자축의 상태를 알 수 있으므로, 자축의 관성 등을 알 수 있다. 이하에, 각 블럭에 대해 상세하게 설명한다.

타축 목표 위치 신호 출력부(600)는 타축에서 마찬가지로 생성되는 필터 연산부(400)로부터의 출력인 필터 후 목표 위치 신호(각도, 각속도, 각가속도)를 출력한다. 예를 들어, 다관절 로봇(1)이 6축의 경우, 타축 목표 위치 신호 출력부(600)는 대상으로 하고 있는 축 이외의 5축분의 필터 후 목표 위치 신호를 모터 제어부(1500)에 출력한다.

관성항 피드 포워드부(1510)는 다관절 로봇(1)의 자세에 의해 결정되는 자축에 작용하는 관성력을 예측하고, 전류 제어부(540)의 피드 포워드 제어의 토크 지령값으로서 출력한다.

중력항 피드 포워드부(1520)는 다관절 로봇(1)의 자세에 의해 결정되는 자축에 작용하는 중력 영향을 예측하고, 전류 제어부(540)의 피드 포워드 제어의 토크 지령값으로서 출력한다.

마찰항 피드 포워드부(1530)는 자축에 작용하는 마찰력을 예측하고, 전류 제어부(540)의 피드 포워드 제어의 토크 지령값으로서 출력한다.

제2 실시 형태에 관한 위빙 제어 장치(20)의 특징은, 위빙 제어 장치(10)의 모터 제어부(500)에 3종의 피드 포워드 제어부를 설치하는 것이다. 이에 의해, 목표 위치 신호로부터 인코더(560)에서 검출되는 현재 위치 신호까지의 동특성을, 필터 연산부(400)에서 사용되는 필터에, 보다 엄밀하게 근사시킬 수 있다.

이 구성에 의하면, 제1 실시 형태에서 설명한, 필터에 기초하여 위빙 신호의 보정을 행하는 것만으로, 양호한 위빙 파형을 얻을 수 있다.

또한, 여기서 실시하고 있는 3종의 피드 포워드 제어는, 다관절 로봇(1)의 모델(운동 방정식)에 기초하여 설계된다. 여기서는, 도 11에 도시하는 2링크 로봇을 대상으로 하여, 각종 피드 포워드 제어(관성항, 중력항, 마찰항)의 계산 방법에 대해 설명한다.

도 11에 도시하는 2링크 로봇에 있어서의 운동 방정식을 구하면, 각 관절에 작용하는 토크 τ1, τ2는, 이하의 수학식 4 및 수학식 5로 각각 나타내어진다. 그리고, 이하의 수학식 6∼수학식 13에 나타내는 바와 같이 계산되어, 1축째에 관한 3종류의 피드 포워드 제어(관성항, 중력항, 마찰항)의 출력이, 수학식 14에 나타내어지는 바와 같이 산출된다.

상기한 3종류의 피드 포워드 제어를 실시하면, 목표 위치 신호로부터 인코더(560)에서 검출되는 현재 위치 신호까지의 동특성을, 필터 연산부(400)에서 사용되는 필터에 일치시킬 수 있다. 또한, 제2 실시 형태는, 타축의 동작의 영향에 관해서는 고려하고 있지 않기 때문에, 단일 축 동작하는 경우로 한정한다. 타축이 동작하는 것에 의한 영향을 고려한 경우에 대해서는, 후술하는 제3 실시 형태에서 검토한다.

또한, 상기한 3종류의 피드 포워드 제어를 부분적으로 실시하는 것뿐이어도, 목표 위치 신호로부터 인코더(560)에서 검출되는 현재 위치 신호까지의 동특성을, 필터 연산부(400)에서 사용되는 필터 특성에, 제1 실시 형태보다도, 더욱 근접시킬 수 있다.

도 12 및 도 13은, 상기한 2링크 로봇의 2축째에, 어느 정현파 신호를 목표 위치 신호로서 입력한 경우의 출력 파형을 나타내고 있다. 1축째의 목표 위치 신호는 제로이다. 실선이 목표 위치 신호를 나타내고, 파선이 필터 후 목표 위치 신호를 나타내고, 일점쇄선이 모터(550)의 현재 위치 신호를 나타낸다.

도 12는, 제1 실시 형태에 대응하는 것으로서, 속도 피드 포워드 제어만을 사용한 경우의 출력 파형을 나타낸다. 도 13은, 제2 실시 형태에 대응하는 것으로서, 속도 피드 포워드 제어, 관성항 피드 포워드 제어, 중력항 피드 포워드 제어 및 마찰항 피드 포워드 제어를 사용한 경우의 출력 파형을 나타내고 있다.

이들을 보면, 속도 피드 포워드 제어 외에 3종류의 피드 포워드 제어를 실시함으로써, 속도 피드 포워드 제어만의 경우보다도, 현재 위치 신호가 필터 후 목표 위치 신호에 더욱 근접하고 있는 것을 알 수 있다. 이와 같이, 목표 위치 신호로부터 현재 위치 신호까지의 동특성을 필터 특성에 근사할 때의 정밀도를 향상시킬 수 있다.

이에 의해, 제1 실시 형태에 있어서 설명한 바와 같이, 필터에 기초하여 위빙 신호의 보정을 행하는 것만으로, 양호한 위빙 파형을 얻을 수 있다.

도 15는, 제1 실시 형태에서 설명한 게인 보정 방법(보정 방법 1)을 사용하여, 제어 블럭(도 14)을 사용한 경우의 출력 파형을 나타낸다. 도 15에는, 보정 방법 1을 제2 실시 형태에 적용한 경우의 출력 파형이 나타내어져 있고, 또한 비교를 위해, 보정 방법 1을 제1 실시 형태에 적용한 경우의 출력 파형이 나타내어져 있다.

도 15는, 2축째에, 어느 정현파 신호를 위빙 신호로서 입력한 경우에, 보정 방법 1(게인 보정)을 행한 경우의 2축째의 출력 파형을 나타내고 있다. 실선이 위빙 신호를 나타내고, 일점쇄선이 제2 실시 형태(실시예 2)에서 보정 방법 1을 행한 경우의 모터(550)의 현재 위치 신호를 나타내고, 점선이 제1 실시 형태(실시예 1)에서 보정 방법 1을 행한 경우의 모터(550)의 현재 위치 신호를 나타낸다. 또한, 여기서는, 단일 축만이 구동되는 것으로 하고 있기 때문에, 타축이 동작하는 것에 의한 영향은 나와 있지 있다. 이것을 보면, 제2 실시 형태 쪽이 모터(550)의 현재 위치 신호의 진폭이 위빙 신호의 진폭에 가깝고, 적절하게 보정할 수 있는 것을 알 수 있다.

이상과 같이, 제2 실시 형태에 관한 위빙 제어 장치에 의하면, 제1 실시 형태에 관한 위빙 제어 장치에 있어서의 제어 외에, 각 축에 작용하는 관성력, 중력 및 마찰력을 미리 예측하고, 적절한 토크 지령값으로서 모터에 입력한다. 이에 의해, 목표 위치로부터 모터 출력 각도까지의 동특성을 필터에 근사하는 정밀도를 향상시킬 수 있다. 이 필터에 기초하여 목표 위치의 진폭의 보정을 행함으로써, 다관절 로봇에 있어서, 다관절 로봇의 축을 동작시키는 모터 자신의 동특성에 기인하는 위빙 동작의 오차 및 타축이 동작하는 것에 의한 자축의 영향에 기인하는 위빙 동작의 오차의 발생이 억제된다. 따라서, 높은 궤적 정밀도로 위빙 등의 동작이 가능하게 된다.

<제3 실시 형태>

이하, 본 발명의 제3 실시 형태에 관한 위빙 제어 장치(30)에 대해 설명한다. 또한, 본 실시 형태에 관한 위빙 제어 장치는, 모터 제어부의 제어 특성의 부여하는 방법에 있어서, 상술한 제1 실시 형태에 관한 위빙 제어 장치(10)와 상이하다. 그 이외의 구성은, 제1 실시 형태와 동일하므로, 상술한 설명과 중복되는 부분에 대해서는 여기서는 반복하지 않는다.

상술한 제2 실시 형태에 있어서는, 제1 실시 형태에서 실장한 속도 피드 포워드부(520)에 3종류의 피드 포워드 제어부를 설치함으로써, 목표 위치 신호로부터 현재 위치 신호까지의 동특성을 임의 설계한 필터에 더욱 근사시킬 수 있도록 하였다. 그러나, 제2 실시 형태에 있어서는, 타축이 동작하는 것에 의한 영향에 관해서는 고려되어 있지 않다. 그러나, 타축의 영향을 고려하여 근사해야 하는 경우도 있다.

제3 실시 형태에 관한 위빙 제어 장치(30)는 목표 위치 신호로부터 현재 위치 신호까지의 동특성을, 임의 설계한 필터에 의해 더욱 엄밀하게 근사시킬 수 있도록 하기 위한 제어를 추가한다. 이와 같이 하면, 임의 설계한 필터에 기초하여 위빙 신호의 보정을 행하는 것만으로, 위빙 정밀도를 더욱 향상시킬 수 있게 된다.

도 16은, 제3 실시 형태에 관한 위빙 제어 장치(30)의 제어 블록도이다. 도 16의 제어 블록도도, 도 2 및 도 10과 마찬가지로, 다관절 로봇(1)의 복수 어느 관절축 중 1개의 관절축에 대응하고 있다.

위빙 제어 장치(30)는 모터 제어부(1500) 대신에 모터 제어부(2500)를 구비하는 점에 있어서, 도 10에 도시한 위빙 제어 장치(20)와 상이하다.

모터 제어부(2500)는 위빙 제어 장치(20)의 모터 제어부(1500)의 구성 외에, 간섭 관성항 피드 포워드부(2510) 및 원심 코리올리항 피드 포워드부(2520)를 구비한다. 타축 목표 위치 신호 출력부(600)로부터 출력되는, 필터 후의 목표 위치 신호(타축)는 이들 간섭 관성항 피드 포워드부(2510) 및 원심 코리올리항 피드 포워드부(2520)에 입력된다. 또한, 타축 목표 위치 신호 출력부(600)로부터, 관성항 피드 포워드부(1510), 중력항 피드 포워드부(1520) 및 마찰항 피드 포워드부(1530)에 필터 후의 목표 위치 신호(타축)가 입력되는 점은, 위빙 제어 장치(20)와 동일하다. 이하에, 각 블럭에 대해 상세하게 설명한다.

간섭 관성항 피드 포워드부(2510)는 타축이 동작하는 것에 의한 자축의 아암에 작용하는 관성력을 예측하고, 전류 제어부(540)의 피드 포워드 제어의 토크 지령값으로서 출력한다.

원심 코리올리항 피드 포워드부(2520)는 타축이 동작하는 것에 의한 자축의 아암에 작용하는 원심 코리올리력을 예측하고, 전류 제어부(540)의 피드 포워드 제어의 토크 지령값으로서 출력한다.

제3 실시 형태에 관한 위빙 제어 장치(30)의 특징은, 제2 실시 형태에 관한 위빙 제어 장치(20)의 모터 제어부(1500)에, 타축이 동작하였을 때의 영향을 보상하기 위한 2종류의 피드 포워드 제어부를 더 설치하는 것이다. 이에 의해, 목표 위치 신호로부터 인코더(560)에서 검출되는 현재 위치 신호까지의 동특성을, 보다 엄밀하게 필터 연산부(400)에서 사용되는 필터에 근사시킬 수 있다.

이 구성에 의하면, 제1 및 제2 실시 형태에서 설명한, 필터에 기초하여 위빙 신호의 보정을 행하는 것만으로, 양호한 위빙 파형을 얻을 수 있다.

또한, 여기서 실시하고 있는 2종의 피드 포워드 제어는, 다관절 로봇(1)의 모델(운동 방정식)에 기초하여 설계된다. 여기서는, 도 11에 도시하는 2링크 로봇을 대상으로 하여, 각종 피드 포워드 제어(간섭 관성항, 원심 코리올리항)의 계산 방법에 대해 설명한다.

도 11에 도시하는 2링크 로봇에 있어서의 운동 방정식을 구하면, 각 관절에 작용하는 토크 τ1, τ2는, 이하의 수학식 15 및 수학식 16으로 각각 나타내어진다. 그리고, 이하의 수학식 17∼수학식 19에 나타내는 바와 같이 계산되어, 1축째에 관한 2종류의 피드 포워드 제어(간섭 관성항, 원심 코리올리항)의 출력이, 수학식 20에 나타내는 바와 같이 산출된다. 또한, 이하의 식에서는, 간섭 관성항 및 원심 코리올리항에 관계가 없는 부분은 기재하고 있지 않다.

상기한 2종류의 피드 포워드 제어를, 제2 실시 형태에 관한 3종류의 피드 포워드 제어에 추가하여 실시하면, 타축이 동작한 경우에도, 목표 위치 신호로부터 인코더(560)에서 검출되는 현재 위치 신호까지의 동특성을, 필터 연산부(400)에서 사용되는 필터에 일치시킬 수 있다.

또한, 상기한 2종류의 피드 포워드 제어를 부분적으로 실시하는 것뿐이어도, 목표 위치 신호로부터 인코더(560)에서 검출되는 현재 위치 신호까지의 동특성을, 필터 연산부(400)에서 사용되는 필터 특성에, 제1 및 제2 실시 형태보다도, 더욱 근접시킬 수 있다.

도 17 및 도 18은, 상기한 2링크 로봇의 각 관절에, 어느 정현파 신호를 목표 위치 신호로서 입력한 경우의 2축째의 출력 파형을 나타내고 있다. 실선이 목표 위치 신호를 나타내고, 파선이 필터 후 목표 위치 신호를 나타내고, 일점쇄선이 모터(550)의 현재 위치 신호를 나타낸다.

또한, 도 17은, 제2 실시 형태에 대응하는 것으로서, 속도 피드 포워드 제어, 관성항 피드 포워드 제어, 중력항 피드 포워드 및 마찰항 피드 포워드 제어를 사용한 경우의 출력 파형을 나타낸다. 도 18은, 제3 실시 형태에 대응하는 것으로서, 간섭 관성항 피드 포워드 제어 및 원심 코리올리항 피드 포워드 제어를 사용한 경우의 출력 파형을 나타내고 있다.

이들을 보면, 간섭 관성항 피드 포워드 제어 및 원심 코리올리항 피드 포워드 제어를 추가함으로써, 타축이 동작한 경우에도, 현재 위치 신호가 필터 후 목표 위치 신호에 더욱 근접하고 있는 것을 알 수 있다. 이에 의해, 목표 위치 신호로부터 현재 위치 신호까지의 동특성을 필터 특성에 근사할 때의 정밀도를 향상시킬 수 있는 것을 알 수 있다.

이에 의해, 제1 및 제2 실시 형태에 있어서 설명한 바와 같이, 필터에 기초하여 위빙 신호의 보정을 행하는 것만으로, 양호한 위빙 파형을 얻을 수 있다.

도 20은, 제1 실시 형태에서 설명한 게인 보정 방법(보정 방법 1)을 사용하여, 제어 블럭(도 19)을 사용한 경우의 출력 파형을 나타낸다. 도 20에는, 보정 방법 1을 제3 실시 형태에 적용한 경우의 2축째의 출력 파형이 나타내어져 있고, 또한 비교를 위해, 보정 방법 1을 제2 실시 형태에 적용한 경우의 2축째의 출력 파형이 나타내어져 있다.

도 20은, 각 관절에, 어느 정현파 신호를 위빙 신호로서 입력한 경우에, 보정 방법 1(게인 보정)을 행한 경우의 2축째의 출력 파형을 나타내고 있다. 실선이 위빙 신호를 나타내고, 일점쇄선이 제3 실시 형태(실시예 3)에서 보정 방법 1을 행한 경우의 모터(550)의 현재 위치 신호를 나타내고, 점선이 제2 실시 형태(실시예 2)에서 보정 방법 1을 행한 경우의 모터(550)의 현재 위치 신호를 나타낸다. 이것을 보면, 제3 실시 형태 쪽이, 제2 실시 형태보다도, 모터(550)의 현재 위치 신호의 진폭이 위빙 신호의 진폭에 가깝고, 적절하게 보정되어 있는 것을 알 수 있다.

이상과 같이, 제3 실시 형태에 관한 위빙 제어 장치에 의하면, 제2 실시 형태에 관한 위빙 제어 장치에 있어서의 제어 외에, i축(자축) 이외의 축이 가감속함으로써 i축에 작용하는 관성력 및 원심 코리올리력이 미리 예측되고, 적절한 토크 지령값으로서 모터에 입력된다. 이에 의해, 타축이 동작하는 것에 의한 영향을 억제하여, 목표 위치로부터 모터 출력 각도까지의 동특성을 필터에 근사하는 정밀도를 향상시킬 수 있다. 이 필터에 기초하여 목표 위치의 진폭 보정을 행함으로써, 다관절 로봇에 있어서, 다관절 로봇의 축을 동작시키는 모터 자신의 동특성에 기인하는 위빙 동작의 오차 및 타축이 동작하는 것에 의한 영향에 기인하는 위빙 동작의 오차의 발생이 억제된다. 따라서, 높은 궤적 정밀도로 위빙 등의 동작이 가능하게 된다.

<변형예>

이하, 본 발명의 실시 형태의 변형예에 관한 위빙 제어 장치에 대해 설명한다. 또한, 본 변형예는, 상술한 제1∼제3 실시 형태에 관한 위빙 제어 장치에 적용할 수 있고, 필터 연산부(400)에 입력되는 목표 위치 신호의 생성 방법에 관한 것이다. 본 변형예는, 그 이외의 구성에 있어서는, 제1∼제3 실시 형태와 동일하므로, 상술한 설명과 중복되는 부분에 대해서는 여기서는 반복하지 않는다.

상술한 실시 형태에 있어서는, 위빙 신호의 보정을 다관절 로봇(1)의 각 축 좌표계에서 행하고 있었다. 이로 인해, 도 21에 도시한 바와 같이, 각 축 좌표계에서 기본 위치 신호와 위빙 신호를 분리하여 산출할 필요가 있다. 그러나, 이와 같이 하면, 위빙하는 경우와 하지 않는 경우에서 각각 각 축의 각도 지령값을 산출하기 위해 역변환을 행할 필요가 있고, 계산 부하가 커진다.

따라서, 본 변형예에 있어서는, 보다 간이하게 보정을 행할 수 있도록 한다. 도 22를 참조하여, 본 변형예에 대해 설명한다.

도 22에 도시한 바와 같이, 본 변형예의 제어 장치는, 상술한 실시 형태(도 2, 도 10, 도 16)의 제어 블럭에 있어서, 역변환 연산부(700)를 더 구비한다. 이하에, 각 블럭에 대해 상세하게 설명한다.

위빙 파형 생성부(1100)는 지령된 진폭 및 주파수로 위빙을 행하기 때문에, 상술한 기본 위치 파형에 추가되는 파형을, 위빙 파형으로서 생성한다. 이 위빙 파형은, 선단 위치를 나타내기 위한 3차원 위치와 선단 자세를 나타내기 위한 각도로 표현된다.

위빙 파형 보정부(1200)는 위빙 파형 생성부(1100)에서 생성된 위빙 파형을 보정하고, 위빙 보정 파형을 생성한다. 보정 방법에 관해서는, 제1 실시 형태에서 설명한 것과 마찬가지이다.

기본 파형 생성부(1300)는 위빙을 하지 않는 경우의 다관절 로봇(1)의 선단 궤적으로서의 기본 위치 파형을 생성한다. 이 기본 위치 파형은, 선단 위치를 나타내기 위한 3차원 위치와 선단 자세를 나타내기 위한 각도로 표현된다.

역변환 연산부(700)는 기본 위치 파형에 위빙 보정 파형이 추가된 목표 파형에 대해 역변환을 행하고, 각 관절의 목표 위치 신호를 생성한다. 이 목표 파형은, 선단 위치를 나타내기 위한 3차원 위치와 선단 자세를 나타내기 위한 각도로 표현된다.

이와 같이, 본 변형예에 의하면, 역변환은 1회만으로 되기 때문에, 계산 부하를 증가시키는 일 없이 본 발명의 실시 형태에 관한 보정 방법을 적용할 수 있다.

또한, 금회 개시된 실시 형태는 모든 점에서 예시이며 제한적인 것은 아니라고 생각되어야 한다. 본 발명의 범위는 상기한 설명이 아닌 특허청구범위에 의해 나타내어지고, 특허청구범위와 균등한 의미 및 범위 내에서의 모든 변경이 포함되는 것이 의도된다. 본 출원은 2012년 10월 19일 출원된 일본 특허 출원(일본 특허 출원 제2012-231871)에 기초하는 것으로, 그 내용은 여기에 참조로서 원용된다.

1 : 다관절 로봇
10, 20, 30 : 위빙 제어 장치
100 : 위빙 신호 생성부
200 : 위빙 신호 보정부
300 : 기본 위치 신호 생성부
400 : 필터 연산부
500, 1500, 2500 : 모터 제어부
510 : 위치 제어부
520 : 속도 피드 포워드부
530 : 속도 제어부
540 : 전류 제어부
550 : 모터
560 : 인코더
600 : 타축 목표 위치 신호 출력부
700 : 역변환 연산부
1100 : 위빙 파형 생성부
1200 : 위빙 파형 보정부
1300 : 기본 파형 생성부
1510 : 관성항 피드 포워드부
1520 : 중력항 피드 포워드부
1530 : 마찰항 피드 포워드부
2510 : 간섭 관성항 피드 포워드부
2520 : 원심 코리올리항 피드 포워드부

Claims (6)

1. 다관절 로봇에 설치된 툴이 원하는 위빙 궤적을 그리도록 복수의 관절축을 구동시키는 다관절 로봇의 위빙 제어 장치이며,
   상기 위빙 궤적에 기초하여, 상기 다관절 로봇의 각 축의 목표 위치 신호를 연산하는 신호 연산부와,
   상기 연산된 목표 위치 신호에 대해, 상기 다관절 로봇의 고유 진동 성분을 제거하기 위한 로우 패스 필터 처리를 행하고, 상기 다관절 로봇의 각 축에 대해 필터 처리된 목표 지령 신호를 연산하는 필터 연산부와,
   상기 필터 처리된 목표 지령 신호를 입력으로 하여, 상기 다관절 로봇의 각 축을 구동하는 모터 제어부를 포함하여 구성되고,
   상기 모터 제어부에 있어서의 게인의 주파수 특성이 플랫하게 구성되고,
   상기 신호 연산부는, 상기 필터 연산부의 필터 특성으로부터 얻어지는 위빙 주파수에 있어서의 필터 게인의 역수를 목표 위치 신호에 승산함으로써, 게인 보정된 목표 위치 신호를 연산하는 것을 특징으로 하는, 제어 장치.
2. 제1항에 있어서,
   상기 모터 제어부는, 위치 피드백부와 속도 피드백부와 속도 피드 포워드부를 포함하고,
   상기 신호 연산부는, 상기 필터 연산부에 있어서의 필터 특성 및 상기 위빙 궤적에 기초하여, 게인 보정된 목표 위치 신호를 연산하는 것을 특징으로 하는, 제어 장치.
3. 제1항에 있어서,
   상기 로우 패스 필터 처리에서 사용하는 필터는 전체 축에서 동일한 것을 특징으로 하는, 제어 장치.
4. 제1항에 있어서,
   상기 신호 연산부는, 상기 필터 연산부의 필터 특성 및 상기 필터 연산부의 필터 특성으로부터 얻어지는 위빙 주파수에 기초하여, 게인 보정 외에, 위상 보상된 목표 위치 신호를 연산하는 것을 특징으로 하는, 제어 장치.
5. 제2항에 있어서,
   상기 모터 제어부는, 자축이 가감속함으로써 자축에 작용하는 관성력, 중력 및 마찰력을 예측하고, 상기 속도 피드백부로부터 출력되는 토크 지령값에 추가하는 피드 포워드 제어부를 더 포함하는 것을 특징으로 하는, 제어 장치.
6. 제5항에 있어서,
   상기 모터 제어부는, 타축이 가감속함으로써 자축에 작용하는 관성력 및 원심 코리올리력을 예측하고, 상기 속도 피드백부로부터 출력되는 토크 지령값에 추가하는 피드 포워드 제어부를 더 포함하는 것을 특징으로 하는, 제어 장치.

Images