KR0155281B1

KR0155281B1 - 다관절 로보트의 직선보간방법

Info

Publication number: KR0155281B1
Application number: KR1019930021025A
Authority: KR
Inventors: 홍용준
Original assignee: 김광호; 삼성전자주식회사
Priority date: 1993-10-11
Filing date: 1993-10-11
Publication date: 1998-12-15
Also published as: KR950011071A

Abstract

본 발명은 다관절 로보트의 직선보간 방법에 관한 것으로, 특히 6관절 로보트의 3축의 관절좌표계상의 각도가 0인 경우 기구적인 특이성을 갖는데, 이러한 특이성을 갖는 위치에서 위치로 직선보간이동을 하는 경우에 관절의 변화율이 모터의 최대속도를 벗어나게 되는데, 상기와 같은 문제점을 해결하기 위하여 속도제한방법과 가·감속방법을 사용하여 직선보간 경유점을 보정함으로써 기구적 특이성이 존재하는 위치, 즉 자코비안(Jacobian)이 0이되는 위치에서도 직선보간이동이 이루어질 뿐만 아니라 직선보간 이동시 모터의 최대속도와 최대토크를 벗어나지 않으므로 소음이나 진동이 발생하지 않는 효과가 있다.

Description

다관절 로보트의 직선보간방법

제1도는 5관절 로보트의 좌표계를 도시한 사시도이고,

제2도는 제1도 5관절 로보트의 작업공간을 도시한 설명도로 (a)는 X-Y 평면상에 나타낸 설명도이고, (b)는 Y-Z 평면상에 나타낸 설명도이며,

제3도는 종래의 직선보간방법을 설명하기 위한 설명도로 (a)는 샘플링시간에 따른 직선보간 경유점을 나타낸 설명도이고, (b)는 (a)를 샘플링시간에 따른 상대거리값으로 나타낸 설명도이며,

제4도는 본 발명의 구성도이고,

제5도는 본 발명에 의한 직선보간방법을 설명하기 위한 설명도로 (a)는 A, B 두점사이의 등간격으로 설정된 직선보간 경유점을 나타낸 설명도이고, (b)는 (c)에 속도제한을 가한 후의 새로운 직선보간 경유점을 나타낸 설명도이며, (c)는 (b)에 가·감속을 가한 후의 새로운 직선보간 경유점을 나타낸 설명도이고,

제6도는 제5도의 (a)(b)(c)를 샘플링시간에 따른 속도의 변화상태로 나타낸 설명도이며,

제7도는 본 발명에 의한 동작흐름도이다.

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

10 : 키보드 20 : 모니터

30 : 주제어기 40 : 듀얼포트 램

50 : 위치제어기 60 : 서보 드라이버

본 발명은 다관절 로보트의 직선보간방법에 관한 것으로, 특히 작업영역내 로보트의 특이성이 존재하는 위치 즉, 자코비안(Jacobian)이 0이 되는 위치에서도 직선보간이동이 이루어지도록 된 다관절 로보트의 직선보간방법에 관한 것이다.

일반적으로, 제1도에 도시된 6관절 로보트에 있어서 3축의 관절좌표계상의 각도가 0°이거나 5축의 관절좌표계상의 각도가 0°인 경우에 기구적인 특이성을 갖는다.

제2도는 제1도에 도시된 5관절 로보트의 작업공간을 도시한 것으로 (a)는 X-Y평면상에 나타낸 것이고 (b)는 Y-Z 평면상에 나타낸 것으로, 상기와 같은 특이성을 갖는 위치(A)에서 위치(B)로 직선이동을 하는 경우에 관절의 변화율이 구동모터의 최대속도를 벗어나게 됨으로 상기의 직선이동이 용이하게 이루어지지 않는다는 문제점이 있다.

종래에는 상기와 같은 문제점을 해결하기 위하여 제3도의 (a)에 도시된 바와 같이 이동초기에는 단위시간당 이동속도를 증가하는, 즉 이동거리를 순차적으로 증가하는 가속이동을 하고 지정된 직선보간속도에 도달하면 일정구간 등속이동을 하게 되며 목표지점에 도달할 때에는 감속이동을 하는 가·감속 방법을 사용하였다.

그리고, 제3도의 (b)는 (a)를 샘플링시간에 따른 상대거리값으로 나타낸 설명도이다.

그러나, 상기와 같은 종래의 방법은 직선보간속도를 높였을 경우에 단위시간동안의 이동거리가 커짐으로 관절의 변화율이 구동모터의 최대속도를 벗어난다는 문제점이 여전히 잔재하게 된다.

또한, 특이성이 있는 위치에서의 속도변화가 불연속적으로 이루어지기 때문에 진동 및 소음이 발생한다는 문제점이 있다.

본 발명은 상기와 같은 문제점을 해결하기 위해 안출된 것으로, 기구적 특이성이 존재하는 위치(A)에서 위치(B)로 이동하는 경우에 일정한 직선보간속도에 따른 등간격의 직선보간 경유점을 구하고, 단위구간 이동속도가 모터의 최대속도를 초과하지 않도록 속도제한을 가하여 새로운 직선보간 경유점을 구한후 상기 새로운 단위구간 이동시 모터의 최대토크를 초과하지 않도록 가·감속방법을 사용하여 새로운 직선보간 경유점을 구하여 로보트 선단(End-effector)이 이 최종보간 경유점을 따라 이동하는 다관절 로보트의 직선보간방법을 제공하는 것을 그 목적으로 한다.

상기와 같은 목적을 달성하기 위하여 본 발명은, 출발위치와 목표위치의 위치데이타를 입력하는 위치데이타 입력단계와, 이 단계에서 입력받은 위치데이타를 통해 일정한 직선보간 속도에 따른 등간격의 직선보간 경유점을 산출하는 직선보간경유점 산출단계와, 이 단계에서 산출된 직선보간 경유점에 따른 관절의 변화율을 산출하는 관절의 변화율 산출단계와, 이 단계에서 산출된 관절의 변화율에 따라 산출된 모터속도가 제한속도를 초과하는 가를 판단하는 속도판단단계와, 이 단계에서 모터의 산출속도가 제한속도를 초과하는 경우에 속도제한을 가하는 속도제한단계와, 상기 속도판단단계에서 산출된 모터속도가 제한속도를 초과하지 않거나 속도제한을 가하여 제한속도를 초과하지 않으면 산출된 모터의 토크가 젠한토크를 초과하는 가를 판단하는 토크판단단계와, 이 단계에서 산출된 모터의 토크가 제한토크를 초과하는 경우에는 토크를 제한하는 토크제한단계와, 상기 토크판단단계에서 산출된 모터의 토크가 제한토크를 초과하지 않거나 토크제한단계를 통해 제한토크를 초과하지 않으면 산출된 현재의 위치로 로보트를 구동하는 로보트 구동단계와, 이 단계에서 로보트구동에 따른 현재위치와 목표위치가 일치하는가를 판단하는 위치판단계로 구성된 것을 특징으로 한다.

이하, 본 발명의 실시예를 첨부된 도면에 따라 상세히 설명한다.

본 발명은 제4도에 도시된 바와 같이, 시스템 전체를 제어하는 주제어기(30)와, 이 주제어기(30)의 출력명령에 따라 모터(M)의 위치를 제어하는 위치제어기(50)와, 이 위치제어기(50)의 출력명령에 따라 상기 모터(M)를 구동하는 서보 드라이버(60)로 구성되어 있으며, 상기 주제어기(30)와 위치제어기(50)는 듀얼포트 램(40)을 매개로 인터럽트 통신을 한다.

또한, 상기 주제어기(30)는 사용자와의 인터페이스를 위하여 키보드(10)와 모니터(20)로 연결되어 있다.

다음에는 상기와 같은 구성으로 이루어진 본 발명의 작용 및 효과를 상세히 설명한다.

시스템에 전원이 인가되면 주제어기(30)에 저장되어 있는 로보트제어용 시스템 프로그램이 로딩되고, 이에따라 사용자가 키보드(10)를 통해 주제어기(30)에 직선보간 이동할 위치, 예컨데 출발위치 A(x, y, z, α, β, γ)와 목표위치 B(x, y, z, α, β, γ)의 좌표를 입력하게 된다.

상기와 같이 입력된 위치데이타 A(x, y, z, α, β, γ)와 B(x, y, z, α, β, γ)를 통해 주제어기(30)는 제5도의 (a)에 도시된 바와 같은 등간격의 직선보간 경유점을 산출하게 된다.

즉, A(x, y, z, α, β, γ)와 B(x, y, z, α, β, γ)사이의 거리(1)는

직선보간 경유점의 수(n)는

상기 A를 원점으로 하는 좌표계로 나타내면,

단위 샘플링시간(ΔT)당 이동할 변화량(ΔP)은

샘플링시간(T1)에서의 위치는

인 등간격의 직선보간 경유점을 산출하게 된다.

그리고, 상기 Po와 Pi를 역기구 해석(Inverse Kinematic)하여 관절좌표계로 나타내면

단위시간당 관절의 변화율(ΔR1)은

상기의 단위시간당 관절의 변화율(ΔR1)을 각관절의 최대속도, MAX-VELOCITY(θ₁, θ₂, θ₃, θ₄, θ₅, θ₆)로 나누어 각관절의 변화율(Var_R)을 구하면

상기와 같이 산출된 각관절의 변화율(Var_R)중 최대변화율(θmax)이 1보다 큰경우 모터의 최대속도를 벗어나게 됨으로 속도제한을 가하게 된다.

즉, 최대변화율(θ_max) ＞ 1인 경우에는 관절의 변화율(ΔR_i)을

최대변화율(θ_max) ＜ 1인 경우에는

모터의 속도가 최대속도를 벗어나지 않는 새로운 직선보간 경유점을 제5도의 (b)와 같이 얻게 된다.

상기와 같이 속도제한을 가하여 얻은 직선보간 경유점으로 이동시 모터의 토크가 제한토크를 넘는가 판단하여 제한토크를 초과하는 경우에는 토크제한을 가하게 된다.

즉, 제한토크를 초과하는 경우에는 관절의 변화율(ΔR_i')을

이때, j ＜ 0 이면 ΔR_j= 0이고,

h(k)는 크기가 1인 n개의 실수배열이며,

N은 모터의 제한토크를 초과하지 않는 가·감속 시정수이다.

제한 토크를 초과하지 않는 경우에는

ΔRi = ΔRi' 로 하여,

모터의 토크가 최대토크를 벗어나지 않는 새로운 직선보간 경유점을 제5도의 (c)와 같이 얻게 된다.

상기 R_i(θ₁, θ₂, θ₃, θ₄, θ₅, θ₆)를 정기구(Forward Kinematic)해석하여 직각좌표계로 변화하면

현재의 직선보간 위치(Q)와 목표위치(B)와의 차(E(x, y, z))를 구하면

상기 E(x, y, z)가 목표위치에 도달하였음을 판단하기 위한 설정값보다 크며 목표위치에 도달하지 않았음을 판단하게 되고 이에따라 직선보간 경유점산출단계로 귀환하여 목표위치에 도달할때까지 상기의 과정을 반복수행하게 되며 설정값과 같으면 목표위치에 도달하였음을 판단하게 된다.

상기와 같이 본 발명은 기구적 특이성이 존재하는 위치(A)에서 위치(B)로 이동하는 경우에 일정한 직선보간속도에 따른 등간격의 직선보간 경유점을 구하고, 단위구간 이동속도가 모터의 최대속도를 초과하지 않도록 속도제한을 가하여 새로운 직선보간 경유점을 구한후 상기 새로운 직선보간 경유점으로 이동시 모터의 최대토크를 초과하지 않도록 가·감속방법을 사용하여 새로운 직선보간 경유점을 구하여 로보트가 이 최종 직선보간 경유점을 따라 이동하도록 함으로써 자코비안(Jacobian)이 0이 되는 위치에서도 직선보간이동이 이루어질 뿐만 아니라 직선보간이동을 할때 모터의 최대속도와 최대토크를 벗어나지 않으므로 소음이나 진동이 발생하지 않는 효과가 있다.

Claims

출발위치와 목표위치의 위치데이타를 입력하는 위치데이타 입력단계와, 이 단계에서 입력받은 위치데이타를 통해 일정한 직선보간 속도에 따른 등간격의 직선보간 경유점을 산출하는 직선보간경유점 산출단계와, 이 단계에서 산출된 직선보간 경유점에 따른 관절의 변화율을 산출하는 관절의 변화율 산출단계와, 이 단계에서 산출된 관절의 변화율에 따라 산출된 모터속도가 제한속도를 초과하는 가를 판단하는 속도판단단계와, 이 단계에서 모터의 산출속도가 제한속도를 초과하는 경우에 속도제한을 가하는 속도제한단계와, 상기 속도판단단계에서 산출된 모터속도가 제한속도를 초과하지 않거나 속도제한을 가하여 제한속도를 초과하지 않으면 산출된 모터의 토크가 제한토크를 초과하는 가를 판단하는 토크판단단계와, 이 단계에서 산출된 모터의 토크가 제한토크를 초과하는 경우에는 토크를 제한하는 토크제한단계와, 상기 토크판단단계에서 산출된 모터의 토크가 제한토크를 초과하지 않거나 토크제한단계를 통해 제한토크를 초과하지 않으면 산출된 현재의 위치로 로보트를 구동하는 로보트 구동단계와, 이 단계에서 로보트 구동에 따른 현재위치와 목표위치가 일치하는가를 판단하는 위치판단단계로 구성된 것을 특징으로 하는 다관절 로보트의 직선보간 방법.