KR940001204B1 - 로보트의 직선보간방법 - Google Patents

Abstract

내용 없음.

Description

로보트의 직선보간방법

제1도는 수평다관절 로보트의 X-Y 평면에서의 로보트의 앤드 이펙터의 직선이송 개념도.

제2도는 직각좌표상의 X-Y 평면에서의 직선이송시 직선의 이송속도를 일정하게 한 경우의 그래프.

제3도는 직각좌표상의 X-Y 평면에서의 직선이송시 직선의 이송속도를 직선함수 및 지수함수적으로 한 경우의 그래프.

제4도는 가감속이 없는 경우의 매 샘플링 시간마다 이송해야할 직선의 증분량을 도시한 그래프.

제5도는 직선이송시 직선함수 및 지수함수 속도분포도.

제6도는 본 발명의 로보트의 직선보간방법을 실현하기 위한 블록도.

제7도는 본 발명의 동작순서를 도시한 플로우챠트.

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

1 : 마이크로 프로세서 2 : 디지털/아날로그 변환기

3 : 서보제어부 4 : 서보모터

5 : 타코제너레이터 6 : 엔코더

7 : 업/다운 카운터 8 : 타이머

본 발명은 다관절 로보트의 아암의 앤드이펙타(End-Effector)를 직각좌표계의 X-Y 평면에서 직선 이송시키기 위한 방법에 관한 것으로써, 특히 직선이송시의 속도분포를 직선함수 및 지수함수적으로 함으로써 로보트의 구동개시점과 종료점에서의 정밀도를 향상시킴과 동시에 직선함수 및 지수함수 가감속을 이산시간 상태방정식에 의해 실현함으로써 샘플링시간을 단축시키고 직선의 정밀도를 향상시킬 뿐만 아니라 고속이동이 가능한 로보트의 직선보간방법에 관한 것이다.

일반적으로 로보트는 직선보간시 직선을 일정거리로 분할하고 그 분할된 미소거리에 의해 포인트 투 포인트(PPT : point-to-point)동작을 하여 로보트를 상기 분할하는 직선의 경로로 구동케 하는 방법이 있었으나 이와같은 방법은 로보토의 구동개시의 구동종류시 직선이동의 정밀도가 저하된다는 문제점이 있었을 뿐만 아니라 고속의 직선이송시에는 구동개시점과 종료점에서 진동 및 충격이 발생한다는 문제점이 있었다.

또한, 로보트를 이용하여 용접 또는 페인팅 작업등을 하거나 로보트의 이동경로내에 장애물이 존재하는 경우 따위의 극히 제한된 작업환경 내에서는 주어진 경로를 따라가야만 하므로 작업효율이 저하한다는 문제점이 있었다.

따라서, 본 발명은 상기와 같은 문제점을 감안해서 이루어진 것으로써, 본 발명의 목적은 로보트의 직선 이송시 속도분포를 직선함수 및 지수함수적으로 가감속 함으로써 로보트의 구동개시와 구동종료시 직선이동의 정밀도를 향상시키고 진동 및 충격을 방지하는 로보트의 직선보간 방법을 제공하는데 있다.

본 발명의 다른 목적은, 직선함수 및 지수함수 가감속을 이산시간 상태방정식에 의해 실현함으로써 샘플링시간이 단축되므로 직선의 정밀도를 향상시킴과 동시에 고속의 직선이송이 가능한 로보트의 직선보간 방법을 제공하는데 있다.

상기 목적을 달성하기 위하여 본 발명에 의한 로보트의 직선보간방법은, 로보트의 관절 좌표계에서의 현재위치의 최종목표 위치로부터 기구학적 방정식에 의해 직각좌표의 X-Y 평면상에서의 현재위치와 목표위치를 구한후 직선의 길이를 계산하는 직선의 길이 계산스텝과, 직선의 이송속도 및 샘플링 시간을 이용하여 가감속이 없는 경우 매 샘플링 시간마다 이송해야할 이동거리를 구하는 이동거리 계산스텝과, 직선의 길이와 가감속이 없는 경우 매 샘플링 시간마다 이송해야될 거리를 토대로 이동횟수를 구하는 이동횟수 게산스텝과, X-Y 평면상의 직선의 속도분포를 직선함수 및 지수함수적으로 하기 위해 이산시간 상태 방정식을 이용하여 직선이송시 매 샘플링 시간마다의 직선의 증분양을 계산하고, 상기의 직선증분양의 계산시 발생하는 나머지 부분을 처리하는 나머지 부분처리 스텝과, 직선의 증분양을 이용하여 각각의 X, Y 축의 위치증분양을 계산하고, 로보트의 역기구학적 방정식에 의해 각 관절의 위치증분양(pulse)을 계산하여 임의의 시점에서의 각 관절의 목표위치를 연산한후 검출된 현재위치를 토대로 위치편차를 계산하는 위치편차 계산스텝과, 상기 위치편차에 비례 적분미분(PID) 제어 및 피드포워드제어를 행한후 서보모터의 구동신호로 출력하는 구동신호출력 스텝으로 이루어진 것을 특징으로 한다.

이하, 본 발명의 일실시예를 도면을 참조하여 상세히 설명한다. 제1도는 수평다관절 로보트의 X-Y 평면에서의 로보트의 앤드 이펙터의 직선이송 개념도이고, 제2도는 직각좌표상의 X-Y 평면상에서의 직선이송시 직선의 이송속도를 일정하게 한 경우의 그래프이고, 제3도는 직각좌표상의 X-Y 평면에서의 직선이송시 직선의 이송속도를 지수함수적으로 한 경우의 그래프이며, 제4도는 가감속이 없는 경우의 매 샘플링 시간마다 이송해야할 직선의 증분양을 도시한 그래프이며, 제5도는 직선이송시 직선함수 및 지수함수 속도분포도이다. 또한, 제6도는 본 발명의 로보트의 직선보간방법을 실현하기 위한 블록도이고, 제7도는 본 발명의 동작순서를 도시한 플로우차트이다.

제1도는 2개의 아암(a₁, a₂)을 가진 로보트가 직각좌표의 X-Y 평면에서 현재위치(C, (xi, yi))로 부터 목표위치(C, (xf, yf))까지 직선을 그리면서 이송하는 경우를 도시한 것으로써, 각각의 아암(a₁, a₂)을 구동하기 위한 서보모터(4)가 관절(A)가 관절(B)에 부착되어 있다.

또한, 제2도는 직각좌표상의 X-Y 평면에서 직선이송시 직선의 이송속도를 일정하게 한 경우에 매 샘플링 시간마다 이송해야될 직선의 길이를 도시한 것으로써, 직선이송시 직선의 증분양 사이는 포인트 투 포인트 동작이 이루어진다.

또한, 제3도는 직각좌표상의 X-Y 평면에서 직선이송시 직선의 이송속도를 직선함수 및 지수함수적으로 한 경우에 매 샘플링 시간마다 이송해야될 직선의 길이를 도시한 것이다.

이어서, 제6도를 설명한다. 제6도에 있어서, (1)은 속도명령을 출력하는 마이크로 프로세서이고, (2)는 상기 마이크로 프로세서(1)에서 출력된 디지털값의 속도명령을 아날로그 값으로 변환하는 디지털/아날로그 변환기(Digital/Analog converter)로써, 상기 변환된 아날로그값을 도시하지 않은 서보 드라이버에 입력하여 서보모터(4)의 정방향 및 역방향으로 구동하는 명령신호와 구동속도의 신호를 출력한다.

또한, (3)은 서보모터(4)의 전류궤한신호(Ix)가 궤한됨과 동시에 타코제너레이터(5)가 상기 서보모터(4)의 구동속도에 따라 검출한 속도궤한신호(Vox)가 궤한되어 서보모터(4)를 서보제어하는 서보제어부이며, (7)은 상기 서보모터(4)의 회전량에 따라 펄스신호를 출력하는 엔코더(6)로부터 출력된 펄스신호를 업카운트 또는 다운 카운트하여 로보트의 현재위치를 상기 마이크로 프로세서(1)에 입력하는 업/다운 카운터이고, (8)은 일정한 주기마다 상기 마이크로 프로세서(1)에 인터럽트를 걸어주기 위한 타이머로써 상기 각 장치는 한축에 대해서만 설명한 것이다.

다음에, 제7도를 제1도 내지 제6도와 연관하여 설명한다.

본 발명에 의한 로보트의 직선보간방법이 개시되면, 먼저 스텝 S1에서 마이크로 프로세서(1)는 제1도에 도시한 바와같이 수평다관절 로보트의 관절좌표에서의 현재값(θ₁, θ₂)을 업/다운 카운터(7)에 의해 읽어들이고, 스텝 S₂에서, X-Y 평면에서의 위치(xi, yi)를 다음과 같은 식을 이용하여 구한다. 즉,

이다. 여기서 a₁: 로보트의 제1임의 길이

a₂: 로보트의 제2임의 길이이다.

상기 X-Y 평면에서의 위치(xi, yi)가 구해지면 스텝 S₃로 나아가서 로보트의 관절좌표에서의 최종목표값(θ₃, θ₄)을 읽고 스텝 S₄에서, 상기 스텝 S₃에서 읽은 최종목표값(θ₃, θ₄)을 참고로하여 X-Y 평면에서 목표 위치값(xf, yf)을 구한다. 즉,

이에따라, 제2도에 도시한 바와같이 X-Y 평면에서의 각축의 위치편차를 스텝 S5에서 구하되, X축 방향의 이동량(xp)은

에 의해서 구하고, y축 방향의 이동량(yp)은

에 의해서 구한다.

상기 스텝 S5에서 X-Y 평면에서의 각축의 위치편차(xp, yp)가 구해졌으므로 상기 xp, yp 값을 이용하여 스텝 S6에서 직선의 길이(Sp)는

(여기서 I : 정수부분, R₁: 소수부분)에 의해 직선의 길이를 구하고, 스텝 S7로 나아가서 제4도에 도시한 바와같이 가감속이 없는 경우 매 샘플링 시간마다 이동해야할 직선의 길이(△Sp)를 구한다.

즉, 직선의 속도를 V[mm/sec]라 하고, 타이머(8)에 의해 마이크로 프로세서(1)에 추가적으로 인터럽트하는 시간 즉, 샘플링 시간을 Ts[sec]라 하는 경우 상기 가감속이 없는 경우에 매 샘플링 시간마다 이동해야할 직선의 길(△Sp)는 △Sp=V×Ts로 나타낼 수 있다.

이에따라 최종직선 위치까지 상기 △Sp로 이동해야할 횟수(N)는 N=에 의해 스텝 S8에서 구해진다(여기서 Q는 몫이고, R₂는 나머지이다.). 예를들어서, 직선의 길이(Sp)가 300.5[mm]이고, 직선의 이송속도(V)가 100[mm/sec]이고, 샘플링시간(Ts)이 4[msec]인 경우 상기 가감속이 없는 경우에 매 샘플링 시간마다 이동해야할 직선의 길이(△Sp)는 △Sp=V×Ts=100×100^-2=0.4[mm]이므로 △Sp로 이동해야 할 횟수(N)는 N=이다.

즉, 0.4mm씩 751번 이동해야 하고 752번째는 0.1mm만 이동하면 되는 것이다.

따라서 스텝 S10에서 직선 이송시의 속도가 구해지는데 매 샘플링 시간마다 계산하면 직선이송시의 속도 분포가 직선+지수함수적으로 된다. 즉, 제3도에 도시한 바와같이 지수 및 직선이송시의 속도(Vsp(k))는

(여기서 Vsp(0)=0, K=0, 1, 2, 3…이다.)

상기 식(8)에서 △Sp(k)는 K가 751번째 까지는 0.4mm이고 752번째는 0.1mm이며 753번째 이후는 제로이다.

또한, 상수 A는 지수함수의 시정수를 나타내는 상수로써, A가 너무 작으면 급속한 가감속이 되고, A가 너무크면 위치결정 시간이 길어지게 되므로 상기 상수 A의 값은 기계의 특성에 따라 적절하게 결정해야 한다.

또한 상기 n값도 직선가감속시 가속시간 및 감속시간을 나타내는 정수로서 기계의 특성에 따라 적절하게 선택하며, 지수함수 가감속시 시정수를 나타내는 A와 직선가감속시 가감속시간을 나타내는 n값의 조합에 의해 가감속 곡선의 형태가 바뀌므로 기계계의 특성에 따라 A값 및 n값을 적절하게 선택한다.

한편, 상기 스텝 S10에서 구해진 직선이송시의 속도(Vsp)는 나눗셈항에 있으므로 정확히 직선의 끝에 도달할 수 있도록 상기 나눗셈항에 대한 보상을 스텝 S11에서 수행한다.

즉, 임의의 K 시점에서 [△Sp(k)-Vsp(k-1)]/A의 나머지에 R(k)라 하면 k+1번째에 △Sp(k+1) 대신에 △Sp(k+1)+R(k)로 한후 K+1번째에 Vsp(K+1)을 계산하고, 임의의 K시점에서 S(K)/n의 나머지값을 R′(K)라 하면 K+1번째에 S(K) 대신 S(K)+R′(K)로 하여 fo(K)를 계산한다.

스텝 S11에서의 나눗셈항에 대한 보상후에는 스텝 S12로 나아가서 x축 및 y축의 이동거리를 구한다.

즉, X축의 이동거리(Xsp(k))는

y축의 이동거리(Ysp(k))는

이다. 이어서, 스텝 S13으로 나아가서 각 관절좌표계에서의 위치증분량(pulse)를 다음과 같이 구한다.

이에따라, 스텝 S14에서 임의의 K시점에서의 각축 마다의 목표위치가

및

에 의해 구해진다.

상기 스텝 S14에서 임의의 K시점에서의 각축마다의 목표위치가 구해졌으므로 스텝 S15에서는 임의의 K시점에서의 각축의 위치편차를 구한다. 즉,

(여기서 Cxθ(k), Cxθ(k)는 임의의 K시점에서의 각축마다 업/다운 카운터(7)에서 읽은 로보트의 현재위치 값이다)에 의해 구하는 것이다. 그러면, 상기 각축의 위치편차량에 대해 스텝 S10에서 비례적분미분(PID : Proportional, Integral, Differential)제어를 행한다.

여기서 T는 샘플링 시간이고, Dx out의 Dy out은 각축의 디지털/아날로그 변환기(2)로 입력되는 값이고, Kpx의 Kpy는 각축의 비례게인이며, Kix와 Kiy는 각축의 적분 게인이다.

또한, Kdx와 Kdy는 각축의 미분게인이며, KFx, Kfy는 각축의 피드포워드(feedfprward)게인이고, 상기 각축의 비례적분미분 게인 및 피드포워드게인은 로보트의 운동상태가 최적이 되도록 결정한다.

따라서 상기의 Dx out(k), Dy out(k)을 디지털/아날로그 변환기(2)에 출력하고 상기 디지털/아날로그 변환기(2)에 의해 아날로그 값으로 변환된 값이 서보제어부(3)에 입력되어 모터(4)를 구동하는 것이다.

이어서, 스텝 S17에서 K+1 값을 K에 대입한후 스텝 S18로 나아가 제4도에 도시된 바와같이 K의 값이 가감속이 없을 경우의 매 샘플링 시간마다의 직선의 길이(△Sp)의 횟수(N)와 동일한가의 여부를 판단하여 동일할 경우(Yes일 경우)에는 스텝 S19에서 가속이 없는 경우의 매 샘플링 시간마다의 직선의 길이(△Sp)의 횟수(N)를 구함으로써 얻어진 나머지(R₂)값을 구하고, 스텝 S20에서 K의 값이 매 샘플링 시간마다의 직선의 길이(△Sp)의 횟수(N)보다 큰가를 판단하여 클경우(Yes일 경우)에는 스텝 S21로 나아가서 직선의 길이(△Sp)에 제로를 대입하여 계산하고 스텝 S22에서 최종목표위치 인가를 판단한다.

상기 스텝 S22에서 최종 목표위치라고 판단되었을 경우(Yes일 경우)에는 로보트의 구동이 종료되는 것이며, 최종 목표위치가 아닐경우(No일 경우)에는 스텝 S10으로 복귀하여 차후의 동작을 반복한다.

한편, 상기 스텝 S18에서 K의 값이 매 샘플링 시간마다의 직선의 길이(△Sp)의 횟수(N)와 동일하지 않을 경우(K≠N, No일 경우)에는 직접 스텝 S20으로 나아가서 차후의 동작을 수행하며, 상기 스텝 S20에서 K의 값이 매 샘플링 시간마다의 직선의 길이(△Sp)의 횟수(N)보다 크지않을 경우(No일 경우)에는 역시 스텝 S22로 나아가서 로보트의 최종목표 위치인가를 판단한다.

한편, 상기 실시예는 수평다관절 로보트에 대한 것이지만, 본 발명은 직교로보트, 수직다관절 로보트에도 적용이 가능함은 물론이다.

이와같은 본 발명의 로보트의 직선보간 방법에 의하면, 직각좌표의 X-Y 평면에서 로보트의 아암종단부 즉 앤드이펙터가 직선을 따라 움직이면서 직선의 속도분포를 이산시간상태 방정식에 의해 직선함수 및 지수함수 가감속으로 실행되므로 샘플링 시간을 단축하고 직선정밀도가 향상될 뿐만 아니라 고속의 직선 이송시 구동개시점과 종료점에서 발생되는 로보트암의 충격과 진동을 억제함으로써 고속으로 직선이동이 가능하다는 뛰어난 효과가 있다.

Claims (5)

1. 로보트의 관절 좌표계에서의 현재위치의 최종목표 위치로부터 기구학적 방정시겡 의해 직각좌표의 X-Y 평면상에서의 현재위치와 목표위치를 구한후 직선의 길이를 계산하는 직선의 길이 계산스텝과, 직선의 이송속도 및 샘플링 시간을 이용하여 가감속이 없는 경우 매 샘플링 시간마다 이송해야될 이동거리를 구하는 이동거리 계산스텝과, 직선의 길이와 가감속이 없는 경우 매 샘플링 시간마다 이송해야될 거리를 토대로부터 이동횟수를 구하는 이동횟수 계산 스텝과, X-Y 평면상의 직선의 속도분포를 직선함수 및 지수함수적으로 하기위해 이산시간 상태 방정식을 이용하여 직선이송시 매 샘플링 시간마다의 직선의 증분양을 계산하고, 상기의 직선증분양의 계산시 발생하는 나머지 부분을 처리하는 나머지 부분처리 스텝과, 직선의 증분양을 이용하여 각각의 X, Y축의 위치증분양을 계산하고, 로보트의 역기구학적 방정식에 의해 각 관절의 위치증분양(pulse)을 계산하여 임의의 시점에서의 각 관절의 목표위치를 연산한후 검출된 현재위치를 토대로 위치편차를 계산하는 위치편차 계산스텝과, 상기 위치편차에 비례 적분미분(PID)제어 및 피드포워드제어를 행한후 서보모터의 구동신호로 출력하는 구동신호출력 스텝으로 이루어진 것을 특징으로 하는 로보트의 직선보간 방법.
2. 제1항에 있어서, 상기 샘플링시간은 가감속이 없는 경우 로보트가 이동해야될 직선 길이를 구하기 위해 마이크로 프로세서에 의해 주기적으로 인터럽트 되는 시간인 것을 특징으로 하는 로보트의 직선보간 방법.
3. 제1항에 있어서, 상기 비례적분미분제어 및 피드포워드 제어 게인은 로보트의 운동상태가 최적이 되도록 결정되는 것을 특징으로 하는 로보트의 직선보간 방법.
4. 제1항에 있어서, 상기 구동신호는 매 샘플링시간 마다의 직선 길이의 횟수의 임의의 구동 시정값이 동일할 경우의 디지털 데이타가 아날로그신호로 변환되어 서보제어부를 통해 모터에 인가되는 것을 특징으로 하는 로보트의 직선보간 방법.
5. 제4항에 있어서, 상기 매 샘플링 시간마다의 직선 길이의 횟수와 임의의 구동시점값이 동일하지 않을 경우에는, 임의의 구동시점 값이 직선길이의 횟수보다 클 경우에만 구동신호로 인가되는 것을 특징으로 하는 로보트의 직선보간 방법.