Post on 10-Nov-2018
Controlo de Formacoes de Veıculos Aereos naoTripulados
Henrique Ribeiro Delgado da Silva
Dissertacao para obtencao do Grau de Mestre em
Engenharia Electrotecnica e de Computadores
Juri
Presidente: Professor Carlos Filipe Gomes BispoOrientador: Professor Pedro Manuel Urbano de Almeida LimaCo-Orientador: Engenheiro Joao Nuno Prata NoemeVogal: Professor Antonio Pedro Rodrigues de Aguiar
Abril de 2012
“The person who merely watches the flight of a bird gathers the impression that the bird hasnothing to think of but the flapping of its wings. As a matter of fact this is a very small part of itsmental labor. To even mention all the things the bird must constantly keep in mind in order to flysecurely through the air would take a considerable part of the evening. If I take this piece of paper,
and after placing it parallel with the ground, quickly let it fall, it will not settle steadily down as a staid,sensible piece of paper ought to do, but it insists on contravening every recognized rule of decorum,
turning over and darting hither and thither in the most erratic manner, much after the style of an untrainedhorse. Yet this is the style of steed that men must learn to manage before flying can become an everydaysport. The bird has learned this art of equilibrium, and learned it so thoroughly that its skill is not
apparent to our sight. We only learn to appreciate it when we try to imitate it.”Wilbur Wright, 1901
Abstract
In a robot team, one of the essential factors to achieve cooperation is its capacity to maintain
any desired geometric configuration, by using appropriate control techniques for the relative poses
between teammates. Formation control must be robust to several types of disturbances, so that
the desired geometry is achieved. But, at the same time, it must be flexible enough to change its
geometry, so that the team can handle environment spacial constraints (e.g., moving from an open
area to a narrow corridor). Artificial potential fields, based on harmonic functions were chosen as
the control method for formations of Unmanned Aerial Vehicles (UAVs), specifically quadrotors.
The first part of this thesis presents the concept of a quadrotor vehicle, by defining its kinema-
tics and its dynamics, and also presents the study and development of several control methods.
Both implemented in Simulink. The second part firstly describes the panel method based on har-
monic potential functions and lastly presents the formation control method with UAVs. Finally, in
the last part of this thesis the results of flight tests with a real quadrotor are presented.
Keywords
Unmanned Aerial Vehicles, Quadrotor, Artificial Potential Fields, Formation Control, Rigid For-
mations, Dynamic Formations.
i
Resumo
Numa equipa de robos, um dos factores essenciais para que haja cooperacao entre eles e a
sua capacidade para manter uma qualquer configuracao geometrica desejada, fazendo controlo
adequado das posicoes relativas e orientacoes entre os varios membros da equipa. O controlo
de formacoes deve ser robusto a varios tipos de perturbacoes, por forma a que a geometria
desejada seja alcancada, mas, ao mesmo tempo, deve ser suficientemente flexıvel para poder
alterar a sua geometria, de modo a que a equipa possa lidar com as restricoes do ambiente onde
se encontra (por exemplo, passar de uma area aberta para um corredor estreito). A partir dos
varios metodos conhecidos, sera utilizada uma abordagem baseada no uso de campos potenciais
artificiais, gerados atraves de funcoes harmonicas, para efectuar o controlo de formacoes de
Veıculos Aereos Nao Tripulados (UAVs), especificamente, quadricopteros.
A primeira parte desta tese apresenta a modelacao de um quadricoptero, onde se define a sua
cinematica e dinamica, e tambem se apresenta o estudo e desenvolvimento de varios metodos
de controlo. Ambos implementados em Simulink. A segunda parte, em primeiro lugar descreve
o metodo dos paineis baseados em funcoes harmonicas e, por fim, apresenta o metodo de con-
trolo de formacoes com UAVs. Finalmente, na ultima parte deste trabalho sao apresentados os
primeiros resultados de ensaios em voo com um quadricoptero real.
Palavras Chave
Veıculos Aereos Nao Tripulados, Quadricoptero, Campos Potenciais Artificiais, Controlo de
Formacoes, Formacoes Rıgidas, Formacoes Dinamicas.
iii
Agradecimentos
A minha dissertacao de mestrado representa todo o meu trabalho realizado ao longo do ultimo ano, ee resultado de todo o sacrifıcio, paixao, trabalho e dedicacao que coloquei nesta fase importante da minhavida. Para que desse frutos, tive o privilegio de ter a meu lado um conjunto de pessoas que me mantiverammotivado e focado nos meus objectivos.
Em primeiro lugar, quero agradecer ao Professor Pedro Lima por, nao so me ter dado a oportunidadede elaborar a minha tese numa area tao interessante e desafiante, como tambem por toda a ajuda edisponibilidade que me deu. Obrigado.
Quero tambem agradecer a UAVision, em particular ao Eng. Nuno Simoes, Joao Noeme, Joao Costa,Ricardo Alcacer e Marco Simoes, pela disponibilizacao de um quadrotor e por toda a disponibilidade quedemonstraram para resolver qualquer tipo de problema. Esta oportunidade proporcionou-me trabalhar navertente mais pratica da engenharia, o que permitiu enriquecer, e muito, o meu trabalho. Obrigado.
Deixo tambem uma palavra de grande apreco a equipa do DSOR, em especial ao Eng. Manuel Rufino,Eng. Luıs Sebastiao, “Eng.” Joao Botelho, Eng. Bruno Cardeira, e Eng. Bruno Gomes. Obrigado pelosvosso sabios conselhos, pela motivacao e pela disponibilidade em ajudar a qualquer momento. Muitoobrigado!
Quero tambem agradecer aos meus companheiros de tese, Andre Silva, Jorge Ribeiro e Pedro Santos,pelas inumeras conversas, discussoes, cafes, que em muito me ajudaram a realizar este trabalho. Devo-vos muito! Obrigado.
Agradeco tambem a equipa do RAPOSA, em especial ao Professor Rodrigo Ventura, Joao Mendes,Filipe Jesus, Pedro Vieira, Miguel Vaz, pelo interesse que sempre demonstraram no meu trabalho e nasminhas ideias.
Um agradecimento muito especial aos 5 que me acompanharam desde o inıcio desta jornada. JoaoFalcao, Ricardo Grizonic, Sılvio Rodrigues, Ricardo Batista e Pedro Carreira, obrigado pela dimensao quetiveram, e tem, na minha vida. A voces devo muito do que sou hoje. Muito, muito obrigado!
Quero tambem agradecer a equipa SocRob, nomeadamente ao Joao Estilita, Aamir Ahmad, Joao Reis,Joao Sousa e Miguel Serafim, pelos bons momentos passados enquanto membro da equipa.
Deixo tambem uma palavra a todos os meus colegas do IST, em especial ao Joao Tomas, Nuno Santos,Valter Matos, Andre Santiago, Alexandre Dias, Carlos Fonseca, Joao Loureiro, Jorge Wan, Rui Figueiredo,Rui Oliveira, Ricardo Oliveira, Andre Grilo, Joao Matos, Pedro Silva, Simao Duarte, Andre Rebocho eAlexander Yefimochkin, por me terem acompanhado neste percurso academico. Muito Obrigado.
Quero deixar uma palavra de grande agradecimento ao Joao “Aguas” Vaz, Miguel “Tedd” Madureira,Bruno Farola, Joao Melo, Joao “Panchy” Oliveira, Miguel Melo, David Pio, Ricardo Manso, Joao Vilela,Tiago Chaves, Filipe Cardoso, Andre Cardoso, Bruno Pio, Ana Cabete, pela vossa amizade. Considero-vos um grande pilar da minha vida e sei que hoje so posso estar a escrever este texto porque voces estaocomigo. Agradeco a Deus por, um dia, vos ter colocado no meu caminho! Muito obrigado*
Por ultimo, quero agradecer a minha famılia pelo apoio e paciencia que tem comigo. E a voces quedevo tudo. Sao os unicos que tem por mim um amor incondicional, sem mentiras nem falsidades, que meaceitam tal e qual como sou, com os meus defeitos e virtudes, nos bons e maus momentos, e e a vocesque, todos os dias, vou buscar a forca e o discernimento para dar o meu melhor em tudo o que faco. Muito,muito obrigado!
Maio, 2012Henrique Silva
v
Indice
1 Introducao 11.1 Motivacao . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21.2 Estado da Arte . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21.3 Objectivos e Contribuicoes . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41.4 Organizacao . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4
2 Modelacao do Quadricoptero 52.1 Introducao . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62.2 Definicoes e Conceitos Basicos . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6
2.2.1 Sistemas de Coordenadas . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62.2.2 Manipulacao de um Quadricoptero . . . . . . . . . . . . . . . . . . . . . . . . 7
2.3 Modelo do Quadricoptero . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 92.3.1 Principais Pressupostos . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 92.3.2 Cinematica . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 92.3.3 Dinamica . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
2.4 Metodos de Controlo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 132.4.1 Controladores PID . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
2.4.1.1 Controlador de Malha Interna . . . . . . . . . . . . . . . . . . . . . 132.4.1.2 Controlador de Malha Externa . . . . . . . . . . . . . . . . . . . . . 15
2.4.2 Controlador LQR . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 162.4.2.1 Formulacao Teorica . . . . . . . . . . . . . . . . . . . . . . . . . . . 162.4.2.2 Implementacao Pratica . . . . . . . . . . . . . . . . . . . . . . . . . 17
2.5 Simulador . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 192.5.1 Arquitectura do Simulador . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 192.5.2 Implementacao . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 202.5.3 Simulacao . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25
3 Controlo de Formacoes 313.1 Introducao . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 323.2 Formulacao de Funcoes Harmonicas . . . . . . . . . . . . . . . . . . . . . . . . . . . 323.3 Metodo dos Paineis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35
3.3.1 Painel Unico . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 353.3.2 Multiplos Paineis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 393.3.3 Garantia de Convergencia . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44
4 Controlo de Formacoes com Quadricopteros 494.1 Introducao . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 504.2 Definicao Geometrica de Formacoes . . . . . . . . . . . . . . . . . . . . . . . . . . . 50
vii
Indice
4.3 Controlo de Formacoes . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 514.4 Implementacao . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 534.5 Resultados . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 55
5 Plataforma de Ensaio Experimental 615.1 Introducao . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 625.2 Caracterizacao do Veıculo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 625.3 Arquitectura de Hardware e Software . . . . . . . . . . . . . . . . . . . . . . . . . . . 645.4 Resultados . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 66
6 Conclusoes e Trabalho Futuro 716.1 Conclusoes . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 726.2 Trabalho Futuro . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 73
Bibliografia 75
viii
Lista de Figuras
2.1 Sistemas de Coordenadas. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62.2 Movimentos obtidos quando se variam as velocidades angulares de cada motor [1]. 82.3 Arquitectura de Controlo PID. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 132.4 Arquitectura de Controlo LQR. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 182.5 Arquitectura do Simulador. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 192.6 Implementacao do bloco Dynamics. . . . . . . . . . . . . . . . . . . . . . . . . . . . 202.7 Implementacao do bloco Inputs. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 212.8 Implementacao do bloco Control. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 222.9 Implementacao do bloco PID. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 232.10 Implementacao do bloco LQR. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 232.11 Implementacao do bloco State Scope. . . . . . . . . . . . . . . . . . . . . . . . . . . 242.12 Modelo VRML do quadricoptero (Modificado a partir de [2]). . . . . . . . . . . . . . . 252.13 Resposta ao escalao unitario para os controladores PID. . . . . . . . . . . . . . . . 262.14 Resposta ao escalao unitario para o controlador LQR. . . . . . . . . . . . . . . . . . 272.15 Resposta a multiplas entradas de referencia para os controladores PID. . . . . . . . 282.16 Resposta a multiplas entradas de referencia para o controlador LQR. . . . . . . . . 29
3.1 Representacao de um sumidouro (λ = 1) para um espaco a 2 dimensoes. . . . . . . 343.2 Representacao de uma fonte (λ = −1) para um espaco a 2 dimensoes. . . . . . . . 343.3 Representacao do Fluxo Uniforme (U = 1) para um espaco a 2 dimensoes. . . . . . 343.4 Exemplo de um painel (Modificado a partir de [3]) . . . . . . . . . . . . . . . . . . . . 353.5 Representacao do campo vectorial normalizado para um painel com λ = −1 e
L = 0.5. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 363.6 Sobreposicao de um fluxo uniforme com um painel para diferentes valores de Vn
(Modificado a partir de [4]). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 383.7 Representacao de um possıvel cenario para planeamento de caminho (Modificado
a partir de [3]). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 393.8 Representacao geometrica de dois paineis (Modificado a partir de [3]). . . . . . . . 393.9 Aplicacao do metodo dos paineis para diferentes valores de Vi. . . . . . . . . . . . . 443.10 Aplicacao do metodo dos paineis para diferentes valores de ra. . . . . . . . . . . . . 47
4.1 Exemplo de uma formacao com tres veıculos e um lıder virtual. . . . . . . . . . . . . 504.2 Exemplo de movimentacao da formacao. . . . . . . . . . . . . . . . . . . . . . . . . 514.3 Modelacao de um quadricoptero com base em paineis. . . . . . . . . . . . . . . . . 524.4 Fluxograma da rotina para controlo de formacoes. . . . . . . . . . . . . . . . . . . . 534.5 Implementacao do simulador para quatro veıculos e um lıder virtual. . . . . . . . . . 544.6 Implementacao do bloco Quad 1. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 544.7 Sequencia das geometrias utilizadas (Triangular, Linear, Losango). . . . . . . . . . . 55
ix
Lista de Figuras
4.8 Controlo de Formacao para tres geometrias diferentes (Ver vıdeo em [5]). . . . . . . 564.9 Controlo de Formacao utilizando Quadricopteros. . . . . . . . . . . . . . . . . . . . . 574.10 Erro de seguimento de referencia. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 584.11 Alteracao da geometria utilizando quadricopteros (Ver vıdeo em [5]). . . . . . . . . . 594.12 Erro de seguimento de referencia. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 60
5.1 Quadrotor UX-4001 Mini (Ver fotos e vıdeos em [5]). . . . . . . . . . . . . . . . . . . 625.2 Dimensoes do Quadrotor UX-4001 Mini. . . . . . . . . . . . . . . . . . . . . . . . . . 635.3 Arquitectura de hardware do sistema. . . . . . . . . . . . . . . . . . . . . . . . . . . 645.4 Arquitectura de software. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 655.5 Montagem para teste de controlador de atitude. . . . . . . . . . . . . . . . . . . . . . 665.6 Teste ao controlador de atitude segundo φ. . . . . . . . . . . . . . . . . . . . . . . . 675.7 Teste ao controlador de altitude com uma referencia de 1.3 metros (Ver fotos e
vıdeos em [5]). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 685.8 Perfil das escadas utilizadas (Ver fotos e vıdeos em [5]). . . . . . . . . . . . . . . . . 685.9 Teste ao controlador de altitude sobrevoando as escadas (Ver fotos e vıdeos em [5]). 69
x
Lista de Tabelas
2.1 Parametros utilizados para simulacao. . . . . . . . . . . . . . . . . . . . . . . . . . . 262.2 Parametros fundamentais para os controladores PID. . . . . . . . . . . . . . . . . . 272.3 Parametros fundamentais para o controlador LQR. . . . . . . . . . . . . . . . . . . . 27
3.1 Forca dos paineis para diferentes valores de ra. . . . . . . . . . . . . . . . . . . . . 47
xi
1Introducao
Conteudo1.1 Motivacao . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21.2 Estado da Arte . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21.3 Objectivos e Contribuicoes . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41.4 Organizacao . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4
1
1. Introducao
1.1 Motivacao
Nos ultimos anos, o estudo de grupos de multiplos robos autonomos, que exibem comporta-
mentos cooperativos, tem emergido e tornou-se numa area de investigacao de grande actividade.
O recente avanco na miniaturizacao de sensores, actuadores e na tecnologia em geral, pos-
sibilitou a criacao de redes distribuıdas de robos para diversas aplicacoes, como por exemplo,
reconhecimento, vigilancia, busca e salvamento ou transporte de cargas em cooperacao. Como
e evidente, estes sistemas necessitam de arquitecturas cada vez mais complexas por forma a
lidar, com facilidade, com todos os robos. Contudo, apesar deste inconveniente e segundo [6], ha
varias razoes que continuam a tornar esta area de desenvolvimento cada vez atractiva:
• Uma determinada tarefa pode ser muito difıcil, ou ate mesmo impossıvel, quando executada
por apenas um robo;
• A construcao de robos de tamanho reduzido torna-se mais facil, mais barata, mais flexıvel
e mais redundante do que ter um unico robo de grande dimensao para cada tarefa;
• Uma equipa de robos oferece maior redundancia: caso um robo apresente uma falha, o
resto do grupo pode continuar a executar a sua tarefa.
Assim, e de uma forma geral, e possıvel afirmar que uma tarefa pode ser executada mais
rapidamente e de forma mais eficaz se houver cooperacao entre multiplos robos.
1.2 Estado da Arte
O controlo de formacoes e actualmente um tema de grande actividade no seio da comuni-
dade cientıfica. Diferentes abordagens podem ser classificadas de acordo com varios criterios.
Segundo [7], os diferentes tipos de abordagens podem ser distribuıdos por 3 grupos: Lıder-
Seguidores [8], onde um dos veıculos e designado lıder e os restantes sao os seguidores. A
pose (posicao e orientacao) do lıder e determinada por uma trajectoria definida a priori e os
seguidores devem acompanhar o lıder seguindo uma geometria previamente definida, mas que
pode ser alterada em qualquer instante de tempo; Comportamental [9], onde o movimento de
cada veıculo resulta de uma media ponderada de diversos comportamentos e que em ultima
analise contribuem para um comportamento geral do grupo; Estruturas Virtuais [10], onde toda
a formacao e tratada como se fosse uma unica estrutura, cujo movimento desejado e traduzido
para o movimento de cada veıculo.
Outro criterio possıvel para agrupar as diferentes abordagens, e um criterio que tem em conta
a rigidez da formacao: alguns autores ([11],[12]) especificam a geometria completa, ou seja,
especificam as distancias e azimutes entre veıculos e controlam cada veıculo para garantir que
2
1.2 Estado da Arte
estas especificacoes sao atingidas. Esta abordagem necessita de uma arquitectura global de
coordenacao para alterar a geometria da formacao, consoante as caracterısticas do ambiente
onde o grupo se encontra (por exemplo, passar de um espaco aberto para um corredor estreito);
Outra abordagem [13] considera as formacoes como estruturas geometricas dinamicas, que de
forma natural se moldam na presenca de obstaculos e/ou caracterısticas estruturais do ambiente
onde o grupo esta inserido.
Neste trabalho existe particular interesse na abordagem lıder-seguidores com geometrias
flexıveis, ou seja, na abordagem onde existe um lıder, que pode ser virtual e que segue uma tra-
jectoria definida a priori, e que arrasta consigo os seus seguidores de acordo com uma geometria
(distancia e azimute) previamente definida, mas que a qualquer momento possa ser alterada por
forma a lidar com mudancas no ambiente.
Leonard e Fiorelli [14] introduziram o conceito de Campos Artificiais entre veıculos em formacao.
A distancia nominal entre veıculos corresponde a um mınimo do campo potencial, e tem origem
na repulsao entre os varios veıculos, na repulsao entre os veıculos e obstaculos estruturais, e
na atraccao entre lıder e seguidores. Em [15] e proposta uma abordagem similar, na qual se
usam sistemas virtuais de mola/amortecedor para manter os veıculos em formacao. O grande
inconveniente destas abordagens e o conhecido problema dos mınimos locais em campos po-
tenciais artificiais, que podem fazer com que o conjunto de veıculos nunca atinga a formacao
desejada. Alguns metodos de procura foram introduzidos para lidar com este problema, mas
apresentam custos computacionais bastante elevados [16]. Outro metodo para evitar a criacao
destes mınimos locais consiste em adicionar multiplos potenciais atractivos auxiliares, em que as
suas posicoes sao geradas atraves de algoritmos geneticos [17].
As funcoes harmonicas potenciais foram introduzidas por Kim e Khosla [18]. Estas funcoes,
que se definem como sendo solucao da equacao de Laplace, possibilitam a criacao de campos
potenciais artificiais sem mınimos locais e com solucao unica. A aplicacao de funcoes harmonicas
para o caso de desvio de obstaculos em movimento foi tambem considerada [19]. As funcoes
harmonicas foram tambem usadas em veıculos terrestres em ambientes conhecidos a priori, utili-
zando o metodo dos paineis, nome pelo qual e conhecido em dinamica de fluıdos [20]. O metodo
dos paineis revelou-se bastante adequado para aplicacoes em tempo real, e por isso foi estendido
para o caso de ambientes desconhecidos [21] e para ambientes a 3 dimensoes [22]. Em [4] foi
desenvolvido um complemento ao tradicional metodo dos paineis, que tem a capacidade de gerar
campos potenciais artificiais mais eficazes para contornar obstaculos. Este complemento torna o
metodo tradicional mais apto a mudancas dinamicas do ambiente envolvente.
3
1. Introducao
1.3 Objectivos e Contribuicoes
Este trabalho tem como objectivo o desenvolvimento de um metodo para controlo de formacoes
de veıculos aereos nao tripulados (UAVs), mais especificamente quadricopteros, mas que possa
ser estendido para qualquer tipo de veıculo e ate mesmo para grupos hıbridos de veıculos. Este
metodo e baseado em campos potenciais artificiais, gerados atraves de funcoes harmonicas. As
formacoes devem ser rıgidas, por forma a que a geometria desejada seja alcancada, mas ao
mesmo tempo deve poder ser flexıvel para ser ajustada em qualquer instante de tempo.
Por forma a atingir estes objectivos, alguns passos tiveram de ser dados:
• Implementacao do modelo dinamico e cinematico de um quadricoptero;
• Desenvolvimento de metodos de controlo em quadricopteros;
• Desenvolvimento do metodo de controlo de formacoes e posterior simulacao com qua-
dricopteros;
• Implementacao dos metodos de controlo desenvolvidos para quadricopteros individuais num
prototipo real de um quadricoptero.
1.4 Organizacao
A primeira parte desta tese apresenta a modelacao de um quadricoptero, onde se define a sua
cinematica e dinamica, e tambem se apresenta o estudo e desenvolvimento de varios metodos
de controlo. Ambos implementados em Simulink. A segunda parte, em primeiro lugar descreve
o metodo dos paineis baseados em funcoes harmonicas e, por fim, apresenta o metodo de con-
trolo de formacoes com UAVs. Finalmente, na ultima parte deste trabalho sao apresentados os
primeiros resultados de ensaios em voo com um quadricoptero real.
4
2Modelacao do Quadricoptero
Conteudo2.1 Introducao . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62.2 Definicoes e Conceitos Basicos . . . . . . . . . . . . . . . . . . . . . . . . . . . 62.3 Modelo do Quadricoptero . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 92.4 Metodos de Controlo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 132.5 Simulador . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19
5
2. Modelacao do Quadricoptero
2.1 Introducao
Por forma a alcancar os objectivos propostos e necessario fazer uma caracterizacao detalhada
de um quadricoptero. Neste capıtulo e apresentada uma abordagem de baixo para cima deste
tipo de veıculo. Para isso, comeca-se por definir os sistemas de coordenadas, bem como os
conceitos gerais de manobragem do veıculo (Seccao 2.2), a descricao do modelo matematico
(Seccao 2.3) e os metodos de controlo utilizados (Seccao 2.4). Na Seccao 2.5 e apresentada a
implementacao de um simulador, como tambem sao apresentados os resultados obtidos.
2.2 Definicoes e Conceitos Basicos
2.2.1 Sistemas de Coordenadas
Quando se lida com veıculos aereos e necessario descrever, nao sao a sua posicao, como
tambem a sua atitude. Assim, e segundo [23], sao definidos dois sistemas de coordenadas; um
referencial inercial I fixo a Terra, que esta centrado em OI e que aponta para Norte (xI ), Oeste
(yI ) e cima (zI ), como se pode ver na Figura 2.1(a). O segundo referencial B esta fixo ao
quadricoptero e esta centrado em OB, que corresponde ao centro de massa do veıculo, conforme
se ve na Figura 2.1(b).
Daqui em diante, os vectores expressos no referencial inercial terao o sobrescrito I enquanto
que os vectores expressos no referencial do veıculo terao o sobrescrito B.
(a) Referencial Inercial (b) Referencial do veıculo
Figura 2.1: Sistemas de Coordenadas.
6
2.2 Definicoes e Conceitos Basicos
O referencial inercial e usado para definir a posicao linear (Γ I [m]) do quadricoptero, que
corresponde ao deslocamento entre OI e OB,
Γ I = [X,Y, Z]T (2.1)
e a sua posicao angular (ΘI [rad]) que define a orientacao do referencial B em relacao ao refe-
rencial I. Segundo o teorema de Euler, qualquer rotacao pode ser descrita por tres angulos. Por
norma, na literatura aeroespacial, estas tres rotacoes sao φ, θ e ψ:
ΘI = [φ, θ, ψ]T (2.2)
No referencial B e definida a velocidade linear (V B [m.s−1]),
V B = [u, v, w]T (2.3)
em que u, v e w correspondem a velocidade linear instantanea segundo xB, yB e zB respectiva-
mente, a velocidade angular (ωB [rad.s−1]),
ωB = [p, q, r]T (2.4)
em que p, q e r correspondem a velocidade angular segundo xB, yB e zB respectivamente, as
forcas (FB [N ]) e os binarios (τB [N.m]).
FB = [Fx, Fy, Fz]T (2.5)
τB = [τx, τy, τz]T (2.6)
2.2.2 Manipulacao de um Quadricoptero
Um quadricoptero esta equipado com quatro motores, em que cada um deles e responsavel
por uma certa forca de impulsao e binario em torno do seu centro de rotacao. Como se pode ver
na Figura 2.1, os motores estao emparelhados dois a dois, em que cada par roda com um sentido
diferente. As helices da frente e tras rodam no sentido contrario aos ponteiros do relogio enquanto
a da esquerda e direita rodam no sentido dos ponteiros do relogio. Com esta configuracao deixa
de ser necessario o motor de cauda que existe nos helicopteros comuns.
Sendo o quadricoptero um sistema sub-actuado, ou seja, apesar de ter 6 graus de liberdade,
mas apenas 4 motores, nao e possıvel controlar directamente todos os 6 graus de liberdade, mas
apenas 4. Contudo, e devido a estrutura do veıculo, e relativamente simples escolher quais as
variaveis a controlar. Estas variaveis correspondem aos quatro movimentos basicos que permi-
tem ao veıculo atingir uma certa altitude e atitude (ver Figura 2.2). Para fazer uma descricao
destes movimentos e necessario definir a velocidade angular dos quatro motores (Ω [rad.s−1]),
Ω = [Ω1,Ω2,Ω3,Ω4]T (2.7)
7
2. Modelacao do Quadricoptero
(a) (e)
(b) (f)
(c) (g)
(d) (h)
1
2
3
4
(a)
(b)
(c)
(d)
(e)
(f)
(g)
(h)
Movimento Vertical PositivoMovimento Vertical NegativoRolamento NegativoRolamento Positivo
Inclinação Negativa
Inclinação PositivaGuinada Negativa
Guinada Positiva
Figura 2.2: Movimentos obtidos quando se variam as velocidades angulares de cada motor [1].
que correspondem a velocidade angular do motor da frente, direita, tras e esquerda, respectiva-
mente.
• Altitude - U1 [N ]:
O movimento vertical e obtido aumentando (ou diminuindo) a velocidade angular de todos
os motores, como se pode ver na Figura 2.2(a) e (b).
• Rolamento - U2 [N.m]:
O movimento de rolamento e obtido quando se aumenta (ou diminui) Ω2 e se diminui (ou
aumenta) Ω4. Estas variacoes de velocidade angular geram um binario em torno do eixo
xB que faz o quadricoptero inclinar. Este movimento e apresentado na Figura 2.2(c) e (d) e
esta directamente relacionado com φ.
• Inclinacao - U3 [N.m]:
O movimento de inclinacao e obtido quando se aumenta (ou diminui) Ω1 e se diminui (ou
aumenta) Ω3. Estas variacoes de velocidade angular geram um binario em torno do eixo yB
que faz o quadricoptero inclinar. Este movimento pode ser visto na Figura 2.2(e) e (f) e esta
directamente relacionado com θ.
8
2.3 Modelo do Quadricoptero
• Guinada - U4 [N.m]:
O movimento de guinada e obtido quando se aumenta (ou diminui) o par Ω1 - Ω3 e se diminui
(ou aumenta) o par Ω2 - Ω4. Estas variacoes de velocidade angular geram um binario em
torno do eixo zB que faz o quadricoptero girar. A execucao de guinada so e possıvel devido
ao facto de o par Ω1 - Ω3 girar no sentido contrario ao par Ω2 - Ω4. Este movimento pode
ser visto na Figura 2.2(g) e (h) e esta directamente relacionado com ψ.
2.3 Modelo do Quadricoptero
2.3.1 Principais Pressupostos
Por forma a estabilizar o quadricoptero, alguns pressupostos foram assumidos com o objectivo
de simplificar o modelo matematico descrito nas sub-seccoes seguintes:
• A origem do referencial do veıculo OB e coincidente com o seu centro de massa;
• O quadricoptero e simetrico ao longo de xB e yB;
• O quadricoptero e um corpo rıgido;
• A inclinacao magnetica e desprezada;
• A vibracao dos motores e desprezada;
• A turbulencia causada pela movimentacao de ar junto ao solo e desprezada;
• Todos os sensores a bordo do veıculo estao centrados em OB;
• As nao-linearidades da bateria sao desprezadas;
• Nao existe escorregamento entre as helices e o rotor do motor;
• Os quatro motores sao iguais;
2.3.2 Cinematica
Como ja foi referido na Seccao 2.2, a posicao do veıculo esta definida no referencial Ienquanto que a sua velocidade esta definida no referencial B. Logo, torna-se necessario fazer
a passagem de um referencial para outro. Esta passagem e dada por tres rotacoes (os tres
angulos de Euler) consecutivas em torno dos tres eixos existentes. Neste trabalho, a sequencia
usada e ”φ - θ - ψ”, ou seja, para que o referencial I esteja coincidente com o referencial B, a
primeira rotacao e efectuada segundo ψ (R(ψ)), a segunda segundo θ (R(θ)) e por fim segundo
φ (R(φ))(ver equacao 2.8):
9
2. Modelacao do Quadricoptero
R(ψ) =
cosψ − sinψ 0
sinψ cosψ 0
0 0 1
,R(θ) =
cos θ 0 sin θ
0 1 0
− sin θ 0 cos θ
,R(φ) =
1 0 0
0 cosφ − sinφ
0 sinφ cosφ
(2.8)
Combinando estas tres rotacoes obtem-se a matriz de rotacao final, que esta apresentada na
equacao 2.9:
RΘ = R(ψ).R(θ).R(φ) =
cψcθ cψsθsφ − sψcφ sψsφ + cψsθcφ
sψcθ cψcφ + sψsθsφ sψsθcφ − cψsφ−sθ cθsφ cθcφ
(2.9)
Na equacao anterior (e nas seguintes), por simplificacao, adoptou-se a seguinte notacao:
ck ≡ cos k, sk ≡ sin k e tk ≡ tan k.
Com a matriz de rotacao calculada e agora possıvel relacionar o vector de posicao Γ I com a
velocidade linear V B (Equacao 2.10).
Γ I = RΘVB ⇔
X
Y
Z
= RΘ
u
v
w
(2.10)
O mesmo raciocınio que se fez para a posicao e velocidade linear tambem deve ser feito para
a sua posicao e velocidade angular, uma vez que a primeira se encontra definida no referencial
I e a segunda no referencial B. Assim, define-se a relacao entre estas duas grandezas como
ωB = T−1Θ ΘI ⇔ ΘI = TΘ ω
B, (2.11)
em que TΘ e a chamada matriz de transformacao. Esta transformacao deve-se ao facto de a
velocidade angular Θ depender da diferenca entre o referencial B e o referencial I [24].
Assim, pode calcular-se esta matriz atraves de
p
q
r
= T−1Θ
φ
θ
ψ
=
φ
0
0
+R(φ)−1
0
θ
0
+R(φ)−1R(θ)−1
0
0
ψ
, (2.12)
de onde sai
T−1Θ =
1 0 −sθ0 cθ sφcθ
0 −sφ cφcθ
. (2.13)
10
2.3 Modelo do Quadricoptero
Invertendo a matriz obtem-se
TΘ =
1 sφtθ cφtθ
0 cφ −sφ0 sφ/cθ cφ/cθ
. (2.14)
E de notar que, devido a formulacao das rotacoes de Euler, RΘ e TΘ nao estao definidas para
θ = ±90. Para evitar estas singularidades poder-se-ia usar uma representacao baseada em
quaternioes [25]. Contudo, como se pretende que o veıculo opere longe destas singularidades
(θ ≈ 0 e φ ≈ 0) sera utilizada a representacao em angulos de Euler.
2.3.3 Dinamica
Para descrever matematicamente o comportamento de um veıculo e necessario aplicar a
segunda lei de Newton. Assim, e segundo [23], define-se a lei de Newton aplicada ao movimento
de translacao (Equacao 2.15) e ao movimento de rotacao (Equacao 2.16):
FB = m(V B + ωB × V B) (2.15)
τB = IωB + ωB × (IωB), (2.16)
onde m [Kg] e a massa do veıculo, V B [m.s−2] e a aceleracao linear, ωB [rad.s−2] e a aceleracao
angular e I = diag(Ixx, Iyy, Izz) [N.m.s2] e a matriz de inercia, que se assume diagonal devido a
simetria do veıculo assumida. Desenvolvendo as equacoes acima descritas, obtem-se:
1
mFB =
u
v
w
+
qw − rv
ru− pw
pv − qu
(2.17)
τB +
(Iyy − Izz)qr
(Izz − Ixx)pr
(Ixx − Iyy)pq
= I
p
q
r
(2.18)
As equacoes 2.17 e 2.18 sao genericas e validas para qualquer corpo rıgido. Contudo, e como
sao usadas para modelar o movimento de um quadricoptero, e necessario adicionar a forca da
gravidade e relacionar as forcas e binarios com as entradas do sistema (U1, U2, U3, U4) que geram
os movimentos referidos na seccao anterior [23]:
11
2. Modelacao do Quadricoptero
FB =
Fx
Fy
Fz
=
RTΘ
0
0
−mg
+
0
0
U1
=
m g sθ
−m g cθ sφ
−m g cθ cφ
+
0
0
b(Ω21 + Ω2
2 + Ω23 + Ω2
4)
, (2.19)
τB =
τx
τy
τz
=
U2
U3
U4
=
b l(Ω2
4 − Ω22)
b l(Ω23 − Ω2
1)
d(Ω22 + Ω2
4 + Ω21 + Ω2
3)
, (2.20)
onde g [m.s−2] e a aceleracao da gravidade, b [Kg.m.rad−2] e o coeficiente de propulsao das
helices, d [Kg.m2.rad−2] e o coeficiente de binario das helices e l [m] e a distancia entre o centro
do quadricoptero e o centro das helices.
Concluindo, e agrupando as equacoes 2.10, 2.11, 2.15 e 2.16, e apresentado o modelo geral
do quadricoptero:
X
Y
Z
= RΘ
u
v
w
(2.21a)
φ
θ
ψ
= TΘ
p
q
r
(2.21b)
u
v
w
=
rv − qw
pw − ru
qu− pv
+ g
sθ
−cθsφ−cθcφ
+1
m
0
0
U1
(2.21c)
p
q
r
=
Iyy−IzzIxx
qr
Izz−IxxIyy
pr
Ixx−IyyIzz
pq
+
1IxxU2
1IyyU3
1IzzU4
(2.21d)
U1
U2
U3
U4
=
b (Ω2
1 + Ω22 + Ω2
3 + Ω24)
b l (Ω24 − Ω2
2)
b l (Ω23 − Ω2
1)
d (Ω22 + Ω2
4 − Ω21 − Ω2
3)
(2.21e)
12
2.4 Metodos de Controlo
2.4 Metodos de Controlo
Na seccao anterior, o modelo do quadricoptero foi caracterizado em funcao das velocidades
de rotacao dos quatro motores. Com base nisso, e no facto de se pretender que o veıculo voe
autonomamente, foram considerados dois tipos de controladores. O primeiro e um controlador
PID (Subseccao 2.4.1), que apesar de ser um controlador que nao tem em conta o modelo de
estado do veıculo, tem uma estrutura bastante simples, oferece bastante versatilidade e um bom
desempenho. O segundo e um controlador linear-quadratico (LQR) (Subseccao 2.4.2), que ja
tem em conta o modelo de estado do veıculo.
2.4.1 Controladores PID
Sendo que este tipo de controladores nao tem em conta o modelo de estado do veıculo so e
possıvel controlar a posicao (X, Y , Z) e a atitude (φ, θ, ψ) do quadricoptero. Assim, e como refe-
rido na subseccao 2.2.2, so existe a possibilidade de controlar quatro variaveis directamente, que,
neste caso, sao a altitude (Z), rolamento (φ), inclinacao (θ) e guinada (ψ). As restante grandezas
(X e Y ) serao controladas com base nas ultimas. Na Figura 2.3 e apresentado o diagrama que
representa a arquitectura utilizada. Nesta figura, o subscrito r refere-se aos valores de referencia
de cada variavel, ou seja, e o valor que se pretende que o veıculo atinja para cada variavel.
Controladorde Malha Externa
Controladorde Malha Interna
Modelo do Quadrotor
Figura 2.3: Arquitectura de Controlo PID.
2.4.1.1 Controlador de Malha Interna
Como se pode ver na figura acima, e necessario calcular qual a velocidade angular de cada
motor para que o veıculo atinga cada um dos valores de referencia. Para isso, inverte-se as
13
2. Modelacao do Quadricoptero
equacoes descritas em (2.21). Contudo, estas equacoes nao tem uma solucao unica, o que torna
esta tarefa bastante complicada. No entanto, e como se pretende que o quadricoptero esteja em
voo pairado (θ ≈ 0 e φ ≈ 0), os produtos externos presentes nas equacoes e provenientes dos
efeitos giroscopicos e de Coriolis podem ser desprezados, pelo que se obtem:
Z = −g + (cθcφ)U1
m
φ = U2
Ixx
θ = U3
Iyy
ψ = U4
Izz
(2.22)
As equacoes presentes em 2.22 sao facilmente invertıveis pelo que se consegue obterU1,U2,
U3 e U4. Utilizando estes valores, calcula-se as velocidades de rotacao dos motores invertendo-
se a equacao 2.21e):
Ω2
1
Ω22
Ω23
Ω24
=
14b 0 − 1
2bl −14d
14b −
12bl 0 1
4d
14b 0 1
2bl − 14d
14b
12bl 0 1
4d
U1
U2
U3
U4
. (2.23)
Por definicao, o controlador PID classico e dado por:
U(t) = KP e(t) +KI
∫ t
0e(τ) dτ +KD
de(t)
dt, (2.24)
onde U e a variavel a controlar, KP o ganho proporcional, KI o ganho integral, KD o ganho
derivativo e e o erro, ou seja, e a diferenca entre o valor de referencia da variavel e a propria
variavel. Desta forma, definem-se os varios erros:
eφ = φr − φ
eθ = θr − θ
eψ = ψr − ψ
eZ = Zr − Z
(2.25)
14
2.4 Metodos de Controlo
Usando as equacoes 2.22 e 2.24, e passando para o domınio de Laplace, obtem-se entao os
varios controladores de malha interna:
U1 =
[ [(KPZ +
KIZ
s
)eZ(s)− sKDZ Z
]+ g
].m
cφcθ
U2 =
[(KPφ +
KIφ
s
)eφ(s)− sKDφ φ
].Ixx
U3 =
[(KPθ +
KIθ
s
)eθ(s)− sKDθ θ
].Iyy
U4 =
[(KPψ +
KIψ
s
)eψ(s)− sKDψ ψ
].Izz
(2.26)
E de notar que nas equacoes acima descritas se efectua a derivada, nao do erro, mas da
propria variavel do sistema. Este pormenor deve-se ao facto de o erro poder nao ser diferenciavel,
o que poderia provocar a saturacao dos actuadores.
2.4.1.2 Controlador de Malha Externa
Este controlador tem como objectivo fazer o controlo de posicao do veıculo, ou seja, de X e
de Y . Assim, e como se pode ver na Figura 2.3, este controlador tem a sua entrada os valores
de referencia Xr e Yr, ou seja, a posicao desejada para o veıculo, e a sua saıda os valores
de referencia de rolamento (φr) e inclinacao (θr) que entrarao posteriormente no controlador de
malha interna. Logo, e necessario relacionar Xr e Yr com φr e θr. Desta forma, e segundo [26],
lineariza-se a equacao de 2.21a), obtendo-se:
X = θrcψ + φrsψ
Y = θrsψ − φrcψ(2.27)
Atraves desta equacao e possıvel calcular quais sao as aceleracoes lineares para uns deter-
minados valores de φr e θr. Contudo como o que se pretende e precisamente o inverso, inverte-se
as equacoes 2.27, de onde resulta:
φr = Xsψ − Y cψθr = Xcψ + Y sψ
(2.28)
15
2. Modelacao do Quadricoptero
Com isto e entao possıvel construir os controladores desejados, definindo para isso os se-
guintes erros:
eX = Xr −X
eY = Yr − Y(2.29)
Assim, e usando mais uma vez a equacao 2.24 no domınio de Laplace, resultam o seguintes
controladores:
X =(KPX +
KIX
s
)eX(s)− sKDX X
Y =(KPY +
KIY
s
)eY (s)− sKDY Y
(2.30)
2.4.2 Controlador LQR
Como ja foi referido, os controladores PID sao controladores que nao tem em conta o modelo
de estados do veıculo, mas sim o modelo de Entrada/Saıda. Assim, nesta subseccao, e apre-
sentada uma abordagem baseada na teoria de controlo optimo, que usa o modelo de estado do
veıculo. Este tipo de controlo tem como objectivo principal fornecer o melhor desempenho rela-
tivamente a uma medida que, neste caso, e a energia do sinal de controlo [27]. Para isso sera
desenvolvido um controlador LQR (Regulador linear quadratico), no qual a energia do sinal de
controlo e dada por uma funcao de custo.
2.4.2.1 Formulacao Teorica
Qualquer modelo matematico linear pode ser descrito na representacao de espaco de estados
por:
x = Ax+Bu (2.31)
em que x e o vector de estados e u e o vector de entrada do sistema. Com isto, e segundo [28],
este controlador LQR consiste em descobrir qual o vector de entrada optimo
u = −KLQR x (2.32)
que minimize a funcao de custo
JLQR =
∫ ∞0
(xT Q x+ uT R u
)dt (2.33)
16
2.4 Metodos de Controlo
Nas equacoes acima, KLQR e a matriz de ganhos optimos enquanto que Q e R sao duas
matrizes de ponderacao simetricas e definidas positivas. O ganho KLQR e calculado atraves de:
KLQR = R−1BT P , (2.34)
em que P e dado pela solucao da equacao de Riccati:
AT P + P A− P B R−1BT P + Q = 0 (2.35)
As matrizes de ponderacao Q e R sao responsaveis pelo trade-off da importancia dada entre
o sinal de controlo e o vector de estados. Estas matrizes sao inicialmente determinadas pela
regra de Bryson, que as define como sendo matrizes diagonais e dadas por:
Q =1
x2i,max
R =1
u2i,max
(2.36)
onde xi,max e o valor mais alto toleravel para o estado xi e ui,max e o valor mais alto toleravel
para a entrada ui.
2.4.2.2 Implementacao Pratica
Como ja foi referido anteriormente, o controlador LQR apenas lida com sistemas lineares, e
sendo que o quadricoptero apresenta um modelo nao-linear e necessario lineariza-lo [29]. Para
isso, efectuou-se a linearizacao em torno do ponto de funcionamento correspondente a um voo
pairado (θ ≈ 0 e φ ≈ 0). Assim, comeca-se por definir o vector de estados x e o vector de
entrada u:
x = [X, Y, Z, φ, θ, ψ, u, v, w, p, q, r]T (2.37)
u = [U1, U2, U3, U4]T (2.38)
17
2. Modelacao do Quadricoptero
Desta forma, e linearizando as equacoes presentes em 2.21, obtem-se as matrizes A e B:
A =
0 0 0 0 0 0 1 0 0 0 0 00 0 0 0 0 0 0 1 0 0 0 00 0 0 0 0 0 0 0 1 0 0 00 0 0 0 0 0 0 0 0 1 0 00 0 0 0 0 0 0 0 0 0 1 00 0 0 0 0 0 0 0 0 0 0 10 0 0 0 g 0 0 0 0 0 0 00 0 0 −g 0 0 0 0 0 0 0 00 0 0 0 0 0 0 0 0 0 0 00 0 0 0 0 0 0 0 0 0 0 00 0 0 0 0 0 0 0 0 0 0 00 0 0 0 0 0 0 0 0 0 0 0
(2.39)
B =
0 0 0 0
0 0 0 0
0 0 0 0
0 0 0 0
0 0 0 0
0 0 0 0
0 0 0 0
0 0 0 01m 0 0 0
0 1Ixx
0 0
0 0 1Iyy
0
0 0 0 1Izz
(2.40)
Calculando os valores proprios da matriz A e possıvel verificar que os doze polos do sistema
se encontram na origem, pelo que se conclui que o sistema e instavel ou marginalmente estavel.
Com as matrizes A e B definidas e entao possıvel calcular o ganho optimo KLQR.
Na Figura 2.4 e apresentado o diagrama estrutural do controlador LQR.
Modelo do Quadrotor
LQR
Figura 2.4: Arquitectura de Controlo LQR.
18
2.5 Simulador
2.5 Simulador
Nesta subseccao, e apresentado o simulador do quadricoptero. Esta ferramenta e bastante util
para verificar o desempenho do modelo do veıculo, bem como para testar os metodos de controlo
desenvolvidos. Este simulador foi desenvolvido utilizando a ferramenta Simulink R© presente no
software MATLAB R©.
A subseccao 2.5.1 fornece um visao geral da arquitectura do simulador, bem como uma breve
descricao da funcao de cada bloco. Na subseccao 2.5.2 e apresentada a implementacao dos
blocos do simulador previamente descritos, e por fim na subseccao 2.5.3 sao apresentados os
resultados obtidos.
2.5.1 Arquitectura do Simulador
Como ja foi referido, o simulador foi implementado no Simulink. Esta ferramenta proporciona
um ambiente grafico interactivo e um conjunto de bibliotecas que permitem, com alguma facili-
dade, desenhar, implementar, simular e testar uma enorme variedade de sistemas.
Na Figura 2.5 mostra-se a arquitectura do simulador. Como se pode ver, o sistema e composto
por varios blocos, cada um com uma funcao especıfica.
Simulator
State Scope
Desired
State Vector
Sensors
State VectorState Vector’
Inputs
Task
Desired
Dynamics
Omega VectorState Vector
Control
Task
State VectorOmega
3D View
State Vector
Figura 2.5: Arquitectura do Simulador.
19
2. Modelacao do Quadricoptero
O bloco Dynamics (Laranja) e um dos blocos fundamentais deste simulador. Neste bloco
encontra-se todo o modelo do veıculo, ou seja, contem as equacoes descritas em 2.21.
O bloco Sensors (Amarelo) tem como funcao adicionar ruido as medicoes dos sensores,
numa tentativa de aproximar a simulacao a realidade.
O bloco Inputs (Cinzento) e responsavel por adquirir os valores de referencia do vector de
estado.
No bloco Control (Verde) estao presentes os metodos de controlo apresentados na Seccao
2.4.
Os blocos 3D View (Vermelho) e State Scope (Azul) tem como objectivo permitir a visualizacao
da evolucao do vector de estados. O primeiro apresenta um modelo 3D do veıculo, o que permite
ter uma nocao espacial do seu funcionamento, enquanto que o segundo apresenta graficos da
evolucao temporal das varias grandezas.
2.5.2 Implementacao
Como ja foi referido, no bloco Dynamics encontra-se o modelo do quadricoptero. Este bloco
tem como entrada a velocidade de rotacao dos quatro motores (Equacao 2.7) e, como saıda, o
vector de estados do sistema (Equacao 2.37). A Figura 2.6 mostra a composicao deste bloco.
Dynamics
[X, Y, Z,φ, θ, ψ, u, v, w, p, q, r]
[u, v, w,φ, θ, ψ]
[φ, θ, p, q ,r]
[φ, θ, u, v, w, p, q, r]
[Ω1, Ω
2, Ω
3, Ω
4]
[p, q, r]
[dp, dq, dr]
[du, dv, dw]
[dφ, dθ, dψ]
[dX, dY, dZ]
State Vector
1
Transformation Matrix
phi_theta_p_q_rdphi_dtheta_dpsi Transformation
Selector5
UY
Selector4
UY
Selector3
UY
Selector2
UY
Selector 1
UY
Roation Matrix
phi_theta_psi_u_v_wdx_dy_dz Rotation
Omega 2 U
Omega_Vector
U1
U2
U3
U4
Omega
fcn
Integrator
1sxo
Initial State Condition
StateI
Gyro Effects
U2
U3
U4
Omega
p_q_r
dp_dq_dr Gyro
Display
Body Acc
phi_theta_u_v_w_p_q_r
U1du_dv_dw BodyAcc
Omega Vector1
Figura 2.6: Implementacao do bloco Dynamics.
20
2.5 Simulador
Este bloco foi implementado usando Embedded Functions do Simulink, pois permite reduzir
bastante o tempo de computacao, o que torna a simulacao mais rapida.
O vector de velocidade linear ([dX, dY, dZ]) e obtido atraves do bloco Rotation Matrix (Verde),
no qual se aplica a equacao 2.21a).
O vector de velocidade angular ([dφ, dθ, dψ]) e obtido atraves do bloco Transformation Matrix
(Laranja), no qual se aplica a equacao 2.21b).
O bloco Omega 2 U (Amarelo) tem como objectivo fazer a conversao da velocidade de rotacao
dos motores para o vector de forcas e binarios (Equacao 2.21e)).
O vector de aceleracao linear ([du, dv, dw]) e calculado atraves do bloco Body Acc (Verme-
lho), no qual esta implementada a equacao 2.21c).
O vector de aceleracao angular ([dp, dq, dr]) e calculado atraves do bloco Gyro Effects (Azul),
no qual esta implementada a equacao 2.21d).
O bloco Initial State Condtion (Rosa) tem como objectivo inicializar os vector de estados do
sistema.
No bloco Inputs sao recebidos os valores de referencia do estado que se pretende que o
sistema atinja. Na Figura 2.7 e apresentada uma imagem da composicao deste bloco.
Inputs
Desired2
Task1
Switch1
~= 0
Switch
~= 0
Signal Builder
Desired X
Desired Y
Desired Roll
Desired Pitch
Desired Yaw
Desired Height
Joypad Processing
Axes
ButtonsTask
Joypad Input
joyinputAxes
Buttons
JoyFlag
JoyFlag
InputFlag
InputFlag
Desired Yaw
0
Desired Y
0
Desired X
0
Desired State Vector
In1 Out1
Desired Roll
0
Desired Pitch
0
Desired Height
0
Figura 2.7: Implementacao do bloco Inputs.
21
2. Modelacao do Quadricoptero
Como se pode ver na figura existem tres metodos para a introducao de valores de referencia.
No primeiro (Verde) inserem-se valores que serao constantes ao longo de toda a simulacao. O
segundo metodo faz uso do bloco Signal Builder (Laranja), que permite gerar sinais que variem
ao longo do tempo. O terceiro metodo consiste no uso de um Joypad para controlar o veıculo. O
bloco Joypad Input faz a leitura dos comandos enviados pelo Joypad atraves da porta USB, en-
quanto que o bloco Joypad Processing (Azul) converte esses mesmos comandos nas variaveis
de referencia do sistema. Este metodo de entrada permite que um operador humano, sem ne-
nhuma experiencia neste tipo de veıculos, possa facilmente ambientar-se com a dinamica de um
quadricoptero.
No bloco Control encontram-se implementados os metodos de controlo (PID e LQR) descri-
tos na Seccao 2.4. Na Figura 2.8 e apresentada a constituicao deste bloco.
Control
Omega1
Switch
~= 0
PID
Task
State Vector
U Vector
Omega Vector
LQRFlag
LQRFlag
LQR
Task
State Vector
Out1
Inverted Movements Matrix
U_Vector Omega_vectorf
Initial Motor Speed
[OmegaI]
State Vector2
Task1
Figura 2.8: Implementacao do bloco Control.
Como se pode ver na figura, a verde encontra-se o controlador PID e a laranja o controlador
LQR.
O bloco Inverted Movements Matrix (Amarelo) faz a conversao do vector de forcas e binarios
para a velocidade de rotacao dos motores (Equacao 2.23).
O bloco Initial Motor Speed (Azul) permite que se definam as velocidades iniciais de rotacao
dos motores.
O semaforo LQRFlag permite escolher que controlador usar, por forma a ser possıvel fazer
uma comparacao entre os dois.
22
2.5 Simulador
Na Figura 2.9 apresenta-se o bloco PID. Os blocos a verde sao os controladores de malha in-
terna (Equacao 2.26) enquanto que o bloco a laranja (Position Control) sao os controladores de
malha externa (Equacao 2.30). E de notar que todos os ganhos presentes nestes controladores
foram obtidos atraves da ferramenta System Identification Toolbox. Esta ferramenta permite que
se calculem os ganhos que fornecam o melhor desempenho ao sistema.
PID
U Vector1
Yaw Control
Desired Yaw_Dot
Yaw
U4
Switch1
~= 0
Switch
~= 0
Selector7
U Y
Selector6
U Y
Selector5
U Y
Selector4
U Y
Selector3
U Y
Selector2
U Y
Selector1
U Y
Selector
U Y
Roll Control
Desired Roll
Roll
U2
Position Control Flag
ctrl_Pos
Position Control
Desired X and Y
State Vector
Desired Roll
Desired Pitch
Pitch Control
Desired Pitch
Pitch
U3
Height Control
Desired Height
Height
U1
State Vector2
Task1
Figura 2.9: Implementacao do bloco PID.
O bloco LQR e basicamente composto por um ganho (Verde) e por uma rotacao (Laranja),
como se pode ver na Figura 2.10.
LQR
Out11
Rotation
Task
State_Vector
Outf
Linearization Point
(g 0 0 0)
LQR Gain
Klqr* uvec
State Vector2
Task1
Figura 2.10: Implementacao do bloco LQR.
23
2. Modelacao do Quadricoptero
O ganho optimo, KLQR, foi calculado usando a funcao lqr presente no MATLAB. E de notar
que quando se procedeu a linearizacao do modelo do quadricoptero, esta foi feita em torno de
ψ ≈ 0, logo e necessario efectuar uma rotacao, em torno de zB, do vector de estado. Esta
rotacao e efectuada no bloco Rotation (Laranja).
O bloco Sensors, como ja foi referido, tem como funcao introduzir algum erro no vector de
estados do sistema, com o objectivo de, nao so tornar a simulacao mais realista, como tambem
para testar a robustez dos controladores.
O bloco State Scope (Figura 2.11) e um bloco com uma constituicao relativamente simples,
pois faz uso do bloco Scope (Laranja) para a visualizacao das variaveis de estado. Este bloco tem
ainda como entrada os valores devolvidos pelo bloco Inputs, por forma a que se possa tambem
visualizar os valores de referencia que estao a ser enviados para o sistema.
State Scope
X , Y , Z & u , v , w
Selector6
U Y
Selector5
U Y
Selector4
U Y
Selector3
U Y
Selector2
U Y
Selector1
U Y
Selector
U Y
Roll , Pitch , Yaw & p , q , r
Radiansto Degrees1
R2D
Radiansto Degrees
R2D
Omega Vector
State Vector2
Desired1
Figura 2.11: Implementacao do bloco State Scope.
Por sua vez, o bloco 3D View , utiliza um modelo grafico VRML (Figura 2.12) para que seja
possıvel visualizar o comportamento do veıculo num mundo virtual em tres dimensoes, sendo
assim mais facil para o olho humano interpretar a evolucao do sistema.
24
2.5 Simulador
Figura 2.12: Modelo VRML do quadricoptero (Modificado a partir de [2]).
2.5.3 Simulacao
Esta subseccao contem a analise do desempenho dos controladores desenvolvidos usando o
simulador. Para uma melhor avaliacao dos resultados obtidos e conveniente comecar por apre-
sentar o controlo de cada variavel de estado individualmente e sao depois apresentar os resul-
tados de casos em que haja multiplas variaveis a ser controladas. Seguindo esta filosofia, foram
concebidos dois conjuntos de testes.
O primeiro consiste em excitar a posicao ([X,Y, Z]) e a guinada (ψ) do veıculo, individual-
mente. Neste primeiro conjunto de testes cada variavel ira ser excitada atraves de um escalao
unitario e, a partir dos resultados, faz-se uma caracterizacao dos controladores.
O segundo conjunto de testes consiste em excitar tambem a posicao ([X,Y, Z]) e a guinada
(ψ) mas desta vez em simultaneo e usando diferentes formas de onda. E de salientar que ambos
os conjuntos de testes serao aplicados aos controladores PID e LQR.
Para que seja possıvel efectuar estas simulacoes e necessario definir alguns parametros do
modelo matematico do veıculo. Contudo, como o calculo destes parametros sai fora do ambito
deste trabalho, foram assumidos valores tıpicos [23] para este tipo de grandezas. O valor destes
parametros e apresentado na tabela 2.1.
25
2. Modelacao do Quadricoptero
Designacao Sımbolo ValorMassa m 1Kg
Comprimento da haste l 0, 24m
Coeficiente de propulsao das helices b 54, 2× 10−6 N.s2.rad−2
Coeficiente de binario das helices d 1, 1× 10−6 N.m.s2.rad−2
Matriz de Inercia I
8, 1 0 0
0 8, 1 0
0 0 14, 2
× 10−3 N.m.s2
Tabela 2.1: Parametros utilizados para simulacao.
Resposta ao Escalao Unitario
No primeiro conjunto de testes, para avaliar o desempenho dos controladores, irao ser usados
tres parametros fundamentais: tempo de subida (tR), tempo de estabelecimento (tS) e sobre-
elevacao (S). O tempo de subida (tR) corresponde ao tempo que que a resposta demora entre a
primeira vez que cruza um determinado limite inferior e a primeira vez que cruza um determinado
limite superior. Estes limites sao definidos em percentagem do valor final e neste caso usou-se o
intervalo de 10%− 90%. O tempo de estabelecimento (tS) e o tempo ao fim do qual a resposta se
encontra definitivamente dentro de uma determinada margem em torno do valor final. E habitual
definir-se a largura dessa margem em percentagem do valor final, e e frequente a utilizacao
duma margem de ± 2%. A sobre-elevacao (S) e a diferenca entre o valor maximo e o valor final
da resposta e e medida como percentagem.
Na Figura 2.13 apresenta-se a resposta do sistema ao escalao unitario utilizando os controla-
dores PID e na tabela 2.2 os respectivos parametros fundamentais.
0 2.5 5 7.5 10 12.5 150
0.5
1
1.5X
X[m
]
DesejadoResposta
0 2.5 5 7.5 10 12.5 150
0.5
1
1.5Y
Y[m
]
DesejadoResposta
0 2.5 5 7.5 10 12.5 150
0.5
1
1.5Z
Z[m
]
DesejadoResposta
0 2.5 5 7.5 10 12.5 150
15
30
45
6065ψ
Tempo [s]
ψ[d
eg]
DesejadoResposta
Figura 2.13: Resposta ao escalao unitario para os controladores PID.
26
2.5 Simulador
X Y Z ψ
tR [s] 2.48 2.50 1.15 2.11
tS [s] 8.6 8.66 3.67 4.82
S [%] 6.99 7.06 2.28 0
Tabela 2.2: Parametros fundamentais para os controladores PID.
Para o controlador LQR apresenta-se a resposta ao escalao na Figura 2.14 e os respectivos
parametros na tabela 2.3.
0 2.5 5 7.5 10 12.5 150
0.5
1
1.5X
X[m
]
DesejadoResposta
0 2.5 5 7.5 10 12.5 150
0.5
1
1.5Y
Y[m
]
DesejadoResposta
0 2.5 5 7.5 10 12.5 150
0.5
1
1.5Z
Z[m
]
DesejadoResposta
0 2.5 5 7.5 10 12.5 150
15
30
45
6065ψ
Tempo [s]
ψ[d
eg]
DesejadoResposta
Figura 2.14: Resposta ao escalao unitario para o controlador LQR.
X Y Z ψ
tR [s] 1.59 1.59 1.77 0.95
tS [s] 5.14 5.14 5.58 3.70
S [%] 3.84 3.84 3.08 4.34
Tabela 2.3: Parametros fundamentais para o controlador LQR.
Atraves das figuras e das tabelas acima apresentadas verifica-se que, com ambos os contro-
ladores, o sistema consegue seguir a referencia sem qualquer problema. Contudo, constata-se
que o controlador LQR apresenta um melhor desempenho, comparando com o controlador PID,
em relacao A s variaveis X e Y . Para este controlador e para estas variaveis, o tempo de subida,
o tempo de estabelecimento e a sobre-elevacao sao menores que com o controlador PID. Esta
diferenca deve-se essencialmente ao facto de as variaveis X e Y serem controladas por dois
controladores (controlador de malha externa e interna), o que introduz algum atraso em toda a
malha de controlo.
27
2. Modelacao do Quadricoptero
Ao nıvel da deslocacao vertical verifica-se que o controlador PID apresenta melhores resul-
tados, com um tempo de estabelecimento aproximadamente 2 segundos inferior ao conseguido
pelo controlador LQR.
Em relacao a guinada (ψ) verifica-se que o controlador LQR apresenta um melhor desem-
penho, pois tem um tempo de subida e estabelecimento inferior ao controlador PID. Contudo,
o controlador PID apresenta uma sobre-elevacao de 0%, o que pode ser uma vantagem se se
pretender que o sistema tenha um comportamento mais suave.
E de notar que ambos os controladores sao muito dependentes dos ganhos introduzidos, pelo
que com o ajuste destes, e ainda possıvel obter melhores resultados. No entanto, e de uma forma
geral, constata-se que ambos os controladores se encontram bem dimensionados e apresentam
um desempenho bastante satisfatorio.
Multiplas Entradas de Referencia
Nas figuras 2.15 e 2.16 apresentam-se os resultados deste segundo conjunto de testes para
o controlador PID e LQR, respectivamente.
0 5 10 15 20 25 30 35 40 45 50
0
1
2
X
X[m
]
DesejadoResposta
0 5 10 15 20 25 30 35 40 45 50−0.5
0
0.5
1
1.5Y
Y[m
]
DesejadoResposta
0 5 10 15 20 25 30 35 40 45 500
1
2
Z
Z[m
]
DesejadoResposta
0 5 10 15 20 25 30 35 40 45 500
50
100ψ
Tempo [s]
ψ[d
eg]
DesejadoResposta
Figura 2.15: Resposta a multiplas entradas de referencia para os controladores PID.
28
2.5 Simulador
0 5 10 15 20 25 30 35 40 45 50
0
1
2
X
X[m
]
DesejadoResposta
0 5 10 15 20 25 30 35 40 45 50−0.5
0
0.5
1
1.5Y
Y[m
]
DesejadoResposta
0 5 10 15 20 25 30 35 40 45 500
1
2
Z
Z[m
]
DesejadoResposta
0 5 10 15 20 25 30 35 40 45 500
50
100ψ
Tempo [s]
ψ[d
eg]
DesejadoResposta
Figura 2.16: Resposta a multiplas entradas de referencia para o controlador LQR.
Como se pode ver nas figuras acima, mais uma vez, ambos os controladores mostraram ser
capazes de seguir uma determinada referencia. No entanto, verifica-se que, em ambos os casos
e entre os segundos 15 e 40, o veıculo nao foi capaz de seguir a referencia em X e em Y devido
a sua frequencia. E de salientar que nos resultados do controlador LQR se verificam algumas
perturbacoes numa determinada variavel quando alguma das outras se altera. Por exemplo,
aos 12 segundos quando e dada uma referencia em X, existe uma perturbacao em Z. Outro
caso acontece por volta dos 37 segundos, quando se altera a referencia de ψ, tanto X como Y
sofrem uma pequena perturbacao. Este fenomeno deve-se a linearizacao que foi feita do modelo
matematico do sistema. Como a linearizacao foi feita em torno de um ponto de funcionamento
correspondente a um voo pairado (θ ≈ 0, φ ≈ 0 e ψ ≈ 0), quando o sistema se afasta deste
ponto, o controlo comeca a deteriorar o seu desempenho. De qualquer das formas, e como ja
foi referido na resposta ao escalao unitario, ambos os controladores estao aptos a controlar o
sistema, mantendo-o nos valores de referencia desejados.
29
3Controlo de Formacoes
Conteudo3.1 Introducao . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 323.2 Formulacao de Funcoes Harmonicas . . . . . . . . . . . . . . . . . . . . . . . . 323.3 Metodo dos Paineis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35
31
3. Controlo de Formacoes
3.1 Introducao
Este capıtulo foca-se no controlo e coordenacao de multiplos veıculos com o objectivo destes
se deslocarem em grupo, mantendo posicoes relativas entre si.
Ao nıvel de controlo de formacoes, este trabalho tem como objectivo desenvolver um metodo
que, de uma forma descentralizada, mantenha todos os veıculos segundo uma certa geome-
tria previamente definida pelo utilizador. Desta forma, e do ponto de vista da descentralizacao,
considera-se que cada veıculo tem como principal objectivo fazer o planeamento de um cami-
nho, ou seja, num ambiente livre de obstaculos estruturais, cada veıculo tem de se colocar para
uma determinada posicao (ponto de chegada), que o coloque em formacao, e ao mesmo tempo
contornar os outros veıculos, que, do ponto de vista do veıculo sao meros obstaculos.
Como ja foi referido no Capıtulo 1, existem inumeros metodos de planeamento de caminho,
contudo este trabalho baseia-se no uso de Campos Potenciais Artificiais gerados atraves de
Funcoes Harmonicas. Assim, na Seccao 3.2 serao introduzidas as funcoes harmonicas e na
Seccao 3.3 e apresentado o Metodo dos Paineis, bem como um metodo que torna os campos
potenciais criados mais aptos para mudancas dinamicas do ambiente.
3.2 Formulacao de Funcoes Harmonicas
Conforme proposto em [3], as funcoes harmonicas sao solucao da chamada equacao de
Laplace na seguinte forma:
∇2φ = 0 (3.1)
onde ∇2 ≡ V.V e o Laplaciano (ou operador de Laplace), V e um campo vectorial de velocidade
e φ e a chamada funcao potencial de velocidade.
No mundo real, existem muitos comportamentos, em varios campos da ciencia, que sao des-
critos pela equacao de Laplace, como por exemplo na mecanica de fluıdos ou no electromagne-
tismo.
Estas funcoes harmonicas gozam de tres propriedades fundamentais, que as tornam ideais
para o trabalho desenvolvido. Essas tres propriedades sao:
• Propriedade de Sobreposicao: Esta propriedade esta relacionada com a linearidade da
equacao de Laplace, isto e, considerando φ1 e φ2 duas funcoes harmonicas, entao qualquer
combinacao linear de φ1 e φ2 e tambem uma funcao harmonica e solucao da equacao de
Laplace.
32
3.2 Formulacao de Funcoes Harmonicas
• Propriedade do Potencial Maximo: O valor maximo de uma funcao harmonica nao-constante
esta situada na sua fronteira e na qual o potencial tende para infinito.
• Propriedade do Potencial Mınimo: O valor mınimo de uma funcao harmonica nao-constante
esta situada na sua fronteira e na qual o potencial tende para menos infinito.
As propriedades das funcoes harmonicas acima descritas sao bastante uteis para a construcao
de campos potenciais artificiais, pois eliminam completamente a existencia de mınimos locais.
Num espaco a 2 dimensoes, a equacao (3.2) representa uma funcao harmonica com simetria
esferica e centrada na origem [3]:
φ(r) =λ
2πln(r), (3.2)
onde r e a distancia euclideana a origem e λ e um escalar que define a forca da funcao harmonica.
Da equacao (3.2) observa-se que qualquer funcao harmonica com simetria esferica tem uma
singularidade na origem (r = 0) e por consequente nao e harmonica nesse ponto. De acordo
com a propriedade do potencial maximo e mınimo, verifica-se que o maximo ou o mınimo destas
funcoes harmonicas encontra-se na origem. Contudo, como se pode deslocar a origem para
qualquer ponto do espaco, pois a equacao de Laplace e invariante a translacoes, se se a colocar
nos obstaculos presentes entao e possıvel construir um campo potencial sem mınimos locais e
apenas com um mınimo global.
Em dinamica de fluıdos a equacao (3.2) e conhecida por sumidouro ou fonte, dependendo do
sinal de λ, ou seja, se λ < 0 e uma fonte, caso contrario se λ > 0 e um sumidouro. Assim, uma
fonte pode ser usada para modelar os obstaculos enquanto que um sumidouro pode ser usado
para modelar o ponto para o qual se pretende que o veıculo se dirija (Ponto de Chegada). Nas
Figuras 3.1 e 3.2 sao apresentados os dois casos num espaco a duas dimensoes, bem como o
campo vectorial gerado por cada um deles.
Outra funcao harmonica que sera usada para a construcao de um campo potencial artificial,
e mais tarde para efectuar o controlo de formacoes, e uma funcao conhecida por Fluxo Uniforme,
que varia linearmente ao longo da direccao do fluxo. Num espaco a 2 dimensoes, quando o fluxo
flui numa direccao que faz um angulo α com o eixo das abcissas (X), esta funcao harmonica e
dada por:
φ(x, y) = −U(x. cosα+ y. sinα), (3.3)
onde U e um escalar que define a forca do fluxo uniforme. Na Figura 3.3 apresenta-se uma
representacao do fluxo uniforme.
33
3. Controlo de Formacoes
−2−1.5
−1−0.5
00.5
11.5
2
−2−1.5
−1−0.5
00.5
11.5
2−6
−5
−4
−3
−2
−1
0
1
2
XY
Attr
activ
eP
oten
tial
(a) Campo Potencial
X
Y
−2 −1.5 −1 −0.5 0 0.5 1 1.5 2−2
−1.5
−1
−0.5
0
0.5
1
1.5
2
(b) Campo Vectorial Normalizado
Figura 3.1: Representacao de um sumidouro (λ = 1) para um espaco a 2 dimensoes.
−2−1.5
−1−0.5
00.5
11.5
2
−2−1.5
−1−0.5
00.5
11.5
2−2
−1
0
1
2
3
4
5
6
XY
Rep
ulsi
veP
oten
tial
(a) Campo Potencial
X
Y
−2 −1.5 −1 −0.5 0 0.5 1 1.5 2−2
−1.5
−1
−0.5
0
0.5
1
1.5
2
(b) Campo Vectorial Normalizado
Figura 3.2: Representacao de uma fonte (λ = −1) para um espaco a 2 dimensoes.
−2−1.5
−1−0.5
00.5
11.5
2
−2−1.5
−1−0.5
00.5
11.5
20
0.5
1
1.5
2
2.5
3
XY
Uni
form
Flow
Figura 3.3: Representacao do Fluxo Uniforme (U = 1) para um espaco a 2 dimensoes.
34
3.3 Metodo dos Paineis
A funcao harmonica de fluxo uniforme possibilita a criacao de um campo potencial mais eficaz,
proporcionando um campo potencial linearmente decrescente desde um ponto de partida ate um
ponto de chegada.
3.3 Metodo dos Paineis
3.3.1 Painel Unico
Num espaco a 2 dimensoes qualquer objecto pode ser aproximado por um conjunto de seg-
mentos de recta. Mais uma vez, em dinamica de fluıdos, esses segmentos de recta sao chamados
de paineis [3]. Assim, como se pretende que os veıculos se desviem dos obstaculos, e necessario
definir o potencial repulsivo de cada painel. Na Figura 3.4 esta ilustrado um exemplo de um painel
(Vermelho), no qual estao distribuıdas uniformemente varias fontes, cada uma com uma forca por
unidade de comprimento de λ.
Figura 3.4: Exemplo de um painel (Modificado a partir de [3])
Com base na equacao (3.2), o potencial num qualquer ponto arbitrario (xi, yi) induzido pelas
fontes contidas num comprimento infinitesimal dl, e dado por:
dφ =λdl
2πln(r) =
−λ2π
ln√x2i + (yi − l)2dl (3.4)
onde l e a ordenada do elemento dl. Desta forma, o potencial induzido por todo o painel e dado
por:
φ(xi, yi) =λ
4π
∫ L
−Lln(x2i + (yi − l)2
)dl. (3.5)
35
3. Controlo de Formacoes
Aplicando as derivadas parciais, sendo x e y, a equacao (3.5) e possıvel obter o campo de
velocidades nas suas duas componentes (ux e y):
ux(xi, yi) = −∂φ∂x
=−λ2π
[arctan
(yi + L
xi
)− arctan
(yi − Lxi
)], (3.6)
uy(xi, yi) = −∂φ∂y
=−λ2π
ln
(x2i + (yi + L)2
x2i + (yi − L)2
). (3.7)
Analisando a equacao (3.6) verifica-se que o valor dos limites da velocidade normal ao painel
sao, para o lado esquerdo e para o lado direito do painel respectivamente, ux(0−, yi) = λ/2 e
ux(0+, yi) = −λ/2 quando −L < yi < L. Este facto sugere que um painel, com uma forca por
unidade de comprimento de λ, cria uma velocidade normal a sua superfıcie de λ/2. Fazendo a
mesma analise para uy, verifica-se que a velocidade tangencial ao painel e zero no seu centro e
vai aumentando em modulo a medida que se caminha para a sua extremidade. Na Figura 3.5 e
apresentado o campo vectorial normalizado para um painel com λ = −1 e L = 0.5 num espaco a
2 dimensoes.
−2.5 −2 −1.5 −1 −0.5 0 0.5 1 1.5 2 2.5−2.5
−2
−1.5
−1
−0.5
0
0.5
1
1.5
2
2.5
X
Y
Figura 3.5: Representacao do campo vectorial normalizado para um painel com λ = −1 e L = 0.5.
36
3.3 Metodo dos Paineis
Como ja foi referido anteriormente, uma das principais propriedades das funcoes harmonicas
e a sobreposicao. Contudo, para gerar um campo potencial que permita que um veıculo chegue
dum ponto a outro, e necessario fazer a analise a forca dos paineis, por forma a descobrir o seu
peso no campo potencial total. Esta analise ira ser feita com base num exemplo. Para isso, ira ser
utilizado o painel apresentado na Figura 3.4 juntamente com um fluxo uniforme de forca U e que
flui no sentido positivo do eixo do x, isto e, o centro do fluxo uniforme encontra-se em (+∞, 0) e
portanto α ≈ 0o. Utilizando a equacao (3.3) e possıvel descrever este potencial como:
φ = −Ux (3.8)
Aplicando as derivadas parciais a equacao acima obtem-se as duas componentes de veloci-
dade deste potencial:
ux(xi, yi) = −∂φ∂x
= U,
uy(xi, yi) = −∂φ∂y
= 0.
(3.9)
Sobrepondo as duas funcoes harmonicas presentes em (3.5) e (3.8), obtem-se um fluxo uni-
forme que e deflectido junto ao painel. A componente da velocidade que e normal ao painel e
entao dada pela soma da forca do fluxo uniforme e da forca do proprio painel, resultando em :
ux(0−, yi) = U +λ
2, (3.10)
quando −L < yi < L. No caso de U ser igual a −λ/2, a velocidade normal gerada pelo painel
anula completamente a velocidade gerada pelo fluxo uniforme, ou seja, ux(0−, yi) = 0. Assim
sendo, no centro do painel, tanto a velocidade normal como a velocidade tangencial sao zero, o
que torna este ponto um ponto de estagnacao. Em dinamica de fluıdos, este ponto de estagnacao
e um requisito, pois pretende-se que o fluxo seja tangente ao painel (obstaculo). No entanto, para
este trabalho, esse requisito tem de ser alterado devido a questoes de seguranca. Assim, o que
se pretende e que a velocidade normal ao painel (Vn) seja maior ou igual que zero, isto e:
Vn = −ux(0−, yi) ≥ 0. (3.11)
Assim, definindo U e Vn e possıvel calcular o valor de λ atraves de:
λ = −2(U + Vn). (3.12)
37
3. Controlo de Formacoes
Na Figura 3.6 sao apresentados tres exemplos deste potencial para o mesmo valor de U e
diferentes valores de Vn.
−3 −2 −1 0 1 2 3−3
−2
−1
0
1
2
3
(a) Vn = 0→ λ = −2
−3 −2 −1 0 1 2 3−3
−2
−1
0
1
2
3
(b) Vn = 0.5→ λ = −3
−3 −2 −1 0 1 2 3−3
−2
−1
0
1
2
3
(c) Vn = 2→ λ = 6
Figura 3.6: Sobreposicao de um fluxo uniforme com um painel para diferentes valores de Vn (Modificadoa partir de [4]).
Estas figuras mostram a trajectoria de varias partıculas, com diferentes posicoes iniciais,
quando sujeitas a este campo potencial. A ideia e fazer a analogia destas partıculas com veıculos
moveis. E de notar que a medida que o valor de Vn aumenta, λ tambem aumenta em modulo e
as partıculas afastam-se mais do painel, o que torna a sua trajectoria mais comprida, mas ao
mesmo tempo mais segura.
38
3.3 Metodo dos Paineis
3.3.2 Multiplos Paineis
Na Figura 3.7 esta representado, de uma forma geral, o problema que este metodo pretende
resolver, ou seja, num espaco a duas dimensoes existe um obstaculo e um ponto de chegada
(xg, yg), que e o ponto para onde se pretende que o veıculo se dirija.
Figura 3.7: Representacao de um possıvel cenario para planeamento de caminho (Modificado a partir de[3]).
Como se pode ver na figura, o obstaculo e aproximado por um conjunto de paineis, que estao
numerados segundo o sentido dos ponteiros do relogio. A Figura 3.8 apresenta com mais detalhe
as caracterısticas geometricas dos varios paineis.
Figura 3.8: Representacao geometrica de dois paineis (Modificado a partir de [3]).
39
3. Controlo de Formacoes
Nesta figura, θi representa o angulo entre o painel i e o eixo das abcissas e βi o angulo entre o
eixo das abcissas e o vector n normal ao painel. E de notar que e necessario definir previamente
a direccao do vector normal n. Desta forma, cada painel e definido pelo seu vector normal (ni),
pelo seu ponto inicial (x0i, y0i), pelo seu ponto central (xci, yci), pelo seu comprimento (Li) e pelo
seu angulo em relacao ao eixo das abcissas (θi). O valor de Rij corresponde a distancia entre o
centro do painel i e um ponto arbitrario no painel j (xj , yj). Definindo m como sendo o numero
total de paineis, entao λ1 ate λm representa a forca de cada painel por unidade de comprimento.
Usando a equacao (3.5), o potencial em qualquer ponto (x, y) causado pelo painel j e dado por:
φj =λj2π
∫jln (Rj) dl, (3.13)
onde Rj e a distancia euclideana entre o ponto (x, y) e o ponto (xj , yj)
Rj =√
(x− xj)2 + (y − yj)2. (3.14)
Como se pretende que o veıculo atinja um certo ponto de chegada (xg, yg) e necessario adi-
cionar, ao campo potencial total, um potencial atractivo, que neste caso sera um sumidouro. Pela
equacao (3.2), este potencial e dado por:
φg =λg2π
ln(Rg), (3.15)
onde λg > 0 e Rg corresponde a distancia euclideana entre o ponto (x, y) e o ponto de chegada
(xg, yg):
Rg =√
(x− xg)2 + (y − yg)2. (3.16)
Acrescenta-se ainda ao campo potencial total um fluxo uniforme por forma a que este seja
mais eficaz:
φu = −U(x cosα+ y sinα), (3.17)
onde
α = arctan
(yg − yxg − x
). (3.18)
40
3.3 Metodo dos Paineis
Juntando o campo potencial gerado pelos obstaculos, pelo ponto de chegada e pelo fluxo
uniforme, obtem-se o campo potencial total na seguinte forma:
φ(x, y) = φu + φg +m∑j=1
φj . (3.19)
Caso U e λg estejam definidos e ainda necessario calcular a forca de cada painel. Para
isso sao necessarias m equacoes linearmente independentes. Na equacao (3.12) foi deduzida a
relacao entre a velocidade normal de um unico painel e a sua propria forca. De forma similar e
segundo [3] e possıvel estende-la para m paineis. Definindo Vi como sendo a velocidade normal
ao centro do painel i, entao as m equacoes sao dadas por:
∂
∂niφ (xci, yci) = −Vi i = 1, 2, ...,m (3.20)
Aplicando a equacao (3.19) ao centro do painel i e desenvolvendo os varios termos nela
presente, resulta:
φ(xci, yci) = −U(xci cosα+ yci sinα) +λg2π
ln Rgi +m∑j=1
λj2π
∫jln Rij dlj (3.21)
na qual Rgi e a distancia entre o ponto de chegada (xg, yg) e o centro do painel i (xci, yci). Da
substituicao de (3.21) em (3.20) obtem-se a equacao (3.22).
λi2
+
m∑j 6=i
λj2πIij = −Vi + U
∂
∂ni(xci cosα+ yci sinα)− λg
2π
∂
∂niln Rgi i = 1, 2, ...,m, (3.22)
onde
Iij =
∫jln Rij dl. (3.23)
Assim, e integrando a equacao (3.23), resulta:
Iij =1
2C ln
(1 +
L2j + 2Lj A
B
)−[arctan
(Lj + A
E
)− arctan
(A
E
)]cos(θi − θj) (3.24)
para E 6= 0 e
Iij = C
(ln
∣∣∣∣Lj + A
B
∣∣∣∣− LjLj + A
)+ D
(1
A− 1
Lj + A
)(3.25)
41
3. Controlo de Formacoes
para E = 0, onde
A = −(xci − x0j) cos θj − (yci − y0j) sin θj
B = (xci − x0j)2 + (yci − y0j)
2
C = sin(θi − θj)
D = −(xci − x0j = sin θi + (yci − y0j) cos θi
E = (xci − x0j) sin θj − (yci − y0j) cos θj
(3.26)
Os restantes termos da equacao (3.22) sao dados por:
U∂
∂ni(xci cosα+ yci sinα) = U sin(α− θi) (3.27)
e
λg2π
∂
∂niln Rgi =
−(xci − xg) sin θi + (yci − yg) cos θi(xci − xg)2 + (yci − yg)2
. (3.28)
Com todos os termos da equacao (3.22) definidos e entao possıvel sintetiza-la da seguinte
forma:
PΛ = q, (3.29)
onde
Pij =
12 i = j
Iij2π i 6= j
, (3.30)
qi = −Vi + U∂
∂ni(xci cosα+ yci sinα)− λg
2π
∂
∂niln Rgi (3.31)
e
Λ = [λ1, λ2, ..., λm]T . (3.32)
42
3.3 Metodo dos Paineis
Com tudo isto, e resolvendo a equacao (3.29) em funcao de Λ, obtem-se todas as forcas de
todos os paineis para o U e Vi pretendidos. Desta forma, e com as forcas dos paineis calculadas,
e necessario calcular a trajectoria que o veıculo percorrera. Para isso, deriva-se a equacao (3.19),
que representa o campo potencial total, em relacao a x e y:
ux(x, y) = −∂φ∂x
= U cosα− λg2π
∂
∂xln Rg −
m∑j=1
λj2π
∫j
∂
∂xln Rij dlj
uy(x, y) = −∂φ∂y
= U sinα− λg2π
∂
∂yln Rg −
m∑j=1
λj2π
∫j
∂
∂yln Rij dlj
(3.33)
onde
∂
∂xln Rg =
x− xg(x− xg)2 + (y − yg)2
∂
∂yln Rg =
y − yg(x− xg)2 + (y − yg)2
(3.34)
∫j
∂
∂xln Rij dlj = −1
2ln
(1 +
L2j + 2Lj F
G
)cos θj +
[arctan
(Lj + F
H
)− arctan
(F
H
)]sin θj
∫j
∂
∂yln Rij dlj = −1
2ln
(1 +
L2j + 2Lj F
G
)sin θj −
[arctan
(Lj + F
H
)− arctan
(F
H
)]cos θj
(3.35)
e
F = −(x− x0j) cos θj − (y − y0j) sin θj
G = (x− x0j)2 + (y − y0j)
2
H = (x− x0j) sin θj − (y − y0j) cos θj
(3.36)
Assim, usando as equacoes presentes em (3.33) e possıvel gerar uma trajectoria para que
um veıculo movel se desloque ate um ponto de chegada predefinido, contornando os obstaculos
presentes no ambiente.
43
3. Controlo de Formacoes
3.3.3 Garantia de Convergencia
Como se viu na seccao anterior, para gerar um campo potencial artificial com base no metodo
dos paineis, e necessario estabelecer a priori o valor de U e Vi. Contudo e tambem necessario
garantir a convergencia do campo potencial, ou seja, e necessario garantir que um veıculo movel
atinja sempre o seu ponto de chegada, facto que pode nao acontecer para um qualquer valor de U
e Vi. Por forma a verificar este fenomeno, na Figura 3.9 apresenta-se um exemplo. Neste exemplo
foi considerado um triangulo equilatero como obstaculo (Vermelho). O veıculo esta inicialmente
colocado no ponto (−1,−4) (Losango Verde) e pretende chegar ao ponto (1, 4) (Losango Preto).
Definindo a forca do fluxo uniforme como U = 1 e a forca do sumidouro do ponto de chegada
como λg = 30, fez-se variar Vi entre 0 e 5. Usando o metodo apresentado na seccao anterior foi
calculada a trajectoria (Azul) que o veıculo devera seguir.
−4 −2 0 2 4 6 8−4
−2
0
2
4
6
x
y
Figura 3.9: Aplicacao do metodo dos paineis para diferentes valores de Vi.
Como se pode ver na figura, o caminho correspondente a Vi = 0 e o caminho mais curto,
contudo nao e um caminho exequıvel, pois passa junto a superfıcie do obstaculo. Como era
esperado, e ja tinha sido referido anteriormente, a medida que Vi aumenta, aumenta tambem o
comprimento do caminho e torna-se mais seguro para o veıculo. No entanto, existe um limite
para o valor de Vi. Como se pode ver na figura, para Vi = 5, o veıculo nao consegue convergir
para o ponto desejado. Este facto acontece quando o valor da forca dos paineis e superior
a forca exercida pelo sumidouro. Desta forma e possıvel afirmar que o valor optimo para Vi
44
3.3 Metodo dos Paineis
e altamente dependente do tamanho do(s) obstaculo(s) e da posicao relativa entre estes e a
posicao do veıculo e do ponto de chegada. Como determinar este valor de Vi por tentativa e erro
nao e um metodo pratico, esta seccao apresenta um metodo que, de forma automatica, ajusta os
parametros do campo potencial consoante o tamanho dos obstaculos [4].
O objectivo de usar um sumidouro no ponto de chegada e precisamente o de criar um mınimo
global. No entanto, e necessario que a forca deste seja suficientemente forte para conseguir atrair
o veıculo ate si. Assim, por forma a minimizar a possibilidade de o veıculo colidir com obstaculos
e/ou falhar o ponto de chegada, a forca do sumidouro e dos multiplos paineis deve satisfazer a
seguinte inequacao:
− λg < λ0 < 0 , (3.37)
onde
λ0 =m∑i=1
λiLi . (3.38)
A inequacao presente em (3.37) e chamada de condicao de convergencia e indica que a
soma das forcas dos varios paineis deve ser menor que a forca do sumidouro. Se esta condicao
de convergencia for satisfeita, entao e garantido que o veıculo converge para o ponto desejado, e
que este e o minimo global de todo o campo potencial. Assim, surge a necessidade de calcular
a forca de cada painel por forma a que a condicao seja sempre satisfeita [4]. Comeca-se por
reescrever a equacao (3.22) da seguinte forma:
m∑j=1
Pijλj = −Vi + Wi i = 1, 2, ...,m, (3.39)
onde
Pij =
12 i = j
Iij2π i 6= j
, (3.40)
e
Wi = U∂
∂ni(xci cosα+ yci sinα)− λg
2π
∂
∂niln Rgi (3.41)
45
3. Controlo de Formacoes
Definindo J = P−1, da equacao (3.39), resulta:
λi =
m∑j=1
Jij(−Vi + Wj) i = 1, 2, ...,m. (3.42)
Substituindo (3.42) em (3.38) e (3.37) e obtida a seguinte inequacao:
m∑i=1
Li
m∑j=1
JijVj < λg +m∑i=1
Li
m∑j=1
JijWj . (3.43)
Assumindo que a velocidade normal a cada painel e proporcional ao seu comprimento, ou
seja
Vj = aLj , (3.44)
entao a equacao (3.43) fica reduzida a
a < amax, (3.45)
onde
amax =λg +
∑mi=1 Li
∑mj=1 JijWj∑m
i=1 Li∑m
j=1 JijLj(3.46)
Assim sendo, como todos os parametros da equacao (3.46) sao conhecidos, facilmente se
calcula o valor de amax e por consequente e possıvel escolher um valor de a que satisfaca a
inequacao (3.45). Com o valor de a conhecido, e aplicando a equacao (3.44), determina-se qual
deve ser o valor de Vi para que a condicao de convergencia seja satisfeita.
O parametro a e chamado de parametro de seguranca, pois quanto maior o seu valor, mais
segura sera a trajectoria. Caso a seja maior que amax, as forcas repulsivas serao maiores que as
atractivas e o veıculo falhara o ponto de chegada. E ainda possıvel definir o racio de seguranca:
ra =a
amax0 < ra < 1. (3.47)
Este racio tera um valor definido a priori e possibilitara o calculo de a. Com o valor de a
conhecido e entao possıvel calcular a forca de cada painel atraves da equacao (3.48).
λi =
m∑j=1
Jij(−aLj + Wj) i = 1, 2, ...,m. (3.48)
46
3.3 Metodo dos Paineis
Por forma a validar este metodo, retomou-se o cenario apresentado na Figura 3.9. O obstaculo
presente e um triangulo equilatero, o ponto inicial do veıculo esta colocado em (−1,−4) e o ponto
de chegada esta situado em (1, 4). Definindo a forca do fluxo uniforme como U = 1 e a forca do
sumidouro do ponto de chegada como λg = 30, fez-se variar ra entre 0 e 1. Os resultados obtidos
estao apresentados na Figura 3.10 e na tabela 3.1.
−4 −2 0 2 4 6 8−4
−3
−2
−1
0
1
2
3
4
5
6
x
y
Figura 3.10: Aplicacao do metodo dos paineis para diferentes valores de ra.
ra a Vi λ1 λ2 λ3 λ0 λg
0 0 0 1.3185 5.5486 −6.9438 −0.1535 30
0.2 0.4485 0.8970 0.3243 4.5533 −7.9390 −6.1228 30
0.4 0.8969 1.7938 −0.6699 3.5581 −8.9343 −12.0921 30
0.6 1.3454 2.6908 −1.6640 2.5628 −9.9295 −18.0614 30
0.8 1.7938 3.5876 −2.6582 1.5676 −10.9248 −24.0307 30
1 2.2423 4.4846 −3.6523 0.5723 −11.9200 −30.0000 30
Tabela 3.1: Forca dos paineis para diferentes valores de ra.
Como se pode verificar na tabela 3.1, a medida que se aumenta o valor de ra, λ0 tambem
aumenta em modulo ate atingir o seu valor maximo, que corresponde ao valor de λg. E de notar
tambem que o valor maximo que Vi pode atingir, neste caso para que garanta a convergencia do
potencial, e de 4.4840, e e por essa razao que no exemplo da Figura 3.9 o veıculo falhou o ponto
de chegada para Vi = 5.
47
4Controlo de Formacoes com
Quadricopteros
Conteudo4.1 Introducao . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 504.2 Definicao Geometrica de Formacoes . . . . . . . . . . . . . . . . . . . . . . . . 504.3 Controlo de Formacoes . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 514.4 Implementacao . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 534.5 Resultados . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 55
49
4. Controlo de Formacoes com Quadricopteros
4.1 Introducao
Este capitulo destina-se a apresentar um metodo de controlo de formacoes geometricas para
veıculos aereos, neste caso, quadricopteros, em que para isso serao usados os conceitos apre-
sentados nos Capıtulos 2 e 3. Desta forma,na Seccao 4.2 sera definida a topologia de formacoes
usada, na Seccao 4.3 sera introduzido o metodo dos paineis aplicados aos quadricopteros, na
Seccao 4.4 sera apresentada a implementacao do algoritmo e, por ultimo, na Seccao 4.5 serao
apresentados os resultados obtidos.
4.2 Definicao Geometrica de Formacoes
O objectivo de fazer controlo de formacoes prende-se com o facto de se pretender que um
determinado grupo de veıculos se desloque segundo uma determinada trajectoria mantendo uma
determinada distribuicao geometrica. Assim sendo, a abordagem adoptada para definir a geome-
tria do conjunto e a chamada abordagem lıder-seguidor(es) [30]. Como o proprio nome indica,
esta abordagem pressupoe a existencia de um lıder. No entanto este lıder nao necessita de ser
necessariamente um veıculo, podendo assumir a forma de um ponto (lıder virtual) ou mesmo
ser o centro de massa de um grupo de veıculos. O objectivo da existencia de um lıder e poder
definir, atraves dele, a geometria que se pretende atingir com o grupo de veıculos. Na Figura 4.1
esta representado um exemplo de uma formacao possıvel, composta por tres veıculos e um lıder
virtual.
Figura 4.1: Exemplo de uma formacao com tres veıculos e um lıder virtual.
50
4.3 Controlo de Formacoes
Como se pode ver atraves da Figura 4.1, para que os veıculos se mantenham segundo a
formacao pretendida e necessario que, previamente, se defina a sua geometria. Esta geometria
e definida com base na posicao e orientacao do lıder. Assim, o veıculo i tem conhecimento de
qual deve ser a sua distancia ao lıder (li), bem como o seu azimute (βi). E de notar ainda que e
tambem necessario definir a priori a trajectoria do lıder.
4.3 Controlo de Formacoes
Como ja foi referido na seccao anterior, cada veıculo tem conhecimento de qual deve ser
a sua localizacao no referencial inercial do ambiente. Em analogia com o metodo dos paineis
apresentado no Capıtulo 3, essa localizacao corresponde ao ponto de chegada. Como e evidente,
caso o lıder, virtual ou nao, esteja em movimento, e necessario calcular, para todos instantes de
tempo, qual o ponto de chegada correspondente.
Figura 4.2: Exemplo de movimentacao da formacao.
Na Figura 4.2 encontra-se representado um exemplo de uma possıvel movimentacao da
formacao. Como se pode ver, a medida que o lıder se vai movimentando segundo uma tra-
jectoria definida a priori, os pontos de chegada de cada veıculo vao sendo arrastados. Contudo,
por se tratar de uma formacao rıgida, as distancias relativas entre os varios pontos e o lıder nao
se alteram.
51
4. Controlo de Formacoes com Quadricopteros
Sendo que o ponto de chegada esta definido e necessario definir os obstaculos que, e como
ja foi referido anteriormente, correspondem aos veıculos, ou seja, cada veıculo considera os seus
veıculos vizinhos como obstaculos. Desta forma garante-se que nao existem colisoes, contudo
e necessario que, a cada instante de tempo, se tenha conhecimento da posicao dos veıculos
vizinhos. Na Figura 4.3 encontra-se representada a modelacao de um quadricoptero com base
em paineis.
Figura 4.3: Modelacao de um quadricoptero com base em paineis.
Como se pode ver na figura, cada veıculo e aproximado por quatro paineis (Vermelho), cada
um com o respectivo vector normal (ni). Assim sendo, basta que um dado veıculo tenha conhe-
cimento da posicao dos restantes para que possa calcular o campo potencial artificial que lhe
permita chegar ao seu ponto de chegada evitando colisoes. Uma grande vantagem deste tipo
de implementacao e o facto de, em qualquer instante de tempo, se poder alterar a geometria da
formacao, pois cada veıculo ira, de forma autonoma, encontrar um novo caminho seguro que o
leve ate a sua nova posicao.
Por forma a ter conhecimento da posicao dos veıculos vizinhos existem varias abordagens
que podem ser adoptadas. A primeira pressupoe a existencia de comunicacao entre veıculos,
ou seja, cada veıculo estima a sua posicao no referencial inercial do ambiente, fazendo uso de
por exemplo laser range finder, sonares, camaras, etc., e de seguida transmite essa informacao
para todos os outros veıculos. A segunda abordagem e uma abordagem mais descentralizada
e consiste na estimacao da posicao dos veıculos vizinhos com base na sua propria localizacao,
ou seja, cada veıculo, utilizando por exemplo laser range finder, sonares, camaras, estima a sua
propria posicao e, utilizando os mesmos sensores, estima tambem a posicao dos seus vizinhos.
52
4.4 Implementacao
A terceira abordagem e uma abordagem totalmente centralizada e consiste no uso de camaras
de alta precisao (por exemplo cameras de infravermelhos), fixas no ambiente, e que atraves de
algum processamento de imagem conseguem estimar a posicao de todos os veıculos. E de notar
que esta ultima abordagem exige que exista uma estacao de base, onde se faz o processamento
das imagens e que, posteriormente, envie essa informacao aos veıculos ([31],[32]). Contudo,
este tema da localizacao sai um pouco fora do ambito deste trabalho, pelo que, e para efeitos de
simulacao, se assumiu que todos os veıculos tem conhecimento da posicao uns dos outros.
4.4 Implementacao
Com os pontos de chegada e os varios paineis definidos e entao possıvel definir o metodo
para efectuar o controlo de formacoes. Assim sendo, na Figura 4.4 e apresentado o fluxograma
com a rotina que cada veıculo devera executar.
Definicaode U,λg e ra
Aquisicaoda
geometriada
formacao
Aquisicaoda posedo lıder
Calculo daposicao doponto dechegada
Aquisicaoda pose
dosveıculosvizinhos
Calculo daforca dospaineis
atraves daequacao
(3.42)
Calculo datrajectoriaa seguir
atraves daequacao
(3.33)
Deslocacaodo veıculoate a nova
posicao
Figura 4.4: Fluxograma da rotina para controlo de formacoes.
Em relacao a implementacao pratica, utilizou-se uma vez mais a ferramenta Simulink por
forma a poder simular a dinamica dos quadricopteros em conjunto com o controlo de formacoes.
Assim utilizou-se o simulador desenvolvido no Capıtulo 2, replicando-o pelo numero de veıculos
que se pretende simular. Na Figura 4.5 e apresentada a composicao do simulador para quatro
veıculos e um lıder virtual.
53
4. Controlo de Formacoes com Quadricopteros
4 Vehicles Simulator
Quad 4
UAV4
Quad 3
UAV3
Quad 2
UAV2
Quad 1
UAV1
Leader
Leader
3D View
Leader
UAV1
UAV2
UAV3
UAV4
Figura 4.5: Implementacao do simulador para quatro veıculos e um lıder virtual.
Como se pode ver na Figura 4.5 existem quatro blocos a verde, correspondentes aos quatros
veıculos, o bloco Leader (Vermelho), no qual se define a trajectoria do lıder, e o bloco 3D View
(Laranja) que tem como objectivo permitir a visualizacao do comportamento dos veıculos. Na
Figura 4.6 esta representada a constituicao do bloco Quad 1, no entanto os restantes blocos a
verde sao identicos a este.
Quad 1
UAV11
Vehicle ID
1
Sensors
State VectorState Vector’
Selector
UY
Goto
UAV1
Formation Control
Task
Dynamics
Omega VectorState Vector
Control
Task
State VectorOmega
Figura 4.6: Implementacao do bloco Quad 1.
Este bloco e bastante semelhante ao apresentado na Figura 2.5. A diferenca mais relevante e
o facto de, ao inves de o utilizador definir as entradas de referencia para o sistema, existir o bloco
Formation Control (Vermelho), no qual esta implementada a rotina apresentada na Figura 4.4 e
que calcula as novas entradas de referencia. Os restantes blocos sao iguais aos apresentados
na Seccao 2.5 do Capıtulo 2. E de notar ainda, que e neste bloco que se difunde a informacao
da posicao do quadricoptero para os outros veıculos.
54
4.5 Resultados
4.5 Resultados
Por forma a testar a fiabilidade do metodo desenvolvido, nesta seccao sao apresentados os
resultados obtidos. Assim, e de forma gradual, serao apresentados resultados com diferentes
graus de dificuldade. Desta forma, o primeiro teste apresentado corresponde a aplicacao do
metodo dos paineis para efectuar o controlo de formacao com quatro veıculos e um lıder virtual.
Como o objectivo deste teste e apenas demonstrar o comportamento do metodo dos paineis
no controlo de formacoes, considerou-se que os veıculos sao pontuais, nao possuem erro na
sua posicao, sao holonomicos e sem dinamica. Em relacao a geometria da formacao, como
tambem se pretende testar a alteracao da sua forma, utilizaram-se tres geometrias diferentes: em
primeiro uma geometria triangular, seguida de uma geometria linear e por ultimo uma geometria
em losango a volta do lıder virtual. A sequencia destas geometrias esta apresentada na Figura
4.7.
Figura 4.7: Sequencia das geometrias utilizadas (Triangular, Linear, Losango).
Por parte do lıder, este ira executar uma simples trajectoria de “corta-relva”, ou seja, ira seguir
uma trajectoria em ’S’. Assim, na Figura 4.8 esta apresentado o resultado obtido para U = 1,
λg = 30 e ra = 0.5.
Como se pode ver na figura, a azul esta representada a trajectoria do lıder e a vermelho, preto,
verde e rosa as trajectorias dos quatro veıculos. A tracejado azul estao representadas as ligacoes
virtuais entre os veıculos e o lıder. Analisando a figura e possıvel observar que o comportamento,
do metodo dos paineis aplicado ao controlo de formacoes, e bastante satisfatorio e que permite,
de forma elegante e eficaz, alterar a geometria da formacao pretendida. Verifica-se tambem que
nao existem colisoes entre veıculos e que o racio de seguranca e preservado.
55
4. Controlo de Formacoes com Quadricopteros
−20 0 20 40 60 80 100 120
0
20
40
60
80
100
x
y
LeaderVehicle 1Vehicle 2Vehicle 3Vehicle 4
Figura 4.8: Controlo de Formacao para tres geometrias diferentes (Ver vıdeo em [5]).
O segundo teste apresentado e semelhante ao primeiro, no entanto ira ser usada a dinamica,
bem como os metodos de controlo, do quadricoptero. A trajectoria do lıder virtual foi tambem
simplificada para simplificar a visualizacao dos resultados e a geometria utilizada sera apenas
uma geometria triangular. Na Figura 4.9 e apresentado o resultado obtido para U = 1, λg = 30,
ra = 0.5 e os controladores usados foram os controladores PID. E de notar ainda que, como o
metodo foi implementado a 2 dimensoes, nao existem variacoes segundo Z e que o valor de
referencia, para todos os veıculos, neste eixo e 1.
Da mesma forma que no primeiro exemplo, a azul esta representada a trajectoria do lıder,
a vermelho, preto, verde e rosa as trajectorias dos quatro quadricopteros e a azul tracejado a
fronteira da formacao para varios instantes de tempo. Mais uma vez, e de um ponto de vista
geral, o comportamento do metodo desenvolvido, e bastante satisfatorio. Contudo, foi necessario
fazer uma alteracao significativa na trajectoria do lıder em relacao ao primeiro teste. Para alem de
se ter reduzido o comprimento da sua trajectoria por forma a melhorar a visualizacao, foi tambem
56
4.5 Resultados
−10 −5 0 5 10 15
−15
−10
−5
0
5
x [m]
y[m
]
LeaderVehicle 1Vehicle 2Vehicle 3Vehicle 4
Figura 4.9: Controlo de Formacao utilizando Quadricopteros.
necessario ajustar a velocidade do lıder porque, caso contrario, os veıculos nao o conseguiriam
acompanhar, principalmente os veıculos que fazem a curva por fora. Este facto acontece porque
o controlo dos quadricopteros e feito em posicao e nao em velocidade. No entanto, sendo que
a trajectoria do lıder tem um comprimento de aproximadamente 35 metros e que a simulacao
teve uma duracao de 135 segundos, e possıvel concluir que o sistema no seu todo tem um
comportamento satisfatorio. Ainda em relacao a Figura 4.9, e de notar que o veıculo numero 2,
em momento algum, colidiu com outros veıculos, apesar de o poder parecer a primeira vista. Se
se acompanhar as ligacoes virtuais da formacao verifica-se que este facto nunca aconteceu e
que a razao pela qual a trajectoria do veıculo 2 se cruzar com a de outros veıculos se deve a
questoes geometricas.
Na Figura 4.10 e apresentado o erro de seguimento de referencia, ou seja, a distancia, em
funcao do tempo, correspondente entre a posicao actual dos veıculos e a posicao onde deveriam
realmente estar para manter a formacao.
57
4. Controlo de Formacoes com Quadricopteros
0 20 40 60 80 100 120 140
0
1
2
Err
or[m
]
Vehicle 1
Mean Value = 0.32618
0 20 40 60 80 100 120 140
0
1
2
Err
or[m
]
Vehicle 2
Mean Value = 0.38996
0 20 40 60 80 100 120 140
0
1
2
Err
or[m
]
Vehicle 3
Mean Value = 0.34679
0 20 40 60 80 100 120 140
0
1
2
Time [s]
Err
or[m
]
Vehicle 4
Mean Value = 0.42827
Figura 4.10: Erro de seguimento de referencia.
Como se pode observar na Figura 4.10, o erro de seguimento de referencia existente ate aos
10 segundos corresponde ao deslocamento dos quatro veıculos, das suas posicoes iniciais ate
a posicao de formacao. E de notar que ate aos 10 segundos o lıder esta parado e, portanto,
a posicao de referencia e constante. A partir dos 10 segundos o lıder inicia o seu trajecto, daı
que todos os veıculos tenham um salto no erro de referencia, pois nao sao instantaneos a iniciar
o movimento. Ate aos 35 segundos, tempo para o qual o lıder se encontra a percorrer uma
recta, todos os veıculos apresentam um erro semelhante a um sinal em “dente de serra”. Este
acontecimento deve-se simplesmente a forma como o quadricoptero efectua o movimento, ou
seja, para que um quadricoptero execute um movimento translacional e necessario que primeiro
este altere a sua atitude, de seguida executa o movimento e depois muda de novo de atitude,
para de forma suave, atingir a sua referencia. Contudo, se a sua referencia estiver em constante
movimento, como e o caso, o quadricoptero tenta sempre atingi-la, mas quando inicia a fase
suavizar o movimento, a referencia ja se deslocou. No entanto, e como se pode ver, o erro e
relativamente baixo, pelo que nao acarreta grandes problemas.
A partir dos 35 segundos, o lıder inicia a curva. E bastante curioso comparar o erro de
referencia dos veıculos 1 e 2 (veıculos que percorrem a curva por dentro) com o erro de referencia
dos veıculos 3 e 4 (veıculos que percorrem a curva por fora). Os veıculos 1 e 2 percorrem a
trajectoria duma forma suave, ja o erro dos veıculos 3 e 4 parece oscilar. Este facto deve-se a
razao que foi referida anteriormente, ou seja, a forma como o quadricoptero efectua o movimento.
58
4.5 Resultados
A acentuada diferenca no erro dos veıculos 1-2 e 3-4 acontece porque, como estes ultimos fa-
zem a curva por fora, cada pequena rotacao do lıder corresponde a uma mudanca relativamente
brusca da referencia dos veıculos, e assim quando o veıculo se aproxima da posicao de referencia
e gerada uma nova posicao de referencia, o que causa esta oscilacao. Ja para os veıculos 1 e
2, como fazem a curva por dentro, cada pequena rotacao do lıder corresponde a uma pequena
variacao da posicao de referencia, daı que o erro se mantem aproximadamente constante.
Por volta do segundo 131, o lıder para e, como se pode ver, todos os veıculos convergem para
a posicao de referencia, pelo que o erro de seguimento de referencia fica a zero. E de notar ainda
que o valor medio dos erros dos quatro veıculos, ao longo do tempo, e relativamente baixo.
O terceiro teste tem como objectivo testar a alteracao da geometria usando a dinamica e
os metodos de controlo dos quadricopteros. A trajectoria do lıder e uma trajectoria linear e a
formacao, composta por quatro veıculos, passara de uma geometria linear para uma geometria
triangular. Nas Figuras 4.11 e 4.12 e apresentado o resultado obtido e o erro de seguimento ao
longo do tempo. Para este teste utilizaram-se os controladores PID e os parametros usados sao:
U = 1, λg = 30, ra = 0.5 e Z = 1.
−5 0 5 10 15 20
−15
−10
−5
0
5
x [m]
y[m
]
LeaderVehicle 1Vehicle 2Vehicle 3Vehicle 4
Figura 4.11: Alteracao da geometria utilizando quadricopteros (Ver vıdeo em [5]).
59
4. Controlo de Formacoes com Quadricopteros
0 10 20 30 40 50 60 70 80 900
2
4
6
Err
or[m
]
Vehicle 1
Mean Value = 0.42133
0 10 20 30 40 50 60 70 80 900
2
4
6
Err
or[m
]
Vehicle 2
Mean Value = 0.6727
0 10 20 30 40 50 60 70 80 900
2
4
6
Err
or[m
]
Vehicle 3
Mean Value = 0.62283
0 10 20 30 40 50 60 70 80 900
2
4
6
Time [s]
Err
or[m
]
Vehicle 4
Mean Value = 1.2396
Figura 4.12: Erro de seguimento de referencia.
Da mesma forma que nos exemplos anteriores, a azul esta representada a trajectoria do lıder,
a vermelho, preto, verde e rosa as trajectorias dos quatro veıculos e a azul tracejado a fronteira
da formacao para varios instantes de tempo. Atraves da Figura 4.11 verifica-se que o metodo de-
senvolvido e capaz de lidar com alteracoes de geometria de uma forma bastante suave e eficaz,
preservando o racio de seguranca desejado. Podera parecer, uma vez mais, que o veıculo 2 e
o veıculo 4 colidiram com o veıculo 1 e 3, respectivamente, mas, como foi dito anteriormente, as
trajectorias sao coincidentes em intervalos de tempo diferentes.
Analisando a Figura 4.12 observa-se que o erro de seguimento de referencia existente ate
aos 17 segundos corresponde ao deslocamento dos quatro veıculos das suas posicoes iniciais
ate a posicao de formacao. E de notar que entre os 0 e os 17 segundos o lıder se encontra pa-
rado. Entre os 17 e os 50 segundos, o erro de seguimento e semelhante ao erro de seguimento
do segundo teste. Aos 50 segundos e feita a alteracao da geometria de linear para triangular,
daı que instantaneamente o erro aumente substancialmente. No entanto os quatro veıculos con-
vergem rapidamente para a sua nova posicao de referencia, compondo assim a nova geometria
pretendida. E de realcar que a amplitude do erro nao e igual para os quatro veıculos aos 50
segundos, pois os veıculos que se encontram mais afastados do lıder, neste caso os veıculos 2
e 4, necessitam de deslocar-se mais, por razoes geometricas, para atingir a formacao desejada.
E possıvel assim concluir, mais uma vez, que o metodo desenvolvido tem um comportamento
bastante satisfatorio.
60
5Plataforma de Ensaio Experimental
Conteudo5.1 Introducao . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 625.2 Caracterizacao do Veıculo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 625.3 Arquitectura de Hardware e Software . . . . . . . . . . . . . . . . . . . . . . . . 645.4 Resultados . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 66
61
5. Plataforma de Ensaio Experimental
5.1 Introducao
Gracas a empresa UAVision R© [33] foi possıvel dar inıcio a implementacao, dos metodos de-
senvolvidos, num veıculo real. Esta empresa cedeu ao grupo de trabalho um veıculo denominado
UX-4001 Mini (Figura 5.1). Este veıculo, a nıvel de hardware, foi totalmente desenvolvido nesta
empresa, ja o firmware e software utilizados foram adaptados de uma empresa alema, MikroKop-
ter R© [34]. Assim sendo, na Seccao 5.2 sera feita uma caracterizacao total do veıculo utilizado, de
seguida na Seccao 5.3 serao apresentadas as arquitecturas de software e hardware que foram
implementadas e na Seccao 5.4 serao apresentados os resultados obtidos.
Figura 5.1: Quadrotor UX-4001 Mini (Ver fotos e vıdeos em [5]).
5.2 Caracterizacao do Veıculo
Como referido no Capıtulo 1, os objectivos propostos para o quadricoptero neste trabalho
destinam-se a ser aplicados indoor, ou seja, em espacos fechados, como e o caso do Laboratorio
de Robotica Movel do Instituto Superior Tecnico. Assim sendo, como este veıculo tem dimensoes
e peso reduzidos (Figura 5.2), torna-se ideal para este trabalho. Contudo, o objectivo da UAVision
prende-se com aplicacoes outdoor, pelo que o veıculo integra alguns sensores que nao serao
utilizados neste trabalho.
Este veıculo, UX-4001 Mini, e, de forma geral, composto por 4 partes diferentes: Chassi,
Electronica, Actuadores e modulo de comunicacao. O chassi e feito em carbono hıbrido, o que o
torna bastante leve e resistente ao mesmo tempo.
62
5.2 Caracterizacao do Veıculo
410mm
657mm
254mm
(a)
270mm
(b)
Figura 5.2: Dimensoes do Quadrotor UX-4001 Mini.
A nıvel de electronica existem 3 conjuntos de placas: MKFlight Ctrl, MKNavi Ctrl e MKBL Ctrl.
A placa MKFlight Ctrl e a placa principal deste veıculo. Esta placa contem um microcontrolador
Atmega 1284, onde e feito o controlo de malha interna, e os seguintes sensores:
• 3 Giroscopios ADXRS610;
• 1 Acelerometro de 3 eixos LIS344ALH;
• 1 Sensor barometrico MPX4115.
A placa MKNavi Ctrl e uma placa usada para navegacao, principalmente outdoor, e que
contem os seguintes sensores:
• 3 Magnetometros KMZ51;
• 1 GPS ublox LEA-4H.
A placa MKBL Ctrl corresponde ao conjunto dos quatro variadores (ESC), que sao responsaveis
por excitar os actuadores.
Em relacao aos actuadores, foram usados 4 motores de corrente contınua sem escovas AXI
2208/26 Gold Line.
Neste veıculo existem ainda 2 modulos de comunicacao: um modulo Spektrum AR9000, que
permite a comunicacao com o veıculo atraves de um sinal PPM, e um modulo Bluetooth que
permite a comunicacao do veıculo com qualquer computador que disponha desta tecnologia.
63
5. Plataforma de Ensaio Experimental
Foi ainda acrescentado ao sistema um Arduino UNO e um sonar Devantech SRF04.
O calculo dos parametros fundamentais apresentados na Tabela 2.1 do Capıtulo 2, sai fora do
ambito deste trabalho, pelo que nao foram calculados.
5.3 Arquitectura de Hardware e Software
Como se pode ver na seccao anterior, nao existe nenhum sensor com o qual seja possıvel
fazer uma estimativa razoavel da posicao (X,Y ) do quadricoptero, pelo que se optou apenas
por implementar, numa primeira fase, o controlador de altitude. Por nao haver disponıvel um
computador a bordo do veıculo nao e possıvel adicionar, por exemplo, uma camara ou um laser
range finder por forma a estimar a sua posicao, pelo que e o operador do veıculo que, por controlo
remoto, esta encarregue de guiar o quadricoptero.
Em relacao ao controlador de altitude, apesar de existir um sensor de barometrico a bordo,
este nao e fiavel em ambiente indoor devido ao reduzido gradiente de pressao existente. Desta
forma adicionou-se ao sistema um Arduino e um sonar que, de forma facil e eficaz, consegue
estimar a altitude do veıculo. Assim sendo, na Figura 5.3 esta representada a arquitectura de
hardware do sistema.
SonarDevantech
SRF04
Arduino
MKNavi Ctrl
MKFlight Ctrl
MKBL Ctrl
Motores
Modulo
Bluetooth
Spektrum
AR9000
Computador
Comandode Controlo
Remoto
Cabo USB
Bluetooth
Sinal PPM
Figura 5.3: Arquitectura de hardware do sistema.
64
5.3 Arquitectura de Hardware e Software
Como ja foi referido na seccao anterior, a placa MKFlight Ctrl e responsavel por efectuar o
controlo de malha interna, ou seja, por controlar a atitude (φ, θ, ψ) do quadricoptero. Este controlo
e executado pelo firmware da MikroKopter. Este firmware tem a vantagem de ser de codigo
aberto, contudo esta muito pouco documentado e o pouco que esta encontra-se em alemao, o
que torna a tarefa de modifica-lo bastante complicada. A pouca informacao que existe sobre este
firmware e que tem implementado um controlador PID por cada variavel de atitude, e que os
seus ganhos podem ser alterados atraves de um software fornecido pela empresa. Para efectuar
o controlo de atitude, esta placa, para alem de receber as informacoes de todos os sensores,
recebe os dados provenientes tanto do comando de controlo remoto, como do computador, e
ainda e responsavel por enviar comandos para os varios motores atraves da placa MKBL Ctrl.
Por sua vez o arduino recebe os dados do sonar e envia-os, atraves de um cabo USB, para o
computador.
No computador e entao feito o controlo de altitude utilizando os dados enviados pelo arduino
e pela placa MKFlight Ctrl. Na Figura 5.4 esta representada, de um modo geral, a arquitectura de
software desenvolvida para efectuar o controlo de altitude.
Arduino
Quadrotor InterfaceSerie
Controlode Altitude
Valor deAltitude deReferencia
Informacao dos Sensores
Codificada
Informacao dos Sensores
Descodificada
Informacao de Altitude
Comando de Propulsao
Descodificado
Comando de Propulsao
Codificado
Figura 5.4: Arquitectura de software.
Esta arquitectura foi implementada num software de codigo aberto chamado ROS1. No bloco
Interface Serie foi implementado o protocolo de comunicacao descrito em [35]. Este bloco
e responsavel pela descodificacao da informacao proveniente do veıculo. Apos efectuar essa
descodificacao, este envia a informacao recebida para o bloco Controlo de Altitude. Este ultimo
bloco, recebe estas informacoes, bem como a informacao da altitude a que o veıculo se encontra,
e, atraves de um controlador PID, calcula qual deve ser a forca a exercer pelos motores, para que
o veıculo se mantenha na altitude desejada. Esse valor da forca e enviado de volta para o bloco
Interface Serie, que converte esse valor, codifica-o e envia-o para a placa MKFlight Ctrl.1Robot Operating System - http://www.ros.org
65
5. Plataforma de Ensaio Experimental
5.4 Resultados
Nesta seccao sao apresentados os resultados dos primeiros testes de voo efectuados. Por
forma a testar os controladores desenvolvidos esta seccao esta dividida em tres testes distintos.
O primeiro teste tem como objectivo testar o controlador de malha interna, ou seja, o con-
trolador de atitude do veıculo. Para isso, suspendeu-se o quadricoptero no ar utilizando cabos,
mas permitindo que este tenha liberdade para mudar de atitude (Figura 5.5), efectuou-se uma
perturbacao na sua atitude e activou-se o controlador. O resultado esta apresentado na Figura
5.6. Os ganhos utilizados foram ajustados a mao, a partir dos ganhos originais, por forma a me-
lhorar o desempenho do controlador. E de notar que apenas se efectuou a perturbacao segundo
φ porque, e devido a simetria existente, o comportamento do veıculo segundo θ seria bastante
identico.
Figura 5.5: Montagem para teste de controlador de atitude.
Como se pode ver na Figura 5.6, o quadricoptero foi colocado inicialmente num angulo de
aproximadamente 40o, e, por volta dos 6 segundos, activou-se o controlo de malha interna. Pela
analise da figura, verifica-se que, apesar de existir uma sobre-elevacao, esta nao ultrapassa os
5o (12.5%) e que o controlador tem um tempo de subida de cerca de 0.5 segundos e um tempo
de estabelecimento de cerca de 5 segundos a atingir a atitude desejada (φ = 0o). Atraves deste
resultado e possıvel afirmar que o controlador de malha interna se encontra bem dimensionado
proporcionando um comportamento bastante satisfatorio.
66
5.4 Resultados
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14−45
−40
−35
−30
−25
−20
−15
−10
−5
0
5
Rol
amen
to[D
eg]
Tempo [s]
Figura 5.6: Teste ao controlador de atitude segundo φ.
Sendo que a atitude do quadricoptero se encontra estabilizada e devidamente controlada, o
segundo teste consiste em fazer um voo pairado (φ ≈ 0o e θ ≈ 0o), mantendo uma certa altitude.
Para isso utilizou-se o controlador de altitude desenvolvido em ROS (Figura 5.4) e a estimacao
desta e feita utilizando o sonar. Assim, na Figura 5.7 apresenta-se o grafico da variacao de alti-
tude do veıculo em funcao do tempo. E de notar que a altitude de referencia foi colocada nos 1.3
metros e que o controlo de posicao horizontal (X e Y ) foi efectuado por um controlador humano.
Este teste teve a duracao de aproximadamente 35 segundos. Entre os 0 e os 6 segundos, o
quadricoptero encontra-se junto ao solo (Z = 0m) a dar inıcio a aceleracao dos quatro motores.
Entre os 6 e os 11 segundos o veıculo inicia efectivamente o seu movimento vertical. O pequeno
pico existente aos 7 segundos deve-se ao facto de o veıculo, e consequentemente o sonar, nao
descolar totalmente paralelo ao solo, pelo que este fornece uma estimativa errada da sua altitude.
Entre os 11 e os 22 segundos o veıculo encontra-se em voo pairado. Como se pode ver atraves
da figura, o veıculo e capaz de manter a altitude desejada de forma bastante satisfatoria. E de
notar que o valor medio apresentado na figura corresponde a media do valor de altitude entre os
segundos 11 e 22. A partir dos 22 segundos da-se inıcio a aterragem do veıculo. Este movimento
e efectuado manualmente por um operador humano. Assim, e de uma forma geral, e possıvel
afirmar que este controlador encontra-se bem dimensionado e apresenta um bom desempenho.
67
5. Plataforma de Ensaio Experimental
0 5 10 15 20 25 300
0.2
0.4
0.6
0.8
1
1.2
1.3
1.41.4
Alti
tude
[m]
Tempo [s]
Valor Medio = 1.3228m
RespostaDesejado
Figura 5.7: Teste ao controlador de altitude com uma referencia de 1.3 metros (Ver fotos e vıdeos em [5]).
O terceiro teste consiste em testar o controlador de altitude quando este e sujeito a perturbacoes.
Para isso definiu-se uma altura de referencia, que no caso deste teste e de 1 metro, e fez-se pas-
sar o veıculo sobre as escadas apresentadas na Figura 5.8.
Figura 5.8: Perfil das escadas utilizadas (Ver fotos e vıdeos em [5]).
Ao fazer com que o quadricoptero sobrevoe as escadas, a sua estimativa de altitude vai ser
alterada, pelo que o controlador tera de reagir a estas alteracoes. O resultado obtido esta repre-
sentado na Figura 5.9.
68
5.4 Resultados
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 180
0.1
0.2
0.3
0.4
0.5
0.6
0.7
0.8
0.9
1
1.1
1.2
1.3
1.4
1.5
1.6
1.7
1.8
Alti
tude
[m]
Tempo [s]
Figura 5.9: Teste ao controlador de altitude sobrevoando as escadas (Ver fotos e vıdeos em [5]).
Entre os 2 e os 6 segundos o veıculo coloca-se na sua altitude de referencia (1 metro). Aos
6 segundos surge o primeiro degrau da escada. Sendo que a altura do degrau e de 0.3 metros,
o veıculo tem agora a percepcao que se encontra a 0.7 metros de altitude. Assim sendo, entre
os 6 e os 8.5 segundos o quadricoptero tenta, de novo, atingir a sua altitude de referencia. Aos
8.5 segundos, surge entao o segundo degrau. Da mesma forma que para o primeiro, o veıculo
estima que a sua altitude diminuiu e, por isso, compensa-a para atingir de novo a altitude de
referencia. Aos 11.5 segundos, o quadricoptero passou por todos os degraus e esta, de novo,
a sobrevoar o solo. Como se pode observar na Figura 5.8, a altura das escadas do lado direito
e de 0.66 metros, pelo que aos 11.5 segundos o veıculo se encontra a aproximadamente 1.66
metros do solo. Sendo que esta acima da altitude de referencia, o veıculo necessita de descer
para se colocar nessa altitude. Por volta dos 15 segundos e dado o comando de aterragem.
Como se pode observar mais uma vez, o controlador de altitude tem um comportamento bastante
satisfatorio e encontra-se bem dimensionado.
69
6Conclusoes e Trabalho Futuro
Conteudo6.1 Conclusoes . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 726.2 Trabalho Futuro . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 73
71
6. Conclusoes e Trabalho Futuro
6.1 Conclusoes
Esta tese apresentou um novo metodo para efectuar o controlo de formacoes em veıculos
aereos nao tripulados, mais especificamente, em quadricopteros. Para atingir este fim, foi ne-
cessario implementar e desenvolver tanto a dinamica e cinematica do veıculo, como tambem os
metodos de controlo necessarios para controlar as varias variaveis de estado. Foram implemen-
tados dois metodos de controlo distintos (PID e LQR), por forma a comparar a sua performance.
Atraves do modelo matematico apresentado foi possıvel criar um simulador que servira de base
para implementar todos os algoritmos desenvolvidos. Os varios testes realizados mostram que
ambos os controladores desenvolvidos tem capacidade tanto para estabilizar o quadricoptero
como tambem para faze-lo seguir determinadas referencias. O controlador LQR demonstrou, de
forma geral, uma melhor performance, contudo o ajuste dos seus ganhos revelou ser um processo
complicado e moroso.
Com o simulador do quadricoptero implementado seguiu-se o desenvolvimento do metodo de
controlo de formacoes. Para isso, e numa primeira parte, apresentou-se toda a teoria de campos
potenciais artificiais baseados em funcoes harmonicas, e como esta se adapta, juntamente com
o metodo dos paineis conhecido em mecanica de fluıdos, para efectuar o problema do contorno
de obstaculos. Foi tambem introduzido um complemento ao tradicional metodo dos paineis que
permite a geracao de campos potenciais mais eficazes para o contorno de obstaculos e que torna
o metodo tradicional mais apto a mudancas dinamicas do ambiente, permitindo que o metodo seja
aplicavel em tempo real. Numa segunda parte, aplicou-se a teoria apresentada ao controlo de
formacoes. Este metodo baseia-se na abordagem lıder-seguidores, o que quer dizer que toda a
geometria da formacao e definida em relacao a um lıder, que pode ser virtual. Assim, assumiu-
se que, a cada instante de tempo, cada veıculo tem conhecimento da posicao dos restantes
veıculos do grupo. Desta forma cada quadricoptero considera os outros veıculos como sendo
obstaculos moveis, permitindo que este planeie uma trajectoria que o mantenha em formacao.
As simulacoes realizadas mostram que o metodo desenvolvido e bastante eficaz no controlo de
formacoes e permite que, de uma forma elegante, a geometria da formacao possa ser alterada
possibilitando que o grupo se adapte a mudancas estruturais do ambiente onde se encontra.
Foi ainda possıvel neste trabalho dar inıcio a implementacao dos metodos de controlo de-
senvolvidos num quadricoptero real. O quadricoptero adquirido contem de raiz o controlador de
malha interna (atitude) implementado no seu microcontrolador. Apos analise do codigo contido
neste, foi possıvel concluir que existe um controlador PID por cada variavel de atitude, sendo que
os seus ganhos foram ajustados a mao por forma a melhorar a sua performance. Foi tambem
adicionado ao veıculo um sonar, o que permitiu implementar o controlador de altitude. Os resulta-
dos dos testes efectuados foram bastante satisfatorios e possibilitaram a realizacao dos primeiros
ensaios em voo pairado.
72
6.2 Trabalho Futuro
6.2 Trabalho Futuro
Existem muitos aspectos que podem ser melhorados neste trabalho.
Em relacao ao simulador e necessario calcular os parametros fundamentais do quadricoptero
real e introduzi-los no simulador. E necessario tambem implementar modelos mais realistas dos
sensores utilizados bem como algum estimador, como por exemplo, um Filtro de Kalman ou
um Filtro Complementar. De seguida e necessario testar o comportamento do simulador e dos
metodos de controlo e compara-lo com o veıculo real, por forma a obter um simulador o mais
realista possıvel.
Em relacao ao metodo de controlo de formacoes dever-se-a estender o metodo desenvolvido
para 3 dimensoes e desenvolver uma forma de efectuar o controlo por velocidade. Sera tambem
interessante verificar o comportamento deste metodo em ambientes com obstaculos estruturais.
Por ultimo, e em relacao a plataforma de ensaio experimental, e necessario equilibrar o veıculo
por forma a que este tenha o mınimo escorregamento possıvel, ou seja, para que este nao se des-
loque “involuntariamente”. Dever-se-a tambem introduzir mais processamento a bordo do veıculo
para que seja possıvel, nao so fazer todo o controlo a bordo, como tambem para poder adicio-
nar um laser ou uma camara que permitira estimar a posicao do quadricoptero segundo X e Y .
Esta estimativa podera ser feita atraves da implementacao de um algoritmo SLAM (Simultaneous
Localization And Mapping) [36] ou, mais simplesmente, atraves de um algoritmo ICP (Iterative
Closest Point) [37]. Apos todas as variaveis do espaco de estados estarem devidamente contro-
ladas, dever-se-a proceder a implementacao do controlo de formacoes. Para isso, em primeiro
lugar e necessario calcular as distancias e azimutes aos veıculos vizinhos. A forma mais viavel
para completar esta tarefa consiste em usar, uma vez mais, uma camara. Esta camara devera
ser omnidireccional para possibilitar que se detectem veıculos em todas as direccoes. Contudo,
existirao sempre angulos mortos, que poderao afectar o comportamento dos veıculos. Para faci-
litar um pouco todo este processo, poder-se-a comecar por implementar um sistema de Ground
Thruth, ou seja, um sistema composto por camaras colocadas no ambiente, e que a cada instante
de tempo calculam a posicao de todos os veıculos presentes no seu campo de visao e difunde
essa informacao a todos os quadricopteros.
73
Bibliografia
[1] S. Bouabdallah, M. Becker, and V. D. Perrot, “Computer obstacle avoidance on quadrotors,”
in Proceedings of the XII International Symposium on Dynamic Problems of Mechanics, Fe-
vereiro 2007.
[2] “Diy drones,” http://http://diydrones.com/forum/topics/extended-kalman-filter-design, 2011.
[3] J.-O. Kim and P. Khosla, “Real-time obstacle avoidance using harmonic potential functions,”
IEEE Transactions on Robotics and Automation, Junho 1992.
[4] F. Fahimi, Autonomous Robots - Modeling, Path Planning, and Control. Springer, 2009.
[5] “Vıdeos e fotos,” http://web.ist.utl.pt/henrique.ribeiro/, 2012.
[6] Y. U. Cao, A. S. Fukunaga, and A. B. Kahng, “Cooperative mobile robotics: Antecedents and
directions,” Autonomous Robots, 1997.
[7] S. Garrido, L. Moreno, and P. U. Lima, “Robot formation motion planning using fast marching,”
Robot. Auton. Syst., Setembro 2011.
[8] J. Desai, J. Ostrowski, and V. Kumar, “Modeling and control of formations of nonholonomic
mobile robots,” Robotics and Automation, IEEE Transactions on, Dezembro 2001.
[9] T. Balch and R. Arkin, “Behavior-based formation control for multirobot teams,” Robotics and
Automation, IEEE Transactions on, Dezembro 1998.
[10] M. Egerstedt and X. Hu, “Formation constrained multi-agent control,” Robotics and
Automation, IEEE Transactions on, Dezembro 2001.
[11] A. Das, R. Fierro, V. Kumar, J. Ostrowski, J. Spletzer, and C. Taylor, “A vision-based formation
control framework,” Robotics and Automation, IEEE Transactions on, Outubro 2002.
[12] R. Fierro, P. Song, A. Das, and V. Kumar, “Cooperative control of robot formations,” 2001.
[13] P. Ogren, E. Fiorelli, and N. Leonard, “Cooperative control of mobile sensor networks:adaptive
gradient climbing in a distributed environment,” Automatic Control, IEEE Transactions on,
Agosto 2004.
75
Bibliografia
[14] N. Leonard and E. Fiorelli, “Virtual leaders, artificial potentials and coordinated control of
groups,” in Decision and Control, 2001. Proceedings of the 40th IEEE Conference on, 2001.
[15] E. MacArthur and C. Crane, “Compliant formation control of a multi-vehicle system,” in
Computational Intelligence in Robotics and Automation, 2007. CIRA 2007. International
Symposium on, Junho 2007.
[16] X. Yun and K.-C. Tan, “A wall-following method for escaping local minima in potential field ba-
sed motion planning,” in Advanced Robotics, 1997. ICAR ’97. Proceedings., 8th International
Conference on, Julho 1997.
[17] F. A. Cosıo and M. P. Castaneda, “Autonomous robot navigation using adaptive potential
fields,” Mathematical and Computer Modelling, 2004.
[18] J.-O. Kim and P. Khosla, “Real-time obstacle avoidance using harmonic potential functions,”
in Robotics and Automation, 1991. Proceedings., 1991 IEEE International Conference on,
Abril 1991.
[19] S. Akishita, S. Kawamura, and T. Hisanobu, “Velocity potential approach to path planning for
avoiding moving obstacles,” Advanced Robotics, 1992.
[20] F. Fahimi, H. Ashrafiuon, and C. Nataraj, “Obstacle avoidance for spatial hyper-redundant
manipulators using harmonic potential functions and the mode shape technique,” Journal of
Robotic Systems, 2003.
[21] Y. Zhang and K. P. Valavanis, “Sensor-based 2-d potential panel method for robot motion
planning,” Robotica, 1996.
[22] Y. Zhang and K. Valavanis, “A 3-d potential panel method for robot motion planning,” Robotica,
1997.
[23] T. Bresciani, “Modelling, identification and control of a quadrotor helicopter,” Master’s thesis,
Lund University, 2008.
[24] J. M. B. Domingues, “Quadrotor prototype,” Master’s thesis, Instituto Superior Tecnico, 2009.
[25] B. Cardeira, “Arquitecturas para navegacao inercial/gps com aplicacao a veıculos
autonomos,” Master’s thesis, Instituto Superior Tecnico, 2009.
[26] D. Mellinger, N. Michael, and V. Kumar, “Trajectory generation and control for precise ag-
gressive maneuvers with quadrotors,” in Proceedings of the International Symposium on
Experimental Robotics, Dezembro 2010.
[27] V. L. S. Frank L. Lewis, Optimal Control. J. Wiley, 1995.
76
Bibliografia
[28] M. L. W. Gene F. Franklin, J. David Powell, Digital Control of Dynamic Systems. Addison-
Wesley, 1998.
[29] S. L. L. B. T. M. M. R. Christian Fink Petersen, Henrik Hansen, “Autonomous hovering with a
quadrotor helicopter,” Aalborg Universitet, Tech. Rep., 2008.
[30] J. Desai, J. Ostrowski, and V. Kumar, “Modeling and control of formations of nonholonomic
mobile robots,” Robotics and Automation, IEEE Transactions on, Dezembro 2001.
[31] N. Michael, J. Fink, S. G. Loizou, and V. Kumar, “Architecture, abstractions, and algorithms
for controlling large teams of robots: Experimental testbed and results,” in Proc. of the Int.
Symposium on Robotics Research, Novembro 2007.
[32] N. Michael, D. Mellinger, Q. Lindsey, and V. Kumar, “The grasp multiple micro-uav testbed,”
Robotics Automation Magazine, IEEE, Setembro 2010.
[33] “Uavision,” http://www.uavision.com/, 2012.
[34] “Mikrokopter,” http://www.mikrokopter.de/ucwiki/en/MikroKopter/, 2012.
[35] “Mikrokopter serial protocol,” http://www.mikrokopter.de/ucwiki/en/SerialProtocol, 2012.
[36] A. Bachrach, A. de Winter, R. He, G. Hemann, S. Prentice, and N. Roy, “Range - robust
autonomous navigation in gps-denied environments,” in Robotics and Automation (ICRA),
2010 IEEE International Conference on, Maio 2010.
[37] “The c(anonical) scan matcher,” http://andreacensi.github.com/csm/, 2012.
77