(19) 대한민국특허청 (KR) (12) 등록특허공보 (B1) (45) 공고일자 2014년08월01일 (11) 등록번호 10-1425057 (24) 등록일자 2014년07월24일 (51) 국제특허분류 (Int. Cl.) B25J 13/00 (2006.01) G06F 17/00 (2006.01) (21) 출원번호 10-2013-0028521 (22) 출원일자 2013 년 03 월 18 일 심사청구일자 (56) 선행기술조사문헌 KR100608070 B1* KR100985713 B1* 2013 년 03 월 18 일 * 는심사관에의하여인용된문헌 (73) 특허권자 주식회사로보스타 경기도안산시상록구수인로 700 ( 사사동 ) (72) 발명자 신우철 경기군포시고산로 643 번길 10, 1151 동 1303 호 ( 산본동, 신안모란아파트 ) 길진수 경기수원시팔달구수성로 101 번길 20, 112 동 50 4 호 ( 화서동, 대우아파트 ) (74) 대리인 박양호전체청구항수 : 총 6 항심사관 : 박영근 (54) 발명의명칭다관절로봇용실시간모션프로파일생성알고리즘을이용한미진동연속적로봇모션구현방법 (57) 요약 본발명은로봇모션구현방법에관한것으로, 특히로봇의동작이정지시에진동을유발하는것을억제하고부드러운로봇모션구현을위한다관절로봇용실시간모션프로파일생성알고리즘을이용한로봇모션구현방법에관한것이다. 본발명에따른다관절로봇용실시간모션프로파일생성알고리즘을이용한미진동연속적로봇모션구현방법은, 다관절로봇의각축별시간에대한위치함수를 4 차다항식, 속도함수를 3 차다항식, 가속도함수를 2 차다항식으로정의하고, 상기위치함수, 속도함수및가속도함수의경계조건을이용하여계수들을계산하여가속구간, 등속구간및감속구간에서의시간에대한위치함수을구하고, 상기가속구간, 등속구간및감속구간에서의시간에대한위치함수를매샘플링타임틱 (Time Tick) 의함수로변환하여실수의제곱항의연산을정수의제곱항의연산으로계산하여다관절로봇의각축을제어하는것으로구현될수있다. 대표도 - 도 1-1 -
특허청구의범위청구항 1 다관절로봇의각축별시간에대한위치함수를 4차다항식, 속도함수를 3차다항식, 가속도함수를 2차다항식으로정의하고, 상기위치함수, 속도함수및가속도함수의경계조건을이용하여계수들을계산하여가속구간, 등속구간및감속구간에서의시간에대한위치함수을구하고, 상기가속구간, 등속구간및감속구간에서의시간에대한위치함수를매샘플링타임틱 (Time Tick) 의함수로변환하여실수의제곱항의연산을정수의제곱항의연산으로계산하여다관절로봇의각축을제어하며, 상기위치함수, 속도함수및가속도함수는 0 u(t) 1의경계조건하에다음수학식으로정의되고, 상기경계조건에의해계산된계수값은다음과같으며, 가속구간 (0 t t a ) 에서의시간에대한위치함수 ( 여기서, υ 는 u(t) 의시간미분값이며, t a 는가속이종료되는 시간 ) 는다음수학식으로계산되는, 다관절로봇용실시간모션프로파일생성알고리즘을이용한미진동연속적로봇모션구현방법. 청구항 2 삭제청구항 3 삭제청구항 4 제1항에있어서, 상기가속구간의위치함수를매샘플링타임틱 (Time Tick) 의함수로변환하기위해상기 t, ta, T는다음과같이정의되며, - 2 -
여기서, ts 는시간함수의샘플링간격을나타내며, n 은샘플링스텝수이고, acc 는가속구간의스텝수이며, total 은 int(t/0.01)+1 이고, int() 은 integer function 을나타내고, 상기샘플링스텝수를변수로하여가속구간의위치함수를표현하면다음수학식으로표현되는, 다관절로봇용실시간모션프로파일생성알고리즘을이용한미진동연속적로봇모션구현방법. 청구항 5 제1항에있어서, 상기등속구간 (t a t T) 에서의시간에대한위치함수 ( 여기서, υ는 u(t) 의시간미분값이며, t a 는가속이종료되는시간, T는등속종료시간 ) 는다음수학식으로계산되는, 다관절로봇용실시간모션프로파일생성알고리즘을이용한미진동연속적로봇모션구현방법. 청구항 6 제5항에있어서, 상기등속구간의위치함수를매샘플링타임틱 (Time Tick) 의함수로변환하기위해상기 t, ta, T는다음과같이정의되며, 여기서, ts 는시간함수의샘플링간격을나타내며, n 은샘플링스텝수이고, acc 는가속구간의스텝수이며, total 은 int(t/0.01)+1 이고, int() 은 integer function 을나타내고, 상기샘플링스텝수를변수로하여등속구간의위치함수를표현하면다음수학식과같이표현되는, 다관절로봇용실시간모션프로파일생성알고리즘을이용한미진동연속적로봇모션구현방법. 청구항 7 제1항에있어서, 상기감속구간 (T<t Tot) 에서의시간에대한위치함수 ( 여기서, υ는 u(t) 의시간미분값이며, T는등속종료시간, Tot는감속종료시간 ) 는다음수학식으로계산되는, - 3 -
다관절로봇용실시간모션프로파일생성알고리즘을이용한미진동연속적로봇모션구현방법. 청구항 8 제7항에있어서, 상기감속구간의위치함수를매샘플링타임틱 (Time Tick) 의함수로변환하기위해상기 t, ta, T는다음과같이정의되며, 여기서, ts 는시간함수의샘플링간격을나타내며, n 은샘플링스텝수이고, acc 는가속구간의스텝수이며, total 은 int(t/0.01)+1 이고, int() 은 integer function 을나타내고, 상기샘플링스텝수를변수로하여감속구간의위치함수를표현하면다음수학식과같이표현되는, 다관절로봇용실시간모션프로파일생성알고리즘을이용한미진동연속적로봇모션구현방법. 명세서 [0001] 기술분야본발명은로봇모션구현방법에관한것으로, 특히로봇의동작이정지시에진동을유발하는것을억제하고부드러운로봇모션구현을위한다관절로봇용실시간모션프로파일생성알고리즘을이용한로봇모션구현방법에관한것이다. 배경기술 [0002] 일반적으로다관절로봇을제어할때시간에대한 2 차함수 혹은 3 차함수 를사용하여위치및속도를제어한다. [0003] [0004] [0005] [0006] 이경우흔히발생할수있는문제점은가속도가상수혹은 1차식이고, 3차식의경우는가속도의미분인저크 (Jerk) 가상수이기때문에로봇의동작이정지시에진동을유발하게된다. 상수의저크는진동을쉽게유발하기때문에저크를 1차식으로만들고가속도를 2차의포물선방식으로만들게되면가감속에의한진동을최소화할수있고로봇의초기위치와최종위치부근에서속도와가속도의연속성으로인해진동을줄일수있다. 다양한논문이나연구에서 3차함수보다더부드럽게정지시모션을제어할수있는다양한방법을연구하고있고, 로봇을개발하고생산하는현장에서활용할수있도록알고리즘개발에필요한비용이나기간등의현실적인관점에서 4차함수를사용하게되면이러한기존의문제점을크게보완할수있다. 하지만, 특히로봇제어에있어서가장중요한부분이실시간 (real-time) 으로모션제어를하는것인데일반적 - 4 -
으로많이사용하는시간 t 에대한함수즉, 로가속도, 위치, 속도를계산하게되면일반적으로최신제어기에서는 0.001[sec] 와같은소수점의시간에대한제곱항을연산하기때문에계산시간에부하가걸리는문제가발생하며, 더욱이부드러운모션제어를위해 4차함수 로속도를계산하면위 3 차함수의계산시에비해서더욱더계산시간에부하가걸리는문제가발생하게된다. [0007] 따라서, 이러한계산시간부하를줄이기위해서는더욱더성능이우수한제어기가필요하게되고, 이는곧생 산설비의과비용이발생하여비합리적이라할수있다. 발명의내용 [0008] [0009] 해결하려는과제본발명이해결하려는과제는, 4차식을사용한모션프로파일의생성을위한기본수식과생성된프로파일의알고리즘을보여주며, 특히시간 t 에대한 3차함수나아가 4차함수로가속도, 위치, 속도를계산하게되면소수점의시간에대한제곱항을연산하기때문에계산시간에부하가걸리는문제를해소하기위해로봇제어기의실수가아닌정수의매샘플링타임틱 (time tick) 의함수로프로파일을생성하는방법을제안한다. 또한, 정수값인타임틱 (Time tick) 으로계산을하여복잡한실수의제곱의항을계산할필요없이단순한연산으로계산이가능하므로고성능제어기가아닌시스템에서도쉽게로봇운동의정지시진동없이부드러운모션을구현하고자한다. [0010] [0011] 과제의해결수단상술한과제를해결하기위한본발명에따른다관절로봇용실시간모션프로파일생성알고리즘을이용한미진동연속적로봇모션구현방법은, 다관절로봇의각축별시간에대한위치함수를 4차다항식, 속도함수를 3차다항식, 가속도함수를 2차다항식으로정의하고, 상기위치함수, 속도함수및가속도함수의경계조건을이용하여계수들을계산하여가속구간, 등속구간및감속구간에서의시간에대한위치함수을구하고, 상기가속구간, 등속구간및감속구간에서의시간에대한위치함수를매샘플링타임틱 (Time Tick) 의함수로변환하여실수의제곱항의연산을정수의제곱항의연산으로계산하여다관절로봇의각축을제어하는것으로구현될수있다. 여기서, 좀더상세하게는다관절로봇의동작중에정지할때가속도또는저크 (Jerk) 에의한로봇의진동을최대한억제하기위해또는진동없는로봇모션구현을위해서본발명에서는시간에대한위치함수를 4차함수 로정의하고, 다관절로봇각축에대한위치를결정한다. 이후시간에대한위치함수 u(t), 속도함수, 가속도함수를결정한다. 각함수의위치프로파일, 속도프로파일및가속도프로파일을그래프로변환한다음각각의그래프와 0 u(t) 1의조건으로경계조건을이용한각각의함수의계수를결정한다. 경계조건에의한계수가결정되면, 가속구간, 등속구간및감속구간에서의시간함수 u(t) 를계산한다. 시간함수 u(t) 를샘플링타임 ( 타임인덱스 n) 을기준으로한함수 u(t) 를계산하고, 샘플링스템수를변수로한타임틱 (Time Tick) 으로변환하여제어기에의해로봇을제어하는것으로구현될수있다. [0012] 발명의효과상술한본발명의구성에따르면, 시간 t 에대한 3차함수나아가 4차함수로가속도, 위치, 속도를계산하는데있어계산시간에부하가걸리는문제를해소하기위해로봇제어기의실수가아닌정수의매샘플링타임틱 (time tick) 의함수로프로파일을생성하고, 정수값인타임틱 (Time tick) 으로계산을하여복잡한실수의제곱의항을계산할필요없이단순한연산으로계산이가능하므로고성능제어기가아닌시스템에서도쉽게로봇운 - 5 -
동의정지시진동없이부드러운모션을구현하는것이가능하게된다. [0013] 도면의간단한설명 도 1 은본발명에따른미진동연속적로봇모션구현방법의구현순서도이다. 도 2는시간에대한위치함수및연속적인속도운동을가정한속도프로파일을도시한것이다. 도 3은본발명에따른로봇모션구현방법에서시간함수에대한가속도의프로파일을보여준다. 도 4는본발명에따른로봇모션구현방법에서시간함수에대한 S커브속도의프로파일을보여준다. 도 5는본발명에따른로봇모션구현방법에의해생성된 6축다관절로봇의각축별위치프로파일을나타낸것이다. 도 6은본발명에따른로봇모션구현방법에의해생성된 6축다관절로봇의각축별속도프로파일을나타낸것이다. 도 7은본발명에따른로봇모션구현방법에의해생성된 6축다관절로봇의각축별가속도프로파일을나타낸것이다. [0014] [0015] [0016] [0017] 발명을실시하기위한구체적인내용이하, 첨부된도면을참조하여본발명에따른다관절로봇용실시간모션프로파일생성알고리즘을이용한미진동연속적로봇모션구현방법및작용효과를설명한다. 본발명에서는가감속구간에서속도와가속도의연속성을유지하기위해서시간을매개변수로하는 u(t) 를 4 차다항식으로정의하며, 나아가다관절로봇은 6축관절을갖는로봇을상정하여바람직한예시로서설명하기로한다. 도 1은본발명에따른미진동연속적로봇모션구현방법의구현순서도이다. 다관절로봇의동작중에정지할때가속도또는저크 (Jerk) 에의한로봇의진동을최대한억제하기위해또는진동없는로봇모션구현을위해서본발명에서는시간에대한위치함수를 4차함수로정의하고, 다관절로봇각축에대한위치를결정한다 (S11). [0018] [0019] [0020] [0021] 이후시간에대한위치함수 u(t), 속도함수, 가속도함수를결정한다 (S12). 각함수의위치프로파일, 속도프로파일및가속도프로파일을그래프로변환한다음각각의그래프와 0 u (t) 1의조건으로경계조건을이용한각각의함수의계수를결정한다 (S13). 경계조건에의한계수가결정되면, 가속구간, 등속구간및감속구간에서의시간함수 u(t) 를계산할수있다 (S14). S14 단계에서구한시간함수 u(t) 를샘플링타임 ( 타임인덱스 n) 을기준으로한함수 u(t) 를계산하고 (S15), 샘플링스템수를변수로한타임틱 (Time Tick) 으로변환하여제어기에의해로봇을제어한다 (S16). [0022] 이러한순차적과정을좀더구체적으로살펴본다. [0023] [0024] [ 로봇각축의위치결정 ] 직교좌표상에주어진시작점과끝점은역기구학 (inverse kinematics) 해를구하여시작점과끝점의조인트 (Joint) 값 θ 0, θ f 을알수있는데모션프로파일파라메터함수 u(t) 를이용하여임의의점 u 에서의각각의 조인트별앵글값 θ u 를나타내면다음 [ 수학식 1] 과같다. - 6 -
수학식 1 [0025] [0026] 여기서, (θ u ) i 는임의의시간 u 에서의현재위치이며 (θ 0 ) i 는조인트 i 의초기위치, (θ f ) i 는조인트 i 의목 표위치, u(t) 는시간의함수로다음 [ 수학식 2] 의성질을갖는다. 수학식 2 [0027] [0028] u(t) 를구하면위치 (θ u ) i 를결정할수있으므로경로계획문제는결국시간함수 u(t) 를결정하는문제와동일 하다. [0029] 본발명에서는가감속구간에서속도와가속도의연속성을유지하기위해서시간을매개변수로하는 u(t) 를다 음과같은 4 차다항식으로정의한다. 수학식 3 [0030] [0031] [ 속도프로파일 (Profile) 의결정 ] [0032] [0033] [0034] [0035] 위에서설명한바와같이시간함수 u(t) 를구하면경로계획상의모든점의위치및방향을구할수있다. [ 수학식 3] 에대한시간미분 ( 즉, joint velocity) 은오직함수 u(t) 의시간미분값에의존하며, 로봇각 joint 가진동없이부드러운운동을하기위하여함수 u(t) 는연속적인속도와가속도프로파일이요구된다. constant velocity 운동을가정한다면속도프로파일은도 2와같이나타낼수있다. 이때 u(t)=1 을고려하면도 2의사각형의면적 υt = u(t) = 1 의관계가성립하므로함수 u(t) 의시간미분값 υ와운동완료시간 T 와의관계가다음 [ 수학식 4] 와같이구해진다. 수학식 4 [0036] [0037] 여기서, T 는도달시간을의미한다. - 7 -
[0038] 그러나, 실제로봇동작시에는가감속을위하여어느정도의시간이필요하기때문에도 2 와같은속도프로파 일을얻는것은불가능하며, 이러한이동명령은급속한가감속에의한진동을유발시키고또한서보컨트롤러에 추종오차를증대시킴으로서보모터에과부하를유발시키게된다. [0039] [0040] [0041] [0042] [0043] 따라서, 본발명에서는다음과같은 3차식 S커브속도프로파일을제안한다. 위에서구한 1차속도프로파일은저크가발생하기때문에진동을쉽게유발하며가감속에의한진동을최소화하기위해서는초기위치와최종위치부근에서속도와가속도의연속성이가감속구간에서도요구된다. 이러한성질을만족시키는가감속도프로파일은최소 2차다항식이되어야하고, 본발명에서도가감속도프로파일을 2차다항식으로표현되도록한다. 도 3은본발명에따른로봇모션구현방법에서사용된가속도의프로파일을보여준다. 속도프로파일은가속도를시간적분하여얻어지므로가감속구간에서 3차다항식으로표현되며도 4에나타난바와같이최종위치에도달하기위해서는 t d 의시간이더소요된다. [0044] [0045] [ 시간에대한위치함수 u(t) 의결정 ] 일반적으로로봇 joint 의속도는최대허용 joint 속도의 % 로지정되며모든 joint 가각각의지정속도로움직인 다면도달시간 T i 는다음 [ 수학식 5] 와같다. 수학식 5 [0046] [0047] 여기서, i=1,2,3,4,5,6 ( 로봇조인트 ), (θ 0 ) i 는조인트 i 의초기위치, (θ f ) i 는조인트 i 의목표위치, V i 는조 인트 i 에서의속도이며 V i =% (Vmax) i 로각축최대허용속도의 % 값으로주어진다. [0048] [0049] 또한, 로봇의각조인트마다도달시간이틀리게되므로가장오래걸리는시간을 T = Max T i 로정한다. 시간함수 u(t) 를다음의 4 차다항식으로정의를하고, 위치, 속도, 가속도를나타내면다음 [ 수학식 6] 과 같다. 수학식 6 [0050] [0051] [0052] [ 가속구간 (0 t t a ) 에서의시간함수 u(t) 의계산 ] 위 [ 수학식 6] 으로부터주어진다항식의계수는 0 u(t) 1 의조건과도 2 내지도 4 로부터아래와같은 [ 수학 - 8 -
식 7] 과같이가속구간의경계조건을이용하여구하여질수있다. 수학식 7 [0053] [0054] [ 수학식 7] 로주어진경계조건을이용하여 [ 수학식 6] 으로부터계수들을구하면다음 [ 수학식 8] 과같다. 수학식 8 [0055] [0056] 정리하면가속구간 (0 t t a ) 에서시간함수 u(t) 는다음 [ 수학식 9] 와같이된다. 수학식 9 [0057] [0058] [0059] [ 등속구간 (t a <t T) 에서의시간함수 u(t) 의계산 ] 등속구간에서 u(t) 는 u(t) = u(t a )+(t-t a ) υ ( 즉, 속도프로파일의면적 ) 이므로 [ 수학식 9] 에 t = t a 를대입하 면, 다음 [ 수학식 10] 이되고, 이를정리하면등속구간에서시간함수는 [ 수학식 11] 이된다. 수학식 10 [0060] - 9 -
수학식 11 [0061] [0062] [0063] [ 감속구간 (T<t Tot) 에서의함수 u(t) 의계산 ] 감속구간에서의가속도함수 g(t) 는 g(t) = -f(t-t) 가되므로 [ 수학식 9] 을이용하면다음 [ 수학식 12] 와같이 된다. 수학식 12 [0064] [0065] 여기서, 이되며, Tot 는운동에소요된총시간으로써, Tot = T + t a 이다. [0066] [0067] [ 샘플링타임 ( 타임인덱스 n) 을기준으로한함수 u(t) 의계산 ] u(t) 를매 10msec 간격으로계산하고가감속구간을 25step 으로고정하여시간변수 t 를 step 수로서나타내면 아래 [ 수학식 13] 과같다. 다만, 10msec 와 25step 수는예시적인것이며변경이가능함은물론이다. 수학식 13 [0068] [0069] 여기서, n 은샘플링 step 수이며, acc 는가속구간 step 수이고, total = int(t/0.01)+1 이며 int() 는 integer function 을나타낸다. [0070] 여기서, 짧은거리이동시가감속시간 ta 가 T 보다클경우가있으며, 그때에한하여가속구간 ta 는 한다. 로 [0071] 이어서, [ 수학식 13] 의샘플링 step 수를변수로하여 [ 수학식 9], [ 수학식 11], [ 수학식 12] 를가속구간 (0 n acc), 등속구간 (acc<n total), 감속구간 (total<n Tot) 에서다시표현하면가속구간 [ 수학식 14], 등속구간 [ 수학식 15], 감속구간 [ 수학식 16] 과같이표현될수있다. - 10 -
수학식 14 [0072] 수학식 15 [0073] 수학식 16 [0074] [0075] [0076] [0077] [0078] [0079] 위 [ 수학식 14], [ 수학식 15] 및 [ 수학식 16] 에서나타난바와같이, 다항식의계산이모두정수의계산이되므로, 복잡한실수의제곱의항을계산할필요없이단순한연산으로계산이가능하게되고고성능제어기가아닌시스템에서도진동을발생시키지않고부드러운모션을쉽게구현할수있게된다. 도 5는본발명에따른로봇모션구현방법에의해생성된 6축다관절로봇의각축별위치프로파일을나타낸것이고, 도 6은본발명에따른로봇모션구현방법에의해생성된 6축다관절로봇의각축별속도프로파일을나타낸것이며, 도 7은본발명에따른로봇모션구현방법에의해생성된 6축다관절로봇의각축별가속도프로파일을나타낸것이다. 도 5 내지도 7은 [ 수학식 14], [ 수학식 15] 및 [ 수학식 16] 을이용하여생성한위치, 속도, 가속도프로파일을나타낸것이며, 도 2 내지도 4에시간함수로표현한위치, 속도및가속도프로파일과동일한경로의프로파일을생성할수있음을알수있다. 도 5 내지도 7에나타난 6개의그래프 ( 프로파일 ) 은 6축다관절로봇을기준으로각축별위치, 속도, 가속도프로파일을나타낸것이며, 가로축은시간축이고세로축은각각위치, 속도, 가속도값을나타낸다. 이상첨부된도면을참조하여본발명의바람직한실시예를설명하였지만, 상술한본발명의기술적구성은본발명이속하는기술분야의당업자가본발명의그기술적사상이나필수적특징을변경하지않고서다른구체적인형태로실시될수있다는것을이해할수있을것이다. 그러므로이상에서기술한실시예들은모든면에서예시적인것이며한정적인것이아닌것으로서이해되어야하고, 본발명의범위는상기상세한설명보다는후술하는특허청구범위에의하여나타내어지며, 특허청구범위의의미및범위그리고그등가개념으로부터도출되는모든변경또는변형된형태가본발명의범위에포함되는것으로해석되어야한다. [0080] - 11 -
도면 도면 1 도면 2-12 -
도면 3 도면 4 도면 5-13 -
도면 6 도면 7-14 -