Planejamento de Trajetória e Controle Pid

Aula 2 - Planejamento de Trajetória,
Controle PID e UArm II
SEM5875 - Controle de Sistemas Robóticos
Universidade de São Paulo
Adriano A. G. Siqueira
SEM5875 - Controle de Sistemas Robóticos Universidade de São Paulo

AulaAdriano
2 - Plan.
A. Trajetória,
G. Siqueira Controle PID, UArm II
Dinâmica no Espaço Cartesiano
Cinamática direta
x = f(q)
Cinemática da velocidade (Jacobiana)
ẋ = J(q)q̇
Derivando com relação ao tempo

˙ q̇
ẍ = J(q)q̈ + J(q)
Assumindo que n = 6 (J quadrada) e não há singularidades

(det(J(q)) 6= 0)
q̈ = J −1 (q)ẍ − J −1 (q)J(q)
˙ q̇

AulaAdriano
2 - Plan.
A. Trajetória,
Dinâmica no Espaço Cartesiano
Modelo Dinâmico no espaço das juntas
M(q)q̈ + b(q, q̇) = τ
Substituindo
M(q)J −1 (q)ẍ + b(q, q̇) − M(q)J −1 (q)J(q)J

˙ −1
(q)ẋ = τ
Considerando τ = J T (q)F
J −T (q)M(q)J −1 (q)ẍ + J −T (q)(b(q, q̇)

− M(q)J −1 (q)J(q)J
˙ −1
(q)ẋ)) = F
Mẍ + b = F

AulaAdriano
2 - Plan.
A. Trajetória,
Controle de Manipuladores
Caracterı́sticas que devem ser consideradas

Tipo do manipulador
Tipo de aplicação
Tipo da trajetória desejada
Atuadores e sensores utilizados
Tipo de transmissão entre atuadores e juntas
Presença de erros de modelagem e distúrbios

AulaAdriano
2 - Plan.
A. Trajetória,
Estrutura de Controle de Robôs Manipuladores
Planejamento no espaço operacional (cartesiano)
(x2, y2)
Y
(x1, y1)
q2
q1
X

AulaAdriano
2 - Plan.
A. Trajetória,
Controle cartesiano ⇒ cinemática direta

AulaAdriano
2 - Plan.
A. Trajetória,
Controle cartesiano ⇒ cinemática direta
Controle no espaço das juntas ⇒ cinemática inversa de toda
trajetória
KUKA KR 16: LIN(P1,P2)

AulaAdriano
2 - Plan.
A. Trajetória,
Planejamento no espaço das juntas
(x2, y2)
Y
(x1, y1)
q2
q1
X

AulaAdriano
2 - Plan.
A. Trajetória,
Planejamento no espaço das juntas
Cinemática inversa apenas em dois pontos
KUKA KR 16: PTP(P1,P2)

AulaAdriano
2 - Plan.
A. Trajetória,
Trajetória desejada: polinômio do 3◦ grau
Dados: qid (t0 ), q̇id (t0 ), qid (tf ) e q̇id (tf )

Polinômio:
qid (t) = ai + bi (t − t0 ) + ci (t − t0 )2 + di (t − t0 )3
q̇id (t) = bi + 2ci (t − t0 ) + 3di (t − t0 )2
Substituindo:
qid (t0 ) = ai
q̇id (t0 ) = bi
qid (tf ) = ai + bi (tf − t0 ) + ci (tf − t0 )2 + di (tf − t0 )3
= ai + bi T + ci T 2 + di T 3
q̇id (tf ) = bi + 2ci T + 3di T 2

AulaAdriano
2 - Plan.
A. Trajetória,
Trajetória desejada: polinômio do 3◦ grau
De forma matricial:
 d    
qi (t0 ) 1 0 0 0 ai
 q̇ d (t0 )   0 1 0 0   bi 
 i
 q d (tf )  = 
   
i 1 T T 2 T 3   ci 
q̇id (tf ) 0 1 2T 3T 2 di
Solução
−1 
qid (t0 )
   
ai 1 0 0 0
 bi   0 1 0 0   q̇id (t0 ) 
 ci  =  1 T
     
T 2 T 3   qid (tf ) 
di 0 1 2T 3T 2 q̇id (tf )

AulaAdriano
2 - Plan.
A. Trajetória,
Trajetória desejada: função linear
com extremidades parabólicas
Dados: qid (t0 ), q̇id (t0 ), qid (tf ), q̇id (tf ), vi e tb
Trajetória:
 ai + bi (t − t0 ) + ci (t − t0 )2 ,

t0 ≤ t ≤ t0 + tb
d
qi (t) = d + vi t, t0 + tb ≤ t ≤ tf − tb
 i
ei + fi (t − tf ) + gi (t − tf )2 , tf − tb ≤ t ≤ tf
Solução
vi − q̇id (t0 )
ai = qid (t0 ) bi = q̇id (t0 ) ci =
2tb
2qi (t0 ) + (q̇id (t0 ) − vi )tb − 2vt0
di = ei = qid (tf ) fi = q̇id (tf )
2
q̇ d (tf ) − vi 2qi (tf ) + (q̇id (tf ) + vi )tb − 2di
gi = i tf =
2tb 2vi
AulaAdriano
2 - Plan.
A. Trajetória,
Dinâmica dos Atuadores
Dinâmica do manipulador
M(q)q̈ + C (q, q̇)q̇ + F (q̇) + G (q) = τ
Dinâmica do atuador
JM q̈M + B q̇M + FM + Rτ = KM v
JM = diag {JMi } ⇒ matriz diagonal dos momentos de inércia

dos motores
B = diag {BMi + Kbi KMi /Rai } ⇒ matriz de amortecimento
FM = vec{FMi } ⇒ vetor de atrito
R = diag {ri } ⇒ matriz das reduções das engrenagens
qM ⇒ coordenadas dos eixos dos motores
KM = diag {KMi /Rai } ⇒ matriz de constante de torque
AulaAdriano
2 - Plan.
A. Trajetória,
Reduções das engrenagens
qi = ri qMi ⇒ q = RqM
Substituindo na dinâmica dos atuadores
R −1 JM q̈ + R −1 B q̇ + FM + Rτ = KM v
Substituindo na dinâmica do manipulador
R −1 JM q̈+R −1 B q̇+FM +R (M(q)q̈ + C (q, q̇)q̇ + F (q̇) + G (q)) = KM v
Simplificando
+ R 2 M(q) q̈+ B + R 2 C (q, q̇) q̇+ RFM + R 2 F (q̇) +R 2 G (q) = RKM v

JM
M 0 (q)q̈ + C 0 (q, q̇)q̇ + F 0 (q̇) + G 0 (q) = K 0 v

AulaAdriano
2 - Plan.
A. Trajetória,
Decomposição em componentes
ri KM
JMi + ri2 mii q̈i + Bi q̇i + RFMi = v − ri2 di

i = 1, · · · , n
Rai
sendo X X
di = mij q̈ij + Cjki q̇j q̇k + Fi + Gi
j6=i j,k
mij ⇒ elementos fora da diagonal da matriz M 0 (q)

Cjki ⇒ componentes de C 0 (q, q̇)q̇
Conclusão: se ri é pequeno ⇒ n equações desacopladas de

segunda ordem com coeficientes constantes

AulaAdriano
2 - Plan.
A. Trajetória,
Controle de Manipuladores
Erro de acompanhamento de trajetória
e(t) = qd (t) − q(t)
Controle independente das juntas (PD)
τ = Kv ė + Kp e
sendo Kv e Kp matrizes diagonais. Valores altos de ganho

para eliminar distúrbios. Erro de regime devido aos torques
gravitacionais.
Controle PD com compensação gravitacional
τ = Kv ė + Kp e + G (q)

AulaAdriano
2 - Plan.
A. Trajetória,
Controlador PID
P =⇒ Proporcional
I =⇒ Integral
D =⇒ Derivativo

AulaAdriano
2 - Plan.
A. Trajetória,
Controlador Proporcional
Entrada de controle linearmente proporcional ao erro
u(t) = KP e(t)
Função de transferência
C (s) = KP

AulaAdriano
2 - Plan.
A. Trajetória,
Controlador Proporcional
Exemplo: Sistema de Segunda Ordem

10
G (s) =
(s + 5)(s + 10)
Sistemas de ordem maior: instabilidade

Redução do erro de regime X estabilidade
Exemplo: Sistema de Terceira Ordem
1
G (s) =
s(s + 1)(s + 5)

AulaAdriano
2 - Plan.
A. Trajetória,
Controlador Integral
Entrada de controle proporcional à integral do erro

Z t
u(t) = KI edη
t0
KI
C (s) =
s

AulaAdriano
2 - Plan.
A. Trajetória,
Controlador Integral
Erro de regime nulo e independente do valor de K

10
G (s) =
(s + 5)(s + 10)

AulaAdriano
2 - Plan.
A. Trajetória,
Controlador Proporcional-Integral
Entrada de controle proporcional ao erro e à integral do erro

Z t
u(t) = KP e(t) + KI edη
t0
KI
C (s) = KP +
s
10
G (s) =
(s + 5)(s + 10)

AulaAdriano
2 - Plan.
A. Trajetória,
Controlador Derivativo
Entrada de controle proporcional à derivada do erro
u(t) = KD ė
C (s) = KD s

AulaAdriano
2 - Plan.
A. Trajetória,
Controlador Derivativo
Natureza antecipatória
Não utilizado sozinho (ė = 0 ⇒ u = 0)
Aumenta estabilidade

10
G (s) =
(s + 5)(s + 10)

AulaAdriano
2 - Plan.
A. Trajetória,
Controlador PID
Entrada de controle proporcional ao erro, à integral do erro e

à derivada do erro
Z t
u(t) = KP e(t) + KI edη + KD ė
t0
KI
C (s) = KP + + KD s
s
Pólos podem ser alocados em qualquer posição

AulaAdriano
2 - Plan.
A. Trajetória,
Regras de Ziegler-Nichols
Ajuste inicial
Modelo do sistema como:
Ke −td s
G (s) =
(τ s + 1)
Taxa de decaimento: 0,25

Fator de amortecimento: ζ = 0, 21

AulaAdriano
2 - Plan.
A. Trajetória,
Método 1: Entrada degrau ao sistema
L T+L

AulaAdriano
2 - Plan.
A. Trajetória,
Controlador KP TI TD
T
P L ∞ 0
PI 0.9 TL L
0.3 0
PID 1.2 TL 2L 0.5L

AulaAdriano
2 - Plan.
A. Trajetória,

Exemplo:
10
G (s) =
(s + 5)(s + 10)
L = 0.04 e T = 0.4
KP = 12, TI = 0.08 e TD = 0.02
KP
KI = TI = 150
KD = KP TD = 0.24

AulaAdriano
2 - Plan.
A. Trajetória,
Método 2: Oscilação contı́nua com ganho proporcional ⇒ Kcr
Pcr

AulaAdriano
2 - Plan.
A. Trajetória,
Método 2: Oscilação contı́nua com ganho proporcional
Controlador KP TI TD
P 0.5Kcr ∞ 0
1
PI 0.45Kcr 1.2 Pcr 0
1
PID 0.6Kcr 0.5Pcr 8 Pcr

AulaAdriano
2 - Plan.
A. Trajetória,
Método 2: Oscilação contı́nua com ganho proporcional

Exemplo:
1
G (s) =
s(s + 1)(s + 5)
Kcr = 30 e Pcr = 2.81
KP = 18, TI = 1.40 e TD = 0.35

AulaAdriano
2 - Plan.
A. Trajetória,
Controlador PID Discreto
Para T0 pequeno
k−1
" #
T0 X TD
τ (k) = Kp e(k) + e(v ) + (e(k) − e(k − 1))
TI T0
v =0

AulaAdriano
2 - Plan.
A. Trajetória,
Controlador PID
Forma recursiva
τ (k) = τ (k − 1) + q0 e(k) + q1 e(k − 1) + q2 e(k − 2)
TD
q0 =Kp (1 + )
T0
2TD T0
q1 = − Kp (1 + − )
T0 TI
TD
q2 =Kp
T0

AulaAdriano
2 - Plan.
A. Trajetória,
Controlador PID
Componentes
τ (k) = τP (k) + τI (k) + τD (k)
τP (k) =Kp e(k)

T0
τI (k) =τ (k − 1) + Kp e(k − 1)
TI
TD
τD (k) =Kp [e(k) − e(k − 1)]
T0

AulaAdriano
2 - Plan.
A. Trajetória,
UArm II
O Robo UArm II é um manipulador planar composto por três

juntas rotacionais e um efetuador.

AulaAdriano
2 - Plan.
A. Trajetória,

Planejamento de Trajetória e Controle Pid

Enviado por

Dados do documento

Direitos autorais

Formatos disponíveis

Compartilhar este documento

Compartilhar ou incorporar documento

Opções de compartilhamento

Você considera este documento útil?

Este conteúdo é inapropriado?

Direitos autorais:

Formatos disponíveis

Planejamento de Trajetória e Controle Pid

Enviado por

Direitos autorais:

Formatos disponíveis

Aula 2 - Planejamento de Trajetória,

Controle PID e UArm II

SEM5875 - Controle de Sistemas Robóticos

Universidade de São Paulo

SEM5875 - Controle de Sistemas Robóticos Universidade de São Paulo

Derivando com relação ao tempo

Assumindo que n = 6 (J quadrada) e não há singularidades

SEM5875 - Controle de Sistemas Robóticos Universidade de São Paulo

Modelo Dinâmico no espaço das juntas

M(q)q̈ + b(q, q̇) = τ

M(q)J −1 (q)ẍ + b(q, q̇) − M(q)J −1 (q)J(q)J

J −T (q)M(q)J −1 (q)ẍ + J −T (q)(b(q, q̇)

SEM5875 - Controle de Sistemas Robóticos Universidade de São Paulo

Caracterı́sticas que devem ser consideradas

SEM5875 - Controle de Sistemas Robóticos Universidade de São Paulo

Planejamento no espaço operacional (cartesiano)

SEM5875 - Controle de Sistemas Robóticos Universidade de São Paulo

SEM5875 - Controle de Sistemas Robóticos Universidade de São Paulo

KUKA KR 16: LIN(P1,P2)

SEM5875 - Controle de Sistemas Robóticos Universidade de São Paulo

Planejamento no espaço das juntas

SEM5875 - Controle de Sistemas Robóticos Universidade de São Paulo

KUKA KR 16: PTP(P1,P2)

SEM5875 - Controle de Sistemas Robóticos Universidade de São Paulo

Dados: qid (t0 ), q̇id (t0 ), qid (tf ) e q̇id (tf )

SEM5875 - Controle de Sistemas Robóticos Universidade de São Paulo

SEM5875 - Controle de Sistemas Robóticos Universidade de São Paulo

M(q)q̈ + C (q, q̇)q̇ + F (q̇) + G (q) = τ

JM = diag {JMi } ⇒ matriz diagonal dos momentos de inércia

Reduções das engrenagens

Substituindo na dinâmica dos atuadores

Substituindo na dinâmica do manipulador

R −1 JM q̈+R −1 B q̇+FM +R (M(q)q̈ + C (q, q̇)q̇ + F (q̇) + G (q)) = KM v

+ R 2 M(q) q̈+ B + R 2 C (q, q̇) q̇+ RFM + R 2 F (q̇) +R 2 G (q) = RKM v

M 0 (q)q̈ + C 0 (q, q̇)q̇ + F 0 (q̇) + G 0 (q) = K 0 v

SEM5875 - Controle de Sistemas Robóticos Universidade de São Paulo

mij ⇒ elementos fora da diagonal da matriz M 0 (q)

Conclusão: se ri é pequeno ⇒ n equações desacopladas de

SEM5875 - Controle de Sistemas Robóticos Universidade de São Paulo

Erro de acompanhamento de trajetória

e(t) = qd (t) − q(t)

Controle independente das juntas (PD)

sendo Kv e Kp matrizes diagonais. Valores altos de ganho

SEM5875 - Controle de Sistemas Robóticos Universidade de São Paulo

SEM5875 - Controle de Sistemas Robóticos Universidade de São Paulo

Entrada de controle linearmente proporcional ao erro

SEM5875 - Controle de Sistemas Robóticos Universidade de São Paulo

Exemplo: Sistema de Segunda Ordem

Sistemas de ordem maior: instabilidade

SEM5875 - Controle de Sistemas Robóticos Universidade de São Paulo

Entrada de controle proporcional à integral do erro

SEM5875 - Controle de Sistemas Robóticos Universidade de São Paulo

Erro de regime nulo e independente do valor de K

Exemplo: Sistema de Segunda Ordem

SEM5875 - Controle de Sistemas Robóticos Universidade de São Paulo

Entrada de controle proporcional ao erro e à integral do erro

SEM5875 - Controle de Sistemas Robóticos Universidade de São Paulo

Entrada de controle proporcional à derivada do erro

SEM5875 - Controle de Sistemas Robóticos Universidade de São Paulo

Exemplo: Sistema de Segunda Ordem

SEM5875 - Controle de Sistemas Robóticos Universidade de São Paulo

Entrada de controle proporcional ao erro, à integral do erro e

SEM5875 - Controle de Sistemas Robóticos Universidade de São Paulo

Taxa de decaimento: 0,25