KR0154433B1

KR0154433B1 - 로보트의 원호보간방법

Info

Publication number: KR0154433B1
Application number: KR1019930002872A
Authority: KR
Inventors: 송진일
Original assignee: 김광호; 삼성전자주식회사
Priority date: 1993-02-27
Filing date: 1993-02-27
Publication date: 1998-12-15
Also published as: KR940019417A

Abstract

본 고안은 2개 이상의 아암을 가지는 다관절형로보트에 있어서, 상기 아암의 손끝을 현재 위치에서 목표위치까지 원호를 따라 움직이게 할수 있도록 한것으로, 원호상의 각각의 원호점을 읽고, 읽어들인 원호점의 원호각도의 대소를 판단하며, 이 판단된 원호각도를 매시간마다 원호상좌표를 계산하고, 원호상좌표를 지수함수 가감속을 행하여 원호상좌표를 계산, 보상하며, 이 보상된 원호의 목표위치를 계산하는 단계와, 상기 단계에서 원호의 목표위치가 계산된 목표위치에서 위치편차를 구하고, 속도명령을 계산, 판단, 출력하여 원호좌표를 계산하는 단계와, 상기 단계에서 계산된 몫으로 속도명령을 계산, 출력, 소정횟수에 도달할때까지 최종명령을 출력하고, 최종목표에 도달하였는가를 판단, 완료하는 단계로하여 원호보간시 속도제한이 발생하였을 경우 원호의 이송속도를 변화시켜 속도명령이 제한값을 넘지 않게 제어하여 위치편차가 커지는 것을 방지함으로써 원호정도를 향상할수 있도록 한것임.

Description

로보트의 원호보간방법

제1도는 일반적인 X-Y 평면에서 원호보간시 X.Y 축의 속도분포를 나타낸 파형도.

제2도는 본 발명에 적용되는 직각좌표상에서 원호보간시 속도분포를 지수함수 가감속한 경우 매샘플링 시간마다 X-Y평면에서 움직여야할 증분량을 나타낸 좌표도.

제3도는 본 발명에 적용되는 가감속이 없는 경우 원호보간시 매샘플링 시간마다 X-Y평면에서 움직여야할 증분량을 나타낸 좌표도.

제4도는 본 발명에 적용되는 원호보간시 위치제어장치의 블럭도.

제5도는 본 발명에 적용되는 X-Y평면에서 원호보간시 가감속이 없는 경우 매샘플링 시간마다 원호를 계산하기 위한 좌표도.

제6도는 본 발명 원호보간방법에 대한 플로우챠트.

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

1 : 마이콤 2 : 디지탈/아날로그변환기

3 : 서보제어부 4 : 모타

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

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

본 발명은 로보트의 원호보간방법에 관한 것으로, 특히 2개이상의 아암(Arm)을 가지는 다관절형 로보트에 있어서, 상기 아암의 손끝(End-effictor)을 현재위치에서 목표위치까지 원호를 따라 움직이게 할수 있도록한 로보트의 원호보간방법에 관한 것이다.

일반적으로 조립용 로보트에서 현재위치에서 목표위치까지 이송시, 중간경로는 피티피(point-to-point)동작이 있으나, 로보트를 이용하여 용접, 페인팅 작업등을 수행하거나, 로보트의 이송경로내에 장애물이 존재하는 경우와 같이, 극히 제한된 작업 환경내에서는 주어진 경로를 따라가야 하는 씨피(continuous path) 동작이 필요하고, 일반적으로 직선및 원호씨피가 많이 사용되고 있다.

따라서 상기 로보트를 이용하여 원호보간시, 제1도에 도시한 바와같이 X-Y 평면에서의 원에 따른 X.Y 축의 속도분포(Vx, Vy)는

로 표시된다.

여기서 A는 원호보간시 각축의 최대속도를 나타낸다.

그러므로 제1도에서 원호의 시작점으로 부터 Y축은 sin 함수형태로 속도가 변하기 때문에 진동이 발생하지 않으나, X축의 속도는 cos 함수형태로 변하기 때문에 속도가 영(ZERO)에서 최고속도 A까지 급격히 변하여 진동이 발생되고, 마찬가지로 원호의 임의의 점에서 (0 ~ 360도 사이의 임의의 점) 정지시 상기 식 1의 속도에서 X.Y축의 속도가 영(ZERO)이 되어야 하므로 마찬가지로 진동이 발생한다.

따라서 저속의 원호보간인 경우 X.Y 축의 최대속도 A가 작기 때문에, 진동발생량이 적어 원호의 시작과 끝에서 원호의 정도가 떨어지지만 고속의 원호보간에서는 진동량이 크고 원호의 왜곡도 심하게 되어 사용이 불가능한 문제점을 가지고 있고, 일반적으로 로보트에 주로 사용되는 서보계에서는 서보모타에 가하는 토오크의 제한(LIMIT)을 두어 모타의 파손을 방지하고 있고, 따라서 위치제어 루프에서 발생하는 속도명령에도 제한을 두고 있다.

그러나 원호보간시 특히 관절형 로보트인 경우 역기구학적특성(inverse kinematic)에 의해 일정시간동안(샘플링시간동안)에 X-Y 좌표상에서는 이동거리는 짧으나, 관절에 부착된 모타의 이동거리를 길게되는 경우가 발생한다.

그러므로 샘플링시간동안 이동해야할 거리가 어떤 정해진 거리를 넘는 경우에는 속도명령을 제한하여야 하기 때문에 위치편차는 증가하게 되어(예를들어 속도제한이 없는 경우에는 1000 이라는 속도명령을 가해야하나 제한때문에 800밖에 속도명령을 가하지 못할경우 모타가 주어진 속도를 추종하는데 지연이 생겨 위치편차가 증가한다) 원호이송시 원호상에서 왜곡되는 현상이 발생하는 문제점을 가지고 있었다.

따라서 본 발명의 목적은 로보트의 원호보간방법에 있어서, 원호보간시 속도명령이 속도제한에 걸려 위치편차가 증가하므로 해서 야기되는 원호의 정도가 떨어뜨리는 것을 방지할수 있는 원호보간방법을 제공하고자 하는데 있으며, 본 발명의 또 다른 목적은 원호보간시 속도제한이 발생할 경우 원호의 이송속도를 변화시켜 속도명령이 제한값을 넘지않게 제어함으로써 원호보간시 정도를 향상시킬수 있는 원호보간방법을 제공하고자 하는데 있다.

상기의 목적을 실현하기 위하여 본 발명은 원호상의 원호점을 읽고, 읽어들인 원호점의 원호각도의 대소판단하여, 이 판단된 원호각도를 매시간마다 원호상좌표를 계산하고, 원호상좌표를 지수함수가 감속을 행하여 원호상 좌표를 계산 보상하고 이 보상된 원호의 목표위치를 계산하는 단계와, 상기 단계에서 원호의 목표위치가 계산된 목표위치에서 위치편차를 구하고 속도명령을 계산, 판단, 출력하여 원호좌표를 계산하는 단계와, 상기 단계에서 계산된 몫으로 속도명령을 계산, 출력, 소정횟수에 도달할때까지 최종 명령출력하고, 최종목표에 도달하였는가를 판단, 완료하는 단계로 이루어짐을 특징으로 한다.

이하 첨부된 도면에 의거 본 발명을 상세히 설명하면 다음과 같다.

제2도는 본 발명에 이용되는 직각좌표상에서 원호보간시 속도분포를 지수함수 가감속한 경우, 매샘플링 시간마다 X-Y 평면에서 움직여야할 증분량을 나타낸 좌표도.

제3도는 제2도에서 가감속이 없는 경우의 원호보간시 매샘플링 시간마다 X-Y평면에서 움직여야 할 증분량을 나타낸 좌표도.

제4도는 본 발명 원호보간제어방법에 적용되는 위치제어장치의 블럭도로서, 소정의 프로그램에 의하여 속도명령을 출력하는 마이콤(1)과, 상기 마이콤(1)에서 출력된 디지탈값의 속도명령을 아날로그 값으로 변환하는 디지탈/아날로그변환기(2)와, 상기 디지탈/아날로그변환기(2)로부터 아날로그속도 명령값을 받고, 모터(4)에 흐르는 전류(IX)와, 상기 모터(4)의 속도에 비례하는 전압(VOX)을 출력하는 타코제너레이터(5)로 부터 피드백하여 상기 모터(4)에 흐르는 전류와 속도제어를 하는 서보제어부(3)와, 상기 모터(4)의 회전에 따라 펄스를 발생하는 인코더(6)와, 상기 인코더(6)에서 발생한 펄스를 증감 카운트하여 상기 마이콤(1)에 모터의 현재위치를 입력하는 업,다운카운터(7)로 구성된다.

도면중 미설명부호 8은 타이머이다.

제5도는 본 발명 X-Y평면에서 원호보간시 가감속이 없는 경우 매샘플링 시간마다 원호를 계산하기 위한 개념도이고, 제6도는 본 발명 원호보간방법의 제어플로우챠트로서, 원호상의 시작점, 중간점, 끝점의 방향을 읽어들이는 원호점 읽기단계(60)와, 상기 원호점읽기단계(60)에서 읽어들인 원호점의 원호각도와 미소원호각도의 대소를 판단하는 원호각판단단계(61)와, 상기 원호각판단단계(61)에서 판단된 상태에 따라 매샘플링 시간마다 원호상좌표를 계산하는 제1원호좌표계산단계(62)(63)와, 상기 원호좌표계산단계(62)(63)에서 계산된 원호좌표의 각축을 지수함수 가감속을 행하여 원호상의 좌표를 계산하는 제2원호좌표계산단계(64)와, 상기 제2원호좌표계산단계(64)에서 계산된 원호좌표의 나눗셈항을 보상하는 원호좌표보상단계(65)와, 상기 원호좌표보상단계(65)에서 보상된 원호의 각축목표위치를 계산하는 원호의 목표위치계산단계(66)와, 상기 원호의 목표위치계산단계(66)에서 현재위치까지 위치편차를 계산하는 위치편차계산단계(67)와, 상기 위치편차계산단계(67)에서 각축의 속도명령을 계산하는 속도명령계산단계(68)와, 상기 속도명령계산단계(68)에서 계산된 속도명령과 제한명령과의 대소를 판단하는 속도명령판단단계(69)와, 상기 속도명령판단단계(69)에서 상기 대소조건이 맞으면 속도명령을 출력하는 속도명령출력단계(70)와, 대소조건이 맞지 않으면 상기 제2원호좌표계산단계(64)에서 원호좌표를 미리 정해진 값으로 나눈후 몫을 구하는 제3원호좌표계산단계(71)와, 상기 제3원호좌표계산단계(71)에서 계산된 몫으로 각축의 속도명령을 계산하는 속도명령계산단계(72)와, 상기 속도명령계산단계(72)에서 계산된 속도명령을 출력하는 속도명령출력단계(73)와, 상기 속도명령출력단계(73)에서 출력된 명령횟수를 1씩증가하고 출력명령이 소정횟수에 도달하였는가를 판단하는 출력명령소정횟수판단단계(74)와, 상기 출력명령소정횟수판단단계(74)에서 출력명령이 소정횟수 도달하였으면, 최종목표 출력명령인가를 판단하여 최종목표출력이면 완료하고, 최종목표출력이 아니면 제1원호좌표계산단계(63)로 부터 반복수행하는 최종목표출력명령판단단계(75)로 이루어지도록 한 것이다.

상기와 같이 이루어진 본 발명의 작용효과를 설명하면 다음과 같다.

먼저 제5도에 도시한 바와같이 1/2원을 그리는 경우에 대하여 설명하면, 일반적으로 원호보간을 하기 위해서는 원호상의 3개의 점이 주어지고, 방향(CW)(CCW)이 주어지며, 상기 1/2원호에 대하여 방향(CCW)으로 원호보간을 행하면 주어진 3점을 각각 A(X₁)(Y₁), B(X₂, Y₂), C(X₃, Y₃)으로 되고, 상기와 같이 주어진 3점에 대하여 제4도의 마이콤(1)에서는 상기 원호점 읽기단계(60)로가서 원호상의 시작점 A(X1,Y1), 중간점 B(X2,Y2), 끝점 C(X3,Y3)을 읽어들이고 상기 마이콤(1)에서는 원호각 판단단계(61)로가서 원호보간을 실행중 원호의 각도를 θ라 하고, 미소원호각도를 △θ라하면, 상기 원호의 각도 θ=Σ△θ(i)라고 할때, 상기 마이콤(1)에서는 θ-Σ△θ(i) △θ인가를 판단하게 되며, 이때 상기 조건에 만족하지 않으면, 가감속이 없으면 상기 마이콤(1)에서는 제1원호좌표계산단계(63)로가서 현재위치 A(X₁,Y₁) 에서 이동해야 할 원호상의 점을 아래와 같이 구한다.

따라서 원호상의 2번째점은 아래와 같이 구해진다.

따라서 (2)식을 이용하여 (3)식을 다시 쓰면 다음과 같다.

여기서 COS △θ. SIN △θ 는 △θ가 일정하기 때문에 일정값을 가지므로

로 놓고 (4)식을 일반적으로 쓰면 아래와 같이 된다.

여기서 K 는 샘플링 시간마다 증가된다.

여기서 X(0)는 원호의 시작점의 X좌표 X₁이고, Y(0) 는 원호의 시작점이 Y 좌표 Y₁이다.

따라서 가감속이 없는 경우 상기 (6)식에 의해 원호상의 점(중감목표위치)를 구할수 있고, X-Y 평면에서 제3도와 같은 형태가 된다.

상기와 같이 제1원호좌표계산단계(63)에서 원호상좌표가 계산되면 상기 마이콤(1)에서는 제2원호좌표계산단계(64)로 가서 상기 계산된 원호상좌표를 원호상에서의 X.Y축의 위치 증분량을 구한후 아래의 식에 의해 각축의 원호상의 좌표값을 구하면 X-Y 평면상에서의 원의 속도분포가 제4도에 도시한 바와같이 지수함수 가감속형태가 된다.

여기서 A는 정수이며, A가 크면 지수함수의 시정수가 크게되어 가감속시간이 길어지고, A가 작으면 지수함수의 시정수가 작아져서 가감속시간이 짧아진다.

상기 제2원호좌표계산단계(64)에서 원호상에서 X.Y 축의 원호상의 좌표값이 구해지면 상기 마이콤(1)에서는 원호좌표보상단계(65)로가서 상기 구해진 원호상의 좌표값에서 나눗셈항을 보상한다.

즉 (K+1) = X(K+1) + R1(K)

(K+1) = Y(K+1) = R2(K) 가 된다.

상기와 같이 좌표값에 보상이 되면, 상기 마이콤(1)에서는 원호의 목표위치계산단계(66)로가서, 매샘플링 시간마다 임의 시점에서의 원호보간을 위해 각축의 목표위치를 아래와 같이 계산한다.

상기 원호의 목표위치계산단계(66)에서 각축목표위치계산이 완료되면, 상기 마이콤(1)에서는 위치편차계산단계(67)로 가서, 각축의 업,다운카운트(7)로 부터 현재위치를 읽어 각축의 현재위치값을 Cx(K), Cy(K)라 하면, 임의의 K 시점에서 X.Y 축의 위치편차 Pεx(K). Pεy(K)는 아래와 같이 계산된다.

상기 위치편차계산단계(67)에서 구해진 위치편차에 대하여 상기 마이콤(1)에서는 속도명령계산단계(68)로가서 상기 각축의 위치편차에 대하여 비례적분, 미분과 피드포워드제어를 아래와 같이 한다.

여기서 T 는 샘플링시간을 나타내고 Dx(K), Dy(K)는 X.Y축의 D/A 변환기(2)로 입력되는 값이고, Kpx, Kpy 는 각축의 비례(P)게인이며, Kix, Kiy는 각축의 적분(I)게인이다.

또한 Kdx, Kdy는 각축의 미분(D) 게인이며, Kfx, Kfy는 각축의 피드포워드(F) 게인이다.

각축의 비례적분 미분, 피드포워드게인은 서보모타에 부착된 기구부하의 운동상태가 최적이 되도록 결정한다.

상기와 같이 각축의 속도명령이 구해지면, 상기 마이콤(1)에서는 속도명령판단단계(69)로 가서 상기 속도명령계산단계(68)에서 계산된 속도명령(Dx)(Dy)에 대하여 속도제한명령(VxLim), (VyLim)과 대소를 판단하되, 속도명령(Dx)가 속도제한명령(VxLim)보다 작은가, 속도명령(Dy)가 속도제한명령(VyLim)보다 작은가를 판단하여 상기 조건에 만족하면 속도명령출력단계(70)로 가서 상기 속도명령계산단계(68)에서 계산된 속도명령을 상기 D/A변환기(2)로 출력하게 되며, 상기 조건에 만족하지 않으면, 제3원호좌표계산단계(71)로가서 상기 제2원호좌표계산단계(64)에서 정해진 Fox(K) Foy(K), 원호좌표값을 미리 정해진 n 값으로 나눈후 속도명령계산단계(72)로 가서 그 값을 상기 식(9)에 n 번 더하여 각축의 속도명령을 구하고, 속도명령출력단계(73)로가서 상기 디지탈/아날로그변환기(2)로 가서 상기 각축의 속도명령을 출력한다.

상기 속도명령이 출력되면 출력될때마다 카운터를 1씩증가시키면 출력명령소정횟수판단단계(74)로가서 출력명령이 소정횟수에 도달할때까지 상기 제1원호좌표계산단계(63)로부터 반복수행하고, 상기 소정의 횟수에 도달하였으면 상기 마이콤(1)에서는 최종 목표출력명령판단단계(75)로가서 최종목표인가를 판단하여 최종목표이면 완료하게 되고 상기 원로각판단단계(61)에서 θ-Σ△θ(i) △θ의 조건에 만족하면, 원호좌표계산단계(62)로가서

X(K+1) = COSθ

Y(K+1) = SINθ 으로 원호좌표를 계산하고, 상기 제2원호좌표계산단계(64)로가서 원호상의 나눗셈 때문에 생기는 에러를 보상하여야지만 정확한 원호보간이 이루어지며 즉 X축에 대하여 [K(K+1) - Fox(K)] /A 를 실행하였을때 몫을 Q1(K) 나머지를 R2(K)라 하면 K+1번째에는 X(K+1) 대신 K(K+1)+R1(K)로 대치하여 상기 (7)식을 계산하고 [Y(K+1) - Foy(K)] /A 을 실행했을때 몫을 Q2(K), 나머지를 R2(K)라 하면 K+1 번째에는 Y(K+1) 대신 Y(K+1)+R2(K)로 대치하여 상기 (8)식을 계산하면서 상기 과정을 수행하게 된다.

이상에서 설명한 바와같이 본 발명은 원호보간방법에 있어서, 원호보간시 속도제한이 발생하였을 경우 원호의 이송속도를 변화시켜 속도명령이 제한값을 넘지않게 제어하여 위치편차가 커지는 것을 방지할수 있게 함으로써, 직각좌표의 X-Y 평면에서 로보트의 손끝이 원호를 따라 움직이며 원호의 속도분포가 지수함수형태가 되어, 원의 시작과 끝에서도 진동이 발생하지 않고 부드러운 동작이 가능하며, 이산시간상태 방정식에 의해 지수함수 가감속을 행하기 때문에 샘플링 시간을 줄일수 있어 원호의 정도를 향상시킬수 있는 장점을 제공할수 있는 것이다.

Claims

로보트의 원호보간방법에 있어서, 원호상의 각각의 원호점을 읽고, 읽어들인 원호점의 원호각도의 대소를 판단하며, 이 판단된 원호각도를 매시간마다 원호상좌표를 계산하고, 원호상좌표를 지수함수 가감속을 행하여 원호상좌표를 계산, 보상하며, 이 보상된 원호의 목표위치를 계산하는 단계와, 상기 단계에서 원호의 목표위치가 계산된 목표위치에서 위치편차를 구하고, 속도명령을 계산, 판단, 출력하여 원호좌표를 계산하는 단계와, 상기 단계에서 계산된 몫으로 속도명령을 계산, 출력, 소정횟수에 도달할때까지 최종명령을 출력하고, 최종목표에 도달하였는가를 판단, 완료하는 단계로 이루어지는 것을 특징으로 하는 로보트의 원호보간방법.