Uma proposta de SLAM para veículos subaquátios

(1)

Uma proposta de SLAM para ve´ıculos subaqu´atios

Felipe Guth

∗

, Silvia Botelho

∗

, Luan Silveira

∗

, Paulo Drews Jr.

∗

and Matheus Machado dos Santos

∗ ∗_{NAUTEC - Centro de Ciˆencias Computacionais}

Universidade Federal do Rio Grande - FURG, Rio Grande - RS, Brasil Email: felipeguth, silviacb, luansilveira,[email protected]

Abstract—The unstructured environment, imprecise sensors, the difficulty for feature extraction and the lack of GPS signals, are some of the challenges encountered in underwater environ-ments. In this adverse context, Simultaneous Localization and Mapping (SLAM) techniques are employed to locate the robot in an efficient way in an unknown environment, while creating a representative model of this environment at the same time. A review of sensors and major studies in the literature is presented here. Moreover, a system for treating the problem of SLAM based on Extended Kalman Filter is proposed. The proposed method employs a new technique for extracting features of sonar, based on filtering algorithms from computer vision. The results of the proposed system are presented through a case study that uses real inercial sensor data and a sonar embedded in a robot that traveled a path from about 600m in an abandoned marina. The trajectory generated by the SLAM system is compared to the ground truth data acquired with a DGPS that provides accuracy of about 2m.

Keywords—Simultaneous Localization and Mapping, Underwa-ter Robotics, Sonar, Sensorial Fusion.

I. INTRODUC¸ ˜AO

O mapeamento robótico tem sido uma área bastante explo-rada em inteligência artificial há pelo menos duas décadas. O problema de localização e mapeamento simultâneos, SLAM, é definido como a capacidade de um robô navegar em um ambiente desconhecido e, por meio de leituras sensoriais, construir um mapa representativo do ambiente ao mesmo tempo que localiza o robô nesse mapa [1]. As técnicas de Localização e Mapeamento Simultâneos (ou SLAM, do inglês Simultaneous Localization and Mapping), são usadas para permitir que robôs móveis construam mapas em ambientes desconhecidos, ou para atualizar um mapa em um ambiente anteriormente percorrido, mantendo sua localização corrente no mapa que é simultaneamente constru´ıdo. Este é um dos problemas mais dif´ıceis encontrados na robótica. Embora existam técnicas de SLAM bem estabelecidas que obtenham resultados satisfatórios, pode-se dizer que o mapeamento e localização simultâneos em ambientes de grande extensão é um problema ainda não resolvido. Isso devido ao grande custo computacional para o tratamento de dados massivos advin-dos de diversas fontes sensoriais e a imprecisão, exatidão e resolução dos dados obtidos através dos sensores. Na literatura encontra-se um grande número de trabalhos de SLAM para ambientes terrestres e indoor.

Em ambientes subaquáticos, nota-se o aumento do uso de sistemas robóticos em tarefas de inspeção [2], manipulação e monitoramento [3]. Oceanos, rios, barragens, mares habitam uma rica biodiversidade, influenciam no clima e têm um grande impacto não apenas na economia como na vida e evolução da sociedade humana. A crescente exploração de elementos

do mar, como as reservas de petróleo localizadas na camada do pré-sal brasileiro, propiciam a geração de uma demanda por tecnologias de manipulação e inspeção de estruturas e recursos localizados nesses ambientes inóspitos e muitas vezes inacess´ıveis ao ser humano. Nessa situação de adversidade, surge a robótica móvel subaquática com ROV’s Remotely Operated Vehiclese AUV’s Autonomous Underwater Vehicles. A localização e mapeamento desses ambientes são fatores essenciais para o uso de tais tecnologias.

Abordagens para a realização de SLAM subaquático encontram-se ainda, em uma fase inicial e um número limitado de abordagens tem sido apresentado [4], [5], [6],[7], [8], [9]. O meio subaquático apresenta dificuldades relacionadas a presença de água e sua interferência na operação de robôs. Entre as quais, pode-se citar a impossibilidade do uso de sinais de GPS, a dificuldade de imageamento devida a turbidez, a influência de correntes mar´ıtimas e do próprio movimento dos propulsores. Esses fatores dificultam a modelagem do compor-tamento dinâmico do robô e seus atuadores para aplicação de técnicas de localização e mapeamento associadas a presença do meio aquoso, o qual, interfere na capacidade de aquisição de dados e funcionamento dos seus sensores e atuadores.

Este trabalho apresenta uma proposta de SLAM para ve´ıculos subaquáticos, utilizando um sonar mecânico de im-ageamento (MSIS) como sensor principal. Um novo método para extração de features de sonar é especificado. Por fim, é feita a validação utilizando-se um data-set obtido em uma marina abandonada, onde é demonstrada a potencialidade do sistema, comparando os resultados gerados com o ground truth.

II. SENSORIAMENTO SUBAQUATICO´

Tendo em vista as particularidades do meio subaquático, o sensoriamento do ambiente é um outro aspecto fundamental a ser mencionado. Geralmente, os dados advindos dos sen-sores tem uma acurácia limitada, principalmente no caso de ambientes com pouca iluminação, fortes correntes mar´ıtimas ou águas turvas. A estimativa de ru´ıdo acaba causando im-pacto significativo nas tarefas de localização e mapeamento, causando muitas vezes a não convergência do sistema. A modelagem do ru´ıdo dos sensores é uma tarefa complicada e importante, principalmente para ambientes de larga escala, uma vez que o ru´ıdo pode ser diferente em determinadas situações, requerendo uma nova calibração para a obtenção de uma melhor estimativa do sistema. Os sensores geralmente operam apenas em uma profundidade limitada, tornando aplicações em grandes profundidades altamente custosas.

Na Tabela I é apresentado uma compilação dos principais sensores utilizados em robôs subaquáticos bem como do tipo

(2)

de informac¸˜ao fornecida para cada um.

TABLE I. PRINCIPAIS SENSORES UTILIZADOS EM APLICAC¸ ˜OES SUBAQUATICAS´

Informac¸˜ao Sensor

Velocidade DVL (Dopler Velocity Log) Informação inercial INS (Inertial Navigation System) Velocidade, orientação IMU (Inertial Motion Unit) Estimação de atitude, aceleração MRU (Motion Reference Unit) Orientação referente ao norte magnético Bússola

Atitude e referência de orientação AHRS (Attitude Heading Reference System) Profundidade Sensor de Pressão, Sensor de Profundidade Imagem visual Câmera

Distˆancia e imagem visual Laser Scanner

Distância e ângulo de um alvo, batimetria Echo sounder, Mechanically scanned profiler, Multibem echo sounder Imagem Acústica Mechanically scanned imaging sonar,

Eletronically scanned imaging sonar, Sidescan sonar

Distância (localização) LBL (Long Base Line), SBL (Short Base Line) Distância e ângulo (localização) USBL (Ultra Short Base Line)

III. ABORDAGENSPROBABIL´ISTICAS

Na forma probabil´ıstica, o problema de localização e ma-peamento simultâneos requere que a distribuição probabil´ıstica P (xk, m|Z0:k, U0:k, x0) (1)

seja computada para todos os tempos k. Essa distribuição de probabilidade descreve a ligação da densidade posterior das localizações dos pontos de referência e o estado do ve´ıculo (no tempo k) dadas as observações tomadas z0:k, os controles

de entrada u0:k e o estado inicial do ve´ıculo x0. Em geral,

uma solução recursiva do problema é desejável. Iniciando com uma estimativa para distribuição P (xk−1, m|Z0:k−1, U0:k−1)

no tempo k−1, a probabilidade posterior, seguindo um controle uk e uma observação zk, é computada usando o teorema de

Bayes. Essa computação requer que um modelo de transição de estados e um modelo de observação sejam definidos, os quais descrevem o efeito do controle de entrada e observação respectivamente.

Os principais algoritmos probabil´ısticos aplicados ao prob-lema de SLAM são o Filtro de Kalman Estendido (EKF) [10], [11], Filtro de Kalman Unscented (UKF) [12], Filtro de Part´ıculas (PF) [13], GraphSLAM [14], [15] e o Filtro de Informação Esparso Estendido (SEIF) [16].

IV. TRABALHOS RELACIONADOS

Um dos primeiros trabalhos no histórico de SLAM sub-aquático foi o trabalho de [17] que propôs em sua primeira versão um algoritmo de SLAM para estimar a navegação do ve´ıculo Oberon e concorrentemente fazer o mapeamento das landmarksartificiais inseridas no ambiente através das leituras de um sonar de imagem. Já [4], propuseram uma abordagem de range-only SLAM. Neste trabalho o ve´ıculo Caribou operou na costa italiana, equipado com transponders LBL. Usando apenas dados de distância através do LBL, foi gerado a trajetória do ve´ıculo e localização dos transdutores. Foi realizado uma comparação entre os resultados e o ground truth.

Em 2005, [18] propuseram um algoritmo para explorar da-dos batimétricos pra realização de SLAM. A solução consiste

em gerar uma série de submapas desde pequenas impressões batimétricas criadas durante curtos per´ıodos de tempo. De uma forma similar às apresentadas anteriormente em algoritmos de SLAM visual subaquático, estimativas prévias de lugares visi-tados são retidas no estado de um EKF para servir como âncora para os submapas. Então, essas impressões são registradas para gerar medidas de posição relativas entre os estados anteriores. Usando medições de um sistema LBL como ground truth, observou-se um melhor alinhamento da estimativa de posição pelo algoritmo de SLAM frente a estimativa dos sensores de dead-reckoning.

No ano de 2007, [6] e [19] através da utilização do AUV DepthX, apresentaram uma abordagem de SLAM utilizando um filtro de part´ıculas Rao-Blackwellized com um grid de evidência 3D para representação do mapa. Ainda, foi abordada um estrutura para manter os milhares de mapas necessários para o filtro de part´ıculas modelar ambientes extensos.

Em 2008, [5] apresentaram um sistema de EKF SLAM para ambientes parcialmente estruturados, utilizando um novo AUV para a criação de uma base de dados tendo como sensor principal um sonar mecânico, além de dados de sensores inerciais e de DGPS para ground truth. Foi proposta uma nova abordagem para extração de features em formato de linhas como para representação da incerteza dos dados acústicos do sonar, construindo múltiplos mapas locais ao longo da trajetória.

Em 2013, [7] propuseram um algoritmo baseado em poses para resolver o problema completo de SLAM para AUVs nave-gando em ambientes desconhecidos e possivelmente desestru-turados. Primeiramente, estimou-se o caminho percorrido pelo robô, enquanto é formada a imagem acústica com um sonar rotativo mono-beam, proporcionando estimativa de posições para corrigir distorções que o ve´ıculo produz ao longo das leituras. Após isso, leituras consecutivas servem de entrada para técnicas probabil´ısticas estimarem o deslocamento do ve´ıculo, incluindo a incerteza do resultado do matching da leitura. Finalmente, um EKF aumentado estima e mantem as poses registradas no estado.

Nota-se que existem aplicações para um número variado de cenários, utilizando-se diferentes algoritmos e sensores. O próximo Cap´ıtulo faz a proposta de uma abordagem de SLAM tendo como sensor principal um sonar mecânico de imagem, além de utilizar dados de dead-reckoning de sensores DVL e MRU.

V. PROPOSTA DESLAM

O sistema opera em dois módulos simultaneamente. O primeiro busca continuamente por features do ambiente, através de dados obtidos por um sonar mecânico. O outro módulo consiste na implementação do Filtro de Kalman Esten-dido que continuamente funde informações de sensores (DVL, bússola, MRU) e as observações do módulo de extração de caracter´ısticas para manter a estimativa de posição do robô e construir simultaneamente um mapa. No trabalho é utilizado o dataset de [5], o qual foi obtido em experimentos conduzido em uma marina abandonada. Esse ambiente em particular é composto por várias paredes. O módulo de extração de features faz o uso de uma técnica inspirada em métodos de visão

(3)

Fig. 1. M´odulos do sistema de SLAM proposto

computacional, em uma versão de votação e identificação da posição correta de marcos relevantes do ambiente.

A Figura 1, descreve em alto n´ıvel a abordagem de SLAM proposta, sendo seus componentes explicados em detalhe ao longo desta sec¸˜ao.

O sonar fornece continuamente dados de intensidade de leituras do ambiente. Integrando-se um conjunto de leituras (beams), onde cada uma contém 500 pontos de intensidade (bins) em uma resolução de 10cm de espaçamento, para uma dada orientação, é formada uma imagem acústica.

A. Uma nova proposta para extração de features de sonar Para a identificação de features significativas advindas dos dados das leituras do sonar é preciso que se adotem estratégias para segmentação dos dados e remoção de ru´ıdos. O módulo de extração de features proposto nesse trabalho é associado à execução do filtro de Kalman, que simultaneamente executa o SLAM e fornece dados de posição do ve´ıculo no momento em que uma leitura é executada. Estes dados de posicionamento são usados para corrigir a distorção da sequência de medidas que formam a representação de uma região do ambiente.

1) Tratamento de dados acústicos: Esse processo consiste em selecionar apenas os retornos de distância (bins) com uma intensidade maior do que um valor pré-estabelecido. Além disso, devem ser eliminados bins redundantes através da exclusão de máximos locais que estão muito próximos, pois estes provavelmente representam os mesmos objetos. Os bins com intensidades mais altas são distinguidos no buffer, uma vez que têm uma maior probabilidade de representar um objeto do ambiente e assim têm um maior peso na estratégia de voto. O processo de threshold dos dados permite que as próximas etapas do processo de extração de features trabalhem apenas com os dados mais consideráveis e que, portanto tem maior probabilidade de representar um marco do ambiente, uma vez que grande parte dos dados redundantes e ru´ıdos são exclu´ıdos nessa primeira fase.

Para armazenar a representação do ambiente é criado um buffer de leituras, o qual representa os 180◦ mais recentes. Nesse buffer são armazenados os valores de ângulo e distância dos bins, conforme os processos descritos anteriormente.

2) Esquema de voto e determinação de features candidatas: A fim de determinar as features existentes no ambiente, peri-odicamente é executada uma abordagem de voto usando os da-dos armazenada-dos no buffer. Isso é feito com uma aplicação em uma Tabela bidimensional. Para as coordenadas de orientação e distância perpendicular, são geradas todas as posições poss´ıveis de localizações de features (pB e θB de acordo com uma base de referência B do sonar). As features que receberem o maior número de votos de acordo com os dados do buffer são os prováveis descritores de objetos do ambiente.

A base de referência B é a posição corrente do sonar. Nesse caso permite-se integrar uma feature diretamente no sistema de SLAM, uma vez que já está representada com relação à última posição do sensor, como uma observação de uma feature já presente no mapa, ou incorporada como uma nova representação depois de ser ajustada com a posição corrente do ve´ıculo. O espaço de voto é discretizado em coordenadas polares, de acordo com a resolução de leitura do sonar, no caso do data set da marina (1.8◦ e 0.1m). Uma resolução maior não aumenta necessariamente a qualidade da detecção, uma vez que a resolução do sonar limita sua precisão.

O algoritmo para extração de features inicia com a chegada de um novo beam. O espaço de voto é referenciado para a posição corrente do sonar como nova base de referência. Depois, todos os bins armazenados no buffer são referenciados para a base de referência B para serem usados em um espaço de voto. Essa transformação para nova referência corrige a distorção dos dados. Uma vez que os dados estejam corrigidos, os votos correspondentes a cada bin são atribu´ıdos para as features candidatas na forma de um modelo baseado em técnicas de filtragem de visão computacional (abordada na subseção seguinte). Então, uma busca pelas features com maior número de votos é executada, a fim de selecionar as candidatas mais significativas.

Nesse contexto, uma feature representa um ponto signi-ficativo que foi observado como um bin de alta intensidade no ambiente e foi posteriormente selecionado através do algoritmo de extração de features. Ao longo do trajeto percorrido, o sistema continuamente faz a busca e extrai point features. Estas são fornecidas ao módulo de associação de dados que faz a correspondência das observações com uma instância já presente no vetor de estados ou insere a nova observação no sistema de SLAM.

3) Selecionando features candidatas: Devido à incerteza produzida pela largura do feixe da onda de sonar, uma medição não pode ser atribu´ıda a um único ponto no espaço. Os trabalhos [20], [21] propuseram uma abordagem comum, bem estabelecida para sonares usados ao ar livre, considerando a medida na forma de um arco cuja abertura representa a incerteza da largura de medida. Uma intensidade alta de retorno é produzida quando a onda acústica atinge uma su-perf´ıcie perpendicularmente, e pode-se inferir que todas às superf´ıcies tangentes ao arco podem explicar o retorno de alta intensidade. Enquanto essa abordagem é bem aceita em ambientes ao ar livre, [5] propõem uma abordagem onde em ambientes subaquáticos a capacidade de detecção de linhas não é limitada às superf´ıcies tangentes ao arco, mas aos beams que fazem intersecção a superf´ıcie, dentro de limites definidos por um ângulo máximo de incidência que também produzem um retorno discern´ıvel. Basicamente, dado um ângulo de largura

(4)

Fig. 2. Método de votação para identificação de features

de feixe horizontal e um ângulo de incidência, o conjunto de linhas é composto não apenas de linhas tangentes ao arco mas também por todas as linhas que interseccionam o arco com um ângulo de incidência.

A Figura 2, descreve em alto n´ıvel a abordagem de voto proposta. Depois dos retornos máximos serem selecionados, e os dados do buffer serem corrigidos para a última localização do sonar, os bins armazenados no buffer são utilizados em um esquema de voto para identificação de features.

Uma Tabela é gerada com todas as poss´ıveis localizações variando a orientação e distância, e discretizadas segundo a resolução do sonar, nesse caso 1.8 graus e 0.1m. Os bins representando os máximos locais de retorno do sonar, recebem um valor alto na Tabela enquanto a sua vizinhança recebe um valor menor. Esse esquema, onde os locais vizinhos aos bins de máximos locais recebem votos, consiste na designação da variação poss´ıvel de ocorrer nos dados acústicos. Atribuindo um valor de incerteza, representada pela atribuição de votos a locais vizinhos.

Cada célula do grid pode receber apenas um voto de um binparticular. Células contendo múltiplos votos, portanto, rep-resentam features compat´ıveis com diferentes bins individuais. Toda vez que um novo beam é adquirido, um novo espaço de voto é gerado para se buscar por features. Qualquer feature candidata com um número suficiente de votos encontrada dentro desta zona é declarada vencedora. Assim que uma feature é detectada todos os bins envolvidos no esquema de votação são removidos do buffer para não interferir com a detecção das próximas features. A Figura 3 mostra um exemplo de seleção de feature onde os valores máximos, representando diferentes observações, são selecionados.

Fig. 3. Selec¸˜ao de features

Estas células com maior número de votos, representam os pontos mais significativos dentro de um conjunto de leituras de sonar que formam uma imagem acústica. As features selecionadas são fornecidas ao módulo de associação de da-dos. Este por sua vez, criará uma nova instância da feature observada, fará uma associação com uma instância existente ou descartará a observação. Este último caso ocorre, de acordo com os parâmetros do sistema, caso a feature observada esteja muito distante de uma instância existente para ser associada e muito próxima para ser criada.

B. Inicializac¸˜ao do sistema

Quando um novo mapa estocástico é inicializado, deve-se deve-selecionar uma badeve-se de referência L no passo 0. Define-se assim a posição inicial do ve´ıculo para localização desta base no tempo 0 com completo conhecimento e sem incerteza. Pode-se representar o estado do ve´ıculo µV:

µV = [x y z ψ u v w r]T (2)

Assumindo que ainda não existam marcos (landmarks) no estado, e que o mesmo encontra-se em uma pose estacionária, o estado do ve´ıculo no tempo 0 é igual a:

ˆ

µ(0) = [0 0 0 0 0 0 0 0 ]T; Σ(0) = ΣV(0) = [08×8] (3)

C. Predic¸˜ao de estado

O vetor de estados e a predição do estado completo, para um AUV como o usado no dataset da marina, pode ser representado por uma função de processo que usa um modelo cinemático de movimento baseado na aceleração do robô. A função g em um tempo t é representada como:

µ(k) =               x y z ψ u v w r p θ               (t) =                   x + (uT + suT2/2)cos(ψ) −(vT + suT2/2)sin(ψ) y + (uT + suT2/2)sin(ψ) +(vT+ svT2/2)cos(ψ) z + wT + s2T2/2 ψ + rT + srT2/2 u + suT v + svT w + swT r + srT 0 0                   (t−1) (4) Onde p e θ representam os parâmetros de um marco (landmarks) e T o intervalo de tempo aplicado no movi-mento. Como os dados do sonar são referenciados em sistema polar, o tamanho do vetor de estados final é igual a 8 (x, y, z, ψ, u, v, w, r) na discretização dos eixos, orientação yaw e correspondentes velocidades lineares e angulares mais 2 × n marcos do ambiente, uma vez que cada marco é representado por θB e pB.

A vari´avel s = [su sv sw sr]T representa um ru´ıdo

(5)

e covariância Q. Em relação às features, presume-se que as mesmas são estáticas e, portanto, sua localização não possui modificações no passo de predição.

A matriz de covariˆancia, que representa a incerteza do estado do ve´ıculo e das features ´e obtida como:

Σ(t|t − 1) = Gv(t) 08×2n 02n×8 I2n×2n Σ(t − 1) Gv(t) 08×2n 02n×8 I2n×2n T ... + Wv(t) 02n×4 Q Wv(t) 02n×4 T (5) Onde GV e WV s˜ao as matrizes Jacobianas de derivadas

parciais do modelo não-linear da função g com respeito ao mesmo estado µV e ao ru´ıdo s.

D. Atualizac¸˜ao Sensorial

Os dados de dead-reckoning providos pela DVL e MRU são usados como observação direta em seus correspondentes componentes no vetor de estados. Pode-se criar uma matriz Z com as leituras sensoriais dos diferentes sensores, interpolando as leituras individuais para um determinado tempo:

Z = [vxDV LvyDV L vzDV LvψM RU ψM RUzM RU]T, (6)

As medições são introduzidas no sistema através da matriz H que mapeia a leitura dos sensores em relação ao vetor de estados.

HD= [03×4I3×303×1 03×2n], (7)

E. Construc¸˜ao do mapa

A construção do mapa é executada a partir dos dados obtidos através de um sonar mecânico de imagem (MSIS). Estes dados são processados pelo módulo de extração de fea-tures. Uma vez que seja identificada uma feature significativa, os valores correspondentes são enviados para o módulo de associação de dados que tem a tarefa de identificar se uma observação corresponde a uma das features do mapa ou se constitui uma nova.

1) Associação de Dados: Para resolver o problema de associação de dados deve-se determinar se a observação aferida zi corresponde a alguma das features F j, j = 1...n que já

existem no mapa e deve ser usada para atualizar o sistema ou se é nova e deve ser incorporada no mapa. O resultado do processo de associação de dados é uma hipótese H = ji

associando a medição z_iV com uma das features do mapa Fj. Caso zVi não tenha correspondência com alguma feature

do mapa isto ´e indicado com ji = 0. Encontrar a hip´otese

correta é um processo envolvendo a análise da discrepância entre a medição da observação atual e sua predição. Essa predição é obtida da função de medida não linear hj, a qual

relata a medição i com o vetor de estados µ(k) contendo as localizações do ve´ıculo e a feature j.

ziV(k) = pV i θV i = pj− xcosθj− ysinθj θj− ψ + si, (8)

onde si, ´e o ru´ıdo branco gaussiano com m´edia zero

afe-tando a observação da feature. Para se calcular a discrepância entre a medição e sua predição, o termo de inovação vij e sua

matriz de covariˆancia associada Sij ´e obtida como:

vij(t) = ziV(t) − hj(ˆµ(t|t − 1)), (9)

Sij(t) = Hj(t)Σ(t|t − 1)H(t)Tj + Ri, (10)

onde Hj representa a matriz Jacobiana que lineariza a

função de medição não-linear hj sobre a melhor estimativa

dispon´ıvel do estado ˆµ(k|k − 1).

Para determinar se a correspondência é válida, um teste individual de compatibilidade (IC) usando a distância de Mahalanobis é executado:

D2ij = vij(k)TSij(t)−1vij(t) < Xd2, α, (11)

onde d = dim(hj) e α ´e o desejado n´ıvel de confianc¸a.

´

E poss´ıvel para uma hipótese múltipla relatar a medição com diferentes features para satisfazer o teste de IC. Então para selecionar o melhor candidato, o critério de Nearest Neighbor (NN) é aplicado.

Após isso, a correspondência ser decidida, esta é usada para atualizar a estimativa do estado através das equações de atualização sensorial do EKF.

No caso de não existir nenhuma hipótese de relatar a medição da linha com alguma feature já existente no mapa, esta pode ser adicionada no sistema.

VI. RESULTADOS

A Figura 4 apresenta os resultados obtidos com o sistema de SLAM proposto. O trajeto com uma linha cont´ınua em azul foi plotado com base nos dados de DGPS, o trajeto com uma linha traçada e pontilhada em vermelho foi gerado com o sistema de SLAM e a trajetória da linha traçada em verde foi gerada apenas com os dados sensoriais de dead-reckoning. Nos primeiros metros da trajetória, nota-se uma grande de-pendência do sistema de SLAM aos dados de dead-reckoning. Ao tempo em que o sistema inicia a fazer a correspondência de features observadas em um tempo anterior e corrigir a estimativa do trajeto, nota-se que a odometria tem um drift acentuado. Esse deslocamento leva ao aumento do erro de odometria enquanto o erro do sistema de SLAM é minimizado devido a correção executada pelo módulo de associação de dados. Em torno da coordenada (-25,105) o robô executa um giro na direção oposta ao deslocamento (drift da odometria). Isso faz com que o erro de odometria seja compensado nos próximos metros, fator que explica a diminuição do erro no gráfico da Figura 5. Após isso, os dados de odometria sofrem um novo drift, aumentando de forma linear o erro ao longo do tempo, enquanto o sistema de SLAM têm a capacidade de

(6)

Fig. 4. Resultado do sistema de SLAM. A trajetória cont´ınua em azul foi obtida com os dados do DGPS e tem precisão de cerca de 2 m. A trajetória pontilhada em vermelho, foi gerada com o sistema de SLAM usando apenas dados dos sensores do ve´ıculo. O trajeto tracejado em verde foi gerado apenas com os sensores de dead-reckoning que fornecem dados de angulação e velocidade do robô. As features identificadas, são exibidas com o sinal ’+’ na cor magenta.

Fig. 5. Erro de posic¸˜ao do sistema de SLAM comparado ao erro gerado com os dados de dead-reckoning ao longo do tempo do experimento

compensar boa parte desse erro, gerando uma trajet´oria mais parecida com o trajeto real do robˆo.

Ao longo da trajetória de aproximadamente 600m o sis-tema de SLAM teve um bom desempenho. Mais precisamente, na coordenada X o erro médio normalizado do sistema foi de 3.08m, o maior erro foi de 10.66m e o erro final de 9.84m. Na coordenada Y o erro médio normalizado foi de 1.56m o maior erro foi de 9.69m e o erro final foi de 1.09m.

VII. CONCLUSAO˜

Este trabalho apresentou um estudo de tópicos como sen-sores e algoritmos correlatos à aplicações de SLAM, bem como, um levantamento de trabalhos relevantes da literatura, focando-se em ambientes subaquáticos. Foi proposto um sis-tema de SLAM aliado a uma nova técnica para extração de features de pontos oriundos de dados obtidos de um sonar mecânico de imagem (MSIS). O sistema de SLAM foi constru´ıdo tendo como núcleo um Filtro de Kalman Estendido (EKF), baseado em um modelo cinemático de aceleração para a predição de estados futuros, atualizações sensoriais de dead-reckoningatravés de sensores DVL e IMU. Para a construção do mapa foram utilizados dados do sonar junto a um módulo de

associação de dados responsável pela identificação de features já existente no mapa e correção de estimativas, ou criação de novas instâncias e extensão do estado e incerteza. Os resultados gerados pelo algoritmo de SLAM foram validados com os que foram produzidos pelo DGPS, que possui precisão de cerca de 2 m. A proposta mostrou um bom desempenho, onde o sistema foi capaz de reconhecer features e recuperar-se de erros de estimativa ao longo do trajeto percorrido pelo AUV.

REFERENCES

[1] H. Durrant-Whyte and T. Bailey, “Simultaneous localization and map-ping: part i,” IEEE Robotics and Automation Magazine, vol. 13, no. 2, pp. 99–110, 2006.

[2] M. Walter, F. Hover, and J. Leonard, “Slam for ship hull inspection using exactly sparse extended information filters,” in ICRA, 2008, pp. 1463–1470.

[3] S. Williams and I. Mahon, “Simultaneous localisation and mapping on the great barrier reef,” in ICRA, vol. 2, 2004, pp. 1771–1776. [4] P. Newman and J. Leonard, “Pure range-only sub-sea slam,” in ICRA,

2003, pp. 1921–1926.

[5] D. Ribas, P. Ridao, J. Tardos, and J. Neira, “Underwater slam in man-made structured environments,” Journal of Field Robotics, vol. 25, no. 11-12, pp. 898–921, 2008.

[6] N. Fairfield, D. Jonak, G. Kantor, and D. Wettergreen, “Field results of the control, navigation, and mapping systems of a hovering auv,” in UUST, 2007, pp. 20–22.

[7] A. Mallios, P. Ridao, D. Ribas, and E. Hern´andez, “Scan matching slam in underwater environments,” Autonomous Robots, pp. 1–18, 2013. [8] L. Silveira, F. Guth, D. Fischer, P. Drews-Jr., and S. Botelho, “3D

robotic mapping: A biologic approach,” in IEEE ICAR, 2013. [9] A. Kim and R. Eustice, “Real-time visual slam for autonomous

un-derwater hull inspection using visual saliency.” IEEE Transactions on Robotics, vol. 29, no. 3, pp. 719–733, 2013.

[10] R. C. Smith and P. Cheeseman, “On the representation and estimation of spatial uncertainly,” IJRR, vol. 5, no. 4, pp. 56–68, 1986. [11] S. Thrun, W. Burgard, and D. Fox, Probabilistic Robotics (Intelligent

Robotics and Autonomous Agents). The MIT Press, 2005.

[12] S. J. Julier and J. K. Uhlmann, “A new extension of the kalman filter to nonlinear systems,” 1997, pp. 182–193.

[13] A. Stentz, D. Fox, M. Montemerlo, and M. Montemerlo, “Fastslam: A factored solution to the simultaneous localization and mapping problem with unknown data association,” pp. 593–598, 2003.

[14] S. Thrun and M. Montemerlo, “The graphslam algorithm with applica-tions to large-scale mapping of urban structures,” IJRR, vol. 25, no. 5, pp. 403–430, 2006.

[15] S. Thrun, “Simultaneous localization and mapping,” Robotics and Cognitive Approaches to Spatial Mapping, pp. 13–41, 2008. [16] ——, “Simultaneous mapping and localization with sparse extended

information filters: Theory and initial results,” 2002.

[17] S. Williams, P. Newman, G. Dissanayake, and H. Durrant-Whyte, “Autonomous underwater simultaneous localisation and map building,” in ICRA, vol. 2, 2000, pp. 1793–1798.

[18] C. N. Roman and H. Singh, “Improved vehicle based multibeam bathymetry using sub-maps and slam.” in IROS, 2005, pp. 3662–3669. [19] N. Fairfield, G. Kantor, and D. Wettergreen, “Real-time slam with octree evidence grids for exploration in underwater tunnels,” Journal of Field Robotics, vol. 24, no. 1-2, pp. 03–21, 2007.

[20] J. J. Leonard and H. F. Durrant-Whyte, “Directed sonar sensing for mobile robot navigation,” 1992.

[21] J. D. Tardos, J. Neira, P. M. Newman, and J. J. Leonard, “Robust mapping and localization in indoor environments using sonar data,” Int. J. Robotics Research, vol. 21, pp. 311–330, 2002.