KR0160699B1 - 로보트의 원호 보간 방법 - Google Patents

Abstract

본 발명은 로보트의 원호 보간 방법에 관한 것으로, 상세하게는 원호 보간시 원호의 시작과 끝점에서 충격이 발생하지 않고 부드러운 동작으로 원호를 그릴 수 있는 로보트의 원호 보간 방법에 관한 것이다.

즉, 본 발명에 따른 로보트의 원호 보간 방법은 (7)식과 같은 S-자가감속을 사용하여 진동과 충격이 없이 고속인 동시에 고정도의 원호 보간이 가능한 장점이 있다.

Description

로보트의 원호 보간 방법

제1도는 X-Y 평면에 원을 그리기 위한 각축의 속도 분포도이고,

제2도는 X-Y 평면에서의 원호 작도법을 나타내고,

제3도는 본 발명의 원호 보간 방법에 사용되는 로보트의 위치 제어 시스템의 블록도이고,

제4도는 본 발명에 따른 로보트의 원호 보간 방법에 의한 로보트 위치 제어 순서도이다.

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

1 : 마이크로프로세서 2 : 타이머

3 : 위치 제어 구동부 31 : 디지털/아날로그 변환기

32 : 서보-제어부 33 : 모터

34 : 타코제너레이터 35 : 인코더

36 : 업/다운 카운터

본 발명은 로보트의 원호 보간 방법에 관한 것으로, 상세하게는 원호 보간시 원호의 시작과 끝점에서 충격이 발생하지 않고 부드러운 동작으로 원호를 그릴 수 있는 로보트의 원호 보간 방법에 관한 것이다.

조립용 로보트에서 현재 위치에서 목표 위치 까지 이송시 도중의 경로는 중요하지 않은 PTP(point-to-point) 동작이 있으나 로보트를 이용하여 용접, 페인팅 작업등을 하거나 로보트의 이송 경로 내에 장애물이 존재하는 경우와 같이 극히 제한된 작업 환경 내에서는 주어진 경로를 따라 가야만 하는 경우가 발생한다. 이와 같이 로보트의 손끝을 원호 경로를 따라 부드럽게 이동시키기 위하여는 적절한 원호 보간 방법이 필요하다.

일반적으로 X-Y 평면에서 원호의 보간을 하기 위해서는, 제1도에 도시된 바와 같이, X-Y 축의 속도 분포가 X축은 코사인(cosine) 함수 Y축은 사인(sine) 함수가 되어야만 원이 형성된다. 따라서 제1도에 도시된 바와 같이, Y축은 sine 형태의 속도 곡선으로서 이루어지기 때문에 시작점에서 원을 그리기 시작하기에 무리가 없으나, X축은 cosine 형태로서 원을 그리기 시작하는 시점에서 속도가 0에서 최고 속도(Vmax)까지 올려야 하기 때문에 고속의 원호 보간시 로보트에 진동이 발생하고 원의 모양이 왜곡되는 문제점이 있다. 또한 제1도에 도시된 바와 같이 1/4 원만 그리는 경우, 점선으로 표시된 지점(A선)에서 X, Y축의 모터를 정지시켜야 하는데, Y축은 속도가 0이고, X축은 최고 속도에서 바로 모터를 정지시켜야 하기 때문에 진동이 발생되고 고속의 원호 운동이 불가능할 뿐만 아니라 원호의 형상도 왜곡되는 문제점이 발생한다. 따라서 고속의 원호 운동과 정확한 원호 운동을 위해서는 가감속이 필요하다.

일반적으로 이 가감속을 사다리꼴, 지수 함수 형태를 많이 사용하나 사다리꼴, 지수 함수 가감속시에는 속도의 불연소간 점이 존재하여 이 속도의 불연속점에서 미세 진동이 발생하는 단점이 있어, 고정도의 원호 보간이 불가능한 문제점이 있다.

본 발명은 상기와 같은 문제점을 개선하고자 창안된 것으로, 고속의 원호 보간시에도 급격한 속도 가감속 점에서 모터의 고속 원호 운동이 진동을 발생하지 않을 뿐만 아니라 원호에 왜곡이 없는 로보트의 원호 보간 방법을 제공하는데 그 목적이 있디.

상기와 같은 목적을 달성하기 위하여 본 발명에 따른 로보트의 원호 보간 방법은, 원호의 시작점(X₀, y₀)를 램에서 읽어내는 단계;

다음식에 의해 소정의 k 시점에서 (x_k, y_k)를 계산하는 단계;

다음식에 의해 x, y축에 대한 가감속을 행하는 단계;

여기서, S_x(k), S'_x(k), S_y(k), S'_y(k)는 매개 변수이다.

다음식에 의해 상기 k 시점에서 각 축의 목표위치(P_x(k), P_y(k))를 구하는 단계;

각 축의 위치 편차를 구해 비례 적분 미분 제어를 행하는 단계;

상기 비례 적분 미분 제어 신호를 디지털/아날로그 변환기로 변환하여 서보 모터를 구동하는 단계;를 포함하는 것을 특징으로 한다.

이하 도면을 참조하면서 본 발명에 따른 로보트의 원호 보간 방법을 설명한다.

제1도는 X-Y 평면에 원을 그리기 위한 각 축(x축, y축)의 속도 분포도이고, 제2도는 X-Y 평면에서의 원호 작도법을 나타낸 것으로 제1상한에 대해서만 표시한다. 즉, r은 반경을 나타내고, θ₁은 원을 그리기 위한 증분각을 나타낸다.

제3도는 본 발명의 원호 보간 방법에 사용되는 로보트의 위치 제어 시스템의 블록도이다. 여기서 그 구성을 살펴보면 다음과 같다.

디지털/아날로그 변환부(31)는 마이크로프로세서(1)에서 출력된 속도 명령을 아날로그 값으로 변환하여 서보 드라이버에 입력하는 것으로 서보 모터의 정 및 역방향으로 구동하는 명령 신호와 구동 속도의 신호를 출력한다.

서보 제어부(32)는 서보 모터(33)의 전류 귀환 신호가 귀환됨과 아울러 타코제네레이터(34)가 서보 모터(33)의 구동 속도에 따라 검출한 속도 귀환 신호(Vox)가 서보 제어부(32)로 귀환되어 서보 제어부(32)는 서보 모터(33)를 서보 제어하게 된다.

업/다운 카운터(36)는 서보 모터(33)의 회전양에 따라 펄스 신호를 출력하는 인코더(35)로부터 출력된 펄스 신호를 업 카운트 또는 다운 카운트하여 로보트의 현재 위치를 마이크로프로세서(1)에 알려주는 역할을 한다.

타이머(2)는 일정한 주기 마다 마이크로프로세서(1)에 인트럽트를 걸어 주기 위한 것이다.

그리고, 마이크로프로세서(1)는 인트럽트가 걸릴 때 마다 원호 운동에 따라 가감속이 없는 경우 각 축의 위치 증분량을 계산한 후 S-자 가감속을 실행하여 임의의 시점에서 목표 위치를 구한 후 업/다운 카운터(36)로부터 귀환된 현재 위치를 읽어들여 PID 제어를 행한 후 D/A 변환기로 출력한다.

이상과 같이 구성된 로보트 원호 보간 장치의 원호 보간 원리와 동작을 설명하면 다음과 같다.

제2도에 도시된 바와 같이, 원호의 시작점을 (x₀, y₀)로 하고, 샘플링 시간(T) 동안 움직여야 할 거리를 (x₁, y₁)이라 하면,

로 표시된다. 또한 그 다음 샘플링 시간 동안 움직여야 할 위치를 (x₂, y₂)라 하면,

로 표시된다. (2a, 2b)식을 다시쓰면,

이 된다. (1a, 1b)식과 (3a, 3b)식에서

운동에 따라 가감속이 없는 경우 각 축의 위치 증분량을 계산한 후 S-자 가감속을 실행하여 임의의 시점에서 목표 위치를 구한 후 업/다운 카운터(36)로부터 귀환된 현재 위치를 읽어들여 PID 제어를 행한 후 D/A 변환기로 출력한다.

이상과 같이 구성된 로보트 원호 보간 장치의 원호 보간 원리와 동작을 설명하면 다음과 같다.

제2도에 도시된 바와 같이, 원호의 시작점을 (x₀, y₀)로 하고, 샘플링 시간(T) 동안 움직여야 할 거리를 (x₁, y₁)이라 하면,

로 표시된다. 또한 그 다음 샘플링 시간 동안 움직여야 할 위치를 (x₂, y₂)라 하면,

로 표시된다. (2a, 2b)식을 다시쓰면,

이 된다. (1a, 1b)식과 (3a, 3b)식에서

로 표시된다. 따라서 원호 이동시 매 샘플링 시간 마다 움직여야 할 각도 θ가 일정하다고 하면,

로 표시되므로, (4a, 4b)식을 다시쓰면,

로 표시된다. (5a, 5b)식을 임의의 시점 k에서 다시쓰면,

로 표시된다. 따라서 (6a, 6b)식으로 x, y 좌표를 계산하면 제1도에 도시된 바와 같은 사인 및 코사인 형태의 속도 분포가 얻어진다. 여기에 가속 혹은 감속을 하기 위해 다음과 같은 식을 사용한다. 즉,

여기서, S_x(k), S'_x(k), S_y(k), S'_y(k)는 매개 변수이다.

위 식의 의미를 살펴보면 다음과 같다.

예를 들어, x₁=x₂=x₃=x₄=x₅=x₆=x₇=x₈=10이고, n=5, m=-5일 때 x_k0, S'_x(k)를 계산하면 아래와 같다.

즉, x가 k=1~8까지 일정하게 입력(가감속이 없는 경우 (7)식의 출력인 S'(k)/m은 상기 표와 같이 S자 형태의 가감속이 됨을 알 수 있고, 마찬가지로 y축에도 동일하게 성립한다.)

f(i)=S'(k)/m. f(i)=S'(k)/m으로 놓으면, 임의의 k 순간에 x, y축의 목표 위치는,

로 표시된다. (8a, 8b)식에 의해 구한 각 축의 목표 위치와 각 축 마다의 업/다운 카운터에서 읽은 로보트의 현재 위치 값을 C_x(k), C_y(k)라 하면 임의의 k시점에서 위치 편차는 다음과 같이 계산된다.

따라서 각 축의 위치 편차에 대하여 비례 적분 미분(PID) 제어를 행하며, 다음과 같이 계산한다.

여기서 T는 샘플링 시간을 나타내고, D_xout, D_yout은 각 축의 디지털/아날로그 변환기로 입력되는 값이고, K_Px, K_Py는 각 축의 비례(P) 이득이며, K_Ix, K_Iy는 각 축의 적분(I) 이득이다. 또한, K_Dx, K_Dy는 각 축의 미분(D) 이득이며, 각 축의 PID 이득은 로보트의 운동 상태가 최적이 되도록 결정한다.

이러한 원리에 의해 로보트를 구동하면 x, y 평면 상에서 원호 보간이 이루어지며, (5)식에서 알 수 있듯이 θ가 cosθ, sinθ 값도 일정하기 때문에 매번 cosθ, sinθ를 계산하지 않아도 되기 때문에 가감속이 없는 경우 원호를 계산하기 위한 시간이 줄어들고, S자 형태의 가감속을 (8a, 8b)식에 의해 수행하기 때문에 원호 보간시 충격이나 진동을 발생시키지 않고, 고속인 동시에 고정도의 원호 보간이 가능하다.

이상과 같은 원리에 의해 실제로 원호 보간을 실행하는 순서를 제4도의 로보트 위치 제어 순서도를 참조하면서 설명한다.

원호의 시작점(x₀, y₀)를 램(RAM)에서 읽은 다음, 다음 (6a, 6b)식에 의해 소정의 k 시점에서 원호 상의 일점(x_k, y_k)를 계산한다.

다음으로 (7a~7f)식에 의해 x, y 축에 대한 가감속을 행한 다음, (8a, 8b)식에 의해 상기 k 시점에서 각 축의 목표위치 (P_x(k), P_y(k))를 구한다.

다음에 각 축의 위치 편차를 위해 비례 적분 미분(PID) 제어를 행한 다음, 상기 비례 적분 미분(PID) 제어 신호를 디지털/아날로그 변환기로 변환하여 서보 모터를 구동한다.

다음에 그리려는 원호의 끝점인가를 판단하여, 끝점이 아니면 (6a, 6b)식의 계산 과정으로 되돌아가 다시 계산 과정을 수행하도록 하고, 끝점이면 모터를 정지시켜 로보트의 원호 보간을 끝낸다.

이상 설명한 바와 같이, 본 발명에 따른 로보트의 원호 보간 방법은 (7)식과 같은 S-자 가감속을 사용하여 진동과 충격이 없이 고속인 동시에 고정도의 원호 보간이 가능한 장점이 있다.

Claims (1)

1. 원호의 시작점(x₀, y₀)를 램에서 읽어내는 단계; 다음식에 의해 소정의 k 시점에서 (x_k, y_k)를 계산하는 단계;

   다음식에 의해 x, y에 대한 가감속을 행하는 단계;

   여기서, S_x(k), S'_x(k), S_y(k), S'_y(k)는 매개 변수이다. 다음식에 의해 상기 k 시점에서 각 축의 목표위치 (P_x(k), P_y(k))를 구하는 단계;

   각 축의 위치 편차를 구해 비례 적분 미분 제어를 행하는 단계; 상기 비례 적분 미분 제어 신호를 디지털/아날로그 변환기로 변환하여 서보 모터를 구동하는 단계;를 포함하는 것을 특징으로 하는 로보트의 원호 보간 방법.