Controle de Robôs Manipuladores

Transcrição

1 Universidade de São Paulo Escola de Engenharia de São Carlos Departamento de Engenharia Mecânica

2 Sumário Introdução e Simulador Introdução ao Simulador de Robô Manipulador Planar de 2 Graus de Liberdade Controle Robusto (H Linear, Representação Quase-LPV)

3 Histórico Introdução e Simulador Introdução Simulador de Robô Manipulador Planar de 2 Graus de Liberdade UNIMATE, Unimation, Inc. - George Devol, Joseph Engelberger : Torque Calculado (PAUL, 1973) : Controle Adaptativo (CRAIG, 1985; SLOTINE & LI, 1987) : Controle Robusto (SPONG & VIDYASAGAR, 1989; DAWSON, 1990)

4 Aplicações Introdução e Simulador Introdução Simulador de Robô Manipulador Planar de 2 Graus de Liberdade

5 Introdução Simulador de Robô Manipulador Planar de 2 Graus de Liberdade Estrutura de Planejamento no espaço operacional (cartesiano) Y (x 2, y 2 ) (x 1, y 1 ) q 2 q 1 X

6 Introdução Simulador de Robô Manipulador Planar de 2 Graus de Liberdade Estrutura de Planejamento no espaço operacional (cartesiano) Controle cartesiano cinemática direta

7 Introdução Simulador de Robô Manipulador Planar de 2 Graus de Liberdade Estrutura de Planejamento no espaço operacional (cartesiano) Controle cartesiano cinemática direta Controle no espaço das juntas cinemática inversa de toda trajetória KUKA KR 16: LIN(P1,P2)

8 Introdução Simulador de Robô Manipulador Planar de 2 Graus de Liberdade Estrutura de Planejamento no espaço das juntas Y (x 2, y 2 ) (x 1, y 1 ) q 2 q 1 X

9 Introdução Simulador de Robô Manipulador Planar de 2 Graus de Liberdade Estrutura de Planejamento no espaço das juntas Cinemática inversa apenas em dois pontos KUKA KR 16: PTP(P1,P2)

10 UARM e Ambiente de Controle Introdução Simulador de Robô Manipulador Planar de 2 Graus de Liberdade

11 Interface Gráfica Introdução e Simulador Introdução Simulador de Robô Manipulador Planar de 2 Graus de Liberdade

12 Introdução Simulador de Robô Manipulador Planar de 2 Graus de Liberdade Representação Esquemática do Manipulador Massas concentradas Y m 2, a 2 a c2 q 2 a c1 m 1, a 1 q 1 X

13 Cinemática Direta Introdução e Simulador Introdução Simulador de Robô Manipulador Planar de 2 Graus de Liberdade Cinemática direta de posição Dados q 1 e q 2, obter (X, Y ): X = a 1 cos(q 1 ) + a 2 cos(q 1 + q 2 ) Y = a 1 sen(q 1 ) + a 2 sen(q 1 + q 2 )

14 Cinemática Direta Introdução e Simulador Introdução Simulador de Robô Manipulador Planar de 2 Graus de Liberdade %Compute sum of joint angles for j = 1:2, phi(j) = sum(theta(1:j)); end; cphi = cos(phi); sphi = sin(phi); % Run forward kinematic to find out where joints are X(1) = 0; Y(1) = 0; for j = 2:2+1, X(j) = X(j-1) + l(j-1)*cphi(j-1); Y(j) = Y(j-1) + l(j-1)*sphi(j-1); end; % Display the robot for j = 1:2+1, set(motor(j),'xdata',x(j),'ydata',y(j)); end; set(link,'xdata',x,'ydata',y);

15 Trajetória desejada: polinômio do 3 grau Introdução Simulador de Robô Manipulador Planar de 2 Graus de Liberdade Dados: q d i (t 0 ), q d i (t 0 ), q d i (t f ) e q d i (t f ) Polinômio: qi d (t) = a i + b i (t t 0 ) + c i (t t 0 ) 2 + d i (t t 0 ) 3 q i d (t) = b i + 2c i (t t 0 ) + 3d i (t t 0 ) 2 Substituindo: qi d (t 0 ) = a i q i d (t 0 ) = b i qi d (t f ) = a i + b i (t f t 0 ) + c i (t f t 0 ) 2 + d i (t f t 0 ) 3 = a i + b i T + c i T 2 + d i T 3 q i d (t f ) = b i + 2c i T + 3d i T 2

16 Trajetória desejada: polinômio do 3 grau Introdução Simulador de Robô Manipulador Planar de 2 Graus de Liberdade De forma matricial: q d i (t 0 ) q i d (t 0 ) qi d (t f ) q i d (t f ) Solução a i b i c i d i = = T T 2 T T 3T T T 2 T T 3T 2 1 a i b i c i d i q d i (t 0 ) q d i (t 0 ) q d i (t f ) q d i (t f )

17 Modelo Dinâmico Introdução e Simulador Introdução Simulador de Robô Manipulador Planar de 2 Graus de Liberdade Equação dinâmica τ = M(q) q + C(q, q) q + G(q) M(q) - matriz de inércia C(q, q) - matriz de forças centrípetas e de Coriolis G(q) - vetor dos torques gravitacionais

18 Modelo Dinâmico Introdução e Simulador Introdução Simulador de Robô Manipulador Planar de 2 Graus de Liberdade Equação dinâmica τ = M(q) q + C(q, q) q + G(q) Elementos de M(q) M 11 (q) = m 1 ac1 2 + m 2(a1 2 + a2 c2 + 2a 1a c2 cos(q 2 )) M 12 (q) = M 21 (q) = m 2 (ac2 2 + a 1a c2 cos(q 2 )) M 22 (q) = m 2 ac2 2

19 Modelo Dinâmico Introdução e Simulador Introdução Simulador de Robô Manipulador Planar de 2 Graus de Liberdade Elementos de C(q, q) C 11 (q, q) = m 2 a 1 a c2 sen(q 2 ) q 2 C 12 (q, q) = m 2 a 1 a c2 sen(q 2 ) q 1 m 2 a 1 a c2 sen(q 2 ) q 2 C 21 (q, q) = m 2 a 1 a c2 sen(q 2 ) q 1 C 22 (q, q) = 0 Elementos de G(q) G 1 (q) = (m 1 ga c1 + m 2 ga 1 )cos(q 1 ) + m 2 ga c2 cos(q 1 + q 2 ) G 2 (q) = m 2 ga c2 cos(q 1 + q 2 )

20 Controle Proporcional-Derivativo Erro de acompanhamento de trajetória e = q d q Controle Proporcional-Derivativo (PD) τ = K p e + K d ė sendo K p e K d matrizes diagonais Valores altos de ganho para eliminar distúrbios Erro de regime devido aos torques gravitacionais

21 Controle Proporcional-Integral-Derivativo Controle Proporcional-Integral-Derivativo (PID) τ = K p e + K i edt + K d ė Valores altos de ganho para eliminar distúrbios Erro de regime nulo

22 Controle PD com Compensação da Gravidade Controle Proporcional-Derivativo (PD) com Compensação da Gravidade τ = K p e + K d ė + G(q) Erro de regime nulo

23 Linearização por Realimentação Linearização por Realimentação Dinâmica do robô manipulador τ = M(q) q + C(q, q) q + G(q) τ = M(q) q + b(q, q) Derivando o erro de acompanhamento de trajetória ė = q d q ë = q d q Substituindo q ë = q d M(q) 1 [τ b(q, q)]

24 Linearização por Realimentação Linearização por Realimentação Espaço de estados: x = [e ė] T [ ė ë ] = [ 0 I 0 0 ] [ ė e ] [ 0 + I Definindo a variável de controle Sistema linear [ ė ë u = q d M(q) 1 [τ b(q, q)] ] = [ 0 I 0 0 ] ( q d M(q) 1 [τ b(q, q)]) ] [ ė e ] [ 0 + I ] u

25 Linearização por Realimentação τ = M(q)( q d u) + b(q, q) Substituindo na equação dinâmica M(q) q + b(q, q) = M(q)( q d u) + b(q, q) ë = u

26 Diagrama de blocos Linearização por Realimentação

27 Linearização por Realimentação + Proporcional-Derivativo (PD) u = K d ė K p e τ = M(q)( q d + K d ė + K p e) + b(q, q) Equação dinâmica do erro ë + K d ė + K p e = 0

28 Linearização por Realimentação Valores estimados de M(q) e b(q, q) τ = ˆM(q)( q d u) + ˆb(q, q) Substituindo na equação dinâmica M(q) q + b(q, q) = ˆM(q)( q d u) + ˆb(q, q) Somando e subtraindo ˆM(q) q [ ë = u + M(q) 1 M(q) ˆM(q) ] [ q + M(q) 1 b(q, q) ˆb(q, ] q)

29 Linearização por Realimentação + PD τ = ˆM(q)( q d + K d ė + K p e) + ˆb(q, q) Equação dinâmica do erro ë + K d ė + K p e = d sendo d = q + M(q) 1 [b(q, q) ˆb(q, q)] = M(q) 1 [M(q) ˆM(q)]

30 Parametrização Linear Parametrização Linear Controle Torque Calculado + H linear Controle via Representação Quasi-LPV Propriedade de robôs manipuladores τ = M(q) q + C(q, q) q + G(q) = Y (q, q, q)θ sendo θ o vetor de parâmetros do manipulador e Y (q, q, q) a matriz de regressão. Elementos de M(q) q M(q) q = m 1 a 2 c1 q 1 + m 2 (a a2 c2 + 2a 1a c2 cos(q 2 )) q 1 +m 2 (a 2 c2 + a 1a c2 cos(q 2 )) q 2 m 2 (a 2 c2 + a 1a c2 cos(q 2 )) q 1 + m 2 a 2 c2 q 2

31 Parametrização Linear Parametrização Linear Controle Torque Calculado + H linear Controle via Representação Quasi-LPV Vetor dos torques de Coriolis e centrípetos 2m 2 a 1 a c2 sen(q 2 ) q 1 q 2 + m 2 a 1 a c2 sen(q 2 ) q 2 2 C(q, q) q = m 2 a 1 a c2 sen(q 2 ) q 1 2 Vetor dos torques gravitacionais m 1 ga c1 cos(q 1 ) + m 2 ga 1 cos(q 1 ) + m 2 ga c2 cos(q 1 + q 2 ) G(q) = m 2 ga c2 cos(q 1 + q 2 )

32 Parametrização Linear Parametrização Linear Controle Torque Calculado + H linear Controle via Representação Quasi-LPV Definindo: θ = [m 1 m 2 ] T Matriz de regressão [ ] y11 y Y (q, q, q) = 12 0 y 22 y 11 = a 2 c1 q 1 + ga c1 cos(q 1 ) y 12 = (a a2 c2 + 2a 1a c2 cos(q 2 )) q 1 + (a 2 c2 + a 1a c2 cos(q 2 )) q 2 a 1 a c2 sen(q 2 )(2 q 1 q 2 + q 2 2 ) + ga 1cos(q 1 )+ ga c2 cos(q 1 + q 2 )) y 22 = (a 2 c2 + a 1a c2 cos(q 2 )) q 1 + a 2 c2 q 2 + a 1 a c2 sen(q 2 ) q ga c2 cos(q 1 + q 2 ))

33 Parametrização Linear Controle Torque Calculado + H linear Controle via Representação Quasi-LPV Torque Calculado + PD τ = ˆM(q)( q d + K d ė + K p e) + Ĉ(q, q) q + Ĝ(q) Valores estimados dos parâmetros são mantidos fixos Controle adaptativo: valores estimados dos parâmetros variam com o tempo Pode-se reescrever τ = ˆM(q)(ë + K d ė + K p e) + ˆM(q) q + Ĉ(q, q) q + Ĝ(q) τ = ˆM(q)(ë + K d ė + K p e) + Y (q, q, q)ˆθ

34 Parametrização Linear Controle Torque Calculado + H linear Controle via Representação Quasi-LPV Considerando: τ = M(q) q + C(q, q) q + G(q) = Y (q, q, q)θ Equação do erro: ë + K d ė + K p e = ˆM 1 (q)y (q, q, q) θ sendo θ = θ ˆθ Epaço de estados: x = [e [ ] [ ė 0 I ẋ = = ë K p K d ė] T ] [ 0 x + I ] ˆM 1 (q)y (q, q, q) θ

35 Parametrização Linear Controle Torque Calculado + H linear Controle via Representação Quasi-LPV Função de Lyapunov V (x) = x T Px + θ T Γ 1 θ > 0 P Matriz simétrica definida positiva Γ Matriz diagonal definida positiva V (x) = x T Pẋ + ẋ T Px + 2 θ T 1 Γ θ V (x) =x T P(Ax + B ˆM 1 (q)y ( ) θ) + (Ax + B ˆM 1 (q)y T ( ) θ)px + 2 θ T 1 Γ θ

36 Parametrização Linear Controle Torque Calculado + H linear Controle via Representação Quasi-LPV Manipulando V (x) = x T Qx + 2 θ T (Γ 1 θ + Y T ( ) ˆM 1 (q)b T Px) Q Matriz simétrica definida positiva que satisfaz a equação de Lyapunov: Definindo: Então A T P + PA = Q θ = ΓY T ( ) ˆM 1 (q)b T Px V (x) = x T Qx < 0

37 Parametrização Linear Controle Torque Calculado + H linear Controle via Representação Quasi-LPV Parâmetro θ é constante Lembrando: θ = θ ˆθ Lei de adaptação: ˆθ = ΓY T (q, q, q) ˆM 1 (q)b T Px x é assintoticamente estável θ limitado se ˆM 1 (q) existe

38 Parametrização Linear Controle Torque Calculado + H linear Controle via Representação Quasi-LPV Resumo Lei de controle: τ = ˆM(q)( q d + K d ė + K p e) + Ĉ(q, q) q + Ĝ(q) Lei de adaptação: ˆθ = ΓY T (q, q, q) ˆM 1 (q)b T Px Equação de Lyapunov: A T P + PA = Q

39 Parametrização Linear Controle Torque Calculado + H linear Controle via Representação Quasi-LPV Excitação Persistente Posição desejada, q d (s) Tempo, t (s)

40 Parametrização Linear Controle Torque Calculado + H linear Controle via Representação Quasi-LPV Controle Torque Calculado + PD + Robusto H Controle Torque Calculado + PD + Robusto H Linear τ = ˆM(q)( q d + K d ė + K p e + u R ) + Ĉ(q, q) q + Ĝ(q) Equação no espaço de estados com distúrbio w [ ] [ ] [ ] [ ] [ ė 0 I ė 0 0 = + u ë K p K d e I R + I y = e = [I 0]x ] w ẋ = Ax + B 1 u R + B 2 w y = C 2 x

41 Controle H linear Parametrização Linear Controle Torque Calculado + H linear Controle via Representação Quasi-LPV Considere o sistema linear ẋ = Ax + B 1 w + B 2 u z = C 1 x + D 12 u y = C 2 x + D 21 w z P w Controlador K ẋ = A K x + B K y u = C K x + D K y y K u Controle sub-ótimo H linear T zw < γ

42 Controle H linear Parametrização Linear Controle Torque Calculado + H linear Controle via Representação Quasi-LPV Solução A K = A B 2 B T 2 X + γ 2 B 1 B T 1 X + (I γ 2 Y X ) 1 YC T 2 C 2 B K = (I γ 2 Y X ) 1 YC T 2 C K = B T 2 X D K = 0 X e Y são soluções das equações algébricas de Ricatti A T X + X A + γ 2 X (B 1 B T 1 B 2B T 2 )X + C T 1 C 1 = 0 AY + Y A T + γ 2 Y (C T 1 C 1 C T 2 C 2)Y + B T 1 B 1 = 0

43 Parametrização Linear Controle Torque Calculado + H linear Controle via Representação Quasi-LPV Controle via Representação Quasi-LPV Sistema Linear a Parâmetros Variantes (LPV) Sistema Quase-LPV ẋ = A(ρ(t))x + B(ρ(t))u, ẋ = A(ρ(x))x + B(ρ(x))u, Estado: erro de acompanhamento de trajetória [ ] q q d x = q q d = [ q q ]

44 Parametrização Linear Controle Torque Calculado + H linear Controle via Representação Quasi-LPV Controle via Representação Quasi-LPV Dinâmica estimada do robô manipulador Representação Quasi-LPV sendo w = ˆM 1(q)δ(q, q, q) τ = ˆM(q) q + Ĉ(q, q) q + Ĝ(q) x = A(q, q) x + Bu + Bw δ(q, q, q) incertezas paramétricas e distúrbios externos

45 Parametrização Linear Controle Torque Calculado + H linear Controle via Representação Quasi-LPV Controle via Representação Quasi-LPV Matrizes dinâmicas A(q, q) = B = [ ˆM 1 (q)ĉ(q, q) 0 [ In ] I n 0 0 ] Torques aplicados τ = ˆM(q)( q d + u) + Ĉ(q, q) qd + Ĝ(q)

46 Controle H para sistemas LPV Parametrização Linear Controle Torque Calculado + H linear Controle via Representação Quasi-LPV Considere o sistema LPV: ẋ = A(ρ(t))x + B 1 (ρ(t))w + B 2 (ρ(t))u, z 1 = C 1 (ρ(t))x, z 2 = C 2 (ρ(t))x + u, ρ(t) P R m e ρ i ν i com ν i 0. Ganho L 2 γ no intervalo [0, T ] se: T 0 T z(t) 2 dt γ 2 w(t) 2 dt 0

47 Controle H para sistemas LPV Parametrização Linear Controle Torque Calculado + H linear Controle via Representação Quasi-LPV Se existe X(ρ(x)) > 0 tal que E(ρ) X(ρ)C T 1 (ρ) B 1(ρ) C 1 (ρ)x(ρ) I 0 B T 1 (ρ) 0 γ2 I < 0, E(ρ) = m i=1 ± ( ν i X ρ i ) + Â(ρ)X(ρ) + X(ρ)Â(ρ)T B 2 (ρ)b T 2 (ρ), o sistema em malha fechada possui ganho L 2 γ se u = (B 2 (ρ)x 1 (ρ) + C 2 (ρ))x

48 Bibliografia Introdução e Simulador Parametrização Linear Controle Torque Calculado + H linear Controle via Representação Quasi-LPV CRAIG, J.J.; Adaptive Control of Mechanical Manipulators - Addison-Wesley Pub. Co., Reading, MA (1985). CRAIG, J.J.; Introduction to Robotics: Mechanics and Control - Addison-Wesley Pub. Co., Reading, MA, 2nd ed. (1989). DAWSON,D.M., et. al.; Robust Control for the Tracking of Robot Motion - International Journal of Control, vol. 52, no. 3, pp , (1990). LEWIS, F.L.; ABDALLAH, C.T.; DAWSON,D.M.; Control of Robot Manipulators - Macmillan. (1993) PAUL, R.P.; Trajectory Calculation and Servoing of a Computer Controlled Arm Technical Report AIM-177. Stanford University, Artificial Intelligence Laboratory (1972). SCIAVICCO, L.; SICILIANO, B; Modeling and Control of Robot Manipulators - McGraw-Hill. (1996). SLOTINE, J.J.; LI, W.; On the adaptive control of robot manipulators - International Journal of Robotics Research, vol. 6, no. 3, p (1987). SPONG, M. W.; VIDYASAGAR, M.; Robot Dynamics and Control - New York : Wiley, (1989).