UM SISTEMA DE ACOMPANHAMENTO VISUAL PARA ROBOS MOVEIS SEMI-AUTONOMOS EM AMBIENTES SEMI-ESTRUTURADOS Ricardo Figueiredo Machado, Paulo Fernando Ferreira

ra Rosa, Diego Felix de Almeida

Instituto Militar de Engenharia, Seo de Engenharia de Defesa, ca Praa General Tibrcio 80, Praia Vermelha c u Rio de Janeiro, RJ, Brasil

Emails: rfm61@hotmail.com, rpaulo@ime.eb.br, diegobill@gmail.com

Abstract This paper presents a visual tracking system of targets by wheeled mobile robots on semistructured environments(Sistema de Acompanhamento Visual de Alvos por Rob - SAVAR), capable of cono trolling two degrees of freedom robot and a network camera loaded. Two controllers are used, one for the robotic platform and another for the camera, in order to allow the independence movements of the camera and the robot in performing the task of tracking with obstacle avoidance. The conguration in the closed loop system is performed under a WLAN(Wireless Local Area Network) technology standard 802.11b/g. In this approach, the major computational cost is achieved by a computer (host) and low computational cost algorithms are embedded in the hardware of the robot. Inertial sensors (gyroscopes, digital compass and accelerometer) are used as support to the guidance and navigation. Keywords Mobile robot, Computer vision, Inertial sensors, Embedded system.

Resumo Este trabalho apresenta um Sistema de Acompanhamento Visual de Alvo por Rob com rodas o (SAVAR) para ambientes semi-estruturados, capaz de controlar dois graus de liberdade do rob e de uma cmera o a de rede embarcada. Dois controladores so usados, um para a plataforma robtica e outro para a cmera, a o a com o objetivo de permitir a independncia de movimentos da cmera e do rob na execuo da tarefa de e a o ca acompanhamento com desvio de obstculos. A conguraao do sistema em malha fechada realizada sob uma a c e WLAN (Wireless Local Area Network) com tecnologia padro 802.11b/g. Nesta abordagem o maior custo a computacional realizado por um computador (host) e algoritmos de baixo custo computacional embarcados e no hardware do rob. So empregados os sensores inerciais (giroscpios, compasso digital e acelermetro) como o a o o suporte ` orientao e navegao. a ca ca Palavras-chave Robs mveis, Viso computacional, Sensores inerciais, Sistemas embarcados. o o a

Introduo ca

Para muitas espcies vivas a percepo visual dee ca sempenha um papel fundamental em seu comportamento. A capacidade de coordenao do tipo ca mo-olho d a exibilidade, a destreza e robusa a tez de movimento que uma mquina ainda no a a pode executar. O ser humano ao rastrear um alvo mvel concentra boa parte da ateno no objeto o ca de interesse, mas mantm relativa percepo do e ca entorno para deteco dos objetos mveis e estca o a ticos, quando na sua conduo em direo ao alvo ca ca aplicando mtodos distintos para rastrear um obe jeto, navegar e evitar colises. A viso do rob o a o para o cumprimento de suas tarefas se estende ao campo da viso computacional, no sendo uma a a entidade isolada, mas parte de um sistema mais complexo. Com isso as informaes visuais foram co inclu das na malha de realimentao das formuca laes de controle da postura do rob ( controle co o servo visual), (Hutchinson et al., 1996). O artigo est estruturado em sete sees distintas: a seo a co ca II apresenta uma s ntese de trabalhos correlatos; a seo III trata do enunciado do problema; a seca o IV apresenta a viso geral do Sistema, a seca a o V os aspectos tericos do desenvolvimento de ca o um rob experimental, o controle da cmera e sua o a eletrnica embarcada, a seo VI descreve o subo ca

sistema de acompanhamento, sua metodologia e simulaes ao nal, na seo VII a concluso e as co ca a referncias. e 2 Trabalhos correlatos

Um sistema de deteco e rastreamento visual de ca faces humanas em tempo real, para um rob huo manoide, foi proposto por (Tsai and Song, 2005), usando um computador embarcado baseado de gradientes de intensidade e histogramas de cores. As imagens so transmitidas por sinal de rdio a a para um computador, que processa o algoritmo de viso, gerencia e executa o rastreamento visual. a O desempenho do sistema consumiu 112ms para processamento da imagem a taxas de 9, 8 quadros/segundo e 0, 21 segundo/imagem para rastrear. Trabalhos recentes como (Tsai et al., 2009) e (Felser, 2005) utilizam o plano da imagem para rastreamento visual. O primeiro baseado na estie mativa de erro e controle robusto baseado (mtodo e direto de Lyapunov). O ultimo utiliza uma dual jacobiana, de forma ` acoplar o controle do modelo a do rob e do alvo. Como estimador foi utilizado o um ltro de Kalman auto ajustvel. O modelo a robusto em relao as leis de controle, entree ca tanto tem restries, devido a distribuio Gausco ca siana incerta e taxa de amostragem uniforme. Um

rastreador usando uma cmera de rede PTZ(Pan, a Tilt e Zoom) em tempo real apresentado por e (Li et al., 2007), utilizando ltros de part culas em duas fases de multi-escala, juntamente com o algoritmo de KLT e um mecanismo de multithreading. Com base nos resultados de trabalhos recentes e o fato de no existir um sistema para a rastreamento visual para os robs do laboratrio o o do IME, surgiu a inspirao para a presente traca balho. 3 Apresentao do Problema ca

No cenrio semi-estruturado, resolveremos o proa blema de acompanhamento visual por um rob o mvel terrestre dotado apenas de uma cmera o a como seu unico sistema sensorial. Na maioria das atividades industriais automatizadas o rob tem o conhecimento exato de sua posio, da posio dos ca ca objetos e obstculos, operando assim em ambiena tes completamente estruturados, a exemplo dos robs industriais utilizados em linha de montao gem. Por ambiente estruturado entende-se, um ambiente no qual algumas pr-condies podem e co ser estabelecidas e garantidas, como o tipo de solo para navegao, que seja adequado a anatomia do ca rob e a existncia de marcos conveis ` navegao e a a o, entre outros. Em ambientes onde estas condica es iniciais no podem ser totalmente garantidas co a e respeitados, semi-estruturado, um certo grau de autonomia deve ser dado ao sistema de controle do rob, para que a execuo da tarefa seja realizada o ca com segurana. Tambm nesses ambientes func e e damental que a resposta do sistema robtico seja o adequada ` dinmica no-linear do meio e ` sua a a a a interao em tempo real. Assim se faz necessrio ca a um aumento da quantidade de sensores para interao com o meio, o que acarreta um aumento dos ca custos e maior complexidade do sistema robtico. o 4 Viso Geral do Sistema a

de robs e reutilizao do sistema com pouca ou o ca nenhuma alterao no hardware do rob e no softca o ware embarcado. Estes incluem ainda redues co substanciais no projeto e nos esforos futuros para c desenvolvimento de novos controladores, pela utilizao de componentes padronizados (hardware e ca software). Potencialmente amplas vantagens operacionais surgem, em especial a de operao reca mota de qualquer local com ligao ` Internet. O ca a sistema exerce, a priori, as seguintes funes bsico a cas: detecta e estima um alvo e interesse; desvia de obstculos e exerce o controle cinemtico do rob a a o para acompanhamento do alvo, usando a imagem como unico sensor do rob. o

Figura 1: Subsistema de acompanhamento visual

Subsistema Rob-Cmera o a

Neste trabalho propomos um Sistema de Acompanhamento de Alvo mvel por Rob (SAVAR) cao o paz de controlar dois graus de liberdade do rob o e de uma cmera embarcada. So usados dois a a controladores: um para a plataforma robtica e o outro para a cmera. O sistema composto por a e dois subsistemas com controladores distintos, guras 1 e 2. E operado sob uma rede ethernet sem o e usa uma cmera de rede. Nesta abordaa gem o maior custo computacional ser realizado a por um computador (host) e algoritmos de baixo custo computacional embarcados no hardware do rob. Esta congurao permite a independncia o ca e de movimentos da cmera e do rob quando na a o execuo da tarefa de acompanhamento com desca vio de obstculos. O uso da tecnologia WLAN e a protocolos TCP/IP, introduz benef cios considera veis ao sistema pela possibilidade de tele-operao ca

Neste subsistema so realizadas a captura da imaa gem do alvo, de obstculos, controle do rob e do a o mecanismo PTZ. Duas portas independentes do mdulo de comunicao WiFi so usadas para o o ca a uxo de dados do mdulo de controle dos motores o e da cmera de rede para um computador cliente, a onde as realimentaes das plantas so realizadas co a sob a rede ethernet. 5.1 Rob o

Para servir de plataforma experimental ao SAVAR, um rob mvel do tipo (2,0) foi desenvolo o vido e constru do. O prottipo possui uma eso trutura mecnica simples, comumente utilizada a em pesquisa de navegao robtica, dotada soca o mente de rodas convencionais, sendo duas rodas dianteiras xas(motoras) e uma traseira orienta vel centrada(tipo bola esfrica). A gura 3 iluse tra o prottipo. Para a descrio de sua posio ca o toma-se por base o plano do seu movimento. ca Nele xado uma base inercial ortonormal arbie a trria {0, X i , Z i }. Dene-se um ponto arbitra rio P sobre o quadro do robo na base arbitra ria {0, X m , Z m }, aplicada ao seu quadro (de Wit

 completamente descrita pelos vetores de coore T , c (t) e denadas (t)T = x(t) z(t) (t) f (t) c (t) , respectivamente a (t)T = postura rob, a orientao e rotao das rodas. o ca ca As Equaes 2 e 3 podem ser reescritas sob uma co forma matricial (de Wit et al., 1996), (Siegwart and Nourbakhsh, 2004) e (Campion et al., 1996): J1 (c )R() + J2 = 0 C1 (c )R() = 0 (4) (5)
T

Para a digiribilidade do rob somente as Nf e as o Nc tm impacto sobre a cinemtica do seu chassi, e a (Siegwart and Nourbakhsh, 2004), assim temos explicitamente as restries da Equao 5 como: co ca Figura 2: Subsistema Rob-Cmera o a C1f (c )R() = 0 (6)

Em (Campion et al., 1996) uma deduo usando ca o modelo Lagrangiano apresentado. Para o rob e o SAVAR foi adotado o controle a partir de uma o tica cinemtica, (de Wit et al., 1996) e (Siegwart a and Nourbakhsh, 2004), pela simplicidade e estabilidade global garantida. 5.2 Controle da cmera e rastreamento a

Figura 3: Rob experimental SAVAR o et al., 1996). A posio completamente denida ca e por trs variveis x, y e , onde x e y so as coore a a denadas do ponto de referncia P na base inercial e e o ngulo de orientao da base {0, X m , Z m } e a ca em relao ` {0, X i , Z i }. A sua postura e matriz ca a de rotao ortogonal em relao ao sistema inerca ca cial so denidas por: a x cos sin 0 = z e R() = sin cos 0 (1) 0 0 1 As restries de rolamento e ortogonal ` direo co a ca de rolagem, respectivamente, resulta em:
[ sin( + ) cos( + )l cos ] R() + r = 0 (2)

O objetivo do controlador alinhar no espao e c 3D o eixo tico com o centro do objeto, onde o o movimento do objeto no espao 3D encarado c e como uma perturbao com dinmica desconheca a cida. A estratgia de unir dois paradigmas de e controle: o servo controle e o mtodo baseado e em aparncia, melhora a compreenso do proe a cesso de acompanhamento. Ao associar as entradas e sa das do sistema com a informao vica sual relega-se para um plano secundrio os aspeca tos mecnicos e cinemticos do sistema. Por esa a tas razes, este trabalho usa o paradigma de reo gulao para modelar o sistema de acompanhaca mento visual do SAVAR, onde a formulao do ca controlador de estado da cmera S denido por a e S(t) = pant(t) til(t) zoom(t) e pode ser alterado por: S(t + 1) = S(t) + S(t) (7)

[cos( + ) sin( + )l sin ] R() = 0

(3)

No rob ==450 , =137mm, rf (raio das roo das motoras)=50mm e a terceira tipo bola esfe rica(omnidirecionais) no possui um eixo princia pal de rotaao, no impondo restries sobre a c a co cinemtica do chassi do rob. O nmero total a o u de conguraes de coordenadas sete. Usando co e o subscrito f e c respectivamente, como referne cia para rodas motoras e esfrica tipo bola do e rob temos que, a congurao temporal do rob o ca o

Considerando o centro do alvo S S0 e os na gulos (PAN) e (TILT) com o eixo tico 0, o adotamos u(t) = KP err(t), onde kP o ganho e do controlador e err(t) o erro de estado. A no a introduo de um termo integral pode tornar o sisca tema oscilante e na prtica fazer a planta-cmera a a responder de forma lenta para um movimento cont nuo do alvo. Assim, um termo de realimentao ca negativo introduzido para truncar o controle evie tando a oscilao e conseguir uma resposta rpida, ca a (Dinh et al., 2009): (x, ) = x 0 if x >= if x <

Teremos que: x C x , 1 ) K1 ( f Cy S(t) = K2 y , 2 ) f S K3 (( 1, , 3 )) S0 (8)

6.1

Rastreador Visual

Para o SAVAR so usados trs tipos de comandos a e divididos em dois grupos distintos, o primeiro para alterar a velocidade de PAN e TILT, adequando a velocidade da cmera ` do alvo (alta prioridade) a a e um segundo, de baixa prioridade, para obteno de status e ajuste da distncia focal. O envio ca a de constantes comandos para a cmera de rede a incrementa a taxa de comunicao e perda de coca mandos(la queue). A soluo adotada para este ca problema foi estabelecer uma estratgia de timee sharing, agrupando comandos como por exemplo: POST/(PAN TILT SPEED), GET/(PAN TILT SPEED) e POST/GET(ZOOM).

O objetivo do rastreamento visual estabelecer e um mtodo de controle servo visual da cmera e a num alvo de interesse. Essas informaes tamco bm so usadas pelo controlador de acompanhae a mento visual na conduo do SAVAR. A formuca lao dessa estratgia , em parte, baseado no ca e e trabalho de (Freda and Oriolo, 2007). O autor T demonstra que considerando = (x y ) o vetor de coordenadas do centride do alvo no plano da o imagem, por uma simples anlise do modelo geoa mtrico do ngulo (PAN), temos que a posio e a ca do alvo no plano da imagem dependente exclue sivamente desse valor, gura 4.

5.3

Eletrnica embarcada o

O projeto do hardware usa mdulos padro da ino a dstria Wi-Fi IEEE 802.11a/b/g (MatchPort b/g u da Lantronix R , Inc.) para implementao de coca nectividade sem o com a Ethernet/Internet. O controle principal do rob baseado no processaoe dor LPC2148, com um core ARM7 de 60Mhz, para controle dos motores e comunicao. Os sensoca res inerciais so: um sensor HMC5843 (magnetoa resistivo) combinado com ADXL345 (acelermeo tro), um giroscpio duplo IDG-650 (eixo X e Y) e o um IDZ-650 (eixo Z). A interface de rede e mdulo o de controle principal so personalizados. a

Figura 4: Dependencia de do PAN As equaes seguintes evidenciam esta relao: co ca x = f tanh( ) f ( ) x = cos2 ( ) (9) (10)

Onde a escolha da velocidade do PAN em malha fechada garante a convergncia exponencial de x e para zero: = + K x , para K > 0 (11)

Subsistema de acompanhamento visual

Rastreamento de objetos com base na cor um e dos mtodos mais rpidos e fceis de implementae a a o. A velocidade dessa tcnica torna muito atraca e entes para aplicaes em tempo real, mas devido co a ` sua simplicidade existem muitas questes que o podem causar o fracasso do acompanhamento. O processo realizado de forma que na imagem cape turada realizado um pr-processamento com a e e aplicao de um ltro RGB para escala de cinca zas. Quando o alvo est longe do rob sua imagem a o muito pequena e poucos pixel so ativos o que e a torna o ltro RGB mais sens ao ru de fundo. vel do Para contornar esse problema usado o ltro de e mdia (mean lter) de tamanho 12, que provoca e o embassamento da imagem e suaviza a mdia em e torno do valores do pixel. Determinado o tamanho relativo do objeto, a transformada Hough usada e para diferencia-lo dos demais objetos no cenrio, a alm de fornece as coordenadas do seu centro. e

O termo exgeno (feedforward), que surge na o relao dependente do movimento relativo insca e tantneo do alvo e rob. Este, de certo modo, a o incrementa uma compensao para o movimento ca relativo cmera-rob e estabelece um n de sea o vel parao de suas dinmicas e pode ser numericaca a mente calculado por: = + arctan( x ) f (12)

Equivalente mtodo aplicado no plano vertical ao e a ngulo (TILT), gura 5, tem tambm garane tido a convergncia exponencial de y para zero, e onde o ngulo entre o eixo horizontal e a linha e a que passa pelo eixo focal da cmera. A distncia a a relativa do alvo estimada por: b= h ) tanh( (13)

6.2

Modelando o deslocamento do alvo

Figura 5: Posio relativa do alvo - vista lateral ca

Na gura 6, o erro de apontamento do SAVAR (erro SAV AR = ) e suas projees so obtidos co a como segue: erro
2

Diferente de (Freda and Oriolo, 2007), todo o controle do SAVAR realizado sobre os valores abe solutos da posio e velocidade alvo e do rob, ca o obtidos por transformaes de rotao e translaco ca o em relao sistema inercial. Considerando a ca ca posio inicial do rob, aps a primeira captura ca o o do alvo, um observador preditor conduz o rob a o posio absoluta do alvo no instante t + 1. Obtica dos a velocidade e acelerao do alvo, temos a sua ca posio P ca os: 1 P x,y+t = P x,y + velx,y + acelx,y t2 (16) os os 2 Decompondo suas componentes linear e angular e colocando na forma de espao de estados xk = c k,k1 xk1 + wk1 e Zk = Hk xk + vk , para T xk = xk yk k k k , temos a matriz de transio de estados k,k1 como: ca
1 0 0 0 0 0 1 0 0 0 0 0 1 0 0 t cos(k1 ) t sin(k1 ) o 1 0 1 k1 t2 sin(k1 ) 2 1 k1 t2 cos(k1 ) (17) 2 t 0 1

= b2 + d2 2 b d cos(pi ) (14)

onde pelo teorema de Carnot temos que: = arcsin b sin(pi ) erro (15)

Considerando o deslocamento do alvo e do rob, o

O ru do do processo, o vetor de medidas, a relao entre as medidas e o vetor de estados ca e os erros de medidas direcional (x e y), so respectivamente representados por wk1 = a T T 0 0 0 v , Zk = xk yk , vk = e 1 0 0 0 0 H= . 0 1 0 0 0 Dada as incertezas dos dados de entrada (informaes de imagem) e o estado de transio de um co ca objeto em movimento a estratgia de utilizar um e ltro de Kalman como observador de estado aqui e aplicada. O ltro minimiza o erro de estimao alca terando o modelo de transio de estado, uma vez ca que o vetor de estado pode ser estimado sobre o plano da imagem. 6.3 Simulao ca x y
T

Figura 6: Posio relativa do alvo - vista topo ca gura 7, a cada quadro de imagem, por uma aproximao backward, a velocidade do alvo em ca t = t2 estimada por valvo P 1P 2 e para e T o SAVAR, por Runge-Kutta de segunda ordem x v1 T cos 1 T , y v1 T sin 1 T e T .

A simulao de acompanhamento do alvo pelo SAca VAR foi realizada no MatLab e um software de controle, gura 8, foi desenvolvido em C#. Os dados de odometria, tenso da bateria e corrente a so obtidos em tempo real. Para simulao o alvo a ca e o rob so posicionados, por sorteio, com veloo a cidades mximas limitadas executando o alvo um a movimento MRUV. A gura 9 e 10 apresentam um resultado obtido. 7 Concluses o

Figura 7: Estimativa da velocidade do alvo

Neste trabalho apresentamos um conceito de um sistema de acompanhamento visual e mdulo de o controle para robs mveis sobre rodas com realio o mentao da planta sobre uma rede Ethernet sem ca

Agradecimentos Este trabalho est sendo realizado como parte de a requisito parcial para obteno do t ca tulo de Mestre em Engenharia de Defesa, no curso de Engenharia de Defesa do Instituto Militar de Engenharia. Os autores agradecem ao Sr. Gilberto Figueiredo Machado, da ITEG - Aao e Ferramentaria, pela ca usinagem das partes mecnicas do rob. a o Referncias e Figura 8: Software preliminar do SAVAR Campion, G., Bastin, G. and Dandrea-Novel, B. (1996). Structural properties and classication of kinematic and dynamic models of wheeled mobile robots, Robotics and Automation, IEEE Transactions on 12(1): 4762. de Wit, C. C., Bastin, G. and Siciliano, B. (1996). Theory of Robot Control, SpringerVerlag New York, Inc., Secaucus, NJ, USA. Dinh, T., Yu, Q. and Medioni, G. (2009). Real time tracking using an active pan-tilt-zoom network camera, Intelligent Robots and Systems, 2009. IROS 2009. IEEE/RSJ International Conference on, pp. 37863793. Figura 9: Simulao do controle cinemtico ca a Felser, M. (2005). Real-time ethernet - industry prospective, Proceedings of the IEEE 93(6): 11181129. Freda, L. and Oriolo, G. (2007). Vision-based interception of a moving target with a nonholonomic mobile robot, Robot. Auton. Syst. 55(6): 419432. Hutchinson, S., Hager, G. and Corke, P. (1996). A tutorial on visual servo control, IEEE Transactions on Robotics and Automation 12: 651670. Li, Y., Ai, H., Yamashita, T., Lao, S. and Kawade, M. (2007). Tracking in low frame rate video: A cascade particle lter with discriminative observers of dierent lifespans, Computer Vision and Pattern Recognition, 2007. CVPR 07. IEEE Conference on, pp. 18. Siegwart, R. and Nourbakhsh, I. R. (2004). Introduction to Autonomous Mobile Robots, Bradford Company, Scituate, MA, USA. Tsai, C.-Y. and Song, K.-T. (2005). Robust visual tracking control of mobile robots based on an error model in image plane, Vol. 3, pp. 1218 1223. Tsai, C.-Y., Song, K.-T., Dutoit, X., Van Brussel, H. and Nuttin, M. (2009). Robust visual tracking control system of a mobile robot based on a dual-jacobian visual interaction model, Robot. Auton. Syst. 57(6-7): 652664.

Figura 10: Simulao do movimento das rodas ca

o WiFi. Uma unidade de controle para o rob o foi testada e programas escritos para teste de suas funes bsicas, mostrando-se completamente funco a cional. Ela pode se comunicar com xito via WiFi e em RS232, recebendo comandos e enviando de dados dos sensores a partir de mdulos padronizados o ethernet/serial. Um rob mvel diferencial expeo o rimental foi desenvolvido para uso pelo sistema e um algoritmo preliminar de acompanhamento visual, baseado somente na cor e forma, foi testado com sucesso. Entre os trabalhos futuros esto a a validao do modelo e a implementao do algoca ca ritmo de deteo de obstculos, habilitando o rob ca a o ao acompanhamento e navegao em ambientes ca semi-estruturados.