Równoległy algorytm obliczeniowy zagadnienia odwrotnego dynamiki manipulatorów

ZESZY TY NAUKOW E POLITECHNIKI ŚLĄSKIEJ Seria: A UTOM ATYKA z.l 15

_______ 1994 N r kol. 1251

K rzysztof R O M A N O W SK I, W aldem ar W R Ó B LEW SK I

K a te d ra A uto m aty k i, R obotyki i Inform atyki, Politechnika Poznańska

RÓW NOLEGŁY ALGORYTM OBLICZENIOW Y ZAG ADNIENIA O DW RO TNEGO D Y N A M IK I M ANIPULATORÓW

S treszczenie: W p racy omówiono paralelizację algorytm u obliczeniowego za­

gad n ien ia odw rotnego dynam iki m anipulatorów , opracowanego z w ykorzystaniem form alizm u L agrange’a i m acierzy rotacji (3 x 3 ). Omówiono również wersję algo­

ry tm u z predykcją pom ocniczych danych. Przedstaw ione zostały rozw ażania na te m a t obliczeniochłonności i złożoności obliczeniowej algorytm ów w zależności od liczby stopni swobody m a n ip u la to ra i liczby w ykorzystywanych procesorów.

PARALLEL CO M PUTATIONAL ALGORITHM OF THE ROBOT IN V E R SE D Y N A M IC S PROBLEM

S um m ary: A parallelization of an algorithm of the solution of th e m anipulator inverse dynam ics problem is presented. T he algorithm is based on th e Lagrange form alism and m akes use of (3 x 3 ) ro tatio n m atrices. A version of th e algorithm w ith p rediction of tem p o rary d a ta is also described. T h e com putational com plexity of th e parallel algorithm w ith respect to th e num ber of degrees of freedom of th e m a n ip u la to r and to th e num ber of processors used is considered.

PARALLEL ALG O RITH M US D E R B E R EC H N U N G E N F Ü R U M K E H R A U F G A B E D E R RO BO TERDYNAM IK

Zusam m enfassung: In diesem Bericht w urde die P arallelisierung der Berechnun- gen für die U m kehraufgabe der R oboterdynam ik beschrieben. D er hier p rä se n tie rte A lgorithm us w urde m ittels der Lagrangeschen G leichungen und der R o tatio n m a­

triz en (3 x 3 ) e ra rb e ite t. Zusätzlich auch eine Version des A lgorithm us m it der P re d ik tio n w urde beschrieben. Ebenso wird die D isskusion über die A bhängigkeit der B erechnungsaufw and von der A nzahl der F reiheitsgrade eines M anipulatores und der A nzahl der verw endeten Prozessoren präsentiert.

360 K. R om anow ski, W. W róblewski

1. W stęp

M etody rozw iązyw ania odw rotnego zagadnienia dynam iki m anipulatorów o p a rte są zazw yczaj n a form alizm ie L agrange’a albo N ew tona-E ulera. G eneralnie form alizm N ew tona-E ulera pozw ala n a o trzym anie najefektyw niejszego algorytm u w postaci ogól­

nej, bez przeprow adzania indyw idualizacji (ang. customization) z uw zględnieniem p a ­ ram etró w geom etrycznych i dynam icznych konkretnej s tru k tu ry m an ip u lato ra. Je d n ak zastosow anie m acierzy rotacji (3 x 3 ) i zależności rekurencyjnych w ram ach form alizm u L agrange’a prowadzi do algorytm u o porównywalnej efektywności, zw łaszcza d la p rak ­ tycznie spotykanej liczby ogniw m a n ip u la to ra [5]. W niniejszej pracy przedstaw iono w ersje tego algorytm u w ykorzystujące obliczenia równolegle. Są one przeznaczone dla system ów wieloprocesorowych- ty p u MIM D (ang. M ultiple In stru ctio n , M ultiple D ata) o pam ięci rozproszonej, takich ja k np. system y transputerow e. System y tego rodzaju są dogodną p la tfo rm ą dla algorytm ów charakteryzujących się równoległością n a poziom ie zadań i stosunkow o lu źn ą zależnością m iędzy danym i przetw arzanym i przez poszczególne zadania.

Z asadą podziału zadań obliczeniowych prezentow anego algorytm u je s t przydzielenie osobnego procesora lub p ary procesorów dla obliczeń odpow iadających każdem u z wę­

złów m a n ip u la to ra . W typow ym przypadku m a n ip u la to ra o sześciu ogniw ach w ym agany je st więc system sześcio- lub dw unastoprocesorow y. Z ak ład a się, że procesory połączone są w konfigurację pipeline um ożliw iającą dw ukierunkow ą tran sm isję danych. K ierunki tran sm isji o d p o w iad ają kierunkom zależności rekurencyjnych: od podstaw y do końcówki m a n ip u la to ra (f orward) - w te n sposób w yznaczane są wielkości kinem atyczne w kolej­

nych węzłach - oraz od końcówki do podstaw y m a n ip u la to ra ( backward) - tak obliczane są wielkości dynam iczne w kolejnych węzłach.

Zależności rekurencyjne w prow adzają silny związek między danym i odnoszącym i się do kolejnych Węzłów. Aby nie dopuścić do zdom inow ania pracy system u przez tra n sm i­

sje danycl^zastosow ano predykcję wielkości pośrednich potrzebnych w obliczeniach [6], m ożliw ą w sytuacji, gdy całość obliczeń p o w tarza się d la kolejnych próbek czasowych.

Dzięki tem u procesory realizujące poszczególne ogniwa rekurencji nie czekają na siebie, lecz k o rzy sta ją z wartości obliczonych dla poprzedniej próbki. Postępow anie ta k ie pro ­ wadzi nieuchronnie do pew nych błędów obliczeń, które m aleją przy skracaniu okresu próbkowania.

2. P o d s ta w o w y a l g o r y t m o b lic z e n io w y

R ozpatrzm y m a n ip u la to r o N ogniwach, stanow iący otw arty łańcuch kinem atyczny, k tó ­ rego kolejne ogniw a są num erow ane od nieruchom ej podstaw y (ogniwo 0) do jego koń­

cówki (ogniwo N ) . Z łącza są num erow ane ta k , że ¿-te złącze je s t um iejscowione m iędzy ogniwem (i — l)-y m a ¿-tym , przy czym każde złącze reprezentuje jed en stopień swobody.

Rów noległy algorytm obliczeniowy

Uogólnione siły 77 (i = 1 ,■■■ , N ) , d ziałające w poszczególnych złączach m an ip u lato ra, są określone rów naniam i L agrange’a o postaci:

T«' = TT d_K_

dąi

dl< d V

n 4" n 1 ^ 1'

0?, % (1)

cos 0, — cos « ¿sin 0; sin oci sin 0,- a,- cos d{

- l R = sin 0,- cos a,- cos 0{ — sin a,- cos 0, ₁ ^{t- 1 p . _}₁ ai sin 0i

0 sin a , cos a,

gdzie ę; je s t ¿-tą w spółrzędną uogólnioną, tzn. w spółrzędną n a tu ra ln ą ogniwa będącą je d n y m z para m etró w D enav ita-H arten b erg a (D-H). A', V są odpow iednio całkow itą energią kinetyczną, obliczoną względem inercjalnego układu w spółrzędnych, oraz energią p o te n c ja ln ą m an ip u la to ra .

Po przyporządkow aniu poszczególnym ogniwom układów w spółrzędnych w edług no­

tacji D -II, m acierz rotacji \~l R i w ektor położenia określające orientację ¿-tego u k ła d u w spółrzędnych względem i — 1-go układu oraz położenie początku ¿-tego układu w spółrzędnych w zględem ¿ — 1-tego układu, określone są następująco [2]:

(2)

gdzie a,-, a ;, d; oraz 0; są p ara m etra m i D-H ¿-tego ogniwa m anipulatora.

Zastosow anie opisu kinem atyki m a n ip u la to ra za pom ocą m acierzy rotacji (3 x 3 ) i w ektora p o łożenia (3 x 1 ), za m ia st tradycyjnie stosowanych m acierzy (4 x 4 ) tran sfo rm a­

cji je d norodnych prowadzi do zm niejszenia liczby wykonywanych działań elem entarnych podczas obliczania sił uogólnionych. Przykładow o, przy m nożeniu dwóch m acierzy (3 x 3 ) uzyskuje się redukcję liczby wykonywanych działań o przeszło 50% w stosunku do wy­

konyw ania tej operacji n a m acierzach (4 x 4 ) [5].

W ykonanie form alnych operacji różniczkowań, w ystępujących we wzorze (1), dla m a­

n ip u la to ra opisanego w edług notacji (2), a następnie w ykorzystanie rekurencyjnych za­

leżności określających prędkości i przyspieszenia prowadzi do rekurencyjnego algorytm u obliczeniow ego' [4, 5], k tó ry m ożna zaim plem entow ać w system ie jednoprocesorow ym . A lgorytm ten m ożna przedstaw ić następująco:

1. O bliczenia rekurencyjne od podstaw y do końcówki m a n ip u la to ra (forward).

Z adanie Fi, i = 1, • • •, N

° R ( t )

dqi

° R ( t )

U R ( t ) \ - ' R ( t ) d ^ R ( t )

= u m

dq,

(

) (

)

(

)

d r m

,

° m = •

dq.K

3 6 2 K. Rom anow ski, W. W róblewski

+ o o ff, , d ° R ( t )

,_1 ' ' d q? ¡ ' ' ~ dq, ^

° P i( 0 = + ?_,*(*) ■•->*(*) (7)

5 ° p ' (0 o

“ ^ T - - ^ ( <) — (8)

°Pj(i) = oPi_,(0 +

i_1Pi(0 + 2 ?_i ¿(0 9,(0 +

+ o ™

+ ¿ - iP (0 9 i ( 0 + ^ 9 i(0 (9)

2. O bliczenia rekurencyjne od końcówki do podstaw y m a n ip u la to ra ( backward).

Z adanie i?,-, i = N , ■ ■ ■, 1

6,(i) = m ; + 6,+1(i) (10)

e,(i) = n n ^ f i ) + ¡ n J ° R T (t) + ei+l(t) (11) D, ( t ) = ¡ni °P?'(t) + J i ° R T (t) + ¡i+iR ( t ) A + i(< ) + ’> i+i( i) e ,+1(t) (12) c,(i) = ' n { + bi+l(t) *Pi+i( i) + ¡+1i?(i) c;+ i( i) (13)

-

gdzie m,- je s t m asą i-tego ogniw a, 'n,-.= m,- ‘P c; je st m om entem staty czn y m i-tego ogniwa w zględem i-tego u kładu w spółrzędnych (‘P a je st w ektorem położenia środka m asy), J, je st m acierzą pseudoinercji i-tego ogniwa względem i-tego u kładu w spółrzędnych, zaś g je s t w ektorem przyspieszenia graw itacyjnego. W y stępujące w powyższych wzorach wielkości )- 1 i?(.i) oraz '~' Pi (t ) są obliczane n a podstaw ie p aram etrów geom etrycznych ogniwa, z których d, lub 0, stanow i w spółrzędną uogólnioną, w zależności od tego,czy i-te ogniwo je s t przesuw ne,czy obrotowe:

¡ - ' R M = /:(«,,«,■,</,(*), 0 ,(i)) (15)

¡ - ' R i t ) = / 2 ( a „ a „ c i , ( 0 , i , ( 0 ) (16) W artości początkow e są następujące: qo = 0, qo = 0, q0 = 0, c,v+i = 0, D ^ + i — 0, cN+l = 0, 6jv+i = 0. Pom ocnicze wielkości, w yznaczane z zależności (10)—(13), m a ją n astęp u jące rozm iary: 6; - skalar, c,- - w ektor kolum nowy (3 x 1 ), Di - m acierz (3 x 3 ), e,- - w ektor wierszowy (1 x 3 ).

A by obliczyć w artości sił uogólnionych r,-, i = 1, — , JV, należy za te m w ykonać se­

kw encyjnie w szystkie obliczenia etapów 1 i 2 powyższego algorytm u. S chem at blokowy przepływ u danych dla tego algorytm u przedstaw iono n a rys. 1. Ze względu n a w ystę­

pow anie dwóch etapów rekurencji algorytm te n je st analogiczny do algorytm u opartego n a form alizm ie N ew tona-E ulera [2], Jego złożoność obliczeniow a je s t też rów na O ( N ) .

Równoległy algorytm obliczeniowy _{3 6 3}

Rys. 1. P rzepływ danych dla podstawowego algorytm u obliczeniowego Fig. 1. D a ta flow of th e basie com putational algorithm

3. A l g o r y t m o b lic z e ń r ó w n o le g ły c h z p r e d y k c ją

Aby um ożliwić podjęcie obliczeń przez zadania Fi, Bi bez oczekiwania n a zakończenie zadań F{- i, 5 , + ])m ożna dokonać predykcji pom ocniczych wielkości, obliczanych przez te o sta tn ie zadania. Taki sposób zwiększenia efektywności obliczeniowej, polegający na w ykorzystaniu zam iast bieżących w artości ich wartości z poprzednich próbek czasowych, przedstaw iono m .in. w pracach [1, 3]. P redykcja, w prow adzana w om aw ianym alg o ry t­

m ie, polega n a za stą p ien iu bieżących wartości wielkości obliczanych przez zad an ia F j_ i, Bi+ 1 przez ich w artości pochodzące z poprzedniej próbki czasowej. A lgorytm z predykcją m a wówczas postać:

1. O bliczenia rekurencyjne od podstaw y do końcówki m a n ip u la to ra (forward).

Z adanie Fi, i = 1, • • •, N

°R(t)

=

-1-

<?■(*)

=

-

i"1

) + 2

~ ¿ 0

*'

*(0 +

+ U Ą t - A t ) d 2ir ' R ( i ) -?(f) , d l R j t - A t ) ,

d q j ^ ' dq,

°Pi(t) = °P t_ , ( f - A i ) + U W - A t ) ‘- ' P i i t ) d 0W ) _ o R ( i A t ] ^ U M

~ d T “ d q,

°Pi(t) = ° P i - , ( t - A t ) + l l R { t - ^ ) ' - l Pi(t) +

qi{t)

(17) (18)

(19)

(20)

( 2 1 ) (22)

3 64 K. R om anow ski, W . W róblewski

Rys. 2: P rzepływ danych dla algorytm u obliczeniowego z predykcją Fig. 2. D a ta flow of th e com p u tatio n al algorithm w ith prediction

%■

2. O bliczenia rekurencyjne od końcówki do podstaw y m a n ip u la to ra [backward).

Z adanie i — N , • • •, 1

6,(i) = m t- + 6,+i(i — AO (24)

e ,( 0 = m,- ° i f ( t - AO + 'n f ? /iT(f - A t) + ,s t ' + l'- A O (25) A ( 0 = 'n,* ° / f ( t - A O 4- Ji ° R T {t - AO +

+ ;+i R { 0 Di+\(t — A t ) + ‘P 1+i ( 0 e , + i ( i -- A t ) (26) c ,(0 = ln, + 6,+i(i — A i) 'F ,+i(i) + ‘+1R ( i) c ,:+i(f - A i) (27) n ( 0 = tr Z a ? R ( i - A 0 r ( i ) ! d ° P i { t - A t )

\ % 1 <9?; e; ( o ) -

Rów noległy algorytm obliczeniowy ₃₆₅

Rys. 3. P rzepływ danych d la zmodyfikowanego algorytm u obliczeniowego z predykcją Fig. 3. D a ta flow of th e m odified com putational algorithm w ith prediction

- f C ?7% ~ A ° c--(f)+ « * " % - ) (28)

S chem at blokowy przepływ u danych dla algorytm u z predykcją przedstaw iono n a rys.

2. W y stęp u jące w powyższych wzorach wielkości ¡-1 /i(f) oraz są obliczane na p odstaw ie zależności (15), (16).

4. Z m o d y f ik o w a n y a l g o r y t m o b lic z e ń r ó w n o le g ły c h z p r e d y k c ją

W sy tu a cji, gdy zad an ia F;, Bi są wykonywane przez ten sam procesor, a w zw iązku z tym tra n sm isja danych m iędzy nim i nie w prow adza istotnego opóźnienia, istnieje możliwość w ykonania ich kolejno. W ta k im przypadku zadanie Bi może w ykorzystać bieżące w ar­

tości obliczane przez zadanie Fi, zam iast pochodzących z predykcji. S chem at blokowy zm odyfikowanego w te n sposób algorytm u przedstaw iono n a rys. 3.

3 6 6 K. Rom anow ski, W . Wróblewski T ab ela 1

Liczba operacji elem entarnych

W ielkość Sinus Cosinus M nożenie D odaw anie O bliczenia i*) forward)

1 I 4 0

. - i p

(

0 0 2 0

■R 0 0 27 18

d ? / ? / % 0 0 27 18

° R 0 0 36 27

° R 0 0 110 81

° P 0 0 9 9

d ° P i / d qi 0 0 9 6

°Pi 0 0 38 30

R azem Fi 1 1 262 189

O bliczenia Bi ( backward)

b 0 0 0 1

e 0 0 12 12

D 0 0 72 63

0 0 12 12

T 0 0 51 41

R azem B 0 0 147 129

R azem 1 1 409 318

5. O s z a c o w a n ie w y m o g ó w o b lic z e n io w y c h

W celu oszacow ania zasobów obliczeniowych w ym aganych przez pow yższe algorytm y w tab eli 1 przedstaw iono liczby operacji dodaw ania, m nożenia i obliczania w artości funkcji sinus i cosinus wykonyw anych w trakcie w yznaczania poszczególnych wielkości [5], a w tab eli 2 długości bloków danych przesyłanych m iędzy zadaniam i.

O znaczm y przez t ( x ) czas w ykonania obliczeń zad an ia x lub przesyłu danych w kie­

ru n k u x. W idać, że dom inującym i czasem obliczeń je st t(Fi), a przesyłu i(F ; —» F;+1).

Czas obliczeń dla jednej chwili próbkow ania przy użyciu podstawowego algorytm u w postaci sekwencyjnej wynosi N ( t ( F i ) + i( 5 ,) ) . Im plem entacja zaprezentow anego algo­

ry tm u z p red y k cją w tak i sposób, że d la każdego z zadań Fi, Bi przydzielony je s t osobny procesor, pozw ala n a w ykonanie obliczeń dla jednej chwili próbkow ania (dla wszystkich ogniw m a n ip u la to ra ) w czasie t(Fi) + i(F ; —> F ,+1) przy 2 N procesorach. Im plem entacja zm odyfikowanego algorytm u z predykcją, gdzie osobny procesor przydzielony je s t każdej parze zad ań Ft, Bi, pozw ala osiągnąć czas t(Fi) + i ( 5 ,) przy N procesorach. W p rak ­ tyce czas obliczeń je st nieco większy, gdyż ponad to wykonyw ane są operacje inicjujące obliczenia rekurencyjne oraz organizujące przesyłanie danych m iędzy procesoram i.

Rów noległy algorytm obliczeniowy

3 6 7 T abela 2

Liczba przesyłanych danych K ierunek przesyłu Liczba danych f l o a t

F{ -+ Fj+1 33

Fi - Bi 24

Bi+1 —► Bi 16

Fi+1 —> Bi 12

Qi i Qi i Qi * Fi 3

Fi - * n 1

6. P o d s u m o w a n ie

A lgorytm zaim plem entow ano w system ie Q uintek Fast9, zaw ierającym dziewięć trans- puterów IMS T805. Stanow i on kartę rozszerzeniową k o m p u tera kom patybilnego ź IRM P C . P rzeprow adzone zostały b ad a n ia sym ulacyjne algorytm ów n a jednym tran sp u terze d la jednego ogniw a rekurencji. A ktualnie trw a ją prace nad w ykorzystaniem pełnych możliwości system u.

L IT E R A T U R A

[1] B inder E .E ., Herzog J.H .: D istributed C om puter A rchitecture and Fast Parallel A lgorithm s in R eal-T im e R obot C ontrol. IE E E T ransactions on System s, M an, and C ybernetics, vo l.16, nr 4, 1986, s. 543-549.

[2] C raig J .J .: In tro d u c tio n to Robotics. M echanics and C ontrol. Addison-W esley P u ­ blishing Com pany, R eading 1989.

[3] Vuskovic M ., Liang T ., A n a n th a K.: D ecoupled P arallel Recursive N ew ton-Euler A lgorithm for Inverse D ynam ics. Proceedings of th e 1990 IE E E In tern atio n al Confe­

rence on R obotics and A utom ation, C incinnati, OH, USA, 1990, s. 832-838.

[4] W arczyński J ., W róblewski W .: Inverse D ynam ics A lgorithm for R obot M anipula­

tors. F oundations of C ontrol Engineering, vol. 13, n r 2, P oznań 1988, s. 93-100.

[5] W róblewski W .: M etody m odelow ania dynam iki m anipulatorów d la celów sterow a­

nia. R ozpraw a doktorska, P oznań 1992.

[6] Y am ak ita M ., Hoshino Y ., M orim oto K., F u ru ta K.: P arallel Im plem entation of N ew ton-E uler A lgorithm w ith One S tep Ahead P rediction. Proceedings of th e 1991

3 6 8 K. Rom anow ski, W. W róblewski

IE E E In tern atio n al Conference on R obotics and A uto m atio n , S acram ento, CA, USA, 1991, s. 1842-1849.

Recenzent: Dr inż. Tadeusz Szkodny W płynęło do Redakcji do 30.04.1994r.

A b s t r a c t

T h e co m p u tatio n of a m a n ip u la to r inverse dynam ics is usually accom plished by using a m e th o d based on e ith er th e Lagrange or th e N ew ton-E uler form ulation. T h is paper presents a recursive alg o rith m using th e Lagrange form ulation w ith 3 x 3 ro ta tio n m atrices th a t has been parallelized for MIMD (M ultiple In stru ctio n , M ultiple D ata) com puting arc h itec tu res w ith d istrib u te d mem ory, such as tra n sp u te r system s.

T h e p arallelization m akes use of th e recursive n a tu re of th e algorithm . T h e calcu­

lations p erta in in g to p artic u la r m a n ip u la to r joints are placed on se p a ra te processors so th a t th e re is a one-to-one m apping between th e joints and th e processors. T he processors are connected to form a bi-directional pipeline, which tra n sm its th e d a ta coupling th e jo in t eq uations in two directions. O ne direction corresponds to th e forw ard recursion, in which th e generalized positions, velocities, and accelerations are com puted, and th e o th e r to th e backw ard recursion, in which th e generalized forces are found. An a lte r­

n ativ e m apping is also presented, w here a pair of processors in stead of one is dedicated to each jo in t, so th a t th e forw ard and backw ard recurrence co m p u tatio n s are placed on different processors. Accordingly, th e single bi-directional pipeline is replaced by two u n i-directional pipelines, each dedicated to one direction of th e recurrence d a ta flow.

A d istrib u ted -m em o ry arch itectu re presents a problem w ith recursive relationships, since th e re is a stro n g d a ta dependence betw een th e calculations perform ed by th e p a r­

tic u la r processors, which h am p ers effective parallelization. T h e required interprocessor d a ta flow m ay easily becom e th e dom inant system activity. T herefore th e recurrence eq u atio n s have been modified to m ake use of p redicted instead of ac tu a l values of in te r­

nal kinem atic qu an tities. As it is assum ed th a t th e calculation of th e inverse dynam ics is perform ed rep e ate d ly in tim e, th e prediction is an ex tra p o latio n of th e values a t previous tim e in stan ts.

T h e perform ance characteristics of th e presented algorithm , b o th theo retical and m easu red , w ith respect to th e num ber of processors and th e num ber of m a n ip u la to r jo in ts, are discussed.