Sobre o controle de robos heterogêneos em formação

XII Simpósio Brasileiro de Automação Inteligente (SBAI)
Natal – RN, 25 a 28 de outubro de 2015
SOBRE O CONTROLE DE ROBÔS HETEROGÊNEOS EM FORMAÇÃO
UTILIZANDO O SISTEMA DE POSICIONAMENTO VICON
Mauricio E. Nakai∗, Roberto S. Inoue†, Valdir Grassi Junior∗, Marco H. Terra∗
∗
Departamento de Engenharia Elétrica
Universidade de São Paulo
São Carlos, São Paulo, Brasil
†
Departamento de Engenharia Elétrica
Universidade Federal de São Carlos
São Carlos, São Paulo, Brasil
Emails: [email protected], [email protected], [email protected], [email protected]
Abstract— This paper deals with a heterogeneous robot formation control, consisting of a quadrotor and four
wheeled mobile robots. In the formation strategy developed, the formation of wheeled mobile robots tracks and
follows a quadrotor. This paper presents experimetal results using a wheeled mobile robot, e-Puck and the a
quadrotor, mQX. For the position measure of the respective robots in formation, it was used a high precision
camera Vicon system.
Keywords—
Formation Control, Heterogeneous Robots, Mobile Robots, Vicon
Resumo— Este artigo trata do controle de formação de robôs heterogêneos composto por um quadricóptero e
quatro robôs móveis com rodas. Na estratégia desenvolvida a formação de robôs móveis acompanha a trajetória
gerada pelo quadricóptero. São apresentados resultados experimentais utilizando o robô móvel com rodas e-Puck
e o quadricóptero mQX. Para a realimentação da posição dos agentes é utilizado o sistema de câmeras de alta
precisão Vicon.
Palavras-chave—
1
Controle de Formação, Robôs Heterogêneos, Robôs Móveis, Vicon
cuja dinâmica de estado são acopladas através de
uma lei de controle comum. O controle de formação pode ser utilizado para as mais diversas tarefas com por exemplo: pequenos robôs de reconhecimento; veı́culos de grande porte não tripulados;
missões de resgate.
Introdução
Com o crescimento da pesquisa voltada para veı́culos autônomos de todos os tipos, aumentou-se
também a pesquisa em controle de formação e cooperação em diversas áreas. Este trabalho trata
do controle de coordenação de um grupo de robôs
heterogêneos, isto é, tem pelo menos um individuo do grupo diferente dos demais, a diferença
pode ser morfológica e/ou funcional. A necessidade de integração fı́sica e funcional de diferentes
plataformas de hardware resulta em considerável
complexidade para o projeto e a aplicação.
Devido à diferença de funcionalidade em um
grupo de robôs heterogêneos, os membros da
equipe devem decidir qual robô deve executar uma
determinada tarefa, com base em suas capacidades individuais. Os agentes não dependem unicamente de seus sensores para obter informação
sobre o ambiente, as informações dos sensores podem ser compartilhadas entre os robôs da formação. Por exemplo em Dorigo et al. (2013) foi apresentado um grupo heterogêneo com três robôs diferentes sendo que um deles é um robô aéreo autônomo. Este se fixa em um ponto mais elevado e
obtém imagens para melhor navegação dos robôs
terrestres.
O controle de formação vem sendo pesquisado
durante muitos anos e tem ganhado cada vez mais
espaço com o surgimento dos veı́culos não tripulados. Uma formação de robôs autônomos refere-se
a um grupo de robôs espacialmente distribuı́dos
Existem três principais abordagens para o
controle de formação, são estas: o controle
baseado no comportamento; configuração lı́der/seguidor; estrutura virtual (Lawton et al.,
2003). Para o presente trabalho utilizou-se o controle baseado em Lı́der/Seguidor descrito em Williams et al. (2005) e Lafferriere et al. (2004). A
principal contribuição deste artigo diz respeito a
um projeto de hardware baseado em câmeras de
alta precisão para o controle em ambientes internos de robôs heterogêneos em formação.
2
Robô móvel com rodas (RMR)
O e-Puck é um robô móvel com rodas desenvolvido na École Polytechnique Fédérale de Lausanne
para utilização nas atividades de ensino (Mondada
et al., 2009). Dentre as caracterı́sticas que contribuem para a versatilidade do robô estão: tamanho reduzido; Várias possibilidades de aplicação;
Interface amigável; Baixo custo; Projeto Aberto;
Opensource software.
1316
XII Simpósio Brasileiro de Automação Inteligente (SBAI)
3
Modelo de Formação
O modelo de formação é caracterizado por
ẋi = Aveh xi + Bveh ui ,
i = 1, · · · , N,
xi ∈ R2n ,
(1)
sendo N quantidade de veı́culos, n é a quantidade
de eixos acessı́veis aos agentes, xi representa os
estados para cada i veı́culos e ui representa a entrada do controle.
No presente estudo iremos utilizar duas dimensões, portanto as matrizes Aveh e Bveh são
dadas por


0
1
0
0
 0 a22 0 a24 
,
Aveh = 
(2)
 0
0
0
1 
0 a42 0 a44
Bveh

0
 1
=
 0
0

0
0 
.
0 
1
Figura 1: Formação com três veı́culos.
inversa. A matriz de adjunta Q que define a topologia de comunicação entre os robôs da formação,
ou seja, a hierarquia lı́der/seguidor dos robôs que
compôe a formação (Veerman et al., 2005).
(3)
L = LG ⊗ I2n .
4
5
Controle de Formação
Para a convergência da formação h, é necessário
uma lei de controle u. O aspecto da distribuição
desse controle é baseado na dependência de u em
informações locais. O erro de saı́da zi é calculado
como a média do posicionamento relativo da vizinhança dos agentes como apresentado na Equação
(9) onde Ji representa a quantidade de vizinhos do
veı́culo i,
(5)
Definição de Formação
A matriz de formação é dada por,
1
h = hp ⊗
∈ R2nN .
0
(8)
O robô lı́der não receberá informação dos demais robôs integrantes da formação. Nesse caso a
linha da matriz de adjacência Q referente ao lı́der
deve ser zero. Isto significa que os outros robôs da
formação são forçados a se coordenarem em função do lı́der. A Figura 1 mostra a representação
vetorial da matriz h e a formação após um deslocamento q.
Os valores das colunas pares controlam a aceleração, enquanto que os zeros nas colunas ı́mpares
garantem a convergência na formação (Lafferriere
et al., 2005). O vetor xi é a composição dos vetores, xp que representa as posições e xv com as
velocidades,
T
xp = (xp )1 ... (xp )N
,
(4)
T
xv = (xv )1 ... (xv )N
.
O vetor x é representado pela equação
1
0
x = xp ⊗
+ xv ⊗
.
0
1
LG = D+ (D − Q).
(6)
zi = (xi − hi ) −
Um dı́grafo Γ representa a topologia de comunicação entre os robôs, consiste em um conjunto
finito de vértices V e arestas E. Cada vértice representa um robô, e a aresta que liga dois vértices
representa a comunicação entre dois robôs. O par
(i, j) pertence ao conjunto de arestas E se i é vizinho de j , Ji indica o número de vizinhos do
i-ésimo robô. A matriz de adjacência Γ é uma
matriz quadrada Q sendo
(
1 se(j, i) ∈ E
qij =
, (i, j ∈ V).
(7)
0 caso contrário
1 X
(xj − hj ) i = 1...N. (9)
|Ji |
j∈Ji
Como resultado o vetor z de saı́da pode ser
escrito como
z = L(x − h),
(10)
sendo L a matriz Laplaciana do dı́grafo de comunicação, obtida em (8).
A lei de controle, dada a existência de uma
matriz de realimentação F , pode ser escrita como
u = F z = F L(x − h).
(11)
Consequentemente o problema de controle de formação é dado pela equação
A matriz Laplaciana L é dada pela Equação
(8) sendo ⊗ representa o produto de Kronecker,
D é a matriz diagonal, D+ é a sua matriz pseudo
ẋ = Ax + BF L(x − h),
1317
(12)
XII Simpósio Brasileiro de Automação Inteligente (SBAI)
sendo A(q̇2 ) = −M2−1 C2 (q̇2 ) e B = M2−1 . As
matrizes M2 e C2 são dadas por,
sendo A = IN ⊗ Aveh e B = IN ⊗ Bveh , sendo
IN a matriz identidade de ordem N . Levando em
consideração as estruturas dos blocos A, B e L, a
matriz F = IN ⊗ Fveh , sendo
f1 f2 0 0
Fveh =
.
(13)
0 0 f1 f2
M2 = Sc (q1 )T M (q1 )Sc (q1 )
.
C2 = Sc (q1 )T C(q, q̇)Sc (q1 ) + Sc (q1 )T M (q1 )Sc (q1 )
(20)
A matriz Sc permite transformar as velocidades angulares das rodas nas velocidades atuantes
no centro de massa do robô móvel, C é a matriz
de forças de coriolis e centrı́peta, M é a matriz de
inércia e q1 = [xc yc α θd θe ]T . Matrizes foram obtidas através dos modelos cinemáticos e dinâmicos
baseados em (Coelho and Nunes, 2003).
Em (Williams et al., 2005) mostra-se que as
condições necessárias e suficientes para o sistema
convergir para a formação são obtidas com f1 < 0
e f2 < 0.
6
Controlador Baseado na Cinemática
8
Lei de controle utilizada baseada na cinemática
proposta por (Kanayama et al., 1990)
v d = vor cos(αe ) + kx xe ,
ω d = ωr + vor (ky ye + kα sin(αe )),
Sistema Vicon
O sistema Vicon consiste em um conjunto de câmeras de alta resolução. As câmeras são posicionadas de forma a se obter vários ângulos de um
mesmo ponto. Essa variação de posicionamento
possibilita a localização do ponto em um sistema
de coordenadas cartesiano (x, y, z) e os ângulos de
rotação (θx , θy , θz ). A localização e o rastreio dos
agentes são obtidos a partir de marcadores reflexivos, e sua identificação é feita a partir do padrão
em que esses marcadores estão posicionados (distância entre os marcadores). As câmeras se utilizam do espectro de luz infravermelho cuja fonte
é localizada em volta da lente da câmera como
é possı́vel observar na Figura 2. Dessa forma o
sistema independe da iluminação presente no ambiente desde que este esteja em espectro diferente
do infravermelho.
(14)
sendo v d a velocidade linear desejada, ω d a velocidade angular desejada, kx , ky e kα os ganhos para
os respectivos erros, xe e ye os erros nas coordenadas x e y respectivamente e αe o erro do ângulo
de orientação do robô, dados pelas equações
xe = (xr − xo )cos(α) + (yr − yo )sin(α),
ye = −(xr − xo )sin(α) + (yr − yo )cos(α),
αe = αr − α,
(15)
sendo xr , yr e αr as posições e ângulo de orientação futuras e xo , yo e αo as posições a ângulo de
orientação atuais. O cálculo de vor é dado por
q
(16)
vor = ẋ2r + y˙r2 .
O controlador baseado na dinâmica considera
as velocidades angulares de cada roda, portanto,
as relações de velocidade são definidas em
d d
θ̇di
1/r b/r
vi
d
q̇2i = d =
(17)
d ,
1/r
−b/r
w
θ̇ei
i
sendo θ̇ddi e θ̇edi as velocidades desejadas nas rodas
direita e esquerda respectivamente, r é o raio da
roda e b é a distância entre o centro de eixo e as
rodas.
7
Figura 2: Iluminação infravermelho em volta da
lente da câmera, vista frontal e lateral.
Controlador Baseado na Dinâmica
As câmeras Vicon são conectadas através de
um cabo coaxial a uma plataforma que realiza o
tratamento das imagem. A comunicação entre a
plataforma e o servidor se dá por um cabo ethernet
ligado a uma placa de rede (1Gbps), o que permite
as altas taxas de aquisição e transmissão de dados
para o servidor. O servidor utiliza um software
Vicon para disponibilizar valores de posição e orientação, esses dados são distribuı́dos através da
rede local para o computador com os algoritmos
de controle dos robôs. As câmeras utilizam lentes
da marca PENTAX modelo C21211KP, as carac-
O controle proporcional derivativo (PD) utilizado
é independente para cada uma das rodas do robô
móvel dado por
ud = −[θd − θdd ]kp1 − [θ̇d − θ̇dd ]kd1
.
ue = −[θe − θed ]kp2 − [θ̇e − θ̇ed ]kd2
(18)
A equação de torque controlado obtida através do modelo dinâmico,
τ = B −1 (q̈2d − A(q̇2 )q̇2d + u),
(19)
1318
XII Simpósio Brasileiro de Automação Inteligente (SBAI)
terı́sticas técnicas das câmeras são apresentadas
na Tabela 1.
Tabela 1: Caracterı́sticas da câmera Vicon.
Câmera
Resolução
Máx. taxa em máx. resolução
Sensor
9
T40S
4Mp (2336 × 1728)
515 fps
Vicon Vegas S-4
Métodos Utilizados e Resultados
As câmeras Vicon estão posicionadas nos cantos
superiores do ambiente de ensaio, as imagens são
transmitidas para um módulo Vicon que realiza o
processamento das imagens. Os dados processados são então enviados para um servidor com o
software de interface Tracker Vicon. O servidor
então envia os dados para o computador com os
algoritmos de controle de formação, que por sua
vez, envia os valores de torque para os robôs via
bluetooth. A Figura 3 apresenta o esquemático
do ambiente de ensaio utilizado para o controle
de formação.
Figura 4: Desenho utilizado para o cálculo dos
momentos do robô e-Puck.
Na Tabela 2 temos as dimensões medidas e
os momentos de inércia calculados para o robô ePuck.
Tabela 2: Caracterı́sticas fı́sicas do robô móvel
com rodas.
Raio da roda
20mm
Centro do eixo até a 29mm
roda
Massa do robô
150g
Dist. entre centro de 0.5mm
massa e centro do eixo
M.I. de inércia da pla- 78, 399Kg mm2
taforma
M.I. da rodas em rela- 1, 314Kg mm2
ção ao eixo da roda
M.I. da roda em rela- 0, 676Kg mm2
ção ao eixo no plano da
roda
Figura 3: Esquemático de configuração do ambiente de ensaio.
9.2
9.1
O algoritmo utilizado é composto de quatro partes: tratamento dos dados enviados pelo sistema
Vicon; controle de formação; controle baseado no
modelo cinemático; controle baseado no modelo
dinâmico. A Figura 5 ilustra o diagrama de blocos do algoritmo.
Caracterı́sticas e Dimensões do RMR Utilizado
As caracterı́sticas fı́sicas do robô são necessárias
para o correto funcionamento do modelo dinâmico. A utilização de um software CAD viabilizou
o cálculo dos valores dos momentos de inércia da
plataforma do robô e de suas rodas para a utilização no controle baseado na dinâmica.
O robô foi desmontado, cada peça foi desenhada no software e pesadas individualmente. Na
Figura 4 tem-se o desenho do robô e-puck utilizado na ferramenta CAD para os cálculos de suas
propriedades fı́sicas.
Algoritmos de Intefarce e Validação
• Algoritmo Vicon: realiza a aquisição das imagens e o processamento de visão computacional para se obter as posições de todos os elementos previamente identificados dentro do
volume de trabalho a uma taxa de 100 fps.
• Controle de Formação: calcula as posições
futuras de cada robô utilizando-se das coordenadas atuais e a trajetória desejada.
1319
XII Simpósio Brasileiro de Automação Inteligente (SBAI)
• Controle Baseado na Cinemática: realiza o
cálculo da velocidade linear e angular de cada
robô levando-se em conta restrições dos agentes. O algoritmo também faz o controle de
saturação das velocidade.
foi h1 = [450 0], h2 = [225 150], h3 = [225 − 150]
e h4 = [0 300]. O ganhos utilizados no controlador cinemático foram kα = 0, 0001 e kx = ky =
1, 5x10−5 e a matriz de realimentação da formação
utilizada foi
−2 −2 0
0
Fveh =
.
(21)
0
0 −2 −2
• Controle Baseado na Dinâmica: realiza o
cálculo do torque de cada robô levando em
consideração as caracterı́sticas do modelo dinâmico e cinemático. Em seguida o torque calculado é transformado em pulsos por
segundos devido à caracterı́stica de acionamento do robô e-puck.
A Figura 7 apresenta o ensaio realizado onde
pode-se ver quatro momentos diferentes no trajeto realizado, que ocorrem na ordem crescente
dos numerais. Nota-se que no primeiro instante
a formação está uniforme, no segundo momento,
percebe-se um pequeno adiantamento na formação do Robô 1, no instante 3 o Robô 1 se mantém abaixo do quadricóptero enquanto os demais
robôs se aproximam da formação e no momento
seguinte nota-se um adiantamento no posicionamento do Robô 1, seguido dos demais robôs da
formação.
Figura 5: Diagrama de blocos do algoritmo elaborado para o controle de formação
9.3
Figura 7: Formação com quatro robôs RMR e um
robô aéreo em quatro momentos distintos da trajetória percorrida.
Resutados Práticos
No ensaio realizado a formação de RMR se mantém sob o quadricóptero, responsável por estabelecer um trajeto para a formação. Utilizou-se o
robô móvel com rodas e-Puck e o quadricóptero
mQX da marca Hobby Horizon, controlado manualmente. A Figura 6 apresenta a configuração de
vizinhança definida para o ensaio.
Na Figura 8 pode-se ver o mesmo percurso
apresentado na Figura 7, com a trajetórias dos
RMRs utilizados na formação, percebem-se pequenas instabilidades, principalmente na trajetória dos Robôs 3 e 4, isso ocorre devido ao acúmulo
do erro no controle de formação, visto que são os
dois últimos robôs na configuração de vizinhança.
Percebe-se também uma pequena irregularidade na trajetória do Robô 1 em destaque na Figura 9, isso se deu devido ao erro real de posição, no instante que o Robô 1 chega próximo à
posição desejada, o algoritmo de controle continua calculando o erro de posição por menor que
ele seja, como consequência da correção deste pequeno erro, o robô realiza uma trajetória circular
em torno da posição desejada.
Figura 6: Configuração de vizinhança utilizada no
ensaio.
10
Os valores adotados dos ganhos para o controlador PD são kp1 = kp2 = 0, 17 e kd1 = kd2 =
0, 05. A matriz de formação utilizada nos ensaios
Conclusões
O sistema Vicon tem proporcionado aquisições de
dados com alta confiabilidade, sem falhas nem
1320
XII Simpósio Brasileiro de Automação Inteligente (SBAI)
Dorigo, M., Floreano, D., Gambardella, L., Mondada, F., Nolfi, S., Baaboura, T., Birattari,
M., Bonani, M., Brambilla, M., Brutschy,
A., Burnier, D., Campo, A., Christensen, A.,
Decugniere, A., Di Caro, G., Ducatelle, F.,
Ferrante, E., Forster, A., Martinez Gonzales,
J., Guzzi, J., Longchamp, V., Magnenat, S.,
Mathews, N., Montes de Oca, M., O’Grady,
R., Pinciroli, C., Pini, G., Retornaz, P., Roberts, J., Sperati, V., Stirling, T., Stranieri,
A., Stutzle, T., Trianni, V., Tuci, E., Turgut,
A. and Vaussard, F. (2013). Swarmanoid: A
novel concept for the study of heterogeneous
robotic swarms, Robotics Automation Magazine, IEEE 20(4): 60–71.
Figura 8: Trajetória percorrida por quatro RMR
em formação durante o ensaio realizado.
Kanayama, Y., Kimura, Y., Miyazaki, F. and
Noguchi, T. (1990). A stable tracking control method for an autonomous mobile robot, Robotics and Automation, 1990. Proceedings., 1990 IEEE International Conference
on, pp. 384–389 vol.1.
Lafferriere, G., Caughman, J. and Williams, A.
(2004). Graph theoretic methods in the stability of vehicle formations, American Control
Conference, 2004. Proceedings of the 2004,
Vol. 4, pp. 3729–3734 vol.4.
Figura 9: Trajetória do Robô 1, região em destaque na Figura 8.
Lafferriere, G., Williams, A., Caughman, J. and
Veerman, J. (2005). Decentralized control of
vehicle formations, Systems & Control Letters 54(9): 899 – 910.
atrasos significativos. A comunicação bluetooth
entre o computador e o robôs e-Puck tem se mostrado uma alternativa também viável para esse
tipo de problema. As caracterı́sticas dinâmica,
cinemática e os pesos utilizadas nos modelos e
no controlador utilizado se mostraram corretas.
Estabeleceu-se um sistema de alta precisão para
localização dos robôs e um método de controle de
formação de robôs heterogêneos eficaz, pretendese em trabalhos futuros obter resultados experimentais utilizando controladores baseados em sistemas lineares sujeitos a saltos Markovianos aplicados para essa classe de robôs. Incluindo controle
Markoviano para sistemas descentralizados, para
tratar de alternância de lı́deres e falhas na formação.
Lawton, J., Beard, R. and Young, B. (2003). A
decentralized approach to formation maneuvers, Robotics and Automation, IEEE Transactions on 19(6): 933–941.
Mondada, F., Bonani, M., Raemy, X., Pugh, J.,
Cianci, C., Klaptocz, A., Magnenat, S., christophe Zufferey, J., Floreano, D. and Martinoli, A. (2009). The e-puck, a robot designed
for education in engineering, In Proceedings
of the 9th Conference on Autonomous Robot
Systems and Competitions, pp. 59–65.
Veerman, J., Lafferriere, G., Caughman, J. and
Williams, A. (2005). Flocks and formations,
Journal of Statistical Physics 121(5-6): 901–
936.
Agradecimentos
Williams, A., Lafferriere, G. and Veerman, J.
(2005). Stable motions of vehicle formations,
Decision and Control, 2005 and 2005 European Control Conference. CDC-ECC ’05.
44th IEEE Conference on, Seville, pp. 72–77.
Este trabalho contou com o apoio financeiro da
FAPESP através dos processos 2007/03484-8 e
2012/08106-0, CAPES e CNPQ.
Referências
Coelho, P. and Nunes, U. (2003). Lie algebra application to mobile robot control: a tutorial,
Robotica 21: 483–493.
1321