KR100540588B1

KR100540588B1 - 모션 제어의 실시간 응답을 위한 궤적발생방법 및 시스템

Info

Publication number: KR100540588B1
Application number: KR1020030098264A
Authority: KR
Inventors: 하영열; 한성호; 우갑주
Original assignee: 삼성중공업 주식회사
Priority date: 2002-12-27
Filing date: 2003-12-27
Publication date: 2006-01-10
Also published as: KR20040060825A

Abstract

모션 제어의 실시간 응답을 위한 궤적발생방법 및 시스템은 로봇제어시스템등에서의 모션 제어에 있어 이동중 실시간으로 변경된 위치 또는 속도의 변경이 반영되도록 하기 위한 것이다. 본 발명은 위치궤적을 초기속도가 "0"일 경우는 가속구간, 등속구간, 감속구간으로 나누고 초기속도가 "0"이 아닐 경우에는 브랜딩구간, 등속구간, 감속구간으로 나누어 가속구간과 감속구간에서는 3차 다항식으로 속도파형을 구하고, 등속구간은 상수값을 이용하며, 브랜딩구간에서는 브랜딩 속도파형과 가속속도파형을 합하는 형태로 하며, 브랜딩 속도 파형은 경우에 따라 4차 또는 5차의 다항식을 이용하여 속도파형을 구하며, 따라서, 본 발명은 사용하는 다항식의 계수 구하는 식이 간단하여 계산시간을 적게 소모할 수 있어 외부사건에 실시간으로 응답하여야 하는 로봇시스템에서 이동중에 속도 또는 위치 명령의 변경을 비교적 느린 CPU에서도 실시간 반영할 수 있는 효과를 제공한다.

실시간, real time, 모션제어, motion control, 궤도발생, trajectory generation, 속도프로필, velocity profile

Description

모션 제어의 실시간 응답을 위한 궤적발생방법 및 시스템{Trajectory generation method and system for real-time response at motion control}

도 1은 본 발명에 따른 선형/부드러운 방식의 속도 프로파일을 나타낸 그래프를 도시한 도면.

도 2는 초기 가속도(a₀)와 속도(v₀)에 따른 속도 프로파일을 나타낸 그래프를 도시한 도면.

도 3은 본 발명에 따른 모션제어 실시간응답을 위한 궤적발생시스템을 나타내는 시스템 구성도.

도 4는 본 발명에 따른 도 3 시스템의 실시간 모션 제어 처리 모델을 보여주는 상태도.

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

100 : 상위제어기(CPU) 200 : 외부하드웨어

10 : 미들웨어 20 : 모션제어기

21 : 커맨드디코더 22 : 궤적발생부

23 : 역운동학계산부 25 : 인터페이스제어부

26 : 운동학계산부 30 : 다축위치제어부

40 : 모터구동부 50 : 모터

본 발명은 로봇제어시스템 등에서의 모션 제어에 관한 것으로, 보다 상세하게는, 위치 브랜딩(blending) 방법을 통해 이동 중 실시간으로 변경된 위치 또는 속도에 실시간 응답이 가능한 궤적을 발생하여 모션 제어에 반영시켜 외부 사건에 실시간으로 응답이 가능토록 한 모션 제어의 실시간 응답을 위한 궤적발생방법 및 시스템에 관한 것이다.

최근 산업현장에는 다양한 종류의 로봇이 사용되고 있으며, 이들 중에는 철판 등의 각종 재료를 용접하는 용접용 로봇이 있다. 이러한 산업용 로봇 시스템은 로봇의 모션(motion)제어를 통해 이동경로를 변경한다. 모션 제어에서 위치 이동은 입력된 목표거리, 목표 속도 이외에 액츄에이터와 로봇의 기계적 한계에 따른 가속도, 감속도, 최고속도의 구속 조건으로 이동시간이 결정된다. 이동 중 속도나 위치의 변경을 하기 위한 기존의 방법으로는 고차 다항식을 이용하는 방법과 속도 브랜딩 방법이 있다. 고차 다항식을 이용하는 방법은 변경하고자 하는 시점에 다항식의 계수를 구하고, 구해진 계수로서 샘플링 시간마다 다항식의 해를 이용하여 이동해야 할 목표점을 구하는 방법이다. 이 방법은 다항식의 계수를 구함에 있어 복잡하고, 많은 계산량을 가진다는 단점이 있다. 속도 브랜딩을 이용한 방법은 계수가 정하여진 다항식을 이용하여, 감속과 가속을 합하는 형태의 방법인데, 이것의 위치 브랜딩 적용에 있어 목표위치를 지정할 수 없다.

그런데, 다양한 센서로부터 즉각적인 반응을 보여야 하는 로봇시스템 등에서 모션 제어시 실시간 응답이 중요한 의미를 가진다. 위치 이동에 따른 모션 제어의 실시간 응답을 위해 종래에는 초기 속도와 가속도가 "0"이 아닌 경우의 속도 프로파일을 만드는 방법이 제시되었다.

이는 초기위치에서 최종위치로의 이동에 있어 초기 속도와 가속도, 최종 속도와 가속도를 가지기 위해 리프트-오프(lift-off)위치와 셋-다운(set-down)위치를 기준으로 3개의 구간으로 나눈다. 이들 구간 사이의 연속적인 속도, 가속도를 가지는 방법은 4-3-4궤적방식과 5개의 큐빅스플라인(cubic spline)을 이용하는 방식이 있다. 이들 방법은 초기 속도와 가속도, 최종 속도와 가속도가 "0"이 아닌 경우의 속도 프로파일 생성 때문에 고차 다항식 처리에 있어 비교적 많은 계산량을 가진다.

물론, 기존에도 실시간 응답을 위한 궤적 발생에 대한 연구가 몇가지 있다. 그 일예로, Volpe의 "Task Space Velocity Blending for Real-Time Trajectory Generation" IEEE International Conference on Robotics and Automation, May 2-6 1993, Atlanta Georgia.와 Loyd의 "Trajectory Generation for Sensor-Driven and Time-Varying Tasks", International Journal of Robotics, August, 1993. 두 문헌이 있다. 이들 문헌에 언급된 방법은 초기 가속도가 "0"이 아닐 경우에는 적용할 수 없으며, 작업 공간에서 목표 위치 변경에 따른 궤적 발생에 대해서는 기재되어 있지 않다.

따라서, 본 발명의 목적은 기존의 다항식 방법보다 간단한 위치 브랜딩 방법을 이용하여 목표위치 또는 목표 속도 변경에 실시간 응답이 가능한 궤적 발생 알고리즘을 통해 이동 중 실시간으로 변경된 위치 또는 속도의 변경을 모션 제어에 반영할 수 있도록 하며 초기 가속도가 "0"이 아닐 경우에도 적용할 수 있는 모션 제어의 실시간 응답을 위한 궤적발생방법 및 시스템을 제공함에 있다.

이와 같은 목적을 달성하기 위한 본 발명에 따른 모션 제어의 실시간 응답을 위한 궤적발생방법은, 소정의 작업시작점과 작업종료점 사이의 작업경로상에서 작업을 수행하는 로봇의 이동경로를 실시간을 변경하기 위한 방법에 있어서, (1) 초기 속도가 "0"인 경우 가속, 등속 및 감속의 세구간으로 나누고, 초기 속도가 "0"이 아닌 경우 브랜딩, 등속 및 감속의 세구간으로 나누어 구간을 설정하는 단계와, (2) 상기 설정된 세구간중 가속구간과 감속구간에서는 3차 다항식을 사용하고, 등속구간에서는 상수 속도를 사용하여 최종 위치로의 이동을 위한 속도 프로파일을 구하는 단계와, (3) 상기 설정된 세구간중 브랜딩구간에서는 주어진 변화시간 동안에 초기 속도에서 속도를 "0"으로 만드는 속도 프로파일과 상기 단계 (2)에서 구해진 속도 프로파일을 브랜딩하여 속도 프로파일을 구하는 단계와, (4) 상기 구해진 궤적에 따라 이동중 제한된 시간 내에 위치 및 속도 변경을 실시간 반영하여 모션 제어하는 단계를 포함한다.

본 발명의 목적을 달성하기 위한 모션 제어의 실시간 응답을 위한 시스템 은, 로봇의 작업수행중 이동경로를 실시간으로 변경할 수 있는 모션제어 시스템에 있어서, 로봇 조작자로부터의 로봇 제어명령을 입력받아 전달해주는 미들웨어와, 로봇의 모션제어를 수행하는 모션제어기를 구비하며, 초기 유휴(DORMANT)상태에서 모션제어 인에이블/디스에이블 및 이동의 사건이 발생하면 토큰이 발생하여 해당 작업을 수행하도록 제어하는 상위제어기(CPU), 및 상기 상위제어기의 제어에 따라 축들 위치 제어를 수행하는 다축위치제어부를 구비하는 외부하드웨어를 포함하며, 상기 모션제어기는 미들웨어와의 인터페이스를 수행하여 명령이 전달되면 해독 작업을 수행하여 축 단위 이동인지, 월드 좌표계에 대한 이동인지를 구별하는 커맨드디코더와, 상기 외부하드웨어에서 인터럽트 사건이 발생하면 상기 커맨드디코더에서 구별된 명령을 사전 처리하여 로봇의 가속, 감속 및 등속에 관한 속도 프로파일을 생성하여 궤적을 계산하는 궤적발생부와, 로봇의 다수개의 축값을 계산하는 역운동학계산부와, 축값으로부터 월드좌표값을 계산하는 운동학계산부, 및 다축위치제어부와의 인터페이스를 수행하여 목표위치값을 전달하는 인터페이스제어부를 구비한다.

이하, 첨부한 도면들을 참조하여 본 발명의 바람직한 실시예를 상세히 기술하기로 한다.

우선, 위치 이동에 따른 모션 제어의 실시간 응답이 가능하게 하기 위해서는 다음의 세가지 조건을 만족해야 한다.

첫째, 사건(event)이 발생한 임의의 순간의 위치, 속도, 가속도의 정보를 알아야 한다. 즉, 사건이 발생한 순간의 위치, 속도, 가속도를 알아야 하는 이유 는 부드러운(smooth) 이동을 위해 초기 위치에서의 연속적인 속도와 가속도를 가지는 속도 프로파일을 만들기 위해 필요한 정보이기 때문이다.

둘째, 초기속도가 "0"이 아닌 경우의 주어진 목표 위치 또는 목표 속도에 따른 속도 프로파일을 만들 수 있어야 한다.

셋째, 궤적 발생 알고리즘을 실시간으로 처리할 수 있는 제어방법이 있어야 한다.

본 발명은 초기 속도와 가속도가 주어지고, 최종 속도와 가속도가 "0"인 경우의 속도 프로파일을 생성하기 위해 초기 속도와 가속도에 따라 비교적 적은 계산량을 가지는 다항식을 선택한다. 즉, 초기 속도와 가속도가 "0"일 경우와 초기 속도와 가속도가 "0"이 아닌 경우로 나누어 처리한다. 초기 속도와 가속도가 "0"일 경우의 속도 프로파일은 가속구간, 등속구간, 감속구간으로 나누어 처리하며, 가속구간과 감속구간에서는 3차 다항식으로 속도 프로파일을 구하고, 등속구간에서는 상수속도를 이용하여 속도 파형을 구한다.

초기 속도와 가속도가 "0"이 아닐 경우에는 브랜딩구간, 등속구간, 감속구간으로 나누어 처리하며, 브랜딩구간에서는 주어진 변화시간 동안에 초기속도에서 속도를 "0"으로 만드는 속도 프로파일과 최종위치로의 이동을 위한 가속구간 속도프로파일을 브랜딩한다. 초기속도에서 속도를 "0"으로 만드는 속도프로파일 생성방법은 4차 또는 5차 다항식이 사용된다. 최종 위치로의 이동을 위한 속도프로파일 생성 방법은 가속구간과 감속구간에서는 3차 다항식이 사용되고, 등속구간에는 상수속도를 이용한다. 따라서, 초기속도가 "0"이 아닐 경우에는 브랜딩구간과 가 속구간이 비대칭적인 형태를 지닌다.

먼저, 축 공간에 있어 궤적을 변경하고자 하는 초기속도가 "0"인 경우와 그렇지 않은 경우로 나누어 축 좌표 괘적 발생에 대해 설명한다.

초기속도가 "0"인 경우의 속도 파형 생성 방법은 다음과 같다.

이동거리(So), 가속도의 크기(aa), 감속도의 크기(ad), 목표속도의 크기(Vd)에 따른 연속적인 속도를 제공하기 위한 궤적(S_n(t)) 생성 방법은 다음의 수학식 1과 같다.

여기서, f_acc(s)는 가속의 형태를 결정하는 정규화된(normalized) 특성함수이고, f_dec(s)는 감속의 형태를 결정하는 정규화된 특성함수이다. s는 가속시간, 감속시간에 대한 정규화된 시간파라메터로 s ∈ [0.1] 이다. Sa, Su, Sd는 가속구간, 등속구간, 감속구간에서 움직이는 거리이고, ta, tu, td는 가속, 등속, 감속이 끝나는 시간을 나타낸다.

본 실시예에서, 가속구간과 감속구간에서 움직이는 거리(Sa,Sd)는 가속이 끝나는 시간(ta)과 감속이 끝나는 시간(td)에서 등속이 끝나는 시간(tu)을 뺀 시간(td-tu)을 등속으로 움직이는 거리(Su)의 1/2인 특성을 가지는 선형적인 방식과 부드러운방식을 사용한다.

주어진 이동거리(So), 가속도 크기(aa), 목표속도 크기(Vd), 감속도 크기(ad)로 가능한 최고속도(Vm)을 하기 수학식 2처럼 구한다.

위의 수학식 2에 의해 구해진 최고속도(Vm)를 이용하여 가속구간, 등속구간, 감속구간에서의 움직이는 거리(Sa,Su,Sd) 및 끝나는 시간(ta,tu,td)를 알 수 있다. 이를 수학식 3에 정의한다.

여기서, d는 모션 제어에서 이동거리(So)의 부호(sign)와 관계없이 목표속도(Vd)를 입력받기 위해 최고속도(Vm)의 부호를 결정하는 값으로, 다음 수학식 4로 정의된다.

위치 궤적 발생에 있어 위의 속도파형생성방법을 적용하기 위해서는 가속의 형태와 감속의 형태를 결정하는 특성함수(f_acc(s), f_dec(s))는 다음 조건 1을 만족해야 한다.

조건 1

가속과 감속구간의 시작인 경우, f_acc(0) = 0, f_dec(0) = 0을 만족한다.

가속과 감속구간의 끝인 경우, f_acc(1) = 1, f_dec(1) = 1을 만족한다.

위의 조건 1을 만족하는 최소 차수 함수는 아래와 같은 표 1의 선형적인 속도특성을 가지는 특성함수이다.

속도특성선형방식부드러운방식가속감속

부드러운 가감속 형태를 가지기 위해서는 아래 조건 2도 만족해야 한다.

조건 2

가속구간과 감속구간의 시작인 경우,f''acc(0)=0, f''dec(0)=0을 만족해야 한다.

가속구간과 감속구간의 끝인 경우,f''acc(1)=0와 f''dec(1)=0을 만족해야 한다.

가속구간과 감속구간 내인 경우, facc(s)와 fdec(s)는 s∈[0,1]에서 연속적인 함수이어야 한다.

이러한 조건들을 만족하는 최소 차수 함수는 표 1의 3차 다항식 속도 특성을 가지는 부드러운 특성 함수이다. 실시간 응답을 가지는 속도 파형을 만들기 위해서는 임의의 순간의 위치, 속도, 가속도 정보를 알아야 한다. 앞서 서술한 속도 프로파일 생성 방법에서의 임의의 순간의 속도(v(t))와 가속도(a(t))는 다음의 수학식 5 및 6과 같이 구할 수 있다.

이러한 속도특성에 따른 속도 프로파일을 도 1에 도시하였다.

도 1은 본 발명에 따른 선형/부드러운 방식의 속도 프로파일을 나타내는 그래프를 도시한 도면이다. 도 1의 가로축은 시간(t)이고, 세로축은 속도(v(t))를 나타낸다.

도 1의 (a) 및 (c)는 선형(linear)방식을 사용했을 경우의 속도프로파일이고, 도 1의 (b) 및 (d)는 부드러운(smooth)방식을 사용했을 경우의 속도프로파일이다. 그리고, 도 1의 (a) 및 (b)는 최고속도(Vm)와 목표속도(Vd)가 같지 않을 경우(Vm ≠Vd)의 속도프로파일이고, 도 1의 (c) 및 (d)는 최고속도(Vm)와 목표속도(Vd)가 같을 경우(Vm = Vd)의 속도프로파일이다.

다음으로, 초기속도가 "0"아닌 경우의 속도파형생성방법을 설명한다.

초기 속도와 가속도가 "0"이 아닐 경우에는 브랜딩구간, 등속구간, 감속구간으로 나눈다. 브랜딩구간에서는 주어진 변화시간(tb)동안에 초기속도(v0)에서 속도를 "0"으로 만드는 속도 프로파일과 주어진 이동거리 S0와 초기 속도에서 속도를 “0”으로 만들기 위해 움직인 거리 Sb의 차이(S0-Sb)를 움직이기 위한 가속구간 프로파일을 브랜딩한다. 주어진 전이시간(tb)동안에 초기속도(v0)에서 속도를 "0"으로 만드는 속도 프로파일은 다음과 같다.

초기 속도(v0)와 가속도(a0)의 곱(v0*a0)의 부호에 따라 사건 발생 순간이 이전 이동의 가속구간, 등속구간, 감속구간중 어떠한 시기인지 예측할 수 있다. 즉, 초기속도와 가속도의 곱(v0*a0)이 "0" 보다 큰 경우(v0*a0>0)는 이전 이동의 가속구간에 사건이 발생한 것이고, "0" 보다 작은 경우(v0*a0<0)는 이전 이동의 감속구간에 사건이 발생한 것이고, 초기 가속도(a0)가 "0"일 경우는 등속구간에서 사건이 발생한 경우이다.

초기 가속도(a0)가 "0"인 경우 즉, 이전 이동의 등속구간에서 사건이 발생한 경우의 브랜딩 특성 함수는 전술한 표 1의 부드러운방식의 감속 특성함수이다. 그리고, 이 경우 이동거리(Sb)는 전이시간(tb)과 초기속도(v0)의 곱(tb*v0)을 1/2로 나누어 얻어진다. 여기서, 초기 가속도(a0)가 "0"이 아닌 경우 브랜딩 특성함수는 하기 조건 3에 의해 결정된다.

조건 3

브랜딩구간의 시작인 경우, f_blend(0)=0, f'_blend(0)=t_b ²a _o/S_b, f''_blend(0)=t_bv_o/S_b

브랜딩구간의 끝인 경우, fblend(1)=1, f'blend(1)=0, f''blend(1)=0

여기서는, 이전 이동의 감속구간일때와 가속구간일 경우에 적용함수를 달리한다.

이전 이동의 감속구간과 가속구간에 따른 브랜딩 특성함수(fblend)는 하기 표 2와 같다.

브랜딩 특성함수(fblend)의 파라메터(α, β, γ, δ, Sb)는 위의 조건 3 및 표 2와 같이 구해진다.

브랜딩 특성함수(fblend(s))로 초기 가속도(a0)와 속도(v0)를 "0"으로 만드는 위치 괘적(Sbb(t)) 생성방법은 다음의 수학식 7로 정의된다.

감속구간에서 움직이는 거리(Sd)를 위한 속도 프로파일 생성방법은 다음과 같이 변경한다.

주어진 이동거리 S0와 Sb의 차이(S0-Sb)를 움직이기 위한 속도 프로파일 생성방법 은 가속시간이 전이시간(tb)으로 지정되었다는 것 이외에는 초기속도가 "0"인 경우의 속도 프로파일 생성방법과 동일하다. 즉, 상기 수학식 3의 가속구간이 끝나는 시간(ta) 이 전이시간(tb)이므로, 최고속도(Vm)는 가속도 크기(aa)에 관계없이 수학식 2 대신 다음의 수학식 8로 결정된다.

여기서,

이다.

초기 속도(v0)와 가속도(a0)를 "0"으로 만드는 속도 프로파일과 주어진 이동거리 S0와 초기 속도에서 속도를 “0”으로 만들기 위해 움직인 거리 Sb의 차이(S0-Sb)를 움직이기 위한 속도 프로파일을 브랜딩하여 얻은 실시간 응답 가능한 궤적(S(t)) 발생 방법은 다음의 수학식 9와 같다.

S(t) = S_n(t) + S_bb(t)

초기 속도(v0)와 가속도(a₀)에 따른 속도 프로파일을 도 2에 도시하였다.

도 2는 위의 속도 프로파일 생성방법에서의 초기 속도(v₀)와 가속도(a₀)에 따른 속도 프로파일을 보여주는 그래프로, 가로축은 시간(t)을 나타내고 세로축은 속도(v(t))를 나타낸다. 도 2의 그래프상에 점선(dotted line)은 초기속도(v₀)에서 속도를 "0"으로 만드는 속도 프로파일을 표시하고, 선과 점으로 표시된 것은 주어진 이동거리 S0와 Sb의 차이(S0-Sb)를 움직이기 위한 속도 프로파일이고,

선으로 표시된 그래프는 브랜딩된 속도 프로파일이다.

도 2의 (a) 및 (b)는 초기가속도(a0)가 "0" 보다 큰 경우(a0 > 0)인, 이전 이동의 가속구간에서 위치와 속도가 변한 속도 프로파일이다. 도 2의 (c) 및 (d)는 초기가속도(a0)가 "0" 보다 작은 경우(a0 < 0)인, 이전 이동의 감속구간에서 위 치와 속도가 변한 속도 프로파일이다. 도 2의 (a) 및 (c)는 초기속도(v0)와 최고속도(Vm)의 곱이 "0" 보다 큰 경우(v0Vm > 0)인, 초기속도(v0)와 최고속도(Vm)의 방향이 같을 경우의 속도 프로파일이다. 도 2의 (b) 및 (d)는 초기속도(v0)와 최고속도(Vm)의 곱이 "0" 보다 작은 경우(v0Vm < 0)인, 초기속도(v0)와 최고속도(Vm)의 방향이 같지 않을 경우의 속도 프로파일이다.

이러한 속도파형을 생성하고 속도변화하는 위치궤적발생방법을 적용한 본 발명에 따른 모션 제어의 실시간 응답을 위한 시스템을 도 6에 도시하였다.

도 3은 본 발명에 따른 모션제어 실시간응답을 위한 궤적발생시스템을 나타내는 시스템 구성도이다. 도 3을 참조하여 설명하면, 도 3에 나타낸 시스템은 크게 두개의 프로세서로 구성된다. 즉, 로봇시스템을 전체적으로 제어하는 상위제어기인 CPU(100)와 축들 위치 제어를 수행하는 외부하드웨어(external hardware)(200)로 구성된다. CPU(100)는 미들웨어(middleware)(10)와 모션제어기(20)를 구비한다. 미들웨어(10)는 클라이언트에서 서버에 있는 응용프로그램이나 자원(resource)을 불러 쓰기 위해 클라이언트와 서버 가운데 놓는 중간자로서, 여기서는 로봇 조작자와 모션제어기(20) 사이에 로봇 제어명령을 전달해주는 수단이다. 모션제어기(20)는 로봇 의존적인 계산을 수행하기 위해 커맨드디코더(command decoder)(21), 궤적발생부(trajectory generation)(22), 역운동학계산부(inverse kinematics calculation)(23), 인터페이스제어부(interface control)(24), 그리고 운동학계산부(forward kinematics calculation)(25)로 이루어진다. CPU(100)는 로봇이 멈추어 있는 경우에 다른 상위 프로그램을 실행할 수 있다. 커맨드디코더(21)는 미들웨어(10)와의 인터페이스를 수행한다. 궤적발생부(22)는 로봇의 가속, 감속, 등속에 관한 계산을 하고, 역운동학계산부(23)는 로봇의 다수개의 축 값을 계산하며, 운동학계산부(25)는 축값으로부터 월드(world)좌표값을 계산하여 커맨드디코더(21)로 전달한다. 인터페이스제어부(24)는 축들 위치를 제어하는 외부하드웨어(200)와의 인터페이스를 수행한다. 외부하드웨어(200)는 모션제어기(20)로부터 전달되는 목표 위치값에 따라 로봇의 다수개의 축의 위치를 제어하는 다축위치제어부(30)와, 모터구동부(40)를 구비한다. 다축위치제어부(30)는 다수개의 축을 동기화하여 위치제어하거나 아니면 특정 축만을 위치제어할 수 있다. 모터구동부(40)는 다축위치제어부(30)의 제어에 의해 다수개의 축이 목표위치값만큼 제어될 수 있도록 모터(50)를 구동한다. 이러한 구성을 갖는 도 6 시스템에서 모션 제어를 위한 실시간 처리과정에 대해 도 7을 통해 구체적으로 설명한다.

도 4는 본 발명에 따른 도 3 시스템의 실시간 모션 제어 처리 모델을 보여주는 상태도이다. 도 4를 참조하여 보면, 기울림 글씨(이텔릭체)는 CPU(100)에서 발생하는 사건 또는 상태이고, 굵은 글씨는 다축위치제어부(30)에서 발생하는 사건 또는 상태이다. 바모양은 전이를 나타내며, CPU(100)에서 주어진 작업을 수행한다. 원모양은 CPU(100)와 다축위치제어부(30)의 상태를 나타낸다.

도 4에서 보면, 초기 토큰은 유휴(DORMANT)상태에 있으며, 사건이 발생하게 되면 토큰이 발생한다. CPU(100)가 발생하는 사건은 모션제어 인에이블 및 디스에이블(MC_ENABLE,MC_DISABLE), 모션제어 이동(MC_MOVE)이고, CPU(100)의 상태는 사용가능한 상태이나 실제적인 작업이 없는 유휴(DORMANT), 준비(READY), 대기(WAITING_UPDATE)상태가 있다. 전이가 발생하는 시기와 전이가 발생할 때 CPU(100)에서 처리하는 작업은 다음과 같다.

외부하드웨어(200)의 다축위치제어부(30)에서 발생하는 사건은 인터럽트(INTERRUPT)이고, 다축위치제어부(30)의 상태는 다축 위치 제어 처리(PROCESSING_JPC)상태이다. 모션제어 인에이블(MC_ENABLE) 사건은 모션제어기(20)를 사용하고자 할 경우 초기에 한번 발생시킨다. 유휴(DORMANT)상태에 토큰이 있을 경우 초기(INITIALIZE) 작업이 수행된다. 초기(INITIALIZE)작업을 마치면 CPU(100)의 준비(READY)상태가 되며, 다축위치제어부(30)는 다축 위치제어 처리(PROCESSING_JPC)상태가 된다. 초기(INITIALIZE)작업은 모션제어기(20) 내부 데이터 초기작업을 수행하고, 다축위치제어부(30)에게 시작한다는 신호를 보내는 것이다. 모션제어 디스에이블(MC_DISABLE) 사건은 모션제어기(20)를 더 이상 사용하지 않을 경우 발생시킨다. 준비(READY)상태에 토큰이 있고 다축위치제어부(30)에서 인터럽트(INTERRUPT)사건이 발생하면 최종(FINALIZE)작업을 수행한다. 최종(FINALIZE)작업은 모션제어기(20) 내부 데이타를 지우고, 다축위치제어부(30)에서 끝낸다는 신호를 보내는 것이다. 모션제어 이동(MC_MOVE) 사건은 목표위치 또는 목표속도가 바뀔 경우 발생한다. 준비(READY)상태에 토큰이 있을 경우 즉, 미들웨어(10)를 통해 로봇 조작자의 명령이 전달되면 모션제어기(20)내의 커맨드디코더(21)에서 명령_해독(COMMAND_DECODE)작업이 수행된다. 명령_해독(COMMAND_DECODE)작업은 축 단위 이동인지, 월드 좌표계에 대한 이동인지를 구별한다. CPU(100)가 대기_갱신(WAITING_UPDATE)상태에서 다축위치제 어부(30)로부터 인터럽트(INTERRUPT) 사건이 발생하게 되면 이렇게 구별된 명령을 사전 처리(PRE_PROCESSING)상태에서 처리한다. 사전 처리(PRE_PROCESSING) 상태로의 전이에서 속도 프로파일을 생성하고 궤적을 구하는 데 필요한 파라메터들(Sa, Su. Sd, ta. tu 및 td)이 계산된다. 이들 파라메터에 대한 계산식은 앞서 서술하였다. 로봇이 움직일 경우, 외부 하드웨어(200)는 주기적으로 주어진 시간(Δt)마다 각 축들 위치(Pj(t))로 제어한다. 이때, 주기적으로 발생하는 사건이 인터럽트(INTERRUPT)이다. 인터럽트(INTERRUPT)사건이 발생하면 주기적인 처리(PERIODIC_PROCESSING) 작업을 수행하게 된다.

이 작업은 초기 속도에 따라 Sn(t) 또는 S(t)를 계산하여 로봇 기구에 맞는 역기구학을 구하여 위치 제어기에 목적 위치를 전달한다.

이러한 방식으로 실시간 처리를 할 경우 최대 명령 수행 지연시간은 인터럽트 발생 주기이다.

이처럼, 모션제어기(20)가 실시간 응답을 가능하게 하기 위해 초기 속도와 가속도가 "0"이 아닌 경우의 궤적발생법은 기존의 4-3-4법과 5 cubic spline방법에 비해 작은 계산량을 가지고 있어 실시간 처리에 용이한 형태이며, 초기 가속도가 "0"이 아닌 경우에도 가속도 연속적인 속도 프로파일 생성이 가능하여 부드러운 이동이 가능하다. 따라서, 본 실시예에서는 궤적발생알고리즘을 직교좌표계의 직선운동에 적용하여 위치변경과 속도변경에 실시간 응답이 가능하다는 것을 보여준다.

상술한 바와 같이, 본 발명에 따른 모션 제어의 실시간 응답을 위한 궤적발 생방법 및 시스템은, 센서와 같은 외부 사건에 실시간으로 응답하여야 하는 로봇시스템에서 이동중 제한된 시간내에 속도 또는 위치 명령의 변경을 반영할 수 있으며, 기존보다 적은 계산량으로 비교적 느린 CPU에서도 실시간 응답을 기대할 수 있는 효과를 갖는다.

Claims

소정의 작업시작점과 작업종료점 사이의 작업경로상에서 작업을 수행하는 로봇의 이동경로를 실시간을 변경하기 위한 방법에 있어서,

(1) 초기 속도가 "0"인 경우 상기 로봇의 이동 속도에 따라 가속, 등속 및 감속의 세구간으로 나누고, 초기 속도가 "0"이 아닌 경우 브랜딩, 등속 및 감속의 세구간으로 나누어 구간을 설정하는 단계;

(2) 상기 설정된 세구간중 가속구간과 감속구간에서는 3차 다항식을 사용하고, 등속구간에서는 상수 속도를 사용하여 최종 위치로의 이동을 위한 속도 프로파일을 구하는 단계;

(3) 상기 설정된 세구간중 브랜딩구간에서는 주어진 변화시간 동안에 초기 속도에서 속도를 "0"으로 만드는 속도 프로파일과 상기 단계 (2)에서 구해진 속도 프로파일을 브랜딩하여 속도 프로파일을 구하는 단계;

(4) 상기 구해진 궤적에 따라 이동중 제한된 시간 내에 위치 및 속도 변경을 실시간 반영하여 모션 제어하는 단계를 포함하는 모션제어의 실시간 응답을 위한 궤적발생방법.
제 1항에 있어서, 상기 단계 (2)는 초기 속도가 "0"인 경우 축 공간에서의 연속적인 속도 제공을 위한 궤적(S_n(t))을 다음의 식에 근거하여 생성함을 특징으로 하는 모션제어의 실시간 응답을 위한 궤적발생방법.

여기서, Sa, Su 및 Sd는 가속, 등속 및 감속구간에서 움직이는 거리이고, facc 및 fdec는 가속 및 감속형태를 결정하는 정규화된 특성함수이며, ta, tu 및 td는 가속, 등속 및 감속이 끝나는 시간을 나타낸다.
제 2항에 있어서, 상기 단계 (2)의 가속, 등속 및 감속구간에서 움직이는 거리(Sa,Su,Sd)는 다음의 식에 근거하여 구해짐을 특징으로 하는 모션제어의 실시간 응답이 가능한 궤적발생방법.

여기서, d는 모션 제어시 이동거리(So)의 부호와 관계없이 목표속도(Vd)를 입력받기 위해 최고속도(Vm)의 부호를 결정하는 값이며, aa는 가속도 크기이고, ad는 감속도 크기이다.
제 3항에 있어서, 상기 단계 (2)의 가속, 등속 및 감속이 끝나는 시간(ta,tu,td)은 다음의 식에 근거하여 구해짐을 특징으로 하는 모션제어의 실시간 응답이 가능한 궤적발생방법.
제 2항에 있어서, 상기 단계 (2)는 가속 및 감속구간의 시작인 경우 가속 및 감속형태를 결정하는 특성함수가 facc(0)=0과 fdec(0)=0을 만족하고, 가속 및 감속구간의 끝인 경우 가속과 감속형태를 결정하는 특성함수가 facc(1)=1과 fdec(1)=1을 만족하는 선형적인 속도특성을 가짐을 특징으로 하는 모션제어의 실시간 응답을 위한 궤적발생방법.
제 5항에 있어서, 상기 단계 (2)는 가속구간에서 f'acc(s)=2s 및 감속구간에서 f'dec(s)=-2s+2의 다항식을 사용함을 특징으로 하는 모션제어의 실시간 응답을 위한 궤적발생방법.
제 5항에 있어서, 상기 단계 (2)는 가속 및 감속구간의 시작인 경우 가속 및 감속형태를 결정하는 특성함수가 f''acc(0)=0과 f''dec(0)=0을 만족하고, 가속 및 감속 구간의 끝인 경우 가속과 감속 형태를 결정하는 특성함수가 f''acc(1)=0, f''dec(1)=0을 만족하며, 가속 및 감속구간 내인 경우 연속적인 함수에 따라 부드러운 속도특성을 가짐을 특징으로 하는 모션제어의 실시간 응답을 위한 궤적발생방법.
제 7항에 있어서, 상기 단계 (2)는 가속구간에서 f'acc(s)=-4s³ + 6s²및 f'dec(s)=4s³ - 6s² + 2의 3차 다항식을 사용함을 특징으로 하는 모션제어의 실시간 응답을 위한 궤적발생방법.
제 6항 또는 제 8항 중 어느 한 항에 있어서, 상기 단계 (2)는 구해진 속도 프로파일에서 임의의 순간의 속도(v(t)) 및 가속도(a(t))를 다음의 식에 근거하여 각각 구함을 특징으로 하는 모션제어의 실시간 응답을 위한 궤적발생방법.
제 8항에 있어서, 상기 단계 (3)은

(3a) 초기 속도와 가속도의 곱의 부호에 따라 사건 발생 순간이 이전 이동의 가속, 등속 및 감속구간중 어떠한 시기인지 예측하는 단계;

(3b) 상기 예측결과에 따라 브랜딩 특성함수를 결정하는 단계;

(3c) 상기 결정된 브랜딩 특성함수로 초기 속도와 가속도를 "0"으로 만드는 속도 프로파일을 구하는 단계; 및

(3d) 상기 구한 속도 프로파일과 주어진 이동거리 S0와 Sb의 차이(S0-Sb)를 움직이기 위한 속도 프로파일을 브랜딩하여 실시간 응답이 가능한 궤적을 발생하는 단계를 구비함을 특징으로 하는 모션제어의 실시간 응답이 가능한 궤적발생방법.
제 10항에 있어서, 상기 단계 (3b)는 초기 가속도가 "0"인 이전 이동의 등속구간에서 사건이 발생한 경우 상기 부드러운 속도특성의 함수를 브랜딩 특성함수로 결정하고, 초기 가속도가 "0"이 아닌 이전 이동의 감속 및 가속구간에서 사건이 발생한 경우 다음의 4차 및 5차 다항식을 브랜딩 특성함수(fblend(s))로 결정함을 특징으로 하는 모션제어의 실시간 응답이 가능한 궤적발생방법.

감속구간에 따른 브랜딩 특성함수(

가속구간에 따른 브랜딩 특성함수(
제 11항에 있어서, 상기 단계 (4)는 초기 속도벡터로부터 속도를 "0"으로 만들기 위한 속도 프로파일로 이동한 거리를 결정하고, 임의의 시간의 초기속도를 "0"으로 만들기 위한 위치궤적을 구하며, 결정된 속도 프로파일로의 이동거리와 목표위치를 이용하여 목표 이동속도의 방향으로 이동한 거리를 구하여 구해진 파라메터들로 주어진 전이시간에 속도변화하여 이동하는 위치궤적을 구함을 특징으로 하는 모션제어의 실시간 응답을 위한 궤적발생방법.
제 11항에 있어서, 상기 단계 (4)는 초기 가속도에 따라 비대칭적인 특성함수들을 사용하여 가속도 연속적인 위치궤적을 생성함을 특징으로 하는 모션제어의 실시간 응답을 위한 궤적발생방법.
로봇의 작업수행중 이동경로를 실시간으로 변경할 수 있는 모션제어 시스템에 있어서,

로봇 조작자로부터의 로봇 제어명령을 입력받아 전달해주는 미들웨어와, 로봇의 모션제어를 수행하는 모션제어기를 구비하며, 초기 유휴(DORMANT)상태에서 모션제어 인에이블/디스에이블 및 이동의 사건이 발생하면 토큰이 발생하여 해당 작업을 수행하도록 제어하는 상위제어기(CPU); 및

상기 상위제어기의 제어에 따라 축들 위치 제어를 수행하는 다축위치제어부를 구비하는 외부하드웨어를 포함하며,

상기 모션제어기는,

미들웨어와의 인터페이스를 수행하여 명령이 전달되면 해독 작업을 수행하여 축 단위 이동인지, 월드 좌표계에 대한 이동인지를 구별하는 커맨드디코더;

상기 외부하드웨어에서 인터럽트 사건이 발생하면 상기 커맨드디코더에서 구별된 명령을 사전 처리하여 로봇의 가속, 감속 및 등속에 관한 속도 프로파일을 생성하여 궤적을 계산하는 궤적발생부;

로봇의 다수개의 축값을 계산하는 역운동학계산부;

축값으로부터 월드좌표값을 계산하는 운동학계산부; 및

다축위치제어부와의 인터페이스를 수행하여 목표위치값을 전달하는 인터페이스제어부를 구비함을 특징으로 하는 모션제어의 실시간 응답을 위한 시스템.
제 14항에 있어서, 상기 다축위치제어부는 주기적으로 주어진 시간마다 인터 럽트 사건을 발생하여 각 축들 위치로 제어함을 특징으로 하는 모션제어의 실시간 응답을 위한 시스템.
제 15항에 있어서, 모션 제어의 최대 명령 수행 지연시간은 인터럽트 발생 주기임을 특징으로 하는 모션제어의 실시간 응답을 위한 시스템.