Plataforma robótica genérica para robô de logística, serviços ou vigilância com mecanismo de troca automática da bateria

FACULDADE DE ENGENHARIA DA UNIVERSIDADE DO PORTO Plataforma robótica genérica para robô de logística, serviços ou vigilância com mecanismo de troca automática de bateria Ivo Emanuel Milheiro de Sousa Mestrado Integrado em Engenharia Eletrotécnica e de Computadores Orientador: Prof. Dr. António Paulo Moreira Coorientador: Eng. Héber Miguel Sobreira 24 de Julho de 2016 c Ivo Sousa, 2016 Resumo O desenvolvimento e a programação de plataformas robóticas genéricas, de baixo custo e com a capacidade para operarem, de forma robusta, em diversos tipos de ambientes e por longos períodos de tempo é um desafio exigente. Assim, esta dissertação tem como principal objetivo a integração e o desenvolvimento de software de controlo, monitorização, localização e tomada de decisões para uma plataforma robótica móvel genérica, com vista a ser utilizada em vários tipos de aplicações, desde a logística, vigilância, publicidade, etc. Entre estes, o foco principal passa pelo estudo e otimização dos seus algoritmos de localização, pois é uma área crucial nos robôs móveis e torna-se um desafio quando aplicado a robôs com limitações de hardware e destinados a operar em ambientes dinâmicos. Com isto em vista, realizaram-se análises comparativas entre os dois mais promissores algoritmos de localização (AMCL e Perfect Match) para o tipo de aplicações já referidas, na sua implementação em ROS. Em adição, investigou-se e desenvolveu-se uma solução de supervisão destes algoritmos de localização, permitindo a sua coexistência e a deteção de situações de falha. Numa outra vertente, a autonomia de um robô e a sua capacidade de estar sempre em operação é um fator muito importante. O tempo de inatividade durante o processo de carga das baterias e o facto de geralmente esta operação estar dependente de um operador humano é um grave problema. É frequente o operador esquecer-se de colocar o robô em carga, provocando a sua saída inesperada de serviço. Por outro lado, baterias descarregadas por largos períodos de tempo acabam por degradar-se e perder capacidade de carga. Assim, para contornar estes problemas criaram-se ferramentas no sentido de integrar um sistema de troca automática de baterias, através do controlo de quatro servomotores juntamente com um mecanismo que permite um encaixe automático das mesmas. i ii Abstract The development and programming of generic robotic platforms, with low cost and the ability to operate robustly in various types of environments and for long periods is a demanding challenge. Thus, this dissertation has as main objective the integration and development of control, monitoring, localization and decision-making software, for a generic mobile robotic platform in order to be used in a variety of applications from logistics, surveillance, advertising, among others. Between them, the main focus is the study and optimization of the localization algorithms, as it is a crucial area in mobile robots and becomes a challenge when applied to robots with limited hardware and designed to operate in dynamic environments. With this in mind, were held comparative analysis of the two most promising localization algorithms (AMCL and Perfect Match) for the type of applications mentioned above, in its ROS implementation. In addition, it was investigated and developed a supervision solution of these localization algorithms, allowing their coexistence and detection of failure situations. In another aspect, the autonomy of a robot and its ability to be always in operation is a very important factor. The downtime during the batteries charging process and that this operation is generally dependent on a human operator is a serious problem. Often the operator forgets to put the robot in charge, causing its unexpected departure service. On the other hand, discharged batteries for extended periods of time eventually degrade and lose its charge capacity. So, to get around these problems have been created tools in order to integrate an automatic batteries exchange system, through the control of four servomotor together with a mechanism that allows an automatic docking of the batteries. iii iv Agradecimentos Inicialmente gostaria de agradecer à empresa CLEVERHOUSE, em particular ao Eng. Jorge Paiva pelo entusiástico apoio a este projeto, fornecendo o robô e todo o equipamento necessário para o desenvolvimento e testes efetuados. Queria também agradecer ao Dr. Paulo Farias pela ajuda e contribuição em várias partes deste projeto, através da partilha de conhecimento e parceria na resolução de problemas que foram surgindo. Ao Sr. Fernando Guedes e ao Jorge Barbosa um enorme obrigado pelo auxílio na integração do hardware do robô, dando uma ajuda indispensável para o sucesso deste projeto. Um obrigado a toda a equipa do INESCTEC pela ajuda neste período. Quero agradecer especificamente ao Héber Sobreira, pelo constante auxílio na execução deste projeto, contribuindo com todo o seu conhecimento e dedicação para que esta dissertação terminasse com sucesso. Um agradecimento especial ao meu orientador António Paulo Moreira, pelo incansável acompanhamento, apoio, contribuição e enorme disponibilidade para este projeto de dissertação e para o autor deste, tendo um papel verdadeiramente fundamental para que tudo corresse da melhor maneira possível. Aos meus amigos no geral, mas especialmente do laboratório de robótica, um grande obrigado pelo apoio e pelos momentos de diversão e descontração, tornando a execução deste projeto um processo muito mais agradável. Por fim, um enorme agradecimento aos meus pais e familiares pelo apoio contínuo ao longo destes anos, pois sem eles isto não seria possível de alcançar. Ivo Sousa v vi Conteúdo 1 Introdução 1 1.1 Contexto....................................... 1 1.2 MotivaçãoeObjetivos ............................... 1 1.3 Contribuiçõesinovadoras.............................. 2 1.4 Estruturadadissertação............................... 3 2 Revisão bibliográfica 5 2.1 Métodosdelocalização............................... 5 2.1.1 Métodos de localização relativa . . . . . . . . . . . . . . . . . . . . . . 5 2.1.2 Localização baseada em marcos e balizas . . . . . . . . . . . . . . . . . 6 2.1.3 Métodos de localização probabilística . . . . . . . . . . . . . . . . . . . 8 2.1.4 Métodos de ajuste de mapa . . . . . . . . . . . . . . . . . . . . . . . . . 14 2.2 Visão geral dos métodos de localização para aplicação em ambientes dinâmicos . 18 2.3 Sistemas para carregamento ou troca de baterias . . . . . . . . . . . . . . . . . . 19 2.4 Conclusões ..................................... 21 3 Robô Clever 23 3.1 Hardware ...................................... 23 3.1.1 Baterias................................... 23 3.1.2 Motores e encoders ............................. 25 3.1.3 Servomotores................................ 26 3.1.4 Conversores Step-Down .......................... 28 3.1.5 Laser .................................... 29 3.1.6 Arduino................................... 30 3.1.7 SonareseLEDS .............................. 31 3.1.8 Computador ................................ 32 3.1.9 Sistema para logística . . . . . . . . . . . . . . . . . . . . . . . . . . . 33 3.2 Software ROS .................................... 35 3.2.1 Nós..................................... 35 3.2.2 Tópicos................................... 36 3.2.3 Serviços................................... 36 3.2.4 Ações.................................... 36 3.2.5 Arquiteturageral.............................. 37 3.2.6 Descrição dos nós de ROS . . . . . . . . . . . . . . . . . . . . . . . . . 39 3.2.7 Criaçãodemapas.............................. 51 3.3 Aplicações da plataforma robótica . . . . . . . . . . . . . . . . . . . . . . . . . 52 vii xiv LISTA DE FIGURAS 5.66 Evolução do erro de distância das estimativas calculadas pelo AMCL (vermelho) e pelo Perfect Match (azul), ao longo do percurso do robô num ambiente com muitosobstáculos................................... 139 5.67 Evolução do erro angular das estimativas calculadas pelo AMCL (vermelho) e pelo Perfect Match (azul), ao longo do percurso do robô num ambiente com muitos obstáculos. ..................................... 139 5.68 (a) Robô inicialmente bem localizado, tanto pela estimativa do AMCL (seta amarela), como pela estimativa do Perfect Match (seta azul), em relação ao groundtruth (seta verde). (b) Primeiro local onde se forçou o erro de odometria, com o consequente aumento do erro por parte dos dois algoritmos. (c) Segunda zona onde a odometria devolveu valores errados, aumentando de novo os erros da estimativas, após primeira recuperação. (d) Ambos os algoritmos de localização conseguiram voltar a recuperar a posição correta. . . . . . . . . . . . . . . . . . 140 5.69 Evolução do erro de distância das estimativas calculadas pelo AMCL (vermelho) e pelo Perfect Match (azul), ao longo do percurso do robô com dois momentos de erros forçados de odometria. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 141 5.70 Evolução do erro angular das estimativas calculadas pelo AMCL (vermelho) e pelo Perfect Match (azul), ao longo do percurso do robô com dois momentos de erros forçados de odometria. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 141 5.71 Diagrama de classes UML do programa supervisor dos algoritmos de localização, desenvolvido em C++ e aplicado em ROS. . . . . . . . . . . . . . . . . . . . . . 145 Lista de Tabelas 3.1 Especificações das baterias KB1272F1 . . . . . . . . . . . . . . . . . . . . . . . 24 3.2 Especificações dos motores e encoders ....................... 25 3.3 Conetores para a alimentação elétrica dos motores . . . . . . . . . . . . . . . . . 26 3.4 Conexões dos encoders ............................... 26 3.5 Especificações dos drivers MD49. ......................... 26 3.6 Especificações dos servomotores AX-12. . . . . . . . . . . . . . . . . . . . . . . 27 3.7 Especificações do conversor LM2575HV. . . . . . . . . . . . . . . . . . . . . . 28 3.8 Especificações do conversor MAX724. . . . . . . . . . . . . . . . . . . . . . . . 29 3.9 Especificações do laser URG-04LX-UG01. . . . . . . . . . . . . . . . . . . . . 30 3.10 Especificações dos sonares SRF01. . . . . . . . . . . . . . . . . . . . . . . . . . 31 3.11 Especificações do processador Atom D400. . . . . . . . . . . . . . . . . . . . . 32 5.1 Parâmetros comuns a todas as configurações desta secção. . . . . . . . . . . . . 66 5.2 Definição da Config1_OF e da Config2_OF_F1F2. ................ 67 5.3 Resultados estatísticos da Config1_OF eConfig2_OF_F1F2, para um ambiente sem outliers...................................... 67 5.4 Definição da Config3_SO e da Config4_SO_F1F2. ................ 71 5.5 Resultados estatísticos da Config3_SO eConfig4_SO_F1F2, na ausência de outliers.71 5.6 Definição da Config5_OF_F1F2WD......................... 74 5.7 Resultados estatísticos da Config5_OF_F1F2WD, na ausência de outliers..... 74 5.8 Resultados estatísticos da Config1_OF, com alguns obstáculos no mapa. . . . . . 76 5.9 Resultados estatísticos da Config2_OF_F1F2, com alguns obstáculos no mapa. . 79 5.10 Definição da Config6_OF_F2. ........................... 80 5.11 Resultados estatísticos da Config6_OF_F2, com alguns obstáculos no mapa. . . . 80 5.12 Definição da Config7_OF_F1. ........................... 83 5.13 Resultados estatísticos da Config7_OF_F1, com alguns obstáculos no mapa. . . . 84 5.14 Resultados estatísticos das Config2_OF_F1F2 eConfig5_OF_F1F2WD, com grande quantidade de obstáculos no mapa. . . . . . . . . . . . . . . . . . . . . . . . . . 87 5.15 Resultados estatísticos das Config2_OF_F1F2 eConfig5_OF_F1F2WD, em condições de falhas da odometria. . . . . . . . . . . . . . . . . . . . . . . . . . . . 93 5.16 Parâmetros comuns a todas as configurações do algoritmo AMCL que serão utilizadasnestasecção. ................................. 106 5.17 Definição da Config1_OA_001 eConfig2_OA_02.................. 108 5.18 Resultados estatísticos das Config1_OA_001 eConfig2_OA_02, num ambiente sem outliers...................................... 108 5.19 Definição da Config3_OA_05 eConfig4_OA_05L.................. 111 5.20 Resultados estatísticos das Config3_OA_05 eConfig4_OA_05L, com grande quantidade de obstáculos no mapa. . . . . . . . . . . . . . . . . . . . . . . . . . . . 111 xv xvi LISTA DE TABELAS 5.21 Resultados estatísticos das Config1_OA_001,Config2_OA_02 eConfig3_OA_05, com erros forçados de odometria, em dois momentos. . . . . . . . . . . . . . . . 116 5.22 Resultados estatísticos das Config1_OA_001,Config2_OA_02 eConfig4_OA_05L, num ambiente com alguns obstáculos. . . . . . . . . . . . . . . . . . . . . . . . 119 5.23 Resultados estatísticos das Config1_OA_001 eConfig2_OA_02, num ambiente com grande número de obstáculos. . . . . . . . . . . . . . . . . . . . . . . . . . 123 5.24 Resultados estatísticos das Config3_OA_05 eConfig4_OA_05L, num ambiente com grande quantidade de outliers.......................... 126 5.25 Comparação estatística do desempenho do AMCL em função do número de partículas utilizadas para o ambiente sem outliers. ................... 127 5.26 Comparação estatística do desempenho do AMCL em função do número de partículas utilizadas, com erros de odometria. . . . . . . . . . . . . . . . . . . . . . . 127 5.27 Comparação estatística do desempenho do AMCL em função do número de partículas utilizadas para o ambiente com poucos outliers................ 127 5.28 Comparação estatística do desempenho do AMCL em função do número de partículas utilizadas para o ambiente com muitos outliers. ............... 128 5.29 Tempos de ciclo médio, máximo e mínimo para o AMCL (com 300 partículas) e paraoPerfectMatch................................. 131 5.30 Tempos de ciclo médio, máximo e mínimo para o AMCL (com 200 partículas) e paraoPerfectMatch................................. 132 5.31 Tempos de ciclo médio, máximo e mínimo para o AMCL (com 100 partículas) e paraoPerfectMatch................................. 133 5.32 Tempos de ciclo médio, máximo e mínimo para o AMCL (com 50 partículas) e paraoPerfectMatch................................. 133 5.33 Resultados estatísticos das estimativas do AMCL e do Perfect Match (PM), num ambiente livre de obstáculos. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 136 5.34 Resultados estatísticos das estimativas do AMCL e do Perfect Match (PM), num ambiente com alguns outliers. ........................... 136 5.35 Resultados estatísticos das estimativas do AMCL e do Perfect Match (PM), num ambiente com grande quantidade de outliers. ................... 139 5.36 Resultados estatísticos das estimativas do AMCL e do Perfect Match (PM), perante erros forçados de odometria. . . . . . . . . . . . . . . . . . . . . . . . . . 141 Abreviaturas e Símbolos AGV Automated Guided Vehicle AMCL Adaptive Monte Carlo Localization API Application Programming Interface DC Direct Current EKF Extended Kalman Filter FEUP Faculdade de Engenharia da Universidade do Porto GUI Graphical User Interface IMU Inertial Measurement Units INESCTEC Instituto de Engenharia de Sistemas e Computadores - Tecnologia e Ciência IP Internet Protocol KF Kalman Filter KLD Kullback–Leibler Divergence LED Light Emitting Diode PC Personal Computer PM Perfect Match MCL Monte Carlo Localization RAM Random Access Memory RPROP Resilient Backpropagation ROS Robot Operating System SLAM Simultaneous Localization and Mapping TCP Transmission Control Protocol UDP User Datagram Protocol UKF Unscented Kalman Filter USB Universal Serial Bus XML-RPC Extensible Markup Language Remote Procedure Call xvii Capítulo 1 Introdução 1.1 Contexto A utilização de robôs em aplicações de vigilância, publicidade, entretenimento, logística, guia de museus ou outro tipo de serviços ainda tem alguns entraves. Estes podem começar no custo do robô (quer de compra, quer de manutenção, configuração e colocação em serviço), estar relacionados com a sua baixa flexibilidade (capacidade de ser utilizado em diferentes aplicações), mas também pela baixa autonomia e necessidade de períodos longos de carga da bateria durante os quais está inoperacional [6,7,8]. 1.2 Motivação e Objetivos Nesta dissertação pretende-se contribuir para o desenvolvimento de uma plataforma móvel robotizada que responda aos pontos focados anteriormente. Nesse sentido será desenvolvido um sistema que possibilite ao robô a mudança de bateria de uma forma automática, ou seja, sem a intervenção de nenhum operador, e sempre que o robô detete que a carga da bateria está a chegar ao fim. Desta forma poderá operar continuamente e sem tempos de paragem. Por outro lado, pretende-se desenvolver um software de configuração do sistema (e.g., autolocalização, navegação, ações a tomar, etc.) que possibilite a utilização da base do robô em diferentes aplicações que podem passar pela logística, publicidade, guia, vigilância ou outras. Para este tipo de aplicações é necessário que o robô possua métodos de localização destinados principalmente a ambientes dinâmicos, nos quais há um acréscimo na dificuldade de auto-localização e uma necessidade adicional da robustez dos algoritmos. Isto é devido essencialmente ao elevado número de pessoas que partilham a mesma área do robô e às constantes mudanças na localização da mobília (e.g., cadeiras, mesas) no ambiente de ação; perturbando os dados adquiridos pelos sensores do robô e, consequentemente, havendo uma maior incerteza da sua auto-localização [2]. Adicionalmente, pretende-se utilizar métodos de localização que aproveitem preferencialmente as características naturais do ambiente em que o robô se encontra, sem a implementação de estruturas artificiais que, apesar de poderem facilitar a sua auto-localização, podem tornar-se pouco 1 2Introdução flexíveis para os tipos de aplicações pretendidas [9,10]. Para além do que já foi referido, o foco desta dissertação passará pela utilização de robôs relativamente baixos, possuindo a capacidade de se movimentar por baixo de algumas peças de mobília (e.g., mesas, prateleiras, cadeiras). Consequentemente, todos os seus sensores também estarão num nível baixo, podendo trazer algumas limitações acrescidas na perceção do meio envolvente. 1.3 Contribuições inovadoras Este projeto de dissertação focou-se especialmente no problema da auto-localização de pequenos robôs móveis, apenas com um laser frontal perto do chão e com um raio de deteção de cerca de 180o. Em adição, devido às sua pequenas dimensões e baixa altura não possuiu espaço suficiente para comportar uma maior quantidade de baterias ou baterias de maiores dimensões, tendo portanto de se utilizar um computador com baixo poder de processamento e baixo consumo energético, com vista a não comprometer a autonomia do robô. Assim, é de grande importância que os algoritmos de localização possuam o maior nível de otimização e fiabilidade possível, tendo-se por isso realizado estudos comparativos entre o algoritmo ”state of the art” do ROS, o Adaptive Monte Carlo Localization (AMCL) [2,11,12,13], muito popular e largamente utilizado pela comunidade científica, mas que se torna computacionalmente pesado e com tempos de ciclo elevados quando utiliza um elevado número de partículas e o Perfect Match, que em contrapartida é um algoritmo leve e rápido, inicialmente criado para ser utilizado no futebol robótico [3], mas que depois foi generalizado para outros tipos de aplicações [4,5]. Neste campo, os problemas que advêm destas características físicas e de equipamento não estão resolvidos de forma comprovadamente robusta. Os casos documentados até à data utilizam um maior número de sensores ou com ângulos de visão superiores (geralmente 360o) [14], por norma a alturas superiores, longe do chão, ou utilizam computadores mais poderosos, permitindo recorrer a um elevado número de partículas, no caso da utilização do filtro de partículas, de modo a conseguir-se uma solução minimamente robusta para o problema da auto-localização. Com isto em vista, estudou-se e desenvolveu-se um algoritmo de fusão e supervisão de diferentes métodos de localização, executados em paralelo ao longo da movimentação do robô, cujo objetivo é detetar situações anómalas: excesso de outliers e início de perda de localização, resultando num sistema de localização robusto, fiável e preciso para as circunstâncias anteriormente referidas (limitações da estrutura e do hardware do robô). Ao comparar o output dado por cada um destes algoritmos e validá-los entre si, juntamente com outros mecanismos de localização relativa (e.g., odometria, IMUs), é possível detetar situações anómalas e corrigi-las. A integração de outro método baseado num paradigma diferente que possui outro tipo de características e pontos fortes, em adição com um processo de supervisão que garanta a sua coexistência e detecte possíveis erros é uma ideia promissora e que merece ser explorada. Noutro campo, criou-se ferramentas com vista à implementação de um mecanismo de troca automática de baterias para este tipo de robôs móveis: de pequenas dimensões e para aplicação em serviços, vigilância, publicidade, etc. Geralmente, os mecanismos de troca automática de bateria 1.4 Estrutura da dissertação 3 só são atualmente aplicados a AGVs industriais, sendo esta uma solução pouco difundida e pouco utilizada por outros tipos de robôs móveis. Para este caso em concreto e para grande parte de outras aplicações, é de grande relevância que os robôs consigam operar autónoma e continuamente, sem a necessidade da intervenção de operadores, ou tempos de paragem para o carregamento das baterias. 1.4 Estrutura da dissertação Esta dissertação está organizada em 7 capítulos. No Capítulo 2fez-se uma análise geral dos métodos de localização existentes, apresentando-se também algumas soluções relacionadas com o carregamento e troca automática de baterias aplicadas em robôs móveis. No Capítulo 3fezse uma descrição detalhada de todo o hardware esoftware utilizado e desenvolvido para o robô Clever, assim como algumas das possíveis aplicações desta plataforma robótica. No Capítulo 4 apresentou-se as ferramentas desenvolvidas para o sistema de troca automática de baterias. O Capítulo 5focou-se na comparação entre dois algoritmos de localização: AMCL e Perfect Match. Por fim, o Capítulo 6expõe as conclusões desta dissertação e no Capítulo 7estão apresentadas algumas sugestões para trabalho futuro. 4Introdução Capítulo 2 Revisão bibliográfica 2.1 Métodos de localização A auto-localização em robôs móveis exprime um papel fundamental na sua autonomia, interação e desempenho das tarefas a eles destinadas. Especialmente em robôs de serviços é importante que se localizem de forma robusta e com flexibilidade para lidar com situações inesperadas . Por outro lado, um fator chave para o sucesso passa por uma interação apropriada com os objetos e pessoas presentes no ambiente em que se encontram. Os métodos de localização podem basear-se na localização relativa, a partir de uma determinada posição inicial conhecida ou, por outro lado, podem destinar-se a localizar o robô no referencial global do mapa. Numa outra vertente, os métodos de localização probabilísticos podem estimar tanto a posição local do robô num determinado subespaço do mapa, como a sua posição absoluta. Para a aplicação de robôs destinados a interagir em ambientes dinâmicos, focar-se-á a análise essencialmente nos métodos de localização probabilística, de ajuste de mapa e nas técnicas mais comuns da localização relativa, pois estas últimas têm sempre um papel importante e complementar em conjugação com os outros tipos de métodos. 2.1.1 Métodos de localização relativa As técnicas utilizadas para a localização relativa baseiam-se na determinação da posição e orientação do robô, de forma incremental, a partir de um determinado ponto inicial. Este método utiliza sensores a bordo do robô, para receber informações internas, sendo principalmente utilizados encoders, acelerómetros, giroscópios, etc. Como estes tipos de técnicas não utilizam observações do ambiente, mas apenas sensores internos ao robô (interocetivos), as estimativas realizadas terão sempre erros acumulados. Por norma, devido às suas limitações, estes métodos são integrados com técnicas de localização exterocetivas. 2.1.1.1 Odometria e Dead Reckoning Quando se pretende obter uma estimativa da posição relativa do robô através de sensores intorecetivos, são utilizadas essencialmente a odometria e o dead reckoning. A odometria é a solução 5 12 Revisão bibliográfica –Para cada partícula, retira-se uma amostra si tde St, de ns0i t,wi to. Obtém-se então Stapós a amostragem do conjunto pesado ns0i t,wi to. •Por fim, entra-se na fase de re-amostragem, que elimina as partículas com um peso abaixo de um determinado limiar e cria novas amostras perto das que têm maior probabilidade. Para a inicialização do filtro, é necessário atribuir um conjunto aleatório S0inicial, a partir da probabilidade p(x0). 2.1.3.3 Melhoramentos do algoritmo MCL Random Particle MCL Como foi exposto anteriormente, o algoritmo MCL regular possuiu algumas limitações que, para algumas aplicações, não são desprezáveis. Uma dessas limitações está relacionada com o evento de rapto, i.e., alterar manualmente a posição do robô enquanto este está em funcionamento, perturbando gravemente o processo de auto localização devido a uma “teleportação” inesperada. Tendo em conta que o algoritmo MCL regular concentra gradualmente as suas partículas na posição com maior probabilidade para a localização do robô, verifica-se que as outras zonas deixarão completamente de ter partículas “sobreviventes”, tornando então impossível a sua recuperação, no caso de rapto. Este comportamento é agravado nas situações em que há uma grande área para a distribuição das partículas e no caso de se utilizar um número reduzido de partículas para o algoritmo [2]. Com o objetivo de colmatar estas limitações, utilizam-se algumas técnicas heurísticas simples. Na base do algoritmo Random Particle MCL está a ideia da injeção de partículas de forma aleatória com uma distribuição relacionada com a precisão da estimação da localização. Essa relação é atingida a partir da monitorização da probabilidade das medidas dos sensores, relacionando-a com a probabilidade média das medidas. Deste modo, focalizam-se os esforços computacionais nas regiões que realmente importam, evitando o sobrecarregamento desses recursos disponíveis [2]. KLD-Sampling: Adaptive Particle Filter Esta abordagem foi desenvolvida como forma de aumentar a eficiência e diminuir os esforços computacionais da utilização dos filtros de partículas. A utilização de um número constante de amostras desde o início do algoritmo pode tornar-se desajustado e ineficiente, visto que a complexidade das densidades de probabilidade pode alterar-se consideravelmente ao longo do tempo [12]. Durante a localização global, o robô desconhece em completo a sua posição atual, por isso, as possibilidades da sua localização cobrem uniformemente todo o espaço. Por outro lado, na fase de seguimento de posição, a incerteza é geralmente pequena e frequentemente focada em espaços de dimensões inferiores. Assim, são necessárias mais amostras durante a localização global, de forma a ser possível aproximar a densidade real com maior precisão, das que são necessárias apenas para o seguimento de posição [25]. Baseando-se neste princípio, aplica-se um método cuja ideia fundamental passa pela limitação do erro introduzido pela representação à base de amostras do filtro, utilizando-se um número reduzido de partículas se a densidade estiver concentrada numa pequena área do espaço, e aumentando-se o número de 2.1 Métodos de localização 13 amostras no caso da incerteza em relação ao estado ser elevada. Este algoritmo apresenta uma baixa complexidade de implementação e um mais reduzido peso computacional [12]. Mixture-MCL Esta técnica surgiu para compensar as falhas suscetíveis de acontecer no caso de se utilizarem sensores com pouco ruído, ou no limite, sem ruído. Uma das possíveis soluções para este problema seria aumentar artificialmente a dimensão de ruído dos sensores, podendo ser interpretada como a incerteza intrínseca à natureza aproximada do filtro de partículas [25]. Por outro lado, pode optar-se por uma solução mais consistente, em que se altera o processo de amostragem de uma fração das partículas [2,11]. Basicamente, esta técnica é a combinação entre uma amostragem regular e o oposto do MCL, invertendo o processo de amostragem. Enquanto os algoritmos MCL básicos primeiro preveem uma nova posição usando odometria e em seguida utilizam as medições do sensor para avaliar a relevância dessa amostra, o Mixture-MCL estima a posição usando as medições mais recentes dos sensores e, em seguida, utiliza a odometria para avaliar a conformidade deste palpite com a anterior confiança do robô, tendo também em conta os dados anteriores da odometria. O objetivo deste algoritmo é incorporar duas metodologias que em combinação se complementam e apresentam bons resultados. No entanto, este método só por si poderá ser ineficiente uma vez que a geração de partículas é realizada ignorando toda a história, i.e., utilizando apenas as mais recentes informações dos sensores. Adicionalmente, o cálculo das duas principais fases do algoritmo (geração de partículas através do modelo de medição e o cálculo do peso das partículas) pode ser difícil de realizar, sendo necessário recorrer a métodos de aproximação (e.g.,density trees) e, consequentemente, aumentar a complexidade da sua implementação. Esta técnica consegue colmatar algumas das desvantagens da utilização de algoritmos MCL básicos, pois consegue estimar de forma bem-sucedida a posição do robô com um conjunto de amostras pequena, recupera rapidamente de raptos e tem um bom funcionamento no caso de os sensores serem demasiado precisos [2]. 2.1.3.4 Filtros de Kalman O filtro de Kalman (KF) [26] é um algoritmo recursivo baseado nos filtros gaussianos, cuja ideia principal é a de que a confiança num determinado conjunto de valores é representada por uma distribuição normal constituída por múltiplas variáveis [2]. Em cada instante t, a confiança é caracterizada por dois fatores: a média (µt) e a covariância (σt). Esta é uma das técnicas mais conhecidas e utilizadas para a fusão de informação, combinando uma fase de predição de estado com os dados recebidos dos sensores, calculando uma média ponderada cujo grau de certeza de cada valor está diretamente relacionada com a sua covariância [27]. O facto deste ser um algoritmo recursivo, i.e., só necessitar da estimativa obtida no ciclo anterior e não de todo o seu histórico temporal, torna-o numa técnica muito eficiente computacionalmente e facilmente aplicado em processos de tempo real [2,28,29]. O filtro de Kalman é um estimador ótimo, garantindo uma minimização do erro de estimação a partir da medição de grandezas ao longo do tempo, perturbadas por ruídos. De forma a operar no sentido da convergência para uma solução ótima é necessário 14 Revisão bibliográfica que o sistema seja linear e que os ruídos associados sejam sequências de ruído branco gaussiano, i.e., independentes e com média nula [30]. De forma a ultrapassar os problemas e as limitações do filtro de Kalman, visto que nos problemas reais raramente se verifica a premissa de que as medições e as transições de estados são funções lineares, surgiu o Extended Kalman Filter (EKF). Esta variante permite a expansão do filtro de Kalman para aplicações com sistemas não lineares, usando uma aproximação linear da estimativa atual, através da aplicação de derivadas parciais [2,31]. O EKF, devido à sua simplicidade de implementação e eficiência computacional, é das ferramentas mais populares para a resolução dos problemas de estimação de estados aplicados à robótica [2]. Contudo, esta técnica, ao contrário do KF não é, no geral, um estimador ótimo e possui limitações. A eficácia com que aproxima as soluções está relacionada tanto com o grau das não linearidades como com o grau de incerteza no estado. Assim, o EKF pode não garantir a convergência das estimativas ou até pode convergir para resultados errados [32]. Outra das principais variantes do KF é o Unscented Kalman Filter (UKF) [33] que foi desenvolvido como alternativa ao EKF, também para aplicações de sistemas não lineares. Este método não utiliza a mesma estratégia de linearização implementada no EKF. Em contrapartida, o algoritmo do UKF aplica uma função matemática denominada de unscented transform que utiliza um conjunto de pontos específicos para parametrizar as médias e as covariâncias das distribuições de probabilidade. Esta abordagem, teoricamente, apresenta melhores aproximações do que o EKF, sem a necessidade de calcular os Jacobianos das matrizes [2,33,34]. Ainda assim, nem sempre é a solução mais adequada para algumas aplicações [35,36,37]. 2.1.4 Métodos de ajuste de mapa 2.1.4.1 Perfect Match O Perfect Match é um algoritmo de auto-localização que surgiu como resposta a algumas dificuldades no processo da auto-localização, de uma forma rápida e pouco pesada computacionalmente, de robôs móveis, criado especificamente para ser aplicado no futebol robótico (“Robocup Midsize league”) [3], mas tendo o potencial de ser generalizado para outro tipo de ambientes (e aplicações) [4]. Como objetivos principais deste método, é necessário que o algoritmo resulte num processo de localização robusto, com uma estimação precisa da posição e que seja computacionalmente eficiente. Por outro lado, não são utilizados marcos artificiais para a localização global do robô, estando apenas limitado às características próprias do ambiente (e.g., linhas, paredes, etc). O Perfect Match faz a modelação do processo de localização como um problema de minimização de erros, utilizando um minimizador numérico eficiente – Resilient Propagation (RPROP). Outro fator importante é o de o mapa ser representado por uma grelha de ocupação, sendo possível calcular-se antecipadamente e uma única vez a matriz com as distâncias de cada célula aos obstáculos, assim como as respetivas matrizes de gradiente (na direção do x e do y) [5]. Desta forma, o tempo de processamento do algoritmo é reduzido consideravelmente - fator crucial para aplicações online. 2.1 Métodos de localização 15 (a) Estimação da posição e orientação do robô num campo de futebol robótico. (b) Representação da função do erro, com um mínimo global (zona clara) bem definido, indicando a posição estimada do robô. Figura 2.3: [3] Devido às características deste método, não é propriamente adequado para ser aplicado em ambientes amplos, com grandes áreas e em que não existam características locais que ajudem a reduzir a ambiguidade [24]. Como adaptação ao algoritmo original, para ser mais direcionado para aplicações em ambientes indoor e com outro tipo de sensores (laser range finder, sensores infravermelhos), foi proposto um método com alguns ajustes para melhor se adaptar à nova realidade, calculando-se, em cada ciclo do programa, um número fixo de alternativas adicionais (aleatórias) à melhor estimativa da posição atual do robô [4]. Após este processo, é feita a comparação entre as funções de custo das estimativas para poder selecionar-se a melhor. Por forma a evitar que a melhor estimativa esteja sempre a alterar-se a cada iteração, é colocado um requisito extra, garantindo que a melhor estimativa se mantém destacada durante um número fixo de iterações, e só nessas circunstâncias é que é adotada. Utilizando uma boa configuração para o método Perfect Match, obtêm-se resultados similares aos dos filtros de partículas com uma diminuição muito considerável dos recursos computacionais (e consequentemente do tempo de processamento). Adicionalmente, apesar de apresentar bons resultados atuando de forma isolada para a localização de robôs móveis, a integração (e.g., através do Filtro de Kalman) de outro tipo de medições (e.g., odometria) pode tornar o algoritmo mais robusto [5], especialmente em situações em que ocorram medidas com demasiado ruído ou em casos de maior ambiguidade. Adicionalmente, ao ser implementado um limiar de rejeição na função de custo deste algoritmo, é possível obter resultados mais robustos na rejeição de outliers [5]. 16 Revisão bibliográfica (a) Utilização apenas da odometria. (b) Utilização de 11 sensores infravermelhos. (c) Utilização de laser range finder considerando 13 pontos. (d) Utilização de laser range finder considerando apenas 7 pontos. Figura 2.4: Comparação entre a posição e a orientação real do robô (branco) com as estimativas (azul) [4]. 2.1 Métodos de localização 17 (a) Algoritmo Perfect Match original. (b) Falha de localização devido a um obstáculo externo ao mapa. (c) Algoritmo Perfect Match com rejeição de outliers. (d) Localização bem-sucedida. Figura 2.5: Localização do robô usando o algoritmo Perfect Match (os pontos vermelhos são utilizados para a localização) com e sem rejeição de outliers (pontos a verde) [5]. 18 Revisão bibliográfica 2.2 Visão geral dos métodos de localização para aplicação em ambientes dinâmicos Após a análise dos diferentes métodos e estratégias de localização de robôs móveis, é possível tirar algumas conclusões quanto à aplicabilidade em vários tipos de ambientes. Especificamente é importante inferir quais os algoritmos que mais se adequam a robôs destinados a participar em locais que tenham características que possam perturbar a auto-localização de robôs móveis (e.g., pessoas a passar e a posicionar-se à volta do robô, aparecimento de obstáculos inicialmente não previstos, etc.). Este tipo de algoritmos tem de apresentar uma elevada robustez, facilidade de adaptação e uma boa capacidade de distinção entre medições válidas e interferências externas ao ambiente envolvente. Adicionalmente, os métodos de localização não poderão ser baseados em características que impliquem uma alteração do ambiente (e.g., implementação de marcos artificiais, de balizas, etc.), mas apenas em características próprias do local. Os algoritmos baseados em filtro de partículas são bons candidatos a serem aplicados em ambientes dinâmicos, devido principalmente à sua grande flexibilidade e robustez na acomodação de vários tipos de ruído e boa adaptação a dinâmicas não modeladas, como é o caso. Contudo, é necessário realizar-se algumas extensões, visto que os métodos probabilísticos são implementados baseando-se no princípio de Markov, i.e., que o ambiente que rodeia o robô é estático. Para contornar o problema, integra-se uma fase de pré-processamento dos dados recebidos, por forma a ser possível filtrar os resultados que não se ajustam com a realidade esperada (outliers) [2]. Deste modo, obtém-se um algoritmo mais robusto com a capacidade de distinção entre as medidas esperadas (de acordo com o mapa do ambiente e da sua localização passada) e as informações causadas por perturbações externas, que de outro modo poderiam influenciar drasticamente a estimação da posição do robô. No campo dos métodos de ajuste de mapa, o algoritmo Perfect Match revela um grande potencial para ser aplicado em locais dinâmicos. Como já foi discutido anteriormente, com algumas adaptações ao algoritmo original [3] é possível utilizá-lo com diferentes tipos de sensores e em ambientes indoor gerais [4,5]. A aplicação do método SLAM para a geração de um mapa do ambiente e de um mapa baseado na movimentação dos humanos provou ser bem-sucedida, através da aquisição e processamento dos dados recebidos dos sensores, extraindo as informações referentes ao ambiente estático e dinâmico. Com esse objetivo, houve a integração de vários algoritmos cuja função é identificar e diferenciar algumas características geométricas humanas em oposição a estruturas que fazem parte do ambiente estático, permitindo acompanhar o movimento das pessoas [38]. Este tipo de técnicas aumenta muito a complexidade da estimação, pois não tem apenas de acompanhar a posição do robô, mas também o movimento de uma quantidade desconhecida de humanos que partilham o mesmo ambiente, sendo um processo desnecessário para grande parte das aplicações. 2.3 Sistemas para carregamento ou troca de baterias 19 2.3 Sistemas para carregamento ou troca de baterias Outro foco de interesse para ser analisado neste capítulo está relacionado com as limitações de autonomia dos sistemas de bateria utilizados pelos robôs móveis atuais e as possíveis soluções para os contornar. Atualmente, os robôs móveis são utilizados em diversos campos e aplicações, tais como: automação industrial, deteção de ambientes perigosos, automação de escritórios, serviços hospitalares, entretenimento, exploração do espaço, automação agrícola, automação na pesca, forças armadas, sistemas de segurança, educação, vigilância, automação de tarefas domésticas, guias de museus [39,7,8,40,6]. Um dos entraves na utilização de robôs móveis para este tipo de aplicações é o tempo de operação a que estes estão limitados, sendo obrigados a atuar em ciclos de funcionamento descontínuos. Isto está diretamente relacionado com a autonomia das suas baterias e com a dependência humana para o seu carregamento, ou troca [6]. A inclusão de baterias de alta capacidade traz associado um aumento do peso e das suas dimensões, sendo estas de grande porte. Consequentemente, leva a uma diminuição da mobilidade dos robôs e também de uma necessidade de aumento significativo das dimensões da estrutura do robô. Por estas razões, estes tipos de soluções não são utilizadas como resposta aos problemas de duração insuficiente da alimentação dos robôs [8]. Por outro lado, existem outros tipos de estratégia que resolvem ou minimizam alguns dos problemas anteriormente referidos: a utilização de estações de carregamento automático para os robôs, estações que permitam uma troca automática de baterias, ou carregamento contínuo sem fios (e.g., por indução [41]). A utilização de estações para o carregamento automático dos robôs fornece os meios para que estes não necessitem de intervenção humana nos momentos em que seja necessário trocar ou recarregar as suas baterias, aumentando a sua independência e autonomia [42]. Um sistema de carregamento foi desenvolvido em sincronismo com um sistema de teleoperação baseado na web que define e atribui remotamente tarefas para um robô [43]. O objetivo destes sistemas é garantir um ciclo de funcionamento que pode ser executado ao longo de todo o ano. Por forma a ser possível atingir esse objetivo, é necessário implementar um sistema de carregamento para o robô se tornar completamente autónomo. Este sistema possuiu uma baliza de infravermelhos, utilizada para a localização do carregador à distância; uma grelha de referência no meio da estação de carregamento, para ser detetada por laser quando o robô já estiver relativamente perto do carregador; e uma ficha elétrica abaixo da grelha de referência, onde o robô se encaixa. Quando o nível de bateria de um robô atinge um valor baixo durante sua operação, este perde a possibilidade de ser controlado por comandos e passa a existir o risco de algum evento perigoso acontecer. Com base nesta premissa, monitorizando o nível da bateria, é possível executar um processo de auto recarregamento em tempo real quando a carga desce abaixo de um determinado limiar. Assim, em [39] desenvolveu-se um mecanismo de deteção desse momento de funcionamento, que quando é ativado, faz o robô procurar pela estação de carregamento através da localização de um marco (integrado na estrutura da estação), usando um laser range finder. 20 Revisão bibliográfica Uma questão importante que se coloca está relacionada com a solução utilizada para guiar o robô apropriadamente até à estação, sendo que a sua estrutura e o método de encaixe também têm um papel fundamental. A estação fornece dois pontos de contacto para se conectar com a estrutura de conexão das baterias do robô móvel, através de um dispositivo auto-switch. Para além disso, de forma a direcionar a inserção dos pinos de carregamento, montou-se uma vara de guia na estação de acoplamento. Durante o carregamento da bateria, o robô móvel comunica com a estação, sabendo o estado da carga em tempo real. Numa outra vertente, para ser aplicada em robôs móveis destinados a interagir em ambientes domésticos, foi proposto um sistema que utiliza sensores infravermelhos de custos reduzidos, mas que pode operar numa ampla região, e uma estação com mecanismo de encaixe passivo com a capacidade para compensar possíveis erros de alinhamento e encaixe [40]. Quando o robô está localizado dentro da região limite da estação, o sistema direciona o robô para se posicionar imediatamente a frente da estação, para carregamento. Como é utilizada a força do robô para se acoplar, não existe a necessidade de a estação possuir elementos ativos, ou sensores. O sistema possui um transmissor e um recetor de infravermelhos, estando o transmissor localizado na parte da frente do robô, enquanto que o recetor está integrado no centro da estação. Especificamente para a aplicação em patrulha e segurança, foi concebido um sistema constituído por um robô móvel e uma estação de carregamento estacionária [7]. O robô tem uma câmara montada na sua parte superior, captando um sinal de vídeo que posteriormente é processado com vista a detetar um marco artificial (que indica a localização da estação de carregamento). Posteriormente, o sistema de visão orienta o movimento com a finalidade de alcançar o acoplamento. Quando o robô se aproxima da estação com uma orientação e um offset desajustados, a vara montada na estrutura serve como guia para garantir que os pinos são corretamente acoplados nos adaptadores. A estação possui dois grupos de pinos para o carregamento: um grupo localizado a um nível superior, responsável pelo carregamento das baterias do o computador e dos os circuitos de sensores; e outro grupo a um nível inferior, responsável pelo carregamento das baterias dos motores e dos drivers. Durante o carregamento, o robô está ligado por forma a acompanhar o processo e faz o desacoplamento quando este estiver concluído. Noutro estudo [8], utiliza-se o método SLAM para construir o mapa do ambiente de ação do robô, a priori, assim como a atribuição da localização específica da estação de acoplamento para o carregamento do robô, nesse mesmo mapa. Assim, quando o robô é confrontado com a situação de bateria fraca durante a execução de uma tarefa, o robô irá mover-se em direção à estação de forma autónoma, posteriormente seguindo o percurso guia construído com fita magnética, que está localizado na entrada da estação. No entanto, existe a possibilidade de o robô entrar mal alinhado na estação, devido aos erros acumulados ou aos erros de localização. Para contornar esse problema, no momento em que os braços da estação entram em contacto com o robô, o suporte móvel roda de forma a ficar alinhado com o robô, que é detetado por um sensor. Quando termina o acoplamento, os elétrodos da estação, em contacto com o robô, fornecem a energia necessária para que sejam evitados possíveis danos no robô, durante a troca de baterias. Assim, a bateria gasta é retirada, sendo colocada numa unidade de carga vazia, e uma bateria totalmente carregada 2.4 Conclusões 21 é fornecida ao robô. Por fim, os braços da estação soltam o robô, que volta a ficar operacional para continuar a executar as tarefas que lhe foram atribuídas. O processo de troca de bateria dura cerca de 45 segundos, não necessita que o robô se desligue e o mecanismo de acoplamento permite +- 9ode desalinhamento entre a estação e o robô móvel. Num outro artigo [44], foi apresentado um conceito de um sistema de troca de baterias “hotswap” contínuo, a partir da ideia da não necessidade de espera enquanto as baterias do robô são carregadas, sendo apenas necessário trocar a bateria descarregada por uma bateria totalmente carregada. Para ser possível a adaptação do conceito “hot-swap” com um robô móvel, é necessário configurá-los para ter a bateria do tipo de cartucho, com trinco mecânico e estrutura de junção. Adicionalmente a estação deverá ter a estrutura de inserção/remoção de baterias, necessitando de estruturas mecânicas adicionais tanto nas baterias como no robô, sendo possível fazê-lo com uma construção simples, baixo custo e com um tempo pequeno na troca. 2.4 Conclusões Neste capítulo analisou-se um conjunto de métodos de localização para robôs móveis, com especial foco nos algoritmos AMCL e Perfect Match, sendo estes os mais promissores para a aplicação concreta desta dissertação. Isto é devido essencialmente aos requisitos de utilização das características próprias do ambiente, sem a necessidade de colocação de objetos externos para facilitar o processo de localização dos robôs móveis, assim como algumas limitações de hardware existentes no robô utilizado para o presente projeto. O AMCL é um dos algoritmos mais utilizados para a auto-localização de robôs móveis aplicados nas circunstâncias descritas anteriormente, tendo revelado bons resultados práticos ao longo dos anos. Por outro lado, o Perfect Match é um algoritmo muito otimizado e com reduzido peso computacional, também provando ser robusto e eficaz em diversas aplicações. Numa outra vertente, depois de explorar alguns trabalhos relacionados com a autonomia das baterias nos robôs móveis concluiu-se que as soluções de troca de baterias ou são para robôs ainda aplicados em ambientes laboratoriais e/ou experimentais [8,44] ou são soluções industriais3,4 muito caras para a maioria das aplicações. Assim, existe a necessidade de uma nova solução de aplicação mais ampla. 3www.egeminusa.com/pages/battery_charging/agvs_battery_charging_autoswap.html 4www.ocme.it/products-and-technologies/machine-range/internal-logistics/agvlgv—automatedlaser-guidedvehicle/agv-battery-station/automatic-battery-exchange.aspx 28 Robô Clever Em cada um destes servos acoplou-se uma peça de alumínio (Figura 3.9), desenhada especificamente para encaixar e elevar a bateria, no momento da sua troca automática. Para o controlo destes atuadores desenvolveu-se um programa em C++ aplicado no ambiente ROS, o qual será exposto no Capítulo 3.2.6. Figura 3.9: Peça de alumínio desenhada para a elevação das baterias. 3.1.4 Conversores Step-Down Na transição entre o andar de base e o andar superior do robô, há um barramento de 24 V, alimentado diretamente das baterias do robô. Assim, para se obter um nível de tensão apropriado para os diferentes componentes eletrónicos presentes, utiliza-se 2 conversores abaixadores DC-DC. Um desses conversores (LM2575HV)5alimenta o Arduino e um circuito eletrónico de controlo auxiliar. Tabela 3.7: Especificações do conversor LM2575HV. Tensão de saída ajustável continuamente, através do potenciómetro, de 3.3 a 15 V Corrente de saída até 1 A Tensão de entrada até 7 a 40 V (a) (b) Figura 3.10: (a) Esquema do circuito conversor abaixador utilizando o LM2575HV e (b) o mesmo circuito integrado no robô. 5http://www.ti.com.cn/cn/lit/ds/symlink/lm2575-n.pdf 3.1 Hardware 29 Para a alimentação dos 4 servomotores, utiliza-se outro circuito conversor abaixador DC-DC, com o MAX7246. Cada servomotor consome, no máximo 900 mA, totalizando 3.6 A de corrente máxima necessária para alimentar os 4 do sistema. Tabela 3.8: Especificações do conversor MAX724. Tensão de saída ajustável 2.5 V a 35 V Corrente de saída até 5 A Tensão de entrada 8 a 40 V (a) (b) Figura 3.11: (a) Esquema do circuito conversor abaixador utilizando o MAX724 e (b) o mesmo circuito integrado no robô. 3.1.5 Laser O laser utilizado por este robô é o URG-04LX-UG017, montado na zona frontal do andar superior do robô, invertido, como é possível verificar na Figura 3.12. Este comunica diretamente com o computador através de uma ligação USB, por onde é eletricamente alimentado. Figura 3.12: Laser URG-04LX-UG01 montado no robô. 6https://datasheets.maximintegrated.com/en/ds/MAX724-MAX726.pdf 7https://www.hokuyo-aut.jp/02sensor/07scanner/download/pdf/URG-04LX_UG01_spec_en.pdf 30 Robô Clever Tabela 3.9: Especificações do laser URG-04LX-UG01. Laser infravermelho comprimento de onda 785 nm Área de deteção 240o Distância de deteção de 20 mm a 4000 mm Tensão de alimentação 5 V DC, através da ligação USB Consumo de corrente 500 mA Resolução 1 mm Resolução angular 0.36o Número de pontos 683 Tempo de varrimento 100 ms/varrimento Peso 160 g Dimensões externas 50 x 50 x 70 mm 3.1.6 Arduino No robô está presente um microprocessador Arduino Uno, o qual é responsável por interagir com um conjunto de sensores e atuadores presentes no robô: sonares, LEDs, solenoides. Este comunica com o computador através de uma ligação USB, operando como interface entre todo o software de controlo que é executado no computador, em ROS, e alguns dos sensores e atuadores integrados no robô. Na Figura 3.13 estão representadas as ligações do Arduino. Figura 3.13: Ligações do Arduino Uno presente no robô. 3.1 Hardware 31 3.1.7 Sonares e LEDS Na zona frontal do andar superior do robô, estão distribuídos 4 sonares SRF018para auxílio extra (além do laser frontal) na deteção e desvio de obstáculos. Estes estão ligados a um circuito elétrico que por sua vez está conectado aos pinos de entrada e saída do Arduino. Tabela 3.10: Especificações dos sonares SRF01. Distância de deteção de 0-18 cm a 600 cm Resolução 3 a 4 cm Frequência 40 KHz Baudrate 9600, 19200 ou 38400 bps Tensão de alimentação entre 3.3 e 12V (5.5V recomendado) Corrente de consumo 25 mA em operação, 11 mA em Standby e 55 uA suspenso Ligação física TTL Dimensões 2 cm (diâmetro) x 4 cm Figura 3.14: Sonares SRF01 com representação dos pinos de ligação. Figura 3.15: (a) Sonares SRF01 integrados na placa pertencente à zona média do robô. (b) Robô com a placa dos sonares montada. Para além dos sonares, o robô possuiu dois conjuntos de LEDs, um na zona frontal e outro na parte traseira (Figura 3.16), cobrindo o perímetro externo do robô. Estes também são controlados 8https://www.robot-electronics.co.uk/htm/srf01tech.htm 32 Robô Clever pelo Arduino, que por sua vez está diretamente ligado ao computador, onde está integrado o software principal, em ROS. Este conjunto de LEDs é principalmente para aplicações de publicidade, colocados com o intuito de despertar a atenção das pessoas que partilhem o mesmo espaço do robô. (a) (b) Figura 3.16: (a) LEDs frontais (placa branca) e (b) LEDs traseiros do robô (placa branca). 3.1.8 Computador O robô está equipado com um computador que utiliza o sistema operativo Linux, com a distribuição Ubuntu 14.049. Esta é a unidade central de todos os processos comportamentais do robô, onde está a ser executada toda a hierarquia ROS que será descrita em pormenor na Secção 3.2. O computador possuiu 1 Gb de memória RAM e utiliza o processador Atom D40010, da Intel R . Aqui reside uma das questões mais importantes referentes às limitações do software do robô. Tabela 3.11: Especificações do processador Atom D400. Núcleos de processamento 1 Threads 2 Arquitetura 64 bits Cache 512 KB L2 Frequência de base 1.66 GHz Intervalo de tensão de operação 0.800 V a 1.175 V Potência de consumo 10W Como é possível verificar por algumas destas especificações técnicas mais relevantes, este é um processador com pouco poder de cálculo (baixa frequência de base e apenas 1 núcleo de processamento), mas em contrapartida consome pouca potência (máximo de 10 W). É importante referir que este é um consumo constante durante todo o período de atividade do robô, mesmo quando este está parado, em oposição, por exemplo, aos motores que, apesar de consumirem mais energia é apenas durante a movimentação do robô. Este é um fator muito relevante no contexto de robôs de reduzidas dimensões, pois estes não têm capacidade para integrar baterias de elevado peso e grandes dimensões. 9http://www.ubuntu.com/ 10http://www.intel.com/content/www/us/en/processors/atom/atom-d400-d500-vol-1-datasheet.html 3.1 Hardware 33 Figura 3.17: Computador interno montado no robô. 3.1.9 Sistema para logística Umas das aplicações pensadas para a plataforma robótica genérica está relacionada com operações de logística, mais concretamente no transporte de prateleiras móveis. Assim, ao longo deste projeto desenvolveram-se os módulos necessários para este sistema, nomeadamente uma estrutura com um mecanismo atuado por solenóides e um encaixe colocado por baixo das prateleiras móveis, para permitir o acoplamento entre estas e o robô. Figura 3.18: Estrutura integrada na superfície superior do robô para o sistema de transporte de prateleiras móveis. Na Figura 3.18 é possível visualizar a placa preta com o mecanismo dos solenóides aparafusada na parte superior do robô, cuja função é elevar dois cilindros metálicos por forma a encaixar com a superfície inferior das prateleiras. Os dois cilindros metálicos encontram-se no relevo central da estrutura, alinhados com a frente do robô, representados com maior detalhe na Figura 3.19a. Os solenóides estão ligados a um barramento de 12 V (junto à estrutura, na parte superior do robô), o qual fornece a potência necessária para a atuação dos mesmos. Na superfície inferior das prateleiras integrou-se um encaixe para permitir o acoplamento entre os cilindros e a prateleira móvel, visível na Figura 3.19b. 34 Robô Clever A ideia geral é o robô deslocar-se para baixo das prateleiras, encaixar os cilindros metálicos na parte inferior destas e arrastá-las até ao local de destino, baixando de novo os cilindros e soltando a prateleira. Na Figura 3.20 está visualmente representado o sistema descrito até aqui. (a) (b) Figura 3.19: (a) Sistema de encaixe com cilindros metálicos, atuado por solenóides. (b) Estrutura de encaixe na parte inferior da prateleira. Figura 3.20: Robô acoplado com a prateleira móvel. 3.2 Software ROS 35 3.2 Software ROS Inicialmente, nesta secção, irá fazer-se uma introdução à plataforma de desenvolvimento ROS. Depois será feita uma descrição, com detalhe, de todo o software que é executado no computador do robô, desde a arquitetura geral até aos programas individuais e as suas funções, desenvolvidos em ROS. Figura 3.21: Logo do ROS. Como já foi referido anteriormente, utilizou-se, para o desenvolvimento deste sistema, o framework de desenvolvimento ROS (com a versão indigo11), muito popular nos últimos anos para a integração e implementação de software a ser utilizado em aplicações de robótica. O Robot Operating System (ROS)12 é um framework de desenvolvimento de software que integra uma vasta coleção de bibliotecas e ferramentas que facilitam a criação e implementação de sistemas robóticos: abstração de hardware, abstração de comunicação entre programas, drivers para diversos tipos de dispositivos, programas para visualização de dados, gestão de pacotes, etc. A reutilização de código é outro aspeto importante no contexto da criação de software de elevada complexidade (como é no caso da robótica) e que é facilitada utilizando as bibliotecas criadas por comunidades de todo o mundo, aumentando a velocidade no desenvolvimento das aplicações. Isto porque a execução de tarefas aparentemente simples na perspetiva humana envolve a integração de um grande número de programas interligados com diferentes tipos de hardware, o que torna a sua implementação um grande desafio. Em adição, a utilização de ROS facilita o trabalho em equipa, pois qualquer pessoa ou entidade pode desenvolver um nó que facilmente se integra com outros nós ou sistemas, pois o framework realiza todos os processos de comunicação entre eles. Na base do ROS está um conjunto de conceitos fundamentais de todo o framework. Numa visão geral, os sistemas desenvolvidos em ROS consistem em vários programas que comunicam entre si por forma a atingir determinados objetivos comportamentais. 3.2.1 Nós Um nó13 é um processo que executa um determinado código - programa. Os nós são os blocos fundamentais de qualquer sistema ROS que fazem parte de um gráfico de comunicações com arquitetura ponto-a-ponto, comunicando entre si através de tópicos, serviços, ações ou a partir 11http://wiki.ros.org/indigo 12http://wiki.ros.org/ 13http://wiki.ros.org/Nodes 36 Robô Clever do servidor de parâmetros. Como o ROS funciona como um sistema distribuído, os nós não têm necessariamente de operar no mesmo computador, podendo ser executados a partir de várias plataformas que comunicam entre si. Entre os nós, destaca-se o mestre (roscore) responsável por fornecer as informações necessárias para as comunicações entre os restantes nós (utilizando XMLRPC (Extensible Markup Language Remote Procedure Call)), sem que seja necessário que estes conheçam o endereço IP (Internet Protocol) e a porta onde os outros nós estão a ser executados. Quando um novo nó aparece no sistema, o roscore providencia os dados necessários para que este possa formar uma ligação ponto-a-ponto com os restantes nós que publicam e subscrevem os mesmos tópicos [45]. Para além disto, o roscore também gere o servidor de parâmetros, atualizando as informações transmitidas pelos nós no início da sua execução, ou a partir de reconfigurações em runtime, utilizando-se a API dynamic_reconfigure14. 3.2.2 Tópicos Os tópicos são barramentos de comunicação (identificados por nomes únicos) pelos quais os nós trocam mensagens entre si, seguindo um modelo de publisher/subscriber que separa a produção e o consumo de dados. Os nós simplesmente subscrevem os tópicos de que necessitam e, quando geram informações, publicam-nas para os tópicos relevantes15. Com esta arquitetura é possível que múltiplos nós subscrevam e publiquem para os mesmos tópicos, podendo processar a mesma informação para aplicações diferentes. Atualmente o ROS suporta transporte de mensagens através de TCP/IP (Transmission Control Protocol/Internet Protocol), conhecido por TCPROS16 e UDP (User Datagram Protocol), nomeado de UDPROS17. Por default é utilizado o transporte por TCP/IP, que é mais fidedigno, garantindo a entrega de todas as mensagens, em oposição ao UDP, que apesar de produzir menos overhead e diminuir a latência, não garante a entrega de todos os pacotes de dados. 3.2.3 Serviços Os serviços são uma forma de comunicação do tipo de pedido/resposta (request/reply), normalmente utilizados quando um nó necessita de consultar alguma informação referente a outro nó ou quando é necessário iniciar a execução de uma determinada atividade. Um nó (servidor) dispõe um determinado serviço, identificado por um nome único, que pode ser requisitado por outro nó (cliente) que chama esse serviço e espera pela resposta18. 3.2.4 Ações As ações são outra forma de comunicação entre os nós, muito úteis nos casos em que a ação requisitada por um determinado nó não é algo imediato e que se prolonga pelo tempo. As ações 14http://wiki.ros.org/dynamic_reconfigure 15http://wiki.ros.org/Topics 16http://wiki.ros.org/ROS/TCPROS 17http://wiki.ros.org/ROS/UDPROS 18http://wiki.ros.org/Services 3.2 Software ROS 37 são definidas por um objetivo final, um resultado e por um conjunto de feedbacks dados ao longo da sua execução. O nó que requisita a ação indica o objetivo final pretendido e o nó que a executa vai informando do desenrolar incremental do processo (através de feedbacks), até à devolução do resultado final da ação19. 3.2.5 Arquitetura geral Por forma a que um robô desempenhe um conjunto de funções, é necessário haver vários grupos de ligações relacionando diversos programas. No caso concreto do robô Clever, foi necessário integrar vários nós, cada um com a sua função, com vista ao bom funcionamento do sistema final. Para que a execução do software seja realizada de uma forma modular e simplificada, utilizamse vários níveis de lançamento dos nós de ROS (ficheiros launch), destacando-se três principais: nível de hardware, nível de navegação e nível de decisão. Figura 3.22: Arquitetura de ROS, visualizada através do rqt_graph, do nível de lançamento de hardware. Na figure 3.22 é possível visualizar graficamente toda a hierarquia ROS relacionada com o hardware que é executada no robô Clever, com este nível de lançamento. Esta representação foi obtida a partir de uma das ferramentas de visualização do ROS: o rqt20, utilizando especificamente oplugin rqt_graph21 que demonstra graficamente toda a hierarquia de nós, interligados pelos tópicos que os relacionam, formando o sistema geral de execução. A partir da análise da imagem verifica-se que a este nível apenas são lançados os nós de ROS referentes ao controlo de hardware, que incluem os drivers dos motores, dos sensores, do laser, do controlo por joystick e o nó de controlo dos servomotores. Isto permite a verificação e validação das funcionalidades de todas as componentes de controlo do hardware, permitindo uma mais fácil resolução de problemas. Para além disso, é muito útil utilizar o controlo por joystick tanto para 19http://wiki.ros.org/actionlib 20http://wiki.ros.org/rqt 21http://wiki.ros.org/rqt_graph 44 Robô Clever 6. Drivers dos motores                 Nó [/driver_motors] Publicações: •/tf [tf2_msgs/TFMessage] Subscrições: •/cmd_vel [geometry_msgs/Twist] Serviços: Nenhum                 Parâmetros: •”port”: Porta USB onde está ligado o driver dos motores. •”wheel_radius”: Raio das rodas do robô, em metros. •”dist_between_wheels”: Distância entre as rodas do robô, em metros. •”enc_pulses_per_revolution”: Número de pulsos do encoder por cada rotação completa das rodas. •”odom_frame_id”: ID do referencial da odometria. •”base_frame_id”: ID do referencial da base do robô. •”laser_frame_id”: ID do referencial do laser do robô. •”laser_x_offset”: Distância (em x) do laser em relação à origem do referencial de base do robô (”base_frame_id”). •”laser_z_offset”: Distância (em z) do laser em relação à origem do referencial de base do robô (”base_frame_id”). Este programa também foi desenvolvido pelo grupo de robótica do INESCTEC/FEUP, antes do início deste projeto. Essencialmente tem como função fazer a comunicação com os motores, subscrevendo o tópico /cmd_vel, no qual são recebidos os comandos de velocidade, e enviá-los para os drivers, através do protocolo de comunicações especificado. Para além disto, este nó é responsável pela publicação das transformadas entre o referencial do laser (laser_frame_id) e o referencial da base do robô (base_frame_id), a partir das informações fornecidas através dos parâmetros laser_x_offset elaser_z_offset, referentes ao posicionamento relativo entre o laser e a base do robô, necessárias para o restante sistema ROS. Como este nó também é responsável pela comunicação com os encoders dos motores, publica a transformada entre o referencial de base do robô (base_frame_id) e o referencial da odometria (odom_frame_id). 3.2 Software ROS 45 7. Localização AMCL                                             Nó [/localization_amcl] Publicações: •/amcl_pose [geometry_msgs/PoseWithCovarianceStamped] •/tf [tf2_msgs/TFMessage] •/particlecloud [geometry_msgs/PoseArray] Subscrições: •/tf [tf2_msgs/TFMessage] •/tf_static [tf2_msgs/TFMessage] •/initialpose [geometry_msgs/PoseWithCovarianceStamped] •/map [nav_msgs/OccupancyGrid] •/scan [sensor_msgs/LaserScan] Serviços: •/global_localization •/request_nomotion_update •/set_map                                             O nó de localização baseado na localização Monte Carlo (conhecido por filtro de partículas) faz parte da biblioteca de navegação standard do ROS. Este subscreve os tópicos referentes à pose inicial do robô (no momento de início de execução do programa - /initialpose), as transformações de referencial (/tf e/tf_static) e os dados de varrimento do laser (/scan). O mapa do ambiente (/map) é recebido através de um pedido (request) ao serviço /static_map, disponibilizado pelo nó /map_server. Com estas informações e aplicando o algoritmo original da localização Monte Carlo juntamente com algumas extensões ao modelo, devolve a pose estimada do robô (no tópico /amcl_pose). Este nó também pode publicar a transformada entre o referencial do mapa (frame: /map) e o referencial da odometria (frame: /odom), sendo outra forma de devolver a pose estimada pelo algoritmo de localização, para posterior controlo do robô. Em adição, também publica para o tópico /particlecloud, onde estão todas as hipóteses atuais das possíveis poses do robô (partículas) que o algoritmo mantém na sua execução. Quanto aos serviços disponibilizados, é possível iniciar uma localização global (/global_localization) distribuindo uniformemente todas as partículas pelo mapa, com vista a convergir para a pose correta do robô. Por outro lado, o algoritmo AMCL é executado de acordo com a movimentação do robô e não de forma periódica. Por isso, quando o robô está parado o 46 Robô Clever nó não calcula uma nova posição, comportamento que pode ser alterado quando se requisita o serviço /request_nomotion_update. O nó amcl tem um vasto número de parâmetros de configuração (que também podem ser alterados em runtime com o dynamic_reconfigure) que serão pormenorizadamente descritos na Secção 5.2. 8. Localização Perfect Match                                                                Nó [/localization_perfect_match] Publicações: •/LocalisationPoseResult [geometry_msgs/PoseStamped] •/LocalisationPoseWithCovResult [geometry_msgs/PoseWithCovarianceStamped] •/tf [tf2_msgs/TFMessage] •/debug/TimeCycle [std_msgs/Int16] •/debug/TimeLoad [std_msgs/Int16] •/debug/LaserScanfiltered [sensor_msgs/PointCloud] •/debug/LaserScan [sensor_msgs/PointCloud] •/debug/RPropIterationNumber [std_msgs/Int16] •/debug/LaserBeam [sensor_msgs/PointCloud] •/parameter_updates [dynamic_reconfigure/Config] •/parameter_descriptions [dynamic_reconfigure/ConfigDescription] Subscrições: •/tf [tf2_msgs/TFMessage] •/tf_static [tf2_msgs/TFMessage] •/initialpose [geometry_msgs/PoseWithCovarianceStamped] •/scan [sensor_msgs/LaserScan] •/map [nav_msgs/OccupancyGrid] Serviços: •/perfect_match/set_parameters                                                                O nó /localization_perfect_match é outra implementação de um algoritmo de localização, o Perfect Match, baseado em ajuste de mapa, em oposição aos modelos probabilísticos do filtro de partículas. Este programa possuiu a mesma função do nó amcl e a estrutura é idêntica: subscreve os tópicos do mapa (/map), transformações de referenciais (/tf e/tf_static), 3.2 Software ROS 47 posição inicial (/initialpose) e os dados publicados pelo nó do laser (/scan). De igual modo, com base nestes parâmetros de entrada, estima a pose atual do robô, publicando-a para os tópicos /LocalisationPoseResult e/LocalisationPoseWithCovResult (pose sem e com covariância associada, respetivamente). Assim como acontece com o /localization_amcl também é dada a possibilidade deste nó devolver a pose calculada publicando a transformada entre o referencial do mapa (frame: /map) e o referencial da odometria (frame:/odom), sendo outra forma de disponibilizar a pose estimada pelo algoritmo. Em adição, e para além dos tópicos relacionados com o dynamic_reconfigure, devolve vários dados para auxílio no debbuging de eventuais maus comportamentos por parte do programa (/debug/*). Os parâmetros de configuração do nó /localization_perfect_match serão descritos de forma mais aprofundada na Secção 5.1. Ambos os algoritmos de localização têm na sua base a utilização de um mapa para estimar a pose do robô. Assim, é sempre necessário lançar um nó que funciona como servidor (map_server26) e que lhes fornece o mapa. Esta implementação do Perfect Match foi desenvolvida pelo grupo de robótica do INESCTEC/FEUP, tendo-se estudado com detalhe o seu funcionamento e configurado os parâmetros necessários para a correta utilização nas aplicações concretas. Para além disto, explorou-se algumas possibilidades de melhoramento do algoritmo, expostas na Secção 5.1.2. 9. Controlo de posição e navegação                          Nó [/control_go_to_x_y_theta] Publicações: •/cmd_vel [geometry_msgs/Twist] Subscrições: •/tf [tf2_msgs/TFMessage] •/tf_static [tf2_msgs/TFMessage] •pose_goal [geometry_msgs/PoseStamped] •/base_scan [sensor_msgs/LaserScan] Serviços: Nenhum                          26http://wiki.ros.org/map_server 48 Robô Clever Este programa de controlo de posição para navegação, também desenvolvido pelo grupo de robótica do INESCTEC/FEUP, integra um conjunto de algoritmos para o desvio de obstáculos durante a movimentação do robô, enquanto este se desloca para a posição final pretendida. Nomeadamente, possuiu a opção de utilizar uma rotina de simples paragem da movimentação, no caso de ser detetado um obstáculo no interior da zona de segurança do robô (definida por um retângulo com dimensões configuráveis através dos parâmetros do nó); um algoritmo de desvio de obstáculos com base em campos de potencial; e um algoritmo de desvio de obstáculos com base em histograma polar (Vector Field Histogram). A ideia fundamental dos campos de potencial passa por considerar que o robô é um ponto afetado por um campo de potencial, em que o objetivo (o ponto de destino) exerce forças de atração enquanto que todos os obstáculos do mapa exercem forças de repulsão ao robô, resultando num trajeto até ao ponto final. Apesar deste método possuir vantagens, principalmente relacionadas com o peso computacional e o número de cálculos necessários até chegar à solução final, está sujeito ao efeito de mínimos locais ao longo do trajeto, ”prendendo” o robô e impedindo que este encontre um caminho para o objetivo [46]. O algoritmo de desvio de obstáculos com base em histograma polar utiliza o varrimento do laser para calcular a direção apropriada para onde o robô se deve deslocar (com espaço suficiente para que este passe), no caso do aparecimento de um obstáculo na zona de segurança do robô. Cada ponto do laser é considerado como uma célula na grelha do histograma, criando uma distribuição de probabilidade com pouco overhead computacional [47]. Esta direção resulta da opção com valor de custo mais baixo dada por a× desvio_orientacao_destino +b×desvio_orientacao_atual. Atualmente, está a utilizar-se o método de desvio de obstáculos com base em histograma polar neste nó de controlo e navegação. Na Figura 3.26 está representado o funcionamento deste nó, através de uma máquina de estados, com o objetivo de clarificar o comportamento do programa, utilizando a mesma nomenclatura dos parâmetros configuráveis do mesmo. Parâmetros: •”FWD_DIST”: Distância de segurança dianteira, em metros. •”LAT_DIST”: Distância de segurança lateral, em metros. •”STEP_HIST”: Passo do histograma no algoritmo de desvio de obstáculos. •”ANG_HIST_INI”: Ângulo inicial do histograma, em radianos. •”MAX_ETF”: Máximo erro angular permitido para a rotação inicial do robô na direção da posição de destino, em radianos. •”HIST_ETF”: Máximo erro angular permitido para a rotação inicial do robô na direção da posição de destino utilizando o desvio por histograma, em radianos. •”DIST_NEWPOSE”: Distância superior à qual o robô volta a rodar sobre ele mesmo para se alinhar com a posição final. 3.2 Software ROS 49 Figura 3.26: Máquina de estados do funcionamento do nó /control_go_to_x_y_theta para controlo de posição e navegação do robô. •”DIST_DA”: Distância da posição de destino a partir da qual o robô começa a desacelerar. •”TOL_FINDIST”: Máximo erro de posição permitido para se considerar que o robô chegou à posição de destino, em metros. •”THETA_NEWPOSE”: Diferença angular superior à qual o robô volta a rodar sobre ele mesmo para se alinhar com a posição final. •”THETA_DA”: Diferença entre o ângulo do robô e o ângulo final pretendido a partir da qual o robô começa a desacelerar na rotação sobre ele mesmo. 50 Robô Clever •”TOL_FINTHETA”: Máximo erro angular permitido para se considerar que o robô chegou à posição de destino, em radianos. •”LIN_VEL_NOM”: Velocidade linear nominal do robô, em m/s. •”LIN_VEL_DA”: Velocidade linear do robô quando este está em modo de desaceleração, em m/s. •”W_NOM”: Velocidade angular nominal do robô, em radianos/s. •”W_DA”: Velocidade angular do robô este está em modo de desaceleração na rotação, em radianos/s. •”GAIN_FORW”: Factor de multiplicação pelo erro de orientação em relação à posição final, para se obter a velocidade angular, quando o robô se está a movimentar à velocidade nominal. •”GAIN_DA”: Factor de multiplicação pelo erro de orientação em relação à posição final, para se obter a velocidade angular, quando o robô está em modo de desaceleração. 10. Decisão                    Nó [/decision] Publicações: •/pose_goal [geometry_msgs/PoseStamped] Subscrições: •/tf [tf2_msgs/TFMessage] •/tf_static [tf2_msgs/TFMessage] Serviços: Nenhum                    O programa de decisão é a camada de mais alto nível em todo o sistema ROS que é executada neste robô, tendo como função a atribuição de objetivos de navegação ao mesmo, ao longo do seu período de operação. Atualmente, este nó carrega um conjunto de posesobjetivo para onde o robô se deve deslocar ao longo do seu percurso, autonomamente. Parâmetros: •”min_position_error”: Máximo erro de posição permitido para que a chegada ao ponto de destino seja considerada terminada, em metros. •”min_orientation_error”: Máximo erro angular permitido para que a chegada à posição de destino seja considerada terminada, em graus. 3.2 Software ROS 51 3.2.7 Criação de mapas Como já foi referido anteriormente, para que os algoritmos de localização utilizados (AMCL e Perfect Match) possam estimar a pose do robô, é necessário que tenham por base um mapa do ambiente onde o robô vai navegar. O mapa é representado por uma imagem que descreve o estado de ocupação de cada célula no mundo, através do valor da cor atribuída a cada pixel da imagem. Os pixels de cor próxima a branco são considerados como ”livres”, os pixels de cor próxima a preto são considerados como ”ocupados” e os pixels com valores intermédios (cor cinzenta) são considerados como ”desconhecidos”. Os limiares considerados para estes três estados são definidos num ficheiro com a metadata do mapa, sendo a probabilidade de ocupação de uma determinada célula calculada por (255−nivel_cinza_celula) 255 , em que os valores por default consideram uma determinada célula como ocupada se a probabilidade for superior a 0.65 e completamente livre se a probabilidade for inferior a 0.196; os valores intermédios classificam as células com estado desconhecido. A criação do mapa para a navegação do robô Clever foi realizada recorrendo ao próprio laser do robô (URG-04LX-UG01 - 3.1.5). Visto que este laser é o utilizado durante a sua normal operação, considera-se de grande relevância que a perceção do meio no momento da criação do mapa e durante a navegação seja o mais similar possível. Para a movimentação do robô durante a geração do mapa utilizou-se o controlo por joystick, com os nós descritos na secção 3.2.5, referentes ao lançamento apenas do nível de hardware. O programa de ROS escolhido para esta tarefa foi o hector_slam27 [48], devido ao seu baixo peso computacional (fator relevante na aplicação concreta ao robô Clever) e aos seus bons resultados, dando a possibilidade de se realizar o mapeamento sem a necessidade da odometria. Outra opção seria a utilização do gmapping28, que requer necessariamente dados (precisos) da odometria e de um laser montado horizontalmente no robô. (a) Mapa da sala I-108. (b) Mapa da sala I-108 e do corredor. Figura 3.27: Mapas criados para a navegação e localização do robô, utilizando o laser URG-04LXUG01. Na figura 3.27 estão os mapas resultantes do procedimento descrito até aqui. Um dos mapas foi criado apenas na sala I-108 (figura 3.27a) enquanto que o outro também incluiu o mapeamento 27http://wiki.ros.org/hector_slam 28http://wiki.ros.org/gmapping 52 Robô Clever do corredor (figura 3.27b). 3.3 Aplicações da plataforma robótica Este projeto tem em vista a utilização deste robô como uma plataforma genérica para a sua aplicação em diversas atividades distintas. O objetivo principal passou pelo desenvolvimento de um sistema robótico de base que conseguisse navegar autonomamente e que possuísse mecanismos de auto-localização robustos, para que a sua utilização em diversas atividades fosse adaptada com a maior facilidade possível. Visto que este robô possui uma baixa altura e todo o hardware esoftware do sistema de navegação e localização se encontram a essa baixa altura, abrem-se possibilidades para a colocação de diversas extensões por cima do robô, sendo possível acrescentar-se, de forma modular, um conjunto de funcionalidades. Essencialmente, manteve-se em vista as seguintes aplicações para a plataforma robótica: •Monitorização e vigilância Ao colocar uma câmara de vigilância e diversos sensores (e.g., temperatura, monóxido de carbono, fumo, fogo, etc) por cima do robô, é possível controlar o ambiente onde este irá operar. •Publicidade Com vista a despertar a atenção e a curiosidade das pessoas, podem ser integradas colunas de som, LEDs, painéis ou cartazes por cima do robô, que servirão para efeitos publicitários, enquanto este se desloca num determinado espaço público. •Logística Aproveitando as pequenas dimensões e a pequena altura do robô, este pode ser aplicado em operações de logística e transporte de objetos. Mais especificamente, desenvolveu-se um sistema de transporte de prateleiras móveis (com rodas), em que o robô se desloca para debaixo destas e as arrasta para os locais pretendidos. Para se movimentar as prateleiras, integrou-se uma placa com solenóides, que atuam um conjunto de cilindros metálicos, fazendo-os movimentar-se verticalmente: nos momentos de encaixe na prateleira os cilindros sobem, e quando estas já estão na posição pretendida e o robô quer soltá-las movem-se os cilindros para baixo. Este sistema está descrito com maior detalhe na Secção 3.1.9. Capítulo 4 Troca automática de baterias No sentido de se desenvolver um sistema capaz de trocar de baterias automaticamente e de forma autónoma, i.e, sem a intervenção de nenhum humano, desenvolveu-se um conjunto de ferramentas e pensou-se em algumas estratégias com vista a esse objetivo. 4.1 Hardware A alimentação elétrica do robô será realizada através de uma caixa com 2 baterias de 12 V ligadas em série, no seu interior, perfazendo 24 V. Na superfície superior da caixa existem 3 contactos metálicos (Figura 4.1a): o contacto central tem o pólo positivo (+) e os dois contactos laterais têm o pólo negativo (-). Esta disposição tem a vantagem de permitir que a caixa das baterias seja acoplada indiferentemente das duas possíveis orientações, visto que na parte inferior do robô existem apenas 2 contactos metálicos (Figura 4.1b), um positivo no centro do robô, e outro negativo, num dos lados. Assim, não há o risco de curto circuito na eletrónica do robô, caso o pólo positivo fosse ligado acidentalmente ao pólo negativo. (a) (b) Figura 4.1: (a) Caixa com as baterias externas do robô, com três contactos de cobre. (b) Dois contactos de cobre na parte inferior do robô, a partir dos quais é recebida a alimentação elétrica para o funcionamento de todos os componentes do robô. Para o encaixe automático das baterias desenvolveu-se de raiz um sistema que inclui 4 servomotores, dispostos paralelamente 2 a 2, cada um com uma peça de alumínio aparafusada, com a 53 60 Análise dos métodos de localização •”MaxErrordistFilterEnabled1”: Booleano que ativa ou desativa a primeira rejeição de oultiers, utilizando o valor do parâmetro LaserFilterMaxDist1. •”LaserFilterMaxDist1”: Limiar (menos exigente) da distância máxima permitida a um ponto detetado pelo laser e o objeto no mapa mais próximo, para decidir se esse ponto é descartado (outlier), não entrando no algoritmo. •”MaxErrordistFilterEnabled2”: Booleano que ativa ou desativa a segunda rejeição de oultiers, utilizando o valor do parâmetro LaserFilterMaxDist2. •”LaserFilterMaxDist2”: Limiar (mais exigente) da distância máxima permitida a um ponto detetado pelo laser e o objeto no mapa mais próximo, para decidir se esse ponto é descartado (outlier), não entrando no algoritmo. •”WeightenMeasureDistEnable”: Booleano que ativa ou desativa a função de custo do erro com pesos diferentes para os pontos mais próximos e mais distantes. •”Lc”: Coeficiente para a rejeição de pontos com grandes erros (outliers), na função de custo. •”MinPercentValideSensorData”: Percentagem mínima de pontos do laser válidos (n_pontos_laser_validos n_pontos_laser_total ×100%) para que o algoritmo estime a posição do robô. •”initial_pose_x”: Coordenada x da posição inicial do robô, utilizada para iniciar o algoritmo. •”initial_pose_y”: Coordenada y da posição inicial do robô, utilizada para iniciar o algoritmo. •”initial_pose_ori”: Orientação da posição inicial do robô, utilizada para iniciar o algoritmo. Parâmetros para o algoritmo de otimização RPROP •”NumIterationMax”: Número máximo de iterações permitidas para o algoritmo de otimização RPROP (Resilient Back-Propagation). •”OptimizationStopTranslation”: Condição de paragem do algoritmo RPROP, quando o passo de translação for inferior ao valor deste parâmetro. •”OptimizationStopRotation”: Condição de paragem do algoritmo RPROP, quando o passo de rotação for inferior ao valor deste parâmetro. •”DeltaUpdateInitPosition”: Passo inicial para a componente da posição do algoritmo RPROP. •”DeltaUpdateInitOrientation”: Passo inicial para a componente da orientação do algoritmo RPROP. Parâmetros para a fusão sensorial do Filtro de Kalman 5.1 Análise e comparação de várias configurações do Perfect Match 61 •”SensorsKErrorXY”: Factor do ruído de posição dos sensores, para o cálculo da segunda derivada. •”SensorsKErrorTheta”: Factor do ruído de orientação dos sensores, para o cálculo da segunda derivada.. •”DriftTranslationDeltaD”: Desvio na translação da odometria pela componente de translação, para a propagação do erro. •”DriftTranslationDeltaRot”: Desvio na translação da odometria pela componente de rotação, para a propagação do erro. •”DriftRotationDeltaRot”: Desvio na orientação da odometria pela componente de rotação, para a propagação do erro. . •”StateMinVarXY”: Mínima covariância (x×y) permitida para os estados do Filtro de Kalman. •”StateMinVarTheta”: Mínima covariância (θ×θ) permitida para os estados do Filtro de Kalman. O algoritmo do Perfect Match utilizado para o presente trabalho de dissertação é baseado no artigo [5] que implementa um conjunto de melhoramentos e estende o algoritmo original [3] para outro tipo de aplicações: robôs de serviços, industriais, utilizando outro tipo de sensores (e.g., lasers), em oposição às técnicas de visão (câmeras). Nesta implementação, em vez de se utilizar um número fixo de iterações para o algoritmo de otimização RPROP, define-se um número máximo de iterações (NumIterationMax) ou, no caso do método convergir para um valor de erro inferior a um determinado limiar previamente definido (OptimizationStopTranslation,OptimizationStopRotation) o algoritmo pára. Em adição, é definido um momento de pré-processamento dos dados recebidos pelo laser, fazendo-se uma rejeição dos pontos que estiverem mais distantes (que um determinado threshold) do obstáculo mais próximo no mapa , indicando a presença de objetos/pessoas externas ao mapa (outliers). Na realidade o processo de rejeição dos outliers é realizado duas vezes: uma primeira vez utilizando um valor de distância máxima menos exigente (LaserFilterMaxDist1 - por exemplo 0.5 m), descartando-se os pontos mais distantes que este valor e de seguida executando-se o RPROP; na segunda vez, a partir do resultado do processo anterior, utiliza-se um valor de distância mais exigente (LaserFilterMaxDist2 - por exemplo 0.1 m) e volta-se a correr o algoritmo do RPROP, obtendo-se o resultado final. A utilização de dois filtros consecutivos, com limiares diferentes, em oposição a apenas um filtro com um valor de distância mais exigente deve-se ao facto de, numa primeira fase, se o algoritmo estiver com uma posição desajustada (com algum erro) em relação à posição real do robô, pode descartar um número considerável de pontos que na realidade não correspondem a outliers. Isto porque todo o algoritmo tem como base a posição estimada pelo próprio, e no caso dessa posição estar com algum erro temporário, resulta num cálculo errado das 62 Análise dos métodos de localização distâncias entre os pontos detetados pelo laser (5.1) e a posição esperada do obstáculo mais próximo no mapa, levando a uma rejeição errada de pontos corretos. Com a utilização de dois filtros, inicialmente rejeita-se os objetos muito longe do esperado, ajusta-se a posição estimada pelo algoritmo RPROP e, com base nesta nova estimativa, faz-se a rejeição dos pontos com o filtro mais exigente, havendo uma maior probabilidade de ter sido utilizada uma posição correta do robô. Para além das funcionalidades descritas, o programa permite utilizar uma opção que atribui pesos diferentes aos pontos detetados pelo laser, de acordo com a sua distância ao robô (WeightenMeasureDistEnable). Isto porque, um objeto que esteja próximo do robô é detetado por um maior número de pontos do laser, para o mesmo comprimento, do que se estivesse mais longe, devido ao efeito radial dos feixes do laser. Por consequência, na fase de atribuição dos pesos aos pontos utilizados pelo algoritmo, para o cálculo da função de custo, os objetos ou as características do ambiente (e.g., paredes, mobília) que estiverem mais próximos do robô terão maior peso. Com vista a equilibrar este fenómeno, há uma compensação linear dos pesos dos pontos em função da distância, através de: Ei=disti·Ei, com disti=p(py−probot)2+(px−probot )2. Numa visão geral, do início da execução do algoritmo até ao resultado final, há 3 fases distintas: 1. Pré-cálculo da matriz das distâncias de cada célula do mapa ao obstáculo mais próximo, assim como as matrizes de gradiente (na direção do x e do y) e cálculo do erro de ajuste. P="px py#="xrobot yrobot#+"cosθ−sinθ sinθcosθ#"dx dy#(5.1) Na Equação 5.1 está o método para calcular a posição de cada ponto detetado pelo laser em coordenadas globais (px,py), a partir da posição (xrobot,yrobot)e orientação (θ) atual do robô e da distância relativa dos pontos do laser ao mesmo, (dx,dy). Utilizando a matriz das distâncias, para cada ponto Pdetetado pelo laser (em coordenadas globais), obtém-se a distância (errori) desse ponto ao obstáculo do mapa mais próximo (na prática, numa imagem binária, é a distância à célula/pixel ocupada mais próxima), através da consulta a uma matriz pré-calculada. 2. Execução do algoritmo de otimização RPROP, minimizando a função de custo. Após a determinação da distância entre os pontos detetados pelo laser e o obstáculo mais próximo presente no mapa, define-se uma função de custo (Figura 5.1), que, com base no erro de distância (errori) para cada ponto ido laser, irá ser minimizada (através do método RPROP) e convergirá para a posição estimada do robô. E= n ∑ i=1 Ei(5.2) 5.1 Análise e comparação de várias configurações do Perfect Match 63 Depois de calcular o valores da função de custo para cada ponto ido laser, através da expressão 5.3, utiliza-se o algoritmo RPROP para minimizar E(5.2). Ei=L2 c·error2 i (L2 c+error2 i)2(5.3) Figura 5.1: Gráfico da função de custo, com o limiares de rejeição de outliers (O). 3. Estimação da covariância, através do cálculo da segunda derivada. Por fim, calcula-se a segunda derivada (Equações 5.4,5.5 e5.6) para, através de uma heurística, se obter a variância da estimativa de pose dada pelo algoritmo. As variâncias da posição (σ2 x,t,σ2 y,t) e orientação do robô (σ2 θ,t), no instante de tempo t, são calculada a partir do conjunto de equações 5.7, em que (b xt+1,b yt+1,b θt+1)são as estimativas da posição e orientação do robô, aplicando o modelo de movimentação do mesmo, com base nos dados recebidos pela odometria (velocidade linear ve velocidade angular ω). Estas variâncias serão depois utilizadas no processo de fusão sensorial do Perfect Match, quando se aplica o Filtro de Kalman. ∂2E (∂px)2≈ n ∑ i=1 E00 i(Pi)·(∂(errori) ∂x)2(5.4) ∂2E (∂py)2≈ n ∑ i=1 E00 i(Pi)·(∂(errori) ∂y)2(5.5) 64 Análise dos métodos de localização ∂2E (∂pθ)2≈ n ∑ i=1 (E00 i(Pi)·(∂(errori) ∂x(−sinθ−cosθ)Pi+∂(errori) ∂y(cosθ−sinθ)Pi)2+ E0 i·(∂(errori) ∂x(−cosθ+sinθ)Pi+∂(errori) ∂y(−sinθ−cosθ)Pi) (5.6)          σ2 b x,t+1=σ2 x,t+α(b xt+1−xt)2 σ2 b y,t+1=σ2 y,t+α(b yt+1−yt)2 σ2 b θ,t+1=σ2 θ,t+α(b θt+1−θt)2 (5.7) 5.1.2 Ideias para melhoramento do desempenho do Perfect Match O Perfect Match, apesar de ser um algoritmo de localização robusto e que devolve boas estimativas, ainda pode ser melhorado. Desde a sua implementação original veio sofrendo alterações e extensões com o objetivo de o tornar mais fiável, com menor sensibilidade a perturbações ou até para ser aplicado em áreas diferentes, com diferentes requisitos. Assim, ao longo deste projeto foram-se explorando alguns possíveis melhoramentos ao algoritmo Perfect Match utilizado. Por vezes o algoritmo pode fazer um ajuste errado com uma parte do mapa que na realidade não está dentro do campo de visão do robô e é fisicamente impossível que este a esteja a detetar. Por exemplo, se o robô estiver a navegar por um local com zonas muito similares e indistinguíveis entre si (e.g., paredes com formas idênticas, consecutivas, entre uma sala e um corredor), o processo de matching pode estar a ser realizado com a parede errada, a qual não é possível que o robô detete por existir outra no intermédio. Neste caso o robô iria devolver uma estimativa errada da posição do robô, uns metros ou centímetros mais deslocada da posição verdadeira. Para contrariar este fenómeno, desenvolveu-se um método de beamtracking, em que se verificava se ao longo do feixe do laser existiam obstáculos no mapa, indicando que, caso existissem, seria fisicamente impossível para o robô detetar algo para além desse ponto. Desta forma, o algoritmo alterava a posição do ponto detetado pelo laser para o obstáculo mais próximo do mapa, na direção desse feixe do laser, impedindo que o fenómeno descrito ocorresse. Apesar desta implementação poder revelarse vantajosa em alguns casos, aumenta muito o peso computacional do algoritmo que tem como uma das suas principais características e vantagens a rapidez e leveza computacional. Isto porque a verificação das várias posições do mapa, ao longo de cada feixe de um varrimento do laser é um processo pesado e demorado. Outra ideia, ainda não implementada, passa pela divisão do mapa global do robô em sub-mapas de zonas específicas, em que se ia comutando entre sub-mapas, de forma automática, à medida que o robô se ia deslocando ao longo do edifício. Este modo de operação traz vantagens para a convergência do processo de ajuste de mapa, pois há menor probabilidade de haver vários mínimos locais à medida que as dimensões do mapa diminuem. Por isso, a estimativa seria mais robusta e fiável para cada sub-mapa, impedindo também a ocorrência da situação referida anteriormente, em 5.1 Análise e comparação de várias configurações do Perfect Match 65 que o algoritmo poderia fazer um matching com características do mapa que não seriam possíveis de detetar, eliminando também esse problema. Apesar de poder ser uma ideia promissora, a sua implementação não é trivial, devido à necessidade de sincronização dos vários sub-mapas com a movimentação do robô, o que é especialmente difícil nos locais de transição entre duas divisões (e.g., passagem de uma sala para um corredor de um edifício). Assim, é um desafio para ser explorado no futuro. 5.1.3 Comparação prática de diferentes configurações do Perfect Match Nesta secção serão demonstrados alguns testes práticos ao algoritmo de localização Perfect Match, realizados no robô Jarvis, com acesso a groundtruth1mas utilizando o laser URG-04LXUG01 (integrado no robô Clever - 3.1.5), a 23 cm do chão (na mesmas condições que no robô Clever), como sensor para a recolha dos dados necessários para o algoritmo de localização. O robô movimentou-se ao longo de uma trajetória aproximadamente circular, a 0.1 m/s, representado na figura 5.2. Figura 5.2: Trajectória descrita pelo robô durante os testes, sobreposta ao mapa utilizado para a navegação, visualizada através do rviz, em ROS. Estes testes debruçar-se-ão na verificação da precisão e robustez do algoritmo, nestas circunstâncias, com diferentes parametrizações e num ambiente com variadas características: sem outliers, com diferentes níveis de obstáculos e sujeito a erros forçados de odometria. As configurações terão como intuito demonstrar, na prática, o impacto real de alguns dos parâmetros do Perfect Match, mantendo outros, menos relevantes (ou com menos interesse para estes estudos), constantes. Todas as configurações terão os parâmetros com os valores apresentados na Tabela 5.1. 1Para o groundtruth utilizou-se o laser SICK NAV350. 66 Análise dos métodos de localização Tabela 5.1: Parâmetros comuns a todas as configurações desta secção. Parâmetros Valores LaserMeasuresNum 683 pontos NumIterationMax 50 OptimizationStopTranslation 0.0 OptimizationStopRotation 0.0 MinPercentValideSensorData 0.0 % DeltaUpdateInitPosition 0.1 m DeltaUpdateInitOrientation 0.1 graus Lc 0.1 DriftRotationDeltaRot 0.001 DriftTranslationDeltaD 0.001 DriftTranslationDeltaRot 0.0003 SensorsKErrorTheta 0.001 SensorsKErrorXY 1.0 StateMinVarTheta 0.001 StateMinVarXY 0.01 Utilizou-se o número máximo de pontos devolvidos pelo laser URG-04LX-UG01: 683. Definiu-se um número máximo de 50 iterações para o algoritmo RPROP, colocando o critério de paragem através dos passos mínimos do RPROP (OptimizationStopTranslation eOptimizationStopRotation) a 0, forçando a convergência total do algoritmo antes de devolver o resultado final (ou utilizar as 50 iterações), com vista a obter a melhor estimativa possível. Não se coloca limite mínimo para o número de pontos totais utilizados pelo Perfect Match (que levaria à paragem temporária do programa), pois esta é uma medida de segurança e para o efeito destes testes não é relevante integrá-la, pois há interesse em verificar o comportamento do algoritmo em situações desfavoráveis. Os parâmetros referentes ao passo inicial atribuído ao método RPROP (DeltaUpdateInitPosition eDeltaUpdateInitOrientation) deverão ser ajustados de acordo com o grau de matching esperado a cada iteração do algoritmo (cerca de alguns centímetros, para este caso), com vista a minimizar o número de iterações necessárias para que convirja. De qualquer modo, desde que não lhes sejam atribuídos valores demasiado altos ou demasiado baixos, esse desajuste inicial será traduzido em apenas algumas (muito poucas) iterações extra. Os restantes parâmetros referentes ao Filtro de Kalman são intrínsecos ao robô e foram calibrados previamente a este projeto, resultando nos valores apresentados na Tabela 5.1. 5.1 Análise e comparação de várias configurações do Perfect Match 67 5.1.3.1 Ambiente sem outliers O conjunto de testes apresentados nesta secção foram realizados na sala I108, no campo RoboCup, sem outliers. Configuração 1 e Configuração 2 As primeiras duas configurações serão denominadas por Config1_OF eConfig2_OF_F1F2 e terão os parâmetros apresentados na Tabela 5.2. Tabela 5.2: Definição da Config1_OF e da Config2_OF_F1F2. Config1_OF Config2_OF_F1F2 ”OperationMode” SensorsOdomFusion SensorsOdomFusion ”MaxErrordistFilterEnabled1” false true ”LaserFilterMaxDist1” - 0.5 ”MaxErrordistFilterEnabled2” false true ”LaserFilterMaxDist2” - 0.05 ”WeightenMeasureDistEnable” false false Tabela 5.3: Resultados estatísticos da Config1_OF eConfig2_OF_F1F2, para um ambiente sem outliers. Config1_OF Config2_OF_F1F2 Média do erro de distância (m) 0.0466 0.0445 Média do erro angular absoluto (o) 0.8010 0.8078 Erro máximo de distância (m) 0.0938 0.0940 Erro angular absoluto máximo (o) 2.482 2.094 Variância do erro da distância 2.545·10−42.157·10−4 Variância do erro angular 0.2546 0.1886 Estas duas configurações são muito similares: ambas utilizam o modelo de fusão sensorial, entre os sensores e a odometria para a estimação da posição. Por outro lado, uma utiliza o filtro de rejeição de outliers (Config2_OF_F1F2) e a outra não (Config1_OF). Visto que nas condições de teste não foram colocados objetos externos ao mapa, era expectável que com estas duas parametrizações os resultados fossem similares, que foi o que aconteceu. Isto é verificável através da observação das evoluções dos erros ao longo do percurso (Figuras 5.3 e5.4), com uma ligeira melhoria na parametrização Config2_OF_F1F2 (Tabela 5.3). Na sequência de imagens da Figura 5.5 a estimativa de localização dada pelo Perfect Match (seta azul) está sempre praticamente sobreposta à posição do groundtruth (seta verde), mantendo um reduzido erro ao longo da trajetória do robô. 68 Análise dos métodos de localização (a) (b) Figura 5.3: Evolução do erro em x, y e em ângulo, ao longo do percurso, utilizando a (a) Config1_OF e a (b) Config2_OF_F1F2, para um ambiente sem outliers. 5.1 Análise e comparação de várias configurações do Perfect Match 69 (a) (b) Figura 5.4: Evolução do erro em distância, ao longo do percurso, utilizando a (a) Config1_OF e a (b) Config2_OF_F1F2, para um ambiente sem outliers. 76 Análise dos métodos de localização Config1_OF Utilizando a Config1_OF, agora para um ambiente com alguns objetos não pertencentes ao mapa obtiveram-se os resultados representados pelas Figuras 5.11,5.12 e na Tabela 5.8. Como é percetível pelos dados apresentados, o robô perdeu-se momentaneamente quando começou a integrar os pontos que correspondiam à deteção dos obstáculos do mapa, resultando num matching errado pelo algoritmo de otimização e consequentemente devolvendo uma posição errada. Após algum tempo, quando o robô se deslocou para uma zona em que já não havia grande número de outliers, acabou por voltar a convergir para a posição correta. Isto pode ser visualizado na Figura 5.13, onde está representada a posição estimada pelo Perfect Match (seta azul), a posição do groundtruth (seta verde) e os pontos do laser (a rosa são todos os pontos devolvidos pelo laser e a azul estão apenas os pontos que o algoritmo utiliza, após a rejeição de outliers). Neste exemplo em concreto, é possível verificar que o algoritmo utiliza todos os pontos do laser (a rejeição de outliers está desativada nesta configuração), levando à sua momentânea perda de localização. Tabela 5.8: Resultados estatísticos da Config1_OF, com alguns obstáculos no mapa. Média do erro de distância (m) 0.4957 Média do erro angular absoluto (o) 4.718 Erro máximo de distância (m) 2.517 Erro angular absoluto máximo (o) 34.96 Variância do erro da distância 0.5072 Variância do erro angular 53.82 Figura 5.11: Evolução do erro em x, y e em ângulo, ao longo do percurso, utilizando a Config1_OF, para um ambiente com alguns outliers. 5.1 Análise e comparação de várias configurações do Perfect Match 77 Figura 5.12: Evolução do erro de distância, ao longo do percurso, utilizando a Config1_OF para um ambiente com alguns outliers. (a) (b) (c) (d) Figura 5.13: (a) Robô na posição inicial, bem localizado. (b) A integração de pontos do laser correspondentes à deteção de obstáculos externos ao mapa resulta num aumento momentâneo do erro de localização. (c) e (d) Numa zona com menos outliers o robô volta a conseguir recuperar a posição correta. 78 Análise dos métodos de localização Config2_OF_F1F2 Com a utilização de uma configuração com o filtro de rejeição de outliers ativo, aumenta a robustez do algoritmo a objetos ou pessoas inesperadas no ambiente de ação do robô. Na Figura 5.16, é possível verificar que o Perfect Match descarta os pontos referentes à deteção de outliers (a cor de rosa), conseguindo manter uma boa localização da posição do robô durante todo o percurso, notório pela sobreposição entre a estimativa do Perfect Match (seta azul) e a posição do groundtruth (seta verde). Ao observar-se as Figuras 5.14,5.15 e os dados fornecidos pela Tabela 5.9, é possível notar um elevada precisão das estimativas de localização do Perfect Match, obtendo um erro médio inferior a 3 cm e um erro máximo pouco superior a 5 cm. Figura 5.14: Evolução do erro em x, y e em ângulo, ao longo do percurso, utilizando a Config2_OF_F1F2, para um ambiente com alguns outliers. Figura 5.15: Evolução do erro de distância, ao longo do percurso, utilizando a Config2_OF_F1F2 para um ambiente com alguns outliers. 5.1 Análise e comparação de várias configurações do Perfect Match 79 (a) (b) (c) (d) Figura 5.16: (a) Robô na posição inicial, bem localizado. (b) Os pontos a cor de rosa (outliers) são descartados pelo algoritmo. (c) e (d) O Perfect Match consegue manter uma boa estimativa da localização do robô (a seta azul corresponde à estimativa da localização e a seta verde ao groundtruth). Tabela 5.9: Resultados estatísticos da Config2_OF_F1F2, com alguns obstáculos no mapa. Média do erro de distância (m) 0.0291 Média do erro angular absoluto (o) 0.9107 Erro máximo de distância (m) 0.0549 Erro angular absoluto máximo (o) 4.639 Variância do erro da distância 1.346·10−4 Variância do erro angular 0.6941 80 Análise dos métodos de localização Configuração 6 Para ilustrar o efeito prático dos dois filtros de outliers, preparou-se uma nova configuração (Config6_OF_F2), descrita na Tabela 5.10. Aqui, utiliza-se apenas o filtro mais exigente do Perfect Match, descartando todos os pontos que estão a uma distância superior a 0.05 m da posição esperada, sem a utilização do primeiro filtro que rejeita outliers mais desajustados. Tabela 5.10: Definição da Config6_OF_F2. ”OperationMode” SensorsOdomFusion ”MaxErrordistFilterEnabled1” false ”LaserFilterMaxDist1” - ”MaxErrordistFilterEnabled2” true ”LaserFilterMaxDist2” 0.05 ”WeightenMeasureDistEnable” false Nas Figuras 5.17 (c) e (d) é possível verificar que alguns pontos do laser correspondentes a secções das paredes não foram integrados pelo algoritmo (pontos que não estão a azul), por serem considerados a uma distância superior ao limiar especificado (0.05 m), causado por um desajuste temporário da estimativa de localização dada pelo Perfect Match, levando à rejeição desses pontos. Este tipo de comportamento pode ser crítico em determinados ambientes de operação, podendo chegar ao limite de descartar grande parte dos pontos e levar à perda do robô. Através da análise dos gráficos das Figuras 5.18,5.19 e dos resultados estatísticos da Tabela 5.11, verifica-se que a estimativa calculada pelo Perfect Match resultou num aumento temporário do erro de localização, devido à integração de pontos correspondentes a obstáculos presentes no ambiente e à não inclusão de alguns pontos corretos (e.g., da deteção de paredes), em algumas zonas do percurso do robô. Tabela 5.11: Resultados estatísticos da Config6_OF_F2, com alguns obstáculos no mapa. Média do erro de distância (m) 0.1913 Média do erro angular absoluto (o) 3.406 Erro máximo de distância (m) 0.9419 Erro angular absoluto máximo (o) 25.74 Variância do erro da distância 0.0812 Variância do erro angular 32.23 5.1 Análise e comparação de várias configurações do Perfect Match 81 (a) (b) (c) (d) Figura 5.17: (a) Robô na posição inicial, bem localizado. (b) O algoritmo integra alguns pontos referentes aos obstáculos que estão no mapa e aumenta o erro de localização. (c) Numa zona onde há menos outliers e mais dados referentes ao mapa, o robô começa a convergir para a posição correta. (d) Robô bem localizado. Figura 5.18: Evolução do erro em x, y e em ângulo, ao longo do percurso, utilizando a Config6_OF_F2, para um ambiente com alguns outliers. 82 Análise dos métodos de localização Figura 5.19: Evolução do erro de distância, ao longo do percurso, utilizando a Config6_OF_F2 para um ambiente com alguns outliers. 5.1 Análise e comparação de várias configurações do Perfect Match 83 Configuração 7 Para o caso inverso ao da Config6_OF_F2, desligou-se o filtro mais exigente para a rejeição de outliers e ligou-se o filtro menos exigente (com o valor 0.5 m) para esta nova configuração (Config7_OF_F1), visível na Tabela 5.12. Tabela 5.12: Definição da Config7_OF_F1. ”OperationMode” SensorsOdomFusion ”MaxErrordistFilterEnabled1” true ”LaserFilterMaxDist1” 0.5 ”MaxErrordistFilterEnabled2” false ”LaserFilterMaxDist2” - ”WeightenMeasureDistEnable” false (a) (b) (c) (d) Figura 5.20: (a) Robô na posição inicial (seta azul), bem localizado. (b) O algoritmo descarta o conjunto de pontos a cor de rosa (outliers a mais de 0.5 m dos pontos esperados). (c) Os obstáculos próximos da parede (a menos de 0.5 m) são integrados no algoritmo (pontos azuis), levando a um aumento do erro de localização. (d) Na zona sem outliers o robô volta a recuperar a posição correta. Tendencialmente, para este tipo de configuração, apesar do algoritmo não atingir valores de erro tão elevados como para a Config6_OF_F2, acaba por integrar conjuntos de pontos do laser referentes a obstáculos localizados a uma distância inferior à especificada (para este caso, 0.5 m), o que leva a um aumento do erro nessas zonas, devido a um ajuste errado por parte do algoritmo 84 Análise dos métodos de localização Figura 5.21: Evolução do erro em x, y e em ângulo, ao longo do percurso, utilizando a Config7_OF_F1, para um ambiente com alguns outliers. ao considerar esses objetos como parte do mapa. Este fenómeno é claramente verificado na Figura 5.20 (c), onde a deteção de uma caixa localizada junto à parede é integrada no algoritmo (indicado pelos pontos azuis do laser), levando a um temporário desajuste na estimativa de localização, aumentando o erro (Figuras 5.21 e5.22). Assim, através de experiências reais, fica demonstrada a importância da utilização dos dois filtros de rejeição de outliers, tendo estes efeitos complementares e devendo ser integrados conjuntamente para um melhor desempenho do algoritmo. Isto porque os filtros são aplicados em iterações consecutivas do algoritmo: numa primeira fase com um limiar menos exigente no erro de distância dos pontos em relação ao obstáculo mais próximo (para estes exemplos 0.5 m) e, depois de um ajuste da estimativa da posição, através do algoritmo RPROP, volta-se a descartar um conjunto de pontos com base num limiar menos exigente (0.05 m, nestes casos) antes de voltar a executar de novo o algoritmo de otimização. Tabela 5.13: Resultados estatísticos da Config7_OF_F1, com alguns obstáculos no mapa. Média do erro de distância (m) 0.0609 Média do erro angular absoluto (o) 1.129 Erro máximo de distância (m) 0.1376 Erro angular absoluto máximo (o) 4.477 Variância do erro da distância 6.539·10−4 Variância do erro angular 1.096 5.1 Análise e comparação de várias configurações do Perfect Match 85 Figura 5.22: Evolução do erro de distância, ao longo do percurso, utilizando a Config7_OF_F1 para um ambiente com alguns outliers. 92 Análise dos métodos de localização (a) (b) Figura 5.29: Evolução do erro em x, y e em ângulo, ao longo do percurso, utilizando a (a) Config2_OF_F1F2 e a (b) Config5_OF_F1F2WD, com dois momentos onde se forçou o erro da odometria (correspondentes aos dois aumentos de erro visíveis no gráfico). Na Figura 5.28 encontra-se uma única sequência de imagens ilustrativas do percurso do robô e da estimativa da sua posição (seta azul), comparando-a com o groundtruth (seta verde), para as duas configurações (dado a grande semelhança na evolução das duas visualizações ao longo do tempo). Os resultados para estas duas configurações foram muito similares, como é possível verificar pelos gráficos das Figuras 5.29,5.30 e pelos dados da Tabela 5.15. Ambas as configurações revelaram ser bastante robustas a erros de odometria, tendo conseguido recuperar rapidamente dessa situação adversa, convergindo para a posição correta. Apesar da similaridade, a Configuração Config5_OF_F1F2WD revelou uma ligeira melhoria nos erros de posição e orientação ao longo do percurso. 5.1 Análise e comparação de várias configurações do Perfect Match 93 (a) (b) Figura 5.30: Evolução do erro em distância, ao longo do percurso, utilizando a (a) Config2_OF_F1F2 e a (b) Config5_OF_F1F2WD, com dois momentos onde se forçou o erro da odometria (correspondentes aos dois picos no gráfico). Tabela 5.15: Resultados estatísticos das Config2_OF_F1F2 eConfig5_OF_F1F2WD, em condições de falhas da odometria. Config2_OF_F1F2 Config5_OF_F1F2WD Média do erro de distância (m) 0.0953 0.0911 Média do erro angular absoluto (o) 1.178 1.113 Erro máximo de distância (m) 0.4474 0.4318 Erro angular absoluto máximo (o) 4.494 4.502 Variância do erro da distância 1.115·10−29.720·10−3 Variância do erro angular 0.5846 0.6522 94 Análise dos métodos de localização 5.1.3.5 Conclusões Numa visão geral de todas as configurações apresentadas, em diversos tipos de ambiente, destacaram-se as configurações que utilizam a fusão sensorial entre a odometria e o laser e os filtros de rejeição de outliers. O facto do algoritmo integrar os dados fornecidos pela odometria impede que a estimativa de posição ao longo da trajetória do robô oscile ou ”salte” de umas posições para as outras e que dependa, nesse caso, unicamente do número de obstáculos presentes no mapa e dos dados fornecidos pelo laser. Mesmo utilizando as informações dadas pela odometria, o algoritmo Perfect Match revelou possuir uma elevada robustez e conseguir recuperar a localização correta, mesmo com dados errados de odometria. Por outro lado, foi visível que, ao utilizar os filtros de rejeição de pontos desajustados do mapa torna o algoritmo muito menos sensível a ambientes com muitos obstáculos à volta do robô, aumentando a sua fiabilidade. Ainda assim, é importante ter em atenção o número total de pontos que resultam depois de se descartarem os referentes aos outliers, pois, se esse número for demasiado baixo, a qualidade da estimativa desce drasticamente e no limite o robô pode perder a sua localização. Deste modo, para aplicações reais, é aconselhável utilizar o parâmetro que analisa a percentagem de pontos totais considerados ”bons”. 5.2 Análise e comparação de várias configurações do AMCL Nesta secção será feita uma análise detalhada do algoritmo de localização Monte Carlo Localization (MCL), na sua versão ROS, em que inclui várias extensões e modelos ao algoritmo, denominado de Adaptive Monte Carlo Localization (AMCL). Mais especificamente, serão explicados todos os parâmetros de ajuste disponibilizados por este programa2, os quais têm um grande impacto na performance e robustez do algoritmo, tornando-se crucial para aplicações mais exigentes, como é o caso do projeto da presente dissertação. 5.2.1 Análise dos parâmetros do AMCL Parâmetros gerais do filtro •”min_particles”: Número mínimo de partículas permitidas para o filtro. •”max_particles”: Número máximo de partículas permitidas para o filtro. •”kld_err”: Erro máximo entre a distribuição estimada e a real distribuição do filtro. •”kld_z”: Probabilidade do erro entre a distribuição estimada e a distribuição real seja inferior a kld_err. Estes parâmetros fazem parte do modelo KLD-sampling, que utiliza o método de Kullback–Leibler divergence (KLD) com vista a estimar o erro da distribuição e consequentemente adaptar o número 2A análise matemática será baseada no livro ”Probabilistic Robotics” [2] 5.2 Análise e comparação de várias configurações do AMCL 95 de partículas ao estado de localização atual do robô. Como a qualidade da estimativa da posição calculada pelo algoritmo MCL é proporcional ao número de partículas mantidas ao longo da sua execução, é complicado encontrar um limiar que por um lado mantenha o peso computacional baixo e que por outro lado forneça uma boa estimativa. Assim, ao adaptar dinamicamente o número de partículas a cada iteração de acordo com as circunstâncias atuais, é possível tornar o algoritmo mais leve, não perdendo muita da sua robustez. No início da execução é necessário utilizar um número elevado de partículas para cobrir uma área suficientemente grande para que a estimativa convirja para a posição real do robô; enquanto que numa fase de seguimento da posição, em que as partículas já estão mais concentradas à volta de uma determinada posição, não justifica manter esse número tão elevado de amostras. Com a adição do modelo do KLD-sampling ao MCL original, a cada iteração, é calculado um novo número de partículas Mzque estimará na iteração seguinte. O número Mzé calculado de forma a que, com probabilidade (1−kld_z), o erro entre a distribuição real e a aproximação calculada não exceda o valor kld_err. •”update_min_d”: Movimento de translação necessário para que seja realizado uma atualização do filtro. •”update_min_a”: Movimento de rotação necessário para que seja realizado uma atualização do filtro. •”resample_interval”: Número de atualizações do filtro necessárias para que seja realizado uma reamostragem. Figura 5.31: Pseudo-algoritmo de Monte Carlo Localization (MCL) [2]. No algoritmo MCL, representado na figura 5.31, é possível identificar 3 momentos principais no processo de cálculo das estimativas de posição. Inicialmente é aplicado o modelo da odometria 96 Análise dos métodos de localização (propagação do deslocamento) a cada uma das partículas, calculando-se a posição previsível para cada uma (linha 4). De seguida, a cada uma destas partículas é atribuído um peso (linha 5) de acordo com o modelo das medições dadas pelos sensores do robô (e.g., laser ou sonar - 5.2.1). Às partículas que estiverem numa posição reforçada pelas medições dos sensores ser-lhe-ão atribuídas um peso elevado, enquanto que as partículas localizadas em posições que não estão de acordo com as medições efetuadas terão um peso inferior. Desta forma é terminada a fase de atualização do filtro, obtendo-se um conjunto de partículas distribuídas pelo mapa com diversos pesos. Por fim, é efetuada uma fase de reamostragem das partículas, em que se cria, com probabilidade proporcional ao peso de cada partícula, novas partículas e eliminando outras com pouca probabilidade. No final resulta o mesmo número de partículas. Ou seja, nesta última ação, as partículas com um peso mais baixo têm uma probabilidade reduzida de serem seleccionadas, desaparecendo as amostras desses locais e concentrando-se nas posições das partículas com maior peso. Uma característica de grande relevância no algoritmo AMCL é que este é executado de acordo com a movimentação do robô e não de forma periódica, como poderia ser de esperar. As atualizações no filtro ocorrem apenas quando o robô se deslocou um determinado valor de distância (update_min_d) ou ângulo (update_min_a) desde a estimação anterior. Relacionado com estes dois parâmetros, a taxa de reamostragem (resampling) do algoritmo AMCL é dependente do número de atualizações realizadas ao filtro, através do parâmetro resample_interval. Estes três parâmetros serão analisados com mais pormenor com exemplos práticos na Secção 5.2.2., considerando-se que têm um impacto não desprezável no desempenho global do algoritmo. •”recovery_alpha_slow”: Taxa de decaimento exponencial do filtro lento, de média pesada, integrado no modelo que determina a necessidade de adicionar partículas em posições aleatórias pelo mapa. •”recovery_alpha_fast”: Taxa de decaimento exponencial do filtro rápido, de média pesada, integrado no modelo que determina a necessidade de adicionar partículas em posições aleatórias pelo mapa. Estes dois parâmetros integram o algoritmo Augmented MCL (figura 5.32), cuja ideia principal passa por adicionar partículas aleatórias ao mapa, para recuperar de uma situação de ”perdido” ou ”raptado”, pesando as médias a curto e longo prazo da probabilidade das medições atuais zt dado o estado atual do robô ut, o mapa me as medições efetuadas no ciclo anterior zt−1. Considera-se que, nesta situação em estudo, estes parâmetros devem ser desligados (atribuindo o valor 0). Isto porque, mesmo no caso em que o robô está perdido, a probabilidade de acertar na posição real deste apenas com algumas partículas distribuídas aleatoriamente pelo mapa é extremamente baixa. •”initial_pose_x”: Coordenada x da média da posição inicial do robô, utilizada para a inicialização do filtro com distribuição Gaussiana. •”initial_pose_y”: Coordenada y da média da posição inicial do robô, utilizada para a inicialização do filtro com distribuição Gaussiana. 5.2 Análise e comparação de várias configurações do AMCL 97 Figura 5.32: Pseudo-algoritmo do modelo Augmented MCL [2]. •”initial_pose_a”: Ângulo de orientação da média da posição inicial do robô, utilizada para a inicialização do filtro com distribuição Gaussiana. •”initial_cov_xx”: Covariância x×xinicial, utilizada para a inicialização do filtro com distribuição Gaussiana. •”initial_cov_yy”: Covariância y×yinicial, utilizada para a inicialização do filtro com distribuição Gaussiana. •”initial_cov_aa”: Covariância θ×θinicial, utilizada para a inicialização do filtro com distribuição Gaussiana. Os parâmetros initial_pose_x,initial_pose_y einitial_pose_a definem a posição e orientação inicial do robô, respctivamente, para a inicialização do filtro. As partículas concentram-se à volta da posição inicial especificada, com uma dispersão igual aos valores das covariâncias initial_cov_xx,initial_cov_yy einitial_cov_aa introduzidas. Parâmetros do modelo do laser 98 Análise dos métodos de localização •”laser_min_range”: Alcance mínimo para ser considerado nas medidas do laser. Ao atribuir o valor -1, o programa considera que o alcance mínimo é igual ao valor de distância mínimo no conjunto de dados de cada varrimento. •”laser_max_range”: Alcance máximo para ser considerado nas medidas do laser. Ao atribuir o valor -1, o programa considera que o alcance máximo é igual ao valor de distância máximo no conjunto de dados de cada varrimento. •”laser_max_beams”: Define o número de feixes do laser, igualmente espaçados, a serem considerados na fase de atualização do algoritmo. Na fase de atribuição de pesos a cada uma das partículas com base nos dados sensoriais do robô (5.31 - linha 5), o algoritmo AMCL apenas utiliza um determinado número de feixes do laser, igualmente espaçados, para os cálculos que efetua, valor especificado pelo parâmetro laser_max_beams, cujo valor máximo permitido é 100. •”laser_z_hit”: Peso dado à parte do modelo que considera os dados obtidos pelo laser como sendo corretos (z_hit). •”laser_z_short”: Peso dado à parte do modelo que considera os dados obtidos pelo laser como sendo mais curtos do que o esperado (z_short). •”laser_z_max”: Peso dado à parte do modelo que considera os dados obtidos pelo laser como sendo demasiado longos (z_max). •”laser_z_rand”: Peso dado à parte do modelo que considera uma componente aleatória nos dados obtidos pelo laser (z_rand). •”laser_sigma_hit”: Desvio padrão da distribuição Gaussiana usado no modelo z_hit. •”laser_lambda_short”: Fator do decaimento exponencial para o modelo z_short. •”laser_likelihood_max_dist”: Distância máxima de desvio com o mapa para que esses pontos detetatos pelo laser não sejam considerados. •”laser_model_type”: Modelo do laser a utilizar: ”beam” ou ”likelihood_field’. Para o modelo do laser existem dois algoritmos possíveis, os quais utilizam os parâmetros anteriores, e que serão descritos pormenorizadamente de seguida. 5.2.1.1 Modelo ”Beam Range Finder” A opção ”beam” para o parâmetro ”laser_model_type’ implementa o modelo ”beam range finder” o qual calcula a probabilidade em cada medida do sensor correspondente a cada feixe do laser, zk t, de um determinado varrimento zt. Com vista a modelar os possíveis cenários de um varrimento laser, a função de densidade de probabilidade é alcançada a partir de 4 densidades que definem um determinado tipo de erro: 5.2 Análise e comparação de várias configurações do AMCL 99 1. Medidas corretas com ruído do laser: Considerando que as medidas do laser, mesmo devolvendo o valor correcto, estão sujeitas a erros, a função de probabilidade destas medidas, phit é modelada (5.8) por uma curva Gaussiana de média zk∗ te desvio padrão σhit (parâmetro do ruído do modelo das medições do laser: laser_sigma_hit), representada na Figura 5.33. Para se obter a distância ”correta” ao obstáculo medido, utiliza-se o método de ray casting, em que é realizado um varrimento ao longo do feixe do laser, usando o mapa (m) e a posição atual estimada para o robô (xt), com vista a detetar o obstáculo mais próximo ao longo do mesmo. phit (zk t|xt,m) =      ηλshort 1 √2πσ2 hit e−1 2 (zk t−zk∗ t)2 σ2 hit 0≤zk t≤zmax 0else (5.8) Figura 5.33: Distribuição da densidade de probabilidade Gaussiana phit [2]. 2. Medidas curtas, devido a objectos inesperados: Com objetos e pessoas constantemente a movimentar-se no ambiente circundante do robô, os dados recolhidos pelo laser são com frequência mais curtos do que se esperava, em comparação com o mapa. Deste modo, é importante modelar estes possíveis resultados como ruído, seguindo a estratégia do ponto 1. A probabilidade de deteção de objetos inesperados à volta do robô decresce com a distância ao mesmo, definindo-se esta probabilidade através de uma distribuição exponencial negativa, pshort , formulada em 5.9 e visualizada na Figura 5.34. Esta densidade de probabilidade depende de λshort , um parâmetro intrínseco do modelo das medições do laser, recebido pelo programa através do laser_lambda_short. pshort(zk t|xt,m) =    ηλshort e−λshort zk t0≤zk t≤zk∗ t 0else (5.9) 3. Medidas falhadas: No caso dos feixes do laser poderem ser absorvidos por superfícies escuras, ou serem refletidos noutras direções, o valor devolvido pelo laser é o máximo valor do alcance do mesmo. 100 Análise dos métodos de localização Figura 5.34: Distribuição da densidade de probabilidade exponencial pshort [2]. Visto que este é um evento frequente, é importante modelá-lo (5.10), como pmax (Figura 5.35), com vista à sua integração no modelo final de densidade de probabilidade do laser. pmax(zk t|xt,m) =    1z=zmax 0else (5.10) Figura 5.35: Distribuição da densidade de probabilidade uniforme pmax [2]. 4. Medidas aleatórias: Por vezes, os lasers devolvem medidas dificilmente explicadas (e.g., por interferência com outros sensores), modelando-se este tipo de dados como eventos aleatórios, com uma distribuição uniforme, prand, ao longo do intervalo [0; zmax]5.11, como é possível visualizar na Figura 5.36. prand(zk t|xt,m) =    1 zmax 0≤zk t≤zmax 0else (5.11) Assim, obtém-se a função de densidade de probabilidade combinando estes 4 modelos, representada na Figura 5.37, através de uma média pesada controlada pelos parâmetros zhit ,zshort ,zmax 5.2 Análise e comparação de várias configurações do AMCL 101 Figura 5.36: Distribuição da densidade de probabilidade uniforme prand [2]. ezrand, com zhit +zshort +zmax +zrand =1. p(zk t|xt,m) =       zhit zshort zmax zrand       T ·      phit (zk t|xt,m) pshort(zk t|xt,m) pmax(zk t|xt,m) prand(zk t|xt,m)       (5.12) Figura 5.37: Distribuição da densidade de probabilidade combinada dos 4 modelos: phit,pshort , pmax eprand [2]. 108 Análise dos métodos de localização Configurações 1 e 2 Na Tabela 5.17 estão representados os valores dos parâmetros utilizados para as duas primeiras configurações (Config1_OA_001 eConfig2_OA_02). Ambas as configurações utilizam um número fixo de partículas (300) e possuem iguais pesos para os dois modelos do laser (phit eprand). A principal diferença encontra-se nos parâmetros referentes à odometria, em que a Config1_OA_001 possui valores mais baixos (menor incerteza para a propagação da odometria) enquanto que a Config2_OA_02 tem estes valores mais elevados. Estes parâmetros têm um grande impacto na dispersão das partículas (quanto maior o valor dos parâmetros maior a dispersão) ao longo da movimentação do robô. Com isto, quer-se verificar de que forma a precisão e a robustez do algoritmo depende da alteração destes parâmetros. Tabela 5.17: Definição da Config1_OA_001 eConfig2_OA_02. Config1_OA_001 Config2_OA_02 min_particles 300 300 max_particles 300 300 laser_z_hit 0.85 0.85 laser_z_rand 0.15 0.15 odom_alpha1 0.01 0.2 odom_alpha2 0.01 0.2 odom_alpha3 0.01 0.2 odom_alpha4 0.01 0.2 Tabela 5.18: Resultados estatísticos das Config1_OA_001 eConfig2_OA_02, num ambiente sem outliers. Config1_OA_001 Config2_OA_02 Média do erro de distância (m) 0.0363 0.0405 Média do erro angular absoluto (o) 0.6556 0.6043 Erro máximo de distância (m) 0.0995 0.1067 Erro angular absoluto máximo (o) 3.707 2.955 Variância do erro da distância 5.015·10−44.039·10−4 Variância do erro angular 0.6928 0.3711 Pelas Figuras 5.41 e5.42 é possível comparar as evoluções dos erros em x, y, orientação e distância, em relação à posição referenciada pelo groundtruth. Os resultados são similares, notando-se um pequeno aumento geral destes erros ao longo do percurso do robô, no caso da Config2_OA_02, pelo facto dos parâmetros da odometria possuírem valores mais elevados, aumentando a incerteza na movimentação do robô e consequentemente dispersando um pouco mais as partículas, levando a uma maior oscilação nas estimativas. 5.2 Análise e comparação de várias configurações do AMCL 109 (a) (b) Figura 5.41: Evolução do erro em x, y e em ângulo, ao longo do percurso, utilizando a (a) Config1_OA_001 e a (b) Config2_OA_02, na ausência de outliers. 110 Análise dos métodos de localização (a) (b) Figura 5.42: Evolução do erro em distância, ao longo do percurso, utilizando a (a) Config1_OA_001 e a (b) Config2_OA_02, na ausência de outliers. 5.2 Análise e comparação de várias configurações do AMCL 111 Configurações 3 e 4 Prepararam-se mais duas configurações (Config3_OA_05 eConfig4_OA_05L), com os valores especificados na Tabela 5.19. Para o caso da Config3_OA_05, mantiveram-se constantes os parâmetros das configurações anteriores, tendo-se apenas aumentado o valor dos coeficientes da odometria para 0.5, dispersando ainda mais as partículas com o movimento do robô. Na Config4_OA_05L, para além disto, alterou-se os pesos dados aos modelos do laser phit eprand, atribuindo o valor máximo ao primeiro e consequentemente eliminando a componente aleatória. Assim, todos os dados do laser serão processados de acordo com o modelo phit , dando, desta forma, um peso superior à componente do laser, comparativamente às configurações anteriores. Tabela 5.19: Definição da Config3_OA_05 eConfig4_OA_05L. Config3_OA_05 Config4_OA_05L min_particles 300 300 max_particles 300 300 laser_z_hit 0.85 1.0 laser_z_rand 0.15 0.0 odom_alpha1 0.5 0.5 odom_alpha2 0.5 0.5 odom_alpha3 0.5 0.5 odom_alpha4 0.5 0.5 Pelo conjunto de resultados obtidos para estas 4 configurações (Tabelas 5.18 e5.20), verificase que há um pequeno aumento (≈4 a 7 mm) no erro médio da estimativa de posição à medida que se aumenta os parâmetros referentes ao ruído esperado da odometria. Com o aumento do peso dado ao zhit do laser, notou-se uma ligeira melhoria no erro de posição (≈2 mm em média) ao longo do tempo (Figuras 5.43 e5.44, visto que o ambiente utilizado para estes testes não possui objetos externos ao mapa e por isso o laser retorna essencialmente dados corretos. Mesmo assim, os resultados obtidos com estas 4 configurações são bastante similares para um ambiente sem outliers, como seria de esperar. Tabela 5.20: Resultados estatísticos das Config3_OA_05 eConfig4_OA_05L, com grande quantidade de obstáculos no mapa. Config3_OA_05 Config4_OA_05L Média do erro de distância (m) 0.0438 0.0425 Média do erro angular absoluto (o) 0.7640 0.8721 Erro máximo de distância (m) 0.0854 0.0868 Erro angular absoluto máximo (o) 3.215 3.348 Variância do erro da distância 4.362·10−44.220·10−4 Variância do erro angular 0.3584 0.4931 112 Análise dos métodos de localização (a) (b) Figura 5.43: Evolução do erro em x, y e em ângulo, ao longo do percurso, utilizando a (a) Config3_OA_05 e a (b) Config4_OA_05L. 5.2 Análise e comparação de várias configurações do AMCL 113 (a) (b) Figura 5.44: Evolução do erro em distância, ao longo do percurso, utilizando a (a) Config3_OA_05 e a (b) Config4_OA_05L. 114 Análise dos métodos de localização Na Figura 5.45 estão representados as três parametrizações escolhidas para o modelo da odometria, onde se pode verificar que, à medida que os valores atribuídos a esses parâmetros aumentam, a dispersão das partículas ao longo do movimento do robô também aumenta. Essa dispersão é realizada de forma aleatória a partir de uma distribuição Gaussiana com média nula e desvio padrão σ. O desvio padrão é controlado proporcionalmente através dos parâmetros da odometria (odom_alpha’s), aumentando com o aumento destes valores. (a) (b) (c) Figura 5.45: Dispersão das partículas em função dos parâmetros odom_alpha’s com (a) 0.01 (b) 0.2 e (c) 0.5. 5.2 Análise e comparação de várias configurações do AMCL 115 5.2.2.2 Erros forçados de odometria Com vista a testar os efeitos dos erros de odometria no algoritmo AMCL, forçou-se a derrapagem das rodas do robô em dois momentos da trajetória. Para este caso, considerou-se interessante analisar as Config1_OA_001,Config2_OA_02 eConfig3_OA_05. Visto que os erros induzidos nesta secção estão relacionados com a odometria do robô, excluiu-se desta análise a Config4_OA_05L, cuja única diferença de parametrização está relacionada com o modelo do laser. Config1_OA_001,Config2_OA_02 eConfig3_OA_05 (a) (b) (c) (d) Figura 5.46: (a) Robô bem localizado, em relação ao groundtruth (seta verde) com uma reduzida dispersão de partículas (setas vermelhas). (b) No primeiro local onde se forçou o erro de odometria a estimativa de posição dada pelo AMCL (seta amarela) ganhou algum erro. (c) Na segunda zona onde houve um erro de odometria o robô já estava com um elevado erro de posição e em (d) continuou a divergir, perdendo-se. Estes exemplos são ilustrativos da importância de possuir uma boa dispersão de partículas à volta da posição estimada do robô, ao longo da sua movimentação, controlada pelos fatores de ajuste da odometria (odom_alpha’s). O facto de se configurar estes parâmetros com valores superiores ao ruído esperado pela odometria traz a vantagem de aumentar a robustez do algoritmo a situações inesperadas em que há perturbação da movimentação do robô por fatores externos ao modelo calculado, permitindo a sua recuperação (Figura 5.47). Na Figura 5.46 é possível verificar-se 116 Análise dos métodos de localização (a) (b) (c) (d) Figura 5.47: (a) Robô bem localizado, em relação ao groundtruth (seta verde) com alguma dispersão de partículas (setas vermelhas). (b) No primeiro local onde se forçou o erro de odometria a estimativa de posição dada pelo AMCL (seta amarela) ganhou algum erro. (c) Segunda zona onde houve um erro nos dados da odometria. (d) O robô conseguiu recuperar a localização correta. um fenómeno denominado de particle deprivation [2], que ocorre quando há uma maior concentração das partículas à volta de uma determinada posição errada e consequentemente deixam de haver partículas na localização correta do robô, impossibilitando a sua recuperação. Tabela 5.21: Resultados estatísticos das Config1_OA_001,Config2_OA_02 eConfig3_OA_05, com erros forçados de odometria, em dois momentos. Config1_OA_001 Config2_OA_02 Config3_OA_05 Média do erro de distância (m) 0.3984 0.0875 0.0758 Média do erro angular absoluto (o) 14.08 1.370 1.338 Erro máximo de distância (m) 1.056 0.5119 0.5317 Erro angular absoluto máximo (o) 33.55 12.48 8.967 Variância do erro da distância 0.1055 0.0132 0.0119 Variância do erro angular 154.7 4.609 2.601 A parametrização Config3_OA_05 devolve valores similares aos da Config2_OA_02, tendo-se utilizado os coeficientes da odometria um pouco mais elevados. Assim, é possível verificar que o aumento da dispersão das partículas, para este exemplo, diminui o tempo de recuperação da falha 5.2 Análise e comparação de várias configurações do AMCL 117 de odometria (Figuras 5.48 e5.49) e diminui o erro geral ao longo do percurso, representado na Tabela 5.21. À medida que se aumenta a dispersão das partículas, a tolerância a erros de odometria também cresce, aumentando a área de erro a partir da qual é possível recuperar a posição correta. (a) (b) (c) Figura 5.48: Evolução do erro em x, y e em ângulo, ao longo do percurso, utilizando a (a) Config1_OA_001, (b) Config2_OA_02 e (c) Config3_OA_05. 124 Análise dos métodos de localização Config3_OA_05 eConfig4_OA_05L Para estas duas configurações são utilizados valores elevados nos parâmetros do modelo do ruído da odometria, aumentando a dispersão ao longo do movimento do robô, tornando-se mais sensível aos dados recebidos do laser, como é possível verificar na Figura 5.54. Isto porque, havendo uma maior variedade no conjunto das estimativas à volta da posição do robô, e como os pesos atribuídos a essas estimativas estão dependentes dos dados do laser, a influência desses pontos resulta numa mais provável alteração das posições estimadas, do que se as partículas estivessem todas sensivelmente na mesma localização. (a) (b) (c) (d) Figura 5.54: Sequência de imagens referentes à Config4_OA_05L: (a) e (b) Robô bem localizado, em relação ao groundtruth (seta verde) com alguma dispersão de partículas (setas vermelhas). (c) Devido aos outliers presentes no mapa, o robô ganha algum erro de posição (seta amarela) e formam-se duas concentrações principais de partículas, paralelas. (d) As partículas correspondentes à posição desajustada do robô começam a ser eliminadas e o robô volta a convergir para a posição correta. Adicionalmente, ao atribuir-se o valor mais elevado ao parâmetro do modelo phit (Config4_OA_05L), os resultados obtidos têm um erro mais elevado (Tabela 5.24) comparativamente com as restantes configurações, devido à influência dos outliers nos dados fornecidos pelo laser. Nas Figuras 5.55 e5.56 é possível identificar um momento, na fase final do percurso do robô, em que este é fortemente afetado pelos outliers presentes no mapa, levando a um aumento do erro nesse período. De forma complementar, na Figura 5.54 (c) e (d) verifica-se esse momento visualmente, em que as estimativas mantidas pelo filtro se dividem em dois grupos distintos, em que um deles 5.2 Análise e comparação de várias configurações do AMCL 125 engloba a posição real do robô e outro está vários centímetros deslocado. Nesse momento a amostra com maior peso (probabilidade) ganhou algum erro em relação à posição do groundtruth, devido à influência dos outliers nos dados do laser. (a) (b) Figura 5.55: Evolução do erro em x, y e em ângulo, ao longo do percurso, utilizando a (a) Config3_OA_05 e a (b) Config4_OA_05L. 126 Análise dos métodos de localização (a) (b) Figura 5.56: Evolução do erro em distância, ao longo do percurso, utilizando a (a) Config3_OA_05 e a (b) Config4_OA_05L. Tabela 5.24: Resultados estatísticos das Config3_OA_05 eConfig4_OA_05L, num ambiente com grande quantidade de outliers. Config3_OA_05 Config4_OA_05L Média do erro de distância (m) 0.0879 0.1071 Média do erro angular absoluto (o) 2.425 1.754 Erro máximo de distância (m) 0.2690 0.3105 Erro angular absoluto máximo (o) 9.309 7.294 Variância do erro da distância 4.390·10−38.172·10−3 Variância do erro angular 4.578 2.092 5.2 Análise e comparação de várias configurações do AMCL 127 5.2.2.5 Resultados estatísticos em função do número de partículas Com vista a estudar o efeito prático do número de partículas no algoritmo AMCL, em termos de precisão das suas estimativas e na robustez geral do filtro, utilizaram-se os mesmos ambientes de estudo e configurações anteriormente apresentadas, obtendo-se os resultados expostos nas Tabelas 5.25,5.26,5.27 e5.28. Assim, utilizaram-se três quantidades diferentes para o número de partículas: 100, 200 e 300, tendo-se realizado uma análise a estes resultados na Secção 5.2.2.6. Tabela 5.25: Comparação estatística do desempenho do AMCL em função do número de partículas utilizadas para o ambiente sem outliers. Configurações Config1_OA_001 Config2_OA_02 Config3_OA_05 Número de partículas 100 200 300 100 200 300 100 200 300 Média do erro de distância (m) 0.0429 0.0393 0.0363 0.0566 0.0507 0.0405 0.0523 0.0401 0.0438 Média do erro angular (o) 0.6592 0.5674 0.6556 0.8559 0.6607 0.6043 0.9767 0.8806 0.7640 Erro máximo de distância (m) 0.1003 0.1000 0.0995 0.1648 0.0938 0.1067 0.1856 0.1000 0.0854 Erro angular máximo (o) 3.668 3.661 3.708 3.673 2.729 2.955 3.783 3.551 3.215 Variância do erro da distância 0.0006 0.0003 0.0005 0.0014 0.0002 0.0004 0.0010 0.0004 0.0004 Variância do erro angular 0.6845 0.6317 0.6928 0.5274 0.3797 0.3711 0.6741 0.5944 0.3584 Tabela 5.26: Comparação estatística do desempenho do AMCL em função do número de partículas utilizadas, com erros de odometria. Configurações Config2_OA_02 Config3_OA_05 Número de partículas 100 200 300 100 200 300 Média do erro de distância 0.1061 0.1108 0.0875 0.1633 0.1021 0.0758 Média do erro angular 1.265 2.976 1.370 3.022 2.406 1.339 Erro máximo de distância 3.239 4.002 0.5119 7.657 4.334 0.5317 Erro angular máximo 13.66 170.1 12.48 166.1 113.6 8.967 Variância do erro da distância 0.0821 0.1204 0.0132 0.4160 0.1350 0.0119 Variância do erro angular 2.564 205.5 4.609 192.0 89.94 2.600 Tabela 5.27: Comparação estatística do desempenho do AMCL em função do número de partículas utilizadas para o ambiente com poucos outliers. Configurações Config1_OA_001 Config2_OA_02 Número de partículas 100 200 300 100 200 300 Média do erro de distância (m) 0.0369 0.0311 0.0338 0.0381 0.0334 0.0351 Média do erro angular (o) 0.4080 0.4996 0.5214 0.9169 0.7292 0.5029 Erro máximo de distância (m) 0.0607 0.0601 0.0824 0.0730 0.0645 0.0962 Erro angular máximo (o) 1.356 1.495 1.642 3.296 2.521 1.856 Variância do erro da distância 0.0001 0.0002 0.0002 0.0003 0.0002 0.0003 Variância do erro angular 0.1097 0.1470 0.1753 0.5329 0.2821 0.1461 128 Análise dos métodos de localização Tabela 5.28: Comparação estatística do desempenho do AMCL em função do número de partículas utilizadas para o ambiente com muitos outliers. Configurações Config1_OA_001 Config2_OA_02 Config3_OA_05 Número de partículas 100 200 300 100 200 300 100 200 300 Média do erro de distância (m) 0.0402 0.0365 0.0386 0.0602 0.0445 0.0696 0.1760 0.1109 0.0879 Média do erro angular (o) 0.5782 0.5019 0.5488 1.284 0.8197 0.7069 2.094 2.361 2.425 Erro máximo de distância (m) 0.0811 0.0778 0.0766 0.1290 0.1540 0.1925 0.5439 0.3094 0.2690 Erro angular máximo (o) 1.455 1.574 1.708 4.021 2.646 2.018 8.536 6.392 9.309 Variância do erro da distância 0.0002 0.0002 0.0002 0.0008 0.0008 0.0025 0.0139 0.0058 0.0044 Variância do erro angular 0.1162 0.1585 0.1438 0.7379 0.2988 0.2164 3.701 3.215 4.578 5.2 Análise e comparação de várias configurações do AMCL 129 5.2.2.6 Conclusões Após ter-se analisado várias configurações aplicadas em ambientes com diferentes características, o maior destaque passa pela boa parametrização dos coeficientes para o modelo da odometria, os quais revelam ter um grande impacto no desempenho geral do filtro em diversas circunstâncias. A utilização de coeficientes elevados para este modelo aumenta o erro de localização mas simultaneamente torna o algoritmo mais robusto a perturbações inesperadas, permitindo a sua recuperação. Os parâmetros referentes ao laser, por si só, não têm muita influência na robustez ou precisão do filtro, devendo atribuir-se uma maior percentagem ao fator z_hit, em oposição ao parâmetro z_rand, que apenas distribui uniformemente os pesos das partículas. Quanto à influência do número de partículas, é de notar que os resultados obtidos com um número de partículas superior são geralmente melhores do que com um número mais reduzido de partículas, sendo que as experiências realizadas com 100 partículas revelaram sempre piores resultados do que com 200 ou 300. Para as condições deste projeto, devido às limitações do computador presente no robô, o máximo número de partículas considerado viável é 300, para ser possível manter um ciclo de execução dos programas suficientemente baixo (ver 5.3). A precisão de cada configuração em função do número de partículas é mais visível num ambiente onde não existem obstáculos que perturbem os dados obtidos pelo robô, como é o caso da situação onde não existem outliers. Nessas circunstâncias, é claro, pelos dados apresentados na Tabela 5.25, que à medida que há um aumento do número de partículas a estimativa de localização também melhora. Esta é uma situação ideal que na prática raramente acontece. Por outro lado, em alguns exemplos onde houveram perturbações externas ao sistema (aumento do número de outliers, erros de odometria forçados), as configurações utilizando 200 partículas revelaram resultados ligeiramente melhores em alguns campos de análise (e.g., erro médio de posição) do que com 300, não querendo dizer que esta diminuição no número de partículas seja mais vantajosa nestas circunstâncias. O algoritmo AMCL é um algoritmo de localização probabilístico onde entram processos de geração de valores de forma aleatória (e.g., na dispersão das partículas ao longo da movimentação do robô, de acordo com as covariâncias especificadas no modelo da odometria), sendo que, em alguns casos, a presença de partículas em posições específicas levam a uma mais rápida convergência e diminuição do erro na movimentação do robô. Quanto maior o número de partículas presentes no filtro, maior a probabilidade de existirem amostras nos locais ”corretos”, levando a um melhor seguimento de posição e a um menor erro. Por outro lado, na fase de seguimento de posição, depois do filtro convergir para uma posição correta, o número de partículas utilizado pode ser reduzido, não perdendo muita precisão na estimação da posição. Na verdade, uma das extensões do algoritmo AMCL, que adapta o número de partículas do filtro de acordo com a fase atual robô (e.g., aumenta o número de partículas numa fase de localização global ou recuperação e diminui esse número durante o seguimento de posição), é baseada exatamente nesse princípio. Para o conjunto de testes realizados, é relevante que as condições de comparação de cada configuração fossem o mais constantes e previsíveis possível, com vista a colocar todas em 130 Análise dos métodos de localização pé de igualdade, não se tendo utilizado um número variável de partículas, que no entanto pode ser vantajoso em aplicações práticas. Deste modo, pode concluir-se, com base nas experiências efetuadas, que a utilização de um número próximo ou menor que 100 partículas torna o sistema menos preciso e menos robusto (e.g., no caso da necessidade de recuperação de alguma situação inesperada, não havendo número suficiente de partículas para esse efeito), podendo definir-se este como o limite mínimo. 5.3 Comparação entre o AMCL e o Perfect Match 131 5.3 Comparação entre o AMCL e o Perfect Match Nesta secção irá realizar-se uma análise comparativa entre os dois algoritmos de localização que têm sido explorados até ao momento: AMCL e Perfect Match. Depois das comparações efetuadas entre cada algoritmo, alterando alguns dos parâmetros que têm mais impacto no desempenho dos mesmos, selecionou-se a configuração que devolveu melhores resultados para cada um e comparou-se os dois algoritmos, utilizando essas parametrizações. Mantiveram-se as 4 situações já apresentadas na Secção 5.1.3 como ambientes de teste para esta análise. Um dos dados iniciais relevantes para este estudo passa pelo tempo de processamento necessário para a execução dos programas, o que para computadores com processadores limitados, como acontece neste projeto, torna-se um factor crucial para a sua aplicação em tempo real. Assim, realizaram-se alguns testes no robô Clever, com vista a quantificar e analisar a evolução dos tempos de ciclo dos dois algoritmos de localização. Figura 5.57: Evolução do tempo de ciclo para a execução do programa AMCL, com 300 partículas em comparação com o programa Perfect Match. Tabela 5.29: Tempos de ciclo médio, máximo e mínimo para o AMCL (com 300 partículas) e para o Perfect Match. AMCL PM Número de partículas 300 - Média do tempo de ciclo (ms) 73.0790 10.2747 Tempo máximo (ms) 171 32 Tempo mínimo (ms) 48 2 132 Análise dos métodos de localização Tabela 5.30: Tempos de ciclo médio, máximo e mínimo para o AMCL (com 200 partículas) e para o Perfect Match. AMCL PM Número de partículas 200 - Média do tempo de ciclo (ms) 50.25 11.97 Tempo máximo (ms) 93 61 Tempo mínimo (ms) 26 5 Figura 5.58: Evolução do tempo de ciclo para a execução do programa AMCL, com 200 partículas em comparação com o programa Perfect Match. Figura 5.59: Evolução do tempo de ciclo para a execução do programa AMCL, com 100 partículas em comparação com o programa Perfect Match. 5.3 Comparação entre o AMCL e o Perfect Match 133 Tabela 5.31: Tempos de ciclo médio, máximo e mínimo para o AMCL (com 100 partículas) e para o Perfect Match. AMCL PM Número de partículas 100 - Média do tempo de ciclo (ms) 31.18 10.45 Tempo máximo (ms) 51 32 Tempo mínimo (ms) 20 3 Figura 5.60: Evolução do tempo de ciclo para a execução do programa AMCL, com 50 partículas em comparação com o programa Perfect Match. Os dados apresentados nas figuras e tabelas anteriores foram obtidos exactamente nas mesmas condições tanto para o AMCL como para o Perfect Match, ao serem executados em paralelo durante a movimentação do robô. De acordo com estes valores, para que o AMCL atinja a leveza computacional do Perfect Match, teriam de se utilizar menos de 50 partículas, o que seria um valor demasiado baixo para este algoritmo, comprometendo muito o seu desempenho. Por outro lado, a operação do filtro utilizando 300 partículas tem tempos de ciclo bastante elevados (com uma média de 73 ms e chegando aos 173 ms - Tabela 5.29), tornando-se no limite máximo viável para as aplicações pensadas para este projeto, em que o robô pode atingir velocidades na ordem dos 0.5 m/s. Tabela 5.32: Tempos de ciclo médio, máximo e mínimo para o AMCL (com 50 partículas) e para o Perfect Match. AMCL PM Número de partículas 50 - Média do tempo de ciclo (ms) 19.84 ms 8.895 ms Tempo máximo (ms) 38 ms 29 ms Tempo mínimo (ms) 9 ms 2 ms