Cinemática Direta

Cinemática Direta 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 Direto • Mapeia posições das juntas em posição e orientação da garra • Para robôs triviais é possível fazer por trigonometria • Para robôs reais é necessário sistematizar • Ferramentas de Álgebra Linear e Geometria Analítica • Associa-se a cada elo um sistema de coordenadas • Obtém-se as transformações entre sistemas de coordenadas adjacentes • Multiplica-se para obter 0Tn Copyright (c) Walter Fetter Lages – p. 2 Atribuição dos Sistemas de Coordenadas • Existem convenções de como atribuir os sistemas de coordenadas • Simplificam a descrição dos elos • Simplificam a obtenção das transformações entre sistemas adjacentes • Padronizam a forma das transformações homogêneas • Simplificam os cálculos • Complicam a atribuição dos sistemas de coordenadas • Inicialmente as convenções serão esquecidas Copyright (c) Walter Fetter Lages – p. 3 Exemplo • Robô planar com 3 graus de liberdade Base Elo 1 Elo 2 Elo 3 Junta 1 Junta 2 Junta 3 Copyright (c) Walter Fetter Lages – p. 4 Sistemas de Coordenadas • Um sistema de coordenadas associado a cada elo ˆX0 ˆY0 ˆZ0 ˆX1 ˆY1 ˆZ1 ˆX2 ˆY2 ˆZ2 ˆX3 ˆY3 ˆZ3 Copyright (c) Walter Fetter Lages – p. 5 Transformações Homogêneas 0T1 =   1 0 0 0 0 cos θ1 − sen θ1 0 0 sen θ1 cos θ1 l1 0 0 0 1   =   1 0 0 0 0 C1 −S1 0 0 S1 C1 l1 0 0 0 1   Copyright (c) Walter Fetter Lages – p. 6 Variáveis de Junta ˆX0 ˆY0 ˆZ0 ˆX1 ˆY1 ˆZ1 ˆX2 ˆY2 ˆZ2 ˆX3 ˆY3 ˆZ3 l1 θ1 Copyright (c) Walter Fetter Lages – p. 7 Transformações Homogêneas 1T2 =   0 1 0 0 − sen θ2 0 cos θ2 l2 + l3 cos θ2 cos θ2 0 sen θ2 l3 sen θ2 0 0 0 1   =   0 1 0 0 −S2 0 C2 l2 + l3C2 C2 0 S2 l3S2 0 0 0 1   Copyright (c) Walter Fetter Lages – p. 8 Variáveis de Junta ˆX0 ˆY0 ˆZ0 ˆX1 ˆY1 ˆZ1 ˆX2 ˆY2 ˆZ2 ˆX3 ˆY3 ˆZ3 l1 θ1 l2 θ2 l3 Copyright (c) Walter Fetter Lages – p. 9 Transformações Homogêneas 2T3 =   − cos θ3 sen θ3 0 l4 sen θ3 0 0 1 0 sen θ3 cos θ3 0 l4 cos θ3 0 0 0 1   =   −C3 S3 0 l4S3 0 0 1 0 S3 C3 0 l4C3 0 0 0 1   Copyright (c) Walter Fetter Lages – p. 10 Variáveis de Junta ˆX0 ˆY0 ˆZ0 ˆX1 ˆY1 ˆZ1 ˆX2 ˆY2 ˆZ2 ˆX3 ˆY3 ˆZ3 l1 θ1 l2 θ2 l3 θ3 l4 Copyright (c) Walter Fetter Lages – p. 11 Modelo Cinemático Direto 0T3 = 0T11T22T3 =   1 0 0 0 0 C1 −S1 0 0 S1 C1 l1 0 0 0 1     0 1 0 0 −S2 0 C2 l2 + l3C2 C2 0 S2 l3S2 0 0 0 1     −C3 S3 0 l4S3 0 0 1 0 S3 C3 0 l4C3 0 0 0 1   =   0 0 1 0 S123 C123 0 l2C1 + l3C12 + l4C123 −C123 S123 0 l1 + l2S1 + l3S12 + l4S123 0 0 0 1   Copyright (c) Walter Fetter Lages – p. 12 & UFRGS Kxemplo i Ke } ? | SS | e Suponha que a garra esta segurando um prego in com 100mm de comprimento. Considerando que o rob6 tem as dimensoes 1) = lg = lg = 1, = 500 mm e que os angulos das . 7 i yLuleleekmy CONC ae [60° ato salle , calcule as coordenadas da ponta do prego no sistema de coordenadas da base. Copvricht (c) Walter Fetter Lages — pn. 13 Exemplo 0Pp = 0T3 3Pp =   0 0 1 0 sen 45° cos 45° 0 0.5 cos 60° + 0.5 cos 105° + 0.5 cos 45° − cos 45° sen 45° 0 0.5 + 0.5 sen 60° + 0.5 sen 105° + 0.5 sen 45° 0 0 0 1     0 0.1 0 1   0Pp =   0 0 1 0 0.707 0.707 0 0.4741 −0.707 0.707 0 1.7695 0 0 0 1     0 0.1 0 1   =   0 0.5448 1.8402 1   Copyright (c) Walter Fetter Lages – p. 14 Convenção de Denavit-Hartenberg • Convenção para atribuir os sistemas de coordenadas • A matriz de transformação homogênea entre sistemas adjacentes fica sempre com a mesma forma • Os elos podem ser parametrizados com apenas 4 parâmetros • Regras Básicas: 1. ˆZi−1 ao longo do eixo a junta i 2. ˆXi normal a ˆZi−1 3. ˆYi completa o sistema • As demais regras são consequências das regras básicas Copyright (c) Walter Fetter Lages – p. 15 Atribuição dos Sistemas de Coordenadas Sistema de coordenadas da base: { ˆX0, ˆY0, ˆZ0} na base do robô, com ˆZ0 sobre o eixo da junta 1 e apontando para o "ombro" do robô. ˆX0 e ˆY0 podem ser convenientemente estabelecidos ˆX0 ˆY0 ˆZ0 Copyright (c) Walter Fetter Lages – p. 16 Atribuição dos Sistemas de Coordenadas Sistemas de coordenadas dos elos: Eixo da junta: ˆZi alinhado com o eixo da junta i + 1 Origem do sistema i: na intersecção de ˆZi e ˆZi−1 ou na intersecção da normal comum a ˆZi e ˆZi−1 e o eixo ˆZi Eixo ˆXi: ˆXi = ± ˆZi−1× ˆZi | ˆZi−1× ˆZi| ou sobre a normal comum entre ˆZi−1 e ˆZi se eles forem paralelos Eixo ˆYi: ˆYi = + ˆZi× ˆXi | ˆZi× ˆXi|, para completar o sistema Copyright (c) Walter Fetter Lages – p. 17 Sistemas de Coordenadas ˆX0 ˆY0 ˆZ0 ˆX1 ˆY1 ˆZ1 ˆX2 ˆY2 ˆZ2 Copyright (c) Walter Fetter Lages – p. 18 Atribuição dos Sistemas de Coordenadas Sistema de coordenadas da garra: Alinhe ˆZn na mesma direção que ˆZn−1e apontando para fora do robô. Alinhe ˆXn de forma que seja normal a ˆZn−1 e a ˆZn. ˆYn completa o sistema. ˆ X0 ˆY0 ˆZ0 ˆ X1 ˆY1 ˆZ1 ˆ X2 ˆY2 ˆZ2 ˆ X3 ˆY3 ˆZ3 Copyright (c) Walter Fetter Lages – p. 19 Sistemas de Coordenadas ˆX0 ˆY0 ˆZ0 ˆX1 ˆY1 ˆZ1 ˆX2 ˆY2 ˆZ2 ˆX3 ˆY3 ˆZ3 Copyright (c) Walter Fetter Lages – p. 20 Parâmetros das Juntas e Elos di: é o deslocamento da origem do sistema i − 1 à intersecção dos eixos ˆZi−1 e ˆXi, medido sobre o eixo ˆZi−1. É a variável de junta, se a junta i for prismática. ai: é o deslocamento da intersecção de ˆZi−1 e ˆXi à origem do sistema i, medido sobre o eixo ˆXi. θi: é o deslocamento angular em torno de ˆZi−1, medido de ˆXi−1 à ˆXi. É a variável de junta se a junta i for rotacional. αi: é o deslocamento angular em torno de ˆXi, medido de ˆZi−1 à ˆZi. Copyright (c) Walter Fetter Lages – p. 21 Parâmetros das Juntas e Elos elo i-1 elo i junta i junta i+1 θi θi θi+1 xi−1 zi−1 xi zi ai αi di Copyright (c) Walter Fetter Lages – p. 22 Parâmetros das Juntas e Elos ˆX0 ˆY0 ˆZ0 ˆX1 ˆY1 ˆZ1 ˆX2 ˆY2 ˆZ2 ˆX3 ˆY3 ˆZ3 l1 θ1 l2 θ2 l3 θ3 Copyright (c) Walter Fetter Lages – p. 23 Parâmetros de Denavit-Hartenberg i di ai θi αi 1 0 l1 θ1 0 2 0 l2 θ2 0 3 0 l3 θ3 ±π ˆX0 ˆY0 ˆZ0 ˆX1 ˆY1 ˆZ1 ˆX2 ˆY2 ˆZ2 ˆX3 ˆY3 ˆZ3 l1 θ1 l2 θ2 l3 θ3 Copyright (c) Walter Fetter Lages – p. 24 Transformação entre Frames • Pi pode ser expresso em {i − 1} através a sequência de transformações: 1. Rotação de um ângulo αi em torno de ˆXi, para alinhar ˆZi−1 com ˆZi 2. Translação de ai ao longo de ˆXi para fazer os eixos ˆZ coincidentes 3. Translação de di ao longo de ˆZi−1 para fazer as origens coincidentes 4. Rotação de um ângulo θi em torno de ˆZi−1, para alinhar ˆXi−1 com ˆXi Copyright (c) Walter Fetter Lages – p. 25 Transformação entre Frames Rx(αi) =   1 0 0 0 0 cos αi − sen αi 0 0 sen αi cos αi 0 0 0 0 1   Dx(ai) =   1 0 0 ai 0 1 0 0 0 0 1 0 0 0 0 1   Dz(di) =   1 0 0 0 0 1 0 0 0 0 1 di 0 0 0 1   Rz(θi) =   cos θi − sen θi 0 0 sen θi cos θi 0 0 0 0 1 0 0 0 0 1   Copyright (c) Walter Fetter Lages – p. 26 G = urkes lransformacao entre Frames pared Gi T= D-UA)R.(O)Ds(a)Ralas) —_— cos6; —cosa;sen§; sena;sen§; a, cos6; = sen@; cosa;cosé; —sena;cos6; a,;sen6; 4 == 0 {oleae Ge) nar CF ¢ ¢ ¢ 1 ae eee else Solace 0 aa —cosa;sen@; cosa;cos@; sena; —d,;sena; sena;sen@; —sena;cos§; cosa; —d; cosa; 0 0 0 1 Copvricht (c) Walter Fetter Lages — np. 27 Exemplo 0T1 =   cos θ1 − sen θ1 0 l1 cos θ1 sen θ1 cos θ1 0 l1 sen θ1 0 0 1 0 0 0 0 1   1T2 =   cos θ2 − sen θ2 0 l2 cos θ2 sen θ2 cos θ2 0 l2 sen θ2 0 0 1 0 0 0 0 1   Copyright (c) Walter Fetter Lages – p. 28 Exemplo 2T3 =   cos θ3 sen θ3 0 l3 cos θ3 sen θ3 − cos θ3 0 l3 sen θ3 0 0 −1 0 0 0 0 1   0T3 =   C123 S123 0 l1C1 + l2C12 + l3C123 S123 −C123 0 l1S1 + l2S12 + l3S123 0 0 −1 0 0 0 0 1   Copyright (c) Walter Fetter Lages – p. 29 Convenção de Denavit-Hartenberg Modificada • Na convenção de Denavit-Hartenberg a junta i gira (ou desloca-se) em torno (ou ao longo) do eixo ˆZi−1 • Na Convenção de Denavit-Hartenberg Modificada a junta i gira (ou desloca-se) em torno (ou ao longo) do eixo ˆZi • Regras Básicas: 1. ˆZi está ao longo do eixo a junta i 2. ˆXi é normal a ˆZi+1 3. ˆYi completa o sistema Copyright (c) Walter Fetter Lages – p. 30 Atribuição dos Sistemas de Coordenadas Sistema de coordenadas da base: { ˆX0, ˆY0, ˆZ0} na base do robô, de forma que o sistema {0} fique alinhado com o sistema {1} quando a variável de junta 1 for zero. ˆX0 ˆY0 ˆZ0 ˆZ1 Copyright (c) Walter Fetter Lages – p. 31 Atribuição dos Sistemas de Coordenadas Sistemas de coordenadas dos elos: Eixo da junta: Alinhe ˆZi com o eixo da junta i (rotacional ou prismática). Origem do sistema i: Localize a origem do sistema i na intersecção de ˆZi e ˆZi+1 ou na intersecção da normal comum a ˆZi e ˆZi+1 com o eixo ˆZi. Eixo ˆXi: ˆXi = ± ˆZi× ˆZi+1 | ˆZi× ˆZi+1| ou sobre a normal comum entre ˆZi e ˆZi+1. Eixo ˆYi: ˆYi = + ˆZi× ˆXi | ˆZi× ˆXi|, para completar o sistema. Copyright (c) Walter Fetter Lages – p. 32 Sistemas de Coordenadas ˆX0 ˆY0 ˆZ0 ˆX1 ˆY1 ˆZ1 ˆX2 ˆY2 ˆZ2 Copyright (c) Walter Fetter Lages – p. 33 Atribuição dos Sistemas de Coordenadas Sistema de coordenadas n: ˆZn é o eixo da junta n. A origem e o eixo ˆXn podem ser escolhidos livremente. Em geral, para fazer os parâmetros dos elos serem 0, alinha-se ˆXn na mesma direção que ˆXn−1 quando qn = 0 e apontando para fora do robô. ˆYn completa o sistema ˆ X0 ˆY0 ˆZ0 ˆ X1 ˆY1 ˆZ1 ˆ X2 ˆY2 ˆZ2 ˆ X3 ˆY3 ˆZ3 Copyright (c) Walter Fetter Lages – p. 34 Sistemas de Coordenadas ˆX0 ˆY0 ˆZ0 ˆX1 ˆY1 ˆZ1 ˆX2 ˆY2 ˆZ2 ˆX3 ˆY3 ˆZ3 Copyright (c) Walter Fetter Lages – p. 35 Parâmetros das Juntas e Elos di: é o deslocamento da intersecção dos eixos ˆXi−1 e ˆZi, à origem do sistema i, medido sobre o eixo ˆZi. É a variável de junta, se a junta i for prismática ai: é o deslocamento da origem do sistema i à intersecção de ˆXi e ˆZi+1, medido sobre o eixo ˆXi θi: é o deslocamento angular em torno de ˆZi, medido de ˆXi−1 à ˆXi. É a variável de junta se a junta i for rotacional αi: é o deslocamento angular em torno de ˆXi, medido de ˆZi à ˆZi+1 Copyright (c) Walter Fetter Lages – p. 36 Parâmetros de Juntas e Elos elo i-1 elo i junta i junta i-1 θi θi θi−1 xi−1 zi−1 xi zi ai−1 αi−1 di Copyright (c) Walter Fetter Lages – p. 37 Parâmetros das Juntas e Elos • Atenção: θ3 < 0! ˆX0 ˆY0 ˆZ0 ˆX1 ˆY1 ˆZ1 ˆX2 ˆY2 ˆZ2 ˆX3 ˆY3 ˆZ3 l1 θ1 l2 θ2 θ3 Copyright (c) Walter Fetter Lages – p. 38 Parâmetros das Juntas e Elos ˆX0 ˆY0 ˆZ0 ˆX1 ˆY1 ˆZ1 ˆX2 ˆY2 ˆZ2 ˆX3 ˆY3 ˆZ3 l1 θ1 l2 θ2 θ3 Copyright (c) Walter Fetter Lages – p. 39 Parâmetros de Denavit-Hartenberg Modificados i di ai−1 θi αi−1 1 0 0 θ1 0 2 0 l1 θ2 0 3 0 l2 θ3 ±π ˆX0 ˆY0 ˆZ0 ˆX1 ˆY1 ˆZ1 ˆX2 ˆY2 ˆZ2 ˆX3 ˆY3 ˆZ3 l1 θ1 l2 θ2 θ3 Copyright (c) Walter Fetter Lages – p. 40 Transformação entre Frames • Pi pode ser expresso em {i − 1} através da seguinte sequência de transformações: 1. Translação de di ao longo de ˆZi 2. Rotação de um ângulo θi em torno de ˆZi, para alinhar ˆXi com ˆXi−1 3. Translação de ai−1 ao longo de ˆXi−1 para fazer as origens coincidentes 4. Rotação de um ângulo αi−1 em torno de ˆXi−1, para alinhar ˆZi com ˆZi−1 Copyright (c) Walter Fetter Lages – p. 41 Transformação entre Frames Dz(di) =   1 0 0 0 0 1 0 0 0 0 1 di 0 0 0 1   Rz(θi) =   cos θi − sen θi 0 0 sen θi cos θi 0 0 0 0 1 0 0 0 0 1   Dx(ai−1) =   1 0 0 ai−1 0 1 0 0 0 0 1 0 0 0 0 1   Rx(αi−1) =   1 0 0 0 0 cos αi−1 − sen αi−1 0 0 sen αi−1 cos αi−1 0 0 0 0 1   Copyright (c) Walter Fetter Lages – p. 42 Transformação entre Frames i−1Ti = Rx(αi−1)Dx(ai−1)Rz(θi)Dz(di) =   cos θi − sen θi 0 ai−1 sen θi cos αi−1 cos θi cos αi−1 − sen αi−1 − sen αi−1di sen θi sen αi−1 cos θi sen αi−1 cos αi−1 cos αi−1di 0 0 0 1   Copyright (c) Walter Fetter Lages – p. 43 Exemplo 0T1 =   cos θ1 − sen θ1 0 0 sen θ1 cos θ1 0 0 0 0 1 0 0 0 0 1   1T2 =   cos θ2 − sen θ2 0 l1 sen θ2 cos θ2 0 0 0 0 1 0 0 0 0 1   Copyright (c) Walter Fetter Lages – p. 44 Exemplo 2T3 =   cos θ3 − sen θ3 0 l2 − sen θ3 − cos θ3 0 0 0 0 −1 0 0 0 0 1   0T3 =   C12−3 S12−3 0 l1C1 + l2C12 S12−3 −C12−3 0 l1S1 + l2S12 0 0 −1 0 0 0 0 1   Copyright (c) Walter Fetter Lages – p. 45 DH × DH Modificada • Na DH a junta i gira em torno de ˆZi−1 • Na DH modificada a junta i gira em torno de ˆZi • Na DH modificada a posição da garra não depende da variável de junta n • Simplifica para obter o modelo cinemático inverso • Na DH há um sistema de coordenadas na garra • Simplifica posicionar objetos na garra • DH não é diretamente compatível com URDF • É preciso criar sistemas de coordenadas extras para compatibilizar • É comum chamarem a DH modificada de DH Copyright (c) Walter Fetter Lages – p. 46 Exercícios Copyright (c) Walter Fetter Lages – p. 47 Exercícios Copyright (c) Walter Fetter Lages – p. 48