De real-time koordinatentransformatie bij de road-robot

De real-time koordinatentransformatie bij de road-robot

Citation for published version (APA):

Kuijpers, P. S. P. (1985). De real-time koordinatentransformatie bij de road-robot. (TH Eindhoven. Afd. Werktuigbouwkunde, Vakgroep Produktietechnologie : WPB; Vol. WPB0208). Technische Hogeschool Eindhoven.

Document status and date: Gepubliceerd: 01/01/1985

Document Version:

Uitgevers PDF, ook bekend als Version of Record

Please check the document version of this publication:

• A submitted manuscript is the version of the article upon submission and before peer-review. There can be important differences between the submitted version and the official published version of record. People interested in the research are advised to contact the author for the final version of the publication, or visit the DOI to the publisher's website.

• The final author version and the galley proof are versions of the publication after peer review.

• The final published version features the final layout of the paper including the volume, issue and page numbers.

Link to publication

General rights

Copyright and moral rights for the publications made accessible in the public portal are retained by the authors and/or other copyright owners and it is a condition of accessing publications that users recognise and abide by the legal requirements associated with these rights. • Users may download and print one copy of any publication from the public portal for the purpose of private study or research. • You may not further distribute the material or use it for any profit-making activity or commercial gain

• You may freely distribute the URL identifying the publication in the public portal.

If the publication is distributed under the terms of Article 25fa of the Dutch Copyright Act, indicated by the “Taverne” license above, please follow below link for the End User Agreement:

www.tue.nl/taverne

Take down policy

If you believe that this document breaches copyright please contact us at:

openaccess@tue.nl

providing details and we will investigate your claim.

Verslag I - I opdracht

DE REAL-TDIE

KOORDTNA TENTRAN'S FOIDIA TIE BIJ DE ROAD-ROBOT

Auteur: P.S.P. Kuypers WPB-rapportnr. 0208

augustus 1985

A~studeerhoog1eraar: Pro£.Dr.Ir. A.C.H. van der Wo1£ Bege1eiders: Ir. C.J. Heuve1man (THE)

In~. J.A.H.G. Bosse (APA)

VOORWOORD

Dit verslag vormt de afronding van een I-l-opdracht in de vakgroep '~PB op de afdeling If aan de TH-Eindhoven. De opdracht werd verricht in het kader van mijn werktuig-bouwkundige studie aan de THE. Het verslag behandelt de

real-time koordinaten-transformatie voor de ROAD-robot. Er worden transformatie-algorithmes ontworpen, die

uiteindelijk in PASCAL geimplementeerd worden.

Het ROAD-projekt is een samenwerkingsverband van de THE en het Eindhovense bedrijf APA. De begeleiding van mijn opdracht in dit projekt werd verzorgd door ira Heuvelman (THE) en ira Bosse (APA), die ik bij deze wil bedanken voor hun bereidheid om steeds voor mij klaar te staan. Hiervoor wil ik ook mijn afstudeerhoogleraar, prof.dr.ir. v.d. Wolf, bedanken. Daarnaast gaat mijn speciale dank uit naar ire Smit (THE) voor zijn bijdrage aan het imple-menteren en testen van de transformatie-algorithmes.

Paul Kuypers

INHOUDSOPGAVE

VOORWOORD

1 : INLEIDING

1.1 'Rea1-time'-koordinatentransformatie 1.2 De besturingsopbouw bij de ROAD-robot 1.3 Transformatie-voorbereidingen

1.4 De atan2-funktie

2 : PROBLEENSTELLING EN OPLOSSINGSHETHODEN 2.1 In1eiding

2.2 Wiskundige formu1ering

2.3 Keuze van de op1ossingsmethode

2

5

6

8

9

12 12 15 3 : TR...\KSFORNATIE VOOR KINEHATISCHE KONFIGURATIE 1 3.1 In1eiding

3.2 De bepa1ing van q1' q2 wijze I

en

oplossings-3.3 De bepa1ing van q1' q2 en q3: op1ossings-wijze II

3.4 De bepa1ing van q4' q5 en q6 J. 5 ~Iechanische beperkingen

3.6 Mechanische vrijheden

3.7 Het algorithme voor konfiguratie 1: oplossingswijze I

3.8

Het algorithme voor konfiguratie 1: op1ossingswijze II 18 20 28 33 39

40

42 47

4 : TRANSFOmlATIE VOOR KINEMATISCHE KONFIGU&\TIE 2

4.1 In1eiding 49

4.2 De bepa1ing van Q4' Q5 en Q6 49

4.3 ~lechanische beperkingen 51

4.4 Nechanische vrijheden 51

5 : TERUGTRANSFOIDlA TIES 5.1 In1eiding

5.2 Kinematische konf'iguratie 1 5.3 Kinematische konf'igura tie 2

6 : DE nfPLEHENTATIE IN PASCAL

6.1 Programma voor konf'iguratie 1 met op1ossings-wijze I

6.2 Programma voor konfiguratie wijze I

6.3 Programma voor konf'iguratie wijze II

7

NAivOORD GER~~DPLEEGDE LITERATUUR BIJLAGE::"l: Initialisatie-procedure Test-procedure 2 met 2 met

op1ossings-Procedure voor vector- en matrixberekeningen Atan2-procedure 54 54 57

59

68 73 80 82 85

89

94

99

1 INLEIDING

1.1 'Real-time'-koordinatentransf'ormatie

Het programmeren van industriele robots geschiedt nog vaak volgens de klassieke teach-by-doing methode. Hierbij beweegt de programmeur de robot, eventueel gebruik makend van een

teach-box, terwijl de encoders in~ormatie over de tussenpo-sities naar de geheugens zenden. Bij het a~spelen wordt deze in~ormatie dan gebruikt om de servo-motoren aan te sturen.

Oorspronkelijk kon men met de teach-box de servomotoren slechts ona~hankelijk aansturen, zodat het al moeilijk was om simpele banen met een redelijke nauwkeurigheid te doorlo-pnn. Daarom raakte het gebruik van koordinatentrans~ormatie tijdens de teach-~ase in de tweede hel~t van de jaren 70 in zwang.

Naast het teach-by-doing programmeren ontwikkelde zich ook het o~~-line programmeren. Hierbij de~inieert de programmeur de gewenste ruimtelijke baan aan de komputer. Via baaninter-polatie en koordinaten-trans~ormatie worden de tussenposities, uitgedrukt in robotkoordinaten, berekend.

De voor- en nadelen van deze twee methodes zijn: TEACH-BY -DOING }1ETHODE:

Voordelen: • Het aanleren is eenvoudig

Nadelen

• Direkte visuele kontrole

• Onmogelijke opgaven worden vermeden

: • Komplexe banen moeilijk te programmeren • Beperkte nauwkeurigheid

• Sensorin~orma tie-ver\verking nagenoeg onmogelijk

• Noeilijk in te passen in een CAD-CAH-orga-nisatie

OFF-LINE ~IETf!ODE:

Nadelen

• Verhoging van de flexibiliteit

• In te passen in een CAD-CAH-organisatie • Sensorinformatie-verwerking is mogelijk • Komplexe banen met redelijke

nauwkeurig-heid teOrealiseren

• Er wordt veel ruimtelijk inzicht van de programmeur gevraagd

• Gevaar voor onmogelijke opgaven

De voordelen van het off-line programmeren leiden tot een steeds belangrijker rol van deze methode.

Sensoren vinden geleidelijk aan meer toepassing bij robots. Een off-line geprgrammeerde robot kan gebruik maken van eventuele sensor-informatie. en bijvoorbeeld zijn baan aanpassen om een obstakel te vermijden. De elementen van de besturing t waaronder de transformator, moeten een derge-lijke snelle verandering kunnen volgent opdat zij de snel-hpid van de robot niet beperken. Men stelt dus eisen aan de snelheid van de koordinatentransformatie en in dit verband spreekt men van 'real-time'-koordinatentransformatie.

1.2 De besturingsopbouw bij de ROAD-robot

De ROAD-robot wordt geschikt gemaakt voor off-line program-meren. Men eist derhalve real-time koordinatentransformatie. De plaats van de te ontwerpen transformator in de besturings-opbouw is weergegeven in figuur 1.1.

De terugkoppeling van werkelijke koordinaten via de terug-transformator naar de baangenerator is in het huidige ROAD-ontwerp nog niet gerealiseerd en is in figuur 1.1 dan ook gestippeld weergegeven. De gedachten gaan er echter weI naar uit dit in de toekomst te realiseren.

Om de transformator te kunnen testen ontwerpen we nu reeds een terugtransformator, die robot-koordinaten omrekent

S

_I I I programmeurl

•

I

BAANGENERATOR karthesische koordinaten

I

GROFINTERPOLATOR

I

10 Hz karthesische koordinaten

I

TR4.NSFOIDtA TOR

I

10 Hz robot-koordinaten

I

FIJNINTERPOLA T(1)R

I

200 Hz robot-koordinaten

I

SERVO-STURING

I

- - - 1

I

I

I

I

I

werkelijke

I

karthesische

I

koordina ten

I

I

I

I

TERUGTR.\NSFOIDIATOR Iwerkelijke Irobot-Ikoordinaten

I

SERVO-NOTOR

- - - -1

SENSOR Fig. 1.1

nnar karthesische koordinaten.

De servo-sturing regelt in het huidige konsept zowe! posi-tie als snelheid. In de toekomst wil men ook hogere tijds-a~geleiden gaan bekijken, om zodoende rekening te kUnnen houden met het dynamisch gedrag van de robot.

Zoa1s uit figuur 1.1 b1ijkt. moet de transformator zijn berekende robotkoordinaten afgeven met een frekwentie van 10 Hz. Dit betekent, dat de transformatieberekeningen niet 1anger dan 100 ms mogen duren.

1.1 Transforrnatie-voorbereidingen

De transforrnator ontvangt inforrnatie over de too1center-vector en de too1-ori~ntatie. De too1centervector wordt gegeven in karthesische koordinaten. terwijl de tool-orien-tatie in ro1l-pitch-yaw hoeken geleverd wordt. Ten behoeve van het transformatieprogramma wordt uit deze hoeken een matrix bepaald, waarin de stand van het handassenste1se1

t.o.v. het vaste wereld-assenstelsel gegeven is.

De 'tool' aan de 1aatste schake1 van de robot kan verwis-seld worden. Verwisseling heeft echter gevolgen voor de transformatie. Het zou betekenen, dat er voor verschillende tools verschi1lende transforrnatoren nodig zouden zijn.

x y z roll pitc yaw h Fig. 1.2 OI-IZETTIN'G r,p.y

~

A x y z A TOOL- x' TRANS-y'

TRANS FOR- z' FOIDIA-

_-..

~IATOR

A' TOR

'~e ldllen de transformator evemie1 een universeel karakter eeven en daarom plaatsen we voor de transformator een

'too1-transformator, die bij gegeven too1centervector en too1tatiematrix de korresponderende positie (x',y',z') en orien-tatie (AI) van de laatste robotschakel berekent. Deze

infor-matie dient dan a1s input voor de transformator.

Zo zal de ROAD een 1astoorts kunnen gaan dragen vo1gens figuur 1.3:

Figuur 1.3

~toolorientatie

!.tool rientatie

In bovenstaand geval draait de tooltransformator het hand-assenstelsel om de y-as en wel over een hoekcx., en dit

levert de orientatie AI. Bovendien transleert ze'het

toolcenterpoint over een afstand Itc naar het middelpunt van het laatste scharnie~

Door de modulaire opbouw van het programma is overscha-keling van de ene tool naar de andere zeer eenvoudig voor "at betreft koordinatentransformatie.

1.4 De atan2-funktie

De atan2-funktie is een alternatieve formulering van de arctan-funktie, en heeft een aantal voordelen waardoor zij bij koordinatentransformatie vaak gebruikt wordt. De

funk-tie behoort niet tot ISO-PASCAL, maar zit wel standaard in bepaalde HP-apparatuur.

De tan-funktie is een periodieke funktie met peri ode IT. De funktie neemt derhalve op het trajekt -li ~ x ,,+U twee rnnal dezelfde waarde aan. Om de oplossing van de arctan eenduidig te maken is dnarom besloten, dat de arctan een oplossing geeft tussen -~rr en

+tr.

Bij de transformatieberekeningen doet zich dikwijls het geval voor, dat sin x en cos x bekend zijn, en dat daar-uit x bepaald moet worden.

Uit:

x'

=

arctan(sin

X)

cos x

vinden we de waarde voor X l . Een teken-analyse van sin x

en cos x leert ons in welk kwadrant de x moet liggen. Om de juiste waarde voor x te vinden kan het zijn dat we

Xl nog moeten vermeerderen of verminderen metTI.

Fig. 1.4: de tan-funktie

De atan2-funktie zoekt zelf de juiste oplossing als zijn t"ee komryonenten x en y opgegeven worden. (zie fig. 1.5)

B

=

atan2(y,x)

y

Fig. 1.5 x

De atan2-funktie heeft nog meer voordelen. Als x in fig. 1.5 een zeer kleine waarde aanneemt, dan znl de verhouding y/x naar oneindig gaan. Bij gebruikmnking van de arctan-funktie gaan afrondfouten in een dergelijk geval een nadelige rol van betekenis spelen. De atan2-funktie is evenwel zodanig

gedefinieerd, dat ze een uniforme nauwkeurigheid heeft over het gehe1e bereik van de funktie.

De atan2-funktie zoekt zijn op1ossing in het gebied:

2 PROBLEE~STELLING EN OPLOSSINGSMETHODEN

2.1 Inleiding

Om de positie en de orientatie van een voorwerp in de ruimte mathematisch te beschrijven zijn er

6

onafhankelijke

parame-ters nodig (bijvoorbeeld J koordinaten van het zwaartepunt en J eulerhoeken). Als de robot een dergelijk voorwerp dus. op aIle plaatsen in aIle orientaties moet kunnen brengen, uiteraard binnen het ¥erkgebied, dan moet hij over minimaal

6

vrijheidsgraden beschikken.

Robots met meer dan 6 vrijheidsgraden voldoen in principe ook: men spreekt hier van 'redundante robots'. Deze redun-dante robots hebben de mogelijkheid om posities en/of

orien-taties aan de rand van het werkgebied aan te nemen, welke

Yoor ~en roLot m0t 6 graden van vrijheid niet haalbaar geweest zouden zijn, in verband met de mechanische beperkingen van de scharnieren. ~!ede door de niet-eenduidigheid van de

oplos-sing zijn de mathematische behandelingen van een redundante robot zeer komplex, hetgeen hem op het ogenblik niet aantrek-kelijk maakt voor een real-time-verwerking.

~itgaande van een 6-assige robot stelt zich nu het probleem om het verband te vinden tussen de

6

parameters welke positie en orientatie bepalen, en de

6

paramters behorende ~ij de 6 vrijheidsgraden van de robot. In verband met de real-time-verwerking is het bovendien noodzakelijk snel over te kunnen gaan van positie/orientatie-koordinaten naar robotkoordinaten en vice versa.

2.2 ~iskundige formulering

De positie en de orientatie van een voorwerp in de ruimte leggen wij vast met behulp van een assenstelsel {~O'

XO'

~o},

opgehangen aan de vaste wereld. De zes bijbehorende parameters of koordinaten groeperen we in de vector x.

(2.1) Voor de ROAD-robot (nieuwe concept) is gekozen voor

6

graden

van vrijheid, zodat we te maken hebben met

6

robo~koordi­

naten, samengebracht in de vector q :

(2.2)

De besturingsopbouw is zodanig, dat de interpolator een

baan ~

=

f{t) doorgeeft aan de transformatory die dit

gegeven vertaalt naar robotkoordinaten: q

=

g{t). Om deze

vertaling te kunnen maken wordt nu dus gezocht naar de

verbanden tussen x en

g

q

=

G{~)

x

=

F{q) (2.3)

In tegensteIIing tot verband F is het verband G meestal niet direkt expliciet uit te drukken. Indien immers een aantal van de vrijheidsgraden rotaties zijn, dan vinden we in de

betrek-king G termen van de vorm cos q. en sin q . • Deze termen

zor-~ ~

gen er voor, dat de relaties niet-lineair worden. Dit bemoei-lijkt het expliciet uitdrukken van de koordinaten van q in

de koordinaten van ~ , of maakt het zelfs onmogelijk.

Een bijkomend probleem is de niet-eenduidigheid van de op-1ossing~q. Een gewenste positie x kan name1ijk weI op meer

-

-dan een wijze gerealiseerd worden. Een simpe1 voorbee1d wordt getoond in fig 2.1. 3 I I , I 1 ; : 1 ' , I , / I 1

---:j/

-

, - - - _ _ _ tiL ... ..,... 3' _{I ... '} 4 fig. 2.1

.\ndersom, in geval van gegeven robotkoordina ten, 1iggen

Voor bepaalde robotkonfiguraties kan het verband q

_-

=

a(x)

-weI exp1iciet uitgedrukt worden, meestal op basis van

analy-tisch-geometrische beschouweingen. In het algemeen zal men echter zijn toevlucht moeten nemen tot iteratieve methodes om de niet-lineaire vergelijkingen op te lossen. Van Aken en Jacobs ( l i t .

[1))

maken echter melding van moei1ijkheden, die optreden, wanneer men tracht een real-time v0rwerking te verwezen1ijken bij robots met meer dan J vrijheidsgraden op basis van iteratieve methodes.

Om iteraties te omzeilen werken zgn. 'incrementen-methodes' daarom met een linearisatie rond het werkpunt. Hogere

orde-termen in de Taylor-ontwikkeling worden daarbij weggelaten en slechts de eerste-orde-term wordt beschouwd. Men brengt hier dus een vereenvoudiging aan.

Cntwikkeling volgens een Taylorreeks gecft:

~F ,2F ~

d F ( ) (V) d 1 (0 - ') • d

.-x. + X.= q. + -~- .• q. + 2 ~~2; q_; + ••

-~ - 1 -~ vX ~ -~ v . ~

':iegla ting van hogere orde-termen geeft: dx. - 1

of

= (;-).' dq. = J (q _ ) . dq . v~ ~ -~ -~-1

(2.4)

(2.5) De matrix J

(q. )

- 1 is de ~xk ~ q1

zgn 'Jacobiaanmatrix' met als elementen: met k

=

1,2 ••.•

6

1 = 1,2, ••• 6

(2.6)

De vektor q. geeft de plaats van het werkpunt in

robotkoBr-~

dinaten aan.

Voor een bepaalde gewenste ver~laatsing uit het werkpunt x vinden we de bijbehorende q u i t :

Aq

=

J(q.)-l· AX (2.7)

_1

-Het nieUi,e werkpunt is nu:

~i+1 = ~i + Aq (2.8)

Het berekenen van de Jacobiaanmatrix en de inverse vergt veel rekentijd. De verschillende incrementen-methodes

onder-~chciden zich dan ook voornamelijk van elkaar vanwege een andere berekeningswijze van deze Jacobiaan. Twee van deze incrementele methodes zijn de methode van Renaud ( lit.[2],

Het bepalen van 6q gebeurt volgens

(2.7)

met behulp van

1

-J(~}- • Deze inverse kan evenwel aIleen bepaald worden als de Jacobiaanmatrix niet singulier is. In bepaalde situaties kunnen echter singulariteiten optreden. Het trans£ormatie-programma moet in een behandeling van deze situaties voorzien.

V~~r de trans£ormatie bij de originele ROAD is ook gebruik gemaakt van een incrementen methode. De ROAD volgens oud concept was echter slechts een 5-assige robot. De robot in zijn huidige opzet hee£t echter 6 vrijheidsgraden. De

benodigde rekentijd voor de Jacobiaan-berekening zal hier-door aanzienlijk toenemen.

Een variant op deze incrementen-methodes is de zgn. 'snel-heids-methode' (eng. 'resolved rate motion control'). Ook zij berust op een linearisatie rond het werkpunt.

Di££erentieren naar de tijd van vergelijking

(2.5)

levert:

"

dx.

=

J(q.)·dq. -~ -~-~ • 1 • Aq. _{_ 1.} = J(q.}- .Ax _~ -• q. 1= q. + Aq·At -~+ -~

(2.10)

J ~ •• \

(2.11)

~t in deze laatste vergelijking stelt de incrementsduur v~~r.

Het singulariteits-probleem zal ook hier weer een rol spe-len.

De principes van deze methode werden gepubliceerd door l.'hitney (lit. [6J t (7) )

De analytisch-geometrische berekeningsmethode zoekt een expliciete uitdrukking voor q uit ~ via een meetkundige

bpschouwing van het kinematisch probleem (voorbeeld l i t .

tIl)

01 door analyse van de trans£ormatiematrix (voorbeeld l i t .

[~1)

.

2.3 Keuze van de oplossingsmethode

Bij de keuze van de oplossingsmethode kan men zich slechts beroepen op een gering aantal tests, die de oplossings-methode onderling vergelijken.

Luc van Aken en Robrecht Jacobs hebben aan de Katholieke Universiteit van Leuven een eenvoudig systematisch onderzoek ingesteld naar transformatie-berekeningen. Voor een bepaalde kinematische konfiguratie (Cincinnati Hilacron T~-robot) werden onderling vergeleken:

snelheidsmethode volgens Whitney - incrementenmethode vo1gens Renaud

incrementenmethode volgens Lien

- analytisch-geometrische berekeningsmethode

Daarbij moet opgemerkt worden, dat de transformatieprogram-ma's nog niet in hun ideale vorm gegoten waren en dat de

tests z~er beperkt waren. Uit de resultaten kan men dus niet meer dan tendenzen afleiden. Bovendien werd de test slechts gedaan voor een kinematische konfiguratie. Een andere kinematische lay-out zal snelheid en nauwkeurigheid van de transformatieberekeningen mogelijk beinvloeden. Over de wisse1werking tussen kinematica en transformatiebereke-ningen is nog nauwelijks iets bekend.

Toch is de conc1usie van het onderzoek van van Aken en Jacobs zeer interessant. De analytisch-geometrische

berekeningsmethode bleek namelijk zowel qua nauwkeurigheid als qua snelheid duidelijk de beste.

De goede nauw'keurigheid is mede te danken aan het fei t, dat men slechts 1 vereenvoudiging toepast: de discretisat~e van de baan. De incrementen- en snelheidsmethode bevatten immers nog een extra vereenvoudiging: de linearisatie

(Jacobiaanmatrix).

Cok qua rekensnelheid is de analytisch-geometrische bereke-ningsmethode aantrekke1ijk. Bij deze methode hoeft men

immers niet te itereren, lineariseren of invert~ren. hetgeen

b~trekkelijk tijdrovende calculaties zijn. De rekensnelheid wordt bovendien nog eens bevorderd door de prob1eemgerichte aanpak van deze methode. Daarentegen is de methode niet

toepasbaar voor elke kinematische konfiguratie.

Het is de bedoeling tlV'ee transforma tieprogramma t s te maken

konfigu-raties lijken beiden veel op de konfiguratie van de in Leuven geteste Cincinnati Milacron TJ-robot. Ze hebben aIle drie dezel£de rotaties van voet, boven- en onderarm. Alleen de kinematica van de polsgewrichten verschilt.

CINCINNATI ₄

₅

₆

NILACRON

~

TJ - ROBOT 4

₅

6

_.' ALTERNATIEF 1

--<I>-(-[}-4

₅

6

ALTERNATIEF 2

---<t>-(---<r>-Figuur 2.2 De polsgewrichten

Het ligt dus voor de hand om het transformatieprobleem voor deze 2 kon£iguraties ook op te lossen met behulp van analytisch-geometrische berekeningsmethode.

In de volgende 2 hoo£dstukken is deze methode dan ook uitge-"erkt v~~r deze 2 kinema tische kon£igura ties.

Opm. De bovenstaande kinematika-notatie is volgens VDI-norm

no.

2861:

~--

=

Ruigende be\,"eging in het vlak van tekenine = Buigende beweging loodrecht op het vlak van

tekening

., TRANSFOl~1ATIE VOOR KI~D]ATISCHE KO~FIGUR"\TIE 1

J.1 Inleiding

Voor kinematische kon~iguratie 1 (zie onderstaande ~iguur) zal nu een transformatieprogramma worden a£geleid v01gens de ana1ytisch-geometrische berekeningswijze. Figuur 3.1 POLSGE1'iRICHT 'too1-center'

4 5 6

--<I>--C.--rn--1=0

...

..

Voor de pols is een konstruktie bedacht, waarbij de draaingsassen van scharnier 5 en 6 door een punt gaan: d."'z. de a£stand 1 tussen deze punten is ge1ijk aan nul. Cook de draaingsas van scharnier 4 gaat door dit punt.

~;e noemen dit punt; 4' en het za1 een be1angrijke rol in de berekening gaan spe1en.

Dij de berekening maken we namelijk gebruik van het feit,

_.

dat de punten 1,2.),4 en 4' in een vlak 1iggen, het zgn.

(u,v)-v1ak.

We de£ini~ren een assenste1se1 {~O' IO' !O} , opgehangen aan de vaste wereld. Een vector r , uitgedrukt in

De tool-transformator levert aan de transformator de infor-matie over de robothandpositie en over de hand-orientatie, d.w.z. de gewenste stand van het assenstelsel opgehangen aan de hand (de laatste schakel van de robot). Al deze

informatie wordt uitgedrukt in koordinaten behorend bij het vaste wereld-assenstelsel.

Het transformatieprobleem laat zich nu als voIgt formu-leren:

-Gegeven : • gewenste robothandpositie

°E4'

• gewenste stand handassensteisel

{O~6,O~6'O~}

-Gevraagd: • robotkoordinaten ql tim q6

\{erkwijze: 0

-

_E4'

0 0 0

-

_.z.6'

~6' ~6 Bepaling van q2 en q., Bepaling van q5 en q6 EIND Figuur 3.2 ql' q4'

De koordinaten van punt

4'

vertellen ons meer over de stand van het (u,v)-vlak en hiermee kunnen we de koordinaten

en q3 bepalen.

Deze eerste drie koordi-naten bepalen de stand van de draaingsas van schar-nier

4

in de ruimte. Ze rnoeten dus bekend zijn voordat we de koordinaten

Q4' qs en q6 gaan berekenen. Deze koordinaten worden dan ook als laatsten berekend.

Voor de berekening van ql

tim q3 worden er twee op-lossingswijzen geformuleerd, die beiden ongeveer evenveel bewerkingen nodig hebben om hun oplossing te bepalen. Latere tests moeten uitwijzen welke van deze oplossingswijzen de beste nauwkeurigheid en/of snelheid

he eft bij de apparatuur, waarop het programma uiteindelijk geimplementeerd zal worden. Zo laat zich het effekt van het gebruik van tabellen bij de berekening van goniometri-sche funkties vooralsnog moeilijk inschatten.

Bij de berekeningen zal rekening gehouden worden met moge-lijke niet-~~nduidigheden in de oplossing.

3.2 De bepaling van gl~2 en gJ: oplossingswijze I

We weten dat de punten 1,2,J,4 en 4' in " n vlak liggen, het zgn. {u,v)-vlak. Een vlak wordt bepaald door J punten van het vlak, tenzij die J punten op " n lijn liggen. Punt 1 en 2 zijn vaste punten in de ruimte. Ook de positie van punt 4' is nu bekend. De stand van het (u,v)-vlak is nu dus bepaald, bchalve als dit punt op de !O-as ligt. Dit geval moet apart bekeken ,.;orden.

~erst wordt de situatie bekeken waarbij punt

4'

niet op de ~o-as ligt en het vlak dus bepaald is. Gezocht wordt naar

punt J in dat {u,v)-vlak. Bekend zijn de lengten van de schakels, d.w.z. de af'standen tussen de punten:

II

!' 2 - EJII

=

12 (J.l)

1I!4 ,-

!:

311

=

lJ (J.2)

Het probleem laat zich dus herleiden tot het zoeken van het snijpunt van een cirkel met middelpunt £2 en straal 12 en vnn een cirkel met middelpunt

!4'

en straal lJ"

T~ee snijdende cirkels hebben in het algemeen" 2 snijpunten. ;:r zijn dus 2 mogelijke oplossingen, hetgeen in figuur 2.1 reeds geillustreerd werd. De keuze uit deze mogelijkheden kan men overlaten aan de programmeur. Deze kan door het zetten van een 'vlag' aangeven welke oplossing, welke mode, er gepref'ereerd wordt:

• de 'up-mode' (d.w.z. punt J in f'iguur 2.1) • de 'down-mode' (d.w.z. punt

J'

in f'iguur 2.1)

Daarbij moeten we bedenken, dat de keuze van de mode invloed kan hebben op de orientatie en/of het toolcenterpoint. Koor-dinaat q4 heeft immers als rotatieas de lijn door de punten

J

en

4'.

Bij gegeven koordinaten ql' q2 en q3 kunnen de

pols-koordina ten q" q_ en q6 echter niet alle orienta ties

4 :J

meer realiseren, in verband met de mechanische beperkingen van de scharnieren. Zo kan het voorkomen, dat bij keuze van de up-mode een bepaalde kombinatie van orientatie en tool-centerpoint niet aangenomen kan worden, terwijl deze kombina-tie weI verwezenlijkt zou kunnen worden bij keuze van de

down-mode. Op dit probleem komen wij in hoofdstuk nog terug •

...

_...

"

Fig

J. J

'normal' -mode Fig.3.4 'overhead'-mode

Ook bij het zoeken naar ql merken wij een

niet-eenduidig-heid OPe Als punt

4'

niet op de !O-as ligt, dan kan ql

afgeleid worden uit de stand van het (u,v)-vlak. Deze stand kan echter op twee manieren gerealiseerd worden: indien ql

voldoet, voldoet immers ook ql

!

7r • Een posi tie kan immers

Inormaal' of 'overhead' bereikt worden.

In de meeste gevallen is het overgaan op een overhead-beweging

niet nodig. Slechts als een baan de ~O-as zou snijden, zou

zo'n overhead-mode handig zijn. Indien zo'n baan nu met de normal-mode doorlopen zou worden! zou de voet bij het

c

In principe zou deze beweging zelfs oneindig snel uitge-voerd moeten worden. In verband met massatraagheden is dit natuurlijk onmogelijk. Naar zelfs als het mogelijk was, zou zo'n snelle draai trillingen exciteren en dit zou baanonnauwkeurigheden tot gevolg hebben. Daarbij moe ten we ons weI realiseren, dat deze onnauwkeurigheden

veroor-zaakt worden door het dynamische gedrag en niet door de trans£ormator, die de theoretisch juiste robotkoordinaten blij£t geven.

De betekenis van zo'n overhead-mod. is evenwel maar zeer beperkt. Het vormt aIleen een oplossing voor een baan die

precies door de ~-as gaat. Ben baan daar vlak naast zal echter in de

x

Figuur

3.5

normal mode doorlopen moeten worden. Stel: men wil deze baan met een zekere snelheid verwezen-lijken. In een bepaalde tijd Il t zal dan een bepaal-de afstand Ax a£gelegd worden. Bij deze bx hoort een bepaalde 6ql (zie figuur

3.5).

Hoe dichter we echter bij de ~O-as komen, hoe groter Aql wordt. Er is hier

sprake van een limiet-situatie. Als de afstand van de baan tot de

~O-as

naar 0 gaat, gaat 2sl naar oneindig.

dt

Het d)~amisch gedrag van de robot bij banen nabij de ~O-as

zal dus een nadelige invloed hebben op de baannauwkeurig-heden. Een oVierhead-belveging van de robot gaat lveliswaar niet gepaard met

*1-

00 , maar is slechts te gebruiken

bij banen door de ~O-as en vormt dus geen wezenlijke op-lossing voor bovengenoemd probleem.

Toch willen we de overhead-mode invoeren, en weI om een heel andere reden. Het bereik van ql is dikwijls kleiner

dan 2 , zo ook bij de ROAD-robot. Toepassing van een over-head-mode levert in dat geval een belangrijke vergroting van het werkgebied van de robot.

De uitdrukkingen v~~r _ql normal-mode:

r

1 = ql = overhead-mode:{Ql = ql

=

zijn hiermee:

(Ql)vorig als X4'=Y4'=O

atan2(Y4' ,x4') in andere gevallen

(3.3)

als x4'=Y4'=o

(3.4)

atan2(-Y4"-x4 ,} in andere gevallen De koordinaat Ql is dan gedefinieerd zoals aangegeven in figuur

3.5,

d.w.z. rechtsdraaiend volgens de kurketrekkers-regel. Ook de andere

koordinaten zullen rechtsdraaiend gede-finieerd zijn. Figuur

3.6

z -as =0 x -as .:.:.0

Vervolgens worden q2 en

q3

bepaald.

~-as

Punt 4' ligt in het (u,v)-vlak en we berekenen eerst zijn (u,v)-koordinaten. De (u,v)-koordinaten van punt 2 zijn per definitie (0,0). (Zie ook fig.

3.7)

(3.4)

De vergelijking van de cirkel met middelpunt E2 is nu:

2

1

Figuur

3.7

4'

(u,v)-v1ak

De verge1ijking van cirke1 met midde1punt r4' 1uidt:

Deze verge1ijking werken we uit en substitueren verg.

(3. 6 )

hierin:

(U

2

_{- -}

?u.u

_4'

+ u

2

_4'

) -- 13

2

-

(v - v

₄

,)2

- 2u·u4'

-2uou4' =

{(1~

- 1;) -

(u~,

+

v~,)}

+ 2v4'ov

(3.8)

Verge1ijking

(3.6)

wordt vermenigvu1digd met 4(u4,)2 en vervo1gens substitueren wij hierin verge

(3.8).

Dit 1evert:

Dit is een norma1e vierkantsverge1ijking van de vorrn

.2

av + bv + c

=

0 met

( 2

- u4' +

_(3.10)

De verge1ijking heeft geen reeele op1ossingen a1s D~O. Dit moet in het programma dus getest worden. D<O betekent dat de gewenste handpositie buiten het werkgebied van de robot va1t.

In het geva1 dat D >,: 0 kunnen we de op1ossing schrijven

a1s:

= -b

±

V

b2_4a~

2a (3.11)

Voor een bepaa1de v3 vinden we de bijbehorende

Uj

uit verge1ijking

(3.8):

u

3

=

;~4'·

[liv4'

+

2v4' v 3]

(3.12) I , u.:>O u<O down Figuur

3.8

Voor de up-mode galdt nu:

- b -

'V ....

b-:!-' --4-a-"c'

indien u

_{4 '}

positief is

indien u4' negatief is

En voor de down-mode:

-b +

v'b2-4a~

_{indien u4' positief is} 2a

-b -

y'

b2 -4ac'

(3.

14 ) indian u4' negatief is 2a

Zoa1s b1ijkt uit verge1ijking (J.12) moeten we de geval-len met u4'

=

_{0 en v 4 '}

=

0 apart bekijken. Daarnaast mag de situatie u4'

=

v4'

=

0 niet optreden en moet daarom getest worden.

Voor de geval1en u4'

=

0 en v₄'

=

0 is eenvoudig een uitdrukking voor u

J en v3 af te 1eiden, bijvoorbeeld met behulp van de cosinusregel.

u u geva1 geva1 u 4 '

=

0 v4'

=

0 v Figuur

3.9

V~~r _{u 4 '}

=

0: 2 2 2

=

v 4 ,+12-1 3 Voor 2v,~ t -tJp-mode

=

{l;_V~)t

., ? ~ -Down-mode: u

J

=-(1--v-)2 2

3

V" = 0: + ..., ._u 4 I ? ').1.

(, -

·")2

=-

~..,-ul .::.. --Up-mode ") ..,.1. -Down-modo: v J

=

(1::;_p~)2 .- -)

:'Iet deze gegevens berekenen we de q3-q2

=

atan2(v₃,u₃)

f

(3.16) robotkoordinaten q2 (3.17) q3

=

~r, -q2-atan2(u4,-u3,v4,-v3) (3.18) v en

Dit zijn de waarden van q2 en q3 in de normal-mode. Om de betre~fende koordinaten behorend bij de overhead-mode te vinden hoe~t men q2 en q3 slechts met -1 te vermenig-vuldigen.

Hierbij is uitgegaan van de de~inities volgens figuur 3.10 en 3.11 e u 3 2~ ________ ~ __ __ v Figuur 3.10 u / /

4'

2~ ______________ ~~ v Figuur 3.11

Ke kunnen de positie van punt 3 ten opzicbte van bet vaste wereld-assenstelsel weergeven door:

V

3

eCOS(Ql) v 3osin(Ql) u

3

+ 11 bij normal-mode bij overhead-mode (3.20)

3.3 De bepa1ing van 91L-S2 en 93= op1ossingswijze II De tweede op1ossingsmethode werkt het prob1eem gehee1 met behu1p van matrices uit. De methode staat beschre-yen in 1i t

t8l

(R. P. Paul).

We maken gebruik van 4A4-matrices van de vorm:

A ~---.~---~ all 8 ₁₂ a 13 : Px T

=

8₂₁ a 22 a 23 : Py a 31 a 32 a33 I Pz

---~---

_o

0 0 : 1

Hierin geeft matrix A een verandering van de stand van het assenste1se1 aan, terwij1 vector peen trans1atie voorste1t. f.1atrix T staat voor een trans1atie gevo1gd door een rotatie.

We kunnen de draaingen van de robotgewrichten voorste11en a1s een aaneenschake1ing van trans1aties en rotaties. A1s gewricht i korrespondeert met matrix R

i , dan kan het effekt van a11e gewrichtsstanden op de robothandpositie en het handassenste1se1 gerep~esenteerd worden door:

Deze matrices R. vindt U gehee1 uitgeschreven in

hoofd-l.

stuk 5. A1s eerste gaan we q1 op1ossen, gebruik makend van de vo1gende uitdrukking:

R- 1 .T

1 = R2oR3oR4·R5·R6 (3.22)

De linker term uitgeschreven: c 1 sl 0 0 all a 12 a 13 0 R- 1 .T ₁ = -s 1 c₁ 0 0

•

a21 a22 a23 0 _(3.23) 0 0 1 0 a₃₁ a₃₂ 8 33 0 0 0 0 1 0 0 0 1

verkorte notaties: c i

.-

.-

cos(qi) c .. 1.J

.-

.-

COS(qi+qj) 8. 0 - sin(q.) Sij 0 - sin(qi+qj)

.-

.-J. 1.

Uitwerking van dit matrixprodukt resulteert in:

clall+sia21 cla12+s1a22 cla13+s1a23 c1Px+slPy

-51a21+c1a21 -51a12+cla22 -sla13+cla23 -51Px+cIPy

a

31 a32 a33 Pz

(3.24)

0 0 0 1

Uitwerking van de rechter term 1eidt tot:

c23(c4CSc6-s4S6)

-~23(C4CSs6+84C6)

_{C23 C4 8}

s

_{5 2J1 3} -s2J s SC6 +5₂₃s_S56 +s23 c S +s2 1 2 s4 cSc 6+C4 s 6 -s4 CS5 6+ c 4 c 6 84 5

5

0 (3.2S) s23(C4 CSc 6-s 4 s 6) 523(C4CSs6+54c6) -s23 c 4s

5

_{c Z31 3+} -c 23s Sc6 +c 2Js Ss 6 +cZJc S c212+11 0 0 0 1

~e hebben dus 12 vergeIijkingen, waaraan voldaan moet

zijn. q1 voIgt uit de vergeIijking:

Zoals we reeds gevonden hadden geeft dit twee mogelijke OpIo5singen: of: = atan2(p ,p ) y x (normal) } (3.27) (overhead)

Bedenk hierbij, dat:

Ook q) i5 uit dit 5te15el vergelijkingen op te lo55en, en weI uit de vergelijkingen:

().28)

Om5chrijven van deze vergelijkingen geeft:

{ 5 2 )1) + 52 1 2 =c 1Px + 51Py c 2 )1) + c 21 2

=

P z - 11

{2 2

5 231) + 25 2 )5 2 1 21) + 52 ? 21

2

2 2 2c 23c 2 121) + 2 c 2 )1) + c212 = _{(c 1 Px + 51Py )2} (p - 1 )2 = _{z 1 +} 2 ₁₂ + 21213(c23c2 + 5 23 5 2)= (c1PX+51Py)2+(pz-11)2 13 + ₂ { C ) 2 ( )2 2 2 1 PX+5 1Pv + p z -l1 -13-1 2 212 1 3 s

3

=

!

V(

1 - c

~

)

(3.30)

Er zijn dus twee mogelijke Op1055ingen voor q3. De keuze uit.deze op1ossingen hangt af van het feit of er up- of dOlin-mode vereist is.

Vervolgens bepalen we q2. iVe gaan ,.,eer ui t van verge1ij-king (3.29).

s2(C

313+1 2 ) + c 2 s 31 3 = c 1 Px + S1Py c 2 {c 31 3+12 ) - s~s313 = P_{z -} 11

Indien we nu de eer5te verge1ijking vermenigvu1digen met (5

313) en de tweede met (C313+12) en we te11en vervo1-eens de verge1ijkingen op, dan vinden we een uitdrukking

v~~r

C2«S313)2+(c313+l2)2)=(C1PX+SlPy)s3l3 + (Pz- l l)(C

3l 3+1 2 )

Als we vervolgens de eerste term uit (3.3l) vermenigvu1-digen met (c

313+l2) en de tweede met (-s313) en we tel1en de verge1ijkingen op, dan vinden we een uitdrukking voor

S2{(s3l3)2+{c3l3+l2)2)=(ClPX+S1PY)(C313+l2) - s3l _{3(Pz -1l )} Dit betekent:

q2

=

atan2«clPx+slPy)(c3l3+12)-S3l3(Pz-ll)' (c1Px+slPy)s3l3+(Pz-ll)(c3l3+l2»

Hiermee zijn de koordinaten Q1' Q2 en Q3 gevonden.

De koordinaten Q4'

qs

en q6 zu1len gevonden worden uit de vergelijking:

Hiervan is het linker lid nu bekend. Omdat er voor de laatste drie robotgewrichten geen translaties meer optreden, is het voldoende om slechts de rotatie-matrix te berekenen, volgens veree1ijking (3.34).

A-1.A-1.A-l.A

₃

_{2 1 =} A A A

_{4· 5· 6}

lfanneer we het linker lid uitschrijven v~rschijnt er:

c23(clall+sla2l) c23{clal2+sla22) c23(clal3+sla23)

-s23 a 3l -s23 a 32 -s23 a 33 s23(clall+sla2l) +c 23a3l s23(clal2+sla22) s23(clal3+sla23) +c 2 )a32 +c 23a33

Ter bepaling van Q4' Qs en Q6 moet bovenstaande matrix gelijk gesteld worden aan de uitdrukking in vergelij-king (3.48). V~~r oplossingswijze I worden er in para-graaf 3.4 eerst een aantal voorbereidende bewerkingen uitgevoerd om te komen tot deze vergelijking. Ve~r oplossingswijze II zijn deze voerbereidende bewerkingen even, ... el niet van belang.

Figuur 3.10

met:

o

~4'

=

=

[-Sin

ql

l

_sin

_q1

Y4'x

1

0

of' Y4'y (3.37)

:r.4'

=

cos q1 -c~s q =

0

normal 1 overhead Y4'z

,11e _{hebben te maken met een rechtsdraaiend assenstelsel.}

0 0 J( 0 (3.38) ~4'

=

Y4'

~4' Y4,y·z4'z

-

y z

1

0 -4'z' 4'y (3.39) ~41

=

Y4'z·z4'x

-

Y~l' x' z 4 I z

Y4'x· z 4'y

-

Y4,y·z4'x

Di t assenstelse1 moet door midde1 van q, , q_ en q6 zodanig

q ::>

gemanipuleerd worden, dat ze samenvalt met het gewenste handassenstelsel

~Ieetkundig is niet direkt in te zien, wat deze gewenste manipu1atie inhoudt voor q4' qs en q6" Daarom gaan wij over

Het gewenst handassenstelsel is gegeven in koordinaten van het vaste assenstelsel. Om de benodigde assenstelsel-manipulatie te bepalen is het handig om dit gewenst

handassenstelsel weer te geven in koordinaten behorend bij het assenstelsel

{~4"

!4"

~4'}

•

Voor een vektor a weergegeven in globale koordinaten °a en in 4'-stelsel-koordinaten geldt:

(3.40)

o

1-

1 [ 0 )

~4

'

•

!!

(3.41)

Voor de transformatiematrix R (det R

=

+1) geldt:

Hiermee kunnen we het gewenst handassenstelsel uitdrukken in 4'-koordinaten.

r

4'

L

~6

o

~4'

l~

[0

~6

De gewenste manipu1atie wordt weergegeven door transformatie-matrix A, met A

=

A₄·A

S·A6• De matrices A4, AS en A6 represen-teren de opvolgende polsrotaties.

A

.

[

=

A _• I

= A

= (a .. _~J

)3.3

_x

Voor A

_{4 ,}

AS en A6 kunnen we schrijven: -sin q4

cos q't

o

cos q5 0 _{sin q,}

1

A5 = 0 1 0 (3.46) -sin q5 0 _{cos q5}

f

1 0 0

1

A6 = 0 _{cos q6 -sin q6 (3.47)} 0 _{sin q6 cos q6}

:\4' AS en A6 stellen hierbij rotaties voor om respektieve-lijk de lokale

z-,

Hiermee: .A

=

y- en x-as. s4 s

s

s 6+ c

4

c 6 c,s6

S4

s ,c6- c

4S

6 c

_S

c6

Deze A moet voldoen aan vergelijking (3.44). De waarden a .. van de matrix A zijn dus bekend, zodat we beschikken

l.J

over een stelsel van 9 (niet-lineaire) vergelijkingen met 3 onbekenden.

Om tot eenvoudiger vergelijkingen te komen herschrijven we dit tot: A- l .,\ _{4 • = A} S'A6 (3.49) [ e" 54 0 _{all a} 12 a13 c .. :;)

-:4

c4 0 a2l a22 a23

=

0 0 1 a 31 a32 a33 -5_ :;)

5,5 6

5,C6

C 6

-56

c_Ss6 c Sc6 Direkt te zien is:

-a

₁₁

_{$4+ a 2l c}

₄

₌

0

zodnt: tan _{q4 =} 5_{4 /}

=

a 21 /

c4 _all

Dit gee~t twee oplossingen tussen - l en

n.

{ A: B: q4

=

_{atan2(a 21 ,all )} q4

=

_{atan2(-a21 ,-all )}

Bovendien is uit vergelijking (3.50) q6 te bepalen. -s4 a lJ + c4a 23 -s4 a 12 + c4a 22 Hiermee: En tenslotte ook qS:

{

Cs5

s

=

=

a llc 4 -a_JI + a 2l s 4 (J.55)

In verband met het bereik van Q5' dat bij de ROAD-robot loopt van

-t.

tot +t~, zijn slechts oplossingen met cos q5~O te verwezenlijken. Dit hee£t gevolgen voor de keuze uit de 2 mogelijke oplossingen.

Voor oplossing A geldt: q4 = atan2(a

21,al l). Dit impli-ceert, dat s4 en a₂₁, alsmede c4 en all hetzel£de teken dragen. Zodoende zal cos qs volgens bovengenoemde uit-drukking voor Qs altijd p05itie~ o~ nul zijn.

V~~r oplossing B geldt: q4

=

atan2(-a

21,-al l). Bij deze oplossing verschillen de tekens van a_2l en 54 en ~an all en c4. Hiermee zal cos qs nooit positie~ kunnen zijn. In deze oplossing zijn we dU5 niet geinteresseerd.

Samenvattend kunnen we dus stellen, dat q4' qs en q6 voor het geval cos qs

I

0 berekend kunnen worden volgens de f'ormules:

q4

=

atan2(a

2l,al l)

qs

=

_{atan2(-a)I,a ll c 4 + a 2l s 4 )} ().57) q6

=

atan2(s4al)-c4a2),-s4a12+c4a22)

Wanneer we vergelijking ().48) bekijken, merken we op, dat all en a₂₁ de waarde 0 hebben als geldt C

s

=

o.

In dat geva1 is bijvoorbee1d q4 niet meer te bepa1en met vergelijking ().S7). Deze situatie za1 daarom nader onderzocht worden. We onderscheiden hierbij de mogelijk-heden sin q5

=

+1 en sin q5 = -1.

Voor de si tua tie sin qs

=

+1 zijn er vier terr.len ui t vergelijking ).48 relevant ter bepa1ing van q4 en q6·

a 12

=

c4 s 6- s 4 c 6 a l )

=

c4 c 6+ s 4 s 6 a 22

=

s4 s 6+ c 4 c

6

().58) a 2 )

=

s4 c

6-

c 4 s

6

De uitdrukkingen zijn duidelijk af'hankelijk: _{a l )} = _{a 22}

Het stelsel kan dU5 worden gereduceerd tot een ste1se1 van 2 vergelijkingen:

Ja

12

=

c4 s

6-

54 c 6

l

a l) = _{c 4 c6+ s 4 5}

6

=

sin(q6-q4) = cos(Q6-q4) ().60)

Er zijn dU5 oneindig veel mogelijke kombinaties van q6 en q4' die deze oriintatie helpen verwezenlijken.

Men kan in geval van sin qs = 1 een bepaalde vaste kombinatie van q4 en q6 aannemen, bijvoorbeeld:

i';anneer men dan echter een zekere baan (met q4 .,. 0 )

doorloopt, waarbij sin qs op een gegeven moment +1 wordt, dan zouden q4 en q6 een plotselinge sprong maken.

Dij het vervolgen van die baan zou er bovendien weer een sprong terug gemaakt worden.

In verband met massatraagheden is zoln sprong echter in "erke1ijkheid niet te realiseren. Bovendien heeft zoln sprong trillingen tot gevolg.

Daarom kiezen we een andere oplossing: we 1aten q4 haar laatste waarde behouden, terwij1 q6 een kleine verandering ondergaat.

{

q4 =(q4)vorig

q6

=

_{atan2(a 12 ,a13 ) + (q4)vorig}

(3.61)

;'loge1ijk moet q6 nog aangepast worden zoda t: -7i ~ q6';: iT'

Ook voor sin qs

=

-1 vinden we afhankelijkheid in de termen

a₁₂

=

a₂₃

a

13 =-a22 Dit gee£t het stelsel vergelijkingen:

-c4 s 6- 5

4

c 6 = -sin(Q4+ q 6) -c4 c 6+ s 45₆

=

_-cos(Q4+q6)

Zodat Q4+ Q6 = atan2(-a₁₂,-a 11) ~e kiezen a1s op1ossing:

(q4 )v'nrig

atan~(-a12,-a13) - (Q4)vorig

(3.62)

Ook voor cos q5

=

0 voorziet het programma nu in een oplossing. V~~r elke mogelijke polsorientatie kunnen nu de bijbehorende polskoordinaten berekend worden.

1.5 Mechanische beperkingen

Xiet aIle scharnieren hebben een rotatiebereik van 2". Slechts rotaties binnen bepaalde grenzen kunnen verwezen-lijkt worden. De trans£ormator moet testen o£ aIle door hem berekende robotkoordinaten binnen deze grenzen 1iggen.

Terwi11e van de duidelijkheid zijn komputerprogramma's vaak modulair van opbouw. Zo zou de bovengenoemde test als een module na het transformatie-programma geplaatst kunnen worden. Er kan zich dan echter het geval voordoen, dat er in het begin van het trans£ormatie-programma een koordinaat berekend wordt, die uiteindelijk niet gerea-liseerd b1ijkt te kunnen worden. De reSL van het trans-£ormatieprogramma is dan nodeloos doorlopen. Voor de

snelheid van het programma is dit uiteraard zeer nadelig. Daarom verweven ,.e de tests in het transforma tieprogramma.

V~~r de robot met kinematische konfiguratie 1:

-

V f. _ql ~

_''4

3 _IT

-tV'!

_qz

f'

trr

.. in

$ _q3 .,:

.2

₄

Tr - IT ~ _{q4 'Sf}

3.1

11" 6

-

,rrr~ q5 S'

_trr

... 1-iT ~ 2 q6 ~ ~1T

De mechanische beperkingen voor q5 hebben tot gevolg, dat in het trans£ormatieprogramma aIleen oplossingen vol gens mogelijkheid A be}.;:eken moeten worden. (Hogelijkheid A korrespondeert met cos

3.6 Mechanische vrijheden

Naast mechanische beperkingen heeft deze robot ook nog mechanische vrijheden, met name voor schnrnier

4. q4

kan immers niet aIleen waarden aannemen tussen - TT en

I

1i, maar zel fs -1T f.

q4

~ 3

₆

rr •

Indien de grenzen van

q4

-~ en ~ zouden zijn, dan zou men bij het doorlopen van een baan bij

q4

=

T op een mecha-nische aanslag }tunnen stui ten. De baan zou dan afgebroken moeten worden om

q4

een omwenteling terug te laten draaien

zodat de baan vervolgd kan worden. Zeker in geval van

lassen is zo'n onderbreking uiteraard in hoge mate onwense-lijk. Daarom is de konstruktie aangepast en kan

q4

draaien

o

over 750 •

Tot nu toe werd in de berekening van

q4

slechts voorzien in een oplossing tussen -11 en Tr • De berekening moet nu zodanig aangepast worden, dat

q4

automatisch de U-grens en de 3u-grens overschrijdt.

Ter identificatie van de gebieden van het rotatiebereik van

q4

voeren we een parameter N in:

N ::; 0 _{kO!l.';res pondcert me t - TT '"}

_q4

<.:

IT

N

=

1

korrespondeert met 1T ~

q4

<.: 3lT

N

=

2 korrespondeert met 31T ~

q4

c S1T

V~~r een automatische grensoverschrijding hebben we een kri terium nodig. 'ie berekenen daartoe eers t de waarde Yan q4 met - 11 4 q4 < 1r • Indien q4 voor twee opeenvolgen-de '''aaropeenvolgen-den plotseling een flinke sprong maakt, tcrwijl opeenvolgen-de polsorientatie slechts "einig veranderd zou moeten worden, dan is aui:omatiDche grensoverschrijding gewenst.

Ale kriterium nemen we dus:

Bij de keuze van de wanrde van a moeten we rekening houden met interpolatie-stapgroottes.

Verder bestaat er de mogelijkheid om q4 in een bepaald geoied te zetten, bijvoorbeeld bij aanvang van een baan. Het programma zal dus moeten testen, o£ er een melding

'3.7 .

Het a11:!ori thme voor kon:figura tie 1: oplossings1d ;ze I De 0p1ossingswegen kunnen nu weergegeven worden in een f'low-schema. y

o

INPUT: !:4 t 0 0 0 ~6'

16'

~6 N melding: "Q1 niet ---- ... rea1iseerbaar" a=

4(v~, +u~t)

b= 4v4 ,

l(l~-1;)-(U~,+v~,)]

c=

{l(l~-l;)-(U~, +v~,)] 2-4u~t .l~}

N ~~--t melding: "buiten --- ... 'verkgebied II

2 2 2 u 3

=

U41+12-11~_Y-< _2u y 2 2 1 v

=-(1

-u

)2

3 2 3 up y down melding: tlniet realiseerbaar"

I

v

J=

-b;a

Iii

I

I Q3=-atan2(u

₄

,-u 3,v

4

,-v3) +-!1f -Q2 melding:"Q2 niet realiseerbaar"

0 v

3C~S

qlj

r = v

3

s1n ql

-3

u 3 + 11

+

Y4,xj

-sin 0 ~4t= _{Y4'y =} cos Y4'z 0 all

=

a 12

=

a 1J = a₂₁ = a 22

=

a 2J

=

a Jl

=

a 32 = a J3 = 2

-

tr'

_{q3 "}

_:! 1 r>-N _ _ --1melding: "Q3 niet realiseerbaar" Y N _overhea y.

qlj

ql

x4'xx6x

Y4'xx6x z4'xx6x

x4'x

Y

6x

Y4'xY6x z4'xY6x x4'xz 6x Y4'xz 6x z4'x z 6x

T

Y, z, -y z '+'y ,+'z 4'z 4'y Y4'zz4'x-Y4'xz 4'z Y4'xZ4'y-Y4'yZ4'x + _X4'yX6y + _{x4 t z X6z} + _Y4'yX 6y + Y4'z x 6z + z, _'+'y x6 _y + _{z4'zx 6z} + _{X4 'yY6y} + _{x 4 'zY6z}

+ _Y4'yY6y + _{Y4'z Y6z}

+ _Z4'yY6y + _z4'zY_6z + _{X4 'yZ6y} + X, I z6 '+ z z + _Y4'yZ_{6y + Y4'zz6z} + _Z4'yZ6y + _z4'zz6z

cb

y

qs=-';-ir

Q4=(Q4)vorig Q6=atan2(-a12 ,-a1J ) -(Q4)vorig y q4=atan2(a 21,a11 ) qS=atan2(-aJl,allc4 +a 21 s 4) q6=atan2(s4 a lJ -C4a 2J,-s4 a 12+ c 4a 22)

Qs=trr

Q4=(Q4)vorig Q6=atan2(a₁₂,a 1J) +(Q4)vorig -N

y

(END]

melding: "Q4 niet - - ... realiseerbaartl

:::>---Il0l melding : "Q6 niet

--

...

realiseerbaar"

X.B.

Qs

hoeft niet getest te worden. De

3.8 Het algorithme voor koni'iguratie 1: oplossingswijze II INPUT: melding: "ql niet realiseerbaar"

---

...

melding: "Bui ten --,.. werkgebiedlt

1 q2=atan2«C 1PX+ s l P

y)

(CJIJ+12)-SJ13(PZ-ll)' (CIPx+SlPY)SJlJ+(PZ-ll){C3l3+l2» a l l:= a_2l:= a J1

:=

a₁₂

:=

a 22

:=

a 32

:=

a 1J:= a 2J

:=

a 33

==

q2:=-q2 qJ:=-qJ N N c2J(clall+sla21) -slall+cla2l s2J(clall+sla2l) meld~ng: "q2 niet realiseerbaar" melding: "qJ niet realiseerbaar" -s 2JaJI + c 2Ja31 c2J{cla12+sla22)-s23aJ2 -sla l2 + c l a 22 s2J(cla12+s1a22) + c 23a")2 c2J(clalJ+sla23)

-

_s23a_3J -sla13+cla23 s2J(cla13+sla2J) + c_2Ja_JJ

---tIo---

...

Het algorithme kan nu dus vervolgd worden bij

(2)

van oplossingswijze II (blz.4S)

4 :

TRANSFORN.\TIE VOOR KIKENATISCHE KOKFIGURATIE 2

4.1

Inleiding

Kinematische konf'iguratie heef't een polskinematika vo1gens figuur 4.1 • De rotaties van de

De ook

4 5 6

-<1>-c!----<l>-

voet, de boven-en de onderarm zijn identiek aan deze rotaties in Figuur 4.1

konfiguratie 1. bepaling van de robotkoBrdinaten ql' q2 en q3 za1

op exakt dezelf'de wijze ver1open. Dit hoofdstuk

dan

zal dan ook slechts handelen over de bepaling van de !aatste 3 rotaties.

4.2 De bepa1ing van a,~~

'+ ::.> en

96-De manipulatie van het polskoBrdinatenstelsel \V'ordt weer-gegeven door middel van matrix A = _A4ASA6

.

[C~5

q4 -sin q4 0

1

A4 = sJ..n q4 cos q4 0 (4.1) 0 0 1

lC~~

qs 0 sin

qsj

AS

=

1 0 (4.2) sJ..n qs 0 _{cos qs} cos q6 -sin q6 0

1

.40

6

=

sin q6 cos q6 0 (4.1) 0 0 1 Hiermee:

[C4CSC6-S4S6

-c4 c Sc

6-

s 4 c 6 c4SS A ₌ s4 c Sc

6+

c 4S

6

-s4 c Ss 6+ c 4 c 6 s4 s

s

(4.4) -sSc6

5

₅

56

c~ ::.>

Dit lossen we op volgens Paul (litCS).

~e herschrijven daartoe vergelijking 4.4 •

c4 s4

o

1

[all

a 12 -54 c4

o ·

_{a 2l} a₂₂ 0 0 1 a J1 aJ2 Direkt te zien is:

= 0

Deze vergelijking heeft twee oplossingen:

{ A: B: q4 = atan2(a 2J,a13) q4 = atan2(-a 23,-a13) (4.S) (4.6) (4.S)

Eenmaal ~~n van deze oplossingen gekozen hebbend, vinden we de bijbehorende qs en q6 uit vergelijking (4.6):

Ss = c4a lJ + s4a 23 c_ = a 33 ;J zodat: qs = atan2(c4a l3 + _{s4 a 23' a JJ}}

(4.9)

s6 =-s4 a l l + _{c4 a 2l} c6 =-s4a 12 + _{c4 a 22} (4.10)

Paul besteedt nauwelijks aandacht aan de situatie sin qS=O. Dit korrespondeert met de situatie, dat de z-as van het gewenste stelsel samenvalt met de ~4,-as.

Zoals blijkt uit vergelijking 4.4 zijn in dat geval de termen a

13, a22, a32 en a33 gelijk aan nul. Paul merkt op, dat atan2 volgens vergelijking 4.8 .q4 arbitrair de

,

waarde nul geeft. Deze stand is echter met vele q4 5

Deze oplossing geeft echter een ongewenst verschijnsel. Indien de pols een baan maakt ( met q4

I

a ),

die door de ~4,-as gaat, dan sin qs plotseling 0 en zal q4 naar nul springen. Ook q6 zal desgevolgd een sprong maken. Bij het vervolgen van de baan, zullen q4 en q6 weer

terug springen. Dit is niet zo'n elegante oplossing. Een betere oplossing is de volgende.

Indien a

33 gelijk wordt aan

!

1, dan houdt q4 zijn laatste waarde. Ook q6 maakt nu geen sprong.

(4.11)

4.1 Mechanische benerkingen

Ook de scharnieren in deze kinematische konfiguratie hebben te maken met mechanische beperkingen.

De bereiken van de gelvrichten in konfiguratie 2 zijn:

"1

,

1T

-4"

-

ql ~

-t

11 of _q2 ~

tr

1 4- <:

~lr

-1(lf

q3

-tIT

~ _q4 <f

tIr

-tir

~ _qs -:;

trr

-IT ~ _q6 ~

3in-Eventuele overschrijding van de grens wordt in het programma getest.

4.4 Mechanische vrijheden

q6 heeft een bereik, dn:t groter is dan 2

iT:

De besturing moet voorzien in een keuze uit de meerdere oplossingen, die in principe mogelijk zijn. Wederom maken ve gebruik van een parameter N, die het deel van het bereik aangeeft.

4.S Het algorithme voor konfiguratie 2

Het algorithrne voor konfiguratie 2 is tot

(2)

identiek aan dat van konfiguratie 1. Daarbij kan bij de bepaling van ql' q2 en q3 zowel oplossingswijze I als I I gebruikt ·worden.

qs heeft een bereik van

-tn

tot

tn.

De waarde van qs is dus slechts realiseerbaar als c

S=a33 ~

o.

Door het bereik van q4 (tussen -t~ en t~) moeten we op bepaalde punten overgaan van oplossing A (voor q4) op oplossing B, of vice versa. De wristmode bepaalt welke oplossing gekozen wordt. De fysische betekenis van de wristmode is het best

te zien aan qS. Oplos-sing A geeft q5~O en B geeft qS<'O. melding: "qs niet

L---realiseerbaar"

I

q4=atan2(-a 23, -a l3) y

>-u.T_~ melding: ttq4 niet realiseerbaartf

q6=atan2(-s4a ll+ c 4 a 2l' -s4 a l2+ c 4 a 22)

---N (q4)vorig

=

q4 (Q6)vorit:>'

=

Q6 ''> melding: "q6 niet realiseerbaar"

--...

5 :

TERUGTRANSFORHATIES

5.1 Inleiding

Teneinde de transformatieprogramma's op korrektheid en nauwkeurigheid te testen, voeren we terugtransfor-maties uit. De terugtran5forterugtransfor-maties bepalen uit de berekende robotkoordinaten de werkelijk ingeste1de e10bale koordinaten van het toolcenterpunt.

5.2 Kinematische konfiguratie 1

We maken gebruik van een representatie, zoa1s deze ver-meld staat in een syllabus van prof. V.L. Esmeijer

(lit.[91)·

(5.1)

De matrices in deze representatie zijn van de vorm:

R

=

Hiermee:

I-~-l

=

~

~p

1

A~p

+

1]

R •

=

~--

---~----;'len kan hiermee dus zOlve1 trans1aties als rotatie5 weergeven.

Voor kinematische konfiguratie 1 kunnen we nu schrijven: c 1 -sl 0 0 51 c₁ 0 0 0 R₁= 0 0 1 I 0

1

-0 0 0 I 1 Voorts:

C 2 0 52

I

0

I

1 0 1 0 I 0 R 2= I -52 0 _{c 2 I 11}

- - ----

----+--0 0 0 I 1 c 3 0 53 t 0 I 2 0 1 0 I 0 R3=

,

-53 0 c 3 I 12

---....1.--

_{0 0 0} J 1 c4 -54 0 I 0 I 3R -

_4-

54 c4 0 I _I 0 (S.4) 0 0 1 _{I 13}

;

-0 0 0 t 1 C

_s

0 5 S I _I 0 4 0 1 0 I 0 RS= I -5 0 C

s

I 0 S

---

- - - -

--1--0 0 0 I 1 1 0 0 I 0 0

I

0 _{c6 -56} I 0 0 SR - I 6 6- en xp = 0 ₅₆ c6 I 0 0

- - -- - - -- -+

0 0 0 I 1 1

~itvermenigvuldiging gee£t voor de globa1e x-,y- en z-koordinaat: 0 _c 1C25313 + C152C313 + c15212 x

=

0 5₁C₂s)1) + 5₁5₂C₃1) + 5₁5₂12 (S.S) y ₌ 0 c₂c)1) 5₂5)1) + c₂1₂ + 11 z

=

-Bovendien 1evert dit een uitdrukking voor de stand van het handassenste1se1 ten opzichte van het vaste assen-5te1se1. De matrixooeffioienten zijn:

ail = °1°2(°3°4°5) + °ls2(-s304°5-c355) - 5254

C

₅

a 21 = 5102(0304c5-5355) + 5152(-53°405-c355) + c154c 5

a 31 =-52(030405-s355) + c2(-53c4°5-0355)

a 12 = °1°2°3(°45 556- 54°6) + c102530556 - °15253(°45556-54°6)

+ °1 52

C

_{30556 -5 1{5 45556 + °4c6)}

a 22 = 51c203{04°556-54°6) + 51c253c556 - s15253(045s56-54°6)

+5 152°3°5 56 + °1(54 s 5

5

_{6 + 04°6)}

a 32 =-52c3(c45556-5406) - 5253

C

₅

56 - c253{045556-54c6)

+ °2 c 30556

a 13 = °lc203(04s506+5456) + °1°253°5°6 + °152°3°5 56

-015253(045S06+5456) - sl(54 5 506- 04°6)

a 23 = 510203(045556+54s6) + 5}02530506 +

51520305~6

-s152s3(c45S06+5456) + cl(545S06-c4c6)

a 33 =-52c3(c45Sc6+5456)-5253c506 + c2c 3c5c6

-c253(0455c6+5456)

5.3 K1nemati5che konf'1gurat1e 2

De tweede kinemati5che konf'1guratie kan op deze1f'de wijze behande1d worden.

De matrice5 z1jn nu: c 1 -51 0 0

OR

51 c1 0 0 = 1 0 0 1 I ₀

-

-

- -

1--0 0 0 I 1 c 2 0 52

I

0 I 1R ₌ 0 1 0 I 0 2 0 _{c 2}

I

11 -52 ....j.. -0 0 0 I 1 c 3 0 53 I _I 0 ? 0 1 0 I 0 (5.7) -R _{= I} 3 -53 0 c 3 I 12 - - - _ . . 1 - __ 0 0 0 I 1 c 4 -54 0 I _I 0 54 c₄ 0 I 0 3 R

₌

I 4 0 0 1 I ₁₎ I

---1--o

0 0 I 1 c 5 0 55 0 4R ₌ 0 1 0 _I 0 5 _{-5 0} c 5 I 0 5

- - - -

----~--0 0 0 _I 1

c6 -s6 0 I 0 0 I 5_{R =} 56 t:6 0

I

0 6 0

o

0 1

I

0 x ₌ 6 p 0

+

-0 0 0

I

1 ·1

Uitvermenigvu1digen geeft nu de vo1gende uitdrukkingen:

0 c 1c25J1J C152c_J1_J c15212 x

=

+ + 0 5 1C25J1J 5152C_J1_J 515212 (S.8) y

=

+ + 0 =-5₂5 J1J + c2cJ1J c212 + 11 z + all = C1(C2J(C4CSC6-5456)-52Js5C6) - 51(54CSC6+C4S6) a_{21 = 51(C2J(C4CSC6-5456)-S2JS5C6) + Cl(54c5c6+e456)} a Jl =-S2J(C4C5C6- 54 5 6) - c 2JS556 a 12 = C1(-C2J(C4C5S6+54C6)+S2JS556) - 51(-54C5S6+C4C6) a 22 = Sl(-C2J(C4C556+54C6)+S2J55S6) + Cl{-S4C5S6+C4C6) a J2 = 52J(C4C5S6+S4C6) + c 2J s Ss 6 a 1J = C1(C2JC4SS+52JCS) - 5 1 s 4 5

s

a 2J = Sl(C2JC455+S2JC5) + c1545_S a JJ = -s2J c 4 5 5 + c 2J c 5 (,S.9)

6 DE INPLEr.1ENTATIE IN PASCAL

6.1 Programma voor kon~iguratie 1 met op1ossingswijze I