Cooperacao de Veıculos Roboticos Marinhos para Apoio ao

Mergulho: da Teoria a Pratica

Rafael Dinis Matos Piresrafael.matos.pires@ist.utl.pt

Instituto Superior Tecnico, Lisboa, Portugal

Dezembro 2015

Resumo

Nesta neste projecto estudou-se o problema do controlo cooperativo entre veıculos marıtimosautonomos. Todos os algoritmos propostos terao como referencia o veıculo Medusa(http://dsor.isr.ist.utl.pt/vehicles/medusa/) propriedade do ISR (Instituto de sistemas e Robotica).O problema sera formulado numa primeira fase tendo em conta o controlo de apenas um veiculo, seraopropostos algoritmos de controlo de orientacao e seguimento de caminho. Numa segunda fase seraoestudados algoritmos de controlo de alto nıvel, ja com o intuito de controlar um conjunto de robos, numcenario onde existira um veiculo lıder (a superfıcie) e um veıculo que o segue o lıder (a uma determinadaprofundidade), sera adoptada uma estrutura de controlo cooperativo denominada Cooperative LeaderTracking (CLT). Numa fase posterior o veiculo que segue o lıder sera substituıdo por um mergulhador,neste contexto sera proposto um algoritmo de controlo (tambem numa configuracao Cooperative LeaderTracking) capaz de lidar com a imprevisibilidade do mergulhador. Os algoritmos propostos serao su-jeitos a um conjunto de testes via simulacao.Palavras-Chave: Veıculos Marıtimos Autonomos, Controlo Cooperativo de Veıculos MarıtimosAutonomos, Cooperative Leader Tracking, Humanos como parte da malha de controlo, VeıculoAutonomo Medusa

1. Introducao

Actualmente um dos maiores problemas das missoessubaquaticas e a dificuldade de localizacao e porconsequencia a navegacao, uma vez que ao contrariodos robos terrestres estes nao tem acesso ao sinal deGPS. Neste projecto recorre-se a um robo autonomomarıtimo (AMV) que se encontra a superfıcie e queportanto, tem acesso ao sinal de GPS, desta formaeste veıculo conhece muito bem a sua posicao iner-cial. Em conjunto com este veiculo encontra-se umveıculo subaquatico ou um mergulhador, se de al-guma forma1 o veiculo submerso ou o mergulhadorconseguir medir a sua posicao relativa ao veıculoque se encontra a superfıcie, estes passam a con-hecer a sua posicao inercial.

O sensor(Ultra Short Baseline) que sera uti-lizado para determinar a posicao relativa do veıculosubmerso ou mergulhador utilizara comunicacaoacustica. A utilizacao de acustica tem associadoproblemas das multiplas trajectorias de propagacaodo sinal devido as multiplas reflexoes2 que o sinalsofre, assim de forma a promover a propagacao di-

1A forma de medir a posicao relativa sera atraves de umsensor de Ultra Short Baseline (USBL)

2Essas reflexoes sao provocadas pela superfıcie aquatica,pelo fundo do mar e pelas diferentes camadas que o compoem

recta do sinal acustico os veıculos deverao navegaralinhados no plano vertical.

O veiculo de superfıcie actuara como lıder nesteprocesso, e o veıculo submerso ou mergulhador de-verao seguir a trajectoria do lıder de forma a mantero alinhamento vertical. Surge aqui um metodo decontrolo denominado de CLT (Cooperative LeaderTracking), que sera o alvo de estudo deste projecto.

2. Estado de Arte2.1. Projecto CO3-AUV’s

O projecto CO-3UAV [2009 - 2012] permitiu de-senvolver e testar um conjunto de sistemas cogni-tivos para coordenacao e controlo cooperativo demultiplos robos autonomos marıtimos (AMV). Esteconjunto de robos foram utilizados com o intuito deauxiliar um mergulhador ao nıvel da localizacao enavegacao.

Figure 1: Visao do Projecto CO3-AUV

1

Numa das aplicacoes [1], utilizou-se um con-junto de robos marıtimos autonomos que mantemuma formacao especifica e predefinida a superfıcie.Encontrando-se a superfıcie estes veıculos podemcontar com acesso ao sinal de GPS. Desta formapode-se utilizar este sistema para executar doistipos de missao:

• Numa primeira missao os robos poderao guiarum agente (humano ou AUV) que se encontraa uma certa profundidade, de acordo com umcaminho desejado. No caso do mergulhador asinstrucoes sao dadas atraves de um modulo de-nominado DAU (Diver Assistence Unit) que sepode visualizar na figura 2.

• Numa outra aplicacao os robos podem seguira trajectoria do agente mergulhador e destaforma guardar um registo da sua actividade en-quanto este efectua livremente a exploracao.

.

Figure 2: Diver Assistence Unit

No ambito deste projecto foi desenvolvido a dis-sertacao [1], esta expoe um algoritmo de con-trolo denominado como cooperative path following.Aqui, um conjunto de veıculos equipados de sis-temas que permitem a cada um deles saber ondeesta a cada instante temporal (i.e. usando GPS)seguem caminhos predefinidos e ajustam as suas ve-locidades (com base na comunicacao que se estab-elece entre eles) de modo a convergirem para umaformacao geometrica desejada que evolui a uma ve-locidade terminal desejada. Neste tipo de controlocooperativo nao existe o conceito de lıder que seraexplorado na presente dissertacao com algoritmosde cooperative path following.

2.2. Projecto CADDYActualmente em execucao, o projecto CADDY [Jan.2014 - Dec. 2016] (Cognitive Autonomous Div-ing Buddy) vai no sentido do projecto CO-3UAVe pretende desenvolver um robo autonomo sub-aquatico para funcionar como parceiro do mergul-hador. Este robo sera auxiliado por um robo desuperfıcie, aumentando a capacidade de monitor-izacao, assistencia e consequentemente a segurancado mergulhador. O sistema desenvolvido sera ca-paz de aprender, interpretar e adaptar-se ao com-portamento do mergulhador, estado fısico e accoes

Figure 3: Visao do Projecto CADDY

do mesmo. O objectivo sera substituir um parceirohumano por um parceiro robotico, este sera capazde:

• a) - Actuar como um observador que monitor-iza constantemente o mergulhador

• b) - Actuar como um ”escravo” do mergul-hador que pode efectuar as mais diversas tare-fas como: captar imagens, fazer reconheci-mento da area, entre outras

• c) - Guiar o mergulhador em seguranca peloambiente sub aquatico que se encontra deacordo com comando de alto nıvel especifica-dos para a missao.

(a) Robot comoguia

(b) Robot comoobservador

(c) Robot como”escravo”

Figure 4: Funcoes a Desempenhar pelo Diving Buddy

E no ambito deste projecto que surge o tema dapresente dissertacao. Surge aqui um problema decontrolo denominado de Cooperative Leader Track-ing (CLT) Este problema de controlo opoe-se decerta forma ao problema explorador em [1]. Nestecaso, existe um veıculo lıder a superfıcie e umveıculo submerso. O veıculo lıder manobra ao longode um caminho e os veıculos que o irao seguir naoconhecem a rota a priori. Os veıculos mergulha-dos nao sabem as suas posicoes inerciais, mas con-seguem medir as suas posicoes relativas ao lıderutilizando uma tecnologia denominada de USBL.Pode-se visualizar esse sensor na figura 5, este,possui um conjunto de pontos (becons) que recebemum sinal acustico proveniente do veıculo a superfıciee atraves da desfasagem entre o sinal que chega aosdiferentes pontos e possıvel determinar em coorde-nadas polares a posicao relativa a fonte do sinal(veıculo de superfıcie).

2

Figure 5: Sensor Ultra Short Baseline (USBL)

A partir das posicoes relativas o veıculos sub-merso devera ser capaz de seguir o veıculo lıder (auma certa profundidade constante) de forma a man-ter o alinhamento vertical.

3. Modelacao do Veıculo Medusa

Ao definir as equacoes do movimento de qualquerveıculo e necessario considerar a direccao e sentidodo movimento. Desta forma existe a necessidade dedefinir pelo menos um referencial solidario com omovimento do veıculo, {U}, composto pelos eixosortogonais {xu, yu, zu} (normalmente denominadode Body Fixed Frame) e um referencial fixo quenunca altera a sua posicao e orientacao, {B}, com-posto pelos eixos ortogonais {xb, yb, zb} (normal-mente denominado World Fixed Frame).

Figure 6: Representacao referenciais Body Frame e World Frame

Em que:

• η1 = [x, y, z]T - vector posicao da origem de{B} expressa em {U}.

• η2 = [φ, θ, ψ]T - vector orientacao do referen-cial {B} em relacao a {U}.

• v1 = [u, v, w]T - vector velocidade linear daorigem 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 actuantesexpressas em {B}

• τ1 = [K,M,N ]T - vector de momentos actu-antes expressos em {B}

3.1. Cinematica

A cinematica do veıculo medusa, quando este ape-nas se desloca no plano horizontal, fica definida

como:x = ucosψ − vsinψy = usinψ − vcosψ

ψ = r

(1)

3.2. DinamicaA dinamica do veıculo medusa, quando este apenasse desloca no plano horizontal, fica definida como:

muu−mvvr + duu = τu

mv v +muur + dvv = 0

mr r −muvuv + drr = τr

(2)

4. Controlo de orientacao no plano horizon-tal

Considerando uma linearizacao das equacoes dadinamica do veıculo medusa (2), em torno do pontode equilibrio [u = 0.5m/s ; v = 0m/s ; r = 0rad/s]e a qual foi concatenado o estado v. Uma vez que apropria natureza da dinamica do veıculo leva a queesta velocidade seja sempre reduzida, resulta:

x = Ax+ bu

y = Cx

A =

[0 1

0Nr+N|r|r2|r|

mr

]B =

01

mr

C =[1 0

](3)

O projecto de um regulador linear quadraticocom o funcional de custo dado por (4) e as matrizesde custo de estado e custo de controlo definidascomo em (5), permite chegar a um vector de ganhosK = [k1, k2]. Consoante a velocidade u os custos deestado e custo de controlo devem ser ajustados, por-tanto para generalizacao deste controlador para to-dos os pontos de operacao do veıculo e feita atravesde uma tecnica de Gain Scheduling.

J = Ψ(x(T )) +

∫ T

0

L[x(t), u(t)]dt (4)

Q =

[q1 00 q2

]R =

[r]

(5)

4.1. Saturacao ao nıvel dos motoresFoi desenvolvido um bloco de saturacao que permitesimultaneamente garantir a proteccao dos motores eprivilegiar as mudancas de direccao em detrimentoda velocidade.

Este diagrama de blocos tem como entrada ossinais τu e τr e saıda os sinais Fest e Fbb dentro doslimites fısicos dos motores.

A metade esquerda do diagrama de blocos 7 (ateaos primeiros blocos ”Motor Sat”) converte os sinaisde modo comum e modo diferencial, τu e τr, respec-tivamente, nos sinais a serem gerados por cada umdos motores do veıculo, Fest e Fbb. Feita a conversao

3

Figure 7: Saturacao ao nıvel dos motores

e aplicada uma saturacao a Fest e Fbb, por ”Motorest sat” e ”Motor bb sat”, respectivamente, de valor12τusat . O diagrama de blocos poderia acabar poraqui, uma vez que os motores estariam protegi-dos e o efeito de Wind-Up ja nao se faria sentir,no entanto, ao utilizar essa configuracao, quandoo veıculo estivesse proximo dos seu limite de ve-locidade e se tentasse efectuar uma mudanca dedireccao este nao seria capaz de a fazer correcta-mente. As saturacoes ao nıvel dos motores anu-lariam parte do modo diferencial exigido para queo veıculo efectuasse a mudanca de direccao. Umaforma de contrariar este problema e fazer com queo veıculo diminua a sua velocidade sempre que janao tem largura de banda num dos motores parapoder executar o torque exigido pelo modo difer-encial para efectuar uma determinada mudanca dedireccao. E precisamente isto que faz a metade es-querda do diagrama apresentado na figura 7. Asultimas duas saturacoes presentes no diagrama dafigura 7 (”Motor est sat 1” e ”Motor bb sat 1”),garantem que numa situacao em que o modo difer-encial exija um torque bastante elevado, a ”porcaode sinal” que e retirada ao motor contrario naocoloca um dos sinais Fest ou Fbb, fora dos limitesfısicos do veıculo.

4.2. Resultados

Figure 8: Angulo de yaw vs referencia de yaw

Figure 9: Forcas solicitadas aos motores apos bloco de saturacao

5. Seguimento de caminho5.1. Seguimento de circunferencias

Figure 10: Configuracao algoritmo Line of Sight modificado

Por analise da figura 10, pode-se concluir que:

σ = −π + β + θ (6)

E por sua vez:

tan(β) =d

y′tan(θ) =

x

y(7)

Portanto a tan(σ), e facilmente definida a custade (6) das tangentes definidas em (7):

tan(σ) =d y + x y′

y′y − d x(8)

E de notar que quando chegar a altura de enviaro angulo de comando para o baixo nıvel, faltaraadicionar −π ao valor obtido para o angulo σ, umavez que a simplificacao feita em 8, retira −π dovalor final de σ. Da mesma forma sera necessarioadicionar π/2 ao angulo ψ uma vez que o angulo σnao e medido relativamente a recta correspondenteao yaw zero mas sim correspondente a recta de yawde noventa graus (ψ = π/2).

5.1.1 Erro em regime permanente

Considere-se que o veıculo ja se encontra a seguir atrajectoria pretendida, e portanto, para o caso de

4

uma circunferencia a evolucao das suas coordenadaspode ser definida como:

{x = (R+ y′0) cos(wt)

y = (R+ y′0) sin(wt), w =

u

R+ y′0(9)

ψ0 = w t+ γ0

γ0 =π

2

(10)

Desta forma, substituindo 10 e 9 em 8, e relem-brando que o angulo ψ encontra-se desfasado de π/2do angulo σ, resulta:

tan(γ0) =d

y′(11)

Sabendo a condicao inicial dada por 10, facil-mente conclui-se que:

tan(γ0) =d

y′→∞⇒

y′ → 0

(12)

5.1.2 Evolucao do erro lateral (Cross-Tracking error)

Considerando o veıculo numa situacao em que aindanao se encontra a seguir a trajectoria:

ψ1 = ψ − ψ0 = γ − γ0

y′1 = y′ − y′0 =√x2 + y2 −R− y′0

y′1 = u sin(ψ1)

(13)

A partir das equacoes anteriores basta que se de-fina ψ1 para ficar determinada uma expressao daevolucao do erro y′1. Desta forma, utilizando 11,resulta:

tan(ψ1) = tan(γ + π/2) = − 1

tan(γ)= −y

′1

d(14)

Portanto, a partir da expressao de y1′ definida

em 13 resulta:

y′1 = −uy′1d√

1 +(y′1d

)2 = −u y′1√d2 + y′21

(15)

Definindo a funcao de Lyapunov:

V =1

2y′1

2(16)

Conclui-se rapidamente que:

V = y′1 y′1 = −u y′1

2√d2 + y′21

< 0 (17)

Figure 11: Configuracao algoritmo Line of Sight modificado parao seguimento de rectas

5.2. Seguimento de segmentos de rectaRapidamente se conclui por inspeccao da figura 11,que:

tan(σ) =y′

d(18)

E o angulo de comando de yaw (ψ) que deveraser dado ao veıculo sera:

ψ = arctan(mpath) + σ =π

2+ σ (19)

Considerando x0 a abcissa na origem da recta quecorresponde a trajectoria, resulta:

tan(σ) =x− x0

d(20)

Para a analise da evolucao do erro lateral (Cross-tracking error) basta focar a atencao na evolucaoda coordenada x, portanto:

x = − u (x− x0)√d2 + (x− x0)2

(21)

Recorrendo a funcao de Lyapunov:

V =1

2y′2 =

1

2(x− x0)2 (22)

Cuja sua derivada temporal se apresenta como:

V = (x− x0) x = − u (x− x0)2√d2 + (x− x0)2

(23)

Portando fica demonstrado atraves da segunda leide Lyapunov, que com este algoritmo de seguimentopara curvas rectilıneas o erro lateral tende igual-mente para zero. Qualquer trajectoria pode serparametrizada por um conjunto de rectas ou cir-cunferencias.

5.3. Resultados

5

(a) Trajectoria

(b) Comando de ψ

Figure 12: Simulacao de seguimento de caminho com ciclo con-trolo de orientacao

6. Seguimento de caminho cooperativo

Considere-se entao um conjunto de n veıculos, cadaum deles a fazer seguimento de caminho a umadada velocidade ud (igual para todos os veıculos).Considere-se tambem que cada um dos caminhos,Pathi

(ηi) e parametrizado pela variavel ηi, sendoque esta representa o comprimento que o veıculoandou ao longo da trajectoria.

Definindo o erro de coordenacao como:

γi = ηi −1

||Ni||∑jεNi

ηj (24)

Sendo que Ni representa o conjunto de veıculoscom que o veıculo i consegue comunicar, portanto||Ni|| representa o numero de veıculos com que oveıculo i consegue comunicar. A custa do erro decoordenacao γ, pode-se definir uma correccao a seraplicada a velocidade para levar esse mesmo erropara zero. Define-se assim ucorri como:

ucorri = −Ke tanh(γi) (25)

6.1. Analise de Estabilidade

Para analise de estabilidade deste algoritmo enecessario ver como evolui o erro de coordenacao detodo o conjunto, e nao apenas de veıculo a veıculo.

Considere-se entao a titulo de exemplo os grafos ap-resentados na figura 13, para um conjunto de tresveıculos .

(a) Grafo (b) Grafo

Figure 13: Exemplo de grafos

Cada no do grafo representa um veıculo, e cadaaresta direccional representa capacidade de comu-nicacao num dado sentido.

Considerando o par de matrizes D (matriz diag-onal com o grau dos vertices) e A (matriz de ad-jacencias).

L = (D −A)

LD = D−1 L(26)

Ao multiplicar o laplaciano normalizado (LD) dografo pelo vector composto pelas variaveis de coor-denacao de cada um dos veıculos que fazem partedo conjunto, resulta uma forma matricial de definiro erro de coordenacao, isto e:

[γ] = LD [η] (27)

A partir da forma matricial do erro, e agorapossıvel analisar o erro de coordenacao em conjuntoe portanto fazer uma analise de estabilidade do al-goritmo.

Comece-se por considerar a funcao de Lyapunov:

V =1

2γT γ (28)

Cuja a sua derivada e definida como:

V = γT γ (29)

Por sua vez:

γ = LD η ⇔ 1

γ = −Ke tanh(γ)(30)

Recuperando novamente 29:

V = −γT Ke tanh(γ) (31)

Portanto conclui-se, pela segunda lei de Lya-punov, que o ponto de equilıbrio dado por (γ =0 ; utotal = ud) e globalmente assimptoticamenteestavel.

1A que salientar que ao utilizar esta equivalencia esta-sea supor que o veıculo ja seque a trajectoria com erro nulo, eque toda a velocidade do mesmo faz variar η.

6

6.2. Estimador da variavel de coordenacao

O algoritmo de CPF apresentado anteriormente,tem por base o conhecimento por parte do veıculo jdo parametro ηi de todos os veıculos dos quais estee capaz de receber informacoes. Acontece que estacomunicacao sera espacada temporalmente, teraum certo atraso (dependendo da distancia entreveıculos) e algumas falhas pontuais. Assim para quese possa executar o algoritmo e necessario recorrera um estimador que possa lidar com estas dificul-dades, e permita ter sempre disponıvel uma estima-tiva de ηi para a execucao do algoritmo.

Comece-se entao por idealizar uma versao disc-reta de um Complementary filter (por aproximacaoa integracao rectangular) progressiva:

η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 )

)(32)

6.2.1 Estimador com falhas nas medidas

caso 1: Quando existirem falhas nas medidas deposicao, nao sera possıvel definir Er(kT ) eportanto o estimador tera de confiar na me-dida de velocidade (ηm(kT )) enquanto naoestiver disponıvel uma medida de posicao(ηm(kT )). Portanto enquanto nao existe me-dida de posicao os estimador rege-se pelasequacoes:

ηest(kT ) = ηm(kT )

ηest(kT + T ) = ηest(kT ) + T ηest(kT )(33)

Quando volta a ter medidas de posicao o esti-mador tera de considerar que o erro de posicaofoi nulo enquanto nao teve medidas, ou seja,Er(kT − T ) = 0, ao fim de um perıodo deamostragem com medidas volta as equacoesoriginais do estimador.

caso 2: Quando existirem falhas nas medidas de ve-locidade o estimador utilizara as ultimas medi-das conhecidas de velocidade. Na practica estaaproximacao corresponde a estender o tempode integracao sobre ηm(kT ) durante todo o in-tervalo em que nao existe medidas. Portantoas equacoes que regem o estimador nestas cir-cunstancias podem ser definidas como (sendon 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 )

(34)

caso 3: Pode existir ainda a situacao em que o veıculonao captou qualquer medida quer de posicao(ηm(kT )), quer de velocidade (ηm(kT )). Nestasituacao o estimador apenas pode correr umaversao do que ”pensa” que esta a acontecer.Assim o estimador continuara a integrar a ve-locidade que teve no passado, resultando:

ηest(kT ) = ηm(kT − nT )

ηest(kT + T ) = ηest(kT ) + T ηest(kT )(35)

6.2.2 Estimador com atraso nas medidas

Figure 14: Processo do estimador com falhas nas medidas (in-versao na ordem de chegada)

Este metodo, consiste em ter um estimador acorrer a uma frequencia mais elevada do que afrequencia de chegada de qualquer uma das medi-das, o mesmo vai guardando todo os seus estadosanteriores, ate um certo tempo (considera-se que eimpossıvel receber uma medida com mais que umcerto atraso). No instante em que e recebida umamedida, o estimador procura na lista de estadosanteriores o tempo em que a medida deveria terchegado, e a partir desse ponto volta a correr todasas estimativas ja efectuadas ate ao tempo actual.Este processo ocorre mais rapido do que um perıodode estimacao, sendo considerado instantaneo.

7

6.3. Resultados

Figure 15: Trajectoria

7. Algoritmo CLTNeste capitulo estuda-se o problema de controlocooperativo cujo os resultados deverao ser semel-hantes aos do algoritmo CLP, no entanto, nestecaso, existe o conceito de um veıculo lıder e umou mais veıculos que seguem esse mesmo lıder. Osveıculos que seguirao o lıder nao tem conhecimentoa priori quer da sua trajectoria quer da trajectoriaque o lıder vai executar. Isto e, o veıculo lıderencontra-se a fazer seguimento de um certo caminhoconhecido apenas por ele, e os veıculos seguidoresdeverao seguir o lıder atraves de informacoes tro-cadas com o mesmo. Este problema ja foi alvo deestudo em [2], propoe-se agora uma abordagem al-ternativa.

Considere-se entao o metodo de line of sightclassico, em que o par (x, y) representa as coor-denadas do veıculo e o par (x2, y2) o ponto alvo,resulta:

tan(σ) =x− xp2y − yp2

ψ =π

2+ σ

(36)

Considere-se entao que a todo o instante epossıvel enviar aos veıculos seguidores a informacaorelativa 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 queos veıculos seguidores descrevam a trajectoria doveıculo lıder, sendo um objectivo da presente dis-sertacao a manutencao do alinhamento vertical en-tre o lıder e um seguidor. Tendo em conta esta pre-ocupacao, queremos que o veıculo seguidor siga oveıculo lıder mesmo que o lıder esteja a seguir a suatrajectoria com um certo erro, portanto, o pontoem que se pretende colocar o veıculo seguidor e aposicao do veıculo lıder. Esta afirmacao e equiv-alente a referir que o ponto sobre a trajectoria doveıculo seguidor (definicao virtual, pois trajectoriado seguidor nao existe) mais proximo do mesmosera a posicao do lıder. Desta forma, o ponto quelevantava constrangimentos no metodo line of sightclassico esta sempre bem definido para este caso.

A configuracao que se propoe e a da figura 16.

Figure 16: Configuracao Algoritmo CLT

O ponto P1 corresponde ao ponto onde se encon-tra o veıculo lıder, definido como [xL, yL]. Por suavez o ponto, P2 e o ponto que se encontra a distan-cia de visibilidade d segundo a direccao do angulode ψL. Portanto este ponto e definido como:

P2 = [xL + d sin(ψL) , yL + d cos(ψL)] (37)

Para o angulo de yaw (ψ) do veıculo seguidor,resulta:

tan(σ) =x− (xL + d sin(ψL))

y − (yL + d cos(ψL))

ψ =π

2+ σ

(38)

Nesta fase, com as equacoes 38, o veıculo seguidorja se encontra a fazer seguimento (Path Follow-ing) ao veıculo seguidor, no entanto apenas isso naogarante o alinhamento vertical que se pretende atin-gir.

Uma forma de garantir melhor esse seguimentoconsiste em igualar a velocidade do veıculo lıder ado veıculo seguidor, no entanto isto deixa o sistemaem cadeia aberta os veıculos poderao navegar eter-namente com um desvio entre eles. Recuperando alei de correccao apresentada no capitulo 6, (quandose efectuava CPF):

8

ucorr = −Ke tanh(γ) (39)

Uma vez que o veıculo seguidor tem sempredisponıveis as coordenadas do veıculo lıder, o errode seguimento γ e facilmente definido como:

γ =x− xL + y − yL|x− xL + y − yL|

√(x− xL)

2+ (y − yL)

2

(40)

Desta forma e possıvel corrigir a velocidade doveıculo seguidor consoante o erro se seguimento aoveıculo lıder.

7.1. Estimador da posicao do veıculo lıder

No metodo de CLT apresentado anteriormente,partiu-se do principio que uma medida da posicaodo veıculo lıder estaria sempre disponıvel. O mesmonao corresponde a verdade, a comunicacao entre osveıculos tem as suas limitacoes, como ja foi referidono capitulo 6, havera atrasos e falhas nas comu-nicacoes. Nesse mesmo capitulo a limitacao nascomunicacoes foi ultrapassada a custa de um esti-mador para a variavel de coordenacao η. O mesmoprincipio pode ser aplicado as variaveis de inter-esse do veıculo lıder ([xL, yL, ψL, uL]), assim, foramutilizadas tres copias do estimador projectado nocapitulo 6. Resultando uma estrutura como a ap-resentada na figura 17.

Figure 17: Estimador da pose do veıculo lıder

7.2. Resultados

Da simulacao via Matlab Simulink do algoritmode CLT proposto, com os estimadores (sem ruıdonas comunicacoes), para os parametros do lıder([xL, yL, ψL, uL]) resulta:

Figure 18: Trajectoria

8. ConclusoesO desenvolvimento de um algoritmo de CLT pres-supoe a existencia de um conjunto de sistemas debaixo nıvel que tiveram de ser desenvolvidos aolongo do projecto.

Foi proposto um algoritmo de controlo de ori-entacao para o veıculo medusa. No desenvolvimentodesse algoritmo, considerou-se que a velocidade vdo veıculo seria sempre nula, tal hipotese justifica-se com base na natureza da dinamica do veıculomedusa. Sobre o modelo simplificado desenvolveu-se um algoritmo de controlo de orientacao com basenum regulador linear quadratico, que foi general-izado para todos os pontos de operacao do veıculomedusa atraves de uma tecnica de escalonamento deganhos. Uma analise do erro de seguimento destealgoritmo com base no teorema do valor final foiefectuada.

Propos-se uma estrutura que limita os sinais decontrolo de forma a garantir a seguranca dos mo-tores que equipam o veıculo medusa. Este metodofoi desenvolvido de forma a que beneficiar as mu-dancas de direccao en detrimento da velocidadequando os motores do veıculo estao perto dos seuslimites fısicos.

Neste projecto foram estudados e analisadosvarios algoritmos de seguimento de caminho pro-postos por diversos autores. Ao reunir de formasistematica um conjunto vasto de algoritmos rel-

9

evantes de seguimento de caminho, podera estamesma colectanea constituir um documento de es-tudo. Posteriormente foi proposto um algoritmode seguimento de caminho com objectivo de lev-antar alguns dos constrangimentos que atingiamalguns dos algoritmos estudados. Esse algoritmoutilizou a hipotese de que todas as trajectorias aserem efectuadas serao compostas ou por segmen-tos de circunferencias ou segmentos de recta. Estahipotese simplificativa, nao limita as trajectoriasque o veıculo podera efectuar, mas permite sim-plificar o algoritmo, a sua implementacao, analisee posterior depuracao no veıculo real. Foi efectu-ada uma analise de estabilidade com base na se-gunda lei de Lyapunov ao algoritmo proposto. Essaanalise de estabilidade nao incorpora a dinamica doveıculo, apenas a sua cinematica, considerou-se queo controlo de orientacao sera suficientemente rapidoe eficaz para lidar com a dinamica do veıculo.

Um algoritmo de seguimento de caminho cooper-ativo (CPF) foi proposto, passo esse que ja tinhasido previamente explorado em [1] e [3]. No en-tanto, o desenvolvimento de um algoritmo de CPFe um passo importante no sentido dos algoritmos deCLT. Durante o desenvolvimento deste algoritmofoi necessario que existisse comunicacao entre osveıculos e portanto, foi necessario desenvolver umestimador capaz de lidar com as comunicacoes eque permiti-se a cada um dos veıculos ter sem-pre uma estimativa de certas grandezas disponıvel.Adoptou-se uma estrutura baseada num filtro com-plementar (Complementary Filter). Foi efectuadauma analise de estabilidade com base na segundalei de Lyapunov ao algoritmo.

Por ultimo, foi proposto um algoritmo de CLTcom base nos seguimento de caminho de CPF pro-postos anteriormente. Este algoritmo devera ser ca-paz de contabilizar o toda a imprevisibilidade asso-ciada ao mergulhador, no entanto devido aos con-strangimentos temporais nao foi possıvel chegar atestar esta hipotese.

References[1] Jorge Ribeiro. Motion control of single and multiple au-

tonomous marine vehicles, msc thesis, Instituto SuperiorTecnico, 2011.

[2] Pedro C. Abreu. Sensor-based formation control of au-tonomous underwater vehicles, msc thesis, Instituto Supe-rior Tecnico, 2014.

[3] Francesco Vanni. Coordinated motion control of multipleautonomous underwater vehicles, msc thesis, Instituto Su-perior Tecnico, 2007.

[4] Andre Meira. Cooperative navigation of multiple au-tonomous underwater vehicles with logic based communi-cation, msc thesis, Instituto Superior Tecnico, 2011.

[5] Cognitive autonomous diving buddy. http://www.caddy-fp7.eu/web/60700−1−1−1izbornikpocetna.aspx, 2015.

[6] Antonio M. Pascoal. Exploring the frontier of cooperativemarine robotics: Motion planning, navigation, and control.https://bts.fer.hr/download/repository/AntonioP ascoal.pdf, 2014.

[7] Antonio M. Pascoal and A. P. Aguiar. ”autonomous vehi-cles: An introduction to modeling”, slides apoio uc: Mod-elacao simulacao, Instituto Superior Tecnico. 2014.

[8] Joao M. Lemos. ”introducao ao controlo Optimo”, slidesapoio uc: Controlo em espaco de estados, Instituto SuperiorTecnico. 2012.

[9] Joao M. Lemos. ”controlo de sistemas nao-lineares”, slidesapoio uc: Controlo em espaco de estados, Instituto SuperiorTecnico. 2012.

[10] Joao M. Lemos. ” retroaccao linear de variaveis de estado”,slides apoio uc: Controlo em espaco de estados, InstitutoSuperior Tecnico. 2012.

[11] Joao M. Lemos. ”retroaccao linear de variaveis de estado”,slides apoio uc: Controlo em espaco de estados, InstitutoSuperior Tecnico. 2012.

[12] Antonio Pascoal, Carlos Silvestre, and Paulo Oliveira. Ve-hicle and mission control of single and multiple autonomousmarine robots , Instituto Superior Tecnico. 2014.

[13] Cooperative cognitive control for autonomous under-water vehicles (co3-auvs). http://www.robotics.jacobs-university.de/projects/Co3-AUVs/.

[14] Advanced system integration for managing thecoordinated operation of robotic ocean vehicles.http://www.horta.uac.pt/projectos/asimov/.

[15] Coordination and control of cooperating heteroge-neous unmanned systems in un- certain environments.http://www.grex-project.eu.

[16] Development of vehicles and advanced systemsfor the execution of underwater inspection tasks.http://welcome.isr.ist.utl.pt/project/index.asp?accao=showprojectidproject =50.

[17] Issac Kaminer, Antonio M. Pascoal, Pramod P. Khar-gonekar, and Edward E. Coleman. A velocity algorithmfor the implementation of gain-scheduled controllers*. Au-tomatica, Vol 31, No 8, 1995.

[18] Antonio Pascoal, Paulo Oliveira, Carlos Silvestre, Luis Se-bastiao, Manuel Rufino, Victor Barroso, Joao Gomes, Ger-ard Ayela, Pascal Coince, Marcus Cardew, Anne Ryan,Hugh Braithwaite, Nicholas Cardew, Jonathan Trepte,Nicolas Seube, J. Champeau, P. Dhaussy, V. Sauce,R. Moitie, Ricardo Santos, Frederico Cardigos, MarcBrussieux, and Paul Dando. Robotic ocean vehicles formarine science applications: the european asimov project*.Presented at OCEANS’2000., year 2000.

[19] Fotis A. Papoulias. Guidance and control laws for vehi-cle pathkeeping along curved trajectories. Applied OceanResearch, 14 (1992) 291-302, 1992.

[20] P. Maurya, A. Aguiar, and A. Pascoal. Marine ve-hicle path following using inner-outer loop control.http://paginas.fe.up.pt/ apra/publications/MCMC09PM.pdf, 2009.

[21] Alain Micaelli and Claude Samson. Trajectory tracking forunicycle-type and two-steering-wheels mobile robots. Re-search Report RR-2097 [inria-00074575 ], 1993.

[22] N J Wildberger. Diffgeom18: The frenetserret equations , school of maths unsw.https://www.youtube.com/watch?v=1HUpNAS81PY,2013.

[23] Miguel Ribeiro. Computer-based cooperative motion plan-ning, programming, and control of autonomous robotic ve-hicles, msc thesis, Instituto Superior Tecnico, 2013.

[24] Francesco Vanni, A. Pedro Aguiar, and Antonio M. Pascoal.Cooperative path-following of underactuated autonomousmarine vehicles with logic-based communication. InstitutoSuperior Tecnico, 2007.

[25] D. Soetanto, L. Lapierre, and A. M. Pascoal. Adaptive,non-singular path-following control of dynamic wheeledrobots. in 42nd IEEE Conference on Decision and Con-trol, vol. 2, no. December. IEEE, 2003, pp. 1765-1770.

10

[26] Jawhar Ghommam and Faıcal Mnif. Coordinated path-following control for a group of underactuated surface ves-sels. IEEE TRANSACTIONS ON INDUSTRIAL ELEC-TRONICS, VOL. 56, NO. 10, OCTOBER 2009.

[27] Lionel Lapierre, Rene Zapata, and Pascal Lepinay. Simu-latneous path following and obstacle avoidance control of aunicycle-type robot. 2007 IEEE International Conferenceon Robotics and Automation Roma, Italy, 10-14 April2007.

[28] Pedro Soares. Discretizacao de controladores contınuos,msc thesis, Faculdade de Eng. da Universidade do Porto,1996.

[29] Pedro C. Abreu, Mohammadreza Bayat, Antonio M. Pas-coal, Joao Botelho, Pedro Gois, Jorge Ribeiro, MiguelRibeiro, Manuel Rufino, Luıs Sebastiao, and HenriqueSilva. Formation control in the scope of the morph project.part ii: Implementation and results. Laboratory of Roboticsand Systems in Engineering and Science (LARSyS),ISR/IST, University of Lisbon, Lisbon, Portugal.

[30] Pedro C. Abreu, Mohammadreza Bayat, Antonio M. Pas-coal, Joao Botelho, Pedro Gois, Jorge Ribeiro, MiguelRibeiro, Manuel Rufino, Luıs Sebastiao, and HenriqueSilva. Cooperative control and navigation in the scope ofthe ec caddy project. Laboratory of Robotics and Systemsin Engineering and Science (LARSyS), ISR/IST, Univer-sity of Lisbon, Lisbon, Portugal.

[31] Medusa class marine robot.http://dsor.isr.ist.utl.pt/vehicles/medusa/ , 2015.

11