Embed Size (px)
Citation preview
Cooperação de Veículos Robóticos Marinhos para Apoio aoMergulho: da Teoria à Prática
Rafael Dinis Matos Pires
Dissertação para obtenção do Grau de Mestre em
Engenharia Electrotécnica e de Computadores
Orientador: Professor António Manuel Santos Pascoal
Examination Committee
Presidente: Prof. João Fernando Cardoso Silva SequeiraOrientador: Prof. António Manuel Santos PascoalVogais: Prof. João Pedro Castilho Pereira Santos Gomes
Dezembro 2015
ii
Dedicated to Eduardo Matos
iii
iv
Acknowledgments
I am using this opportunity to express my gratitude to everyone who supported me throughout this
project.
I am specially thankful to professor Antonio Pascoal for the oportunity of making part of this project,
for the aspiring guidance, constructive criticism and friendly advice during the entire project work.
A big thank you to all my colleages and friends who gave me support and feedback throughout this
project. In special to my friend Joao Santos for all the feedback that he gave over this journey.
I have to express my gratitude to my parents that always believed in me and gave me the best
conditions to do my entire course and in particular this project.
Last but not least, I want to give a big thank you to my sister for the support and the experienced
advices that she gave me during all my student life.
v
vi
Resumo
No domınio da exploracao do oceano, tem-se verificado um crescente interesse em poder contar com
os mais avancados veıculos marıtimos autonomos, de forma a poder realizar operacoes que nao se
encontram ao alcance do mergulhador humano, apresentam-se como operacoes de risco ou simples-
mente apresentam custos bastante elevados.
E no contexto do projecto Caddy [1], que surge o tema da presente dissertacao. O projecto Caddy
pretende desenvolver um veıculo robotico ”amigo” do mergulhador que o auxilia em diversos cenarios
de operacao. Desta forma sera possıvel reduzir os custos de exploracao, cobrir vastas areas e ainda
reduzir os riscos a que os mergulhadores estao sujeitos.
Nesta dissertacao estuda-se um dos muitos problemas que criar um veıculo deste tipo levanta, o
problema do controlo cooperativo entre veıculos marıtimos autonomos. Todos os algoritmos propostos
terao como referencia o veıculo Medusa [2], propriedade do ISR (Instituto de sistemas e Robotica).
O problema sera formulado numa primeira fase tendo em conta o controlo de apenas um veıculo,
sendo propostos algoritmos de controlo de orientacao e seguimento de caminho.
Numa segunda fase serao estudados algoritmos de controlo de alto nıvel, ja com o intuito de contro-
lar um conjunto de robos, num cenario onde existira um veıculo lıder (a superfıcie) e um veıculo que o
segue o lıder (a uma determinada profundidade). Nesta etapa sera adoptada uma estrutura de controlo
cooperativo denominada Cooperative Leader Tracking.
Numa fase posterior o veiculo que segue o lıder sera substituıdo por um mergulhador, neste contexto
sera proposto um algoritmo de controlo (numa configuracao Cooperative Leader Tracking) que devera
ser capaz de lidar com a imprevisibilidade do mergulhador.
Os algoritmos propostos serao sujeitos a um conjunto de testes via simulacao Matlab Simulink.
Palavras-chave: Veıculos Marıtimos Autonomos, Controlo Cooperativo de Veıculos Marıtimos
Autonomos, Cooperative Leader Tracking, Humanos como parte da malha de controlo, Veıculo Autonomo
Medusa
vii
viii
Abstract
In the field of ocean exploration there has been growing interest in the development of advanced auto-
nomous marine vehicles capable of performing operations that cannot be executed by human divers or
jeopardize their lives, or are simply too costly to accomplish by resorting to classical systems.
The theme of this thesis arises from the Caddy project, [1]. This project aims to develop a robot that
acts like a diver’s ”Buddy” and is capable of assisting the diver in a number of operating scenarios. By
doing this, it will be possible to reduce operation costs, cover large areas, and reduce the risks to divers.
The subject of this thesis is the problem of controling a set of vehicles in a cooperative way. All
proposed algorithms will focus on the Medusa autonomous marine vehicle [2], property of ISR (Institute
for Systems and Robotics).
The first part of this thesis, takes into account the problem of controling only one vehicle, some
algorithms will be proposed for orientation control and path following.
On the second part of this thesis, the subject will be the high level control algorithms, in order to
control a set of robots, in particular under the scenario where there are a surface leader vehicle and
a underwater vehicle that follows the leader (at a constant depth), it will be used a cooperative control
setup know as Cooperative Leader Traking.
On the last stage the underwater vehicle that follows the leader will be replaced by a diver. This way
the Cooperative Leader Traking algorithm should be adapted to handle the diver unpredictability.
The proposed algorithms will be first subjected to a set of simulation tests on Matlab Simulink.
Keywords: Autonomous Marine Robots, Cooperative Control of Autonomous Marine Vehicles,
Cooperative Leader Tracking, Humans in the control loop, Medusa Class Marine Robot.
ix
x
Conteudo
Acknowledgments . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . v
Resumo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . vii
Abstract . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . ix
Lista de Tabelas . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . xiii
Lista de Figuras . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . xvi
Glossario xvii
Lista de Abreviacoes xviii
Glossario . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . xviii
1 Introducao 1
1.1 Motivacao . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2
1.2 Definicao do Problema . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2
1.3 Estado de Arte . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
1.3.1 Projecto INFANTE . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
1.3.2 Projecto ASIMOV . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
1.3.3 Projecto CO3-AUV’s . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4
1.3.4 Projecto CADDY . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5
1.4 Metodologia da Solucao . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7
2 Modelacao do Veıculo Medusa 9
2.1 Definicao dos eixos de coordenadas . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
2.2 Cinematica . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
2.3 Dinamica . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
2.4 Equacoes do movimento simplificadas . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
3 Controlador de Orientacao 17
3.1 Modelo de Estados . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17
3.2 Regulador Linear Quadratico . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19
3.2.1 Equacoes Gerais . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19
3.2.2 Projecto Regulador . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20
3.2.3 Modelo de estados Simplificado . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24
xi
3.2.4 Inclusao de Efeito Integral . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27
3.3 Gain Scheduling . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30
3.3.1 Saturacoes e Anti Wind-Up . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33
4 Controlador de Seguimento de Caminho 37
4.1 Estrategias para seguimento de caminho . . . . . . . . . . . . . . . . . . . . . . . . . . . 37
4.1.1 Metodo line of sight segundo Fotis A. Papoulias . . . . . . . . . . . . . . . . . . . 38
4.1.2 Metodo segundo Pramod Maurya . . . . . . . . . . . . . . . . . . . . . . . . . . . 41
4.1.3 Metodo segundo Alain Micaelli e Claude Samson . . . . . . . . . . . . . . . . . . 43
4.1.4 Metodo Line of Sight modificado . . . . . . . . . . . . . . . . . . . . . . . . . . . . 48
4.1.5 Metodo Line of Sight com Rabbit . . . . . . . . . . . . . . . . . . . . . . . . . . . . 53
4.2 Implementacao Algoritmo Seguimento de Caminho . . . . . . . . . . . . . . . . . . . . . . 56
4.2.1 Seguimento de segmentos de circunferencia . . . . . . . . . . . . . . . . . . . . . 56
4.2.2 Seguimento de segmentos de recta . . . . . . . . . . . . . . . . . . . . . . . . . . 57
4.2.3 Resultado de Simulacao Algoritmo de seguimento de caminho . . . . . . . . . . . 58
4.3 Seguimento de caminho na presenca de correntes . . . . . . . . . . . . . . . . . . . . . . 61
4.3.1 Seguimento de caminho sem correccao de correntes . . . . . . . . . . . . . . . . 62
4.3.2 Seguimento de caminho conhecendo as correntes . . . . . . . . . . . . . . . . . . 63
4.3.3 Seguimento de caminho com estimacao de correntes . . . . . . . . . . . . . . . . 65
4.4 Algoritmo de seguimento de caminho com malha de controlo de orientacao . . . . . . . . 67
5 Controlo Cooperativo (Cooperative Path Following) 69
5.1 Controlador de seguimento de caminho cooperativo . . . . . . . . . . . . . . . . . . . . . 69
5.1.1 Analise de Estabilidade . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 70
5.1.2 Estimador da variavel de coordenacao . . . . . . . . . . . . . . . . . . . . . . . . . 72
6 Controlo Cooperativo (Cooperative Leader Tracking) 77
6.1 Controlo cooperativo segundo um veıculo lıder . . . . . . . . . . . . . . . . . . . . . . . . 77
6.1.1 Estimador da posicao do veıculo lıder . . . . . . . . . . . . . . . . . . . . . . . . . 79
7 Conclusao 81
7.1 Resultados Obtidos . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 81
7.1.1 Algoritmos de controlo de baixo nıvel . . . . . . . . . . . . . . . . . . . . . . . . . 81
7.1.2 Algoritmos de controlo de alto nıvel . . . . . . . . . . . . . . . . . . . . . . . . . . 82
7.2 Trabalho Futuro . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 82
Referencias 87
xii
Lista de Tabelas
2.1 Parametros SNAME . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10
3.1 Parametros controladores para Escalonamento de Ganhos . . . . . . . . . . . . . . . . . 31
xiii
xiv
Lista de Figuras
1.1 Robo INFANTE desenvolvido pelo Instituto de Sistemas e Robotica . . . . . . . . . . . . 3
1.2 Estrutura utilizada no projecto Azimov . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
1.3 Visao do Projecto CO3-AUV . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4
1.4 Diver Assistence Unit . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4
1.5 Visao do Projecto CADDY . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5
1.6 Funcoes a Desempenhar pelo Diving Buddy . . . . . . . . . . . . . . . . . . . . . . . . . 6
1.7 Sensor Ultra Short Baseline (USBL) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6
2.1 Representacao referenciais Body Frame e World Frame . . . . . . . . . . . . . . . . . . . 9
2.2 Resultado do teste aquatico para determinar X|v|v . . . . . . . . . . . . . . . . . . . . . . 15
3.1 Estrutura do Controlador LQR . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22
3.2 LQR Simulation Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23
3.3 Resposta do Controlador LQR com Variacao de v . . . . . . . . . . . . . . . . . . . . . . 24
3.4 Diagrama de Bode Modelo de Estados Simplificado . . . . . . . . . . . . . . . . . . . . . 24
3.5 Controlador LQR simplificado . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25
3.6 LQR Simplificado Simulation Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26
3.7 Perturbacoes LQR simplificado . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26
3.8 LQR com efeito integral . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28
3.9 LQR com efeito integral Simulation Results . . . . . . . . . . . . . . . . . . . . . . . . . . 28
3.10 LQR integrador a saıda do controlador . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29
3.11 Estrutura de Escalonamento de Ganhos . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32
3.12 LQR Simplificado Simulation Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32
3.13 Saturacao ao nıvel dos motores . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33
3.14 Diagrama de blocos completo do controlo de Orientacao . . . . . . . . . . . . . . . . . . 34
3.15 Bloco de Controlo de Orientacao . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35
3.16 Resultados simulacao do Controlador de Orientacao (teste a saturacao) . . . . . . . . . . 36
4.1 Configuracao segundo algoritmo proposto por Fotis Papoulias . . . . . . . . . . . . . . . 38
4.2 Configuracao algoritmo proposto por Pramod Maurya . . . . . . . . . . . . . . . . . . . . 42
4.3 Configuracao algoritmo proposto por Claude Samson . . . . . . . . . . . . . . . . . . . . 44
4.4 Configuracao algoritmo Line of Sight modificado . . . . . . . . . . . . . . . . . . . . . . . 49
xv
4.5 Configuracao algoritmo Line of Sight modificado para o seguimento de rectas . . . . . . . 51
4.6 Configuracao algoritmo Line of Sight com Rabbit . . . . . . . . . . . . . . . . . . . . . . . 53
4.7 Seguimento de caminhos composto apenas por segmentos de recta (u = 0.5m/s) . . . . 59
4.8 Seguimento de caminho composto apenas por circunferencias (u = 1m/s) . . . . . . . . 60
4.9 Simulacao de seguimento de caminho complexo (varrimento espacial) . . . . . . . . . . 61
4.10 Efeito da corrente perpendicular a trajectoria . . . . . . . . . . . . . . . . . . . . . . . . . 62
4.11 Simulacao do efeito da corrente para uma velocidade u = 0.5m/s e corrente Vx = 0.3m/s 63
4.12 Esquema vectorial para correccao de corrente . . . . . . . . . . . . . . . . . . . . . . . . 64
4.13 Simulacao da correccao de corrente para uma velocidade u = 0.5m/s e corrente Vx =
0.3m/s . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 65
4.14 Estrutura do Complementary Filter utilizado para a estimacao de correntes . . . . . . . . 66
4.15 Resultados estimacao de correntes com medidas corrompidas por ruıdo branco gaussiano 67
4.16 Estrutura em cascata dos controladores de seguimento de caminho e de orientacao . . . 68
4.17 Simulacao de seguimento de caminho com ciclo controlo de orientacao . . . . . . . . . . 68
5.1 Exemplo de grafos . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 70
5.2 Cronologia da transmissao da mensagens entre veıculos . . . . . . . . . . . . . . . . . . 74
5.3 Processo do estimador com falhas nas medidas (atraso simples) . . . . . . . . . . . . . . 75
5.4 Processo do estimador com falhas nas medidas (inversao na ordem de chegada) . . . . 75
5.5 Trajectoria . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 76
5.6 Estimacao do parametro S e erro de seguimento entre veıculos . . . . . . . . . . . . . . . 76
5.7 Estimacao do parametro S e erro de seguimento entre veıculos . . . . . . . . . . . . . . . 76
6.1 Configuracao Algoritmo CLT . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 78
6.2 Estimador da pose do veıculo lıder . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 79
6.3 Trajectoria . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 80
6.4 Parametro dS e evolucao das velocidades dos veıculos . . . . . . . . . . . . . . . . . . . 80
xvi
Glossario
Ultra Short Baseline Sensor acustico capaz de medir a distancia e os angulos de azimute e elevacao
(posicao relativa) do sensor a um ”beacon” acustico.
WindUp Normalmente denominado de Wind-Up do integrador, este efeito e visivel especialmente nos
controladores PID. Quando algum elemento da cadeia de accao atinge a saturacao, o esforco de
controlo nao se traduz numa reducao directa do erro da variavel de seguimento, assim a parte da
resposta integral vai crescer indefinidamente enquanto nao existir uma inversao de polaridade do
sinal de erro. Devido a este efeito, a variavel a controlar pode apresentar transitorios de elevada
amplitude e duracao.
Angulo Side Slip E o angulo definido entre o eixo longitudinal ao veıculo e a direccao do vector velo-
cidade do fluıdo em relacao ao veıculo.
xvii
Lista de Abreviacoes
AMV Autonomous Marine Vehicle.
ASC Autonomous Surface Craft.
ASV Autonomous Surface Vehicle.
AUV Autonomous Underwater Vehicle.
CLT Cooperative Leather Traking.
CPF Cooperative Path Following.
DAV Diver Assistence Unit.
GPS Global Positioning System.
LQR Linear Quadratic Regulator.
PD Proportional Derivative.
PI Proportional Integral.
PID Proportional Integral Derivative.
ROV Remote Operated Vehicle.
RPM Rotacoes Por Minuto.
USBL Ultra Short Baseline.
xviii
Capıtulo 1
Introducao
O oceano cobre cerca de setenta por cento da superfıcie do planeta terra, apresentando-se como uma
das maiores fontes de recursos deste planeta. A exploracao deste meio permitira conhecer melhor toda
a sua geologia, fauna, flora, assim como a sua dinamica. Este conhecimento permitira a preservacao e
uma melhor utilizacao dos recursos marıtimos.
Nos ultimos anos verificou-se um crescente interesse na exploracao dos oceanos, no entanto, a
exploracao do meio subaquatico apresenta novos desafios em termos de engenharia. Trata-se de um
meio em que nao e possıvel comunicar atraves de ondas electromagneticas e a localizacao e difıcil,
pois e um meio muito vasto, em constante mudanca, que apresenta constantes perturbacoes, como
as correntes e mares. O meio subaquatico apresenta assim riscos e perigos que dificultam a sua
exploracao e desta forma e necessario poder contar com a mais avancada tecnologia e metodologias
para a recolha e tratamento de dados. Existe a necessidade de poder contar com AMV’s (Autonomous
Marine Vehicles). No futuro, estes veıculos permitirao chegar a sıtios ate agora inexplorados, quer por
humanos ou ROV’s (Remote Operated Vehicles) e explorar vastas areas ou ainda, auxiliar humanos em
missoes cooperativas.
O desenvolvimento destes veıculos requer metodos de controlo robustos e confiaveis, que funcio-
nem sujeitos a perturbacoes externas e com comunicacao limitada. E importante considerar que estes
robos vao operar num ambiente bastante hostil, onde nao ha acesso a sinal de GPS (Global Positi-
oning System), ha uma forte atenuacao das ondas electromagneticas, sendo as comunicacoes efec-
tuadas atraves de sinais acusticos que apresentam uma propagacao complexa neste meio (multiplas
trajectorias de propagacao). E ainda de notar que este tipo de veıculos sao muitas vezes veıculos sub-
actuados, o que significa que tem menos inputs de controlo do que graus de liberdade, o que constitui
um desafio extra para o controlo dos mesmos num cenario tao adverso.
O controlo destes veıculos tem sido um tema polemico ao longo dos ultimos anos, com diferentes
contribuicoes de diferentes areas e perspectivas do controlo. Pretende-se que a presente dissertacao
seja ela propria uma contribuicao para o problema do controlo dos AMV’s.
1
1.1 Motivacao
Actualmente existem operacoes subaquaticas que apresentam elevados riscos para os mergulhadores
que as executam. E nesse sentido que os AUV (Autonomous Underwater Vehicles) podem desempe-
nhar um papel importante, ajudando (e por vezes substituindo) os mergulhadores nessas missoes.
Desta forma, essas operacoes podem tornar-se mais rapidas, eficientes e de custos mais reduzi-
dos. Esta visao de como deve ser uma operacao subaquatica requer novos algoritmos de controlo
e navegacao capazes de interagir com os humanos. Surge assim um conjunto de aplicacoes em que
se pode considerar os humanos (mergulhadores) como parte integrante do ciclo de controlo.
Uma possıvel aplicacao seria ter um AMV que se desloca a superfıcie com acesso ao sinal de GPS.
Este seria capaz de se localizar via GPS e por comunicacao com o mergulhador, descobrir a localizacao
do mesmo. Assim o veıculo podera guardar um registo da actividade do mergulhador ou ajudar este na
sua localizacao e navegacao de forma a reduzir o tempo e o custo da missao subaquatica e aumentando
o nıvel de seguranca do mergulhador.
1.2 Definicao do Problema
Actualmente um dos maiores problemas das missoes subaquaticas e a dificuldade de localizacao e por
consequencia a navegacao, uma vez que, ao contrario dos veıculos terrestres, estes nao tem acesso
ao sinal de GPS.
Neste projecto recorre-se a um AMV que se encontra a superfıcie e que, portanto, tem acesso ao
sinal de GPS, conhecendo assim a sua posicao inercial. Em conjunto com este veıculo, encontra-se
um veıculo subaquatico ou um mergulhador. Se de alguma forma1, o veıculo submerso ou o mergu-
lhador conseguir medir a sua posicao relativa ao veıculo que se encontra a superfıcie, estes passam a
conhecer as suas posicoes inerciais.
O sensor USBL (Ultra Short Baseline), que sera utilizado para determinar a posicao relativa do
veıculo submerso ou mergulhador, utilizara comunicacao acustica. A utilizacao de acustica apresenta
problemas relativos as multiplas trajectorias de propagacao do sinal, devido as multiplas reflexoes2 que
o sinal sofre. Assim, de forma a promover a propagacao directa do sinal acustico, os veıculos deverao
navegar alinhados no plano vertical.
O veiculo de superfıcie actuara como lıder neste processo, e o veıculo submerso ou mergulhador
deverao seguir a trajectoria do lıder de forma a manter o alinhamento vertical. Surge aqui um metodo de
controlo denominado de CLT (Cooperative Leader Tracking), que sera o alvo de estudo deste projecto.
O algoritmo de controlo sugerido devera ser capaz de lidar com a imprevisibilidade que estara associada
ao cenario em que o AUV e substituıdo por um mergulhador.
1A forma de medir a posicao relativa sera atraves de um sensor de Ultra Short Baseline (USBL)2Essas reflexoes sao provocadas pela superfıcie aquatica, pelo fundo do mar e pelas diferentes camadas que o compoem
2
1.3 Estado de Arte
1.3.1 Projecto INFANTE
O projecto INFANTE [1997 - 2001] tinha como principal objectivo era construir um veıculo subaquatico
para tarefas como inspeccao. Desse projecto resultou o robo INFANTE [3].
Figura 1.1: Robo INFANTE desenvolvido pelo Instituto de Sistemas e Robotica {Fonte:[3]}
1.3.2 Projecto ASIMOV
O projecto ASIMOV [1998 - 2001] trouxe alguma evolucao relativamente ao projecto INFANTE, nome-
adamente a nıvel da cooperacao entre veıculos. O principal objectivo deste projecto era desenvolver
a cooperacao entre um veıculo de superfıcie (ASC) e um veıculo submerso (AUV). Estes dois veıculos
funcionam numa configuracao master/slave, e navegam alinhados na vertical, para aumentar a veloci-
dade das comunicacoes e evitar as reflexoes existentes no canal de comunicacao acustico pelo qual os
robos comunicam (configuracao bastante semelhante a que sera proposta na presente dissertacao). A
principal aplicacao deste projecto era utilizar este conjunto de veıculos para explorar as fontes hidroter-
mais presentes no banco D. Joao de Castro, nos Acores, Portugal, [4].
Figura 1.2: Estrutura utilizada no projecto ASIMOV {Fonte:[4]}
3
1.3.3 Projecto CO3-AUV’s
O projecto CO3-UAV [2009 - 2012] permitiu desenvolver e testar um conjunto de sistemas cognitivos
para coordenacao e controlo cooperativo de multiplos AMV’s. Este conjunto de AMV’s foi utilizado com
o intuito de auxiliar um mergulhador ao nıvel da localizacao e navegacao, [5].
Figura 1.3: Visao do Projecto CO3-AUV {Fonte:[6]}
Numa das aplicacoes [7], utilizou-se um conjunto de AMV’s que mantem uma formacao especifica e
predefinida a superfıcie. Encontrando-se a superfıcie, estes veıculos podem contar com acesso ao sinal
de GPS, portanto cada um dos veıculos a sua posicao inercia. Cada um dos veıculos de superfıcie pode
”interrogar” o mergulhador, que transporta um modem acustico e pelo principio de ”time-of-flight” e
possıvel determinar a posicao do mergulhador. Desta forma pode-se utilizar este sistema para executar
dois tipos de missao:
• Numa primeira missao os veıculos poderao guiar um agente (humano ou AUV) que se encontra
a uma certa profundidade, de acordo com um caminho desejado. No caso do mergulhador as
instrucoes sao dadas atraves de um modulo denominado DAU (Diver Assistence Unit) que se
pode visualizar na figura 1.4.
• Numa outra aplicacao os veıculos podem seguir a trajectoria do agente mergulhador e desta forma
guardar um registo da sua actividade enquanto este efectua livremente a exploracao.
.
Figura 1.4: Diver Assistence Unit {Fonte:[6]}
4
No ambito deste projecto foi desenvolvida a dissertacao [7], que propoe um algoritmo de controlo
denominado como CPF (cooperative path following). Aqui, um conjunto de veıculos equipados com
sistemas que permitem a cada um deles saber onde esta a cada instante temporal (i.e., utilizando
GPS) seguem caminhos predefinidos e ajustam as suas velocidades (com base na comunicacao que
se estabelece entre eles) de modo a convergirem para uma formacao geometrica desejada, que evolui
a uma velocidade terminal desejada. Neste tipo de controlo cooperativo nao existe o conceito de lıder
que sera explorado na presente dissertacao com algoritmos de CLT.
1.3.4 Projecto CADDY
Figura 1.5: Visao do Projecto CADDY {Fonte:[1]}
Actualmente em execucao, o projecto CADDY [Jan. 2014 - Dec. 2016] (Cognitive Autonomous
Diving Buddy) vai no sentido do projecto CO-3UAV e pretende desenvolver um AUV para funcionar
como parceiro do mergulhador. Este veıculo sera auxiliado por um ASV, aumentando a capacidade de
monitorizacao, assistencia e consequentemente a seguranca do mergulhador. O sistema desenvolvido
sera capaz de aprender, interpretar e adaptar-se ao comportamento do mergulhador, estado fısico e
accoes do mesmo. O objectivo sera substituir um parceiro humano por um parceiro robotico capaz de:
• a) - Actuar como um observador que monitoriza constantemente o mergulhador
• b) - Actuar como um ”escravo” do mergulhador que pode efectuar as mais diversas tarefas como:
captar imagens, fazer reconhecimento da area, entre outras
• c) - Guiar o mergulhador em seguranca pelo ambiente subaquatico em que se encontra, de acordo
com um comando de alto nıvel especificado para a missao.
E no ambito deste projecto que surge o tema da presente dissertacao. Surge aqui um problema
de controlo denominado de Cooperative Leader Tracking (CLT) Este problema de controlo opoe-se de
certa forma ao problema explorador em [7] (projecto CO3-AUV’s). Neste caso, existe um veıculo lıder
5
(a) Robot como guia (b) Robot como observador (c) Robot como ”escravo”
Figura 1.6: Funcoes a Desempenhar pelo Diving Buddy {Fonte:[1]}
a superfıcie e um veıculo submerso ou seguidor. O veıculo lıder manobra ao longo de um caminho
predefinido e o veıculo submerso devera seguir o lıder, embora nao conheca o caminho a seguir a
priori. O veıculo lıder encontra-se a superfıcie e por isso tem acesso a sinal de GPS, transporta tambem
consigo um sensor de USBL. Utilizando o sensor de USBL o veıculo lıder sera capaz de determinar a
posicao relativa do veıculo submerso. Pode-se visualizar esse sensor na figura 1.7, este possui um
conjunto de pontos (Hidrofones) que recebem um sinal acustico proveniente do veıculo a superfıcie e
atraves da desfasagem entre o sinal que chega aos diferentes pontos, e do tempo de propagacao do
sinal e possıvel determinar em coordenadas polares, a posicao relativa a fonte do sinal acustico, que
na presente aplicacao encontrar-se-a no veıculo submerso. Nesta configuracao, o veıculo lıder passa
agora a conhecer as posicoes globais de ambos os veıculos, portanto, comunicando estas posicoes ao
veıculo submerso atraves de um canal acustico, ambos os veıculos passam a ter conhecimento das
suas posicoes globais.
Figura 1.7: Sensor Ultra Short Baseline (USBL) {Fonte:[6]}
Desta forma, embora o veıculo submerso nao conheca o caminho, podera efectuar um seguimento
do lıder uma vez que conhece a posicao do mesmo. Adicionalmente, o veiculo seguidor, necessitara
tambem de informacao da velocidade do veıculo lıder para que possa atingir a coordenacao com o
veıculo lıder (alinhamento vertical).
6
1.4 Metodologia da Solucao
De forma a cumprir os objectivos propostos dividiu-se o problema principal em sub-problemas. A meto-
dologia a utilizar pressupoe o cumprimento desses sub problemas pela ordem apresentada:
• Modelar o Veıculo - Estudo e Implementacao de um modelo em Matlab para o veıculo Medusa.
• Controlador Orientacao -Estudo e implementacao de um controlador de orientacao para o veıculo
Medusa, este controlador devera ser capaz de rejeitar perturbacoes externas que surjam a saıda
do controlador.
• Controlador de Seguimento de Caminho - Estudo e implementacao de um controlador para
seguimento de caminho (Path Following controller). Este controlador tera como base o controlador
de orientacao na sua operacao. Trata-se de um controlo de mais alto nıvel que enviara sinais de
comando ao controlo de baixo nıvel.
• Controlo Cooperativo de Multiplos Veıculos - A partir dos controladores projectados anterior-
mente passar ao estudo e implementacao de uma estrategia de controlo para o conjunto veıculo
de superfıcie mais veıculo submerso ou mergulhador. O conjunto de agentes apresentado deve
ser capaz de navegar no mesmo plano vertical atraves de um metodo de controlo denominado
CLT.
• Integracao e Teste - Integracao de todos os algoritmos desenvolvidos e teste do conjunto por
simulacao via Matlab Simulink.
O principal objectivo desta dissertacao sera o desenvolvimento de um sistema de controlo coope-
rativo, como preparacao para a inclusao de humanos na malha de controlo. Para isso, e proposto um
algoritmo de CLT capaz de se adaptar a presenca de um mergulhador na malha de controlo.
7
8
Capıtulo 2
Modelacao do Veıculo Medusa
No processo de desenvolvimento de um controlador e essencial poder contar com o modelo do sistema
ou processo a controlar. Desta forma e possıvel desenvolver e testar os controladores atraves do
modelo matematico do sistema. Neste capıtulo sera derivado um modelo matematico para o veıculo
Medusa que sera o veıculo utilizado neste projecto. Este trabalho ja foi previamente realizado em [8],
[7], [9] e [10] pelo que este capıtulo apresenta semelhancas notorias com os trabalhos posteriores. No
entanto este e um passo essencial para o desenvolvimento dos controladores que se propoe ao longo
da dissertacao e a omissao deste capıtulo dificultaria a compreensao do restante documento.
2.1 Definicao dos eixos de coordenadas
Para definir as equacoes do movimento de qualquer veıculo e necessario considerar a direccao e sen-
tido do movimento. Desta forma, existe a necessidade de definir pelo menos um referencial solidario
com o movimento do veıculo, {U}, composto pelos eixos ortogonais {xu, yu, zu} (normalmente de-
nominado de Body Fixed Frame) e um referencial fixo que nunca altera a sua posicao e orientacao,
{B}, composto pelos eixos ortogonais {xb, yb, zb} (normalmente denominado World Fixed Frame). A
representacao destes referenciais encontra-se explicita na Figura 2.1.
Figura 2.1: Representacao referenciais Body Frame e World Frame, {Fonte: [7]}
9
O referencial do veıculo {B} e definido de forma a estar centrado com o centro de massa do veıculo.
Os eixos que o compoem sao definidos da seguinte forma:
• xb - Eixo definido ao longo da longitudinal do veıculo com direccao da popa para a proa.
• yb - Eixo definido ao longo da transversal do veıculo com direccao de bombordo para estibordo.
• yb - Eixo normal ao veıculo com direccao de cima para baixo do mesmo.
Definidos os referenciais e possıvel representar concretamente a posicao e o movimento do veıculo,
basta para isso representar a posicao e o movimento do eixo {B} relativamente ao eixo {U}.
De forma a representar a posicao do veıculo (i.e. do eixo {B}) relativamente ao eixo {U} sao ne-
cessarios seis parametros, a que correspondem seis graus de liberdade, tres de posicao {x, y, z} e
tres de orientacao {φ, θ, ψ} que, neste caso, correspondem a angulos de Euler definidos como uma
sequencia de rotacoes sobre {x − y − z} que levam o referencial {U} a alinhar-se com o referen-
cial {B}. Da mesma forma, a representacao do movimento do veıculo nao e mais que uma variacao
dos seis graus de liberdade descritos anteriormente. Assim utilizando a notacao SNAME, resultam os
parametros apresentados na tabela 2.1.
Graus de liberdade Forcas e momentos Velocidades angulares e lineares Posicao e orientacao
Movimento em x (surge) X u x
Movimento em y (sway) Y v y
Movimento em z (heave) Z w z
Rotacao sobre x (Roll) K p φ
Rotacao sobre y (Pitch) M q θ
Rotacao sobre z (Yaw) N r ψ
Tabela 2.1: Parametros SNAME{Fonte:[8]}
Escrevendo todos estes parametros em forma vectorial resulta:
• η1 = [x, y, z]T - vector posicao da origem de {B} expressa em {U}.
• η2 = [φ, θ, ψ]T - vector orientacao do referencial {B} em relacao a {U}.
• v1 = [u, v, w]T - vector velocidade linear da origem de {B} em relacao a {U} expresso em {B}.
• v2 = [p, q, r]T - vector velocidade angular de {B} em relacao a {U} expresso em {B}
• τ1 = [X,Y, Z]T - vector de forcas actuantes expressas em {B}
• τ2 = [K,M,N ]T - vector de momentos actuantes expressos em {B}
Juntando as posicoes, velocidades, e as forcas e momentos resulta numa forma compacta:
10
η = [ηT1 , ηT2 ]T
v = [vT1 , vT2 ]T
τRB = [τT1 , τT2 ]T
2.2 Cinematica
A cinematica trata apenas do aspecto geometrico do movimento sem entrar em conta com momentos
ou forcas aplicadas. Permite assim definir as transformacoes ”transportam” um vector posicao ou
velocidade de um referencial para outro. Comecando por definir η1 podemos pensar que se os eixos se
encontrarem alinhados resulta (2.1):
η1 = v1 (2.1)
Na realidade, isso pode nao ser verdade e e necessario aplicar uma transformacao de forma a
garantir que os referenciais se encontram alinhados. Ao faze-lo, esta-se a transformar um vector de
velocidades de um referencial para o outro, resultando (2.2).
η1 = UBR(η2)v1 (2.2)
Esta transformacao sera definida a custa de tres transformacoes elementares (equacao (2.3))1, uma
para cada angulo de Euler (Roll, Pitch, Yaw, ou sequencia X-Y-Z).
Rx,φ =
1 0 0
0 Cφ −Sφ
0 Sφ Cφ
Ry,θ =
Cθ 0 Sθ
0 1 0
−Sθ 0 Cθ
Rz,ψ =
Cψ −Sψ 0
Sψ Cψ 0
0 0 1
(2.3)
Juntando as tres transformacoes elementares, e possıvel chegar a transformacao que leva o vec-
tor da velocidade linear da origem de {B} relativamente a {U} expresso em {B}, para o vector da
velocidade linear da origem de {B} relativamente a {U} expresso em {U} (equacao (2.4)).
UBR(η2) = Rz,ψRy,θRx,φ =
CψCθ −SψCφ+ CψSθSφ SψSθ + CψSθCφ
SψCθ CψCφ+ SψSθSφ −CψSφ+ SψSθCφ
−Sθ CθSφ CθCφ
(2.4)
O vector de velocidade angular v2 e definido a custa do vector η2 considerando a sequencia de
angulos de euler X-Y-Z:
v2 =
φ
0
0
+RTx,φ
0
θ
0
+RTx,φRTy,θ
0
0
ψ
= Q−1(η2)η2 (2.5)
1Por simplicidade obtou-se por representar a funcoes Sin e Cos por S e C respectivamente
11
O raciocınio por detras desta transformacao consiste em considerar os dois eixos (B e U ) inicial-
mente alinhados. A primeira rotacao a aplicar sera em torno do eixo dos X de B nesta fase tem-se que
φ = p. Ao ter em conta a segunda rotacao da sequencia (no eixo dos Y do referencial B) e necessario
voltar a alinhar os eixos, ou seja, e necessario ”desfazer” a rotacao sobre o eixo dos X de B feita an-
teriormente, portanto resulta o segundo termo da equacao (2.5). Pelo mesmo principio de ”desfazer”
as rotacoes previamente efectuadas resulta o terceiro termo da equacao (2.5).
Uma vez que se pretende definir a transformacao de v2 para η2 e necessario calcular a transformacao
inversa a aplicada na equacao (2.5). Desta forma resulta (2.6):
Q−1(η2) =
1 0 −Sθ
0 Cφ CθSφ
0 −Sφ CθCφ
⇒ Q(η2) =
1 SφTθ CφTθ
0 Cφ −Sφ
0 SφCθ
CφCθ
(2.6)
De notar que existe uma singularidade em Q(η2) para um angulo de pitch de ±90o graus, no entanto,
as condicoes em que se pretende operar o veıculo estao longe deste angulo de pitch (cerca de 0o),
nao sendo um problema para o controlo. Caso se quisesse operar o veıculo nestas condicoes seria
necessario alterar a sequencia considerada para os angulos de Euler, ou utilizar outro sistema de
representacao dos angulos.
Juntando as equacoes da cinematica apresentadas pode-se escrever de forma compacta (equacao
(2.7)):
η1
η2
=
UBR(η2) 03×3
03×3 Q(η2)
v1
v2
⇔ η = J(η)v (2.7)
2.3 Dinamica
As equacoes da dinamica ficam mais simples quando sao expressas no referencial {B}, uma vez que
as forcas e momentos sao gerados(as) ou aplicados(as) no veıculo. Utilizando a expressao geral da
dinamica do corpo rıgido:
MRB v + CRB(v)v = τRB (2.8)
Em que:
• MRB e a matriz com os parametros da inercia
• CRB(v) e a matriz com os termos de Coriolis e centripticos
• τRB e o vector generalizado de todas as forcas e momentos externos, e pode ele proprio ser
decomposto em:
τRB = τin + τA + τD + τR + τdist
Onde:
12
– τin− corresponde as forcas e momentos gerados pelos inputs de controlo, (propulsores, no
caso do veıculo medusa).
– τA− corresponde as forcas e momentos resultantes da ”added mass”, apresentando a ex-
pressao:
τA = −MAv − CA(v)v
Por sua vez:
∗ MA− corresponde a matriz dos termos de ”added mass”, cada um desses termos
corresponde a uma forca quando se tenta acelerar o veıculo ao longo de uma certa
direccao. (exemplo: Xu corresponde a forca criada em x quando se tenta acelerar o
veıculo na direccao x)
∗ CA(v)− corresponde a matriz dos termos de Coriolis e de added mass, tem um sig-
nificado semelhante ao de MA no entanto neste caso o que e gerado sao momentos e
nao forcas.
– τD− corresponde aos termos hidrodinamicos devido ao impulso, resistencia na agua, friccao,
entre outros, representado pela expressao:
τD = −D(v)v
– τR− corresponde aos momentos e forcas de restituicao, que dependem da gravidade e da
densidade do fluıdo em que o veıculo se encontra, e apresenta a expressao:
τR = −g(η)
– τdist− momentos e forcas provocados(a) por perturbacoes externas como ondas, vento, cor-
rentes, entre outras.
Substituindo os resultados anteriores na equacao geral para o corpo rıgido resulta (equacao (2.9)):
MRB v + CRB(v)v +MAv + CA(v)v +D(v)v + g(η) = τin + τdist (2.9)
que de forma compacta resulta (equacao (2.10)):
Mv + C(v)v +D(v)v + g(η) = τin + τdist (2.10)
considerando, M = MRB +MA e C(v) = CRB + CA.
2.4 Equacoes do movimento simplificadas
Considerando que o veıculo em questao apenas vai operar num plano bidimensional (i.e., a uma pro-
fundidade constante), as equacoes (2.7) da cinematica podem ser simplificadas para (2.11):
13
x = ucosψ − vsinψ
y = usinψ − vcosψ
ψ = r
(2.11)
O veıculo a controlar sera o robo medusa que possui dois propulsores, um a estibordo outro a
bombordo distanciados de uma distancia l, considerando Fest e Fbb as forcas geradas a estibordo e a
bombordo respectivamente, pode-se definir (equacao (2.12)):
τu = Fest + Fbb
τr = l(Fest − Fbb)(2.12)
Onde τu representa a forca gerada no sentido de xB pelos propulsores (modo comum); τr representa
o torque gerado em torno do eixo zB devido a diferenca de forcas geradas pelos dois propulsores (modo
diferencial).
A semelhanca do que aconteceu com a cinematica, tambem as equacoes da dinamica ficam sim-
plificadas quando consideramos o movimento num plano, alem disso considera-se, para ja, que nao
existem perturbacoes externas, resulta assim (equacao (2.13)):
muu−mvvr + duu = τu
mv v +muur + dvv = 0
mr r −muvuv + drr = τr
(2.13)
Os termos mu, mv e mr, representam os termos massa e de ”added mass”, enquanto os termos du,
dv e dr representam os termos da resistencia hidrodinamica. Esses mesmos termos podem ser ainda
separados em parametros que reflectem as caracterısticas fısicas do veıculo, equacao (2.14).
mu = m−Xu du = −Xu −X|u|u|u|
mv = m− Yv dv = −Yv − Y|v|v|v|
mr = Iz −Nr dr = −Nr −N|r|r|r|
muv = mu −mv
(2.14)
Obtidas as equacoes do movimento para o robo medusa, falta saber os parametros que adaptam
estas equacoes ao veıculo. Alguns desses parametros podem ser medidos directamente no veıculo,
outros sao determinado atraves de experiencias realizadas em tanques aquaticos.
Na figura 2.2 apresenta-se de forma esquematica o que podera ser um teste aquatico para deter-
minar o impacto de v em X e portanto, determinar o parametro X|v|v.
Pode-se verificar que quando v 6= 0, aparece uma forca em X que e proporcional a esse v, desta
forma e possıvel chegar a X|v|v. Em trabalhos anteriores, [7], estes parametros ja foram determinados.
Apresentando-se para o robo Medusa como:
m = 17.0Kg Iz = 1Kg.m2
14
Xu = −20Kg Yv = −30Kg Nr = −8.69Kg.m2
Xu = −0.2Kg/s Yv = −50Kg/s Nr = −4.14Kg.m/s
X|u|u = −25Kg/m Y|v|v = −0.01Kg/m N|r|r = −6.23Kg.m
Figura 2.2: Resultado do teste aquatico para determinar X|v|v {Fonte:[6]}
15
16
Capıtulo 3
Controlador de Orientacao
Nesta fase sera considerado um modelo de estado construıdo a partir das equacoes simplificadas
obtidas no Capıtulo anterior, cujo o objectivo sera desenvolver um controlador de orientacao a unica
saıda do sistema sera [ψ] e a unica entrada de controlo sera [τr].
Uma vez que o sistema a controlar nao e linear, sera feita uma linearizacao em torno do ponto de
equilıbrio. Posteriormente sera utilizada um tecnica de escalonamento de ganhos (Gain Scheduling)
para generalizar para outros pontos de operacao.
3.1 Modelo de Estados
Recuperando as equacoes da dinamica e isolando as variaveis de interesse para o modelo de estados
(considerando um vector de estados [ψ, v, r]), pode-se obter:
ψ = r
y = v =−muur − dvv
mv
ψ = r =Tr +muvuv − drr
mr
(3.1)
O modelo de estado pode agora ser obtido atraves de uma linearizacao das equacoes apresentadas
anteriormente. Para isso sera necessario encontrar um ponto de equilıbrio, considera-se o candidato a
ponto de equilıbrio como:
[u = 0.5 m/s ; v = 0 m/s ; r = 0 rad/s] (3.2)
O valor de u foi escolhido tendo em conta um compromisso entre velocidade e economia da bateria
do robo medusa. A funcionar em torno deste ponto, o robo apresenta uma autonomia satisfatoria sem
estar a trabalhar a uma velocidade visıvelmente reduzida.
Este sera ponto de equilıbrio caso o sistema, quando iniciado nesse mesmo estado, jamais saıa
dele (sem actuacao externa). Dado o sistema nao linear isso corresponde a:
x :∂x
∂t= f(x) para f(x) = 0 (3.3)
17
Desta forma, recuperando (3.1) e seguindo o apresentado em (3.3) resulta:
r = 0
−muur − dvvmv
= 0
Tr +muvuv − drrmr
= 0
⇔
r = 0
v = 0
Tr = 0
(3.4)
Portanto, o ponto e efectivamente ponto de equilıbrio desde que Tr cumpra a igualdade apresentada
em (3.4), ou esteja numa vizinhanca deste ponto. E de salientar que qualquer velocidade u e um ponto
de equilıbrio para o sistema considerado. Esse facto sera utilizado mais tarde quando for necessario
linearizar o sistema em torno de outros pontos de operacao diferentes de u = 0.5m/s.
A linearizacao em torno do ponto de equilıbrio apresentado anteriormente fica assim definida como:
A = ∂f∂x |x=x,u=u
B = ∂f∂u |x=x,u=u
A =
∂ψ∂ψ
∂ψ∂v
∂ψ∂r
∂v∂ψ
∂v∂v
∂v∂r
∂r∂ψ
∂r∂v
∂r∂r
x=x
B =
∂ψ∂τr
∂v∂τr
∂r∂τr
u=u
Considerando que a unica saıda do sistema e [ψ] resulta o seguinte modelo de estados:
A =
0 0 1
0Yv+Y|v|v2|v|
mv−muumv
0 muvumr
Nr+N|r|r2|r|mr
B =
0
01
mr
C =[1 0 0
](3.5)
H(s) =−0.04062
s(s2 + 1.491s+ 0.2514)(3.6)
Este sistema tem como funcao de transferencia a funcao H(s) apresentada em (3.6). Os polos da
funcao H(s) sao [s = 0 rad/s ; s = −0.1938 rad/s ; s = −1.2974 rad/s], que como se pode verificar
coincidem com os valores proprios da matriz A. Portanto nao existe cancelamento de polos e zeros.
No entanto existe um polo nao estavel na origem, recorrendo-se assim as matrizes de controlabilidade
e observabilidade para verificar se o sistema e estabilizavel e detectavel (respectivamente).
C(A, b) =[b Ab A2b
]=
0 0 −0.0406
0 −0.0406 0.0606
0.1032 −0.0441 0.0398
O(A,C) =
C
CA
CA2
=
1 0 0
0 1 0
0 −1.0638 −0.3936
(3.7)
Analisando a equacao (3.7), verifica-se que ambas as matrizes tem caracterıstica tres, portanto, o
polo na origem e observavel e controlavel e por consequencia o sistema sera detectavel e estabilizavel.
A partir deste modelo sera desenvolvido um controlador conhecido como regulador linear quadratico.
18
3.2 Regulador Linear Quadratico
Nesta seccao serao explicados os passos utilizados para o desenvolvimento do LQR (Regulador linear
Quadratico) que efectuara o controlo de orientacao do veıculo medusa. Numa primeira fase, serao
apresentadas as equacoes genericas deste tipo de controlador. Posteriormente, essas equacoes serao
particularizadas para o modelo do veıculo obtido anteriormente.
3.2.1 Equacoes Gerais
Considerando um modelo de estados generico definido em (3.8).
x = Ax+ bu = f(x, u)
x(0) = x0 u(t) ∈ <m x(t) ∈ <n(3.8)
Definindo o funcional de custo:
J = Ψ(x(T )) +
∫ T
0
L[x(t), u(t)]dt (3.9)
O limite temporal T , representa o intervalo temporal de optimizacao, ou seja, o intervalo de tempo
em que a funcao de controlo u(t) (que se pretende determinar), esta definida.
O parametro Ψ(x(T )) representa a contribuicao do estado final para o funcional do custo. Nesta
aplicacao em particular, verifica-se que Ψ(x(T )) = 0. A funcao L[x, u] denomina-se funcao lagrangeana
e reflecte a contribuicao para o funcional do custo do que acontece durante o intervalo de optimizacao.
Sera atraves da minimizacao do funcional do custo definido anteriormente, que resultara a lei de
controlo para o sistema a considerar.
Notacao a utilizar:
Ψx(x)|x=x(T ) ={∂Ψ(x)∂x1|x=x(T )
∂Ψ(x)∂x2|x=x(T ) ... ∂Ψ(x)
∂xn|x=x(T )
}Lx(u, x) =
{∂L∂x1
∂L∂x2
... ∂L∂xn
}
fx =
∂f1∂x1
∂f1∂x2
... ∂f1∂xn
∂f2∂x1
∂f2∂x2
... ∂f2∂xn
......
......
∂fn∂x1
∂fn∂x2
... ∂fn∂xn
(3.10)
A custa da notacao (3.10), define-se, utilizando o princıpio de Pontriagyn a equacao adjunta para
o Co-estado λ, (3.11), o λ representa um multiplicador de lagrange. De forma sucinta, o problema a
resolver e conseguir maximizar L(x, u), (presente em (3.9)), sujeito ao constrangimento imposto pela
dinamica do sistema fx, (3.10).
−λ′(t) = λ′(t) fx + Lx(x, u)
λ′(T ) = Ψx(x(T ))(3.11)
19
Referindo novamente o princıpio de Pontriagyn, define-se a funcao Hamiltoniana H(λ, x, u) (3.12).
Repare-se que esta funcao permite passar de um problema de maximizacao de uma funcao (funcional
de custo) em ordem a uma funcao, para maximizacao de uma funcao relativamente a uma variavel
(u(t)) para cada instante de tempo.
H(λ, x, u) = λ′(t) f(x, u) + Lx(x, u) (3.12)
Segundo o princıpio de Pontriagyn a funcao Hamiltoniana sera maxima para o controlo optimo, a
condicao de maximo sera entao satisfeita por uma das solucoes da equacao.
∂H
∂u= 0 (3.13)
3.2.2 Projecto Regulador
Definindo o funcional de custo na forma particular:
J =1
2
∫ T
0
[x′(t)Qx(t) + u′(t)Ru(t)]dt
Q = Q′ ≥ 0 R = R′ ≥ 0
(3.14)
Os parametros Q e R definidos em (3.14) sao de extrema importancia, pois sao os parametros que
permitem ajustar a resposta do controlador. Ambos os parametros sao matrizes diagonais, Q tem
dimensao Q ∈ <n×n e cada elemento da sua diagonal penaliza cada uma das variaveis de estado. R
tem dimensao R ∈ <m×m e cada elemento da sua diagonal associa um custo a cada uma das entradas
u(t) do sistema, por esse motivo esta associado ao custo do controlo.
E de notar que para minimizar o funcional de custo sera necessario minimizar a funcao L[x, u], e
minimizar essa funcao corresponde a maximizar −L[x, u]. Desta forma considere-se:
L(x, u) = −1
2[x′Qx+ u′Ru] (3.15)
A equacao (3.15) sera a equacao a maximizar, levando a minimizacao do funcional (3.14).
Aplicando a este caso especıfico a notacao (3.10), resulta:
Ψx(x)|x=x(T ) = 0
Lx(u, x) = −x′(t)Q
fx = A
(3.16)
E portanto, recuperando a equacao adjunta (3.11) resulta:
−λ′(t) = λ′(t)A− x′Q)
λ′(T ) = 0(3.17)
Pode-se assim definir a funcao Hamitoniana:
20
H(λ, x, u) = λ′(t) f(x, u) + Lx(x, u) (3.18)
Aplicando a condicao necessaria de maximo de Hamiltoniana, (3.19).
∂H
∂u= λ′(t) b− u′(t)R = 0 (3.19)
Conclui-se assim que o controlo optimo sera dado pela expressao (3.20). No entanto λ(t) ainda nao
e conhecido, portanto a lei de controlo ainda nao esta determinada.
u(t) = R−1 b−1 λ(t) (3.20)
Substituindo u(t) nas equacoes do modelo de estados resulta:
x = Ax+ b[R−1 b−1 λ(t)] (3.21)
Pode-se agora construir um sistema de equacoes diferencias ao juntar (3.21) com (3.17), resultando
(3.22).
x = Ax+ b[R−1 b−1 λ(t)]
λ(t) = −A′ λ′(t) +Qx(t)(3.22)
Surge assim um sistema com duas equacoes diferenciais com a informacao adicional de que x(0) =
x0 e λ(T ) = 0. Admite-se agora que existe uma matriz P (t) tal que:
λ = −P (t)x(t) (3.23)
O calculo da derivada de λ(t) a partir de (3.23), resulta em:
λ = −P (t)x(t)− P (t) x(t) (3.24)
Utilizando a informacao adicional presente em (3.22), resulta, por substituicao do estado e do co-
estado:
(Q+A′P (t))x = −P (t)x(t)− P (t) (A− bR−1 b′ P )x (3.25)
Colocando x(t) em evidencia na equacao anterior, resulta:
[P (t) + P (t)A+A′P (t)− P (t) bR−1 b ′P +Q ]x(t) = 0 (3.26)
Para que a equacao (3.26) cumpra a igualdade a zero, a expressao entre parentesis tem que ser
igual a zero. Desta forma chega-se a equacao diferencial de Ricatti apresentada em (3.27).
−P (t) = P (t)A+A′P (t)− P (t) bR−1 b ′P +Q (3.27)
21
Agora, se o par A, b for estabilizavel e se C for detectavel, entao a medida que T estende (T− >∞)
a solucao da equacao de Riccati tende a ficar constante, portanto isso vai implicar que P (T ) = 0 e
desta forma chega-se a equacao algebrica de Riccati (3.28). A partir desta equacao obtem-se a matriz
P (t).
P (t)A+A′P (t)− P (t) bR−1 b ′P +Q = 0 (3.28)
Recuperando a lei de controlo (3.20), substituindo o λ(t) obtem-se a lei de controlo (3.29).
u(t) = −R−1 b−1 P (t)x(t)
u(t) = −kx(t) com : k = R−1 b−1 P(3.29)
A lei de controlo (3.29), aplicada ao modelo de estados (3.5), permite chegar ao diagrama de blocos
apresentado na figura 3.1. E de notar que este diagrama foi construıdo considerando que todas as
medidas do estados estao acessıveis. Na pratica, no veıculo real isto significa a existencia de sensores
capazes de medir todas as grandezas do vector de estados, [ψ, v, r].
Figura 3.1: Estrutura do Controlador LQR
Nesta fase, e necessario ajustar as matrizes Q e R, para que a resposta do sistema seja o mais
suave e rapida possıvel, ou seja, sem oscilacoes e com uma sobre-elevacao reduzida ou inexistente.
Para se conseguir a resposta desejada, deve-se ter em conta que R representa o custo do controlo
e aparece directamente multiplicado no calculo do vector de ganhos K dado por (3.29). Ao aumentar R
estamos na verdade a diminuir todos os ganhos do vector K. Desta forma, a matriz unitaria R deve ser
ajustada a partir da observacao da saıda de controlo assinalada na figura 3.1 como Tr, de forma a que
este sinal nao apresente picos muito elevados (para uma velocidade linear u = 0.5m/s considerou-se
Tr < 1Nm o maximo). No caso da matriz Q, como ja foi explicado, esta representa o peso de cada
estado, os seus valores sao ajustados por observacao da saıda do sistema que representa a variavel
de interesse, neste caso essa variavel e a assinalada em 3.1 como Y aw. O raciocınio para ajustar esta
matriz tem em conta essencialmente tres princıpios simples:
22
• Quanto menor a penalizacao de um dado estado mais rapido este evoluira,
• Se um dado estado apresenta picos indesejados (sobre-elevacao), deve-se penalizar o estado
que e a sua derivada temporal,
• A penalizacao de cada um dos estados e relativa a penalizacao dos restantes, (i.e. penalizar
todos os estados com um peso 1, ou penalizar todos os estados com um peso 100 e equivalente).
Utilizando os princıpios apresentados para as escolhas deQ eR, chega-se aos valores das matrizes
Q e R apresentados em (3.30). Desta forma, resulta o vector de ganhosK apresentado em (3.31) (apos
os calculos apresentados na fase de projecto do regulador). Ao simular o sistema apresentado na figura
3.1 com o vector K (3.31) resulta a resposta do sistema presente na figura 3.2.
Q =
30 0 0
0 1 0
0 0 35
R =[0.01
](3.30)
K =
54.7723
50.5395
58.3314
(3.31)
(a) Sinal de controlo Tr (b) Resposta do controlador LQR em comparacao com osinal de referencia
Figura 3.2: LQR Simulation Results
Nesta fase, verificou-se por simulacao que a variacao da velocidade v pouco influenciava a resposta
do controlador. Desta forma, pode-se pensar no estado v como se de um estado desacopolado do
sistema se tratasse, mas e de notar, que esta aproximacao apenas se verificou para o caso em que a
variavel de interesse e ψ.
Na figura 3.3 apresenta-se uma comparacao da resposta do controlador consoante a velocidade v do
veıculo, a partir dessa mesma figura e possıvel visualizar que as curvas referentes a diferentes valores
de v sao indistinguıveis. Este resultado comprova que a velocidade v pouco influencia a resposta do
controlador de orientacao.
23
Figura 3.3: Resposta do Controlador LQR com Variacao de v
3.2.3 Modelo de estados Simplificado
Uma vez que se verificou que a variavel de estado v se encontra desacoplada das restantes variaveis
de estado [ψ, r] no feedback de controlo, considera-se pertinente reduzir o modelo de estados simples-
mente as variaveis de estado [ψ, r].
Resulta o modelo de estados (3.32).
A =
0 1
0Nr+N|r|r2|r|
mr
B =
01
mr
C =[1 0
](3.32)
O diagrama de bode do sistema pode ser visualizado na figura 3.4.
Figura 3.4: Diagrama de Bode Modelo de Estados Simplificado
G(s) =0.1032
s(s+ 0.4272)(3.33)
24
Este novo sistema apresenta a funcao de transferencia G(s) apresentada em (3.33). Como se pode
verificar, os polos deste sistema serao [s = 0 s = −0.4272], que coincidem com os valores proprios
da matriz A, portanto nao existe cancelamento de polos com zeros. No entanto, existe um polo instavel
(s = 0), sendo assim necessario verificar se este polo e controlavel e observavel, para se garantir que
o sistema e detectavel e estabilizavel.
C(A, b) =[b Ab
]=
0 0.1032
0.1032 −0.0441
O(A,C) =
C
AC
=
1 0
0 1
(3.34)
A partir das matrizes de controlabilidade e observabilidade apresentadas em (3.34), e possıvel con-
cluir que todos os polos sao observaveis e controlaveis. Assim, o polo instavel tambem o sera, e
portanto o sistema e detectavel e estabilizavel. Nestas condicoes pode ser projectado um LQR pelo
metodo descrito anteriormente.
Seguindo o raciocınio apresentado na subseccao 3.2.2, resultam as matrizes Q e R em (3.35), que
dao origem ao vector de ganhos K em (3.36).
Q =
25 0
0 10
R =[0.01
](3.35)
K =
50.0000
40.4261
(3.36)
Resulta assim o novo modelo de estados deste novo controlador LQR, figura 3.5.
Figura 3.5: Controlador LQR simplificado
Da simulacao do diagrama de blocos da figura 3.5 com o vector de ganhos K apresentado na
equacao (3.36), surgem os resultados apresentados na figura 3.6.
Nesta fase do projecto, e necessario verificar o comportamento do sistema realimentado relativa-
mente a perturbacoes externas. Considerando que apenas podem existir perturbacoes a saıda do
controlador ou a saıda do processo, resultam as perturbacoes assinaladas na figura 3.7, a verde e a
azul, respectivamente.
25
(a) Sinal de controlo Tr (b) Resposta do controlador LQR em comparacao com osinal de referencia
Figura 3.6: LQR Simplificado Simulation Results
Figura 3.7: Perturbacoes LQR simplificado
As funcoes de transferencia de cada uma das perturbacoes para a saıda podem ser obtidas por
observacao do diagrama de blocos da figura 3.7, resultando assim as funcoes de transferencia, (3.37)
e (3.38) respectivamente.
Y aw(s)
d(s)=
bA12
s(s−A22+bK1)
1 + K0bA12
s(s−A22+bK1)
=bA12
s(s−A22 + bK1) + bK0A12(3.37)
Y aw(s)
d1(s)=
1
1 + A12bK0
s(s−A22+bK1)
=s(s−A22 + bK1)
s(s−A22 + bK1) +A12bK0(3.38)
Aplicando o Teorema do Valor Final (equacao (3.39)) e possıvel prever o comportamento temporal
do sistema quando sujeito a cada uma das perturbacoes.
x(t→∞) = limx→∞
x(t) = limS→0
s x(s) (3.39)
Considerando que as perturbacoes sao constantes, a sua transformada de Laplace surge como:
d(s) = d1(s) = Ks , pode-se considerar que K = 1 e portanto, por aplicacao do Teorema do Valor Final
resulta (3.40) e (3.41) respectivamente.
26
Y awd(t→∞) = limS→0
sY aw(s)
d(s)d(s) = lim
S→0s
Y aw(s)
d(s)
1
s= limS→0
Y aw(s)
d(s)=
1
K0(3.40)
Y awd1(t→∞) = limS→0
sY aw(s)
d1(s)d1(s) = lim
S→0s
Y aw(s)
d1(s)
1
s= limS→0
Y aw(s)
d1(s)= 0 (3.41)
Conclui-se assim que o sistema e capaz de rejeitar perturbacoes constantes do tipo d1 (assinalado
na figura 3.7 a azul), mas no entanto, nao e capaz de rejeitar as perturbacoes do tipo d (assinalado na
figura 3.7 a verde).
E de todo o interesse que o sistema consiga rejeitar perturbacoes do tipo d, uma vez que numa
realizacao do sistema real, e comum existir um erro relativamente a ordem de controlo que e dada e
aquela que efectivamente o sistema executou. Este fenomeno pode acontecer por diversos motivos, no
caso concreto do veıculo medusa este erro esta fortemente associado a uma diferenca de resposta dos
dois motores que equipam o veıculo. Uma vez que e a diferenca das forcas geradas por cada motor que
gera o binario responsavel pelo controlo de orientacao do veıculo, uma diferenca de resposta de um
motor em relacao ao outro, vai gerar uma perturbacao no binario. Este tipo de perturbacao e equivalente
a perturbacao denominada de d (assinalado na figura 3.7 a verde).
3.2.4 Inclusao de Efeito Integral
Quando se pretende rejeitar uma perturbacao constante existe uma estrategia simples, que consiste
em colocar um integrador puro imediatamente antes da perturbacao. Este integrador vai criar o cha-
mado efeito integral, que se traduz basicamente numa capacidade do sistema em rejeitar perturbacoes
constantes (pode ser demonstrado com o Teorema do Valor Final).
Uma vez que se pretende ter a capacidade de rejeitar perturbacoes do tipo d (assinalado na figura
3.7 a verde) incluir-se-a um integrado puro antes da perturbacao.
Comece-se por considerar um novo vector de estado composto por [ψ, r, E] em que este novo estado
E pode ser definido como em (3.42).
E =
∫ t
0
ψ − ψref dt (3.42)
Desta forma, o novo modelo de estados pode ser definido como:ψ
r
E
=
0 1 0
0Nr+N|r|r2|r|
mr0
1 0 0
ψ
r
E
+
0
0.1032
0
u+
0
0
−1
ψrefcom : u = −(K0ψ +K1r +K2E)
A =
0 1 0
0Nr+N|r|r2|r|
mr0
1 0 0
b =
0
0.1032
0
(3.43)
27
Os K ′s assinalados em (3.43), sao os valores do vector K que sera novamente determinado pelo
projecto de um novo regulador linear quadratico para o sistema apresentado em (3.43). Dessa forma,
utilizando as matrizes Q e R (3.44) obtidas pelo processo apresentado na subseccao 3.2.2, resulta o
vector de ganhos K (3.45).
Q =
25 0 0
0 10 0
0 0 40
R =[0.01
](3.44)
K =
96.4094
49.5773
63.2456
(3.45)
O modelo de blocos relativo ao sistema com o novo controlador LQR pode ser visualizado na figura
3.8. Por simulacao deste sistema em Matlab Simulink, resulta a resposta que pode ser visualizada na
figura 3.9.
Figura 3.8: LQR com efeito integral
(a) Sinal de controlo Tr (b) Resposta do controlador LQR em comparacao com osinal de referencia
Figura 3.9: LQR com efeito integral Simulation Results
Devido a inclusao do integrador, este novo sistema tera a capacidade de rejeitar perturbacoes
28
quando estas acontecem a saıda do controlador (assinalado a verde na figura (3.45)). Aplicando o
Teorema do Valor Final pode-se provar essa propriedade.
O integrador adicional pode originar efeito Wind-Up normalmente associado aos controladores PID
(Proporcional Integral e Derivativo). Assim, e necessario complementar o diagrama da figura 3.8 com
uma estrutura Anti Wind-Up. Este procedimento e claramente facilitado caso se consiga ter o integrador
directamente a saıda do controlador, pois nesse caso, a saıda do integrador coincide com a saıda do
controlador e dessa forma uma simples saturacao a saıda do controlador funcionara como sistema anti
Wind-Up. Na realidade, esta implementacao traz um conjunto de vantagens que ja foram exploradas
em [11]. Em particular, ao utilizar a estrutura com o integrador como saıda do controlador existe uma
garantia de que os valores proprios do conjunto, sistema linearizado realimentado com regulador linear
sao iguais aos valores proprios da linearizacao do conjunto sistema nao linear realimentado com regu-
lador linear, e a mesma propriedade e valida para as funcoes de transferencia dos dois conjuntos (ver
[11] seccao 4).
Esta propriedade garante que vai existir uma equivalencia em termos de comportamento do sistema
nao linear realimentado com o controlador linear (configuracao a utilizar na realidade) relativamente
ao sistema linearizado realimentado com o controlador linear (configuracao da fase de projecto). O
raciocınio por detras desta conclusao advem do facto de que a linearizacao do conjunto sistema nao li-
near realimentado com o regulador linear vai replicar o comportamento do conjunto para um dado ponto
de equilıbrio. Se os valores proprios e funcao de transferencia deste conjunto linearizado sao iguais ao
do conjunto sistema linearizado realimentado com regulador linear (para o mesmo ponto de equilıbrio),
entao, existe uma equivalencia entre as suas respostas temporais. Portanto o conjunto sistema line-
arizado realimentado com regulador linear replica a resposta do sistema nao linear realimentado com
regulador linear.
Conhecidas as vantagens de trazer o integrador para a saıda do controlador e uma vez que o sistema
e linear, essa manipulacao resume-se a algebra de blocos, resultando assim o diagrama apresentado
na figura 3.10.
Figura 3.10: LQR integrador a saıda do controlador
Repare-se que este novo diagrama de blocos (figura 3.10) e equivalente ao diagrama de blocos
da figura 3.8 e portanto, as repostas dos dois sistemas sao iguais, por esse motivo nao e necessario
29
apresentar resultados de simulacao para o novo diagrama de blocos (figura 3.10).
Nesta fase, bastaria ter uma saturacao a saıda do controlador para resolver os problemas de Wind-
Up que o integrador adicional possa causar. Porem serao necessarias mais saturacoes capazes de
evitar algum tipo de sobrecarga ao nıvel dos motores do veıculo. Assim sera de todo o interesse tratar
da saturacao directamente ao nıvel dos motores (ver subseccao 3.3). Como tal, antes mesmo de se
aplicar algum tipo de saturacao ha que generalizar este controlador para outros pontos de operacao,
diferentes do ponto de equilıbrio inicialmente considerado [u = 0.5m/s ; v = 0m/s ; r = 0rad/s].
3.3 Gain Scheduling
O Gain Scheduling (escalonamento de ganhos), consiste em fazer variar os ganhos de um controlador
consoante uma variavel (variavel de Scheduling). Recorre-se assim a um conjunto de controladores
que cobrem, de um modo geral, toda a zona de operacao que se pretende para o controlador geral. Os
ganhos do controlador geral resultam da interpolacao dos diferentes controladores projectados para os
diferentes pontos de operacao.
No presente caso em estudo, a variavel de escalonamento sera a velocidade linear u do veıculo.
No entanto, o modelo de estados simplificado (concatenado em v) nao contem qualquer informacao
relativamente a u. Recuperando a matriz A da equacao (3.32):
A =
0 1
0Nr+N|r|r2|r|
mr
E um facto que v pouca influencia tem na variacao da posicao angular ψ, mas a velocidade u tem
uma influencia consideravel que nao esta reflectida neste modelo. Uma vez que nao se tem acesso
a medida de v do veıculo, nao se pode considerar voltar a fazer realimentacao com esta variavel de
estado para poder recuperar a informacao de u. Assim, optou-se por aplicar a lei de controlo projectada
com este modelo e atraves de sucessivas simulacoes directamente no sistema nao linear, ajustar os
parametros Q e R por observacao da resposta do sistema nao linear (pelo metodo ja descrito). Assim,
apesar de o modelo de estados linearizado ser sempre igual, independentemente do valor de u, a
resposta do modelo nao linear do veıculo medusa vai variar, traduzindo o comportamento do veıculo
real.
Desta forma considerou-se um conjunto de pontos de operacao para o veıculo com u de 0.1m/s ate
proximo da velocidade maxima anunciada para o veıculo medusa de u = 1.5m/s, resultando a tabela
de ganhos 3.1. Os resultados de simulacao de todos estes pontos de operacao apresentados na tabela
3.1 e que suportam os valores apresentados na mesma encontram-se em anexo no Apendice 1.
Partindo agora do conjunto de controladores apresentados na tabela 3.1, pode-se fazer interpolacao
linear para determinar os ganhos do controlador de orientacao para qualquer valor de u (dentro dos
limites fısicos do veıculo). Desta forma apresenta-se na figura 3.11 uma estrutura capaz de fazer a
interpolacao dos ganhos do controlador consoante a velocidade u do veıculo.
E de salientar que o bloco assinalado como ”Dynamics1” e o modelo nao linear do veıculo medusa
obtido anteriormente na seccao 2.3, considerando que as entradas do sistema sao τu e τr. A simulacao
30
Variavel de Escalonamento u[m/s] Matriz Q Matriz R Vector de Ganhos K
0.1
25 0 0
0 10 0
0 0 60
[0.01]
[105.18 51.14 77.46
]
0.3
25 0 0
0 10 0
0 0 70
[0.01]
[108.89 51.78 83.67
]
0.5
25 0 0
0 10 0
0 0 90
[0.01]
[115.43 52.90 94.86
]
0.7
25 0 0
0 10 0
0 0 100
[0.01]
[116.92 53.16 97.47
]
1.0
25 0 0
0 10 0
0 0 250
[0.01]
[149.24 58.39 158.11
]
1.2
25 0 0
0 10 0
0 0 500
[0.01]
[180.40 63.04 223.61
]
1.4
25 0 0
0 10 0
0 0 900
[0.01]
[213.49 67.66 300.00
]
Tabela 3.1: Parametros controladores para Escalonamento de Ganhos
do diagrama apresentado na figura 3.11, quando se faz variar u ao longo do seu espectro de operacao,
permite comprovar o bom funcionamento da estrutura de escalonamento de ganhos. Para fazer u variar
ao longo do seu espectro considerou-se que o sinal Tu seria do tipo rampa. Obtem-se desta forma os
resultados apresentados na figura 3.12.
31
Figura 3.11: Estrutura de Escalonamento de Ganhos
(a) Evolucao da Velocidade u (b) Sinal de controlo Tr
(c) Resposta no tempo em comparacao com referencia
Figura 3.12: LQR Simplificado Simulation Results
32
3.3.1 Saturacoes e Anti Wind-Up
A estrutura final do controlador de orientacao devera contar com algum tipo de estrutura Anti Wind-Up
(como ja foi referido) e devera ter uma saturacao em cada motor com o objectivo de proteger os motores
do veıculo de eventuais sobrecargas.
Recuperando a equacao (2.12), e reescrevendo-a noutra forma resulta, (3.46):
τu = (Fest + Fbb)
τr = l(Fest − Fbb)⇔Fest = τu + τr
2l
Fbb = τu − τr2l
(3.46)
A utilizacao das forcas geradas pelos motores, Fest e Fbb, como sinais de saıda do controlo permite
aplicar as saturacoes directamente a entrada dos motores. Estes sinais representam tambem as gran-
dezas que efectivamente cada um dos motores vai gerar, pelo que em alguma fase esta conversao teria
de ser feita.
Propoe-se a estrutura apresentada na figura 3.13, que funcionara simultaneamente como proteccao
dos motores e como estrutura Anti Wind-Up.
Figura 3.13: Saturacao ao nıvel dos motores
Este diagrama de blocos tem como entrada os sinais τu e τr e saıda os sinais Fest e Fbb dentro dos
limites fısicos dos motores.
O primeiro passo e saturar o modo comum, tendo em conta que a velocidade maxima anunciada
para o veıculo medusa e de 1.5m/s, considerou-se assim que a forca maxima que os motores podem
disponibilizar (em modo comum) e a forca necessaria para levar o veıculo a essa velocidade. Pelas
equacoes da dinamica do veıculo (3.1) resulta τusat = ±56.55N .
O segundo passo sera saturar o modo diferencial. A metade esquerda do diagrama de blocos 3.13
(ate aos primeiros blocos ”Motor Sat”) converte os sinais de modo comum e modo diferencial, τu e τr,
respectivamente, nos sinais a serem gerados por cada um dos motores do veıculo, Fest e Fbb (segundo
(3.46)). Feita a conversao e aplicada uma saturacao a Fest e Fbb, por ”Motor est sat” e ”Motor bb
sat”, respectivamente, de valor 12τusat . O diagrama de blocos poderia acabar por aqui, uma vez que os
33
motores estariam protegidos e o efeito de Wind-Up ja nao se faria sentir, no entanto, ao utilizar essa
configuracao, quando o veıculo estivesse proximo dos seu limite de velocidade e se tentasse efectuar
uma mudanca de direccao este nao seria capaz de a fazer correctamente. As saturacoes ao nıvel
dos motores anulariam parte do modo diferencial exigido para que o veıculo efectuasse a mudanca de
direccao. Uma forma de contrariar este problema e fazer com que o veıculo diminua a sua velocidade
sempre que ja nao tem margem de operacao num dos motores para poder executar o torque exigido
pelo modo diferencial para efectuar uma determinada mudanca de direccao. E precisamente isto que
faz a metade esquerda do diagrama apresentado na figura 3.13. Os blocos ”Add4” e ”Add5” da figura
3.13, verificam se alguma das saturacoes, ”Motor est sat” ou ”Motor bb sat” cortou parte do sinal, se
isso aconteceu, ha que retirar a mesma ”porcao de sinal” ao outro motor de forma a manter o torque (
τr) exigido pelo modo diferencial. E precisamente isso que fazem os blocos ”Add2” e ”Add3” da figura
3.13. As ultimas duas saturacoes presentes no diagrama da figura 3.13 (”Motor est sat 1” e ”Motor
bb sat 1”), garantem que numa situacao em que o modo diferencial exija um torque bastante elevado,
a ”porcao de sinal” que e retirada ao motor contrario nao coloca um dos sinais Fest ou Fbb, fora dos
limites fısicos do veıculo.
O projecto do controlador de orientacao fica completo com a juncao do modulo de controlo apre-
sentado na figura 3.11 com o modulo que efectua a saturacao do sinal apresentado em 3.13. Resulta
assim o diagrama de blocos da figura 3.14.
Figura 3.14: Diagrama de blocos completo do controlo de Orientacao
Na estrutura apresentada na figura 3.14, o bloco ”Dynamics1” representa o modelo nao linear do
veıculo apresentado em (3.1), com a pequena mudanca de variavel apresentada em (3.46), para que
as entradas do bloco sejam Fest e Fbb.
O bloco a azul, na figura 3.14, representa a estrutura de controlo de orientacao previamente apre-
sentada em 3.11. A estrutura apenas foi re-arranjada, apresentando-se agora com o formato da figura
3.15 .
Por ultimo, o bloco a verde representa directamente o bloco que faz a saturacao dos sinais Fest e
Fbb, apresentado anteriormente em 3.13.
Para comprovar o bom funcionamento e robustez de toda a estrutura apresentada deve-se efectuar
uma simulacao com o veıculo proximo da velocidade maxima e ”pedindo”uma mudanca de direccao com
34
Figura 3.15: Bloco de Controlo de Orientacao
um angulo elevado. Desse modo existira com certeza necessidade de activar os blocos de saturacao.
Da simulacao via Matlab Simulink da estrutura apresentada na figura 3.14, com τusat=56.55N (para que
o veıculo atinja a velocidade maxima de u = 1.5m/s) resultam as respostas registadas na figura 3.16.
No grafico (a) presente na figura 3.16 e visıvel uma diminuicao da velocidade u do veıculo, precisa-
mente nas zonas em que os sinais Fest e Fbb fazem activar os blocos de saturacao. Nos graficos (c) e
(d) e possıvel comparar os sinais Fest e Fbb, antes e apos passarem pelo bloco de saturacao. Como se
pode verificar, qualitativamente, quando um dos sinais e truncado, o outro sinal diminui a sua amplitude
na mesma medida em que o outro foi afectado. Como resultado o veıculo e capaz de efectuar uma
mudanca de direccao brusca de 1rad mesmo quando se encontra proximo da sua velocidade maxima.
E certo que a resposta apresenta picos indesejaveis, mas estas condicoes de operacao encontram-
se no limite fısico do veıculo. Em condicoes normais de operacao tal esforco nao sera solicitado ao
veıculo.
35
(a) Evolucao da Velocidade u (b) Resposta no tempo em comparacao com referencia
(c) Sinais de controlo Fest e Fbb antes de passarem pelobloco de saturacao
(d) Sinais de controlo Fest e Fbb apos passagem pelo blocode saturacao
Figura 3.16: Resultados simulacao do Controlador de Orientacao (teste a saturacao)
36
Capıtulo 4
Controlador de Seguimento de
Caminho
Neste capitulo apresentam-se diferentes abordagens para o problema de controlo de seguimento de ca-
minho (Path Following) tendo em conta apenas a cinematica do veıculo a controlar. Sera proposto um
algoritmo de seguimento de caminho seguindo uma das abordagens que serao introduzidas. Posterior-
mente, este novo controlador fara uso do controlador de mais baixo nıvel (Controlador de Orientacao)
previamente desenvolvido no capıtulo 3. O controlo de velocidade nao sera efectuado, pois o veıculo
vai operar num ambiente hostil, sujeito a correntes, o que vai originar constantes perturbacoes a ve-
locidade do veıculo. Uma estrategia de controlo que tente superar todas estas perturbacoes vai exigir
um grande esforco por parte dos motores do veıculo. Esse esforco sera variavel e apresentara picos
consoante as correntes, picos esses que desgastam os componentes (como os motores) e reduzem
o tempo de operacao das baterias. Tendo em conta que manter uma velocidade exacta com elevado
grau de precisao nao e uma necessidade, o controlo de velocidade sera deixado em cadeia aberta. Por
outras palavras, o controlo de velocidade sera simplesmente um conjunto de referencias a dar aos mo-
tores, das quais se conhece a velocidade aproximada que resulta quando aplicadas cada uma dessas
referencias. Um estudo desta natureza ja foi efectuado anteriormente em [7], pelo que o controlador de
velocidade sera tomado como garantido.
Serao os controladores baixo nıvel (orientacao e velocidade) que entrarao em linha de conta com a
dinamica do veıculo a controlar (veıculo Medusa). O controlador de seguimento de caminho proposto
apenas gerara as referencias de ψ (yaw) a serem seguidas pelo controlador de orientacao previamente
projectado.
4.1 Estrategias para seguimento de caminho
Nesta seccao serao apresentadas diferentes estrategias para seguimento de caminho tendo em conta
apenas a cinematica do veıculo a controlar. Sera feita uma comparacao entre os diferentes metodos.
37
4.1.1 Metodo line of sight segundo Fotis A. Papoulias
O nome desta seccao esta directamente relacionado com o autor do metodo de controlo apresentado
em [12]. Na seccao 2 deste artigo, o autor (Fotis A. Papoulias) propoe um algoritmo de seguimento de
caminho a duas dimensoes, baseado na cinematica do veıculo a controlar.
Considere-se entao que a trajectoria a ser seguida e apenas uma circunferencia com um certo raio
R, o veıculo encontra-se num certo ponto arbitrario e quer-se que o veıculo convirga para a trajectoria
e se mantenha nesta. Resulta assim uma configuracao semelhante como a da figura 4.1.
Figura 4.1: Configuracao segundo algoritmo proposto por Fotis Papoulias
O veıculo encontra-se numa posicao arbitraria no espaco em que e possıvel determinar o ponto
mais proximo sobre a trajectoria (ponto P1). A partir do ponto P1, e medido o comprimento d sobre
a trajectoria (denominada distancia de visibilidade) obtendo-se o ponto P2. O ponto P2 representa o
ponto que se pretende que o veıculo ocupe futuramente. Ou seja, pretende-se levar o veıculo para P2, e
portanto levar o erro lateral y′ para zero. Para tal, deve-se apontar o mesmo segundo o angulo formado
entre a linha que liga P2 ao centro de massa do veıculo e a recta horizontal (corresponde ao angulo
assinalado na figura 4.1 como σ). Um algoritmo de seguimento de caminho e entao definido uma vez
determinado o angulo σ, sendo a lei de controlo simplesmente ψ = π2 +σ. E necessario acrescentar π/2
ao angulo σ uma vez que o angulo σ e medido relativamente a horizontal, que se encontra desfasada
de π/2 rad do angulo de yaw zero.
Considerando que o veıculo se encontra no ponto generico (x, y), resulta, por analise da figura 4.1:
tan(σ) =x− xP2
y − yP2=x−R sin(β)
y −R cos(β)(4.1)
Sendo β definido como:
β = arctan(xy
)− d
R(4.2)
38
Substituindo β em (4.1) resulta (quando aplicadas as transformacoes apresentadas em (4.4)):
tan(σ) =x√x2 + y2 −R
[x cos
(dR
)− y sin
(dR
)]y√x2 + y2 −R
[y cos
(dR
)+ x sin
(dR
)] (4.3)
tan(σ) =x−R sin
(arctan
(xy
)− d
R
)y −R cos
(arctan
(xy
)− d
R
) ⇔
tan(σ) =
x−R[sin(arctan
(xy
))cos(dR
)− sin
(dR
)cos(arctan
(xy
))]y −R
[cos(arctan
(xy
))cos(dR
)+ sin
(dR
)sin(arctan
(xy
))] ⇔
tan(σ) =
x−R
[xy√
1+(xy
)2 cos( dR)− sin( dR) 1√1+(xy
)2]
y −R
[1√
1+(xy
)2 cos( dR)+ sin(dR
) xy√
1+(xy
)2] ⇔
tan(σ) =x√x2 + y2 −R
[x cos
(dR
)− y sin
(dR
)]y√x2 + y2 −R
[y cos
(dR
)+ x sin
(dR
)]
(4.4)
Definido o algoritmo de seguimento, e necessario verificar de que forma evolui o erro lateral, y′
(Cross-Tracking Error). Fotis A. Papoulias comeca por verificar a evolucao do erro em regime perma-
nente, numa situacao em que o veıculo ja se encontra a seguir a trajectoria e so depois verifica a
evolucao do erro, considerando que o veıculo se encontra num ponto arbitrario e ainda nao convergiu
para a trajectoria.
Erro em regime permanente
Estando o veıculo a seguir uma dada trajectoria, qual sera o erro lateral y′ expectavel com a utilizacao
deste algoritmo? Para responder a essa pergunta considere-se que a posicao do veıculo evolui segundo
(4.5).
x = (R+ y′0) cos(wt)
y = (R+ y′0) sin(wt), w =
u
R+ y′0(4.5)
A cada instante o yaw do veıculo, ψ0, sera:
ψ0 = w t+ γ0 (4.6)
Sabendo que a cinematica do veıculo e dada por (considerando que nao existe velocidade v):
x = u cos(ψ)
y = u sin(ψ)(4.7)
39
Desta forma, diferenciando, (4.5) e utilizando as equacoes da cinematica, (4.7), resulta:
u cos(wt+ γ0) = −(R+ y′0) sin(wt) w
u sin(wt+ γ0) = (R+ y′0) cos(wt) w(4.8)
De (4.8) conclui-se que a condicao inicial γ0, sera dada por γ0 = π/2 ou qualquer multiplo de 2π desse
angulo.
Recorre-se agora a equacao (4.3), onde se substituira (4.6), resultando assim:
tan(ψ0 − π/2) =(R+ y′0) cos(wt) (R+ y′0)−R
[(R+ y′0) cos(wt) cos
(dR
)− (R+ y′0) sin(wt) sin
(dR
)](R+ y′0) sin(wt) (R+ y′0)−R
[(R+ y′0) sin(wt) cos
(dR
)+ (R+ y′0) cos(wt) sin
(dR
)] ⇔tan(w t+ γ0 − π/2) =
(R+ y′0) cos(wt) −R[cos(wt) cos
(dR
)− sin(wt) sin
(dR
)](R+ y′0) sin(wt)−R
[sin(wt) cos
(dR
)+ cos(wt) sin
(dR
)] ⇔tan(w t+ γ0 − π/2) =
(R+ y′0)−R[cos(dR
)− tan(wt) sin
(dR
)](R+ y′0) tan(wt)−R
[tan(wt) cos
(dR
)+ sin
(dR
)] ⇒tan(γ0 − π/2) =
R+ y′0 −R cos(dR
)−R sin
(dR
) ⇔
tan(γ0) =R sin
(dR
)R+ y′0 −R cos
(dR
)(4.9)
Uma vez que ja se sabe γ0 = π/2 entao resulta que:
y′0 = R[cos( dR
)− 1]
(4.10)
Pode-se assim verificar que quanto menor a relacao dR menor sera o erro cometido com este metodo
de seguimento de caminho. Por outras palavras, quanto maior o raio R e menor a distancia de visibili-
dade d menor sera o erro cometido.
Evolucao do erro lateral (Cross-Tracking error)
Pode-se agora introduzir um conjunto de coordenadas locais, para levar o veıculo para uma situacao
em que ainda nao se encontra a seguir a trajectoria e a partir daı, verificar como se da a evolucao do
erro lateral. Para isso considere-se:
ψ1 = ψ − ψ0 = γ − γ0
y′1 = y′ − y′0 =√x2 + y2 −R− y′0
(4.11)
Neste novo conjunto de coordenadas locais, ψ1 representa no fundo ”a porcao” do comando de Yaw
(alem de wt) que e responsavel por anular o erro lateral e y′1 representa o erro lateral do veıculo sem
contar com o erro que tera quando convergir para o regime permanente. A evolucao do erro lateral ate
40
que o veıculo convirja para o regime permanente (caso convirja) sera entao dada por:
y′1 = u sin(ψ1) (4.12)
Atraves de (4.9), e possıvel definir ψ1, resultando entao:
tan(ψ1) = tan(γ + π/2) = − 1
tan(γ)' − y′1
R sin(dR
) (4.13)
Facilmente se conclui que a evolucao do erro lateral y′1 pode ser definida a custa de (4.12) e (4.13),
resultando:
y′1 = −u y′1√y′1
2 +R2 sin2(dR
) (4.14)
Definindo a funcao de Lyapunov:
V =1
2y′1
2(4.15)
Conclui-se rapidamente que:
V = y′1 y′1 = −u y′1
2√y′1
2 +R2 sin2(dR
) < 0 (4.16)
Logo, pela segunda lei de Lyapunov, pode-se concluir que o ponto y′1 = 0 e ponto de equilıbrio
assimptoticamente estavel. Por outras palavras, significa que y′1 → 0 independentemente do valor em
que y′1 seja iniciado.
4.1.2 Metodo segundo Pramod Maurya
Em [13], e proposto o algoritmo de seguimento de caminho que sera apresentado nesta seccao.
P.Maurya, A.Aguiar e A.Pascoal, propoem um algoritmo de seguimento de caminho projectado directa-
mente sobre o erro lateral (Cross Tracking). Este algoritmo tem a vantagem de considerar logo a partida
a presenca de correntes.
Na figura 4.2 apresenta-se um cenario em que a trajectoria a ser seguida e simplesmente uma
recta, Vw representa a velocidade do veıculo em relacao ao fluido, expressa em {B}, Vc representa a
velocidade do fluido em relacao ao referencial fixo {U} e V representa a velocidade inercial resultante
de {B} em relacao a {U}. A custa de Vw define-se o Angulo de Side Slip β.
A variacao de posicao do veıculo expressa em (U) e entao dada por:
P = R(ψ)Vw + Vc (4.17)
Sendo R(ψ) uma matriz de rotacao que ”leva” o referencial (B) a ficar alinhado com (U). Ao utilizar
o angulo β na matriz de rotacao, pode-se considerar que Vw esta alinhado com a direccao longitudinal
41
Figura 4.2: Configuracao algoritmo proposto por Pramod Maurya
do veıculo, resultando:
P = R(ψ + β)[||Vw|| 0]T + Vc (4.18)
Portanto a evolucao do erro lateral fica definida como:
y′ = sin(ψ + β) ||Vw||+ Vcy (4.19)
Por enquanto, considere-se que o angulo de side slip (β) e Vcy sao ambos zero. A unica forma de
levar o erro y′ para zero e atraves do angulo de yaw (ψ). Por simplicidade, considere-se g = sin(ψ),
desta forma g passa a ser uma grandeza que permite levar o erro para zero, resultando assim:
y′ = g ||Vw|| (4.20)
Determinar uma expressao de g que leve o erro para zero e na verdade bastante simples. Um
controlador proporcional devera cumprir o proposito:
g = − K1
||Vw||y′ (4.21)
Para lidar com valores de Vcy diferentes de zero, pode-se considerar que Vcy e uma perturbacao.
Portanto, a presenca de um integrador antes desta perturbacao gerara o chamado efeito integral (ja
anteriormente utilizado na seccao 3.2.4). Resultando:
g = − 1
||Vw||
(K1y
′ +K2
∫ t
0
y′dt)
(4.22)
Portanto, substituindo na equacao da evolucao do erro (4.19), resulta:
y′ +K1y′ +K2
∫ t
0
y′dt = 0 (4.23)
42
Utilizando a parametrizacao:
ε =
∫ t
0
y′dt = 0 (4.24)
Resulta um sistema de segunda ordem:
ε+K1ε+K2ε = 0 (4.25)
A resposta temporal dos sistemas de segunda ordem e um caso de estudo bem conhecido e docu-
mentado. Assim, e possıvel utilizar esses conhecimentos para ajustar os ganhos K1 e K2 de acordo
com o factor de amortecimento e frequencia natural que se queira para a resposta do controlador.
Tendo um controlador que gera o sinal g, e preciso converter isso para um comando de yaw.
ψ = arcsin(g) = arcsin
[Sat
(− 1
||Vw||
[K1y
′ +K2
∫ t
0
y′dt])]
(4.26)
Sendo Sat(.) uma saturacao que limita o sinal g uma vez que o sin e limitado.
Sat(x) =
x if |x| < |es|
es if x ≥ es−es if x ≤ es
com es < 1
(4.27)
E de notar que ate agora nao foi levantada a simplificacao do angulo de Side Slip (β) ser zero. Na
verdade, se o angulo de Side Slip for diferente de zero e nao for retirado do angulo de yaw (ψ) estamos
na verdade a corrigir a presenca de correntes pois o angulo que o veıculo ira adoptar sera a soma do
angulo de yaw desejado, com um angulo que se destina apenas a anular a componente da corrente
lateral ao veıculo.
Desta forma fica definido o algoritmo de seguimento de caminho. E de notar que da forma como ele
esta definido permite o seguimento de trajectorias de qualquer forma, apesar de o exemplo explicativo
utilizado ter sido apenas uma recta.
4.1.3 Metodo segundo Alain Micaelli e Claude Samson
Nesta seccao aborda-se um algoritmo de seguimento de caminho proposto por Alain Micaelli e Claude
Samson em [14]. Este algoritmo e definido para qualquer trajectoria generica, desde que esta cum-
pra um conjunto de pre requisitos que serao abordados adiante. Considere-se entao a configuracao
apresentada na figura 4.3.
Este algoritmo e ”construido” a custa de um referencial de Serret-Frenet, (este corresponde ao re-
ferencial (F (s)) da figura 4.3). Para melhor compreensao deste tipo de referencial, ver [15]. O centro
deste referencial encontra-se no ponto sobre a trajectoria que se encontra mais proximo do veıculo,
esse ponto e parametrizado por s, que corresponde a distancia (positiva ou negativa) medida sobre
o caminho desde a origem (inıcio) do mesmo. No caso em que o veıculo esteja sempre sobre a tra-
43
Figura 4.3: Configuracao algoritmo proposto por Claude Samson
jectoria corresponde exactamente a distancia que andou. Agora, as coordenadas do veıculo podem ser
expressas neste novo referencial. Comece-se por considerar que a posicao de (B) expressa em (U)
e dada por o par de coordenadas cartesianas (X;Y ). Este ponto pode ser expresso no referencial de
Serret-Frenet,(F (s)), a custa do par de variaveis (s, y′).
Considere-se agora a matriz de rotacao de {U} para {F (s)}:
RUF =
sin(θc) cos(θc) 0
cos(θc) −sin(θc) 0
0 0 1
(4.28)
E de notar que a matriz apresentada em (4.28) apresenta uma forma pouco habitual para uma matriz
de rotacao. Acontece que na altura em que foi efectuado o estudo deste algoritmo, ja tinham sido
definidos os eixos de xx como coordenada longitudinal e yy como coordenada tranversal do veıculo
e a convencao utilizada no desenvolvimento deste algoritmo e precisamente a oposta (com os eixos
trocados). Portanto RUF−1 apresenta as colunas correspondentes a xx e yy trocadas, relativamente ao
que seria habitual.
Recorrendo a mecanica classica pode-se definir:
RUF
[d−−→UB
dt
]U
= RUF
[d−−−−→UF (s)
dt
]U
+
[d−−−−→F (s)B
dt
]F
+(−→wc ×−−−−→F (S)B
)(4.29)
Em que:
RUF
[d−−→UB
dt
]U
= RUF
X
Y
0
(4.30)
44
[d−−−−→UF (s)
dt
]F
=
s
0
0
(4.31)
[d−−−−→F (s)B
dt
]F
=
0
y′
0
(4.32)
Uma vez que o referencial F (S) apenas pode rodar em torno do eixo dos zz (pois estamos a considerar
apenas trajectorias no plano bi dimensional) resulta:
(−→wc ×−−−−→F (S)B)
=
0
0
θc = cc(s) s
×
0
y′
0
=
−cc(s) s y′
0
0
(4.33)
Em que[−→wc]
Urepresenta o vector de velocidade angular da rotacao do eixo (F ) relativamente a
(U) e sendo cc(s) a curvatura do caminho num dado ponto s.
Ao juntar todas as partes que compoem a equacao (4.29), resulta apos alguma manipulacao ma-
tematica simples:
s
y′
0
= RUF
X
Y
0
+
y′ s cc(s)
0
0
(4.34)
Que pode ser reescrito na forma:
s = X sin(θc)+Y cos(θc)1−cc(s) y′
y′ = X cos(θc)− Y sin(θc)(4.35)
Tendo em conta a cinematica do veıculo a controlar, apresentada em (4.36), (considerando que nao
existe velocidade v) da equacao (4.35), resulta a equacao (4.37) que descreve a posicao do veıculo
relativamente ao eixo (F ).
x = u cosψ
y = u sinψ
ψ = r
(4.36)
s = u cosψ sin(θc)+u sinψ cos(θc)
1−cc(s) y′
y′ = u cosψ cos(θc)− u sinψ sin(θc)
ψ = r
=
s = u sin(ψ+θc)
1−cc(s) y′
y′ = u cos(ψ + θc)
ψ = r
(4.37)
Agora, pretende-se levar o erro y′ para zero. Partindo de (4.37) ha que determinar ψ que cumpre
essa tarefa. No sistema de equacoes apresentado em (4.37), estas estao parametrizadas em ordem ao
45
tempo, mas para o seguimento do caminho nao estamos interessados em constrangimentos temporais.
Dessa forma considere-se a parametrizacao apresentada em (4.38).
η =
∫ t
0
|s|dt (4.38)
Este novo parametro η tem um significado fısico, que corresponde a distancia que o veıculo percor-
reu medida sobre o caminho, ou seja, caso o veıculo nao se encontre sobre o caminho η corresponde
a distancia que o ponto mais proximo sobre a trajectoria descreve a medida que o veıculo se desloca.
Para simplificar ainda mais a expressao (4.37) pode-se considerar que:
ψ′ = ψ + θc (4.39)
Desta forma, as equacoes apresentadas em (4.37) podem ser parametrizadas em ordem a esta
nova variavel η, como:
ddη s =
u sin(ψ′)1−cc(s) y′∣∣ u sin(ψ′)1−cc(s) y′
∣∣ddηy′ = u cos(ψ′)∣∣ u sin(ψ′)
1−cc(s) y′
∣∣ddηψ
′ = r∣∣ u sin(ψ′)1−cc(s) y′
∣∣ + ccddη s
=
ddη s = sign
(u sin(ψ′)1−cc(s) y′
)ddηy′ = 1−cc(s) y′
tan(ψ′) sign(u sin(ψ′)1−cc(s) y′
)ddηψ
′ = r |1−cc(s) y′|∣∣u sin(ψ′)∣∣ + cc sign
(u sin(ψ′)1−cc(s) y′
) (4.40)
Pretende-se levar o erro lateral y′ para zero (como ja foi referido) e pretende-se fazer isso atraves
do controlo da velocidade angular r. Portanto, uma vez que r nao aparece directamente na equacao
de ddηy′, recorre-se entao a segunda derivada:
d2
dη2y′ =
[−1
sin2(ψ′)
d
dη(ψ′)(1− cc y′) +
1
tan(ψ′)(−y′ gc
d
dη(s)) +
1
tan(ψ′)(− d
dη(y′)cc)
]sign
( u sin(ψ′)
1− cc(s) y′)⇔
d2
dη2y′ =
r
u sin3(ψ′)(1− cc y′)2 − 1 + cos2(ψ′)
sin2(ψ′)(1− cc y′)cc −
y′ gctan(ψ)
(4.41)
Ao isolar a grandeza r que corresponde a velocidade de rotacao do veıculo sobre o eixo dos zz,
resulta:
r =
[d2
dη2(y′) +
1 + cos2(ψ′)
sin2(ψ′)(1− cc y′)cc +
y′ gctan(ψ′)
]u sin3(ψ′)
(1− cc y′)2⇔
r =
[d2
dη2(y′)
sin2(ψ′)
(1− cc y′)+ (1 + cos2(ψ′)) cc +
y′ gc sin(ψ′) cos(ψ′)
(1− cc y′)
]u sin(ψ′)
(1− cc y′)
(4.42)
Considerando agora a entrada de controlo uc e fazendo:
uc =d2
dη2(y′) (4.43)
E preciso entao encontrar uma lei de controlo para uc que assegure a estabilidade do sistema
em cadeia fechada. Desta forma, comece-se por considerar um simples controlador PD (Propotional
46
Derivative).
uc = −kpy y′ − kvyd
dη(y′) (4.44)
Ao substituir a lei de controlo (4.44) na equacao (4.42), resulta:
r =
[(− kpy y′ − kvy
d
dη(y′)
)sin2(ψ′)
(1− cc y′)+ (1 + cos2(ψ′)) cc +
y′ gc sin(ψ′) cos(ψ′)
(1− cc y′)
]u sin(ψ′)
(1− cc y′)⇔
r =u sin(ψ′)
(1− cc y′)
[y′ sin(ψ′)
1− cc y′(gc cos(ψ
′)− kpy sin(ψ′))
+ (1 + cos2(ψ′)) cc − kvy sin(ψ′) cos(ψ′) sign
(u sin(ψ′)
1− cc y′
)](4.45)
A partir da lei de controlo definida para r em (4.45), pode-se reescrever as equacoes apresentadas
em (4.37), resultando:
s = u sin(ψ+θc)
1−cc(s) y′
y′ = u cos(ψ′)
ψ′ = u sin(ψ′)(1−cc y′)
[y′ sin(ψ′)1−cc y′
(gc cos(ψ
′)− kpy sin(ψ′))
+ (2 + cos2(ψ′)) cc − kvy sin(ψ′) cos(ψ′) sign
(u sin(ψ′)1−cc y′
)](4.46)
Desta forma fica definido um algoritmo de controlo para seguimento de caminho atraves de r. No
entanto, nesta fase o seu correcto funcionamento ainda nao esta provado.
Analise de Estabilidade
Em primeiro lugar ha que salientar que este algoritmo apresenta uma limitacao que se prende com o
facto de a cada momento o algoritmo estar a calcular a evolucao temporal do ponto sobre a trajectoria
mais proximo ao veıculo. O algoritmo nao funcionara caso as condicoes iniciais do ponto mais proximo
a trajectoria nao possam ser bem definidas (exemplo: o veıculo tentar seguir uma trajectoria que e
simplesmente uma circunferencia e encontrar-se no seu centro). Por esse motivo o autor comeca por
definir um ”tunel” no qual o veıculo se deve encontrar quando arranca o algoritmo.
Considere-se entao que |gc(s)| e limitado, assim como c(s), de tal forma que:
|c(s)| ≤ cmax (4.47)
Caso a velocidade u(t) seja limitada, nao tenda para zero e y′(0) e ψ′(0) forem tais que:
y′2(0) +1
kpy tan2(ψ′(0))<
1
cmax
|ψ′(0)| < π
2
(4.48)
Entao y′(t) e ψ′(t) convergem assimptoticamente para zero, ou seja, o algoritmo sera capaz de gerar
os comandos de ψ para que o veıculo siga a trajectoria levando o erro lateral para zero.
47
Para provar esta propriedade recorre-se a funcao de Lyapunov apresentada em (4.49).
V =1
2
(y2 +
( ddη
(y′))2)⇔
V =1
2
(y2 +
(1− cc(s) y′
tan(ψ′)
)2) (4.49)
A partir desta funcao de Lyapunov (4.49), Alen Micaelli e Claude Samson provam em [14] que efec-
tivamente o erro de seguimento lateral tende para zero desde que seja tomada em linha de conta o
facto de o veıculo ja se encontrar num ”tunel” junto a trajectoria quando arranca o algoritmo. Esta
consideracao so nao e estendida para toda a trajectoria uma vez que o algoritmo assegura que o
veıculo segue a trajectoria, portanto esse constrangimento esta cumprido (pois estara sempre no refe-
rido ”tunel”).
Ao considerar uma implementacao pratica deste algoritmo seria necessario utilizar uma forma de es-
timar o ponto mais proximo a trajectoria a cada instante. E certo que o algoritmo apresenta um conjunto
de equacoes diferenciais que preveem a evolucao deste ponto, no entanto, numa implementacao real
seria necessario que as medidas das grandezas que fazem parte dessas mesmas equacoes fossem
acessıveis, com ruıdo nulo ou muito proximo disso. Isto porque ao integrar esse determinado conjunto
de grandezas (para obter s e y′) esta-se a acumular o erro provocado pela variavel ruıdo. Desta forma,
ao fim de um certo perıodo de tempo, a utilizacao directa das equacoes diferenciais iria levar a falsas
medidas de s (com erro crescente no tempo).
4.1.4 Metodo Line of Sight modificado
Nesta seccao propoe-se um metodo de seguimento de caminho (Path Following) bastante semelhante
ao metodo proposto por Fotis A. Papoulias e descrito na seccao 4.1.1. Relembrando o metodo anterior
(em 4.1.1) considere-se agora que a distancia de visibilidade d anteriormente medida sobre a trajectoria
(a ser executada pelo veıculo) passa agora a ser medida sobre a recta tangente ao ponto que corres-
ponde a projeccao do veıculo sobre a trajectoria (considerando o veıculo reduzido a um ponto). No
fundo, este ponto corresponde ao ponto sobre a trajectoria que se encontra mais proximo do veıculo.
Neste cenario resulta uma configuracao como a apresentada na figura 4.4. Nesta fase, a semelhanca
do que foi feito na seccao 4.1.1, considera-se que as trajectorias sao apenas circunferencias com um
certo raio R. Pretende-se levar o veıculo a seguir a trajectoria, portanto, a cada instante o veıculo devera
ser ”apontado” segundo o angulo definido na figura 4.4 como σ. Pretende-se que este metodo possa
ser generalizado para qualquer trajectoria, e que nao apresente as limitacoes do algoritmo proposto por
Alain Micaelli e Claude Samson (ver seccao 4.1.3) relativamente as condicoes iniciais do veıculo ou ao
calculo do ponto mais proximo a trajectoria (numa implementacao pratica).
Por analise da figura 4.4, pode-se concluir que:
σ = −π + β + θ (4.50)
48
Figura 4.4: Configuracao algoritmo Line of Sight modificado
E por sua vez:
tan(β) =d
y′
tan(θ) =x
y
(4.51)
Portanto a tan(σ), e facilmente definida a custa de (4.50) e de (4.51):
tan(σ) = tan(−π + β + θ) = tan(β + θ)⇔
tan(σ) =tan(β) + tan(θ)
1− tan(β) tan(θ)=
dy′ + x
y
1− d xy′ y
⇔
tan(σ) =d y + x y′
y′y − d x
(4.52)
E de notar, que quando chegar a altura de enviar o angulo de comando para o baixo nıvel, faltara
adicionar −π ao valor obtido para o angulo σ, uma vez que a simplificacao feita em (4.52), retira −π do
valor final de σ. Da mesma forma, sera necessario adicionar π/2 ao angulo ψ uma vez que o angulo
σ nao e medido relativamente a recta correspondente ao yaw zero mas sim correspondente a recta de
yaw de noventa graus (ψ = π/2).
Desta forma, fica definido o algoritmo de seguimento (a semelhanca do que foi feito na seccao
4.1.1). Passa-se agora a analise do erro lateral y′, quer na situacao em que o veıculo ja se encontra a
seguir a trajectoria, quer na situacao em que o veıculo ainda nao foi ”atraıdo” para a mesma.
Erro em regime permanente
Considere-se que o veıculo ja se encontra a seguir a trajectoria pretendida e portanto, para o caso de
uma circunferencia, a evolucao das suas coordenadas pode ser definida exactamente da mesma forma
49
que em (4.5), resultando:
x = (R+ y′0) cos(wt)
y = (R+ y′0) sin(wt), w =
u
R+ y′0(4.53)
Relembrando os resultados obtidos na seccao 4.1.1, atraves das equacoes (4.6) , (4.7) e (4.8),
recupera-se assim:
ψ0 = w t+ γ0
γ0 =π
2
(4.54)
Desta forma, substituindo (4.54) e (4.53) em (4.52), e relembrando que o angulo ψ encontra-se
desfasado de π/2 do angulo σ, resulta:
tan(wt+ γ0 − π/2) =d (R+ y′0) sin(wt) + (R+ y′0) cos(wt) y′
y′(R+ y′0) sin(wt)− d (R+ y′0) cos(wt)⇔
tan(wt+ γ0 − π/2) =d sin(wt) + cos(wt) y′
y′ sin(wt)− d cos(wt)⇔
tan(wt+ γ0 − π/2) =y′
d + tan(wt)y′
d tan(wt)− 1⇒
tan(γ0) =d
y′
(4.55)
Sabendo a condicao inicial dada por (4.54), facilmente se conclui que:
tan(γ0) =d
y′→∞⇒
y′ → 0
(4.56)
Assim, pode-se concluir que o metodo Line of Sight modificado apresenta menor erro em regime
permanente que o metodo proposto por Fotis A. Papoulias.
Evolucao do erro lateral (Cross-Tracking error)
Considerando o veıculo numa situacao em que ainda nao se encontra a seguir a trajectoria, recuperam-
se as equacoes (4.11) e (4.12), definidas na seccao anterior (4.1.1). Reescrevendo essas equacoes
em (4.57):
ψ1 = ψ − ψ0 = γ − γ0
y′1 = y′ − y′0 =√x2 + y2 −R− y′0
y′1 = u sin(ψ1)
(4.57)
A partir das equacoes anteriores basta que se defina ψ1 para ficar determinada uma expressao da
evolucao do erro y′1. Desta forma, utilizando (4.55), resulta:
tan(ψ1) = tan(γ + π/2) = − 1
tan(γ)= −y
′1
d(4.58)
50
Portanto, a partir da expressao de y1′ definida em (4.57) resulta:
y′1 = −uy′1d√
1 +(y′1d
)2 = −u y′1√d2 + y′21
(4.59)
Definindo a funcao de Lyapunov:
V =1
2y′1
2(4.60)
Conclui-se rapidamente que:
V = y′1 y′1 = −u y′1
2√d2 + y′21
< 0 (4.61)
Logo, pela segunda lei de Lyapunov, pode-se concluir que o ponto y′1 = 0 e ponto de equilıbrio
assimptoticamente estavel e portanto, y′1 → 0 independentemente do valor em que y′1 seja iniciado.
Nesta fase pode-se concluir que este metodo de seguimento de caminho apresentara melhores
resultados do que o algoritmo proposto por Fotis A. Papoulias. No entanto, este novo algoritmo apenas
sera capaz de seguir trajectorias compostas por circunferencias (a semelhanca do algoritmo de Fotis A.
Papoulias). Pretende-se agora generalizar o comportamento deste algoritmo para qualquer trajectoria,
mas sem os constrangimentos a que esta sujeito o algoritmo proposto por Alain Micaelli e Claude
Samson.
Seguimento de trajectorias rectilıneas
Comece-se entao por considerar que a trajectoria a ser seguida agora nao e uma circunferencia mas
sim uma recta. Resulta assim uma configuracao como a apresentada na figura 4.5.
Figura 4.5: Configuracao algoritmo Line of Sight modificado para o seguimento de rectas
Rapidamente se conclui por inspeccao da figura 4.5, que:
51
tan(σ) =y′
d(4.62)
E o angulo de comando de yaw (ψ) que devera ser dado ao veıculo sera:
ψ = arctan(mpath) + σ ⇔
ψ =π
2+ σ
(4.63)
Considerando x0 a abcissa na origem da recta que corresponde a trajectoria:
tan(σ) =x− x0
d(4.64)
Desprezando a velocidade v do veıculo (devido a sua dinamica), pode-se escrever as equacoes da
cinematica do mesmo, simplesmente como:
x = u cosψ
y = u sinψ
ψ = r
(4.65)
Para a analise da evolucao do erro lateral (Cross-tracking error) basta focar a atencao na evolucao
da coordenada x, portanto:
x = u cos
(π
2+ arctan
(x− x0
d
))⇔
x = −sin(arctan
(x− x0
d
))u⇔
x = − u (x− x0)√d2 + (x− x0)2
(4.66)
Recorre-se a funcao de Lyapunov:
V =1
2y′2 =
1
2(x− x0)2 (4.67)
Cuja sua derivada temporal se apresenta como:
V = (x− x0) x = − u (x− x0)2√d2 + (x− x0)2
(4.68)
Portando, fica demonstrado atraves da segunda lei de Lyapunov, que com este algoritmo de segui-
mento para trajectorias rectilıneas o erro lateral tende igualmente para zero.
A partir destes dois tipos de algoritmos apresentados nesta seccao pode-se construir qualquer tra-
jectoria, uma vez que qualquer trajectoria pode ser construıda a custa de um conjunto de circunferencias
e rectas. O algoritmo de seguimento de circunferencias pode ser generalizado para qualquer uma alte-
rando o seu centro ou ate mesmo o sentido de rotacao do veıculo. O algoritmo de seguimento de rectas
pode ser adaptado a qualquer recta aplicando uma matriz de rotacao para que essa mesma recta fique
alinhada com o exemplo que foi explorado nesta seccao.
52
4.1.5 Metodo Line of Sight com Rabbit
Este metodo apresenta alguma evolucao relativamente a todos os metodos de seguimento de cami-
nho apresentados anteriormente. Todos os algoritmos de seguimento de caminho abordados ate entao
estavam de certa forma ”agarrados” ao calculo implıcito ou explicito do ponto sobre a trajectoria mais
proximo do veıculo. Este algoritmo quebra essa linha de pensamento e propoe que se imprima uma
dinamica a esse mesmo ponto. Dessa forma, o ”ponto mais proximo” passa a poder ter o seu proprio
movimento (podendo deixar de ser efectivamente o mais proximo). Devido ao movimento esse ponto
passa a chamar-se de ”Rabbit”. A vantagem desta abordagem e que a dinamica desse ponto e efec-
tivamente um grau de liberdade extra que podera ser utilizado para facilitar a tarefa do veıculo. A titulo
de exemplo, esta dinamica poderia ser aplicada ao algoritmo proposto por Alain Micaelli (ver seccao
4.1.3) para levantar os constrangimentos a que o algoritmo esta sujeito. Este metodo ja foi estudado
em [16],[17] e [9].
Figura 4.6: Configuracao algoritmo Line of Sight com Rabbit
Assim considere-se uma configuracao em tudo semelhante a apresentada em 4.3, mas com a tal
dinamica extra no chamado ”ponto mais proximo”, resultando a configuracao da figura 4.6. Muita da
notacao que foi utilizada para a defenicao e analise do algoritmo proposto por Claude Samson e Alain
Micaelli, pode ser agora recuperada. Considere-se entao que a posicao da origem do referencial do
veıculo {B} relativamente a {U} e definida pelas coordenadas (X,Y ). Da mesma forma, pode-se
expressar o mesmo ponto mas relativamente a {F (s)}, resultando o par de coordenadas (s′, y′), em que
s′ representa o erro do Rabbit relativamente ao ponto sobre a trajectoria que se encontra mais proximo
do veıculo e y′ representa o erro de lateral (cross tracking). E de salientar que {F (s)} representa
um referencial de Serret Frenet em tudo semelhante ao referencial apresentado na seccao 4.1.3, mas
desta vez aplicado no ponto apelidado de Rabbit, que nao e obrigatoriamente o ponto mais proximo da
trajectoria. Considere-se agora a matriz de rotacao de {U} para {F (s)}:
53
RUF =
sin(θc) cos(θc) 0
cos(θc) −sin(θc) 0
0 0 1
(4.69)
Recorrendo agora a mecanica classica, podera escrever-se:
[d−−→UB
dt
]U
=
[d−−−−→UF (s)
dt
]U
+RUF−1
[d−−−−→F (s)B
dt
]F
+RUF−1(−→wc ×−−−−→F (S)B
)(4.70)
Que ”transportado” para o referencial {F (s)} fica agora na forma:
RUF
[d−−→UB
dt
]U
=
[d−−−−→UF (s)
dt
]F
+
[d−−−−→F (s)B
dt
]F
+(−→wc ×−−−−→F (S)B
)(4.71)
Em que:
RUF
[d−−→UB
dt
]U
= RUF
X
Y
0
(4.72)
[d−−−−→UF (s)
dt
]F
=
s
0
0
(4.73)
[d−−−−→F (s)B
dt
]F
=
s′
y′
0
(4.74)
(−→wc ×−−−−→F (S)B)
=
0
0
θc = cc(s) s
×s′
y′
0
=
−cc(s) s y′
cc(s) s s′
0
(4.75)
E desta forma resulta:
RUF
X
Y
0
=
s′ + s
(1− cc(s) s y′
)y′ + s cc(s) s
′
0
⇔ (4.76)
s′ = X sin(θc) + Y cos(θc)− s(1− cc y′)
y′ = X cos(θc)− Y sin(θc)− cc s s′(4.77)
Considerando que o veıculo apenas apresenta velocidade ao longo do seu eixo dos xx resulta a
cinematica:
x = u cos(ψ)
y = u sin(ψ)
ψ = r
(4.78)
54
Ao substituir (4.78) em (4.77) resulta:
s′ = u cos(ψ) sin(θc) + sin(ψ) cos(θc)− s(1− cc y′)
y′ = u cos(ψ) cos(θc)− sin(ψ) sin(θc)− cc s s′(4.79)
Que ao considerar θ = ψ + θc, pode ser reescrito como:
s′ = u sin(θ)− s(1− cc y′)
y′ = u cos(θ)− cc s s′(4.80)
Verifica-se agora que (4.80) e exactamente igual a (4.37) (da seccao 4.1.3) quando se considera
que s′ de (4.77) e zero. No entanto, como ja foi referido, a dinamica do ”Rabbit” permite levantar os
constrangimentos relativos ao ”tunel” a que o algoritmo da seccao 4.1.3 estava sujeito.
Lei de controlo nao linear
A partir de (4.77), uma vez que se pretende ”levar” o par (s′, y′) para zero, pode-se definir uma lei de
controlo nao linear recorrendo a segunda lei de Lyapunov. Assim, considere-se a funcao de Lyapunov
definida em (4.81).
V =1
2(s′ + y′)2 (4.81)
Do calculo da sua derivada temporal resulta:
V = s′ s′ + y′ y′ ⇔
V = s′[u sin(θ)− s(1− cc y′)
]+ y′
[u cos(θ)− cc s s′
]⇔
V = s′[u sin(θ)− s
]+ y′u cos(θ)
(4.82)
Para que a primeira parte da equacao, nao possa ser positiva, e portanto sejam aplicadas as
condicoes da segunda lei de Lyapunov, a dinamica do ”Rabbit” (s) devera ser dada por:
s = u sin(θ) +K1s′ , K1 > 0 (4.83)
Portanto, juntando (4.83) a (4.82), resulta:
V = −K1 s′2 + y′u cos(θ) (4.84)
Assumindo agora que o controlo de Yaw previamente desenvolvido e suficientemente rapido para
que se possa definir o comando de Yaw (referencia de Yaw que devera ser enviado para o controlador de
baixo nıvel) da mesma forma que foi definida na seccao 4.1.2 (algoritmo proposto por Pramod Maurya).
Pode-se escrever que:
θ = −arccos[sat(y′γ
)], γ > 0 (4.85)
55
Sendo que o parametro γ, permite ajustar o quao rapido o veıculo tende para a trajectoria. Quanto
maior γ mais suave sera a convergencia do veıculo para o caminho a seguir. Desta forma, a equacao
(4.84), pode agora ser reescrita apresentando a forma:
V = −K1 s′2 − y′2u (4.86)
A partir de (4.86) e possıvel concluir, utilizando a segunda lei de Lyapunov, que o sistema quando
nao se encontra saturado nem apresenta velocidade u < 0 leva o par (s′, y′) assimptoticamente para
zero.
Desta forma o comando de Yaw a enviar para o baixo nıvel (controlo de orientacao) e que define o
algoritmo sera dado por:
ψ = θc + arccos
[sat(y′γ
)](4.87)
4.2 Implementacao Algoritmo Seguimento de Caminho
Nesta seccao define-se a implementacao em Matlab Simulink de um algoritmo de seguimento de cami-
nho para o caso concreto do veıculo medusa. Esse algoritmo sera o metodo de Line of Sight modificado,
apresentado anteriormente. Foi escolhido este metodo pois apresenta a melhor conjugacao entre re-
sultados e complexidade. E certo que o algoritmo de Line of Sight com ”Rabbit” deveria ser capaz de
apresentar melhores resultados e capacidade de se adaptar a qualquer trajectoria, no entanto a sua
complexidade faz com que a sua implementacao no veıculo real se torne bastante complexa e de difıcil
depuracao. A implementar-se o Line of Sight com ”Rabbit” sera, portanto, sempre numa fase posterior
a implementacao do metodo abordado nesta seccao.
O algoritmo de seguimento de caminho denominado Line of Sight Modificado, apresentado anterior-
mente, aborda o seguimento de dois tipos de trajectorias, circunferencias e rectas. Nesta seccao sera
feita a generalizacao para qualquer trajectoria a partir destas ”pecas” fundamentais.
4.2.1 Seguimento de segmentos de circunferencia
O algoritmo de seguimento de circunferencias apresentado anteriormente em 4.1.4 pode ser generali-
zado para que o veıculo siga qualquer circunferencia. No sentido retrogrado (sentido no qual foi definido
o algoritmo) basta dar a capacidade de levar o centro da circunferencia para qualquer ponto.
Recuperando a equacao (4.52), ao permitir que o centro seja diferente do ponto (cx = 0, cy = 0)
resulta:
tan(σ) =d (y − cy) + (x− cx) y′
y′(y − cy)− d (x− cx)(4.88)
56
Com:y′ =
√(x− cx)2 + (y − cy)2 −R
ψ =π
2+ σ
(4.89)
Ja no sentido directo o algoritmo podera ser escrito como:
tan(σ) =(x− cx) y′ − d (y − cy)
y′(y − cy) + d (x− cx)(4.90)
Com:y′ =
√(x− cx)2 + (y − cy)2 −R
ψ = −π2− σ
(4.91)
Ha agora que acrescentar um ultimo factor, para que possam estar reunidas todas as condicoes
necessarias para que se possa efectuar o seguimento de qualquer tipo de trajectoria, composta por
segmentos de circunferencia. Esse ultimo factor e a definicao do ponto de comutacao entre porcoes da
trajectoria. Para tal recupera-se a definicao de η apresentada quando abordado o algoritmo proposto
por Alain Micaeli e Claude Samson (seccao 4.1.3). Este parametro representa o integral ao longo do
tempo da variavel s, que fisicamente representa a distancia que o ponto que e a projeccao do veıculo
sobre a trajectoria (ponto sobre a trajectoria mais proximo do veıculo) percorreu sobre essa mesma
trajectoria. Para o caso de segmentos de circunferencia η pode ser definido simplesmente como:
η = ∆θ R
∆θ = arctg
(y − ystartx− xstart
) (4.92)
E de notar que, ao utilizar esta parametrizacao para a variacao do angulo, mesmo que o veıculo
nao inicie o seguimento da trajectoria no ponto ”start” acabara por terminar o seguimento da trajectoria
apos completar o varrimento ∆θ relativamente ao ponto de interesse (”start”).
4.2.2 Seguimento de segmentos de recta
No caso de seguimento de segmentos de recta e necessario generalizar para qualquer recta no espaco
nos dois sentidos possıveis para cada recta. Define-se como sentido directo como aquele que leva o
veıculo para mais longe da origem do referencial, segundo a direccao positiva de ambos os eixos. O
sentido retrogrado define-se como aquele que leva o veıculo para mais longe da origem do referencial
segundo a direccao negativa de ambos os eixos.
Pode-se entao definir th como o angulo definido pelo declive do segmento de recta que se pretende
seguir:
th = arctan(mrecta) (4.93)
Ao utilizar a matriz de rotacao apresentada em 4.94, pode-se transformar o problema de seguir
qualquer segmento de recta na configuracao apresentada anteriormente na figura 4.5.
57
R =
sin(th) cos(th)
cos(th) −sin(th)
(4.94)
Dessa forma resulta: y′s
= R
x
y − y0
(4.95)
Ou, em alternativa, quando y0 nao esta definido:y′s
= R
x− x0
y
(4.96)
Em que x0 representa a coordenada x do segmento de recta quando y = 0 e y0 representa a
coordenada y do segmento de recta em x = 0. E portanto a lei de controlo para o seguimento de
segmentos de recta no sentido directo e dada simplesmente por:
ψ = th+ arctan(y′
d) (4.97)
Ja no sentido definido como retrogrado fica definida como:
ψ = −th− arctan(y′
d) (4.98)
A semelhanca do que foi feito para os segmentos de circunferencia falta agora arranjar uma forma
de comutar entre segmentos de recta que compoem a trajectoria. Utiliza-se assim o conceito de η que,
neste caso, pode ser definido como:
η =
∫ t
0
s dt (4.99)
que na pratica corresponde a somar ∆s ao longo do tempo.
4.2.3 Resultado de Simulacao Algoritmo de seguimento de caminho
Seguidamente apresentam-se alguns exemplos ilustrativos do funcionamento do algoritmo de segui-
mento de caminho, aplicado quer a segmentos de recta, quer a circunferencias. Os resultados que se
apresentam foram obtidos atraves de simulacao via Matlab Simulink.
58
(a) Simulacao seguimento de segmento de recta no sentido directo, parametri-zado por (m = 1, by = −1)
(b) Simulacao seguimento de segmento de recta no sentido directo, parametri-zado por (m = −1, by = −5)
(c) Simulacao seguimento de segmento de recta no sentido retrogrado, para-metrizado por (m = 1, by = −1)
(d) Simulacao seguimento de segmento de recta no sentido retrogrado, para-metrizado por (m = −1, by = −5)
Figura 4.7: Seguimento de caminhos composto apenas por segmentos de recta (u = 0.5m/s)
59
(a) Simulacao seguimento no sentido retrogrado de circunferencia centrada(x = 0, y = 0)
(b) Simulacao seguimento no sentido retrogrado de circunferencia centrada(x = −5, y = 5)
(c) Simulacao seguimento no sentido directo de circunferencia centrada (x =0, y = 0)
(d) Simulacao seguimento no sentido directo de circunferencia centrada (x =−5, y = 5)
Figura 4.8: Seguimento de caminho composto apenas por circunferencias (u = 1m/s)
60
Por ultimo apresenta-se uma trajectoria mais complexa composta quer por segmentos de recta
quer por segmentos de circunferencia para demonstrar o funcionamento global de todo o algoritmo de
seguimento de caminho.
(a) Trajectoria
(b) Comando de ψ
Figura 4.9: Simulacao de seguimento de caminho complexo (varrimento espacial)
4.3 Seguimento de caminho na presenca de correntes
Ao executar manobras de seguimento de caminho no oceano sera de esperar a presenca de correntes
que vao perturbar essas mesmas manobras. O algoritmo devera ser capaz de lidar com essas mesmas
61
correntes e aplicar as correccoes necessarias.
4.3.1 Seguimento de caminho sem correccao de correntes
Nesta subseccao, verifica-se qual o erro que se esta a cometer ao tentar efectuar seguimento de cami-
nho com presenca de correntes, sem aplicar qualquer tipo de correccao ao algoritmo para contrariar as
mesmas. Faz-se uma analise da evolucao do erro de lateral de seguimento (Cross-Tracking Error) na
presenca de correntes nao contabilizadas.
Quando o veıculo se encontra a fazer seguimento de caminho, a perturbacao provocada pela cor-
rente sera mais sentida quando for perpendicular a trajectoria a ser seguida. Se for a favor ou contraria
ao movimento do veıculo, apenas atrasara ou acelerara o movimento do mesmo, nao afectando o erro
lateral de seguimento de caminho.
Considere-se entao o pior caso, em que o veıculo navega na presenca de corrente lateral, perpen-
dicular ao seu movimento, resulta portanto uma configuracao como a da figura 4.10.
Figura 4.10: Efeito da corrente perpendicular a trajectoria
O angulo assinalado na figura como σ′ corresponde ao angulo de desvio a partir do qual o proprio
algoritmo de seguimento de caminho anula a componente da corrente que esta a desviar o veıculo (Vx).
Este angulo e atingido no ponto em que y′ permanece constante (para um Vx constante). Neste ponto,
sabe-se que (considerando a velocidade v do veıculo nula):
ψ∗ = π2 + σ′
u cos(ψ∗) = −Vxtan(σ′) = y′
d
y′ = x− x0
(4.100)
Que por manipulacao algebrica pode ser escrito como:
62
−sin(σ′) = −Vxu⇔
Vxu
=x− x0√
(d2 − (x− x0)2)⇔
y′ = x− x0 ≤dVxu√
1−(Vxu
)2(4.101)
Considerando uma velocidade do veıculo de 0.5 m/s e uma corrente Vx = 0.3 m/s e d = 0.1m
resulta pela expressao (4.101):
y′ ≤ 0.075 m (4.102)
A simulacao efectuada via Matlab Simulink confirma este resultado, como se pode visualizar na
figura 4.11.
Figura 4.11: Simulacao do efeito da corrente para uma velocidade u = 0.5m/s e corrente Vx = 0.3m/s
A partir da figura 4.11 e possıvel verificar que a corrente Vx afecta a posicao do veıculo da forma
descrita pela expressao (4.101). A imagem do veıculo foi colocada repetidamente entre um intervalo
fixo de pontos, portanto, e possıvel verificar que nas zonas em que a corrente e oposta ao veıculo, este
desloca-se mais lentamente, ao passo que nas zonas em que corrente e a favor, o mesmo desloca-se
mais rapidamente .
4.3.2 Seguimento de caminho conhecendo as correntes
Conhecendo desta vez as correntes que afectam o veıculo a cada instante, apresenta-se agora um
metodo de compensacao das mesmas atraves do angulo de comando de yaw (ψ).
Considere-se entao uma configuracao como a apresentada na figura 4.12, em que o veıculo efectua
um deslocamento do ponto assinalado como P1 para o ponto P2, mas que devido a corrente vai terminar
63
o seu movimento no ponto P3.
Figura 4.12: Esquema vectorial para correccao de corrente
Pretende-se levar entao o veıculo para P2. Conhecendo o valor da corrente (V x, V y) pode-se forcar
o vector de velocidade total a ir de P1 a P2. Desta forma, considere-se u∗ e ψ∗ como a velocidade u e
o angulo ψ ja com correccao:
total = (u cos(ψ), u sin(ψ))↔
(u cos(ψ), u sin(ψ)) = (Vx, Vy) + (u∗ cos(ψ∗), u∗ sin(ψ∗))(4.103)
E portanto os valores de u∗ e ψ∗ que forcam o vector total a coincidir com−−−→P1P2 sao dados por:
u∗ = ||(u cos(ψ), u sin(ψ))− (Vx, Vy)||
ψ∗ = arctan
(u sin(ψ)− Vyu cos(ψ)− Vx
) (4.104)
No caso de estudo em que se pretende fazer seguimento de caminho e nao levar o veıculo para um
ponto especifico, a correccao em u, nao e a mais importante a fazer. A nao correccao de u nao afecta a
direccao que o veıculo ira tomar, mas apenas a velocidade quando sujeito a correntes. Na figura 4.13,
demonstra-se o funcionamento do algoritmo de correccao de corrente via Matlab Simulink.
A figura 4.13, pode ser confrontada com a figura 4.11, onde se verifica que com o algoritmo de
correccao (com u superior ao modulo da corrente) a trajectoria nao sofre deformacao, no entanto o
veıculo efectuara a mesma mais lentamente, uma vez que parte de u que e utilizada para compensar
o efeito da corrente em certos trocos do caminho (parte superior e inferior da circunferencia). Na figura
4.13 e ainda visivel que nas zonas em que a corrente e perpendicular a trajectoria (com o algoritmo
de correccao) o veıculo ”orienta-se mais” contra a corrente, de forma a que parte da sua velocidade u
anule o efeito da mesma.
64
Figura 4.13: Simulacao da correccao de corrente para uma velocidade u = 0.5m/s e corrente Vx =0.3m/s
4.3.3 Seguimento de caminho com estimacao de correntes
Uma vez que todo o algoritmo de correccao de correntes proposto anteriormente em 4.3.2, assenta no
facto de que se conhece as correntes a priori, e necessario explorar uma forma de medir ou estimar
essas mesmas correntes. Nesta seccao propoe-se um estimador da velocidade da corrente que ao
mesmo tempo, e tambem um estimador da velocidade do veıculo. Esse estimador sera baseado numa
estrutura de um Complementary Filter. O Complementary filter e aqui utilizado para fundir dois sensores
com larguras de banda bem diferenciadas, sendo a estrutura composta por um filtro passa baixo e um
filtro passa alto, em que, cada um deles ”processa” as medicoes do seu respectivo sensor.
Na estrutura utilizada, considerou-se que o veıculo medusa esta equipado com: um sensor capaz
de medir o angulo ψ do veıculo; um sensor de GPS, capaz de determinar a posicao do veıculo, um
sensor Doppler capaz de medir a velocidade do veıculo em relacao ao fluido ou, em alternativa mais
economica, em condicoes estacionarias e possıvel determinar a velocidade do veıculo atraves das
RPM dos motores. Para o caso do veıculo submerso este estimador nao recebe medidas de posicao
fornecidas pelo sensor de GPS, mas sim pelo sensor de USBL. Todas as medidas tem de entrar no
filtro referidas ao mesmo referencial. Desta forma, a medida de velocidade efectuada no veıculo tera de
sofrer uma rotacao dada por RUB . Considere-se assim que a entrada do filtro ja todas as medidas sao
relativas ao referencial {U}, resultando uma configuracao como a da figura 4.14. O estimador e entao
composto por um Complementary Filter para cada uma das coordenadas do veıculo.
Considerando apenas uma das coordenadas do estimador apresentado na figura 4.14, verifica-se
que o estimador e definido por (m = medida e est = estimado):
65
Figura 4.14: Estrutura do Complementary Filter utilizado para a estimacao de correntes
Xest(s) =Xest(s)
s
Xest(s) = Xm(s) +K1(Xm(s)−Xest(s)) +K2Xm(s)−Xest(s)
s
(4.105)
Considerando, nesta fase, que se pretende estimar posicao, resulta:
Xest(s) =Xm(s) +
(K1 + K2
s
)Xm(s)
s+K1 + K2
s
⇔
Xest(s) = T1 Xm(s) + T2 Xm(s)
(4.106)
Sendo que T2 representa um filtro passa baixo e T1 representa um filtro passa alto, com funcoes de
transferencia definidas por:
T1 =1
s+K1 + K2
s
T2 =K1 + K2
s
s+K1 + K2
s
(4.107)
Sendo ωs a frequencia de interseccao entre o filtro passa baixo e o filtro passa alto que compoem o
estimador. Considerando uma frequencia intermedia ωs = 0.5 rad/s.
Igualando as frequencias de corte dos dois filtro a ωs = 0, 5 rad/s, chega-se aos ganhos K1 e K2:
K1 = 1.4
K2 = 1(4.108)
66
A partir deste estimador criado para estimar posicao sera agora possıvel retirar a estimativa da
corrente e velocidade do veıculo no referencial {U}. A estimativa de velocidade ( ˙xest), resulta directa-
mente da derivada da posicao, portanto estara presente antes do integrador que calcula a estimativa
da posicao (xest). Ja a velocidade da corrente sera como uma perturbacao constante para a posicao
estimada, o segundo integrador deste diagrama, sera capaz de introduzir efeito integral para compensar
essa perturbacao. Assim, ter-se-a a saıda do segundo integrador uma estimativa da corrente Vx.
Para a coordenada y tem-se uma copia do estimador em x, pelo que nao sera repetida a analise.
Considerando o veıculo a deslocar-se ao longo de uma recta, como uma corrente Vx = 0.3m/s e
apenas animado de velocidade u = 0.5m/s, apresenta-se de seguida o resultado via simulacao Matlab
Simulink do desempenho do filtro para medidas corrompidas com ruıdo branco Gaussiano.
Figura 4.15: Resultados estimacao de correntes com medidas corrompidas por ruıdo branco gaussiano
A partir da estimativa das corrente e possıvel agora correr o algoritmo de correccao de correntes
apresentado na seccao 4.3.2.
4.4 Algoritmo de seguimento de caminho com malha de controlo
de orientacao
O algoritmo de seguimento de caminho proposto ate entao, foi desenvolvido tendo em linha de conta
apenas a cinematica do veıculo medusa e nao a sua dinamica. Adoptou-se a hipotese de que o controlo
de orientacao seria suficientemente rapido para lidar com os comandos de yaw gerados pelo algoritmo
de seguimento de caminho. Apesar de nao ter sido demonstrado, sera de esperar que a dinamica
do veıculo apresente melhores resultados para trajectorias mais suaves, ou seja, que no seu todo
apresentem taxas de variacao do angulo de yaw mais baixas. Este tipo de trajectorias permitira ao
controlo de orientacao ter mais tempo para conseguir lidar com a dinamica do veıculo e atingir as
67
referencias de yaw geradas pelo controlador de seguimento de caminho.
Considerando a estrutura ja com o controlo de orientacao e a dinamica do veıculo resulta um es-
quema como o da figura 4.16.
Figura 4.16: Estrutura em cascata dos controladores de seguimento de caminho e de orientacao
Na figura 4.17 apresenta-se o que se considera ser uma trajectoria plausıvel para uma futura missao
com o veıculo medusa. Essa mesma figura e resultado da simulacao via Matlab Simulink da estrutura
apresentada na figura 4.16, com uma velocidade u = 0.5m/s e sem a presenca de correntes.
(a) Trajectoria
(b) Comando de ψ
Figura 4.17: Simulacao de seguimento de caminho com ciclo controlo de orientacao
68
Capıtulo 5
Controlo Cooperativo (Cooperative
Path Following)
5.1 Controlador de seguimento de caminho cooperativo
Nesta seccao sera estudado o problema de levar um conjunto de veıculos a seguir uma dada trajectoria
com uma formacao especifica entre eles. Considera-se agora que todos os veıculos tem acesso a tra-
jectoria que devem seguir, devendo existir, neste caso, um ajuste coordenado das velocidades do con-
junto de veıculos para atingir uma determinada formacao. A ideia base e ter um conjunto de veıculos,
todos eles a efectuar seguimento de caminho sobre o seu proprio caminho, e ir ajustando ao longo
do tempo as velocidades de cada um, conforme esteja atrasado ou adiantado em relacao a formacao.
Abordagens semelhantes ja foram estudadas em [7] ,[18], [19] e [20].
Considere-se entao um conjunto de n veıculos, cada um deles a fazer seguimento de caminho a
uma dada velocidade ud (igual para todos os veıculos). Considere-se tambem que cada um dos ca-
minhos, Pathi(ηi) e parametrizado pela variavel ηi, sendo que esta representa o comprimento que o
veıculo andou ao longo da trajectoria, conceito este ja abordado em detalhe no capıtulo 4. No presente
capıtulo utilizaremos η como a variavel de coordenacao, ou seja, dois veıculos encontram-se coorde-
nados caso ηi = ηj . Atraves da variavel η, e possıvel atribuir um erro de coordenacao a cada um dos
veıculos da formacao. Existem diversas formas de definir este erro, por exemplo, a custa da teoria de
grafos e matrizes de adjacencias como em [18], ou de forma algebrica como em [7]. Os metodos sao
equivalentes, porem a forma algebrica pode ser definida sem recorrer a analise de grafos, sendo de
mais facil compreensao. Definindo o erro de coordenacao como:
γi = ηi −1
||Ni||∑jεNi
ηj (5.1)
Sendo que Ni representa o conjunto de veıculos com que o veıculo i consegue comunicar, portanto
||Ni|| representa o numero de veıculos com que o veıculo i consegue comunicar. Assim, o que se
apresenta em (5.1) nao e mais do que um erro ponderado entre o veıculo i e os restantes com que este
69
consegue comunicar, ou seja, quao atrasado ou adiantado e que se encontra o veıculo i, em relacao a
todos os veıculos com que este consegue comunicar.
A custa do erro de coordenacao γ, pode-se definir uma correccao a ser aplicada a velocidade para
levar esse mesmo erro para zero. Define-se assim ucorri como:
ucorri = −Ke tanh(γi) (5.2)
Em que Ke define o limite superior para a correccao que e aplicada, nao podendo a mesma ser
superior a velocidade ud, sob o risco do veıculo poder andar com velocidade negativa (andar para tras).
No fundo, a equacao (5.2), nao e mais que um controlador proporcional, em que se utiliza o grafico
da tangente hiperbolica pois esta apresenta uma curva suave entre 1 e −1 e fora desta regiao satura,
impedindo que quando o erro aumenta, a velocidade possa aumentar ou diminuir indefinidamente.
Desta forma a velocidade de cada veıculo a cada instante e definida como:
utotali = ud + ucorri (5.3)
5.1.1 Analise de Estabilidade
Para analise de estabilidade deste algoritmo e necessario ver como evolui o erro de coordenacao de
todo o conjunto, e nao apenas veıculo a veıculo. Dessa forma, e necessario recorrer (mesmo que de
forma superficial) a teoria dos grafos.
Considere-se entao, a titulo de exemplo, os grafos apresentados na figura 5.1, para um conjunto de
tres veıculos .
(a) Grafo (b) Grafo
Figura 5.1: Exemplo de grafos
Cada no do grafo representa um veıculo, e cada aresta direccional representa capacidade de
comunicacao num dado sentido. Definindo a matriz de incidencias para cada um dos grafos:
A(a) =
0 0 0
1 0 0
1 0 0
A(b) =
0 1 1
1 0 1
1 1 0
(5.4)
Definindo a matriz diagonal com o grau dos vertices (na diagonal esta o numero de veıculos j dos
quais o veıculo i pode receber informacao):
70
D(a) =
0 0 0
0 1 0
0 0 1
D(b) =
2 0 0
0 2 0
0 0 2
(5.5)
A partir das matrizes apresentadas anteriormente pode-se definir o chamado laplaciano do grafo L
e o seu laplaciano normalizado LD, como:
L = (D −A)
LD = D−1 L(5.6)
Para os grafos apresentados como exemplo (figura 5.1), resulta:
LD(a)=
0 0 0
−1 1 0
−1 0 1
LD(b)=
1 − 1
2 − 12
− 12 1 − 1
2
− 12 − 1
2 1
(5.7)
Ao multiplicar o laplaciano normalizado do grafo pelo vector composto pelas variaveis de coordenacao
de cada um dos veıculos que fazem parte do conjunto, resulta uma forma matricial de definir o erro de
coordenacao, isto e:
[γ] = LD [η] (5.8)
Por inspeccao visual da matriz LD, e possıvel concluir a equivalencia entre o metodo apresentado
em (5.1) e (5.8).
A partir da forma matricial do erro, e agora possıvel analisar o erro de coordenacao em conjunto e
portanto, fazer uma analise de estabilidade do algoritmo.
Comece-se por considerar a funcao de Lyapunov:
V =1
2γT γ (5.9)
Cuja a sua derivada e definida como:
V = γT γ (5.10)
Por sua vez:γ = LD η ⇔ 1
γ = LD utotal ⇔
γ = ucorr ⇔
γ = −Ke tanh(γ)
(5.11)
1De salientar que ao utilizar esta equivalencia esta-se a supor que o veıculo ja seque a trajectoria com erro nulo, e que toda avelocidade do mesmo faz variar η.
71
Recuperando novamente (5.10):
V = −γT Ke tanh(γ) (5.12)
Portanto conclui-se, pela segunda lei de Lyapunov, que o ponto de equilıbrio dado por (γ = 0 ; utotal =
ud) e globalmente assimptoticamente estavel.
5.1.2 Estimador da variavel de coordenacao
O algoritmo de CPF apresentado anteriormente tem por base o conhecimento por parte do veıculo
j do parametro ηi de todos os veıculos dos quais este e capaz de receber informacoes. Acontece
que esta comunicacao sera espacada temporalmente, tera um certo atraso (dependendo da distancia
entre veıculos) e algumas falhas pontuais. Assim para que se possa executar o algoritmo e necessario
recorrer a um estimador que possa lidar com estas dificuldades, e permita ter sempre disponıvel uma
estimativa de ηi para a execucao do algoritmo.
Comece-se entao por idealizar uma versao discreta do Complementary Filter desenvolvido no capi-
tulo 4. Recuperando as suas equacoes:
ηest(s) =ηest(s)
s
ηest(s) = ηm(s) +K1(ηm(s)− ηest(s)) +K2ηm(s)− ηest(s)
s
(5.13)
Re-escrevendo estas mesmas equacoes em tempo discreto, e possıvel chegar a (5.14).
ηest(kT ) =
∫ kT
(k−1)T
ηest(τ) dτ
ηest(kT ) = ηm(kT ) +K1(ηm(kT )− ηest(kT )) +K2
∫ kT
(k−1)T
(ηm(τ)− ηest(τ)) dτ
(5.14)
Durante todo o intervalo τ , nao e possıvel ter medidas a chegar constantemente ao filtro, portanto
tera de ser feita uma aproximacao. Existem varias aproximacoes baseadas na integracao numerica,
como ja foi explorado em [21]. Para o efeito sera utilizada a aproximacao a integracao rectangular
progressiva, que consiste em considerar que o sinal a ser integrado sera constante durante o intervalo
de integracao (mesmo nao correspondendo a verdade). Assim, pode-se reescrever (5.14) como:
ηest(kT ) = ηest(kT − T ) + T ηest(kT − T )
Er(kT ) = ηm(kT )− ηest(kT )
ηest(kT ) = ηm(kT ) +K1 Er(kT ) +K2
(T [Er(kT − T )] +
k−1∑k=1
T Er(kT − T )
) (5.15)
Uma vez que o interesse e estimar ηest(kT + T ) pode-se organizar as equacoes anteriores, resul-
tando:
72
Er(kT ) = ηm(kT )− ηest(kT )
ηest(kT ) = ηm(kT ) +K1 Er(kT ) +K2 T
k∑k=1
Er(kT − T )
ηest(kT + T ) = ηest(kT ) + T ηest(kT )
(5.16)
Nesta fase, esta definido um estimador discreto, com base no Complementary Filter projectado
anteriormente. Porem este estimador nao sera capaz de lidar com atrasos nas medidas ou com falhas
na recepcao de medidas que vao alem de um perıodo de amostragem T , ou estejam dessincronizadas
com este.
Estimador com falhas nas medidas
Caso 1: Quando existirem falhas nas medidas de posicao, nao sera possıvel definir Er(kT ) e, portanto,
o estimador tera de confiar na medida de velocidade (ηm(kT )) enquanto nao estiver disponıvel
uma medida de posicao (ηm(kT )). Ou seja, enquanto nao existe medida de posicao o estimador
rege-se pelas equacoes:
ηest(kT ) = ηm(kT )
ηest(kT + T ) = ηest(kT ) + T ηest(kT )(5.17)
Quando volta a ter medidas de posicao o estimador tera de considerar que o erro de posicao foi
nulo enquanto nao teve medidas, ou seja, Er(kT − T ) = 0. Ao fim de um perıodo de amostragem
com medidas, volta as equacoes originais do estimador.
Caso 2: Quando existirem falhas nas medidas de velocidade o estimador utilizara as ultimas medidas
conhecidas de velocidade. Na practica, esta aproximacao corresponde a estender o tempo de
integracao sobre ηm(kT ) durante todo o intervalo em que nao existem medidas. Portanto, as
equacoes que regem o estimador nestas circunstancias podem ser definidas como (sendo n o
numero de perıodos sem medidas):
Er(kT ) = ηm(kT )− ηest(kT )
ηest(kT ) = ηm(kT − nT ) +K1 Er(kT ) +K2 T
k∑k=1
Er(kT − T )
ηest(kT + T ) = ηest(kT ) + T ηest(kT )
(5.18)
Caso 3: Pode existir ainda a situacao em que o veıculo nao captou qualquer medida quer de posicao
(ηm(kT )), quer de velocidade (ηm(kT )). Nesta situacao o estimador apenas pode correr uma
versao do que ”pensa” que esta a acontecer. Assim o estimador continuara a integrar a veloci-
dade que teve no passado, resultando:
73
ηest(kT ) = ηest(kT − nT )
ηest(kT + T ) = ηest(kT ) + T ηest(kT )(5.19)
Estimador com falhas e atraso nas medidas
O estimador projectado ate entao devera equipar cada um dos veıculos de forma a estimar o parametro
ηest(kT + T ) do seu homologo. Para cumprir essa finalidade, e necessario que ambos os veıculos
troquem informacoes quer de posicao (ηm(kT )), quer de velocidade (ηm(kT )) um do outro. Como ja foi
referido, essas comunicacoes podem ter falhas e terao certamente um atraso temporal correspondente
ao tempo de propagacao das mensagens. Desta forma, considerando que a velocidade de propagacao
das ondas sonoras em meio aquatico e de 1500m/s, pode-se obter facilmente o tempo medio de atraso
das mensagens. Adicionalmente, as medidas podem chegar com um atraso superior caso sofram
reflexoes, ou se percam e tenham de ser retransmitidas. Formas de lidar com estes atrasos ja foram
exploradas em [22], [23] e [10]. Propoe-se agora um metodo denominado de Backpropagation, que ja
foi alvo de estudo em [10], ainda que aplicado a outro tipo de filtro.
Considere-se entao, a tıtulo de exemplo, a evolucao cronologica das medidas, apresentadas na
figura 5.2
Figura 5.2: Cronologia da transmissao da mensagens entre veıculos
Este metodo, consiste em ter um estimador a correr a uma frequencia mais elevada do que a
frequencia de chegada de qualquer uma das medidas. O mesmo vai guardando todo os seus esta-
dos anteriores, ate um certo tempo (considera-se que e impossıvel receber uma medida com mais que
um certo atraso). No instante em que e recebida uma medida, o estimador procura na lista de estados
anteriores o tempo em que a medida deveria ter chegado, e a partir desse ponto volta a correr todas as
estimativas ja efectuadas ate ao tempo actual. Este processo acontece mais rapido do que um perıodo
de estimacao, sendo considerado instantaneo.
Nas figuras 5.3 e 5.4 e possıvel visualizar este processo de forma esquematica. A figura 5.3 exem-
plifica como o estimador lida com o atraso nas medidas, ao passo que a figura 5.4 explora o caso em
que existe uma inversao na ordem de chegada das medidas.
74
Figura 5.3: Processo do estimador com falhas nas medidas (atraso simples)
Figura 5.4: Processo do estimador com falhas nas medidas (inversao na ordem de chegada)
O resultado da simulacao do algoritmo de CPF em conjunto com o estimador, com um atraso nas
medidas de velocidade de 1s, e com um atraso nas medidas de posicao de 2s, esta presente nas figuras
5.5 e 5.6.
A partir da figura 5.6 e possıvel verificar o bom funcionamento do estimador do parametro S. Da
mesma forma, e visıvel na figura 5.5 que os veıculos conseguem atingir o alinhamento vertical, conforme
comprovado pelo grafico de erro de seguimento da figura 5.6.
Ao considerar que existem falhas nas medidas de velocidade nos intervalos [5s a 7s] e [12s a 15s],
e falhas nas medidas de posicao nos intervalos [2s a 4s] e [12s a 15s], resultam os graficos presentes
na figura 5.7. Aqui fez-se um zoom apenas aos graficos relativos ao parametro estimado S, para
verificar como evolui este com a falha de medidas. Como se pode verificar quando o estimador nao tem
medidas de posicao, surge um pequeno erro, visıvel no grafico no intervalo [4s a 6s] (devido ao atraso).
No instante 6s sao cortadas as medidas de velocidade e e visıvel que o erro se mantem ate que voltem
a surgir medidas. Nos instantes [12s a 15s], que em termos de medidas de velocidade correspondem
75
Figura 5.5: Trajectoria
Figura 5.6: Estimacao do parametro S e erro de seguimento entre veıculos
a falhas na recepcao de [13s a 16s], e em termos de medidas de posicao a [14s a 17s], nao e cometido
um erro relevante. Nestes instantes, S varia de forma constante e portanto a previsao do estimador
consegue ter pouco erro, uma vez que arranca de uma posicao inicial em que o erro era pequeno.
Figura 5.7: Estimacao do parametro S e erro de seguimento entre veıculos
76
Capıtulo 6
Controlo Cooperativo (Cooperative
Leader Tracking)
6.1 Controlo cooperativo segundo um veıculo lıder
Neste capıtulo estuda-se o problema de controlo cooperativo cujos resultados deverao ser semelhantes
aos do algoritmo CPF. No entanto, neste caso, existe o conceito de um veıculo lıder e um ou mais
veıculos que seguem esse mesmo lıder. Os veıculos que seguirao o lıder nao tem conhecimento a priori
quer da sua trajectoria, quer da trajectoria que o lıder vai executar. Isto e, o veıculo lıder encontra-se
a fazer seguimento de um certo caminho conhecido apenas por ele, e os veıculos seguidores deverao
seguir o lıder atraves de informacoes trocadas com o mesmo. Este problema ja foi alvo de estudo
em [10], propondo-se agora uma abordagem alternativa, para que de futuro, possa ser considerada a
presenca de um Humano na malha de controlo (substituir o veıculo submerso por um mergulhador).
O controlador de seguimento de caminho proposto na presente dissertacao tem por base a equacao
de line of sight classico (equacoes (6.1)) que foi particularizada para o caso de circunferencias e rectas,
de forma a levantar constrangimentos, como e a definicao do ponto mais proximo do veıculo sobre a
trajectoria (ver capıtulo 4 seccao 4.1.4 para melhor compreensao). Ponto esse que tem de ser expli-
citamente calculado, quando se aplicam directamente as equacoes do metodo line of sight classico
(equacoes (6.1)).
tan(σ) =x− xp2y − yp2
ψ =π
2+ σ
(6.1)
Considere-se entao que a todo o instante e possıvel enviar aos veıculos seguidores a informacao
relativa a posicao do veıculo lıder. Ou seja, cada veıculo seguidor tem a sua disposicao um vector
[xL, yL, ψL, uL]. Ao efectuar CLT pretende-se que os veıculos seguidores descrevam a trajectoria do
veıculo lıder, sendo um objectivo da presente dissertacao a manutencao do alinhamento vertical entre o
lıder e um seguidor. Tendo em conta esta preocupacao, queremos que o veıculo seguidor siga o veıculo
77
lıder mesmo que o lıder esteja a seguir a sua trajectoria com um certo erro. Portanto, o ponto em que
se pretende colocar o veıculo seguidor e a posicao do veıculo lıder. Esta afirmacao e equivalente a
referir que o ponto sobre a trajectoria do veıculo seguidor (definicao virtual, pois trajectoria do seguidor
nao existe) mais proximo do mesmo sera a posicao do lıder. Desta forma, o ponto que levantava
constrangimentos no metodo line of sight classico esta sempre bem definido para este caso.
A configuracao que se propoe e a da figura 6.1.
Figura 6.1: Configuracao Algoritmo CLT
O ponto P1 corresponde ao ponto onde se encontra o veıculo lıder, definido como [xL, yL]. Por sua
vez, o ponto P2 e o ponto que se encontra a distancia de visibilidade d segundo a direccao do angulo
de ψL. Portanto este ponto e definido como:
P2 = [xL + d sin(ψL) , yL + d cos(ψL)] (6.2)
Para o angulo de yaw (ψ) do veıculo seguidor, resulta:
tan(σ) =x− (xL + d sin(ψL))
y − (yL + d cos(ψL))
ψ =π
2+ σ
(6.3)
Nesta fase, com as equacoes (6.3), o veıculo seguidor ja se encontra a fazer seguimento (Path
Following) ao veıculo lıder no entanto, isso nao garante o alinhamento vertical que se pretende atingir.
Uma forma de garantir melhor esse seguimento consiste em igualar a velocidade do veıculo se-
guidor a do veıculo lıder. No entanto, isto deixa o sistema em cadeia aberta e os veıculos poderao
navegar eternamente com um certo erro de coordenacao entre eles. Recuperando a lei de correccao
apresentada no capıtulo 5, (quando se efectuava CPF):
78
ucorr = −Ke tanh(γ) (6.4)
Uma vez que o veıculo seguidor tem sempre disponıveis as coordenadas do veıculo lıder, o erro de
seguimento γ e facilmente definido como:
γ = (x− xL) cos(ψL) + (y − yL) sin(ψL) (6.5)
Esta expressao permite avaliar se o veıculo seguidor se encontra ou nao a frente do veıculo lıder.
Desta forma, e possıvel corrigir a velocidade do veıculo seguidor consoante o erro de seguimento ao
veıculo lıder.
Para lidar com a imprevisibilidade que esta associada ao factor humano, quando se substitui o
veıculo submerso por um mergulhador, pode-ser aplicada esta tecnica de correccao de velocidade ao
veıculo lıder. E da mesma forma utilizar-se a velocidade do mergulhador como velocidade de referencia,
que o veıculo lıder deve de seguir.
6.1.1 Estimador da posicao do veıculo lıder
No metodo de CLT, apresentado anteriormente, partiu-se do princıpio que uma medida da posicao do
veıculo lıder estaria sempre disponıvel. O mesmo nao corresponde a verdade, pois a comunicacao
entre os veıculos tem as suas limitacoes, como ja foi referido no capıtulo 5, havera atrasos e falhas
nas comunicacoes. Nesse mesmo capıtulo, a limitacao nas comunicacoes foi ultrapassada a custa de
um estimador para a variavel de coordenacao η. O mesmo princıpio pode ser aplicado as variaveis de
interesse do veıculo lıder ([xL, yL, ψL, uL]). Assim, foram utilizadas tres copias do estimador projectado
no capıtulo 5, resultando uma estrutura como a apresentada na figura 6.2.
Figura 6.2: Estimador da pose do veıculo lıder
Da simulacao via Matlab Simulink do algoritmo de CLT proposto, com os estimadores (sem ruıdo
nas comunicacoes), para os parametros do lıder ([xL, yL, ψL, uL]) resulta:
79
Figura 6.3: Trajectoria
Figura 6.4: Parametro dS e evolucao das velocidades dos veıculos
80
Capıtulo 7
Conclusao
Esta dissertacao abordou o problema de controlo de um veıculo do tipo medusa 1 propriedade do
ISR (Instituto de sistemas e Robotica). Mais concretamente pretendia-se desenvolver um algoritmo
numa configuracao CLT, que permiti-se manter o alinhamento vertical de dois veıculos. Assumiu-se que
ambos os veıculos navegavam a uma profundidade constante, tendo o controlo CLT lugar num plano a
duas dimensoes. Pretendia-se ainda que o algoritmo fosse capaz de lidar com toda a imprevisibilidade
associada a um Humano na malha de controlo. Neste contexto a presente dissertacao vai de encontro
aos objectivos do projecto CADDY, que pretende vir a desenvolver um veıculo que possa actuar como
um ”amigo” do mergulhador, prestando apoio nas missoes do mergulhador.
7.1 Resultados Obtidos
7.1.1 Algoritmos de controlo de baixo nıvel
O desenvolvimento de um algoritmo de CLT pressupoe a existencia de um conjunto de sistemas de
baixo nıvel que tiveram de ser desenvolvidos ao longo da presente dissertacao.
Apesar de ja se encontrar bem documentado, foi efectuada a modelacao do veıculo medusa e
analise do trabalho previamente desenvolvido neste aspecto. A compreensao da cinematica e dinamica
do veıculo e essencial para o restante processo de desenvolvimento.
Foi proposto um algoritmo de controlo de orientacao para o veıculo medusa. No desenvolvimento
desse algoritmo, considerou-se que a velocidade v do veıculo seria sempre nula, tal hipotese justifica-se
com base na natureza da dinamica do veıculo medusa. Sobre o modelo simplificado desenvolveu-se
um algoritmo de controlo de orientacao com base num regulador linear quadratico, que foi generalizado
para todos os pontos de operacao do veıculo medusa atraves de uma tecnica de escalonamento de
ganhos. Uma analise do erro de seguimento deste algoritmo com base no teorema do valor final foi
efectuada.
Considerou-se que o controlo de velocidade seria feito em cadeia aberta, uma vez que nao seria
necessario seguir com precisao uma velocidade especifica. Este tipo de controladores ja se encontrava1http://dsor.isr.ist.utl.pt/vehicles/medusa/
81
desenvolvido em [7], pelo que apenas foi feita uma analise do trabalho previamente desenvolvido.
Propos-se nesta dissertacao uma estrutura que limita os sinais de controlo de forma a garantir a
seguranca dos motores que equipam o veıculo medusa. Este metodo foi desenvolvido de forma a que
beneficiar as mudancas de direccao en detrimento da velocidade quando os motores do veıculo estao
perto dos seus limites fısicos.
O controlo de profundidade foi dado como adquirido ao longo de todo o processo.
7.1.2 Algoritmos de controlo de alto nıvel
Nesta dissertacao foram estudados e analisados varios algoritmos de seguimento de caminho propos-
tos por diversos autores. Ao reunir de forma sistematica um conjunto vasto de algoritmos relevantes
de seguimento de caminho, podera esta mesma colectanea constituir um documento de estudo. Pos-
teriormente foi proposto um algoritmo de seguimento de caminho com objectivo de levantar alguns dos
constrangimentos que atingiam alguns dos algoritmos estudados. Esse algoritmo utilizou a hipotese de
que todas as trajectorias a serem efectuadas serao compostas ou por segmentos de circunferencias
ou segmentos de recta. Esta hipotese simplificativa, nao limita as trajectorias que o veıculo podera
efectuar, mas permite simplificar o algoritmo, a sua implementacao, analise e posterior depuracao no
veıculo real. Foi efectuada uma analise de estabilidade com base na segunda lei de Lyapunov ao al-
goritmo proposto. Essa analise de estabilidade nao incorpora a dinamica do veıculo, apenas a sua
cinematica, considerou-se que o controlo de orientacao sera suficientemente rapido e eficaz para lidar
com a dinamica do veıculo.
Um algoritmo de seguimento de caminho cooperativo (CPF) foi proposto, passo esse que ja tinha
sido previamente explorado em [7] e [18]. No entanto, o desenvolvimento de um algoritmo de CPF
foi um passo importante no sentido do algoritmo de CLT proposto. Durante o desenvolvimento deste
algoritmo foi necessario que existisse comunicacao entre os veıculos e, portanto, foi necessario desen-
volver um estimador capaz de lidar com as comunicacoes e que permitisse a cada um dos veıculos
ter sempre uma estimativa de certas grandezas disponıvel. Adoptou-se uma estrutura baseada num
filtro complementar (Complementary Filter). Foi efectuada uma analise de estabilidade com base na
segunda lei de Lyapunov ao algoritmo.
Por ultimo, foi proposto um algoritmo de CLT com base nos seguimento de caminho de CPF propos-
tos anteriormente. Este algoritmo devera ser capaz de contabilizar toda a imprevisibilidade associada
ao mergulhador. No entanto, devido aos constrangimentos temporais, nao foi possıvel chegar a testar
esta hipotese.
7.2 Trabalho Futuro
O presente trabalho aborda problemas de diversas areas de controlo, desde controlo classico, a estimacao
de parametros, pelo que alguns dos resultados que aqui se apresentam sao ainda preliminares e cer-
tamente ha lugar a muitas melhorias.
82
Implementacao no veıculo real - Devido aos constrangimentos temporais a que este projecto es-
teve sujeito, e a dificuldade de acesso ao veıculo medusa, nao foi possıvel implementar os algoritmos
propostos no veıculo real. Nesta fase, e de esperar que existam algumas dificuldades devido ao Hard-
ware existente no veıculo e os algoritmos deverao ser revistos e adaptados para essa nova realidade.
Como tal, recomenda-se uma implementacao sequencial, comecando pelos algoritmos de baixo nıvel
e so quando garantida a funcionalidade destes subir a complexidade ate aos algoritmos de alto nıvel.
Implementacao de ”Rabbit” no algoritmo de seguimento de caminho e veıculo real - Uma
vez implementado o algoritmo de seguimento de caminho proposto neste trabalho, pode-se melhorar
o desempenho do mesmo ao acrescentar o denominado ”Rabbit” (ver [16]), o mesmo permitira lidar
melhor com a dinamica do veıculo, exigindo referencias de yaw com variacoes menos acentuadas, o
que por usa vez da mais margem ao baixo nıvel para processar os comandos de yaw gerados pelo
controlador de seguimento de caminho.
Analise de estabilidade de toda a malha de controlo composta pelo controlo de orientacao
e de seguimento de caminho - Apenas foi efectuada uma analise de estabilidade aos algoritmos de
controlo de orientacao e de seguimento de caminho em separado e nao a dinamica total do conjunto.
O controlo de orientacao foi projectado tendo em conta a dinamica do veıculo, ao passo que o con-
trolo de seguimento de caminho foi projectado tendo em conta apenas a cinematica do veıculo com a
premissa que o controlo de orientacao seria suficientemente rapido para lidar com toda a dinamica do
veıculo. Uma analise de estabilidade de todo o conjunto devera ser efectuada para assegurar o bom
funcionamento do algoritmo, ou descobrir eventuais singularidades no funcionamento do conjunto.
Estimadores tipo Kalman filter - O estimador utilizado neste projecto para lidar com comunicacoes
foi baseado na versao discretizada de um filtro complementar. Este tipo de filtro nao possui um passo
de previsao que possa oferecer uma estimativa quando o mesmo nao recebe medidas (foi utilizada um
aproximacao nestas circunstancias). Os resultados podem ser melhorados ao considerarmos estruturas
mais complexas, como um filtro de Kalman.
Desvio de obstaculos - Ao longo de todo este projecto nao se considerou a presenca de qualquer
obstaculo. Os veıculos poderao ser equipados com sensores extra, como sonares ou camaras para
possibilitar a deteccao de obstaculos. Numa configuracao CLT essa informacao podera ser passada ao
veıculo lıder para que este possa re-parametrizar a sua trajectoria como se de um obstaculo detectado
pelo proprio lıder se tratasse. Uma forma de lidar com obstaculos ao nıvel do seguimento de caminho
(utilizando ”rabbit”) e proposta por [17].
Estender a profundidade de operacao - O sensor USBL esta limitado a uma profundidade de
cerca de cento e cinquenta metros. Por esse motivo, se quisermos estender o alcance do sistema
podera considerar-se uma sequencia de lıderes seguidores com uma profundidade crescente entre
cada par de veıculos ate atingir a profundidade de interesse. Uma analise de estabilidade de todo o
conjunto devera ser efectuada e um limite para o numero maximo de veıculos podera ser encontrado
com essa mesma analise.
83
84
Referencias
[1] Cognitive autonomous diving buddy. http://www.caddy-fp7.eu/web/60_70_0_-1_-1_-1_
izbornik_pocetna.aspx, 2015.
[2] Medusa class marine robot. http://dsor.isr.ist.utl.pt/vehicles/medusa/, 2015.
[3] Development of vehicles and advanced systems for the execution of underwater inspection tasks.
http://welcome.isr.ist.utl.pt/project/index.asp?accao=showproject&id_project=50.
[4] Advanced system integration for managing the coordinated operation of robotic ocean vehicles.
http://www.horta.uac.pt/projectos/asimov/.
[5] Cooperative cognitive control for autonomous underwater vehicles (co3-auvs). http://www.
robotics.jacobs-university.de/projects/Co3-AUVs/.
[6] Antonio M. Pascoal. Exploring the frontier of cooperative marine robotics: Motion planning, naviga-
tion, and control. https://bts.fer.hr/_download/repository/Antonio_Pascoal.pdf, 2014.
[7] Jorge Ribeiro. Motion control of single and multiple autonomous marine vehicles, msc thesis,
Instituto Superior Tecnico, 2011.
[8] Antonio M. Pascoal and A. P. Aguiar. ”autonomous vehicles: An introduction to modeling”, slides
apoio uc: Modelacao simulacao, Instituto Superior Tecnico. 2014.
[9] Miguel Ribeiro. Computer-based cooperative motion planning, programming, and control of auto-
nomous robotic vehicles, msc thesis, Instituto Superior Tecnico, 2013.
[10] Pedro C. Abreu. Sensor-based formation control of autonomous underwater vehicles, msc thesis,
Instituto Superior Tecnico, 2014.
[11] Issac Kaminer, Antonio M. Pascoal, Pramod P. Khargonekar, and Edward E. Coleman. A velocity
algorithm for the implementation of gain-scheduled controllers*. Automatica, Vol 31, No 8, 1995.
[12] Fotis A. Papoulias. Guidance and control laws for vehicle pathkeeping along curved trajectories.
Applied Ocean Research, 14 (1992) 291-302, 1992.
[13] P. Maurya, A. Aguiar, and A. Pascoal. Marine vehicle path following using inner-outer loop control.
http://paginas.fe.up.pt/~apra/publications/MCMC09_PM.pdf, 2009.
85
[14] Alain Micaelli and Claude Samson. Trajectory tracking for unicycle-type and two-steering-wheels
mobile robots. Research Report RR-2097 [inria-00074575 ], 1993.
[15] N J Wildberger. Diffgeom18: The frenet serret equations , school of maths unsw. https://www.
youtube.com/watch?v=1HUpNAS81PY, 2013.
[16] D. Soetanto, L. Lapierre, and A. M. Pascoal. Adaptive, non-singular path-following control of dy-
namic wheeled robots. in 42nd IEEE Conference on Decision and Control, vol. 2, no. December.
IEEE, 2003, pp. 1765-1770.
[17] Lionel Lapierre, Rene Zapata, and Pascal Lepinay. Simulatneous path following and obstacle avoi-
dance control of a unicycle-type robot. 2007 IEEE International Conference on Robotics and Auto-
mation Roma, Italy, 10-14 April 2007.
[18] Francesco Vanni. Coordinated motion control of multiple autonomous underwater vehicles, msc
thesis, Instituto Superior Tecnico, 2007.
[19] Francesco Vanni, A. Pedro Aguiar, and Antonio M. Pascoal. Cooperative path-following of underac-
tuated autonomous marine vehicles with logic-based communication. Instituto Superior Tecnico,
2007.
[20] Jawhar Ghommam and Faıcal Mnif. Coordinated path-following control for a group of underactuated
surface vessels. IEEE TRANSACTIONS ON INDUSTRIAL ELECTRONICS, VOL. 56, NO. 10,
OCTOBER 2009.
[21] Pedro Soares. Discretizacao de controladores contınuos, msc thesis, Faculdade de Eng. da Uni-
versidade do Porto, 1996.
[22] Pedro C. Abreu, Mohammadreza Bayat, Antonio M. Pascoal, Joao Botelho, Pedro Gois, Jorge Ri-
beiro, Miguel Ribeiro, Manuel Rufino, Luıs Sebastiao, and Henrique Silva. Cooperative control and
navigation in the scope of the ec caddy project. Laboratory of Robotics and Systems in Engineering
and Science (LARSyS), ISR/IST, University of Lisbon, Lisbon, Portugal.
[23] Pedro C. Abreu, Mohammadreza Bayat, Antonio M. Pascoal, Joao Botelho, Pedro Gois, Jorge
Ribeiro, Miguel Ribeiro, Manuel Rufino, Luıs Sebastiao, and Henrique Silva. Formation control in
the scope of the morph project. part ii: Implementation and results. Laboratory of Robotics and
Systems in Engineering and Science (LARSyS), ISR/IST, University of Lisbon, Lisbon, Portugal.
[24] Andre Meira. Cooperative navigation of multiple autonomous underwater vehicles with logic based
communication, msc thesis, Instituto Superior Tecnico, 2011.
[25] Joao M. Lemos. ”introducao ao controlo Optimo”, slides apoio uc: Controlo em espaco de estados,
Instituto Superior Tecnico. 2012.
[26] Joao M. Lemos. ”controlo de sistemas nao-lineares”, slides apoio uc: Controlo em espaco de
estados, Instituto Superior Tecnico. 2012.
86
[27] Joao M. Lemos. ”retroaccao linear de variaveis de estado”, slides apoio uc: Controlo em espaco
de estados, Instituto Superior Tecnico. 2012.
[28] Joao M. Lemos. ”retroaccao linear de variaveis de estado”, slides apoio uc: Controlo em espaco
de estados, Instituto Superior Tecnico. 2012.
[29] Antonio Pascoal, Carlos Silvestre, and Paulo Oliveira. Vehicle and mission control of single and
multiple autonomous marine robots , Instituto Superior Tecnico. 2014.
[30] Coordination and control of cooperating heterogeneous unmanned systems in un- certain environ-
ments. http://www.grex-project.eu.
[31] Antonio Pascoal, Paulo Oliveira, Carlos Silvestre, Luis Sebastiao, Manuel Rufino, Victor Barroso,
Joao Gomes, Gerard Ayela, Pascal Coince, Marcus Cardew, Anne Ryan, Hugh Braithwaite, Ni-
cholas Cardew, Jonathan Trepte, Nicolas Seube, J. Champeau, P. Dhaussy, V. Sauce, R. Moitie,
Ricardo Santos, Frederico Cardigos, Marc Brussieux, and Paul Dando. Robotic ocean vehicles for
marine science applications: the european asimov project*. Presented at OCEANS’2000., year
2000.
87
