mÉtodo de controle e detecÇÃo de obstÁculos para robÔs ... · palavras-chave— controle de...

MÉTODO DE CONTROLE E DETECÇÃO DE OBSTÁCULOS PARA ROBÔS MANIPULADORESAPLICADO À INTERAÇÃO HUMANO-ROBÔ

THIAGO B. DE ALMEIDA -ANTONIO, FERNANDO L IZARRALDE , L IU HSU, ANTONIO C. LEITE∗

∗Departamento de Engenharia Elétrica / COPPEUniversidade Federal do Rio de Janeiro

C.P. 68504, CEP 21941-972, Rio de Janeiro, RJ, Brasil

Email: [email protected], [fernando, liu, toni]@coep.ufrj.br

Abstract— This work presents a kinematic control and collision detection method for redundant robot manipulators operating inhuman environments. The control algorithm is based on the concept of artificial potential fields and uses the pseudo-inverse of theJacobian matrix with a weighting factor for the joint mechanical limits, taking advantage of the robot redundancy for the purposeof obstacle avoidance and control goal achievement. The detection algorithm uses a depth sensor based on the structured lightto 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 DIA10robot and a natural user interface Microsoft KinectTM illustrate 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 apseudo-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çãoutiliza um sensor de profundidade baseado na tecnologia de luz estruturada para obter uma descrição 2D 1/2 do ambiente a partirde uma nuvem de pontos. Campos repulsivos são gerados em torno dos obstáculos detectados permitindo que o robô realize atarefa de interesse sem colisões. Resultados experimentais obtidos com um robô Motoman DIA10 e um dispositivo de interfacenatural 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; Haddadinet al., 2008). Para permitir essa interação é preciso co-nhecer o ambiente ao redor do robô garantindo que omesmo se movimente sem causar danos aos operado-res próximos a ele. Portanto, o robô deve ser capazde evitar colisões com obstáculos localizados no seuespaço de trabalho e alcançar os objetivos de controlesimultaneamente.

Na área de robótica diversos métodos de plane-jamento de movimento e desvio de obstáculos forampropostos ao longo dos anos com o intuito de gerar umcaminho ou trajetória livre de colisões entre dois pon-tos de interesse (LaValle, 2011). Entretanto, como aproposta do presente trabalho é promover a interaçãoentre robôs e seres humanos em um ambiente dinâ-mico e imprevisível, o uso de técnicasoff-linenão é re-comendado, uma vez que elas consideram os modelosdo robô e do cenário perfeitamente conhecidos. Umaalternativa é utilizar técnicason-lineou reativas capa-zes de lidar com a presença de objetos desconhecidos emóveis em ambientes incertos (Minguez et al., 2008).

Uma abordagem de desvio de obstáculos bem co-nhecida é baseada no conceito de campos potenciaisartificiais, onde campos virtuais atrativos e repulsivossão criados no espaço de trabalho do robô, e dessaforma o efetuador pode mover-se na direção de umponto desejado, enquanto evita colidir com obstáculos

do ambiente (Khatib, 1986; Chung et al., 1997). Outraabordagem consiste em utilizar campos artificiais cir-culares, que rotacionam a trajetória do robô ao redordos obstáculos ao invés de apenas repelir o robô comofaz o campo potencial repulsivo (Singh et al., 1996).Campos virtuais circulares e potenciais também po-dem ser combinados a fim de aproveitar as vantagensde ambas as abordagens (Haddadin et al., 2011). Nes-ses trabalhos, os autores assumem que o movimentodos 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 baseadoem 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, quandoeles compartilham o mesmo espaço de trabalho.

No presente trabalho, apresenta-se um método decontrole cinemático e detecção de obstáculos para ma-nipuladores redundantes operando em ambientes hu-manos. O algoritmo de controle baseia-se no conceitode 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, aproveitandoa característica de redundância do robô para desviarde obstáculos. Um sensor de profundidade baseadona tecnologia de luz estruturada é utilizado para obteruma 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ô

Anais do XX Congresso Brasileiro de Automática Belo Horizonte, MG, 20 a 24 de Setembro de 2014

4036

realize a tarefa de interesse sem colisões e garantindoa segurança dos operadores. Resultados experimentaisobtidos 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 determinadatarefa em um ambiente humano, na presença de pes-soas trabalhando e movendo-se livremente dentro doespaço de trabalho do robô. A tarefa de interesse con-siste em mover o efetuador de um ponto inicialP0 atéo ponto finalPf , seguindo uma trajetória de referênciar(t), enquanto mantém uma orientação desejada vari-ante no tempo (vide Figura 1). Assume-se aqui quea tarefa é previamente planejada e que a descrição doambiente de trabalho é parcialmente conhecida.

Para adquirir uma representação geométrica 2D1/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ô poderealizar 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

Figura1: Robô evitando colisões com um operador,um DIN e os sistemas de coordenadas de interesse.

Como a maioria dos robôs industriais possui umamalha interna de controle de velocidade para aciona-mento direto das juntas, uma lei de controle puramentecinemática baseada em comandos de velocidade podeser 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 deum manipulador comn graus de mobilidade. Nestecontexto, quando o robô possui altas taxas de redu-ção nas engrenagens ou quando baixas velocidadessã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 dai-ésima junta eui éo sinal de controle de velocidade aplicado aodrive domotor dai-ésima junta. A partir do vetor de ângulodas juntasθ ∈R

n pode-se obter a pose do efetuadorx∈R

m através do mapeamento da cinemática diretak(·) como:

x=

[

pq

]

= k(θ) , (2)

ondep∈R3 é a posição do efetuador com respeito a

base do robô eq=[

qs q⊤v]

⊤

∈R4 é oquaternionuni-

tário correspondente a matriz de rotaçãoR∈ SO(3)(Wen and Kreutz-Delgado, 1991), que representa aorientação do sistema de coordenadas do efetuadorcom respeito ao sistema de coordenadas da base. Noteque,qs∈R eqv∈R

3 são as partes escalar e vetorial doquaternion, sujeitas a restriçãoq⊤q=1.

A derivada temporal de (2) resulta na seguinteequação de cinemática diferencial:

x=

[

pq

]

= Ja(θ)θ , (3)

ondeJa(θ)= ∂k(θ)∂θ ∈R

m×n é o Jacobiano analítico. Avelocidade angular do efetuadorω é relacionada à de-rivada temporal doquaternionunitário por (Wen andKreutz-Delgado, 1991):

ω = 2Jr(q)q, (4)

ondeJr(q)= [−qv qsI −Q(qv)]∈R3×4 é o Jacobiano

da representação eQ(·) é 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 2Jr(q)

]

Ja(θ)︸︷︷︸

J(θ)

θ , (5)

ondeJ(θ)∈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 cartesianavc é transformada emum sinal de controle das juntas usando:

u= J−1(θ)vc = J−1(θ)[

vp

vo

]

, (6)

ondevp e vo sã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çãobaseada na pseudo-inversa ponderada da matriz Jaco-biana pode ser utilizada:

u=W−1J⊤(J W−1J⊤)−1vc = J†wvc , (7)

ondeW∈Rn×n é uma matriz de ponderação, simétrica

positiva definida, eJ J†w= I . Note que, as relações (6)

e (7) são válidas desde quevc não conduza o robô paraconfigurações singulares, onde a matriz Jacobiana ésingular.


4037

3.1 Restrições das Juntas

Nestetrabalho, o sinal de controle (7) pode serutilizado para evitar que os movimentos do robô al-cancem os limites mecânicos das juntas. A matriz deponderaçãoW é construída de forma que quando umajunta do robô aproxima-se do seu limite, os elementosda diagonal tendem a infinito. Isso garante que as ve-locidades das juntas tendam a zero na vizinhança deum limite mecânico (Bjerkeng et al., 2011). Então, oselementos diagonais deW−1 tendem a zero conformea junta aproxima-se do seu limite, e são iguais a 1 se ajunta está no meio da sua faixa, isto é:

W−1i,i =

11+Hi(θi)2 , (8)

com

Hi(θi) =(θimax−θimin)

2(2θi −θimax−θimin)

(θi −θimax)2(θi −θimin)

2 , (9)

ondeθimax e θimin são os limites superior e inferior dai-ésima junta respectivamente.

3.2 Controle de Posição

Considere que o objetivo do controle para umadeterminada tarefa é seguir uma posição desejada va-riante no tempopd(t) a partir de uma posição inicialp, isto é:

p→ pd(t), ep = pd(t)− p → 0, (10)

ondeep ∈R3 é o erro de posição. A partir de (5) e

(6) chega-se a ˙p=vp obtendo-se a seguinte equação deerro: ep= pd−vp. Usando uma lei de controle baseadaem uma ação proporcional e um termofeedforwarddada por

vp = Kpep+ pd , (11)

ondeKp=kp I é a matriz de ganho de posição, a dinâ-mica do erro de posição é governada por ˙ep+Kpep=0.Então, escolhendo-sekp como 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 desejadavariante no tempoRd(t) a partir de uma orientação ini-cial R, isto é:

R→ Rd(t), Rq = R⊤Rd → I , (12)

ondeRq ∈ SO(3)é a matriz de erro de orientação. Arepresentação emquaternionunitário deRq é dada poreq = [eqs e⊤qv]

⊤, comeq=q−1∗qd(t), ondeq e qd são

as representações emquaternionunitário deR e Rd

respectivamente e “∗” é o operador produto dequater-nion. Note que,eq=[1 0⊤]⊤ seReRd estão alinhados.A partir de (5) e (6) chega-se aω=vo. Usando uma lei

de controle baseada em uma ação proporcional e umtermofeedforwarddada por:

vo = ωd +Koeqv, (13)

ondeKo=K⊤

o >0 é a matriz de ganho de orientaçãoe ωd ∈R

3 é a velocidade angular desejada, os pontosde equilíbrioeq = [±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ô permitegerar 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 (Bjerkenget al., 2011):

u= J†w(θ) vc+α(I −J†

w 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†w J)

projeta o vetorW−1θ0 no espaço nulo do Jacobianosem violar a restrição (5). Note que, o primeiro termode (14) corresponde a norma mínima das velocidadesdas 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)

ondeg(θ) é a função objetivo secundária (e.g., distân-cia para um obstáculo). A solução de (15) move-se aolongo da direção do gradiente da função objetivo se-cundária para maximizá-la localmente, satisfazendo oobjetivo primário.

4 Desvio de Obstáculos

Uma abordagem eficiente para planejamento demovimentoonline baseia-se no conceito de que umdeterminado ponto do robô pode mover-se no espaçocartesiano sob a influência de campos potenciais arti-ficiais. Neste contexto, campos atrativos conduzem orobô na direção de um ponto desejado, enquanto quecampos repulsivos são criados ao redor de obstáculospara 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 (Chunget al., 1997); o outro atua sobre o efetuador e mo-difica a lei de controle de posição, garantindo que omesmo siga uma trajetória de referência livre de coli-sões (Flacco et al., 2012).


4038

4.1 Desvio de Obstáculos para as Juntas

Parauma juntaJi e um obstáculoOk arbitrários,o campo potencial repulsivoUJ,O(θ) é definido por(Khatib, 1986):

UJ,O(θ)=

kJ,O2

(

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 eo obstáculo eηlim é o raio de influência do obstá-culo ou a distância limite de detecção do obstáculo.Note que, quanto menor éηJ,O(θ), maior é a intensi-dade do campo potencial repulsivo. Dessa forma, seηJ,O(θ)→0, implica queUJ,O(θ)→∞. Então, a restri-ção adicional resultante é:

θ0 =no

∑J,O

∇UJ,O(θ) , (16)

ondeno é o número de obstáculos e∇UJ,O(θ) é o gra-diente do potencial repulsivo definido por:

∇UJ,O(θ) =−

kJ,O

η2J,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 movimentos entre duas ou mais juntas. De fato,um conflito ocorre quando o campo repulsivo afastaum junta de um obstáculo, mas aproxima outra juntade um ou mais obstáculos, criando um ciclo de mo-vimentos oscilatórios. Uma solução alternativa, queresulta em movimentos mais suaves, consiste em usara distância entre o elo e o obstáculoηL,O, ao invés dadistância entre a junta e o obstáculoηJ,O na expressãodeUJ,O. ConsiderePL um ponto arbitrário localizadoem um eloL j do robô, conectando dois pontosP1 eP2 localizados em duas juntas consecutivasJ1 eJ2 res-pectivamente. A posição do pontoPL expressa em umsistema de coordenadas inercial é dada por:

pL = p1+λ (p2− p1) , (17)

ondep1 e p2 são as posições dos pontosP1 eP2 respec-tivamente, eλ >0 é uma constante paramétrica a serdefinida. A distância entre um obstáculo pontualOk eum eloL j pode ser obtida pelo cálculo da distância en-tre esse obstáculo e o ponto mais próximo localizadono elo. A partir de (17), o valor deλ que correspondeao ponto mais próximo do eloL j é obtido por:

λ =−

(p1− po) · (p2− p1)

|p2− p1|2 , (18)

ondepo é a posição do obstáculo. A expressão parao valor depL 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áculoOk arbitrário é definidopor (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(θ) eda 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 eb> 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.30

0.5

1

1.5

2

Distância, ηE,O

(m)

Mag

nitu

de, v

mag

Função Repulsiva vmag

b = 1b = 2b = 3b = 4b = 5b = 6b = 7b = 8b = 9b = 10

Figura 2: Magnitude da função repulsiva parab vari-andode 1 a 10, comvmax=2 ms−1 e ρ =0,3m.

Então, considerando a contribuição de todos osobstá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 aoobstáculo mais próximo do efetuadorvmag(ηE,Omin

),gerando um campo repulsivo resultante com variaçõesmais suaves (Flacco et al., 2012) descrito por:

Ur(ηE,Omin) = vmag(ηE,Omin

)Ut

||Ut ||. (23)

Então, considerando-se o vetor repulsivoUr comouma velocidade repulsiva pode-se adicionar (23) na leide controle de posição (11) como:

vp = Kpep+ pd +Ur(ηE,Omin) , (24)

garantindo que o efetuador realize a tarefa de interesseenquanto desvia de obstáculos presentes no seu espaçode trabalho.


4039

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 efetuador eo obstáculo. Então, quando o obstáculo aproxima-sedo 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 davelocidade do obstáculo.

Uma estratégia de reação mais efetiva, similar areação humana, consiste em escapar da colisão mo-vendo o efetuador em uma direção aproximadamentenormal a velocidade do obstáculo. Esse efeito pode serobtido utilizando-se a variação temporal do campo re-pulsivoUr para modificar a direção do vetor repulsivona vizinhança de um ponto de interesse localizado noefetuador. O seguinte algoritmo proposto em Flaccoet al. (2012) modifica a direção do vetor repulsivo deacordo 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(sT r);

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á seaproximando do efetuador (β <π/2), o campo repul-sivo é modificado. QuandoUr e Ur estão alinhados(β = 0), o vetor~n é nulo e o vetor~t não é definido.Nesse caso,~t é obtido ao rotacionarUr deπ/2 radia-nos e a ortogonalidade é mantida.

A direção deUr,mod é uma combinação da direçãodeUr e da direção do vetor~t, que está no mesmo planoqueUr eUr .O ânguloγ, que define essa nova direção,é igual aβ seUr =0, e tende aπ/2 seUr → Ur,max,que é a máxima variação permitida para o campo re-pulsivo. O escalar positivoc funciona como o escalarb 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 MicrosoftKinectTM para detectar os obstáculos presentes no es-paço de trabalho do robô, a partir de uma nuvem depontos (vide Figura 3). Os dados obtidos pelo sen-sor fornecem uma representação geométrica 2D 1/2 doambiente, que consiste de uma tripla(xd,yd,zd) ondeo par (xd ,yd) representa as coordenadas da projeçãode um ponto cartesiano no plano e o elementozd re-presenta a distância entre o ponto e o plano.

O sensor de profundidade pode ser modeladocomo uma câmerapin-hole, em termos de parâmetrosintrínsecos de uma matrizKim que modela a projeçãode um ponto cartesiano no plano da imagem, e em ter-mos de parâmetros extrínsecos de uma matrizTrs querepresenta a transformação homogênea entre o sistemade coordenadas de referência (e.g., base do robô) e osistema 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 dosensor e de referência (vide Figura 1).

As coordenadas em pixel no plano da imagem e aprofundidade de um ponto cartesiano genéricoPd sãoexpressas 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 nosistema de coordenadas de referência por:

xr

yr

zr

= Rrs

xs

ys

zs

+ prs . (25)

Note que, uma nuvem de pontos cartesianos genéricospode ser composta por objetos próximos ou distantesdo robô, bem como por elos e juntas. Então, quandoo sensor de profundidade é inserido no ambiente deoperação, o algoritmo de detecção pode considerar oselementos do robô como obstáculos. Neste contexto,um método é proposto para eliminar o robô do cenárioobtido pelo sensor delimitando os elos e as juntas poruma sequência de esferas (vide Figura 4) de acordocom os seguintes passos:

Passo 1:Considere duas juntas consecutivasJi e Ji+1

conectadas por um eloL j de comprimentoℓ e raior;

Passo 2:Calcule a posição das juntas dos robôs pormeio da equação de cinemática diretapJi =k(θi) parai = 1,· · · n;

Passo 3:Defina pontos equidistantes dentro do eloL j

ao longo do menor segmento de reta interligando asjuntasJi e Ji+1. O número de pontos é definido pornp=

(

ℓ

d

)

+1, onded=(

ℓ

λ)

e λ ∈N∗;

Passo 4: Use as juntas e os pontos intermediárioscomo centros de esferas (vide Figura 4) e crie esferasem todos os elos do robô;

Passo 5:A partir do raio do elor e da distância en-tre os pontosd, obtenha os raios das esferasrs pelo

Teorema de Pitágoras:r2s =(

d2

)2+ r2;


4040

Figura 3: (a) Dados obtidos pelo Kinect, (b) nuvem de pontos iniciale (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 docálculo da distância, tal queη ′

L,O=ηL,O−rs.

r

ℓ

rs rs

d

L jJi Ji+1

L j

Ji

Ji+1

≡

Figura 4: Método para remoção do robô da detecçãode obstáculos baseado no uso de esferas.

Note que, como o sensor de profundidade doKinect tem um alcance mínimo (0,4m) e máximo(4,0m), caso um objeto esteja localizado fora da faixade detecção o valor da profundidade medida será zero,e por isso esse objeto não deve ser considerado comoobstáculo. Além disso, pode-se descartar os objetosque estão fora do alcance do robô em um determinadoinstante 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 pararemover o robô da nuvem de pontos são reutilizadas,aumentando-se os seus raios. Finalmente, remove-seos 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 umgrande número de pontos, implicando no aumento docusto 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 ouelipses, possibilitando a delimitação dos elos com umnúmero menor de elementos geométricos e evitandosobreposições.

Observação 1Uma 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 oude 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 suaslinhas de visão perpendiculares umas a outras, ou re-alizar uma fusão sensorial entre o Kinect e um sensorexterno que forneça uma representação geométrica doambiente 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 (Motoman Inc.) de dois braços com15 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 sistemaHSC (High-speed Synchronous Controller). Nos ex-perimentos apenas um braço será utilizado (n=8) e atarefa de interesse consiste em alcançar uma posiçãoe 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 computadorexterno, rodando Windows OS, que é conectado aocontrolador NX100 pelo sistema HSC. Então, as re-ferências são enviadas para uma malha de controle deposição das juntas, que é implementada em Simulinke executada a uma frequência de 500Hz. O acessoao sistema HSC é realizado através do Matlab usandoum protocolo de comunicação fornecido pelaRobotRaconteur, uma arquitetura e biblioteca desenvolvidapara aplicações de robótica e automação, distribuídapelas redes do CATS/RPI1 .

Um dispositivo de interface natural MicrosoftKinectTM é posicionado de frente para o robô a umadistância de 2,25m e a uma altura de 0,96m. Essesvalores 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 comuma resolução de 320× 240pixels a uma frequên-cia de 30Hz e os parâmetros intrínsecos do sensorsão obtidos a partir de um método de calibração pro-posto em (Herrera C. et al., 2012). Os parâmetros decontrole ajustados empiricamente são:Kp= 70I s−1,Ko = 70I rad s−1, α =−10−2, kJ,O = 5, ηlim = 0,6m.

1Centerfor Automation Technologies and Systems / RensselaerPolytechnic Institute, Troy, NY, USA - http://www.cats.rpi.edu/


4041

Outros parâmetros são:vmax=0,35ms−1, ρ =0,6m,b=9, Vrmax=1 ms−1, c=9.

Dois experimentos foram realizados para avaliar ocomportamento do robô em tarefas de regulação e ras-treamento, considerando a presença de seres humanosno seu espaço de trabalho2 . No Experimento 1, o efe-tuador do robô deve manter a sua posição e orientaçãooriginais, enquanto um operador aproxima-se do robô,fazendo-o reagir (vide Figura 5). A pose inicial do efe-tuadorx0=[0,816 0,656 1,337]⊤ m e os movimen-tos realizados pelo efetuador para evitar a colisão como operador são apresentados na Figura 6. A Figura 7apresenta a evolução no tempo do erro de posição eda norma do erro de orientação respectivamente, ondepode-se observar a variação do erro de posição devidoao 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.820.84

0.860.88

0.90.92

0.5

0.6

0.71.2

1.25

1.3

1.35

1.4

x (m)y (m)

z (m

)

x0

Figura 6: Posição inicial do efetuadorx0 e movimen-tos do efetuador durante uma tarefa de regulação comdesvio de obstáculos.

No Experimento 2, o efetuador movimenta-seatravé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)

e p (m

)

0 5 106.5

7

7.5x 10

−4

Tempo (s)

||eqv

||

0 5 10−0.2

0

0.2(b)

0 5 100

2

4

6x 10

−3

Tempo (s)

Figura 7: Erro de posição e norma do erro de orien-taçãopara uma tarefa de regulação (a) sem desvio deobstáculos e (b) com desvio de obstáculos. Legenda:epx (−−− ), epy (−−−···), epz (−−−−−−).

robô(vide Figura 8). A evolução no tempo da posiçãodo efetuador durante a execução da tarefa de rastrea-mento com desvio de obstáculos é apresentada na Fi-gura 9. Na Figura 10 observa-se o comportamento doerro de posição e da norma do erro de orientação res-pectivamente, devido à ação reativa do robô para evitarcolisões e alcançar oswaypointssimultaneamente.

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 sereshumanos e robôs. O algoritmo de controle baseia-sena abordagem de campos potenciais artificiais, utili-zando a pseudo-inversa ponderada da matriz Jacobi-


4042

0 20 40 600

0.5

1

1.5

Tempo (s)

Pos

ição

(m

)

(a)

0 20 40 600

0.5

1

1.5

Tempo (s)

(b)

px

py

pz

Figura 9: Evolução no tempo da posição do efetua-dorpara uma tarefa de rastreamento (a) sem desvio deobstáculos e (b) com desvio de obstáculos.

0 20 40 60−0.5

0

0.5(a)

e p (m

)

0 20 40 600

0.005

0.01

Tempo (s)

||eqv

||

0 20 40 60−0.5

0

0.5(b)

0 20 40 600

0.02

0.04

Tempo (s)

Figura 10: Erro de posição e norma do error de ori-entaçãopara uma tarefa de rastreamento (a) sem des-vio de obstáculos e (b) com desvio de obstáculos. Le-genda:epx (−−− ), epy (−−−···), epz (−−−−−−).

anae 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 umsensor de profundidade baseado na tecnologia de luzestruturada para obter uma descrição geométrica 2D1/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 sucedidada 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-robotcollision 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 detectionand reaction: A contribution to safe physicalhuman-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 inpartially unknown environments,Proceedings ofthe 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 distortion correction,IEEE Transac-tions on Pattern Analysis and Machine Intelli-gence34(10): 2058–2064.

Khatib, O. (1986). Real-time obstacle avoidance formanipulators and mobile robots,The Internatio-nal Journal of Robotics Research5(1): 90–98.

LaValle, S. (2011). Motion planning,IEEE RoboticsAutomation Magazine18(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 Research28(7): 911–926.

Minguez, J., Lamiraux, F. and Laumond, J.-P. (2008).Motion planning and obstacle avoidance,inB. 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 Symposiumon 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 Conferenceon 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 Control36(10): 1148–1162.


4043

mÉtodo de controle e detecÇÃo de obstÁculos para robÔs ... · palavras-chave— controle de...

Documents