CONTROLADOR SERVO-VISUAL APLICADO AO LÍDER DE UMA EQUIPE DE ROBÔS COM CONTROLE DE FORMAÇÃO CENTRALIZADO Douglas Almonfrey∗, Raquel Frizera Vassallo∗ ∗ Dpto. de Engenharia Elétrica, Universidade Federal do Espı́rito Santo Av. Fernando Ferrari 514 Vitória, ES, Brasil Emails: [email protected], [email protected] Abstract— This work implements a visual-servo control that generates motor commands for a mobile robot according to a visual target. This robot is also the leader of a robot team that uses a centralized, non-linear and stable controller to keep group formation. Therefore, whenever the target moves, the leader moves to correct its position and orientation, also forcing the whole team to move in formation. The combination of these two controllers will allow the future implementation of a hybrid cooperation strategy, joining a decentralized (visualservo control) and a centralized (the formation control) approaches. Some experiments were done, so the system performance could be analyzed. Keywords— Visual-servo control, formation control, hybrid cooperation strategy. Resumo— Este trabalho aborda a implementação de um controle servo-visual que comanda o movimento de um robô de acordo com um objeto de interesse. O robô em questão é o lı́der de uma equipe de robôs que se mantém em formação por meio de um outro controlador não linear, centralizado e estável. Desta maneira, quando o objeto se deslocar, o lı́der, por meio do controle servo-visual, também irá se movimentar, mantendo sempre as mesmas posição e orientação iniciais em relação ao objeto. Paralelo a isso, a equipe de robôs seguidores também se movimenta, mantendo a formação junto ao seu lı́der. Esse esquema tem por objetivo preparar a adoção de uma estratégia de cooperação mista. Por fim, testes foram realizados de maneira a validar o funcionamento do sistema. Palavras-chave— 1 Controle servo-visual, controle de formação, estratégia de cooperação mista. Introdução Devido ao avanço tecnológico dos últimos anos, o homem tem buscado cada vez mais desenvolver máquinas e sistemas capazes de realizar suas tarefas. Estas variam de atividades repetitivas, que exigem um alto grau de acerto, a atividades que apresentam risco ao ser humano. Os robôs têm sido cada vez mais empregados para realizar tais tarefas (Mark W. Spong, 2006). Entretanto, muitas vezes, a utilização de apenas um robô não é suficiente, sendo necessário ou aconselhável o emprego de uma equipe de robôs. Em (Tamio Arai, 2002) e (Parker, 2003) são apresentados e discutidos diversos tópicos acerca de sistemas multi-robôs, suas vantagens e aplicações. Em se tratando de equipe, o conceito de formação torna-se, na maioria das vezes, fundamental para o sucesso da atividade realizada. O transporte de cargas é um exemplo de trabalho onde a manutenção da formação é fundamental. Caso uma equipe de robôs saia da formação durante o transporte de uma carga, esta pode ter seu peso mal distribuı́do e cair, causando o fracasso da atividade. Além de manter a formação, uma equipe de robôs deve ser capaz de navegar pelo ambiente, aumentando sua área de atuação e mantendo a segurança da equipe. Baseado nesse contexto, este trabalho visa permitir que o lı́der de uma equipe de robôs móveis, que possuem uma estratégia de controle de formação centralizada, navegue pelo ambiente ba- seado nos movimentos de um padrão colorido, por meio de um controlador servo-visual. Enquanto se move, seguindo o padrão, o lı́der guia a sua equipe para um determinado local a fim de executar uma tarefa. Desta maneira, se o padrão for adaptado sobre um outro robô, o lı́der da equipe seguirá os movimentos deste robô. Isso permitirá o desenvolvimento de uma estratégia de cooperação mista, onde o lı́der da equipe de robôs segue o padrão (outro robô) em um esquema de cooperação descentralizado e ao mesmo tempo mantém os membros de sua equipe em formação de forma centralizada. 2 Sistemas de Visão No presente trabalho, serão empregadas imagens perspectiva e omnidirecional, ambas capturadas pelo mesmo sistema de visão. A primeira será utilizada para realimentar o controlador servo-visual e a segunda para obter informações para o controle de formação. Imagens perspectivas são úteis quando se deseja uma representação mais realista da cena, enquanto imagens omnidirecionais proporcionam a vantagem de um campo de visão de 360◦ . A fim de evitar a utilização de um sistema perspectivo e outro omnidirecional, optou-se por realizar a retificação da imagem omnidirecional, para obter, assim, uma imagem perspectiva livre de distorções. Desta forma, apenas um sistema de visão omnidirecional catadióptrico formado por uma câmera e um espelho hiperbólico foi empregado, permitindo que uma câmera perspectiva virtual fosse idealizada sobre o foco do espelho hiperbólico. O processo de retificação utilizado foi proposto por (Junior, 2002), levemente corrigido e implementado em (Vale, 2009). 3 Os demais deslocamentos possı́veis para o robô são gerados por movimentos opostos aos das Figuras 1(a) e 1(b) ou pela combinação destes. Conhecidos os comportamentos desejados do robô, pode-se estabelecer as leis de controle. Considere os referenciais adotados para o robô e para a câmera na Figura 3. Controle Servo-Visual O controlador servo-visual utiliza a variação das coordenadas, na imagem, dos vértices de um padrão retangular localizado no mundo, para gerar sinais de controle para um robô. A primeira imagem capturada do padrão é utilizada para definir as coordenadas de referência usadas para comparação com as demais imagens capturadas posteriormente durante o movimento do robô. Dessa forma, o robô tentará manter sempre as mesmas distância e orientação, em relação ao retângulo localizado no mundo, que ele possuı́a no momento da captura da primeira imagem (Vassallo, 2004). As Figuras 1(a) e 1(b) exibem dois casos que definirão o comportamento do robô, a partir do movimento dos vértices do retângulo na imagem. O retângulo verde é formado pelos vértices de referência, e o azul, pelos vértices da imagem capturada em um instante posterior. Z Y Câmera X Robô Figura 3: Referenciais do robô e da câmera. Dado um movimento genérico do robô com velocidades linear e angular iguais a T (Tx , Ty , Tz ) e Ω(ωx , ωy , ωz ), o movimento de um ponto P com relação ao robô, expresso no referencial do robô, é dado pelas Equações 1 e 2. Ṗ Ẋ Ẏ Ż = = −T − Ω × P −Tx − ωy Z + ωz Y −Ty − ωz X + ωx Z −Tz − ωx Y + ωy X (1) (2) Considerando o ponto P projetado perspectivamente sobre o plano da imagem, conforme mostra a Figura 4, tem-se as expressões da Equação 3. (a) (b) P X ,Y , Z Figura 1: (a) Tendência de movimento para frente. (b) Movimento composto de translação e rotação. No caso expresso na Figura 1(a), o robô se aproximaria do retângulo localizado no mundo, tendo em vista que a imagem deste diminuiu em relação a referência, ou seja, ele se afastou do robô. Na Figura 1(b), como o robô é de tração diferencial, ele transladaria e rotacionaria de maneira a visualizar a imagem do retângulo azul da mesma forma que o retângulo verde. As posições inicial e final do robô para o caso da Figura 1(b) podem ser vistas na Figura 2. Padrão Retangular d Yc px , y z c Zc C x Xc Figura 4: Projeção de um ponto P no plano da imagem. x= X Z , z= Y Y (3) Derivando as expressões da Equação 3, obtém-se as expressões da Equação 4. Essas expressões relacionam a variação da posição do ponto na imagem com seu movimento no mundo. d ẋ = Frente do Robô Posição Final Posição Inicial Figura 2: Posição inicial e final do robô para a movimento do retângulo, na imagem, da Figura 1(b). ẊY − X Ẏ ŻY − Z Ẏ , ż = Y2 Y2 (4) Como o robô deste trabalho possui plataforma diferencial, ele possui apenas dois graus de liberdade, ou seja, velocidades de translação na direção Y (Ty ) e de rotação em torno do eixo Z (ωz ). Logo, por meio das Equações 1, 2 e das expressões da Equação 4, pode-se obter a velocidade do robô dado o movimento de um ponto na imagem, conforme exibido na Equação 5. ẋ ż = x Y z Y 1 + x2 xz Ty ωz (5) A matriz que se encontra logo após a igualdade é um Jacobiano. Dado que os quatro vértices do retângulo serão utilizados como entrada do controlador, pode-se compor um Jacobiano estendido, conforme mostra a Equação 6. Ṗ8×1 = J̇8×2 Ṗ8×1 = J8×2 = ẋ1 Ty ωz ż1 x1 Y1 1 + x21 com ... ẋ4 z1 Y1 ż4 T e x4 Y4 z4 Y4 1 + x24 (6) T x 4 z4 Isolando a matriz de velocidades do robô e substituindo a matriz da velocidade dos vértices pela matriz da variação de suas coordenadas, a cada imagem, em relação a posição de referência, pode-se obter a velocidade do robô conforme a Equação 7. Ty ωz = C2×8 E8×1 com x1r − x1 z1r − z1 .. . x4r − x4 z4r − z4 E8×1 C2×8 = = T T e (7) ξ1 ξ2 ... ξi T (8) onde ξi = (xi , yi ) é a posição do i-ésimo seguidor no referencial fixado sobre o lı́der da equipe. Derivando ρ(ξ) em relação ao tempo, obtém-se a Equação 9. (9) Como o vetor de parâmetros de formação foi escolhido igual a posição dos seguidores, ρ(ξ) = ξ, o Jacobiano da Equação 9 torna-se uma matriz identidade. Logo, por meio da Equação 9, podese definir a lei de controle dada na Equação 10. ξ˙f r = ρ̇d + fρ̃ (ρ̃) com ρ̃ = ρd − ρ (10) onde ρ̃ é o vetor erros de formação, ρd é o vetor com parâmetros de formação desejados e ρ é o vetor com os parâmetros de formação atuais. A função fρ̃ (ρ̃) é a saturação sobre o erro de formação utilizada em (Gava, 2007). A velocidade ξ˙f r é gerada pelo controlador de formação com o objetivo de levar os seguidores à formação desejada. As velocidades linear (ξ˙l ) e angular (ξ˙ω ) do lı́der são adicionadas à velocidade ξ˙f r pelo módulo de compensação (Equação 11). ξ˙r = ξ˙f r + ξ˙l + ξ˙ω (11) −1 T (J J) J onde C é a pseudo-inversa do Jacobiano. Tanto C quanto J dependem da distância Yi de cada vértice i no espaço 3D. Estes valores deveriam ser estimados, porém, isto não é possı́vel quando se trabalha sem qualquer conhecimento prévio sobre a cena e com apenas uma imagem. Desse modo, conforme em (Espiau et al., 1992), a estratégia adotada é considerar que os vértices do retângulo se encontram à mesma profundidade constante na cena, Yi = Yc . Sendo assim, conforme (Espiau et al., 1992), a convergência do controle é garantida na vizinhança da posição de referência Yc pretendida. É importante mencionar que os parâmetros intrı́nsecos da câmera perspectiva devem ser considerados nas coordenadas dos vértices, antes de passá-los como entrada para o controlador servovisual. 4 ξ= ρ̇ = J(ξ)ξ˙ ... ... x 1 z1 parado. O segundo módulo se preocupa em compensar as velocidades linear e angular do lı́der nos comandos de velocidade gerados pelo controlador de formação. Considere o vetor de parâmetros de formação ρ(ξ), onde ξ é o vetor de posições dos n robôs seguidores existentes, conforme mostra a Equação 8. Controle de Formação O controle de formação empregado neste trabalho, baseado em (Gava, 2007), possui dois módulos: o controlador de formação e de compensação. O primeiro é responsável por levar o robô seguidor até a posição desejada, considerando o robô lı́der A Figura 5 exibe um dos casos da compensação do comando de velocidade ξ˙f r . Por simplicidade, o seguidor já se encontra em formação. y v r Líder i Centro de Rotação ri yi xi x vi Seguidor Figura 5: Compensação das velocidades linear e angular do lı́der, nos comandos de velocidade gerados para o seguidor. Como as velocidades v e ω do lı́der são conhecidas, pode-se encontrar, por meio da Equação 12, o raio de sua trajetória (r) e o da trajetória de seu seguidor (ri ). Assim, obtém-se as compensações da Equação 11 por meio das Equações 13 - 15. r = φi = vix = viy = p v e ri = (r + xi )2 + (yi )2 ω yi arctan e vi = ωri r + xi π |vi | cos φi + 2 π |vi | sen φi + 2 (12) (13) (14) (15) Após obtidas as compensações, os comandos de velocidades que devem ser enviados aos seguidores são obtidos pelas Equações 16 e 17. ξ˙ci = kξ˙ri k cos (α̃) (16) ωci = α˙ri + fα̃ (α̃i ) + ω (17) onde kξ˙ri k é a norma da velocidade desejada para o seguidor, α˙ri é a variação de sua orientação no tempo, α̃i = αri − αi é seu erro angular e αi é sua orientação atual. A função fα̃ (α̃i ) é a saturação sobre o erro de orientação utilizada em (Gava, 2007). Maiores detalhes do controle de formação, assim como a prova da estabilidade podem ser vistas em (Gava, 2007). 5 Processamento de Imagens O processamento de imagens tem por objetivo prover as informações necessárias para realimentar os controles servo-visual e de formação. Para isso, foi utilizada a função CAMSHIFT (Continuously Adaptive Mean-Shift), disponibilizada pela R OpenCV (Intel Open Source Computer Vision Library) (Bradski and Kaehler, 2008). Esta função é capaz de realizar o rastreamento de um objeto, baseado em seu histograma e sua localização inicial. Como o controlador servo-visual é realimentado com as posições na imagem dos vértices de um retângulo localizado no mundo, uma forma de conseguir a imagem desses vértices é utilizar o padrão com marcas laranja da Figura 6(a). (a) (b) Figura 6: (a)Padrão com marcas coloridas utilizado. (b) Rastreamento das marcas coloridas realizado pela CAMSHIFT. A Figura 6(b) exibe uma imagem perspectiva obtida por meio do processo de retificação da imagem omnidirecional e mostra o processo de rastreamento das marcas. Desta forma, o centro de cada marca pode ser obtido mesmo que o padrão se mova. Como esses centros são imagens dos vértices de um retângulo, pode-se, então, realimentar o controlador servo-visual. Já para o controle de formação, o processamento de imagens é utilizado para obter, a partir de uma imagem omnidirecional, a postura dos seguidores em relação ao lı́der, ou seja, sua posição e orientação. Atribuindo a cada seguidor uma cor, pode-se obter o histograma de cada robô e a região onde ele inicialmente se encontra na imagem. Em seguida, utilizando essas informações na CAMSHIFT, pode-se realizar o rastreamento do seguidor, conforme mostra a Figura 7. Figura 7: Processo de rastreamento do seguidor. Sendo assim, pode-se obter a posição de cada seguidor na imagem. Com o auxı́lio de uma função polinomial, converte-se as coordenadas da posição na imagem para o referencial fixado sobre o lı́der, eliminando as distorções impostas pelo espelho hiperbólico. Utilizando essas posições no referencial do robô, pode-se estimar a orientação de cada seguidor. A atualização da postura do seguidor não é realizada a todo momento, mas sim após ele realizar um deslocamento mı́nimo. Isso diminui o ruı́do presente nas medidas de posição e orientação. O método utilizado para se estimar a orientação do seguidor também está detalhado em (Gava, 2007). 6 Resultados Experimentais Para a realização dos experimentos foram utilizados um robô Pioneer-2DX, escolhido como lı́der da equipe e dotado de um sistema de visão omnidirecional, e um robô seguidor, conforme mostra a Figura 8. Além disso, um padrão colorido como o da Figura 6(a) foi empregado para realização do controle servo-visual. O primeiro experimento avalia o desempenho do controlador servo-visual quando aplicado apenas ao lı́der da equipe, sem que o controle de formação esteja atuando. Em seguida, é avaliado o desempenho do controle de formação quando o lı́der da equipe segue o padrão, utilizando o controle servo-visual. 6.1 Experimento 1 A Figura 9 exibe os resultados obtidos no Experimento 1. A posição inicial do Pioneer foi escolhida sobre a origem do referencial absoluto do mundo. Neste experimento, o padrão colorido foi afastado do Pioneer de maneira que este se movimentasse para frente aproximando-se do padrão, ou seja, mantendo as mesmas distância e orientação inicial em relação ao padrão. As oscilações nos gráficos de posição e orientação são resultado 6.2 (a) (b) Figura 8: (a)Pioneer e sistema omnidirecional. (b) Robô seguidor . Experimento 2 A Figura 10 exibe os resultados obtidos no segundo experimento. A posição inicial do seguidor foi estimada em x = −0.45 m e y = 0.39 m em relação ao referencial absoluto. A orientação inicial do seguidor em relação ao referencial fixo sobre o lı́der foi estimada em α0 = 89◦ . A formação desejada para o seguidor foi x = (xP − 0.30) m e y = (yP + 0.50) m, onde xP e yP são as coordenadas do robô lı́der expressas no referencial absoluto. Assim como no Experimento 1, o padrão foi afastado do Pioneer de maneira que este último se movimentasse em direção ao primeiro. Erro de Posição do Seguidor 0.2 Erro em x Erro em y 0.15 Erro de Posição do Pioneer em Relaçao ao Padrão Posição (m) 0.1 0.05 Posição (m) 0 −0.05 −0.1 0.1 0.05 0 −0.05 −0.1 −0.15 −0.15 0 50 −0.25 0 50 100 150 150 250 Orientação do Seguidor (a) 140 Orientação (Graus) Erro de Orientação do Pioneer em Relação ao Padrão 40 30 20 10 0 −10 120 100 80 60 40 −20 0 50 100 150 200 250 Tempo (s) −30 −40 0 20 40 60 80 100 120 140 (b) Tempo (s) (b) Trajetórias Trajetória do Pioneer 3 Traj. Pioneer Padrão com Marcas Laranja Distância entre o Pioneer e o Padrão 2 1 0 −1 Posições Absolutas y (m) 0.5 Posições Absolutas y (m) 200 (a) Tempo (s) Erro de Orientação (Graus) 100 Tempo (s) −0.2 0 −0.5 Traj. Real Seg. Traj. Pioneer Traj. Ideal Seg. Formação Real Formação Desejada −1 −1.5 −2 −2.5 −3 −3.5 −4 0 0 1 2 3 4 5 6 1 2 3 4 5 Posições Absolutas x (m) Posições Absolutas x (m) (c) Figura 9: (a) Erro de posição do Pioneer em relação ao Padrão (b) Erro de Orientação do Pioneer em relação ao Padrão (c) Trajetória descrita pelo Pioneer. da inércia do Pioneer, que tarda em responder aos movimentos do padrão. Apesar das oscilações, o erro de posição final ficou abaixo de 0.05 m e o de orientação em torno de 10◦ . A posição e orientação do padrão, nos gráficos da Figura 9(c), foram obtidas por meio de técnicas de reconstrução 3D. (c) Figura 10: (a) Erro de posição do Seguidor (b) Orientação do seguidor(c) Trajetória descrita pelo Pioneer e pelo seguidor. Por meio da Figura 10(a), observa-se que, durante o movimento, o erro de posição do seguidor foi menor que 0.10 m em x e menor que 0.15 m em y. Entretanto, ao final do movimento o erro de posição foi menor que 0.05 m. O alto erro obtido durante o movimento da equipe é devido a consideração que as velocidades v e ω que o lı́der recebe do controle servo-visual são instantaneamente aplicadas, desconsiderando a inércia relativa ao lı́der. Essa consideração é feita pois a leitura de odometria do Pioneer-2DX é bastante ruidosa e, portanto, não é utilizada. Desta forma, este resultado pode melhorar significativamente quando se adicionar ao sistema uma melhor estimativa das velocidades do robô lı́der. O gráfico da Figura 10(b) mostra que o seguidor inicia o experimento desalinhado com o lı́der, porém, durante o movimento, sua orientação varia em torno de 90◦ em relação ao referencial fixo sobre o lı́der. No final do movimento, quando o lı́der pára, o seguidor busca sua formação ideal e permanece com a orientação com a qual chegou a esta formação. A Figura 10(c) mostra que o seguidor executou a trajetória de maneira adequada. A Figura 11 exibe o erro de posição da imagem dos vértices do retângulo em relação à referência. Pode-se perceber que este erro tendeu a um valor próximo de zero, garantindo que o robô lı́der mantivesse a posição e orientação em relação ao padrão próximos aos valores iniciais. 20 10 0 −10 −20 −30 −40 −50 −60 0 Erro em x Erro em z 50 100 Erro de Posição do Vértice 2 Posição (Pixeis) Posição (Pixeis) Erro de Posição do Vértice 1 150 0 −10 −20 −30 −40 Erro em x Erro em z 50 −20 −30 Erro em x Erro em z 50 100 150 Tempo (s) (c) 200 250 Erro de Posição do Vértice 4 Posição (Pixeis) Posição (Pixeis) 0 −10 −50 0 150 (b) 10 −40 100 Tempo (s) (a) 20 200 250 Os autores gostariam de agradecer FAPES (Fundação de Apoio Ciência e Tecnologia do Espı́rito Santo) pelo suporte financeiro através do projeto 38425052/2007. Referências Bradski, G. and Kaehler, A. (2008). Learning OpenCV, 1 edn, O’Reilly Media, Inc., Sebastopol. 10 Tempo (s) Erro de Posição do Vértice 3 Agradecimentos 20 −50 0 200 250 sua equipe em formação através de um esquema centralizado. Por enquanto, o padrão colorido foi movimentado de maneira manual e sua adaptação sobre um robô é deixado como trabalho futuro. Por fim, testes foram realizados e resultados promissores foram obtidos. Entretanto, o fato de não se possuir uma leitura de odometria confiável, certamente influenciou na não obtenção de menores erros de posição e orientação. Portanto, a utilização de encoders, com medidas confiáveis que permitam uma melhor estimativa de velocidades, é encorajada. 20 10 0 −10 −20 −30 −40 −50 0 Erro em x Erro em z 50 100 150 200 250 Tempo (s) (d) Figura 11: (a) Erro de Posição do vértice 1 (b) Erro de Posição do vértice 2 (c) Erro de Posição do vértice 3 (d) Erro de Posição do vértice 4 Espiau, B., Chaumette, F. and Rives, P. (1992). A new approach to visual servoing in robotics, IEEE Transactions on Robtics and Automation 8(3): 313–326. Gava, C. C. (2007). Controle de formação de robôs móveis baseado em visão omnidirecional, Master’s thesis, Universidade Federal do Espı́rito Santo - UFES. Junior, V. G. (2002). Sistema de visão omnidirecional aplicado no controle de robôs móveis, Master’s thesis, Escola Politécnica da Universidade de São Paulo. Mark W. Spong, Seth Hutchinson, M. V. (2006). Robot Modeling and Control, Wiley, John & Sons, Inc. Parker, L. E. (2003). Current research in multirobot systems, Artif Life Robotics . 7 Considerações Finais Este trabalho apresentou a implementação de um controlador servo-visual aplicado a um lı́der de uma equipe de robôs, de maneira que este seja capaz de navegar pelo ambiente guiado pelos movimentos de um padrão colorido. Além disso, enquanto se movimenta, o lı́der mantém seu seguidor em formação. Isso prepara a adoção de uma estratégia de cooperação mista, onde o padrão seria adaptado a um robô capaz de navegar pelo ambiente, por exemplo, desviando de obstáculos. Assim o lı́der da equipe de seguidores manteria um esquema de cooperação descentralizada com o robô possuidor do padrão colorido, enquanto guia Tamio Arai, Enrico Pagello, L. E. P. (2002). Guest editorial - advances in multirobot systems, IEEE Transactions on Robtics and Automation 18(5): 655–661. Vale, I. M. (2009). Sistema de visão estéreo hı́brido com recuperação da posição relativa entre as câmeras. Projeto de Graduação, Universidade Federal do Espı́rito Santo. Vassallo, R. F. (2004). Uso de mapeamentos visuomotores com imagens omnidirecionais para aprendizagem por imitação robótica, PhD thesis, Universidade Federal do Espı́rito Santo.