Planejamento de Trajetoria

Renato Purita Paes Leme

Coordenadas e parametros do manipulador

Considere um manipulador robotico com seis graus de liberdade, como o
manipulador de Stanford. Vimos dada uma pose [x, y, z, , , ]> , podemos
determinar os parametros (1 , 2 , d3 , 4 , 5 , 6 ) que produzem essa pose resol-
vendo o sistema de equacoes nao lineares:

T (1 , 2 , d3 , 4 , 5 , 6 ) = P

onde P eh a matriz 4 4 que da a pose do manipulador. Usando as co-

ordenadas [x, y, z, , , ]> conseguimos produzir os vetores normal, open e
approach : n,

o ea , que formam uma matriz ortogonal R entao, definimos
P = [ 0 1 ], onde d = [x, y, z]> e T eh o produto de matrizes:
R d

T = A1 (1 )A2 (2 )A3 (d3 )A4 (4 )A5 (5 )A6 (6 )

e:
c1 0 s1 0 c2 0 s2 0
s1 0 c1 0 s2 0 c2 0
A1 (1 ) =
1 A2 (2 ) =

0 0 l0 1 0 0 l1
0 0 0 1 0 0 0 1

1 0 0 0 c4 0 s4 0
0 0 1 0 s1 0 c1 d0
A3 (3 ) = 1
A4 (4 ) =
0 0 d3 1 0 0 0
0 0 0 1 0 0 0 1

c5 0 s5 0 c6 s6 0 0
s5 0 c5 0 s6 c6 0 0
A5 (5 ) =
1 A6 (6 ) =
0 0 0 1 0 0 l2
0 0 0 1 0 0 0 1
Queremos determinar = (1 , 2 , d3 , 4 , 5 , 6 ). Note que a igualdade
T () = P eh uma igualdade de matrizes 4 4 da forma [ R0 d1 ] onde R eh uma
matriz 3 3 ortogonal, e d eh um vetor de 3 componentes. Da igualdade do

1
vetor d das duas matrizes tiramos 3 equacoes. Note que a matriz ortogonal
R na verdade soh traz informacao sobre as coordenadas [, , ], logo eh
transformar a igualdade de R em mais tres equacoes (eh possivel provar
que o conjunto das matrizes ortogonais eh uma variedade de dimensao 3 no
R9 ). Assim temos 6 equacoes e seis incognitas, o que nos permite aplicar o
mehtodo de Newton. Uma forma mais direta de ver esse problema, eh buscar
escrever:

x = x(1 , 2 , d3 , 4 , 5 , 6 )
y = y(1 , 2 , d3 , 4 , 5 , 6 )
...
= (1 , 2 , d3 , 4 , 5 , 6 )
h x() i
Tomar F () = , tomar uma aproximacao 0 inicial dos parametros
()
do manipulador, e calcular:

i = JF (i )1 F
i+1 = i + i
(F eh a pose do manipulador que desejamos) ateh que o valor de i se esta-
bilize.

Planejamento de trajetoria
Vimos que a partir de uma pose, podemos calcular as coordenadas do ma-
nipulador que produzem aquela pose. Nosso problema agora eh, dadas
duas poses distintas do manipulador P1 = [x1 , y1 , z1 , 1 , 1 , 1 ]> e P2 =
[x2 , y2 , z2 , 2 , 2 , 2 ]> achar um movimento do manipulador que leve de P1
ateh P2 . Isso pode ser especificado de duas formas: a mais intuitiva eh
atravehs de uma caminho: P : [0, t] R6 tal que P (0) = P1 e P (t) = P2 .
Essa abordagem tem a vantagem de ser facil de ser vizualizada, mas tem a
desvantagem de precisarmos para cada instante t de tempo converter de co-
ordenadas x, y, z, , , para parametros do manipulador 1 , 2 , d3 , 4 , 5 , 6 ,
o que ja vimos ser um problema computacionalmente custoso, que para de-
terminados casos pode nem ao menos ter solucao.
Uma solucao menor intuitiva eh considerar uma funcao do tempo nos
parametros do manipulador : [0, t] R6 . Assim, devemos ter que a
configuracao (0) = (1 (0), 2 (0), d3 (0), 4 (0), 5 (0), 6 (0)) produza P1 e a

2
configuracao (t) produza P2 . Dados P1 e P2 , podemos obter (0) e (t),
so precisamos definir os valores intermediarios de .

Esse problema fica bastante complicado quando existem obstaculos no

ambiente em que o robo esta se movimentando - e mais complicado ainda
quando esses obstaculos devem ser identificados em tempo real. Consider-
aremos nesse problema que o ambiente eh conhecido e livre de obstaculos.
Mesmo assim ainda existem restricoes sobre o movimento do manipulador
suponha que estamos resolvendo esse problema para pegar um objeto em
uma posicao e leva-la a outra onde esse objeto ira ser deixado, entao:

quando o objeto for pego a velocidade do pulso do manipulador nao

pode ser alta, pois quanto mais alta for a velocidade, maior eh a flu-
tuacao nesta, logo existe a possibilidade de esmagar esse objeto contra
a superficie da qual o estamos tirando. Assim, eh interessante que
inicialmente o movimento seja bastante suave.

eh interessante que o movimento inicial do objeto se de na direcao

normal a superficie, para que nao corra o risco de ficar preso ou ter seu
movimento dificultado pelo seu atrito com esta

o mesmo vale para o momento em que o objeto eh deixado na superficie

Traduzindo matematicamente os requisitos acima, pedimos que a veloci-

dade e aceleracao de em 0 e em t sejam nulas, ou, ao menos, bastante
pequenas. Se forem nulas, teremos:

d2 d
(0) = (0) = 0
dt2 dt
d2 d
(t) = (t) = 0
dt2 dt

3
 Existem uma sehrie de esquemas classicos de planejamento de trajetoria.
Na maioria utiliza-se interpolacao de polinomios, que sao funcoes bastantes
suaves (de classe C ). Os mais usados sao o mehtodo 3 - 4 - 3, em a
interpolacao eh feita em tres trechos: o primeiro e o ultimo sao equacoes do
terceiro grau e o central eh uma equacao do quarto grau, o mehtodo 3 - 5 - 3,
semelhante ao 3 - 4 - 3, mas com o trecho central definido por uma equacao
do quinto grau. Apresentaremos aqui a interpolacao pelo mehtodo das cinco
cubicas.

Mehtodo das cinco cubicas

Uma maneira de definir uma funcao vetorial : I R R6 eh definir as
suas funcoes componentes:

i : I R i = 1, 2, 4, 5, 6

e:
d3 : I R
Faremos entao uma interpolacao componente a componente. Assim, seja
h : I R uma das componentes que queremos interpolar. Temos os
valores de h no inicio e no fim do intervalo I e temos valores desejados para
2
as derivadas dh
dt
e ddt2h no inicio e no fim do intervalo (em geral zero, mas nao
vamos fixar isso para nao perder a generalidade no nosso raciocinio).
No mehtodo das cinco cubicas, vamos dividir o intervalo de tempo em
que estamos trabalhando em cinco partes [i1 , i ], e considere dentro de
cada subintervalo uma normalizacao:
i1
t=
i i1

Seja ainda i = i i1 . Assim, vamos definir hi : [0, 1] R como o

trecho de h dentro de cada subintervalo [i1 , i ] com a variavel normalizada.
Dessa forma:
 
i1
h( ) = hi se [i1 , i ]
i i1
E vamos definir hi como um polinomio cubico:

hi (t) = ai3 t3 + ai2 t2 + ai1 t + ai0

Velocidade e aceleracao no tempo parametrizado t [0, 1] sao dados por:

4
 1 dhi 1
vi (t) = (t) = (3ai3 t2 + 2ai2 t + ai1 )
i dt i
1 d2 hi 1
ai (t) = 2
(t) = (6ai3 t + 2ai2 )
i dt2 i2

Temos as seguinte condicoes de contorno:

Instante Condicao Comentario

0 h = 0
0 v = v0
0 a = a0
1 h(1 ) = h(0 ) + 1
n h(n ) = h(n1 ) + n Deslocamento no ultimo trecho (set-down)
i vi (1) = vi1 (0)
i ai (1) = ai1 (0)
n h = f
n v = vf
n a = af
Posicao inicial do movimento
Velocidade inicial do movimento
Aceleracao inicial do movimento
Deslocamento no primeiro trecho (lift-on)
Continuidade da velocidade
Continuidade da aceleracao
Posicao final do movimento
Velocidade final do movimento
Aceleracao final do movimento

Comecemos com a analise do primeiro e do ultimo trecho, que sao aqueles

trechos para os quais temos mais condicoes de contorno disponiveis. Apli-
cando as tres primeiras condicoes de contorno ao primeiro trecho, chegamos
que:

a0 12 2
h1 (t) = a13 t3 + t + v0 1 t + 0
2

5
 Usando a proxima condicao de contorno, temos que h1 (1) = h1 (0) + 1 , o
que nos permite determinar a13 . Com isso passamos a ter:

a0 12
a13 = 1 v0 1
2
Aplicando agora as condicoes de contorno para o ultimo segmento:

hn (0) = an0 = f n = 4
hn (1) = an3 + an2 + an1 + 4 = f
3an3 + 2an2 + an1
vn (1) = vf =
n
6an3 + 2an2
an (1) = af =
n2
Resolvendo o sistema acima para an3 , an2 , an1 , an0 , obtemos:

af n2 3
 
hn (t) = n vf n + t + (3 + 3vf n af n2 )t2 +
2
af n2
 
+ 3n 2vf n + t + 4
2

Tendo estabelecido os parametros no inicio e no fim do movimento, vamos

analisar os trechos intermediarios, aplicando as condicoes de contorno, que
sao, basicamente, a continuidade na velocidade e na aceleracao e o intervalo
de tempo que dura cada segmento(i ). Como se trata de um movimento
fisico, naturalmente existe continuidade na posicao, ou seja:

hi (1) = hi+1 (0)

No entanto, o custo de controle quando assumimos a continuidade na

posicao eh maior. Essa analise que estamos fazendo eh feita em tempo real -
embora esperemos que o trecho i produza determinadas posicao, velocidade
e aceleracao esperadas, eh natural que ocorram erros - eh mais barato mon-
itorar somente velocidade e aceleracao e corrigir os parametros levando em
conta somente o valor destas. Dito isso, passemos analise do trecho 2:

h2 (t) = a23 t3 + a22 t2 + a21 t + a20

Aplicando as condicoes de contorno:

h2 (0) = 1 = 0 + 1

6
 1 dh2 1 dh1
v2 (0) = v1 (1) = (0) = (1)
2 dt 1 dt
1 d2 h2 1 d2 h1
a2 (0) = a1 (1) = (0) = (1)
22 dt2 12 dt2
Aplicando essas condicoes, determinamos parametros a20 , a21 e a22 , ob-
tendo assim:

a1 22 2
 
3
h2 (t) = a23 t + t + (v1 2 )t + 1
2
onde
31 a0 1 61 6v0
v1 = 2v0 a1 = 2
2a0
1 2 1 1
Vejamos a configuracao dos parametros do manipulador no fim do trecho
2
2, ou seja, o comportamento de h2 , dh
dt
2
, ddth22 em t = 1. Temos:

a1 22
h2 (1) = 2 = a23 + + v1 2 + 1
2
3a23
v2 (1) = v1 + a1 2 +
2
6a23
a2 (1) = a1 +
22
A mesma analise eh feita para o trecho 3, aplicando-se condicoes de con-
torno semelhantes:

h3 (t) = a33 t3 + a32 t2 + a31 t + a30

Aplicando as condicoes de contorno:

a2 22
h3 (0) = 2 = a23 + + v1 2 + 1
2
1 dh3 1 dh2
v3 (0) = v2 (1) = (0) = (1)
3 dt 2 dt
1 d2 h3 1 d2 h2
a3 (0) = a2 (1) = (0) = (1)
32 dt2 22 dt2
Aplicando essas condicoes, determinamos parametros a30 , a31 e a32 , ob-
tendo assim:

a2 32 2
 
3
h2 (t) = a33 t + t + (v2 3 )t + 2
2

7
onde
31 a0 1 61 6v0
v1 = 2v0 a1 = 2
2a0
1 2 1 1
Vejamos a configuracao dos parametros do manipulador no fim do trecho
2
3, ou seja, o comportamento de h3 , dh
dt
3
, ddth23 em t = 1. Temos:

a2 32
h3 (1) = 3 = a33 + + v2 3 + 2
2
3a33
v3 (1) = v2 + a2 3 +
3
6a33
a3 (1) = a2 +
32
Fazendo exatamente a mesma analise pra o trecho 4 chegamos a conclusao
que:

a3 42 2
 
3
h4 (t) = a43 t + t + (v3 4 )t + 3
2
Ficam ainda indeterminados 3 coeficientes: a23 , a33 e a43 . Isso pode ser
feito comparando-se h4 e suas duas primeiras derivadas em t = 1 com hn e
suas duas primeiras derivadas em t = 0. Fazemos:

a3 42
h4 (1) = a43 + + v3 4 + 3 = 4 = hn (0)
2
3a43 3n af n
v4 (1) = v3 + a3 4 + = 2vf + = vn (0)
4 n 2
6a43 6n 6vf
a4 (1) = a3 + 2
= 2 + + 2af = an (0)
4 n n
Resolvendo esse sistema de 3 equacoes e 3 incognitas, podemos calcular
a23 , a33 e a43 passando a ter todos os valores necessarios. A partir de entao eh
so variar os parametros (1 , 2 , d3 , 4 , 5 , 6 ) do manipulador de acordo com a
seguinte interpolacao.
Para cada parametro, podemos medir em tempo real a sua primeira e
segunda derivada temporal. Assim, nos instantes i temos a nossa disposicao
2
os valores de dh dt
e ddt2h . Esses valores podem ser previstos a partir dos dados
de entrada, no entanto podem ocorrer erros durante o trajeto que produzam
valores diferentes do esperado. Como escrevemos os parametros ai0 , ai1 , ...,
ai3 em funcao da velocidade e aceleracao em i1 eh mais interessante usar os
valores atualizados obtidos dos sensores do que os valores previstos. Assim,
erros na trajetoria podem ser corrigidos em tempo real.

8
Bibliografia
[1] FU, GONZALEZ, LEE, Robotics - Control, Sensing, Vision
and Intelligence, Mc Graw Hill
[2] Notas de Aula do Professor Paulo Rosa