MÉTODO DE CONTROLE E DETECÇÃO DE OBSTÁCULOS PARA ROBÔS MANIPULADORES APLICADO À INTERAÇÃO HUMANO-ROBÔ
THIAGOB.DEALMEIDA-ANTONIO, FERNANDOLIZARRALDE, LIUHSU, ANTONIOC. LEITE∗
∗Departamento de Engenharia Elétrica / COPPE Universidade Federal do Rio de Janeiro C.P. 68504, CEP 21941-972, Rio de Janeiro, RJ, Brasil
Email: braga.thiago@poli.ufrj.br, [fernando, liu, toni]@coep.ufrj.br
Abstract— This work presents a kinematic control and collision detection method for redundant robot manipulators operating in human environments. The control algorithm is based on the concept of artificial potential fields and uses the pseudo-inverse of the Jacobian matrix with a weighting factor for the joint mechanical limits, taking advantage of the robot redundancy for the purpose of obstacle avoidance and control goal achievement. The detection algorithm uses a depth sensor based on the structured light to obtain a 2D 1/2 description of the surroundings from a point cloud. Repulsive fields are created around the detected obstacles, allowing for the robot to perform the task of interest without collisions. Experimental results obtained from a Motoman DIA10 robot and a natural user interface Microsoft KinectTMillustrate the feasibility of the proposed scheme.
Keywords— Robot Control, Obstacle Avoidance, Artificial Potential Fields, Human-Robot Interaction
Resumo— Este trabalho apresenta um método de controle cinemático e detecção de obstáculos para robôs manipuladores redun-dantes operando em ambientes humanos. O algoritmo de controle baseia-se no conceito de campos potenciais artificiais e utiliza a pseudo-inversa da matriz Jacobiana com um fator de ponderação para os limites mecânicos das juntas, aproveitando a caracterís-tica de redundância do robô para desviar de obstáculos e atingir o objetivo de controle simultaneamente. O algoritmo de detecção utiliza um sensor de profundidade baseado na tecnologia de luz estruturada para obter uma descrição 2D 1/2 do ambiente a partir de uma nuvem de pontos. Campos repulsivos são gerados em torno dos obstáculos detectados permitindo que o robô realize a tarefa de interesse sem colisões. Resultados experimentais obtidos com um robô Motoman DIA10 e um dispositivo de interface natural Microsoft KinectTM são apresentados para ilustrar a viabilidade do método proposto.
Palavras-chave— Controle de Robôs, Desvio de Obstáculos, Campos Potenciais Artificiais, Interação Humano-Robô.
1 Introdução
Atualmente, um dos principais desafios da robó-tica é lidar com a interação entre humanos e robôs, possibilitando que eles trabalhem juntos de forma na-tural, efetiva e segura (Santis et al., 2008; Haddadin et al., 2008). Para permitir essa interação é preciso co-nhecer o ambiente ao redor do robô garantindo que o mesmo se movimente sem causar danos aos operado-res próximos a ele. Portanto, o robô deve ser capaz de evitar colisões com obstáculos localizados no seu espaço de trabalho e alcançar os objetivos de controle simultaneamente.
Na área de robótica diversos métodos de plane-jamento de movimento e desvio de obstáculos foram propostos ao longo dos anos com o intuito de gerar um caminho ou trajetória livre de colisões entre dois pon-tos de interesse (LaValle, 2011). Entretanto, como a proposta do presente trabalho é promover a interação entre robôs e seres humanos em um ambiente dinâ-mico e imprevisível, o uso de técnicas off-line não é re-comendado, uma vez que elas consideram os modelos do robô e do cenário perfeitamente conhecidos. Uma alternativa é utilizar técnicas on-line ou reativas capa-zes de lidar com a presença de objetos desconhecidos e móveis em ambientes incertos (Minguez et al., 2008). Uma abordagem de desvio de obstáculos bem co-nhecida é baseada no conceito de campos potenciais artificiais, onde campos virtuais atrativos e repulsivos são criados no espaço de trabalho do robô, e dessa forma o efetuador pode mover-se na direção de um ponto desejado, enquanto evita colidir com obstáculos
do ambiente (Khatib, 1986; Chung et al., 1997). Outra abordagem consiste em utilizar campos artificiais cir-culares, que rotacionam a trajetória do robô ao redor dos obstáculos ao invés de apenas repelir o robô como faz o campo potencial repulsivo (Singh et al., 1996). Campos virtuais circulares e potenciais também po-dem ser combinados a fim de aproveitar as vantagens de ambas as abordagens (Haddadin et al., 2011). Nes-ses trabalhos, os autores assumem que o movimento dos obstáculos é conhecido a priori ignorando a ne-cessidade de sensoreamento externo. Em Flacco et al. (2012) um método de detecção de obstáculos baseado em dados obtidos a partir de um dispositivo Micro-soft KinectTMé proposto para evitar colisões em ta-refas de interação entre humanos e robôs. Em Rak-prayoon et al. (2011) é apresentado um método de ca-libração para o Kinect bem como um método para dis-tinguir entre o manipulador e os obstáculos, quando eles compartilham o mesmo espaço de trabalho.
No presente trabalho, apresenta-se um método de controle cinemático e detecção de obstáculos para ma-nipuladores redundantes operando em ambientes hu-manos. O algoritmo de controle baseia-se no conceito de campos potenciais artificiais e utiliza a pseudo in-versa da matriz Jacobiana com um fator de pondera-ção para os limites mecânicos das juntas, aproveitando a característica de redundância do robô para desviar de obstáculos. Um sensor de profundidade baseado na tecnologia de luz estruturada é utilizado para obter uma descrição 2D 1/2 do ambiente a partir de uma nu-vem de pontos. Campos repulsivos são gerados ao re-dor dos obstáculos detectados, permitindo que o robô
realize a tarefa de interesse sem colisões e garantindo a segurança dos operadores. Resultados experimentais obtidos a partir de um robô Motoman DIA10 e um dis-positivo de interface natural Microsoft KinectTM ilus-tram a viabilidade do método proposto.
2 Formulação do Problema
Neste trabalho, considera-se que um robô mani-pulador redundante deve executar uma determinada tarefa em um ambiente humano, na presença de pes-soas trabalhando e movendo-se livremente dentro do espaço de trabalho do robô. A tarefa de interesse con-siste em mover o efetuador de um ponto inicial P0até
o ponto final Pf, seguindo uma trajetória de referência
r(t), enquanto mantém uma orientação desejada
vari-ante no tempo (vide Figura 1). Assume-se aqui que a tarefa é previamente planejada e que a descrição do ambiente de trabalho é parcialmente conhecida.
Para adquirir uma representação geométrica 2D 1/2 do ambiente, um dispositivo de interface natural (DIN), equipado com um sensor de profundidade, é montado em frente ao robô. Dessa forma, objetos fi-xos e móveis (incluindo pessoas) podem ser identifica-dos na forma de uma nuvem de pontos e o robô pode realizar a tarefa de maneira segura sem colidir com es-ses obstáculos. Robô Redundante DIN Operador Trajetória r(t) ~xb ~yb ~zb ~xe ~ye ~ze ~xs ~ys ~zs P0 Pf
Figura 1: Robô evitando colisões com um operador, um DIN e os sistemas de coordenadas de interesse.
Como a maioria dos robôs industriais possui uma malha interna de controle de velocidade para aciona-mento direto das juntas, uma lei de controle puramente cinemática baseada em comandos de velocidade pode ser implementada para garantir o desempenho satisfa-tório do manipulador durante a execução da tarefa.
3 Controle Cinemático
Considere o problema de controle cinemático de um manipulador com n graus de mobilidade. Neste contexto, quando o robô possui altas taxas de redu-ção nas engrenagens ou quando baixas velocidades são empregadas para realizar uma tarefa, o movimento
do manipulador pode ser descrito por: ˙
θi= ui, i= 1, · · · , n , (1)
onde ˙θié a velocidade angular da i-ésima junta e uié o sinal de controle de velocidade aplicado ao drive do motor da i-ésima junta. A partir do vetor de ângulo das juntasθ∈ Rn pode-se obter a pose do efetuador x ∈ Rm através do mapeamento da cinemática direta
k(·) como: x= p q = k(θ) , (2)
onde p ∈ R3é a posição do efetuador com respeito a base do robô e q=qs q⊤v
⊤
∈ R4é o quaternion
uni-tário correspondente a matriz de rotação R ∈ SO(3) (Wen and Kreutz-Delgado, 1991), que representa a orientação do sistema de coordenadas do efetuador com respeito ao sistema de coordenadas da base. Note que, qs∈ R e qv∈ R3são as partes escalar e vetorial do quaternion, sujeitas a restrição q⊤q= 1.
A derivada temporal de (2) resulta na seguinte equação de cinemática diferencial:
˙ x= ˙ p ˙ q = Ja(θ) ˙θ, (3)
onde Ja(θ) =∂k∂θ(θ)∈ Rm×né o Jacobiano analítico. A
velocidade angular do efetuadorωé relacionada à de-rivada temporal do quaternion unitário por (Wen and Kreutz-Delgado, 1991):
ω= 2 Jr(q) ˙q , (4)
onde Jr(q) = [−qv qsI − Q(qv)] ∈ R3×4é o Jacobiano da representação e Q(·) é o operador produto vetorial.
A relação entre as velocidades das juntas ˙θe as velo-cidades linear ˙p e angularωdo efetuador é dada por:
v= ˙ p ω = I3×3 03×4 03×3 2 Jr(q) Ja(θ) | {z } J(θ) ˙ θ, (5)
onde J(θ) ∈ R6×né o Jacobiano geométrico do
mani-pulador. A partir da abordagem de controle cinemático (1), o seguinte sistema de controle é obtido: v=J(θ) u.
Uma lei de controle cartesiana vcé transformada em um sinal de controle das juntas usando:
u= J−1(θ)v c= J−1(θ) vp vo , (6)
onde vpe vosão leis de controle projetadas para con-trolar a posição e a orientação do efetuador respecti-vamente. Para robôs redundantes, a seguinte relação baseada na pseudo-inversa ponderada da matriz Jaco-biana pode ser utilizada:
u= W−1J⊤(J W−1J⊤)−1v
c= Jw†vc, (7)
onde W ∈Rn×né uma matriz de ponderação, simétrica positiva definida, e J J†
w= I. Note que, as relações (6)
e (7) são válidas desde que vcnão conduza o robô para
configurações singulares, onde a matriz Jacobiana é singular.
3.1 Restrições das Juntas
Neste trabalho, o sinal de controle (7) pode ser utilizado para evitar que os movimentos do robô al-cancem os limites mecânicos das juntas. A matriz de ponderação W é construída de forma que quando uma junta do robô aproxima-se do seu limite, os elementos da diagonal tendem a infinito. Isso garante que as ve-locidades das juntas tendam a zero na vizinhança de um limite mecânico (Bjerkeng et al., 2011). Então, os elementos diagonais de W−1tendem a zero conforme a junta aproxima-se do seu limite, e são iguais a 1 se a junta está no meio da sua faixa, isto é:
W−1 i,i = 1 1+ Hi(θi)2 , (8) com Hi(θi) = (θimax−θimin) 2(2θ i−θimax−θimin) (θi−θimax)2(θi−θimin)2 , (9)
ondeθimaxeθiminsão os limites superior e inferior da i-ésima junta respectivamente.
3.2 Controle de Posição
Considere que o objetivo do controle para uma determinada tarefa é seguir uma posição desejada va-riante no tempo pd(t) a partir de uma posição inicial
p, isto é:
p → pd(t), ep= pd(t) − p → 0 , (10) onde ep∈ R3 é o erro de posição. A partir de (5) e
(6) chega-se a ˙p=vpobtendo-se a seguinte equação de erro: ˙ep= ˙pd− vp. Usando uma lei de controle baseada em uma ação proporcional e um termo feedforward dada por
vp= Kpep+ ˙pd, (11)
onde Kp= kpI é a matriz de ganho de posição, a dinâ-mica do erro de posição é governada por ˙ep+Kpep=0. Então, escolhendo-se kpcomo uma constante positiva, tem-se que limt→∞ep(t) = 0.
3.3 Controle de Orientação
Considere que o objetivo de controle para uma de-terminada tarefa é alcançar uma orientação desejada variante no tempo Rd(t) a partir de uma orientação ini-cial R, isto é:
R → Rd(t), Rq= R⊤Rd→ I , (12)
onde Rq∈ SO(3) é a matriz de erro de orientação. A
representação em quaternion unitário de Rqé dada por
eq= [eqse⊤qv]⊤, com eq= q−1∗ qd(t), onde q e qdsão as representações em quaternion unitário de R e Rd respectivamente e “ ∗” é o operador produto de quater-nion. Note que, eq=[1 0⊤]⊤se R e Rdestão alinhados. A partir de (5) e (6) chega-se aω=vo. Usando uma lei
de controle baseada em uma ação proporcional e um termo feedforward dada por:
vo=ωd+ Koeqv, (13)
onde Ko= Ko⊤>0 é a matriz de ganho de orientação eωd∈ R3é a velocidade angular desejada, os pontos de equilíbrio eq= [±1 0⊤]⊤são ditos ser quase glo-balmente assintoticamente estáveis (Wen and Kreutz-Delgado, 1991). Uma análise de convergência e esta-bilidade para o sistema de controle de orientação pro-posto baseada na Teoria de Estabilidade de Lyapunov é apresentada em Leite et al. (2009).
3.4 Controle de Tarefa Secundária
A caractérística de redundância do robô permite gerar movimentos internos nas juntas que reconfigu-ram a estrutura do manipulador sem modificar a po-sição e orientação do efetuador. Isso pode ser al-cançado adicionando-se um termo, denominado solu-ção homogênea, à lei de controle (7) como (Bjerkeng et al., 2011):
u= Jw†(θ) vc+α(I − Jw†J) W−1θ˙0, (14)
ondeα é um escalar negativo e ˙θ0∈ Rn×1é um vetor
de velocidade das juntas arbitrária. A matriz(I −J†
wJ) projeta o vetor W−1θ˙
0no espaço nulo do Jacobiano
sem violar a restrição (5). Note que, o primeiro termo de (14) corresponde a norma mínima das velocidades das juntas e o segundo termo satisfaz a restrição adi-cional de prioridade secundária especificada via ˙θ0.
Uma escolha típica para a restrição adicional é: ˙ θ0= ∂g(θ) ∂θ ⊤ , (15)
onde g(θ) é a função objetivo secundária (e.g.,
distân-cia para um obstáculo). A solução de (15) move-se ao longo da direção do gradiente da função objetivo se-cundária para maximizá-la localmente, satisfazendo o objetivo primário.
4 Desvio de Obstáculos
Uma abordagem eficiente para planejamento de movimento online baseia-se no conceito de que um determinado ponto do robô pode mover-se no espaço cartesiano sob a influência de campos potenciais arti-ficiais. Neste contexto, campos atrativos conduzem o robô na direção de um ponto desejado, enquanto que campos repulsivos são criados ao redor de obstáculos para evitar colisões (Khatib, 1986). Neste trabalho, dois tipos de campos potenciais repulsivos são utiliza-dos: um é definido em termos de uma restrição adi-cional onde as juntas são consideradas como os pon-tos de interesse para o cálculo da distância (Chung et al., 1997); o outro atua sobre o efetuador e mo-difica a lei de controle de posição, garantindo que o mesmo siga uma trajetória de referência livre de coli-sões (Flacco et al., 2012).
4.1 Desvio de Obstáculos para as Juntas
Para uma junta Jie um obstáculo Ok arbitrários, o campo potencial repulsivo UJ,O(θ) é definido por
(Khatib, 1986): UJ,O(θ) = kJ,O 2 1 ηJ,O(θ)− 1 ηlim 2 , η J,O(θ) ≤ηlim 0 , η J,O(θ) >ηlim onde kJ,O>0, η
J,O(θ) é a distância entre a junta e
o obstáculo e ηlim é o raio de influência do obstá-culo ou a distância limite de detecção do obstáobstá-culo. Note que, quanto menor éηJ,O(θ), maior é a
intensi-dade do campo potencial repulsivo. Dessa forma, se
ηJ,O(θ) →0, implica que UJ,O(θ) →∞. Então, a
restri-ção adicional resultante é: ˙ θ0= no
∑
J,O ∇UJ,O(θ) , (16)onde noé o número de obstáculos e∇UJ,O(θ) é o gra-diente do potencial repulsivo definido por:
∇UJ,O(θ) = −η2kJ,O J,O(θ) 1 ηJ,O(θ) −η1 lim ! ∇ηJ,O(θ) ,
onde∇ηJ,O(θ) é o gradiente da distância.
4.2 Cálculo da Distância entre Elos e Obstáculos O uso da distância entre uma junta e um obstá-culoηJ,O no algoritmo de detecção pode gerar confli-tos de movimenconfli-tos entre duas ou mais juntas. De fato, um conflito ocorre quando o campo repulsivo afasta um junta de um obstáculo, mas aproxima outra junta de um ou mais obstáculos, criando um ciclo de mo-vimentos oscilatórios. Uma solução alternativa, que resulta em movimentos mais suaves, consiste em usar a distância entre o elo e o obstáculoηL,O, ao invés da distância entre a junta e o obstáculoηJ,O na expressão de UJ,O. Considere PLum ponto arbitrário localizado em um elo Lj do robô, conectando dois pontos P1 e P2localizados em duas juntas consecutivas J1e J2
res-pectivamente. A posição do ponto PLexpressa em um sistema de coordenadas inercial é dada por:
pL= p1+λ(p2− p1) , (17)
onde p1e p2são as posições dos pontos P1e P2
respec-tivamente, eλ>0 é uma constante paramétrica a ser
definida. A distância entre um obstáculo pontual Oke um elo Ljpode ser obtida pelo cálculo da distância en-tre esse obstáculo e o ponto mais próximo localizado no elo. A partir de (17), o valor deλ que corresponde ao ponto mais próximo do elo Ljé obtido por:
λ = −(p1− po) · (p2− p1)
|p2− p1|2
, (18)
onde poé a posição do obstáculo. A expressão para o valor de pL varia de acordo com o valor deλ. Se
λ≤ 0, pL= p1, seλ≥ 1, pL= p2, e nos demais casos,
pL é obtido de (17). Então, a distância resultanteηL,O é calculada como (Khatib, 1986):
ηL,O= ||(po−p1)×(po−p2)|| ||p2−p1|| , 0 <λ<1 ||(po− p1)|| , λ≤ 0 ||(po− p2)|| , λ≥ 1 . (19)
4.3 Desvio de Obstáculos para o Efetuador
O campo potencial repulsivo associado ao efe-tuador E e a um obstáculo Ok arbitrário é definido por (Flacco et al., 2012)
UE,O= vmag(ηE,O)
ηE,O(θ) ηE,O(θ) , (20)
em termos da direção de um vetor unitário ~ηE,O(θ) e
da magnitude vmag(ηE,O) =
vmax
1+ e(||ηE,O(θ)||(2/ρ)−1)b
, (21)
ondeηE,O(θ) é a distância entre o efetuador e o
obs-táculo, vmax é a magnitude máxima,ρ é o raio de in-fluência e b > 0 é uma constante que define a incli-nação da curva (vide Figura 2). Note que, quando
ηE,O(θ) = 0, vmag(ηE,O) = vmax/(1 + e
−b), e quando
ηE,O(θ) →ρ, vmag(ηE,O) → 0.
0 0.05 0.1 0.15 0.2 0.25 0.3 0 0.5 1 1.5 2 Distância, ηE,O (m) Magnitude, v mag Função Repulsiva v mag b = 1 b = 2 b = 3 b = 4 b = 5 b = 6 b = 7 b = 8 b = 9 b = 10
Figura 2: Magnitude da função repulsiva para b vari-ando de 1 a 10, com vmax= 2 ms−1eρ= 0,3 m.
Então, considerando a contribuição de todos os obstáculos o campo repulsivo resultante é dado por:
Ut= no
∑
i
UE,O. (22)
Na presença de múltiplos obstáculos (20) pode ser re-escrita em termos da magnitude do campo devido ao obstáculo mais próximo do efetuador vmag(ηE,Omin), gerando um campo repulsivo resultante com variações mais suaves (Flacco et al., 2012) descrito por:
Ur(ηE,Omin) = vmag(ηE,Omin)
Ut
||Ut||
. (23)
Então, considerando-se o vetor repulsivo Ur como uma velocidade repulsiva pode-se adicionar (23) na lei de controle de posição (11) como:
vp= Kpep+ ˙pd+Ur(ηE,Omin) , (24) garantindo que o efetuador realize a tarefa de interesse enquanto desvia de obstáculos presentes no seu espaço de trabalho.
4.4 Mudança da Direção do Vetor Repulsivo De acordo com (23), o campo repulsivo é base-ado apenas na direção da distância entre o efetubase-ador e o obstáculo. Então, quando o obstáculo aproxima-se do efetuador com uma velocidade maior do que a ca-pacidade de movimento do robô, pode ser difícil evi-tar colisões retraindo o efetuador na mesma direção da velocidade do obstáculo.
Uma estratégia de reação mais efetiva, similar a reação humana, consiste em escapar da colisão mo-vendo o efetuador em uma direção aproximadamente normal a velocidade do obstáculo. Esse efeito pode ser obtido utilizando-se a variação temporal do campo re-pulsivo ˙Urpara modificar a direção do vetor repulsivo na vizinhança de um ponto de interesse localizado no efetuador. O seguinte algoritmo proposto em Flacco et al. (2012) modifica a direção do vetor repulsivo de acordo com a sua variação:
Algoritmo I - Mudança da Direção do Vetor Repulsivo
~s= ˙ Ur(ηE,O) || ˙Ur(ηE,O)|| ; ~r= Ur(ηE,O)
||Ur(ηE,O)||; β= arccos(s
Tr); ifβ<π/2
~n=~s×~r; ~t =~n×~s/ ||~n×~s||;
γ=β+ β−π/2
1+ e−( || ˙Ur (ηE,O )|| (2/ ˙Ur,max)−1 ) c;
Ur,mod(ηE,O) = ||Ur(ηE,O)|| (cos(γ)~s + sin(γ)~t);
else
Ur,mod(ηE,O) =Ur(ηE,O);
end
ondeβ é o ângulo entre o vetor repulsivo e sua va-riação. Note que, apenas quando o obstáculo está se aproximando do efetuador (β<π/2), o campo
repul-sivo é modificado. Quando Ur e ˙Ur estão alinhados
(β= 0), o vetor ~n é nulo e o vetor ~t não é definido.
Nesse caso,~t é obtido ao rotacionar ˙Urdeπ/2
radia-nos e a ortogonalidade é mantida.
A direção de Ur,modé uma combinação da direção de ˙Ure da direção do vetor~t, que está no mesmo plano que Ure ˙Ur.O ânguloγ, que define essa nova direção, é igual aβ se ˙Ur= 0, e tende a π/2 se ˙Ur→ ˙Ur,max,
que é a máxima variação permitida para o campo re-pulsivo. O escalar positivo c funciona como o escalar b em (21), e modifica a inclinação da função || ˙Ur||.
5 Detecção de Obstáculos
Neste trabalho, utiliza-se um sensor de profundi-dade de um dispositivo de interface natural Microsoft KinectTMpara detectar os obstáculos presentes no es-paço de trabalho do robô, a partir de uma nuvem de pontos (vide Figura 3). Os dados obtidos pelo sen-sor fornecem uma representação geométrica 2D 1/2 do ambiente, que consiste de uma tripla(xd,yd,zd) onde
o par (xd,yd) representa as coordenadas da projeção
de um ponto cartesiano no plano e o elemento zd re-presenta a distância entre o ponto e o plano.
O sensor de profundidade pode ser modelado como uma câmera pin-hole, em termos de parâmetros intrínsecos de uma matriz Kimque modela a projeção de um ponto cartesiano no plano da imagem, e em ter-mos de parâmetros extrínsecos de uma matriz Trsque representa a transformação homogênea entre o sistema de coordenadas de referência (e.g., base do robô) e o sistema de coordenadas do sensor:
Kim= f sx 0 cx 0 f sy cy 0 0 1 , Trs= Rrs prs 0⊤ 1 ,
onde f é a distância focal da câmera,(sx,sy) são
fato-res de escalamento em pixels por milímetros,(cx,cy)
são as coordenadas do centro do plano da imagem,
(Rrs,prs) são a matriz de rotação e o vetor de
trans-lação relativos entre os sistemas de coordenadas do sensor e de referência (vide Figura 1).
As coordenadas em pixel no plano da imagem e a profundidade de um ponto cartesiano genérico Pdsão expressas no sistema de coordenadas do sensor como:
xs = ((xd− cx)zd)/ f sx,
ys = ((yd− cy)zd)/ f sy,
zs = zd.
As coordenadas (xs,ys,zs) podem ser expressas no
sistema de coordenadas de referência por:
xr yr zr = Rrs xs ys zs + prs. (25)
Note que, uma nuvem de pontos cartesianos genéricos pode ser composta por objetos próximos ou distantes do robô, bem como por elos e juntas. Então, quando o sensor de profundidade é inserido no ambiente de operação, o algoritmo de detecção pode considerar os elementos do robô como obstáculos. Neste contexto, um método é proposto para eliminar o robô do cenário obtido pelo sensor delimitando os elos e as juntas por uma sequência de esferas (vide Figura 4) de acordo com os seguintes passos:
Passo 1: Considere duas juntas consecutivas Jie Ji+1 conectadas por um elo Ljde comprimento ℓ e raio r;
Passo 2: Calcule a posição das juntas dos robôs por meio da equação de cinemática direta pJi= k(θi) para i= 1, · · · n;
Passo 3: Defina pontos equidistantes dentro do elo Lj ao longo do menor segmento de reta interligando as juntas Ji e Ji+1. O número de pontos é definido por np=
ℓ
d+1, onde d =
ℓ
λ eλ∈ N∗;
Passo 4: Use as juntas e os pontos intermediários como centros de esferas (vide Figura 4) e crie esferas em todos os elos do robô;
Passo 5: A partir do raio do elo r e da distância en-tre os pontos d, obtenha os raios das esferas rs pelo Teorema de Pitágoras: r2s= d
2
2 + r2;
Figura 3: (a) Dados obtidos pelo Kinect, (b) nuvem de pontos inicial e (c) nuvem de pontos sem o robô.
Passo 6: Remova todos os pontos da nuvem locali-zados dentro das esferas e subtraia os seus raios do cálculo da distância, tal queη′
L,O=ηL,O−rs. r ℓ rs rs d Lj Ji Ji+1 Lj Ji Ji+1 ≡
Figura 4: Método para remoção do robô da detecção de obstáculos baseado no uso de esferas.
Note que, como o sensor de profundidade do Kinect tem um alcance mínimo (0,4 m) e máximo (4,0 m), caso um objeto esteja localizado fora da faixa de detecção o valor da profundidade medida será zero, e por isso esse objeto não deve ser considerado como obstáculo. Além disso, pode-se descartar os objetos que estão fora do alcance do robô em um determinado instante de tempo. Primeiramente, os pontos locali-zados fora do espaço de trabalho do robô são remo-vidos da nuvem. Em seguida, as esferas criadas para remover o robô da nuvem de pontos são reutilizadas, aumentando-se os seus raios. Finalmente, remove-se os pontos da nuvem localizados fora das esferas.
É válido ressaltar que para delimitar completa-mente os elos e as juntas do robô a partir do mé-todo de remoção baseado em esferas deve-se usar um grande número de pontos, implicando no aumento do custo computacional e na redução do espaço de traba-lho livre de colisões. Além disso, os objetos locali-zados muito próximos do robô podem ser considera-dos como parte dele. Uma alternativa para evitar es-sas limitações é substituir as esferas por cilindros ou elipses, possibilitando a delimitação dos elos com um número menor de elementos geométricos e evitando sobreposições.
Observação 1 Uma desvantagem do método de de-tecção baseado em um único sensor de profundidade é a possibilidade de existência de zonas de oclusão ou de objetos oclusos, aumentando o risco de colisões. Uma solução alternativa para minimizar a ocorrência
de oclusão é aumentar a densidade de sensores, utili-zando mais de um sensor de profundidade com suas linhas de visão perpendiculares umas a outras, ou re-alizar uma fusão sensorial entre o Kinect e um sensor externo que forneça uma representação geométrica do ambiente na forma de uma nuvem de pontos.
6 Resultados Experimentais
Nesta seção, resultados experimentais são apre-sentados para ilustrar a viabilidade do método pro-posto. O sistema robótico consiste de um robô Mo-toman DIA10 (MoMo-toman Inc.) de dois braços com 15 graus de mobilidade (DoF, degrees of freedom), com 7-DoF por braço mais 1-DoF na base, um con-trolador industrial de baixo nível NX100 e um sistema HSC (High-speed Synchronous Controller). Nos ex-perimentos apenas um braço será utilizado (n= 8) e a
tarefa de interesse consiste em alcançar uma posição e orientação desejadas no espaço Cartesiano (m= 6),
com apenas 2 graus de redundância (n − m = 2). As referências são geradas por um computador externo, rodando Windows OS, que é conectado ao controlador NX100 pelo sistema HSC. Então, as re-ferências são enviadas para uma malha de controle de posição das juntas, que é implementada em Simulink e executada a uma frequência de 500 Hz. O acesso ao sistema HSC é realizado através do Matlab usando um protocolo de comunicação fornecido pela Robot Raconteur, uma arquitetura e biblioteca desenvolvida para aplicações de robótica e automação, distribuída pelas redes do CATS/RPI1.
Um dispositivo de interface natural Microsoft KinectTM é posicionado de frente para o robô a uma distância de 2,25 m e a uma altura de 0,96 m. Esses valores são medidos com respeito ao sistema de coor-denadas de referência fixado na base do robô. O sen-sor de profundidade do Kinect captura imagens com uma resolução de 320 × 240 pixels a uma frequên-cia de 30 Hz e os parâmetros intrínsecos do sensor são obtidos a partir de um método de calibração pro-posto em (Herrera C. et al., 2012). Os parâmetros de controle ajustados empiricamente são: Kp= 70I s−1,
Ko= 70I rad s−1, α= −10−2, kJ,O= 5, ηlim= 0,6 m.
1Center for Automation Technologies and Systems / Rensselaer
Outros parâmetros são: vmax= 0,35 m s−1,ρ= 0,6 m,
b= 9, ˙Vrmax= 1 m s−1, c= 9.
Dois experimentos foram realizados para avaliar o comportamento do robô em tarefas de regulação e ras-treamento, considerando a presença de seres humanos no seu espaço de trabalho2. No Experimento 1, o
efe-tuador do robô deve manter a sua posição e orientação originais, enquanto um operador aproxima-se do robô, fazendo-o reagir (vide Figura 5). A pose inicial do efe-tuador x0= [0,816 0,656 1,337]⊤m e os
movimen-tos realizados pelo efetuador para evitar a colisão com o operador são apresentados na Figura 6. A Figura 7 apresenta a evolução no tempo do erro de posição e da norma do erro de orientação respectivamente, onde pode-se observar a variação do erro de posição devido ao comportamento reativo do robô.
Figura 5: Sequência de imagens do robô para uma ta-refa de regulação (a) sem desvio de obstáculos e (b) com desvio de obstáculos.
0.82 0.84 0.86 0.88 0.9 0.92 0.5 0.6 0.7 1.2 1.25 1.3 1.35 1.4 x (m) y (m) z (m) x0
Figura 6: Posição inicial do efetuador x0e
movimen-tos do efetuador durante uma tarefa de regulação com desvio de obstáculos.
No Experimento 2, o efetuador movimenta-se através de quatro pontos de passagem (waypoints) for-mando uma trajetória retangular em um um plano, enquanto o operador aproxima-se constantemente do
2Um vídeo apresentando os dois experimentos pode ser
encon-trado em: http://www.youtube.com/watch?v=yYi0jCC2i2M
0 5 10 −2 0 2x 10 −3 (a) ep (m) 0 5 10 6.5 7 7.5x 10 −4 Tempo (s) ||e qv || 0 5 10 −0.2 0 0.2 (b) 0 5 10 0 2 4 6x 10 −3 Tempo (s)
Figura 7: Erro de posição e norma do erro de orien-tação para uma tarefa de regulação (a) sem desvio de obstáculos e (b) com desvio de obstáculos. Legenda: epx(−−−), epy(−−− ···), epz(−−− −−−).
robô (vide Figura 8). A evolução no tempo da posição do efetuador durante a execução da tarefa de rastrea-mento com desvio de obstáculos é apresentada na Fi-gura 9. Na FiFi-gura 10 observa-se o comportamento do erro de posição e da norma do erro de orientação res-pectivamente, devido à ação reativa do robô para evitar colisões e alcançar os waypoints simultaneamente.
Figura 8: Sequência de imagens do robô para uma ta-refa de rastreamento (a) sem desvio de obstáculos (b) e com desvio de obstáculos.
7 Considerações Finais
Neste trabalho é apresentado um método de con-trole cinemático e detecção de obstáculos para permi-tir uma interação natural, efetiva e segura entre seres humanos e robôs. O algoritmo de controle baseia-se na abordagem de campos potenciais artificiais, utili-zando a pseudo-inversa ponderada da matriz
Jacobi-0 20 40 60 0 0.5 1 1.5 Tempo (s) Posição (m) (a) 0 20 40 60 0 0.5 1 1.5 Tempo (s) (b) p x py p z
Figura 9: Evolução no tempo da posição do efetua-dor para uma tarefa de rastreamento (a) sem desvio de obstáculos e (b) com desvio de obstáculos.
0 20 40 60 −0.5 0 0.5 (a) ep (m) 0 20 40 60 0 0.005 0.01 Tempo (s) ||e qv || 0 20 40 60 −0.5 0 0.5 (b) 0 20 40 60 0 0.02 0.04 Tempo (s)
Figura 10: Erro de posição e norma do error de ori-entação para uma tarefa de rastreamento (a) sem des-vio de obstáculos e (b) com desdes-vio de obstáculos. Le-genda: epx(−−−), epy(− ···−− ), epz(−−− −−−).
ana e a característica de redundância do robô para des-viar de obstáculos e alcançar o objetivo de controle si-multaneamente. O algoritmo de detecção utiliza um sensor de profundidade baseado na tecnologia de luz estruturada para obter uma descrição geométrica 2D 1/2 do ambiente a partir de uma nuvem de pontos. Campos repulsivos são criados em torno dos obstácu-los identificados permitindo a execução bem sucedida da tarefa de interesse sem colisões. Experimentos re-alizados com um robô Motoman DIA10 e um disposi-tivo de interface natural Microsoft KinectTM demons-tram a viabilidade do método proposto.
Agradecimentos
Este trabalho foi parcialmente financiado pelo CNPq, CAPES e FAPERJ.
Referências
Bjerkeng, M., Transeth, A., Pettersen, K., Kyrkjebo, E. and Fjerdingen, S. (2011). Active camera con-trol with obstacle avoidance for remote operati-ons with industrial manipulators: Implementa-tion and experimental results, IEEE/RSJ Interna-tional Conference on Intelligent Robots and Sys-tems, pp. 247–254.
Chung, C. Y., Lee, B.-H. and Lee, J.-H. (1997). Obs-tacle avoidance for kinematically redundant ro-bots using distance algorithm, Proceedings of the
IEEE/RSJ International Conference on Intelli-gent Robots and Systems, Vol. 3, pp. 1787–1793. Flacco, F., Kroger, T., De Luca, A. and Khatib, O. (2012). A depth space approach to human-robot collision avoidance, IEEE International Confe-rence on Robotics and Automation, pp. 338–345. Haddadin, S., Albu-Schäffer, A., De Luca, A. and Hirzinger, G. (2008). Collision detection and reaction: A contribution to safe physical human-robot interaction, IEEE/RSJ Internatio-nal Conference on Intelligent Robots and Sys-tems, pp. 3356–3363.
Haddadin, S., Belder, R. and Albu-Schäffer, A. (2011). Dynamic motion planning for robots in partially unknown environments, Proceedings of the 18th IFAC World Congress, Vol. 18, Milan, Italy, pp. 6842–6850.
Herrera C., D., Kannala, J. and Heikkilä, J. (2012). Joint depth and color camera calibra-tion with distorcalibra-tion correccalibra-tion, IEEE Transac-tions on Pattern Analysis and Machine Intelli-gence 34(10): 2058–2064.
Khatib, O. (1986). Real-time obstacle avoidance for manipulators and mobile robots, The Internatio-nal JourInternatio-nal of Robotics Research 5(1): 90–98. LaValle, S. (2011). Motion planning, IEEE Robotics
Automation Magazine 18(1): 79–89.
Leite, A. C., Lizarralde, F. and Hsu, L. (2009). Hy-brid adaptive vision-force control for robot ma-nipulators interacting with unknown surfaces, The International Journal of Robotics Research 28(7): 911–926.
Minguez, J., Lamiraux, F. and Laumond, J.-P. (2008). Motion planning and obstacle avoidance, in B. Siciliano and O. Khatib (eds), Springer Hand-book of Robotics, Springer Berlin-Heidelberg, pp. 827–852.
Rakprayoon, P., Ruchanurucks, M. and Coundoul, A. (2011). Kinect-based obstacle detection for ma-nipulator, IEEE/SICE International Symposium on System Integration, pp. 68–73.
Santis, A. D., Siciliano, B., Luca, A. D. and Bic-chi, A. (2008). An atlas of physical human-robot interaction, Mechanism and Machine The-ory 43(3): 253–270.
Singh, L., Stephanou, H. and Wen, J. (1996). Real-time robot motion control with circulatory fields, Proceedings of IEEE International Conference on Robotics and Automation, Vol. 3, Minneapo-lis, MN, pp. 2737–2742.
Wen, J.-Y. and Kreutz-Delgado, K. (1991). The atti-tude control problem, IEEE Transactions on Au-tomatic Control 36(10): 1148–1162.
