Controle de Sistemas Robóticos: PID e Controle de Torque Computado

Meggiolaro MA 1 Controle de Sistemas Robóticos Parte 3 Controle de Trajetórias I PID e Controle de Torque Computado Marco Antonio Meggiolaro PhD Departamento de Engenharia Mecânica Pontifícia Universidade Católica RJ Versão 29032022 Meggiolaro MA 2 t i i Pi desi i Di desi i Ii desi i 0 u K q q K q q K q q dt PID Robô u q Controle de Trajetórias I Controle PID Individual i i Pi desi i Di i u K q q K q para baixas velocidades qdesi 0 sem gravidade pouco atrito KIi 0 controle PD se já houver amortecimento KDi 0 i i Pi desi i u K q q controle P No text found No text found Meggiolaro MA 5 Exemplo de rotina de controle PID void mainloop increase the global time ms tT reads AD voltages from force sensors readADC calcwrenchSI calculates wristwrenchSI and wrenchSI reads resolver angles q averages 50 times ReadResolver fori0imaxjointi COMPUTE THE ACTUAL VELOCITY dqi qtabi1qtabi0T EndEffector directkinematicsq DesiredEndEffector EndEffector DesiredVariation qdes inversekinematicsDesiredEndEffector Meggiolaro MA 6 fori0imaxjointi COMPUTE THE DESIRED VELOCITY dqdesi qdtabi1qdtabi0T COMPUTE THE INTEGRAL TERM if fabsqiqdesi RESOLUTION05 integposerrori qdesiqiT ANTIWIND UP if integposerrori MAXINTEGERROR integposerrori MAXINTEGERROR else if integposerrori MAXINTEGERROR integposerrori MAXINTEGERROR COMPUTE THE PID POSITION COMMAND commandi PidPGiqdesiqi PidDGidqdesidqi PidIGiintegposerrori if commandi 29126 commandi 29126 if commandi 29126 commandi 29126 send DAC number for volt out fori0imaxjointi setDACi1commandi Meggiolaro MA 7 extern void directkinematicsint x static int i j k l static double s1 c1 s2 c2 s3 c3 s4 c4 s5 c5 s6 c6 qaux6 s1sinqaux0pi1800 c1cosqaux0pi1800 s2sinqaux1pi1800 c2cosqaux1pi1800 s3sinqaux2pi1800 c3cosqaux2pi1800 s4sinqaux3pi1800 c4cosqaux3pi1800 s5sinqaux4pi1800 c5cosqaux4pi1800 s6sinqaux5pi1800 c6cosqaux5pi1800 A000 c1 A002s1 A010 s1 A012 c1 A003 KP0aic1 A013 KP0ais1 A100 c2 A101s2 A110 s2 A111 c2 A103 KP1aic2 A113 KP1ais2 A200 c3 A201s3 A210 s3 A211 c3 A203 KP2aic3 A213 KP2ais3 A300 c4 A302 s4 A310 s4 A312c4 A303 KP3aic4 A313 KP3ais4 A400 c5 A402 s5 A410 s5 A412c5 A500 c6 A501s6 A510 s6 A511 c6 fori0 i4 i forj0 j4 j Tr0ijA0ij forl1 l6 l fori0 i4 i forj0 j4 j Trlij00 fork0 k4 k TrlijTrl1ikAlkj Meggiolaro MA 8 Exemplo Manipulador SCARA diversas indústrias contam com linhas de montagem e de processos automatizadas por manipuladores Meggiolaro MA 9 configuração SCARA com 2 graus de liberdade paralelos em um plano horizontal é muito utilizada na indústria Meggiolaro MA 10 estrutura mecânica dois motoredutores de corrente continua par de polias e correia perfis de alumínio encoders e tacômetros ponteira laser sistema eletrônico computador PC placa AD e DA ServoToGo amplificador RoboteQ Projeto de baixo custo de um SCARA Meggiolaro MA 11 manipulador RoboteQ PC ServoToGo Bateria 24V encoder tacômetro atuador laser Meggiolaro MA 12 Meggiolaro MA 13 Software de Controle de Posição Meggiolaro MA 20 elo 2 desejado elo 2 real elo 1 desejado elo 1 real Controle de Posição PID movimento do Elo 1 causa perturbações indesejáveis no Elo 2 devido ao acoplamento dinâmico Meggiolaro MA 21 robô plano 2 graus de liberdade motores nas juntas 1 1 2 1 2 1 1 2 1 2 x l cos l cos y l sin l sin Simulação robom 2 1 11 1 12 2 2 1 2 1 H H h 2h G 2 2 22 2 12 1 1 2 H H h G Meggiolaro MA 22 Simulação robom caso 0 sem controle 1 0 2 0 Meggiolaro MA 23 trajetória circular desejada Simulação d c d c x t x r cos 2 f t y t y r sin 2 f t d d x t r 2 f sin 2 f t y t r 2 f cos 2 f t 2 d 2 d x t r 2 f cos 2 f t y t r 2 f sin 2 f t Meggiolaro MA 24 no espaço das juntas cinemática inversa Jacobiana Simulação 1d 1 d d 2d 2 d d t f x t y t t f x t y t 1 1d 2 1d 2d 2 1d 2d d 1 1d 2 1d 2d 2 1d 2d l sin l sin l sin J l cos l cos l cos d 1d d d 2d d 1 1d d d 2d x t t J y t t x t t J y t t velocidades desejadas nas juntas Meggiolaro MA 25 acelerações desejadas nas juntas Simulação d 1d 1d d d d 2d 2d d 1 1d 1d d d d 2d 2d x t t t J J y t t t x t t t J J y t t t 2 2 1 1d d 1 1d 1d 2 1d 2d 1d 2d d 2 2 2d d 1 1d 1d 2 1d 2d 1d 2d t x l cos l cos J t y l sin l sin 2 d 2 d x t r 2 f cos 2 f t y t r 2 f sin 2 f t Meggiolaro MA 26 caso 1 controle P f 05Hz Simulação robom 1 P1 1d 1 K 2 P2 2d 2 K Meggiolaro MA 27 caso 2 controle PD f 05Hz Simulação robom 1 P1 1d 1 D1 1d 1 K K 2 P2 2d 2 D2 2d 2 K K Meggiolaro MA 28 por Feedforward baseado na trajetória desejada pouco robusto caso a trajetória real descole da desejada PD ou PID Dinâmica inversa Robô PD ou PID Controle de Torque Computado Robô q q Formas de eliminar os efeitos dinâmicos indesejáveis por Feedback baseado na trajetória real medida calculando em tempo real os torques ou forças necessárias para compensar os efeitos dinâmicos Controle de Torque Computado Meggiolaro MA 29 Controle de Torque Computado para compensar efeitos dinâmicos e da gravidade é necessário utilizar o controle de torque computado as equações da dinâmica são escritas na forma matricial exemplo para o manipulador RR G h H 2 1 2 1 1 2 2 2 1 22 12 12 11 2 1 G G 0 h h h 2 H H H H Meggiolaro MA 30 i n 1 j n 1 k j k ijk n 1 j ij j i G q q h H q H q hqq Gq 1 Compensação de Gravidade 0 hqq 0 q H q Gq de forma geral compensação da gravidade é feita estimandose o termo G e adicionandoo à lei de controle u para velocidades baixas Meggiolaro MA 31 lei de controle Gq u medido assim a dinâmica fica H q u PD Robô Gq u sem gravidade aparente a gravidade é eliminada porém o acoplamento continua devido a H q No text detected Meggiolaro MA 33 caso 3 controle PDG f 05Hz Simulação robom 1 P1 1d 1 D1 1d 1 1 K K G 2 P2 2d 2 D2 2d 2 2 K K G Meggiolaro MA 34 caso 4 controle PDG f 2Hz Simulação robom Meggiolaro MA 35 2 Desacoplamento lei de controle Hq u Gq medido idealizado Hq u Gq Hq q Gq como H é positivodefinida det H 0 logo Hq u Hq q u q i i q u logo Meggiolaro MA 36 PD Robô Gq u sem gravidade nem acoplamento aparentes Hq podese criar leis de controle distintas e desacopladas para cada junta todas com ganhos calibrados para massas unitárias em sistema linear q i i q u lei de controle pode também incluir termo de feedforward com as acelerações desejadas i desi Pi desi i Di desi i u q K q q K q q Meggiolaro MA 37 3 Linearização Hq u hqq Gq medidos Gq hqq Hq considere lei de controle idealizados H q u h qq Gq H q q h qq Gq i i u q logo H q u H q q u q onde i desi Pi desi i Di desi i u q K q q K q q Meggiolaro MA 38 caso 5 CTC com PD f 2Hz Simulação robom 1 P1 1d 1 D1 1d 1 u K K 2 P2 2d 2 D2 2d 2 u K K 1 1 1 2 2 2 u G H h u G Hq u hqq Gq Meggiolaro MA 39 caso 6 CTC com PD feedforward f 2Hz Simulação robom 1 1 1 2 2 2 u G H h u G Hq u hqq Gq 1 1d P1 1d 1 D1 1d 1 u K K 2 2d P2 2d 2 D2 2d 2 u K K Meggiolaro MA 40 caso 7 CTC com PD feedforward f 2Hz porém com erros nos valores estimados de l1 e l2 Necessita de controle adaptativo Simulação robom