Modelo Cinemático Inverso. Prof. Walter Fetter Lages 16 de setembro de 2007

Transcrição

1 Universidade Federal do Rio Grande do Sul Escola de Engenharia Departamento de Engenharia Elétrica Programa de Pós-Graduação em Engenharia Elétrica ELE00070-Tópicos Especiais em Controle e Automação I Modelo Cinemático Inverso Prof. Walter Fetter Lages 16 de setembro de Problema Cinemático Inverso O problema cinemático inverso consiste em obter-se os valores das variáveis de junta do manipulador a partir da posição e orientação (desejada) do efetuador final. Ou seja, deseja-se computar θ 1, θ 2,..., θ n a partir de um 0 T d n especificada. Igualando-se a matriz 0 T d n e a matriz 0 T n computada pelo modelo cinemático direto pode-se obter 16 equações envolvendo as variáveis de junta. Destas equações, 4 são triviais. Outras 9 são referentes à parte de rotação da matriz de transformação homogênea e portanto apenas 3 destas equações são independentes. A parte de translação da matriz de transformação homogênea fornece outras 3 equações independentes. Tem-se portanto um sistema com 6 equações e n incógnitas correspondentes às variáveis de junta. Assim, se o manipulador tiver 6 graus de liberdade, tem-se, a princípio, um sistema de equações que pode ser solucionado para obter-se os valores das variáveis de junta para qualquer posição e orientação especificada para o efetuador final. 2 Considerações 1. Existência de Soluções 2. Multiplicidade de Soluções 3. Método de Solução (a) Soluções em Forma Fechada 1

2 i. Método Algébrico ii. Método Geométrico iii. Solução de Pieper (b) Soluções em Forma Aberta i. Métodos Numéricos 3 Método Algébrico Figura 1: Modelo cinemático inverso pelo método algébrico. 2

3 0 T 3 = C 123 S l 1 C 1 + l 2 C 12 + l 3 C 123 S 123 C l 1 S 1 + l 2 S 12 + l 3 S T3 d = C φ S φ 0 x S φ C φ 0 y Fazendo-se 0 T d 3 = 0 T 3 pode-se obter as equações C φ = C 123 (1) S φ = S 123 (2) x = l 1 C 1 + l 2 C 12 + l 3 C 123 (3) y = l 1 S 1 + l 2 S 12 + l 3 S 123 (4) Substituindo-se as expressões (1) e (2) em (3) e (4) e rearranjando-se de forma que os termos conhecidos estejam de um lado da igualdade e os termos dependentes das incógnitas estejam do outro, tem-se x l 3 C φ = l 1 C 1 + l 2 C 12 (5) y l 3 S φ = l 1 S 1 + l 2 S 12 (6) Elevando-se ao quadrado e somando-se as expressões (5) e (6) resulta (x l 3 C φ ) 2 + (y l 3 S φ ) 2 = l1c l 1 C 1 l 2 C 12 + l2c l1s l 1 S 1 l 2 S 12 + l2s = l1 2 + l l 1l 2 (C 1 C 12 + S 1 S 12 ) e portanto = l l l 1l 2 cos (θ 1 (θ 1 + θ 2 )) = l l l 1l 2 C 2 C 2 = (x l 3C φ ) 2 + (y l 3 S φ ) 2 l 2 1 l 2 2 2l 1 l 2 (7) Obviamente deve-se ter 1 C 2 1. No entanto, o valor calculado através da expressão (7) pode eventualmente estar fora desta faixa. Isto significa que o ponto (x, y, φ) está fora do espaço de trabalho do manipulador. 3

4 Para obter-se o valor do ângulo θ 2 deve-se evitar o uso da função acos ( ), pois desta forma perde-se a informação de quadrante do ângulo. O correto é calcular o valor de θ 2 através da função atan2 (, ) 1 Para tanto necessita-se obter o valor de S 2, através de S 2 = ± 1 C2 2 (8) Note-se que os dois sinais na expressão (8) indicam a existência de duas possíveis soluções: Uma com o cotovelo do robô para cima e outra com o cotovelo para baixo. Pode-se portanto, calcular o valor de θ 2 pela expressão Conhecendo-se θ 2, pode-se, de (5) e (6), escrever: θ 2 = atan2 (S 2, C 2 ) (9) de onde é possível obter-se x l 3 C φ = l 1 C 1 + l 2 C 1 C 2 l 2 S1S 2 y l 3 S φ = l 1 S 1 + l 2 S 1 C2 + l 2 C 1 S 2 com x l 3 C φ = K 1 C 1 K 2 S 1 (10) y l 3 S φ = K 1 S 1 + K 2 C 1 (11) tem-se Através das seguintes mudanças de variáveis K 1 = l 1 + l 2 C 2 K 2 = l 2 S 2 (12) r = K1 2 + K2 2 γ = atan2 (K 2, K 1 ) K 1 = r cos γ (13) K 2 = r sen γ (14) 1 Esta função retorna o valor do ângulo no quadrante correto, entre π e +π. 4

5 Aplicando as transformações (13) e (14) nas expressões (10) e (11), tem-se x l 3 C φ = rc γ C 1 rs γ S 1 y l 3 S φ = rc γ S 1 + rs γ C 1 que pode ser escrita de forma mais compacta como de onde pode-se obter x l 3 C φ r y l 3 S φ r = cos(γ + θ 1 ) = sen (γ + θ 1 ) ou γ + θ 1 = atan2 ( y l3 S φ r, x l ) 3C φ = atan2 (y l 3 S φ, x l 3 C φ ) r θ 1 = atan2 (y l 3 S φ, x l 3 C φ ) γ = atan2 (y l 3 S φ, x l 3 C φ ) atan2 (K 2, K 1 ) e finalmente θ 1 = atan2 (y l 3 S φ, x l 3 C φ ) atan2 (l 2 S 2, l 1 + l 2 C 2 ) (15) Note-se que o sinal de θ 2 afeta S 2 que afeta θ 1. Conhecendo-se θ 1 e θ 2 pode-se determinar θ 3. De (1) e (2) tem-se ou atan2 (S φ, C φ ) = atan2 (S 123, C 123 ) de onde atan2 (S φ, C φ ) = θ 1 + θ 2 + θ 3 θ 3 = atan2 (S φ, C φ ) θ 1 θ 2 (16) 5

6 Figura 2: Modelo cinemático inverso pelo método geométrico. 4 Método Geométrico A solução para o problema cinemático inverso através do método geométrico baseia-se na decomposição do manipulador em planos. Considerando-se a configuração com o cotovelo para cima tem-se, pela Lei dos Cossenos: (x l 3 C φ ) 2 + (y l 3 S φ ) 2 = l l2 2 2l 1l 2 cos(180 + θ 2 ) já que nesta situação θ 2 < 0. E como cos(180 + θ 2 ) = cos θ 2, chega-se a C 2 = (x l 3C φ ) 2 + (y l 3 S φ ) 2 l l 2 2 2l 1 l 2 Neste caso, o valor de θ 2 pode ser calculado por 6

7 θ 2 = acos (C 2 ) pois devido à hipótese de que θ 2 < 0, o quadrante do ângulo está bem definido. Pode-se facilmente perceber que para a configuração com o cotovelo para baixo tem-se Definindo-se θ 2 = θ 2 β = atan2 (y l 3 S φ, x l 3 C φ ) Tem-se que quando o cotovelo está para cima θ 1 = β + α, com α 0 e quando o cotovelo está para baixo θ 1 = β α, com α 0. α pode ser obtido utilizando-se a Lei dos Cossenos: l 2 2 = ( (x l 3 C φ ) 2 + (y l 3 S φ ) 2 ) 2+l 2 1 2l 1 (x l 3 C φ ) 2 + (y l 3 S φ ) 2 cos α Logo cos α = (x l 3C φ ) 2 + (y l 3 S φ ) 2 + l 2 1 l 2 2l 1 (x l3 C φ ) 2 + (y l 3 S φ ) 2 e novamente tem-se que α pode ser calculado por α = acos (cos α), já que o quadrante do ângulo é conhecido. Note que α = α. Tem-se também que φ = θ 1 + θ 2 + θ 3. Portanto e θ 3 = φ θ 1 θ 2 θ 3 = φ θ 1 θ 2 5 Solução de Pieper Para manipuladores com 6 (ou 5 ou 4) graus de liberdade, quanto as 3 (ou 2 ou 1) últimas juntas forem rotacionais e os seus eixos interceptam-se em um único ponto, é possível obter-se uma solução geral para o problema cinemático inverso [3]. Neste caso, é possível desacoplar o problema cinemático inverso em dois problemas mais simples, o problema de posicionamento inverso e o problema de orientação inverso [5]. 7

8 Supondo-se um manipulador com n graus de liberdade, o problema cinemático inverso é encontrar os valores de q = [ q 1... q n ] T tais que 0 T d n = 0 T 1... n 1 T n = 0 T n (17) onde 0 Tn d é a matriz de transformação homogênea desejada, ou seja, a matriz de transformação homogênea com a posição e orientação do efetuador do robô para a qual se deseja determinar os valores das variáveis de junta. A expressão (17) pode ser ser desmembrada em duas equações, uma correspondendo as especificações de posição e outra correspondendo as especificações de orientação: 0 P d n = 0 P n (18) 0 R d n = 0 R n (19) onde 0 P d n e 0 R d n representam, respectivamente, a posição e a orientação desejadas para o sistema de coordenadas n. Se os eixos das juntas n 2, n 1 e n se interceptam no ponto Q, as origens dos sistemas de coordenadas {n 1} e {n 2} (atribuídos segundo as convenções de Denavit-Hartenberg) estarão neste ponto. Neste caso, o movimento das juntas n 2, n 1 e n não alterará a posição do ponto Q. Como a origem do sistema {n} é apenas uma translação por uma distância d n ao longo de Ẑn 1 a partir de Q e Ẑn 1 está alinhado com Ẑn, tem-se que 0 P norg = 0 Q + 0 R n 1 d n n 1 Ẑ n 1 = 0 Q + 0 R n d n n Ẑ n Portanto, para posicionar o efetuador do robô no ponto 0 Pn d, basta fazer Por outro lado, 0 Q = 0 P d n 0 R d n d n n Ẑ n 0 Q = 0 P (n 2)org = 0 T n 3 n 3 P (n 2)org que é função apenas de q 1... q n 3. Assim, pode-se determinar q 1... q n 3 a partir de 0 P d n 0 R d n d n n Ẑ n = 0 T n 3 n 3 P (n 2)org A determinação dos valores de q 1... q n 3 implica 0 R n 3 estar determinada e como 0 R d n = 0 R n 3 n 3 R n 8

9 deve-se fazer ou ainda n 3 R n = 0 R 1 n 3 0 R d n n 3 R n = 0 R T n 3 0 R d n de onde pode-se determinar os valores de q n 2... q n. 6 Método Numérico Existem diversos métodos numéricos para calcular a cinemática inversa. Vide [1, 2] para um resumo das técnicas. Aqui serão apresentadas apenas as mais simples. Justamente por serem mais simples, estas técnicas são pouco eficientes do ponto de vista computacional e muito suceptíveis a problemas com sigularidades. 6.1 Inversa generalizada A inversa do jacobiano é tal que, dada uma pequena variação da posição dda garra, é possível calcular a variação nas coordenadas de junta[4]: q = J 1 (q) X Em geral, não existe a inversa do jacobiano, mas sim uma inversa generalizada B, que cumpre alguma das condições de Moore-Penrose: 1. JBJ = J 2. BJB = B 3. (JB) T = JB 4. (BJ) T = BJ Se B cumpre todas as quatro condições, é dita pseudo-inversa, e é única: B = J. Achar a inversa generalizada é um processo lento e que não lida adequadamente com singularidades. 9

10 6.2 Transposta do Jacobiano Em lugar de utilizar a pseudo-inversa do jacobiano, pode-se utilizar a transposta: q = J T (q) X É muito mais eficiente do ponto de vista computacional e evita problemas com singularidades. Esta 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, e iterar até atingir a convergência: q (i+1) = hj T (q) X (i) Referências [1] S. R. Buss. Introduction to inverse kinematics with jacobian transpose, pseudoinverse and damped least squares methods. Typeset Manuscript, available from Apr [2] S. R. Buss and J.-S. Kim. Selectively damped least squares for inverse kinematics. Typeset Manuscript, available from ~sbuss/researchweb, Apr [3] J. J. Craig. Introduction to Robotics Mechanics and Control. Addison-Wesley, second edition, [4] V. F. Romano, editor. Robótica Industrial Aplicação na Indústria de Manufatura e de Processos. Edgard Blücher, São Paulo, [5] M. W. Spong and M. Vidyasagar. Robot Dynamics and Control. John Wiley & Sons, A Relações Trigonométricas Úteis A.1 Cosseno da Soma cos(a ± b) = cos a cos b sen a sen b 10

11 A.2 Seno da Soma A.3 Lei dos Cossenos sen (a ± b) = sen a cos b ± cos a sen b Figura 3: Definição de ângulos e vértices para a Lei dos Senos e Lei dos Cossenos. a 2 = b 2 + c 2 2bc cos α A.4 Lei dos Senos a sen α = b sen β = c sen γ A.5 Lei das Tangentes a + b a b = tan ( 1 (α + β)) 2 tan ( 1(α β)) 2 B Soluções Algébricas Reduzidas a Polinômios Seja uma equação trigonométrica na forma a cos θ + b sen θ = c Esta equação pode ser resolvida para θ através das seguintes transformações algébricas: 11

12 Aplicando-se as transformações, tem-se cos θ = 1 u2 (20) 1 + u 2 2u sen θ = (21) 1 + u 2 tan θ 2 = u (22) a 1 u2 1 + u 2 + b 2u 1 + u 2 = c a(1 u 2 ) + 2bu = c(1 + u 2 ) a au 2 + 2bu = c + cu 2 (c + a)u 2 2bu + (c a) = 0 u = 2b ± 4b 2 4(c + a)(c a) 2(c + a) u = b ± b 2 c 2 + a 2 c + a ( ) b ± θ = 2 tan 1 a2 + b 2 c 2 a + c 12