Modelo JACO trajetória obstáculos ROS

Universidade Federal da Bahia - UFBA
Escola Politécnica da UFBA - EPUFBA

Departamento de Engenharia Elétrica
Trabalho Final de Graduação
Modelagem e análise do manipulador robótico JACO

para planejamento de trajetória com desvio de
obstáculos por campos potenciais artificiais
utilizando o ROS
Ubiratan de Melo Pinto Júnior
Salvador - Bahia
2016

para planejamento de trajetória com desvio de obstáculos
por campos potenciais artificiais utilizando o ROS
Trabalho Final de Graduação apresentado

como requisito parcial para a graduação no
curso de Engenharia Elétrica da UFBA
Universidade Federal da Bahia - UFBA

Escola Politécnica da UFBA
Graduação
Orientador: Prof. Dr. André Gustavo Scolari Conceição
Brasil
2016
fichacatalografica

para planejamento de trajetória com desvio de obstácu-
los por campos potenciais artificiais utilizando o ROS/
Ubiratan de Melo Pinto Júnior. – Brasil, 2016-
51 p. : il. (algumas color.) ; 30 cm.
Orientador: Prof. Dr. André Gustavo Scolari Conceição
TFG (Graduação) – Universidade Federal da Bahia - UFBA

Escola Politécnica da UFBA
Graduação, 2016.
1. Campos Potenciais Artificiais 2. JACO. 3. Denavit-
Hartenberg. 4. Parâmetros D-H. 5. Cinemática Inversa. 6.
Cinemática Direta 7. ROS 8. Python I. Prof. Dr. André
Gustavo Scolari Conceição. II. Universidade Federal da
Bahia. III. Escola Politécnica da UFBA. IV. TFG
CDU
02:141:005.7

para planejamento de trajetória com desvio de obstáculos
por campos potenciais artificiais utilizando o ROS
Trabalho Final de Graduação apresentado

como requisito parcial para a graduação no
curso de Engenharia Elétrica da UFBA
Prof. Dr. Cristiane Corrêa Paim

Coordenador do curso de engenharia elétrica
Trabalho aprovado. Brasil, 24 de novembro de 2017:
Prof. Dr. André Gustavo Scolari

Conceição
Orientador
Prof. Dr. Augusto Loureiro da Costa

Convidado 1
Prof. Dr. Tiago Trindade Ribeiro

Convidado 2
Brasil
2016
À minha família e amigos
Agradecimentos
– Gato, pode dizer-me que caminho devo tomar?
– Isso depende do lugar para onde você quer ir. (Res-
pondeu com muito propósito, o gato)
– Não tenho destino certo.
– Neste caso qualquer caminho serve.
Alice no País das Maravilhas - Lewis Carroll
Resumo
O planejamento de trajetórias com desvio de obstáculos pelo método dos campos potenciais
artificiais é amplamente utilizado em aplicações robóticos. Isso por conta de sua robustez
e conceito intuitivo. Este trabalho tem como objetivo a utilização deste método para o
manipulador robótico JACO através do ROS e, dessa forma, construir uma base de partida
para melhorias e trabalhos futuros utilizando esse sistema.
Palavras-chaves: Campos Potenciais Artificiais. JACO. Denavit-Hartenberg. Robótica.

ROS. Python
Abstract
The path planning with obstacles avoidance by the method of artificial potential fields is
widely used in robotic applications. This is due to its robustness and intuitive concept.
This work aims to use this method for the JACO robotic manipulator through the ROS
and, in this way, to construct a starting point for improvements and future works using
this system.
Key-words: Artificial Potential Field. JACO. Denavit-Hartenberg. Robotics. ROS. Python.

Lista de ilustrações
Figura 1 –Exemplos de robôs . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16

Figura 2 –Manipulador JACO adaptado a uma cadeira de rodas . . . . . . . . . . 17
Figura 3 –Funções do ROS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18
Figura 4 –Rotação de θ graus do sistema o1 x1 y1 z1 em relação ao eixo z0 . . . . . 20
Figura 5 –Exemplo de sistemas de coordenadas rigidamente fixados a um manipu-
lador robótico . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23
Figura 6 – Sistemas de coordenadas que satisfazem às considerações 1 e 2 . . . . . 26
Figura 7 – Trajetória I no espaço cartesiano . . . . . . . . . . . . . . . . . . . . . 31
Figura 8 – Módulo das forças atrativa (Fatt ) e repulsiva (Frep ) para a trajetória I . 31
Figura 9 – Trajetória I no espaço de juntas . . . . . . . . . . . . . . . . . . . . . . 32
Figura 10 – Trajetória II no espaço cartesiano . . . . . . . . . . . . . . . . . . . . . 33
Figura 11 – Trajetória II no espaço de juntas . . . . . . . . . . . . . . . . . . . . . 33
Figura 12 – Trajetória III no espaço cartesiano . . . . . . . . . . . . . . . . . . . . 34
Figura 13 – Módulo das forças atrativa (Fatt ) e repulsiva (Frep ) para a trajetória III 34
Figura 14 – Trajetória III no espaço de juntas . . . . . . . . . . . . . . . . . . . . . 35
Figura 15 – Trajetória IV no espaço cartesiano . . . . . . . . . . . . . . . . . . . . 36
Figura 16 – Trajetória IV no espaço de juntas . . . . . . . . . . . . . . . . . . . . . 36
Figura 17 – Módulo das forças atrativa (Fatt ) e repulsiva (Frep ) para a trajetória IV 37
Figura 18 – Trajetória V no espaço cartesiano . . . . . . . . . . . . . . . . . . . . . 38
Figura 19 – Trajetória V no espaço de juntas . . . . . . . . . . . . . . . . . . . . . 38
Figura 20 – Módulo das forças atrativa (Fatt ) e repulsiva (Frep ) para a trajetória V 39
Figura 21 – Coordenadas do efetuador, em metros, para as trajetórias III e V
(mesmos eixos, mesmas cores) . . . . . . . . . . . . . . . . . . . . . . . 40
Figura 22 – Trajetórias da junta 1, em graus, para as trajetórias III (azul) e V
(vermelho) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 40
Figura 23 – Trajetórias da junta 2, em graus, para as trajetórias III (roxo) e V (verde) 41
Figura 24 – Trajetórias da junta 3, em graus, para as trajetórias III (verde) e V
(mostarda) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41
Figura 25 – Trajetórias da junta 4, em graus, para as trajetórias III (mostarda) e V
(preto) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 42
Figura 26 – Trajetórias da junta 5, em graus, para as trajetórias III (preto) e V
(verde) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 42
Figura 27 – Trajetórias da junta 6, em graus, para as trajetórias III (verde) e V
(marrom) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43
Lista de tabelas
Tabela 1 – Parâmetros DH do manipulador JACO . . . . . . . . . . . . . . . . . . 26

Lista de abreviaturas e siglas
RIA Robot Institute of America
PRM Pessoa com mobilidade reduzida
ROS Robot Operatins System
DH Denavit-Hartenberg
Lista de símbolos
oi xi yi zi Sistemas de coordenadas i
pi Coordenadas do ponto p em relação ao sistema de coordenadas i
oi Origem do sistema de coordenadas i
xi Versor na direção x do sistema de coordenadas i
yi Versor na direção y do sistema de coordenadas i
zi Versor na direção z do sistema de coordenadas i
oij Coordenadas da origem do sistema de coordenadas i em relação ao

sistemas de coordenadas j
R Matriz de rotação resultante
Rji Matriz de rotação do sistema de coordenadas j em relação ao sistema

de coordenadas i
Rotx,α Rotação de α graus de um sistema de coordenadas em relação ao seu

eixo x
Roty,γ Rotação de γ graus de um sistema de coordenadas em relação ao seu

eixo y
Rotz,θ Rotação de θ graus de um sistema de coordenadas em relação ao seu

eixo z
sα Seno do ângulo α
cα Cosseno do ângulo α
T Matriz de translação resultante
T ransx,dx Translação de uma distância dx de um sistema de coordenadas em

relação ao seu eixo x
T ransy,dy Translação de uma distância dy de um sistema de coordenadas em

relação ao seu eixo y
T ransz,dz Translação de uma distância dz de um sistema de coordenadas em
relação ao seu eixo z
H Tranformação homogênea entre sistemas de coordenadas
qi Variável de junta da iésima junta de um manipulador robótico
Ai Transformação homogênea que relaciona os sistemas de coordenadas i e

i − 1 de um manipulador robótico
Ai (qi ) Transformação homogênea que relaciona os sistemas de coordenadas i e

i − 1 de um manipulador robótico calculada para os valores de variáveis
de juntas qi
U (q) Campo potencial resultante para o espaço de juntas q
Uatt (q) Campo potencial atrativo para o espaço de juntas q
Urep (q) Campo potencial repulsivo para o espaço de juntas q
F (q) Força resultante para o espaço de juntas q
Fatt (q) Força atrativa para o espaço de juntas q
Frep (q) Força repulsiva para o espaço de juntas q
qinit Coordenadas do ponto inicial da trajetória do manipulador
qf inal Coordenadas do ponto final da trajetória do manipulador
qobstaculo Coordenadas do ponto obstáculo da trajetória do manipulador
qoi Coordenadas do iésimo ponto obstáculo da trajetória do manipulador

Sumário
1 INTRODUÇÃO . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16
2 OBJETIVO . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18
2.1 Objetivos específicos . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18
3 DESENVOLVIMENTO . . . . . . . . . . . . . . . . . . . . . . . . . 19
3.1 Movimentos Rígidos . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19
3.1.1 Matrizes de Rotação Tridimensionais . . . . . . . . . . . . . . . . . . . . . 19
3.1.2 Matrizes de Translação Tridimensionais . . . . . . . . . . . . . . . . . . . 21
3.2 Transformações Homogêneas . . . . . . . . . . . . . . . . . . . . . . . 21
3.3 Cinemática Direta . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22
3.3.1 Convenção Denavit-Hartenberg . . . . . . . . . . . . . . . . . . . . . . . . 24
3.4 Cinemática Inversa . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25
3.5 Planejamento de Trajetória . . . . . . . . . . . . . . . . . . . . . . . . 27
3.5.1 Campos Potenciais Artificiais . . . . . . . . . . . . . . . . . . . . . . . . . 27
4 RESULTADOS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30
4.1 Planejamento de Trajetória sem Obstáculos . . . . . . . . . . . . . . 31
4.2 Planejamento de Trajetória com Obstáculos . . . . . . . . . . . . . . 35
4.3 Gráficos Comparativos . . . . . . . . . . . . . . . . . . . . . . . . . . . 39
5 CONCLUSÃO . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44
REFERÊNCIAS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45
ANEXOS 46
ANEXO A – CÓDIGO PYTHON . . . . . . . . . . . . . . . . . . . 47

16
1 Introdução
A robótica é um campo relativamente novo na engenharia. Seu estudo envolve

múltiplas áreas como eletrônica, elétrica, mecânica, matemática, economia, dentre outras.
Atualmente a maior aplicação da robótica é no ambiente industrial através de braços
manipuladores, SPONG, Mark et al. (SPONG; HUTCHINSON; VIDYASAGAR, 2006). A
definição oficial do termo robô, segundo a Robot Institute of America (RIA), é: "Um robô
é um manipulador multifuncional reprogramável projetado para mover materiais, partes,
ferramentas ou dispositivos especializados através de movimentos programados variáveis
para a performance de uma variedade de tarefas". Assim, existem também outras áreas
de aplicação como a utilização de robôs domésticos, como aspiradores de pó autônomos
(como o Housekeeper Pro, da Polishop, figura 1a) ou até mesmo humanoides que interagem
com as pessoas (como o ASIMO, da Honda, figura 1b), e a robótica assistiva, em que a
aplicação de robôs visa o auxílio a pessoas com mobilidade reduzida (PMR) a terem maior
independência nas atividades do cotidiano.
(a) Robô Aspirador Housekeeper Pro Polishop (b) Evolução dos robôs humanoides da Honda
(Polimport - Comércio e Exportação Ltda, - mais à frente, o ASIMO (Honda Motor
2017) Company, Limited, 2007)
Figura 1 – Exemplos de robôs
Esta última aplicação, tem como exemplo o manipulador robótico JACO, figura
2 (Kinova Robotics, 2017), desenvolvido e comercializado pela Kinova Robotics. Este
manipulador foi utilizado no desenvolvimentos dos estudos descritos neste documento.
O manipulador JACO possui seis juntas, três juntas rotativas para o braço (RRR
ou articulado) e três juntas rotativas para o pulso não esférico. Sendo um manipulador
antropomórfico. Seu elemento final, ou ferramenta, é uma garra com três dedos com
uma atuador para cada dedo. Suas especificações estão disponíveis no endereço <http:
//www.gaitech.net/userfiles/file/JACO-2%20Specification%20Sheet%201.0.1.pdf>
Capítulo 1. Introdução 17
Figura 2 – Manipulador JACO adaptado a uma cadeira de rodas

(Kinova Robotics, 2017)
Para o controle de um manipulador robótico, é necessário o desenvolvimento de um

modelo matemático para ele. Isso é feito através da representação de movimentos relativos
entre sistemas de coordenadas convenientemente escolhidos de forma a representarem os
movimentos de um manipulador em relação a um sistema de coordenadas de referência, ou
sistema inercial. Para tanto, a noção de movimentos rígidos e transformações homogêneas é
essencial para a compreensão e desenvolvimento de modelos deste tipo. No desenvolvimento
deste texto são tratados conceitos importantes sobre esses tópicos.
18
2 Objetivo
Este trabalho tem como foco a modelagem e análise do braço robótico articulado
JACO para aplicação em planejamento de trajetória através dos campos potenciais arti-
ficiais. Este método permite, a partir de uma pose (posição e ângulos determinados do
manipulador) inicial, chegar a uma pose final predefinida planejando uma trajetória de
forma que sejam evitadas colisões com obstáculos de localização conhecida.
A aplicação deste método será feita através do sistema operacional Robot Operating
System (ROS). O ROS é um espaço de trabalho flexível para escrever softwares para robôs.
Ele é uma coleção de ferramentas, bibliotecas, e convenções que visam simplificar a tarefa
de criação de comportamento robótico complexo e robusto dentre uma ampla variedade de
plataformas robóticas (ROS.ORG, 2017). Como resultado, ROS foi construído do zero para
encorajar desenvolvimento de softwares para robótica de forma colaborativa (ROS.ORG,
2017).
Figura 3 – Funções do ROS

(ROS.ORG, 2017)
2.1 Objetivos específicos
• Modelagem do manipulador através da cinemática direta e inversa.
• Desenvolvimento do método para o planejamento de trajetória por campos potenciais

artificiais.
• Desenvolvimento do código para planejamento de trajetória por campos potenciais

artificiais artificiais para o ambiente ROS.
• Comprovação do método através de experimentos no ambiente sem obstáculos e com

obstáculos.
19
3 Desenvolvimento
O planejamento de trajetória por campos potenciais gravitacionais utilizado neste

documento é baseado no método descrito por SPONG, Mark et al. (SPONG; HUTCHIN-
SON; VIDYASAGAR, 2006). Para este método é necessário desenvolver os modelos para
o manipulador JACO de cinemática direta e inversa, e também o jacobiano para esse
modelo. A seguir, são discutidas essas deduções.
3.1 Movimentos Rígidos

Seja um sistema de coordenadas o0 x0 y0 z0 fixo no espaço. Um ponto p pode ser
representado neste espaço em relação ao sistema o0 x0 y0 z0 através de suas coordenadas nas
direções x, y e z deste eixo. Essa representação é denotada por p0 .
 
u
 
p = ux0 + vy0 + wz0 =
0
v 
 
 
z
Da mesma forma, o ponto p pode ser representado no espaço em relação a outro

sistema o1 x1 y1 z1 pelo produto escalar de cada uma de suas coordenadas pelos versores x1 ,
y1 e z1 do sistema. Isto é:
 

(ux0 + vy0 + wz0 ).x1 
p = ux0 .x1 + vx0 .x1 + wx0 .x1 =  (ux0 + vy0 + wz0 ).y1 
1 

 
(ux0 + vy0 + wz0 ).z1
De forma similar, a posição de um sistema oj xj yj zj pode ser representada em

relação a outro sistema oi xi yi zi . Para isto basta saber quais as coordenadas da origem de
oj xj yj zj , oj , em relação a oi xi yi zi , isto é, oij . E a relação de ângulos entre os versores do
sistema j em relação aos do sistema i. Logo, para se representar a posição de um sistema
qualquer, j, em relação a outro sistema, i, de referência basta determinar a relação de
deslocamento, ou translação, e giro, ou rotação, entre eles.
3.1.1 Matrizes de Rotação Tridimensionais

A rotação de um sistema o1 x1 y1 z1 em relação a um sistema o1 x1 y1 z1 pode ser
representada por uma matriz, R10 . Ela representa a projeção dos versores do sistema 1 nos
versores do sistema 0.
Capítulo 3. Desenvolvimento 20
 
x .x y1 .x0 z1 .x0 
 1 0
R1 =  x1 .y0 y1 .y0 z1 .y0 
0 

 
x1 .z0 y1 .z0 z1 .z0
Figura 4 – Rotação de θ graus do sistema o1 x1 y1 z1 em relação ao eixo z0

(SPONG; HUTCHINSON; VIDYASAGAR, 2006)
Se, por exemplo, o sistema o1 x1 y1 z1 for rotacionado de θ graus em relação ao eixo

z0 , como na figura 4, a matriz R10 , ou Rotz,θ , será dada por:
 

cosθ −senθ 0
R10 = Rotz,θ = senθ cosθ 0
 
 
0 0 1
Por conveniência, daqui por diante será adotada a seguinte notação abreviada:
cosθ = cθ e senθ = sθ .
De forma similar, é possível determinar as matrizes de rotação em relação aos eixos
x0 , Rotx,α , e y0 , Roty,γ .
 

1 0 0 
Rotx,α = 0 cα −sα 
 
 
0 s α cα
 
c
 γ
0 sγ 
Roty,γ =  0 1 0
 
 
−sγ 0 cγ
É possível ainda ser feita a composição de matrizes do rotação. Se por exemplo,

for feita a rotação de θ graus em relação ao eixo z0 e em seguida a rotação de α graus
em relação ao eixo x1 , do sistema de coordenadas 1, ou corrente, a matriz de rotação
resultante, R, será dada por:
R = Rotz0 ,θ Rotx1 ,α
Isto é, multiplicando-se a matriz de rotação seguinte à direita da primeira matriz

de rotação. Se forem feitas, porém, rotações sucessivas em relação ao sistema 0, ou fixo,
por exemplo, uma rotação de θ graus em relação ao eixo z0 seguida de outra rotação de γ
graus em relação ao eixo y0 , então deve fazer a multiplicação da segunda matriz à direita
a primeira. A matriz R fica:
R = Roty0 ,γ Rotz0 ,θ
3.1.2 Matrizes de Translação Tridimensionais

Uma matriz de translação, T ou oij , representa as coordenadas da origem de um
sistema j em relação a um sistema i.
T = oij = T ransx,dx + T ransy,dy + T ransz,dz
A composição de translações é feita pela soma das matrizes de translação, assim:
     
d 0
 x    
0
T = oj =  0  + dy  +  0 
i      
     
0 0 dz
 
d
 x
oj = dy 
i  
 
dz
3.2 Transformações Homogêneas

Transformações homogêneas nada mais são do que formas de representação com-
pactas de movimentos rígidos em apenas uma matriz. Uma matriz de transformação, H,
homogênea tem a seguinte forma geral.
 
R3x3 T3x1 
H=
f1x3 s1x1
Sendo R uma matriz de rotação tridimensional, T uma matriz de translação

tridimensional, f a perspectiva e s o fator de escala. Para as aplicações em movimentos
rígidos f1x3 = [0 0 0] e s1x1 = [1]. Variações nessas matrizes são utilizadas principalmente
em aplicações de visão computacional ou simulações computacionais.
Dessa forma, a matriz H fica na da seguinte forma:
 
n s x ax d x 
 x
ny sy ay dy 
 
H=
 
nz sz az dz 

 
0 0 0 1
A composição de matrizes de transformação homogênea é dada pela composição

das partes de rotação e translação individualmente para que então seja montada a matriz
H. Isto é, multiplicando-se convenientemente as matrizes de rotação e somando-se as
matrizes de translação em relação ao mesmo sistema.
3.3 Cinemática Direta

Um manipulador robótico pode ser representado por um conjunto de elos, ou links,
conectados entre si através de juntas. Essas juntas permitem o movimento relativo entre
os elos correspondente à sua natureza. Juntas podem ser simples, como as de revolução ou
prismáticas, ou elas podem ser mais complexas como as do tipo esfera e soquete. Juntas de
revolução funcionam como dobradiças e permitem um movimento de rotação em relação a
determinado eixo. Juntas prismáticas permitem um deslocamento em relação a um eixo,
isto é, extensões ou retrações. Juntas do tipo esfera e soquete são como o ombro humano e
permitem a rotação em relação a dois eixos. A diferença principal entre este tipo de juntas
é que os duas primeiras são juntas com apenas um grau de liberdade, enquanto a última
possui dois graus de liberdade.
Em modelagem de sistemas robóticos é possível se considerar apenas juntas com
um grau de liberdade sem perda de generalidade, pois uma junta tipo esfera e soquete
pode ser modelada como duas juntas prismáticas unidas por um elo de comprimento zero.
Com isso, a atuação de cada junta pode ser representada por um número real, ângulo
de rotação para uma junta de revolução e valor de deslocamento linear para uma junta
prismática. Essa convenção será adotada daqui por diante.
Assim, para cada junta i será associada uma variável de junta qi , tal que:

 θi , se a junta i f or de revolução
qi =
 di , se a junta i f or prismática
Para que seja feita a análise cinemática de um manipulador, a cada elo i é fixado
rigidamente um sistema de coordenadas oi xi yi zi de forma que se a junta i é atuada, o
sistema oi xi yi zi sofre um movimento equivalente (Ex: se a junta i é prismática e causa
um deslocamento di , o sistema sofrerá uma translação de valor di no eixo de atuação da
junta).
Também é interessante fixar um sistema de coordenadas o0 x0 y0 z0 à base do ma-

nipulador. Este será o sistema inercial e é em relação a ele que serão expressos todos os
movimentos resultantes dos outros sistemas. A figura 5 mostra um exemplo de seleção de
sistemas fixados rigidamente a um manipulador robótico de forma conveniente para uma
análise cinemática.
Figura 5 – Exemplo de sistemas de coordenadas rigidamente fixados a um manipulador

robótico
O último sistema, o3 x3 y3 z3 , corresponde à posição da ferramenta do manipulador

(uma garra, ou um soldador por exemplo) e é a posição de maior interesse no manipulador,
pois é ela que deve ser controlada para que seja executada alguma atividade.
Cada sistema i se relaciona com o sistema anterior, i − 1, através de uma transfor-
mação homogênea, Ai , que representa a orientação do sistema i em relação ao sistema
i − 1. Logo, a matriz Ai é função da variável da junta i, qi , e tem a seguinte forma:
 
Ri−1 oi−1
Ai =  i i 
0 1
Em que Rii−1 e oi−1

i representam a orientação e a posição da origem do sistema
oi xi yi zi em relação ao sistema oi−1 xi−1 yi−1 zi−1 , respectivamente.
A transformação homogênea que relaciona a orientação e posição de um sistema
oj xj yj zj em relação a um sistema oi xi yi zi é chamada de matriz de transformação e é
representada da seguinte forma:





Ai+1 Ai+2 . . . Aj−1 Aj , se i < j
Tji = I , se i = j

 (T j )−1

, se i > j

i
Logo,
 
R i oi
Tji = Ai+1 Ai+2 . . . Aj−1 Aj =  j j 
0 1
Sendo que a matriz Rji é dada por:
Rji = Ri+1
i
. . . Rjj−1
E o vetor de coordenadas oij é dado por:
oij = oij−1 + Rj−1

i
oj−1
j
Dessa forma, a orientação e posição da ferramenta de um manipulador robótico de

n juntas em relação ao sistema inercial o0 x0 y0 z0 é encontrada pela seguinte relação:
 
R0 o0
H =  n n
0 1
H = Tn0 = A1 (q1 )A2 (q2 ) . . . An (qn )
Assim, para se determinar a posição e orientação da ferramenta de um manipulador

robótico, basta que sejam conhecidos os valores das variáveis de cada junta e suas matrizes
de transformação homogênea, Ai (qi ).
3.3.1 Convenção Denavit-Hartenberg

Com o intuito de simplificar e padronizar a análise de cinemática direta é comum
se utilizar a convenção de Denavit-Hartenberg, ou DH, como orientação para escolha de
sistemas de referência para cada elo de um manipulador. A análise cinemática de um
manipulador de n elos pode ser extremamente complexa e essa convenção simplifica a análise
consideravelmente, além de fornecer uma linguagem universal com a qual engenheiros da
área podem se comunicar, SPONG, Mark et al. (SPONG; HUTCHINSON; VIDYASAGAR,
2006).
A convenção DH avalia quatro parâmetros para cada elo de modo a determinar
sua matriz Ai . Esses parâmetros são o comprimento do elo, ai , a torção do elo, αi , o
deslocamento ou offset do elo, di , e o ângulo da junta, θi . Dessa forma, a matriz Ai é

expressa por uma rotação de θi no eixo z, seguida de uma translação de di no eixo corrente
z, seguida de uma translação de ai no eixo x corrente, seguida de uma rotação de αi no
eixo x corrente. Isto é:
Ai = Rotz,θi T ransz,di T ransx,ai Rotx,θi
    
c −sθi
 θi
0 0 1 0 0 0  1 0 0 ai  1 0 0 0
sθi cθ i 0 0 0 1 0 0 0 1 0 0 0 cαi −sαi 0
    
Ai =     
0 0 1 0 0 0 1 di  0 0 1 0 0 sαi cα i 0
    
    
0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 1
 
c −sθi cαi sθi sαi ai cθi 
 θi
sθi cθi cαi −cθi sαi ai sθi 
 
Ai = 
 
0 s c d

αi αi i 
 
0 0 0 1
Assim, a convenção DH representa uma transformação homogênea com apenas

quatro parâmetros enquanto uma transformação homogênea arbitrária é completamente
representada por seis parâmetros. Isso dá à representação por parâmetros DH dois graus
de liberdade. Então, para garantir a existência e unicidade de uma representação DH uma
vez escolhidos os sistemas de referência, devem ser levadas em conta duas considerações.
Considerando dois sistemas o0 x0 y0 z0 e o1 x1 y1 z1 , as considerações são as seguintes:
1. O eixo x1 é perpendicular ao eixo z0
2. O eixo x1 intercepta o eixo z0
Exemplos de sistemas que obedecem a essas propriedades são mostrados na figura

6.
A tabela 1 mostra os parâmetros DH do manipulador JACO de acordo com o
modelo do simulador V-REP (Coppelia Robotics, 2017).
3.4 Cinemática Inversa

O problema da cinemática inversa é o problema contrário ao da cinemática direta.
Seu objetivo é, a partir da posição cartesiana da ferramenta, ou elemento final, do
manipulador, encontrar os valores das variáveis de juntas que satisfaçam a essas condições.
Figura 6 – Sistemas de coordenadas que satisfazem às considerações 1 e 2

Tabela 1 – Parâmetros DH do manipulador JACO
Elo θi (limites) di ai αi
1 q1 0,2755 0,0000 90°
2.1 q2 (47° - 313°) 0,0000 0,0000 90°
2.2 0° 0,4100 0,0000 90°
3.1 q3 (19° - 341°) 0,0098 0,0000 90°
3.2 0° 0,2073 0,0000 0°
4.1 q4 0,0658 0,0000 -90°
4.2 0° 0,0342 0,0000 35°
5.1 q5 0,0000 0,0000 -35°
5.2 0° 0,0737 0,0000 90°
5.3 0° 0,0098 0,0000 -110°
(Coppelia Robotics, 2017)
Este, porém, é em geral um problema mais complexo que o da cinemática direta

como descrevem SPONG, Mark et al. (SPONG; HUTCHINSON; VIDYASAGAR, 2006),
pois nem sempre ele apresenta solução única, isto é, para uma determinada posição, mais de
um conjunto de valores de variáveis de juntas satisfazem a essa condição, e a complexidade
do problema aumento de acordo com o número de juntas de cada manipulador. Além
disso, tais soluções nem sempre podem ser encontradas de forma analítica, necessitando
de métodos numéricos e iterativos para sua resolução. Isso impossibilita um método de
descrição e solução única para o problema, o que requer que cada análise seja considerada
caso a caso.
No ambiente ROS existe o conjunto de pacotes Kinova-ROS (Schmidt, Jeff (Clear-
path) and Bencz, Alex (Clearpath) and DeDonato, Matt (WPI), 2017), baseado na API
Kinova, disponível na página <https://github.com/Kinovarobotics/kinova-ros>, que já
possui implementada a cinemática inversa e direta do manipulador JACO. Neste estudo
será utilizada este pacote para o controle do JACO.
3.5 Planejamento de Trajetória

Para que manipuladores robóticos executem tarefas determinadas de modo a
evitar colisões com obstáculos no ambiente de trabalho, existem pesquisas que visam o
desenvolvimento de algoritmos de planejamento de trajetórias livres de colisões. A maioria
desses métodos utilizam controle de baixo nível para que sejam evitadas as colisões e isso
exige tempo de ciclo de máquina muito superiores às capacidades dos manipuladores que
utilizamos, isso impossibilita aplicações precisas e em tempo real.
Existe contudo o método de planejamento de trajetórias por campos potenciais
artificiais (KHATIB, 1986), que possibilita o controle em alto nível aproveitando-se do
feedback do ambiente para aplicações em tempo real e interativas. Por estas características
e por possuir um conceito intuitivo de funcionamento este método é bastante utilizado
no planejamento de trajetória com desvio de obstáculos. Por isso, será o método aplicado
neste estudo.
O planejamento de trajetória por campos potenciais artificiais leva em conta a
disposição no espaço cartesiano dos obstáculos, do ponto objetivo e do efetuador para que
através dessas posições seja gerado um campo potencial que causará sobre o efetuador
forças de atração e repulsão cujo resultante definirá o caminho a ser seguido até a posição
final.
3.5.1 Campos Potenciais Artificiais

A filosofia deste método pode ser descrita da seguinte forma.
"O manipulador se move num campo de forças. A posição a ser alcançada é um
polo atrativo para o efetuador e os obstáculos são superfícies repulsivas para as partes do
manipulador" (KHATIB, 1986)
O campo de forças é composto pela sobreposição de um campo atrativo, Uatt (q), e
um campo repulsivo, Urep (q).
U (q) = Uatt (q) + Urep (q)
A partir disto, o planejamento de trajetória pode ser resolvido de várias maneiras.

Um algoritmo simples para isto é o planejamento a partir do gradiente mínimo que define
a força que age sobre a ferramenta como sendo.
F (q) = − 5 U (q) = − 5 Uatt (q) − 5Urep (q)

E é definido da seguinte forma.
1. qinit = q[0] , i = 0
2. SE |q[i] − qf inal | < δ

q[i + 1] = q[i] + α |FF (q[i])
(q[i])|
i=i+1
3. SENÃO retorne < q[0] , q[1] , ... , q[i] >
4. IR PARA 2
O valor q[i] é a posição do efetuador na iteração i. O valor α é o tamanho do passo

que será dado a cada iteração. Como na prática é bastante improvável que q[i] = qf inal
é necessário que se admita uma tolerância δ para a distância do efetuador ao ponto
final para a qual seja considerado que o objetivo foi alcançado. Os valores de retorno
< q[0] , q[1] , ... , q[i] > são a trajetória a ser seguida pelo manipulador.
O campo atrativo pode ser definido da seguinte forma.
1
Uatt (q) = ζρ2f (q)
2
Sendo ρ2f (q) a distância euclidiana entre a posição atual do efetuador, q, e a posição
objetivo, qf inal .
O campo possui característica quadrática, pois para distâncias muito pequenas a
força atrativa tende a zero com derivada contínua. Porém, para distâncias muito grandes,
essa força pode assumir valores excessivamente altos. Por isso é interessante que a partir de
determinada distância d o campo assuma um comportamento cônico e aumente linearmente
com a distância. Assim, uma forma mais conveniente é.

 1
ζρ2 (q)
 2 f

 , se ρf (q) ≤ d
Uatt (q) =

dζρf (q) − 21 ζd2

, se ρ(q) > d


A força atrativa a cada ponto terá o módulo, sentido e direção definidos pelo
gradiente mínimo, como a seguir.



 −ζ(q − qf inal ) , se ρf (q) ≤ d

Fatt (q) = − 5 Uatt (q) =

 dζ(q−qf inal )

 − ρf (q)
, se ρ(q) > d
Características interessantes para o campo repulsivo são: crescer até o infinito

quando à medida que o efetuador se aproxima do obstáculo e diminuir com o aumento
da distância, ρ(q), entre eles. Além disso, para o caso de existirem diversos obstáculos, é
interessante que cada campo repulsivo tenha uma distância de influência, ρ0 , a partir da
qual o campo se torna nulo e não influencie na trajetória do efetuador. Dessa maneira o
campo repulsivo pode ser definido da seguinte maneira.

 1
η( 1
 2 ρ(q)

 − 1 2
ρ0
) , se ρ(q) ≤ ρ0
Urep (q) =

0

, se ρ(q) > ρ0


A força repulsiva em cada ponto terá o módulo, sentido e direção definidos pelo
gradiente mínimo do campo como segue.





1
η( ρ(q) − 1
) 1
ρ0 ρ2 (q)
5 ρ(q) , se ρf (q) ≤ ρ0
Frep (q) = − 5 Urep (q) = 
0

, se ρ(q) > ρ0


A força resultante sobre a ferramenta a cada ponto é a resultante das duas forças.
F (q) = Fatt (q) + Frep (q)

30
4 Resultados
A aplicação do método dos campos potenciais artificiais para planejamento de

trajetória com desvio de obstáculos foi feita no manipulador JACO através do ROS
utilizando o pacote Kinova-ROS (Schmidt, Jeff (Clearpath) and Bencz, Alex (Clearpath)
and DeDonato, Matt (WPI), 2017). A partir da função de cinemática inversa desse pacote
pose_action_client.py foram feitas modificações para a definição dos pontos inicial, qinit ,
final, qf inal , e obstáculo, qobstaculo , através das coordenadas cartesianas e orientação; e,
através deles, ser calculada a trajetória, iteração a iteração, de qinit até qf inal de forma que
o efetuador não colida com qobstaculo . O código final, em python, encontra-se em anexo.
Como foi descrito nas seções anteriores, fatores importantes para a o algoritmo
de cálculo dos campos potenciais artificias são o tamanho do passo que será dado a
cada iteração, α, as constantes de intensidade dos campos atrativo e repulsivo, ζ e η,
respectivamente, a distância de influência do obstáculo, ρ0 , e a tolerância da distância do
efetuador para qf inal , δ. Por conta do espaço de trabalho da ordem de grandeza de dezenas
de centímetros, foram definidos experimentalmente ρ0 = 0, 5 m e δ = 0, 05 m.
De acordo com o algoritmo de planejamento de trajetória por mínimo gradiente
apresentado na seção anterior, a relação entre ζ e η determina qual força terá maior
influência na resultante sobre o efetuador. O campo atrativo tem característica de reduzir
quadraticamente à medida que se aproxima de qf inal . O campo repulsivo tem característica
de aumentar quadraticamente à medida que se aproxima de qobstaculo . Como qobstaculo está
tipicamente localizado entre qinit e qf inal a tendência é que se ζ e η forem muito próximos,
o caminho de gradiente mínimo não tenda a contornar qobstaculo , mas ao invés disso, se
afastar dele, uma vez que Frep será muito maior que Fatt . Isso faz com que a trajetória
planejada não leve efetivamente a ferramenta ao ponto objetivo. Por isso é interessante que
ζ seja muito maior que η de forma que a força do campo atrativo nas primeira iterações
tenha uma intensidade suficiente para que o efetuador seja levado a qf inal . A seguir são
mostrados alguns experimentos feitos com a variação das contantes de ganho das forças e
a inclusão ou não de obstáculos no espaço de trabalho e as suas influências no resultado
do algoritmo de planejamento de trajetória.
Os valores de qinit e qf inal , em metros, e a orientação do efetuador durante a
trajetória, em quaternions, para os testes a seguir foram definidos como:
qinit = [0.212322831154, −0.257197618484, 0.509646713734]
qf inal = [0.240000000000, −0.190000000000, 0.960000000000]
orientação = [0.608000000000, 0.349000000000, 0.355000000000, 0.618000000000]

Capítulo 4. Resultados 31
4.1 Planejamento de Trajetória sem Obstáculos

Para a trajetória I foram considerados ζ = 50, η = 0, 1 e α = 0, 01. Com esse
valores, o algoritmo chegou ao resultado dentro da tolerância exigida em 5 pontos. Abaixo
são mostrados os gráficos da sua trajetória no espaço cartesiano e no espaço de juntas.
(a) Coordenadas no efetuador, em metros (b) Orientação do efetuador, em quaternions
Figura 7 – Trajetória I no espaço cartesiano
Pode-se perceber que a orientação do manipulador permaneceu praticamente

constante durante todo o percurso, como era desejado. Além disso, a cada iteração vê-se
que a variação de distância percorrida pelo efetuador é menor. Isso porque a força atrativa
diminui à medida que o efetuador se aproxima do ponto objetivo. As variações no módulo
das forças atrativa e repulsiva a cada iteração são mostradas no gráfico que segue.
Figura 8 – Módulo das forças atrativa (Fatt ) e repulsiva (Frep ) para a trajetória I
Como não há obstáculos, a força repulsiva é numa durante todo o trajeto. A força
atrativa diminui quadraticamente com a aproximação de qf inal , que é o comportamento
esperado.
O movimento do manipulador no espaço de junta é mostrado na figura 9.
Se a constante de força atrativa for o dobro do caso anterior e os outros parâmetros
forem os mesmos. Isto é, se ζ = 100, η = 0, 1 e α = 0, 01,o algoritmo encontrará como
solução uma trajetória II de apenas 1 ponto. Ou seja, o efetuador levará apenas uma
iteração para chegar ao ponto objetivo.
(a) Junta 1 (segundos x graus) (b) Junta 2 (segundos x graus)
(c) Junta 3 (segundos x graus) (d) Junta 4 (segundos x graus)
(e) Junta 5 (segundos x graus) (f) Junta 6 (segundos x graus)
Figura 9 – Trajetória I no espaço de juntas
Comparando as trajetórias no espaço cartesiano, figuras 7 e 10, e no espaço de

juntas, figuras 9 e 11, é fácil ver que o manipulador chega ao mesmo ponto qf inal , porém
em número de iterações, logo, em tempos, diferentes. Isso ocorre pelo fato de que o produto
das constantes de força e α define a distância que o efetuador percorrerá a cada iteração.
Assim, como αFatt é dez vezes maior na trajetória II do que na I, o efetuador percorrerá
por iteração uma distância dez vezes maior. Logo, o ganho em II é mais que suficiente
para encontrar uma solução em apenas uma iteração.
Uma situação deste tipo não é interessante para o desvio de obstáculos, pois com
distâncias muito grandes percorridas por iteração é possível que o manipulador colida com o
obstáculo, já que pode calcular o ponto seguinte antes de entrar na distância de influência
do obstáculo e se deslocar o suficiente para uma colisão antes mesmo de o algoritmo
perceber o campo repulsivo. Por isso é desejável para este tipo de aplicação possua uma
trajetória de vários pontos com pequenos deslocamentos executados em pequenos tempos
a fim de que se possa perceber e evitar colisões o mais breve possível.
Figura 10 – Trajetória II no espaço cartesiano
Figura 11 – Trajetória II no espaço de juntas

O experimento seguinte tem como resultado uma trajetória III que atende aos
requisitos citados acima. Para este caso foram usados ζ = 100, η = 0, 1 e α = 0, 001. O
algoritmo encontra uma solução para estes parâmetros em 22 pontos. A trajetória III é
mostrada na figura 12 no espaço cartesiano e na figura 14 no espaço de juntas.
Figura 12 – Trajetória III no espaço cartesiano
Novamente é possível que as trajetórias I, II e II são, em geral, iguais. Porém a

trajetória III possui um maior número de iterações e distâncias menores percorridas a
cada iteração. Com isso, o manipulador apresenta uma maior capacidade de adaptação ao
ambiente, já que consegue perceber obstáculos logo que entra na sua distância de influência.
Uma forma intuitiva de ver esse fato é por conta de que as variações de distância a cada
ponto calculado são muito menores do que ρ0 , o que faz com que se diminua o risco de
que o efetuador se aproxime de qobstaculo , pois o campo repulsivo começará a atuar muito
próximo da fronteira de influência do obstáculo.
Comparando o comportamento de módulos das forças na trajetória I, figura 8, com
o da trajetória III, figura 13, nota-se mesmo comportamento no decorrer da trajetória,
mas com característica mais suave no último caso. O que evidencia ainda mais a maior
sensibilidade ao ambiente.
Figura 13 – Módulo das forças atrativa (Fatt ) e repulsiva (Frep ) para a trajetória III
Figura 14 – Trajetória III no espaço de juntas
Com isso, conclui-se que os valores escolhidos para a trajetória III dão ao algoritmo
um melhor desempenho para desvios de obstáculos. Na próxima seção será testada essa
capacidade do método.
4.2 Planejamento de Trajetória com Obstáculos

As trajetórias I, II e III, uma vez que não existiam obstáculos, são uma linha reta.
Dessa forma, foi inserido um primeiro obstáculo qo1 num ponto pertencente à trajetória
III.
qo1 = [0.23034956794039238, −0.213430360791436, 0.8029715186508624]
Neste caso, o método de planejamento não encontrará uma solução que leve o
efetuador para qf inal . Ao invés disso, já na iteração de número 7 o efetuador chega à posição
q = [0.22211248723795193, −0.23342923457597461, 0.6689406000904017] e não consegue
sair dela. As figuras 15 e 16 mostram essa trajetória, IV, no espaço cartesiano e de juntas,
respectivamente.
Figura 15 – Trajetória IV no espaço cartesiano
Figura 16 – Trajetória IV no espaço de juntas
É possível ver que o efetuador não chega em qf inal não importa quanto tempo
se passe. Este é um problema de mínimo local, comum no método de planejamento de
trajetória por campos potenciais artificiais. Ele ele se dá quando o gradiente do campo
resultante formado por obstáculos e ponto objetivo apresenta, além do valor mínimo global
em qf inal , valores mínimos locais em ponto intermediários da trajetória. Nesses pontos,
a resultante das forças que atuam sobre o efetuador é nula e mesmo que a condição de
parada do algoritmo não seja atendida, ele não sairá deste ponto. A figura 17 mostra a
variação do módulo das forças no decorrer da trajetória.
Figura 17 – Módulo das forças atrativa (Fatt ) e repulsiva (Frep ) para a trajetória IV
O problema dos mínimos locais pode ser resolvido de diversas maneiras, como, por
exemplo, a aplicação de campo rotacionais nos obstáculos ao invés de lineares ou, uma
vez preso num mínimo local, o manipulador executar uma série de movimentos aleatórios,
controlados para tentar se afastar o suficiente deste ponto e conseguir chegar ao ponto
objetivo, ou ainda a determinação de pontos obstáculos auxiliares para que este mínimo
deixe de existir. Contudo, este trabalho não visa propor uma solução para o problema dos
mínimos locais e este experimento foi feito com o intuito de mostrar oportunidades de
melhorias para a aplicação.
Para evitar o mínimo local é possível deslocar o obstáculo de forma que não fique
sobre uma linha reta entre qinit e qf inal . Assim, qo2 = [0.26, −0.25, 0.7729635104026684] e
o algoritmo não é alterado.
Uma trajetória é encontrada pelo algoritmo em 38 pontos. O manipulador desvia do
obstáculo e chega ao ponto final como pode ser visto nas figuras 18, no espaço cartesiano,
e 19, no espaço de juntas. Comparando essa trajetória com as trajetória I, II e III é
perceptível que não são iguais apesar de chegarem ao mesmo ponto final.
Pode ser percebido que a modificação do percurso ocorre logo nas primeiras
iterações, pois é na sétima iterações que a força repulsiva assume seu valor máximo, logo,
é neste instante que o efetuador está mais próximo de qo2 e sofre maior influência do
campo repulsivo alterando mais acentuadamente seu trajeto. Após isso, a campo repulsivo
influencia cada vez menos o manipulador.
Figura 18 – Trajetória V no espaço cartesiano
Figura 19 – Trajetória V no espaço de juntas

Avaliando a figura 20 pode-se ver o comportamento do módulo das forças atrativa

e repulsiva a cada iteração.
Figura 20 – Módulo das forças atrativa (Fatt ) e repulsiva (Frep ) para a trajetória V
O pico da força repulsiva ocorre na sétima iteração. O que altera o valor da

força atrativa, pois o percurso do efetuador deixa de ser uma linha reta. Ao se alcançar
qf inal pode-se perceber que Esse comportamento das forças e das variáveis das juntas do
manipulador mostram a eficácia do método de planejamento de trajetórias por campos
potenciais artificiais para aplicações em tempo real e com desvio de obstáculos. O código
python em anexo está configurado para os parâmetros deste experimento.
4.3 Gráficos Comparativos

A seguir são mostrados gráficos comparativos das trajetórias III e V no espaço
cartesiano e de juntas. Dessa forma é possível observar as diferenças de tempo e cami-
nho para o caso sem obstáculos e com obstáculos. O tempo do eixo das abscissas é de
aproximadamente 200 segundos.
Pelas análise das comparações, pode-se ver que a trajetória III dura menos tempo
que a trajetória V. Isso porque III descreve o caminho mais curto entre qinit e qf inal , pois
o caminho entre os dois pontos é livre. Em V, o obstáculo faz com que o manipulador
percorra um caminho mais longo para desviar de qo2 e então o algoritmo passa por mais
iterações, levando mais tempo para chegar ao ponto objetivo.
Figura 21 – Coordenadas do efetuador, em metros, para as trajetórias III e V (mesmos

eixos, mesmas cores)
Figura 22 – Trajetórias da junta 1, em graus, para as trajetórias III (azul) e V (vermelho)

Figura 23 – Trajetórias da junta 2, em graus, para as trajetórias III (roxo) e V (verde)
Figura 24 – Trajetórias da junta 3, em graus, para as trajetórias III (verde) e V (mostarda)

Figura 25 – Trajetórias da junta 4, em graus, para as trajetórias III (mostarda) e V (preto)
Figura 26 – Trajetórias da junta 5, em graus, para as trajetórias III (preto) e V (verde)

Figura 27 – Trajetórias da junta 6, em graus, para as trajetórias III (verde) e V (marrom)

44
5 Conclusão
Com os resultados expostos anteriormente é possível comprovar o funcionamento

do planejamento de trajetórias por campos potenciais artificiais em aplicações de tempo
real e com desvio de obstáculos no manipulador robótico JACO. Este trabalho pode servir
como ponto de partida para o desenvolvimento de aplicações mais complexas em desvio
de obstáculos através de utilização de visão computacional e percepção de ambientes
dinâmicos.
Alguns pontos de melhoria podem ser observados, como a susceptibilidade a falhas
em encontrar uma solução por conta de mínimos locais, visto na trajetória IV. Além disso,
os campos potencias aplicados neste trabalho apenas induzem forças na ferramenta do
manipulador. Isso faz com que a ferramenta não colida com obstáculos no caminho até
o objetivo, mas nada se pode garantir dos outros elos e juntas do manipulador. Uma
alternativa a esse problema é fazer com que os campos também exerçam forças sobre outros
pontos do manipulador, normalmente as juntas. É usual se adicionar tantos pontos quanto
graus de liberdade tenha o manipulador. No caso do JACO, haveriam seis forças atrativas
e seis forças repulsivas. Neste caso, cada força possui sua constante, e é recomendado que
a maior delas seja a do efetuador, pois isto fará com que seja ele quem chegue primeiro ao
ponto objetivo. Isso garante que todo o manipulador desvie dos obstáculos.
Com isso, a utilização deste algoritmo em aplicações mais complexas pode ser
desenvolvida pela sua melhoria através das técnicas citadas acima e amplamente utilizadas
nesta área. Dessa forma a solução de problemas de planejamento de trajetória pode se
tornar muito mais robusta.
45
Referências
Coppelia Robotics. V-REP. 2017. Disponível em: <http://www.coppeliarobotics.com>.

Citado 2 vezes nas páginas 25 e 26.
Honda Motor Company, Limited. Advanced Step in Innovative Mobility (ASIMO). 2007.
Citado na página 16.
KHATIB, O. Real-time obstacle avoidance for manipulators and mobile robots. The
International Journal of Robotics Research, 1986. Citado na página 27.
Kinova Robotics. Kinova Website. 2017. Disponível em: <http://www.kinovarobotics.

com>. Citado 2 vezes nas páginas 16 e 17.
Polimport - Comércio e Exportação Ltda. Robô Aspirador Housekeeper Pro Polishop.

2017. Citado na página 16.
ROS.ORG. ROS Website. 2017. Disponível em: <http://www.ros.org>. Citado na página

18.
Schmidt, Jeff (Clearpath) and Bencz, Alex (Clearpath) and DeDonato, Matt (WPI). Pacote
Kinova-ROS. 2017. Disponível em: <https://github.com/Kinovarobotics/kinova-ros>.
Citado 2 vezes nas páginas 26 e 30.
SPONG, M. W.; HUTCHINSON, S.; VIDYASAGAR, M. Robot modeling and control.

[S.l.]: wiley New York, 2006. v. 3. Citado 6 vezes nas páginas 16, 19, 20, 23, 24 e 26.
Anexos
47
ANEXO A – Código Python
1 import roslib; roslib.load_manifest('kinova_demo')

2 import rospy
3
4 import sys
5 import numpy as np
6
7 import actionlib
8 import kinova_msgs.msg
9 import std_msgs.msg
10 import geometry_msgs.msg
11
12
13 import math
14 import argparse
15 import time
16
17 """ Global variable """
18 arm_joint_number = 6
19 finger_number = 3
20 prefix = 'j2n6s300_'
21 finger_maxDist = 18.9/2/1000 # max distance for one finger
22 finger_maxTurn = 6800 # max thread rotation for one finger
23 coordInit = [0.212322831154, -0.257197618484, 0.509646713734] # ...
default home in unit mq
24
25 tolerancia = 0.05
26 orientInit = [0.608, 0.349, 0.355, 0.618]
27 coordFinal = [0.24, -0.19, 0.96]
28 orientFinal = [0.608, 0.349, 0.355, 0.618]
29 #qObstaculo = [0.2244571668875923, -0.2277365681966782, ...
0.7070924136422845]
30 qObstaculo = [0.26, -0.25, 0.7729635104026684]
31
32 ro = 100 # constante da forca atrativa
33 eta = 0.1 # constante da forca repulsiva
34 alpha = 0.001
35 di = 0.5 # distancia de influencia do obstaculo
36
37 global q
38 q = []
39
ANEXO A. Código Python 48
40 def cartesian_pose_client(position, orientation):

41 """Send a cartesian goal to the action server."""
42 action_address = '/' + prefix + 'driver/pose_action/tool_pose'
43 client = actionlib.SimpleActionClient(action_address, ...
kinova_msgs.msg.ArmPoseAction)
44 client.wait_for_server()
45
46 goal = kinova_msgs.msg.ArmPoseGoal()
47 goal.pose.header = std_msgs.msg.Header(frame_id=(prefix + ...
'link_base'))
48 goal.pose.pose.position = geometry_msgs.msg.Point(
49 x=position[0], y=position[1], z=position[2])
50 goal.pose.pose.orientation = geometry_msgs.msg.Quaternion(
51 x=orientation[0], y=orientation[1], z=orientation[2], ...
w=orientation[3])
52
53 # print('goal.pose in client 1: {}'.format(goal.pose.pose)) # debug
54
55 client.send_goal(goal)
56
57
58 return client.get_result()
59
60 #def get_posicao(posicao):
61 # return x
62
63 def distancia_pontos(ponto1, ponto2):
64 '''calculate the euclidean distance, no numpy
65 input: numpy.arrays or lists
66 return: euclidean distance
67 '''
68 dist = [(a - b)**2 for a, b in zip(ponto1, ponto2)]
69 dist = math.sqrt(sum(dist))
70 return dist
71
72 def planejar_trajetoria(qInit, qFinal,i):
73
74
75 if i == 0:
76 q.append(qInit)
77 #while True:
78 # if (distancia_pontos(q[i],qFinal)) > 0.5:
79
80 qaux = q[i]
81 qx = qaux[0]
82 qy = qaux[1]
83 qz = qaux[2]
84
85 # Calculo da forca atrativa F
86
87 global F
88 F = []
89
90 F.append(qx - qFinal[0])
91 F.append(qy - qFinal[1])
92 F.append(qz - qFinal[2])
93
94 F[0] = F[0]/distancia_pontos(q[i],qFinal)
97
98 modF = ro*distancia_pontos(q[i],qFinal)
99 #modF = ro
100 # mod = math.sqrt(F[0]**2 + F[1]**2 + F[2]**2) # Verificando ...
modulo unitario de F
101
102 #F[0] = qx - ro*F[0]
103 #F[1] = qy - ro*F[1]
104 #F[2] = qz - ro*F[2]
105
106 # Calculo da forca repulsiva R
107
108 global R
109 R = []
110
111 R.append(qx - qObstaculo[0])
112 R.append(qy - qObstaculo[1])
113 R.append(qz - qObstaculo[2])
114
115 if distancia_pontos(q[i],qObstaculo) > di:
116 modR = 0
117 else:
118 R[0] = R[0]/distancia_pontos(q[i],qObstaculo)
121
122 modR = eta*(1/distancia_pontos(q[i],qObstaculo) - ...
1/di)/math.pow(distancia_pontos(q[i],qObstaculo),2)
123 #modR = eta
124
125 F[0] = qx - alpha*modF*F[0] + alpha*modR*R[0]
126 F[1] = qy - alpha*modF*F[1] + alpha*modR*R[1]
127 F[2] = qz - alpha*modF*F[2] + alpha*modR*R[2]
128
129 q.append(F)
130 i = i + 1
131 return q,i,modR,distancia_pontos(q[i],qObstaculo),modF
132
133
134 def QuaternionNorm(Q_raw):
135 qx_temp,qy_temp,qz_temp,qw_temp = Q_raw[0:4]
136 qnorm = math.sqrt(qx_temp*qx_temp + qy_temp*qy_temp + ...
qz_temp*qz_temp + qw_temp*qw_temp)
137 qx_ = qx_temp/qnorm
138 qy_ = qy_temp/qnorm
139 qz_ = qz_temp/qnorm
140 qw_ = qw_temp/qnorm
141 Q_normed_ = [qx_, qy_, qz_, qw_]
142 return Q_normed_
143
144
145 def Quaternion2EulerXYZ(Q_raw):
146 Q_normed = QuaternionNorm(Q_raw)
147 qx_ = Q_normed[0]
148 qy_ = Q_normed[1]
149 qz_ = Q_normed[2]
150 qw_ = Q_normed[3]
151
152 tx_ = math.atan2((2 * qw_ * qx_ - 2 * qy_ * qz_), (qw_ * qw_ - ...
qx_ * qx_ - qy_ * qy_ + qz_ * qz_))
153 ty_ = math.asin(2 * qw_ * qy_ + 2 * qx_ * qz_)
154 tz_ = math.atan2((2 * qw_ * qz_ - 2 * qx_ * qy_), (qw_ * qw_ + ...
qx_ * qx_ - qy_ * qy_ - qz_ * qz_))
155 EulerXYZ_ = [tx_,ty_,tz_]
156 return EulerXYZ_
157
158
159 def EulerXYZ2Quaternion(EulerXYZ_):
160 tx_, ty_, tz_ = EulerXYZ_[0:3]
161 sx = math.sin(0.5 * tx_)
162 cx = math.cos(0.5 * tx_)
163 sy = math.sin(0.5 * ty_)
164 cy = math.cos(0.5 * ty_)
165 sz = math.sin(0.5 * tz_)
166 cz = math.cos(0.5 * tz_)
167
168 qx_ = sx * cy * cz + cx * sy * sz
169 qy_ = -sx * cy * sz + cx * sy * cz
170 qz_ = sx * sy * cz + cx * cy * sz
171 qw_ = -sx * sy * sz + cx * cy * cz
172
173 Q_ = [qx_, qy_, qz_, qw_]

174 return Q_
175
176
177
178
179 if __name__ == '__main__':
180 rospy.init_node(prefix + "Reativo")
181
182 print "\nPonto inicial: ", coordInit
183 print "\nPonto final: ", coordFinal
184 print "\nPonto obstaculo: ", qObstaculo
185 print "\n"
186
187 d = distancia_pontos(coordInit,coordFinal)
188 i = 0
189 while d > tolerancia:
190 trajetoria,i,modR,dist,modF = ...
planejar_trajetoria(coordInit,coordFinal,i)
191 d = distancia_pontos(trajetoria[i],coordFinal)
192 print i,q[i],dist,modR,modF
193 if i > 100:
194 break
195
196 # print "\nTrajetoria: ", trajetoria
197
198 print "\n", distancia_pontos(trajetoria[i],coordFinal)
199 print "\n"
200
201 n = 0
202 for n in range(0,i+1):
203 print "Ponto ",n
204 result = cartesian_pose_client(trajetoria[n],orientFinal)
205 time.sleep(5)

Modelo JACO trajetória obstáculos ROS

Enviado por

Dados do documento

Título original

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

Modelo JACO trajetória obstáculos ROS

Enviado por

Direitos autorais:

Formatos disponíveis

Universidade Federal da Bahia - UFBA

Escola Politécnica da UFBA - EPUFBA

Modelagem e análise do manipulador robótico JACO

Ubiratan de Melo Pinto Júnior

Modelagem e análise do manipulador robótico JACO

Trabalho Final de Graduação apresentado

Universidade Federal da Bahia - UFBA

Orientador: Prof. Dr. André Gustavo Scolari Conceição

Ubiratan de Melo Pinto Júnior

Orientador: Prof. Dr. André Gustavo Scolari Conceição

TFG (Graduação) – Universidade Federal da Bahia - UFBA

Modelagem e análise do manipulador robótico JACO

Trabalho Final de Graduação apresentado

Prof. Dr. Cristiane Corrêa Paim

Trabalho aprovado. Brasil, 24 de novembro de 2017:

Prof. Dr. André Gustavo Scolari

Prof. Dr. Augusto Loureiro da Costa

Prof. Dr. Tiago Trindade Ribeiro

Palavras-chaves: Campos Potenciais Artificiais. JACO. Denavit-Hartenberg. Robótica.

Key-words: Artificial Potential Field. JACO. Denavit-Hartenberg. Robotics. ROS. Python.

Figura 1 –Exemplos de robôs . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16

Tabela 1 – Parâmetros DH do manipulador JACO . . . . . . . . . . . . . . . . . . 26

RIA Robot Institute of America

PRM Pessoa com mobilidade reduzida

ROS Robot Operatins System

pi Coordenadas do ponto p em relação ao sistema de coordenadas i

oi Origem do sistema de coordenadas i

xi Versor na direção x do sistema de coordenadas i

yi Versor na direção y do sistema de coordenadas i

zi Versor na direção z do sistema de coordenadas i

oij Coordenadas da origem do sistema de coordenadas i em relação ao

R Matriz de rotação resultante

Rji Matriz de rotação do sistema de coordenadas j em relação ao sistema

Rotx,α Rotação de α graus de um sistema de coordenadas em relação ao seu

Roty,γ Rotação de γ graus de um sistema de coordenadas em relação ao seu

Rotz,θ Rotação de θ graus de um sistema de coordenadas em relação ao seu

T Matriz de translação resultante

T ransx,dx Translação de uma distância dx de um sistema de coordenadas em

T ransy,dy Translação de uma distância dy de um sistema de coordenadas em

H Tranformação homogênea entre sistemas de coordenadas

qi Variável de junta da iésima junta de um manipulador robótico

Ai Transformação homogênea que relaciona os sistemas de coordenadas i e

Ai (qi ) Transformação homogênea que relaciona os sistemas de coordenadas i e

U (q) Campo potencial resultante para o espaço de juntas q

Uatt (q) Campo potencial atrativo para o espaço de juntas q

Urep (q) Campo potencial repulsivo para o espaço de juntas q

F (q) Força resultante para o espaço de juntas q

Fatt (q) Força atrativa para o espaço de juntas q

Frep (q) Força repulsiva para o espaço de juntas q

qinit Coordenadas do ponto inicial da trajetória do manipulador

qf inal Coordenadas do ponto final da trajetória do manipulador

qobstaculo Coordenadas do ponto obstáculo da trajetória do manipulador

qoi Coordenadas do iésimo ponto obstáculo da trajetória do manipulador

ANEXO A – CÓDIGO PYTHON . . . . . . . . . . . . . . . . . . . 47

A robótica é um campo relativamente novo na engenharia. Seu estudo envolve

Figura 1 – Exemplos de robôs

Figura 2 – Manipulador JACO adaptado a uma cadeira de rodas

Para o controle de um manipulador robótico, é necessário o desenvolvimento de um

Figura 3 – Funções do ROS

2.1 Objetivos específicos

• Modelagem do manipulador através da cinemática direta e inversa.

• Desenvolvimento do método para o planejamento de trajetória por campos potenciais

• Desenvolvimento do código para planejamento de trajetória por campos potenciais