Professional Documents
Culture Documents
CAPTULO 4
ci s i Ti 1 ( ai , i , di ,i ) = i 0 0
si c i c i c i s i 0
si s i ci s i c i 0
ai c i ai s di 1
(4.1)
e a cinemtica direta do manipulador foi obtida atravs do produto das matrizes de cada uma das juntas como mostra a equao (4.2)
54
(4.2)
O resultado deste produto representado pela matriz de transformao homognea cujos elementos so 12 equaes no lineares e por comodidade, os elementos da matriz de orientao sero denominados rij a partir deste ponto, como mostra a equao (4.3)
px py pz 1
(4.3)
Assumindo que as dimenses do rob, sua posio e orientao so conhecidas, ento a matriz H obtida na anlise da cinemtica direta que d a posio e orientao do efetuador final uma matriz numrica. A matriz de transformao homognea contm o vetor posio e sua orientao representados na equao (4.4) ;
R0n H =T = 0
6 0
n p0 1
(4.4)
onde
0 = [0 0 0] .
A equao (4.2) da cinemtica direta uma seqncia de transformaes homogneas para as articulaes do manipulador, que inicia na base e seque at o efetuador final. O grafo da Figura 40 ilustra esta seqncia transformaes realizadas na equao. 2
1 T 0 T
3 T 4 T
55 A matriz de transformao homognea do manipulador, dada pelo termo no lado esquerdo na equao (4.2), representada pela seta verde que segue no sentido anti-horrio do grafo uma matriz numrica que representa a posio e a orientao do efetuador final. O lado direito da mesma equao (4.2), ilustrado pela seta azul que segue no sentido horrio e representa os produtos das matrizes das transformaes homogneas de cada articulao do manipulador e seus elementos so funes das variveis das juntas. O lado esquerdo da equao (4.2) representa o vetor posio que vai da base do rob, cujo sistema coordenado tem a origem O0, at o ponto central do efetuador final, no sistema coordenado com origem O6, representado na Figura 41 .
X3
d4
X2 Z5 Z3
Z5
d6 O6 X6 Y6 Z6
Y2 Z2
a2
Y1
Z1
X1
Z0 d1 O0 Y0 X0
Figura 41 Representao das articulaes do manipulador Robothron Os valores numricos dos parmetros D-H,representados por d1, a2, d4 e d6 podem ser obtidos da Tabela 3.2.
56 interceptados num ponto constituem o chamado punho esfrico, que difcil de ser implementado mecanicamente. Desta forma a soluo da cinemtica inversa segue trs passos: A decomposio do manipulador no punho; A determinao dos ngulos das trs primeiras juntas; A determinao dos trs ngulos do efetuador final.
A decomposio do manipulador no punho o passo mais importante deste procedimento que se inicia com a localizao do centro do punho [26]. Na Figura 42, o vetor p que determina a posio do efetuador final conhecido e d6 tambm; j o vetor p w que representa a posio do centro do punho do manipulador pode ser obtido atravs da soma vetorial de p e o vetor de orientao do punho. d6
X3 X2 Y2 Z2 Z3 Z4 Z5
X6 pw Y6
Z6
Y1
Z1
X1
Z0 Y0 X0
Figura 42 - Representao grfica dos vetores de posio do punho e efetuador final Desta forma, o vetor p w e a matriz de orientao do sistema do centro do punho possibilitam determinar as variveis das juntas encarregadas da posio do punho [26].
T06 T56
( )
(4.5)
57 As operaes realizadas pela equao (4.5) so ilustradas pelo grafo da Figura 43 ilustra onde a seta verde, no sentido anti-horrio representa o produto do lado esquerdo e a seta azul no sentido horrio, representa o lado direito desta equao. 1 T 0 T 6 5 T 2 T 3 T 4 T
T Figura 43 Grafo representativo da equao (4.5) O produto da multiplicao das matrizes do lado esquerdo da equao (4.5) apresentado na equao (4.6).
(4.6)
A quarta coluna da equao (4.6) o vetor p w , cujos elementos so valores numricos e referem-se s coordenadas do centro do punho do manipulador. Esta posio a origem O5 do sistema coordenado localizada no centro do punho. A Figura 44 exibe a vista superior do modelo do manipulador com as coordenadas numricas (O5)x e (O5)y desta origem. O5 d4 Y0 a2 (O5)y
1
X0 (O5)x
Figura 44 Vista superior do modelo como punho desacoplado O produto do lado direito da equao (4.5) est na equao B1 do Apndice B e as
58 trs primeiras linhas da quarta coluna da equao B1, mostradas na equao (4.7) , tambm representam as coordenadas da posio do centro do punho em funo das variveis das trs primeiras juntas. Igualando os elementos da quarta coluna da matriz da equao (4.6) aos elementos da quarta coluna da matriz da equao B1,obtm-se:
(4.7)
Para isolar a varivel da primeira junta, o ngulo q1, divide-se (O5)y por (O5)x no lado esquerdo da equao (4.7) e as mesmas linhas no lado direito, como mostra a equao (4.8)
1 = atan2
r23d 6 d y r13 d 6 d x
(4.8)
Deduzida a equao do ngulo da articulao da base do manipulador calcula-se o seu valor numrico.
p1w
59
x3 z2
x2
O5
P1w
x1
O1 z1 2
Figura 45 Representao grfica das origens das articulaes do ombro e punho O grafo da Figura 46 mostra de forma grfica as operaes necessrias para a deduo das equaes dos ngulos de cotovelo e ombro do manipulador. A seta verde indica os valores numricos at agora calculados e a seta em azul indica o que ainda precisa ser calculado. 1 T 0 T 6 T 2 T 3 T 4 T
5 T Figura 46 Grafo das operaes de determinao dos ngulos do cotovelo e ombro A equao (4.9) representa o conjunto de transformaes envolvidas no grafo da Figura 4.7
(T )
1 0
T06 T56
( )
(4.9)
Efetuando-se a multiplicao das matrizes da equao (4.9), cujo produto pode ser observado na equao B2 do apndice B e tomando-se os trs primeiros elementos da quarta coluna, obtm-se as equaes (4.10), referente ao lado esquerdo, e (4.11) referente ao lado direito da equao (4.9)
60
(4.10)
Igualando-se as trs primeiras linhas da quarta coluna das equaes (4.10) e (4.11), obtm-
(4.12)
Elevando-se ao quadrado os elementos componentes da primeira segunda linhas da equao (4.12) , elimina-se a varivel de junta desconhecida 2. Direcionando-se a equao para a forma da lei dos co-senos, como mostra a equao (4.13) , obtm-se:
(O )
Isolando sen(3),
5 1 x
+ ( O15 )
2
y
= ( a2 c2 + d 4 s23 ) + ( a2 s2 d 4 c23 )
2
(4.13)
s3 =
Aplicando-se a lei dos senos
(O )
5 2 1 x
+ O15
( )
2 y
2 2 a2 d 4
2a2 d 4 2 v 2
(4.14)
c3 = 1 s32
Logo, a determinao do ngulo 3
3 = atan2( s3 , c3 )
como mostra a equao (4.15) .
O15 O5 1
( ) ( )
= a2 + d 4 s3 d 4 c3 y
d 4 c3 c2 a2 + d 4 s3 s2
(4.15)
c2 =
(4.16)
s2 =
( a2 + d 4 s3 )
((O ) ) + d c (O )
5 1 y 4 3
5 1 x
(4.17)
2 2 a2 + d 4 + 2a2 d 4 s3
2 = atan2( s 2 , c2 )
(4.18)
At aqui foram determinados dois dos trs passos do procedimento para a obteno da cinemtica inversa. Estabeleceu-se a posio do centro do punho e os trs ngulos do antebrao do manipulador. A partir deste ponto ser o terceiro passo que a determinao dos ngulos do punho do manipulador.
(T
1 0
(4.19)
Os termos do lado esquerdo da equao so valores numricos e os termos do lado direito dependem das variveis das juntas do punho. O grafo da Figura 47 ilustra este procedimento, onde seta verde no sentido anti-horrio representa o lado esquerdo de (4.19) e a seta azul no sentido horrio, representa as transformaes do lado direito cujas variveis de junta ainda permanecem incgnitas .
62
T 1 T 0 T 6
3 T 4 T
Figura 47 Grafo representativo das transformaes do punho. As multiplicaes do lado esquerdo e do lado direito da equao (4.19) , podem ser observadas nas equaes B6 e B7,respectivamente, no apndice B. Igualando os elementos de equaes B6 e B7 e extraindo-se as coordenadas da terceira coluna da equao B6 obtm-se
r13c1c23 r23 s 1 c23 + r33 s 23 Lado _ esquerdo(1,3) = r13 s 1 r23c11 r13c1s 23 r23 s 1 s 23 + r33c23
equao B7.
(4.20)
c4 s 5 Lado _ direito(1,3) = s 4 s 5 , c5
Igualando-se as equaes (4.20) e (4.21) , como mostra a equao 4.22) ,
(4.21)
(4.22)
(4.23)
(4.24)
63
2 5 = atan2 s5 , 1 c5
(4.25)
(4.26)
c4 = s 4=
(4.27)
(T
1 0
(4.29)
64 2
1 T 0 T 6
3 T 4 T 5
T Figura 48 Grafo representativo das transformaes necessrias para obter 6 A equao B8 do apndice B apresenta o lado esquerdo e a equao B9 apresenta o lado direito da equao (4.29) . Igualando o elemento da terceira linha e primeira coluna da equao B8 e B9, obtm-se:
r11 ( s 1 c4 c1c23 s 4 ) r21 ( c1c4 s 1 c3 s 4 ) r31s 23 s 4 = s 6 r13 ( s 1 c4 c1c23 s 4 ) r23 ( c1c4 s 1 c3 s 4 ) r33 s 23 s 4 = c6
6 = ATAN 2( s 6 , c6 )
Logo,
(4.30)
(4.31)
(4.32)
Com a cinemtica direta possvel determinar a posio e orientao do efetuador final, j a forma inversa permite calcular os ngulos das juntas a partir da posio e orientao do mesmo efetuador final.
65 Na Figura 49 apresentado o grafo das oito solues possveis para a obteno da cinemtica inversa. Matriz de Transformao Homognea
+1 +3 +2 -3 -2 +3 +2
-1 -3 -2
+5
-5
+5
-5
+5
-5
+5
-5
4 6
4 6
4 6
4 6
4 6
4 6
4 6
4 6
Figura 49 rvore de solues para a cinemtica inversa A comprovao dos resultados apresentados acima so mostrados nas simulaes implementadas no programa Matlab.
66 Tabela 4.1 ngulos das juntas e correspondente matriz de transformao homognea ngulos 0 q1 q2 q3 q4 q5 q6 0 0 0 0 0 -0.0000 -1.0000 -0.0000 0 MTH -0.0000 1.0000 0.0000 -1.0000 0 -0.0000 -0.0000 0 530.0000 0.0000 750.0000 1.0000
Tabela 4.2 Simulao da cinemtica direta e inversa e o erro correspondente Cinemtica Direta 0 0 1,0x10-015 * 0 0 0 0 Inversa 0,8668 0 0 0 0 0 erro -0.8668 0 0 0 0 0
Como pode ser observado o erro resultante da ordem de 10-015 que praticamente nulo. Com a finalidade de comprovar a validade das equaes foi novamente calculada a cinemtica direta a partir dos valores da cinemtica inversa da Tabela 4.2 e so apresentados na Tabela 4.3 Tabela 4.3 Simulao da cinemtica direta a partir da inversa calculada ngulos q1 0,8668 q2 q3 q4 q5 q6 0 0 0 0 0 -0.0000 -1.0000 -0.0000 0 MTH -0.0000 1.0000 0.0000 -1.0000 0 -0.0000 -0.0000 0 530.0000 0.0000 750.0000 1.0000
67 Tabela 4.4 Simulao da cinemtica direta a partir da inversa calculada Cinemtica Direta 0,8668 0 1,0x10
-015
Inversa 0,8668 0 0 0 0 0
erro 0 0 0 0 0 0
0 0 0 0
Nesta segunda simulao o erro nulo. A segunda simulao implementada exibe os grficos (Figura 50) das trajetrias das trs primeiras juntas geradas a partir de uma trajetria retilnea entre os pontos [-100, -100, 1130] a [1140, 100, 380] do espao cartesiano.
0.5
1.5
3.5
4.5
150
100
50
0.5
1.5
3.5
4.5
-100 -125 J unta 3 (graus ) -150 -175 -200 -225 -250 0 0.5 1 1.5 2 2.5 Tempo (s) 3 3.5 4 4.5 5
68 Para validar a cinemtica inversa a partir dos ngulos de Euler, foram atribudos valores iniciais para os ngulos das juntas e calculou- se os ngulos de Euler no Matllab. Na seqncia, forma determinados os valores dos ngulos das juntas.
=[ 45
0]
] = [45.0000
-90.0000]
Cinemtica inversa Clculo da matriz de transformao homognea mthinveuler = 0.7071 0.0000 0.7071 374.7666 0.7071 374.7666 0 0.0000 1.0000 0.0000 0.0000] -0.7071 -0.0000 0 0 0.0000
=[ 45.0000
0.0000
Neste captulo foi obtida a cinemtica inversa a partir da matriz de transformao homognea. As simulaes comprovaram a validade das equaes deduzidas.