Modelo Cinemático Inverso

Modelo Cinemático Inverso Walter Fetter Lages fetter@ece.ufrgs.br Universidade Federal do Rio Grande do Sul Escola de Engenharia Departamento de Sistemas Elétricos de Automação e Energia ENG10026 Robótica A Copyright (c) Walter Fetter Lages – p. 1 Introdução • Modelo Cinemático Inverso • Calcula as posições das juntas em função da posição e orientação (desejadas) da garra • Fazendo 0T d n = 0Tn =⇒ 16 equações • 4 triviais • 9 referentes à rotação · Apenas 3 independentes • 3 referentes à translação • 6 equações e n incógnitas • Se n = 6, a princípio, há solução • Se n > 6 tem-se graus de liberdade redundantes • Se n < 6 tem-se um sistema com restrições Copyright (c) Walter Fetter Lages – p. 2 Considerações • Existência de soluções • Multiplicidade de soluções • Método de solução • Soluções em forma fechada • Método algébrico • Método geométrico • Solução de Pieper • Soluções em forma aberta • Métodos numéricos Copyright (c) Walter Fetter Lages – p. 3 Método Algébrico ˆ X0 ˆY0 ˆZ0 ˆ X1 ˆY1 ˆZ1 ˆ X2 ˆY2 ˆZ2 ˆ X3 ˆY3 ˆZ3 x y φ Copyright (c) Walter Fetter Lages – p. 4 Método Algébrico 0T3 =   C123 −S123 0 l1C1 + l2C12 + l3C123 S123 C123 0 l1S1 + l2S12 + l3S123 0 0 1 0 0 0 0 1   0T d 3 =   Cφ −Sφ 0 x Sφ Cφ 0 y 0 0 1 0 0 0 0 1   • Fazendo-se 0T d 3 = 0T3 pode-se obter: Copyright (c) Walter Fetter Lages – p. 5 Método Algébrico: Equações Cφ = C123 Sφ = S123 x = l1C1 + l2C12 + l3C123 y = l1S1 + l2S12 + l3S123 • Rearranjando com os termos conhecidos de um lado da igualdade e os termos dependentes das incógnitas do outro: x − l3Cφ = l1C1 + l2C12 y − l3Sφ = l1S1 + l2S12 • Elevando ao quadrado e somando: Copyright (c) Walter Fetter Lages – p. 6 Método Algébrico: Distância do Punho (x − l3Cφ)2 + (y − l3Sφ)2 = l2 1C2 1 + 2l1C1l2C12 + l2 2C2 12 + l2 1S2 1 + 2l1S1l2S12 + l2 2S2 12 = l2 1 + l2 2 + 2l1l2 (C1C12 + S1S12) = l2 1 + l2 2 + 2l1l2 cos (θ1 − (θ1 + θ2)) = l2 1 + l2 2 + 2l1l2C2 C2 = (x − l3Cφ)2 + (y − l3Sφ)2 − l2 1 − l2 2 2l1l2 • Obviamente deve-se ter −1 ≤ C2 ≤ 1. Caso contrário, o ponto (x, y, φ) está fora do espaço de trabalho do manipulador Copyright (c) Walter Fetter Lages – p. 7 A P a urrcs Método Algébrico: atan2(-) ¢ Deve-se evitar 0 uso da funcgdo acos(-) os e Perde-se a informacao de quadrante do angulo ° Calcular 6, através da funcdo atan2(., -) e Angulo no quadrante correto, entre —7 e +7 re, y e Duas possiveis solucdes e Umacom 0 cotovelo do rob6 para cima e Outra com o cotovelo para baixo (PE aI GoD ea OD® Copvricht (c) Walter Fetter Lages — pn. 8 Método Algébrico: Projeção • Conhecendo θ2 x − l3Cφ = l1C1 + l2C1C2 − l2S1S2 y − l3Sφ = l1S1 + l2S1C2 + l2C1S2 x − l3Cφ = K1C1 − K2S1 y − l3Sφ = K1S1 + K2C1 K1 = l1 + l2C2 K2 = l2S2 Copyright (c) Walter Fetter Lages – p. 9 G 4 4 Y e urresVletodo Algebrico: Mudanca de Variaveis _— REY r=/h;+ K5 Cn CO ta) cae eet TV CLO Mm LGD Ky =rseny LL — Gr = Ones —_ me eTe ol Lo AGH ico emaL) mete) if in sen(y + 41) A 4 4 e TT Yer Metodo Algebrico bal maa Eye) EO) r r = atan2(y — [354,27 — I3C4) 0, = atan2(y — 1354,” — I3Cy) — 7 = atan2(y — 1354, — I3Cy) — atan2( Ko, K4) 0, = atan2(y—I3Sy, v—I[3Cg) —atan2(lgS9, ly +l2C2) ° Osinal de 65 afeta Sy que afeta 6, Método Algébrico • Conhecendo θ1 e θ2 pode-se determinar θ3 atan2(Sφ, Cφ) = atan2(S123, C123) φ = θ1 + θ2 + θ3 θ3 = φ − θ1 − θ2 Copyright (c) Walter Fetter Lages – p. 12 G 2 V4 Oo urrn»cs Metodo Algebrico: Resumo PAR eee Or (Path oy POD) 0, = atan2(y—I35y, v—I[3Cg) —atan2(lgSo, ly +l2C 2) Fe el ie Método Geométrico • Baseia-se na decomposição do manipulador em planos y x ˆ X0 ˆY0 ˆZ0 φ θ2 θ′ 2 l3 sen φ l3 cos φ Copyright (c) Walter Fetter Lages – p. 14 Método Geométrico • Configuração com o cotovelo para cima • Aplicando a Lei dos Cossenos (x−l3Cφ)2+(y−l3Sφ)2 = l2 1+l2 2−2l1l2 cos(180◦+θ2) • θ2 < 0 =⇒ cos(180◦ + θ2) = − cos θ2 C2 = (x − l3Cφ)2 + (y − l3Sφ)2 − l2 1 − l2 2 2l1l2 θ2 = acos(C2) • Como θ2 < 0, o sinal está definido • Com o cotovelo para baixo: θ′ 2 = −θ2 Copyright (c) Walter Fetter Lages – p. 15 Método Geométrico y x ˆ X0 ˆY0 ˆZ0 φ θ2 θ′ 2 l3 sen φ l3 cos φ α α′ β θ1 θ′ 1 Copyright (c) Walter Fetter Lages – p. 16 G P = Tr Metodo Geometrico ~~ ° Definindo 8 = atan2(y — 1354, x — IsCg) ¢ Cotovelo paracima => 6; = G+a,a <0 7 ¢ Cotovelo para baixo => 6; = G—-—a’,a’ >0 ° a pode ser obtido pela a Lei dos Cossenos y Fd ceo eC — 2h4/(a@ — I3Cg)? + (y — I3Sy)? cos a Z y 7 ae. cosa = (x — IsCg)" + (y = 1356)" + 1 — 211 4/ G a OPN as C7 a I3.S4)? ° @ = acos(cos @) (o sinal do angulo é conhecido) @ Q! — —( Método Geométrico y x ˆ X0 ˆY0 ˆZ0 φ θ2 θ′ 2 l3 sen φ l3 cos φ α α′ β θ1 θ′ 1 θ3 θ′ 3 Copyright (c) Walter Fetter Lages – p. 18 Método Geométrico • φ = θ1 + θ2 + θ3 θ3 = φ − θ1 − θ2 θ′ 3 = φ − θ′ 1 − θ′ 2 Copyright (c) Walter Fetter Lages – p. 19 G P a UT te A Carre Orn itagu Cee oa (x — I3Cg)* + (y — 3So)° — i —b5 Sine WT (Paleo (Oe (ip) — | yy — | yy IE __ I A eo) A Co) PAW, G — Ora 7 CF I3.S4)? @ = acos(cos @) ere om act 100-1 C) eal Yon ee deem One) aero’ rare’ OE elk eel eel eek Deel eee Solução de Pieper • Para manipuladores com 6 (ou 5 ou 4) graus de liberdade • Quando as 3 (ou 2 ou 1) últimas juntas forem rotacionais e os seus eixos interceptam-se em um único ponto • É possível desacoplar o problema cinemático inverso • Problema de posicionamento inverso • Problema de orientação inverso • Encontrar q = [ q1 . . . qn ]T tal que 0T d n = 0T1 . . . n−1Tn = 0Tn Copyright (c) Walter Fetter Lages – p. 21 Solução de Pieper 0P d n = 0Pn ; 0Rd n = 0Rn • Se os eixos das juntas n − 2, n − 1 e n se interceptam no ponto Q, as origens de {n − 1} e {n − 2} (atribuídos segundo as convenções de Denavit-Hartenberg) estarão neste ponto • O movimento das juntas n − 2, n − 1 e n não alterará a posição do ponto Q • A origem de {n} é apenas uma translação dn ao longo de ˆZn−1 a partir de Q • ˆZn−1 está alinhado com ˆZn 0Pnorg = 0Q+0Rn−1dn n−1 ˆZn−1 = 0Q+0Rndn n ˆZn Copyright (c) Walter Fetter Lages – p. 22 Solução de Pieper • Para posicionar o efetuador no ponto 0P d n, basta fazer 0Q = 0P d n − 0Rd ndn n ˆZn • A partir da base: 0Q = 0P(n−2)org = 0Tn−3 n−3P(n−2)org • Função apenas de q1 . . . qn−3 • Pode-se determinar q1 . . . qn−3 de 0P d n − 0Rd ndn n ˆZn = 0Tn−3 n−3P(n−2)org Copyright (c) Walter Fetter Lages – p. 23 Solução de Pieper • q1 . . . qn−3 conhecidos =⇒ 0Rn−3 conhecida 0Rd n = 0Rn−3 n−3Rn n−3Rn = 0R−1 n−3 0Rd n n−3Rn = 0RT n−3 0Rd n • de onde pode-se determinar os valores de qn−2 . . . qn Copyright (c) Walter Fetter Lages – p. 24 Métodos Numéricos • Existem diversos métodos numéricos para calcular a cinemática inversa • Aqui serão apresentadas apenas as mais simples • Pouco eficientes do ponto de vista computacional • Muito suceptíveis a problemas com sigularidades Copyright (c) Walter Fetter Lages – p. 25 Método de Newton-Raphson • Usa a inversa do jacobiano q[k + 1] = q[k] + J−1(q[k])(X − fk(q[k])) • Quando J não é quadrada é usada a inversa generalizada J† • Calculada por decomposição em valores singulares • Seja J ∈ Rm×n tal que J = USV T • U ∈ Rm×m, S ∈ Rm×n é uma matriz diagonal e V ∈ Rn×n • A pseudoinversa é J† = V S†U T • S† é computada substituindo cada elemento diferente de zero da diagonal de S pelo seu recíproco e transpondo a matriz resultante Copyright (c) Walter Fetter Lages – p. 26 Transposta do Jacobiano • Em lugar de utilizar a pseudo-inversa do jacobiano, pode-se utilizar a transposta q[k + 1] = q[k] + JT(q[k])(X − fk(q[k])) • É computacionalmente mais eficiente • Evita problemas com singularidades • Aproximação é motivada por considerações físicas com base no conceito de trabalho virtual • Para resolver certos problemas de escala, pode-se introduzir um fator de escala h q[k + 1] = q[k] + hJT(q[k])(X − fk(q[k])) Copyright (c) Walter Fetter Lages – p. 27 Algoritmo de Levenberg-Marquardt • Problema de otimização: q = arg min q (X − fk(q))T (X − fk(q)) • A cada iteração fk(q) é substituído por fk(q + δ) • δ é determinado pela linearização de fk(q + δ): fk(q + δ) ≈ fk(q) + J(q)δ • O custo a minimizar é: S(q + δ) ≈ (X − fk(q) − J(q)δ)T (X − fk(q) − J(q)δ) Copyright (c) Walter Fetter Lages – p. 28 S — = UFRGS Minimizacao do Custo eiaiaiaias e Derivado em relacao a 0 e igualado a zero resulta: —2J° (q) (X — fk(q) — J(q)d) = 0 (J"(g)I(q)) 6 = J (q) (X — fk(q)) e sistema linear de onde 0 pode ser determinado e Contribuigao de Levenberg: versao amortecida: (J°(g)J(q) + AL) 5 = J" (q) (X — fk(q)) ° Marquardt: substituir J por diag (J* (q)J(q)): (J* (q)J(q) + Adiag (J" (q)J(q))) 6 = J* (a) (X — fk(q))