Para que as trajetórias dos dois braços e mãos, de uma postura inicial, θ0 = (θ1,0, , . . . , θ22,0)>, a uma final, θf = (θ1,f, , . . . , θ22,f)>, seja suave e livre de coli-
sões, o movimento total das juntas do braço-mão direito e esquerdo (com total de juntas igual a nj = 22) ao longo do tempo t é dado pela sobreposição de um movi- mento direto (Tdireto) e de um movimento de back-and-forth (Tbkfor) (Rosenbaum et al., 1999):
θ (t) = T (t; θf, θb) = θ0+ Tdireto(t; θf) + Tbkf or(t; θb) , (4.12)
onde t ∈ [0, T ], em que T ∈ R+ é o tempo total do movimento e t é discretizado num número NT de passos igualmente espaçados de ti = i∆, onde ∆ = NT
T é o tamanho
do passo e i = 0, 1, . . . , NT. Posto isto, θ (ti) representa o movimento total das
juntas θ (t) no momento ti.
4.3.1 Movimento Direto
O movimento direto é concebido para produzir um perfil de velocidade bell-shaped (ou se preferir, suave) em todas as juntas dos braços e mãos, de uma posição inicial a uma final. Para tal, de forma a gerar a trajetória baseada no princípio do minimum
angular jerk, implica minimizar as alterações da aceleração angular. Assim, este
(Costa e Silva, 2011)): Tdireto(t; θf) = (θf− θ0) 10τ3− 15τ4+ 6τ5 + ω0T τ − 6τ3+ 8τ4− 3τ5+ ωfT −4τ3+ 7τ4− 3τ5 + 12α0T2 τ2− 3τ3+ 3τ4− τ5 + 12αfT2 τ3− 2τ4+ τ5 , (4.13) onde θ0 = (θ1,0, θ2,0, . . . , θ22,0)> são os valores iniciais dos ângulos das juntas do
braço-mão direito e esquerdo no instante inicial (t = 0) e θf = (θ1,f, θ2,f, . . . , θ22,f)>
são os valores finais dos ângulos das juntas dos braços-mãos no momento final (t = T ) e em que τ = Tt ∈ [0, 1] é o tempo normalizado. Os parâmetros ω0 e α0 ∈ Rnj são vetores que representam a velocidade e aceleração inicial das juntas dos dois braços, respetivamente. Enquanto que os parâmetros ωf e αf ∈ Rnj são vetores que representam a velocidade e aceleração final das juntas dos dois braços, respetivamente.
4.3.2 Movimento de Back-and-Forth
De forma a evitar colisões ao longo das trajetórias dos dois braços e mãos propõe-se um movimento de back-and-forth que, essencialmente, consiste num movimento de “forth”: da postura inicial, θ0, à postura de bounce, θb, seguido de um movimento de “back”: da postura de bounce de volta à postura inicial. De tal modo que a trajetória gerada por este movimento de back-and-forth para o conjunto das juntas dos braços e mãos é dado por:
Tbkfor(t; θb) = (θb− θ0) sen2
πτϑ , (4.14) em que ϑ = −ln2
lntb onde o parâmetro tb ∈ ]0, 1[ é usado para controlar quando a
postura de bounce, θb, é alcançada e, consequentemente, o movimento “forth” finaliza e começa o movimento de “back”.
Para calcular este movimento é necessário calcular a postura de bounce, θb, postura essa que permita que não ocorram colisões em todo o movimento dos dois braços. Assim sendo, no próximo ponto é explicado o processo de seleção desta postura.
4.3.2.1 Seleção da Postura de Bounce
Para a seleção da postura de bounce aplica-se o mesmo critério que para a seleção das posturas finais dos braços, isto é: o mínimo deslocamento das juntas, ou antes, o prin- cípio do minimum angular jerk. No entanto, uma correta postura de bounce é aquela que: 1) permite um movimento livre de colisões do início ao fim, sempre dentro dos limites físicos das juntas e 2) apresenta um custo mínimo de deslocamento das juntas do braço direito e esquerdo, da postura inicial θ1,0, . . . , θnj
2,0 , θ12,0, . . . , θ11+nj 2,0 > à postura de bounce θ1,b, . . . , θnj 2 ,b , θ12,b, . . . , θ11+nj 2,b >
, que por sua vez pode ser matematicamente escrito da seguinte forma:
min Θb⊂Rnj nj 2 X k=1 λk(θk,0− θk,b)2+ 11+nj2 X k=12 λk(θk,0− θk,b)2, (4.15)
onde Θb ⊂ Rnj representa o conjunto das possíveis posturas admissíveis para os
braços que permitem uma trajetória livre de colisões para cada uma das juntas k e
λk> 0 é um fator de custo atribuído a cada junta.
O número de juntas, nj, depende do tipo de movimento a planear e pode ser de 14 (apenas as juntas dos braços) ou 18 juntas (as juntas dos braços mais duas juntas de cada mão). Por exemplo, casos como os movimentos de alcançar objetos ou retornar à postura de repouso são formulados com nj = 18, porque tanto a trajetória das juntas dos braços como as juntas dos dedos devem ser planeadas ao longo do movimento. Assim para cada braço calculam-se as suas sete juntas: direito (θ1, . . . , θ7) e esquerdo (θ12, . . . , θ18) mais duas juntas de cada mão: θ8= 0 e θ9 para
a mão direita e θ19 = 0 e θ20 para a mão esquerda. Note-se que, para cada mão,
apenas basta calcular o valor da junta da mão direita θ9 e da mão esquerda θ20, pois as outras duas juntas dos dedos de cada mão são iguais.
No entanto, para os casos de transporte de objetos e de inserção, estes devem ser formulados com nj = 14, porque a trajetória dos dedos não necessita de ser calculada.
De modo a selecionar uma única postura de bounce ótima para o braço direito e esquerdo de entre todas as possíveis posturas, Θb ⊂ Rnj, o problema de otimiza- ção 4.15 é reformulado como um problema de otimização com restrições nas suas
variáveis, nomeadamente:
Restrição 1: Os valores das juntas dos braços, ao longo do movimento, devem estar
dentro dos seus limites. Sendo que θk,min e θk,max, são os limites mínimo e máximo das juntas do braço-mão direito e esquerdo:
Tk(t; θf, θb) ∈ [θk,min, θk,max] , (4.16)
onde k = 1, . . . ,nj2 e k = 12, . . . , 11 + nj2 com t ∈ [0, T ].
Restrição 2: Os valores das juntas dos braços e mãos no instante tb devem estar dentro dos limites:
θk,b∈ [θk,min, θk,max] , (4.17)
com k = 1, . . . ,nj2 e k = 12, . . . , 11 +nj2.
Restrição 3: Os braços e mãos não podem colidir com o objeto-alvo do outro braço.
Para tal, ao longo do movimento é verificado se existe colisão entre algum dos pontos dos braços-mãos e os objetos em causa:
kPR,i(t) − Otar,Lk2 > εtar,R(t) (4.18)
kPL,i(t) − Otar,Rk2 > εtar,L(t) , (4.19)
onde PR,i(t) e PL,i(t) são respetivamente os pontos, i, do braço-mão direito e esquerdo para uma determinada postura T (t; θf, θb), com t ∈ [0, T ]. Otar,L
e Otar,R representam as posições do alvo esquerdo e do alvo direito. εtar,R(t) e εtar,L(t) são funções no tempo, estabelecidas para representar a distância
mínima entre os pontos dos braços-mãos e o objeto-alvo do braço oposto.
Restrição 4: Os braços e mãos não podem colidir com os obstáculos nem com o
corpo do robô. Por isso, ao longo do movimento é verificado se existe colisão verificando a intersecção dos pontos dos braços e mãos, dada uma postura T (t; θf, θb), com os obstáculos e o corpo do robô:
kPR,i(t) − Ojk2 > εobs,R(t) (4.20)
onde Oj representa as posições dos obstáculos, j. εobs,R(t) e εobs,L(t) são
funções no tempo, estabelecidas para representar a distância de segurança entre o braço e o obstáculo ao longo do movimento.
Restrição 5: Para evitar colisões entre os dois braços e mãos ao longo do movimento,
verifica-se a colisão a partir da interseção dos pontos de cada braço, assim: kPL,i(t) − PR,i(t)k2 > ψ (t) , (4.22)
em que ψ (t) é uma função no tempo que representa uma distância de segurança entre os diferentes pontos dos braços.
Este problema de otimização permitirá encontrar a postura de bounce mais adequada para a trajetória do braço direito e esquerdo. A este problema de otimização não-linear com restrições dá-se o nome de problema Pb.