Implementação de Fusão Sensorial para Localização de um Veículo Autônomo

Transcrição

1 Universidade Federal de Minas Gerais Escola de Engenharia Curso de Graduação em Engenharia de Controle e Automação Implementação de Fusão Sensorial para Localização de um Veículo Autônomo João Henrique Rodrigues Costa Orientador: Guilherme Augusto Silva Pereira Supervisor: Jullierme Emiliano Alves Dias Belo Horizonte, Janeiro de 2013

2 Monografia Implementação de Fusão Sensorial para Localização de um Veículo Autônomo Monografia submetida à banca examinadora designada pelo Colegiado Didático do Curso de Graduação em Engenharia de Controle e Automação da Universidade Federal de Minas Gerais, como parte dos requisitos para aprovação na disciplina Projeto Final de Curso II. Belo Horizonte, Janeiro de 2013

3 Agradecimentos Agradeço primeiramente a Deus por me conceder saúde para realizar o curso de graduação em Engenharia de Controle e Automação. Agradeço aos meus pais pelo amor, carinho e apoio financeiro. Aos meus familiares pela ajuda nos momentos difíceis, especialmente a minha tia Inha que me acolheu na sua casa em Belo Horizonte. Agradeço a minha namorada, Stefânia, pelo amor e compreensão nos momentos de estudo e a todos os meus amigos de Curvelo e Belo Horizonte pelos incentivos. Agradeço também a todos os COROnianos, Professor Guilherme Pereira, Jullierme, Breno, Rogério e todos os outros, pela ajuda e pelos ensinamentos. Agradeço também a FAPEMIG pelo financiamento deste projeto. i

4 Resumo No que diz respeito ao desenvolvimento de veículos autônomos terrestres, a tarefa de localização consiste em um dos passos mais importantes a serem desenvolvidos. Para viabilizar a locomoção autônoma de um automóvel, a estimativa da posição geográfica deve ser a mais precisa possível. Para que o problema da localização seja solucionado, o veículo necessita de informações de sensores como GPS, Encoders e Unidade de Medição Inercial, a qual possui acelerômetros, girômetros e magnetômetros. As informações de cada sensor, separadamente, são incompletas e muitas vezes ruidosas, portanto, a confiabilidade na informação da posição e orientação depende da combinação, por meio de técnicas de fusão sensorial, das melhores características de cada sensor. Portanto, o presente trabalho utiliza técnicas de fusão sensorial, especificamente o Filtro de Kalman Estendido, para que as medições de cada sensor sejam combinadas de forma que a confiabilidade em termos de robustez e precisão seja aumentada. Para isto, a predição da posição do veículo foi testada off-line, para algumas configurações de sensores e modelos do sistema. A configuração que apresentou melhor reconstrução da trajetória, menor traço da matriz de covariância da estimativa da posição e maior robustez da estimativa à perda de sinal do GPS, foi escolhida para ser implementada on-line para localização em tempo real no veículo.

5 Abstract Regarding the development of autonomous ground vehicles, the task of localization is one of the most important steps to be developed. To enable autonomous locomotion of a car, estimated geographical location should be the most accurate possible. For the localization problem to be solved, the vehicle requires information from sensors such as GPS, Encoders and Inertial Measurement Unit, which is composed by accelerometers, gyroscopes and magnetometers. The information of each sensor, separately, is incomplete and often noisy. Thus the reliability in position and orientation information depends on the combination, through techniques of sensory fusion of the best features of each sensor. Therefore, the present work utilizes techniques of sensory fusion, specifically the Extended Kalman Filter, so that the measurements of each sensor are combined in such a way the reliability in terms of robustness and accuracy is increased. For this, the prediction of the position of the vehicle was tested off-line, for some configurations of sensors and system models. The configuration that presented the best trajectory reconstruction, the lower trace of covariance matrix estimation of position, and greater robustness of estimation of GPS signal loss was chosen to be implemented online for real-time localization on the vehicle.

6 Sumário Agradecimentos Resumo Abstract Lista de Figuras Lista de Tabelas i ii iii viii viii 1 Introdução Contextualização Problema Motivação Contribuições Organização do Texto Revisão Bibliográfica Veículos Autônomos Sistema de Localizaçao Localização Absoluta Localização Relativa Fusão Sensorial O Filtro de Kalman Trabalhos Relacionados iv

7 3 Metodologia Modelagem do Sistema Sensores do Sistema de Localização GPS Unidade de Medição Inercial Sensor de Velocidade das Rodas Encoder do Volante Implementação da Fusão Sensorial e Resultados Configurações Configuração Configuração Configuração Estimativa dos ruídos associados a entrada e a medição e determinação das Matrizes de Covariância Q e R Resultados Experimentais Conclusões e Trabalhos Futuros Principais Resultados Obtidos e Conclusões Sugestões para Trabalhos Futuros Referências Bibliográficas 58 v

8 Lista de Figuras 1.1 Carro Autônomo da UFMG Projeto SARTRE, desenvolvido pela VOLVO Veículo MadeInGermany da Freie Universität Berlin Fusão direta dos dados do sensor Fusão dos dados do sensor através de vetor de características Veículo Odin da equipe VictorTango Sistema de Coordenadas fixado ao carro Sistema de Coordenadas ENU(Leste, Norte, Cima) Zonas do Sistema de Coordenadas UTM Modelo cinemático do tipo uniciclo para o carro Modelo de Ackerman para o carro GPS Hemisphere R Unidade de Medição Inercial Encoder da roda traseira Motor RE 40 - Maxon para controle do volante Controlador EPOS 24/5 do motor Aceleração em x: sinal IMU(azul), sinal filtrado(amarelo) Comparação da inovação com o limite de confiança ±3 P ix e ±3 P iy para os estados P x e P y utilizando a Configuração Comparação da inovação com o limite de confiança ±3 P ix e ±3 P iy para os estados P x e P y utilizando a Configuração Comparação da inovação com o limite de confiança ±3 P ix e ±3 P iy para os estados P x e P y utilizando a Configuração vi

9 4.5 Gráfico da reconstrução da trajetória, usando a Configuração 1 (percurso inicia no centro do gráfico e termina no canto superior esquerdo) Visão em zoom da reconstrução da trajetória, usando a Configuração 1 (percurso inicia na parte de baixo do gráfico e finaliza no canto superior esquerdo) Visão em zoom da reconstrução da trajetória, indicando a orientação do carro usando a Configuração 1 (percurso inicia na parte de baixo do gráfico e finaliza no canto superior esquerdo) Gráfico da reconstrução da trajetória, usando a Configuração 2 (percurso inicia no centro do gráfico e termina no canto superior esquerdo) Visão em zoom da reconstrução da trajetória, usando a Configuração 2 (percurso inicia na parte de baixo do gráfico e finaliza no canto superior esquerdo) Visão em zoom da reconstrução da trajetória, indicando a orientação do carro usando a Configuração 2 (percurso inicia na parte de baixo do gráfico e finaliza no canto superior esquerdo) Gráfico da reconstrução da trajetória, usando a Configuração 3 (percurso inicia no centro do gráfico e termina no canto superior esquerdo) Visão em zoom da reconstrução da trajetória, usando a Configuração 3 (percurso inicia na esquerda do gráfico e finaliza na parte de baixo) Visão em zoom da reconstrução da trajetória, indicando a orientação do carro usando a Configuração 3 (percurso inicia na esquerda do gráfico e finaliza na parte de baixo) Comparação dos traços das matrizes de covariância da posição e do GPS - Configuração Comparação dos traços das matrizes de covariância da posição e do GPS - Configuração Comparação dos traços das matrizes de covariância da posição e do GPS - Configuração vii

10 Lista de Tabelas 2.1 Filtro de Kalman discreto - Equações de Predição Filtro de Kalman discreto - Equações de Medição Filtro de Kalman Estendido(EKF) - Equações de Predição Filtro de Kalman Estendido(EKF) - Equações de Medição Resumo das Configurações Traço médio da matriz de covariância da estimativa de posição e do GPS viii

11 Capítulo 1 Introdução O crescente avanço na utilização de veículos autônomos é consequentemente acompanhado pelo aumento de desafios para que estes se locomovam sem nenhuma intervenção humana em ambientes externos. Assim, para que a locomoção autônoma em ambientes não controlados seja possível, faz-se necessária a implementação de sistemas de localização, movimentação e percepção, entre outros. Este trabalho apresenta a implementação de métodos e técnicas de fusão sensorial, com o objetivo principal de aumentar a confiabilidade do sistema de localização, em termos de robustez e precisão. 1.1 Contextualização Os robôs podem ser classificados em duas grandes classes, os manipuladores robóticos, que trabalham em um espaço delimitado, executando um trabalho repetitivo e possuem sua maior aplicabilidade na indústria, e os robôs móveis, os quais podem se locomover independente do ambiente em 1

12 que são inseridos [Siegwart and Nourbakhsh, 2004]. O que diferencia a robótica móvel de outras áreas como manipuladores robóticos, inteligência artificial e visão computacional é a escala de espaço que os robôs trabalham [Dudek and Jenkin, 2010]. Os veículos autônomos fazem parte de uma classe de robôs móveis que conseguem executar tarefas, baseando-se em decisões próprias. Os sistemas autônomos, geralmente possuem as seguintes características [Pereira, 2005]: sensores para obter dados do ambiente; capacidade de processamento para interpretar os dados e tomar decisões; comunicação para obter informação complementar aos sensores e interagir com o operador, permitindo, quando necessário, sua intervenção; gestão da energia do sistema e; mobilidade mesmo diante de alterações do ambiente. Para que a locomoção autônoma seja realizada, o veículo deve possuir um sistema de navegação eficiente. Esse sistema deve conduzir o veículo a um objetivo, através de ambientes conhecidos ou não e desviando de possíveis obstáculos. A navegação deve ser feita de acordo com uma arquitetura robótica, que leva em conta os sensores utilizados e como os dados são tratados. Desta forma, busca executar três premissas fundamentais: perceber, planejar e agir [Pham, 2002]. Para atender tais premissas, foram desenvolvidos diversos modelos de arquitetura robótica, merecendo destaque três deles, por serem fortemente utilizadas: Deliberativa: A partir de dados coletados pelos sensores do robô, este realiza uma modelagem do ambiente em que está presente. De acordo 2

13 com essa modelagem, o robô planeja as próximas ações a serem executadas antes de tomá-las. A dificuldade desta arquitetura encontra-se na obtenção de um modelo do ambiente que seja satisfatório e também na tomada de decisões quando uma ação não prevista for necessária. Reativa: Nesta arquitetura não exite planejamento, o robô atua à medida que os dados são obtidos. Assim, não há como prever o caminho que o robô irá percorrer a priori. Tal característica pode ser crítica para alguns tipos de aplicação. Híbrida: Consiste na combinação das duas arquiteturas acima, de modo que, o robô realiza um planejamento inicial e à medida que forem surgindo mudanças no ambiente, ele realiza ações reativas. Portanto, para que o veículo autônomo navegue de forma satisfatória, fazse necessário que ele se localize, ou seja, saiba onde está, e que ele tenha um objetivo determinado, isto é, para onde ir. O veículo também deve planejar o movimento que deve fazer para alcançar o objetivo e assim, executar o controle para seguir o planejamento realizado. [Borenstein et al., 1997]. 1.2 Problema A navegação autônoma dos veículos terrestres possui como principal dificuldade a localização [Bongard et al., 2008]. Dessa forma, esta tarefa deve ser realizada com a máxima precisão possível. Porém, os sensores envolvidos na localização, tais como: o GPS(Global Positioning System), IMU(Inercial Measurement Unit), Encoders e Bússolas possuem incertezas associadas às suas medidas. Assim, as informações de cada sensor, separadamente, são incompletas e muitas vezes ruidosas. Neste contexto, existem muitas técnicas para prover a localização, porém, cada técnica possui suas vantagens e desvantagens. O algoritmo de localização deve usar o ponto mais forte de cada sensor e suprir os seus pontos 3

14 fracos. Portanto, a confiabilidade na informação da posição e orientação depende da combinação, através de técnicas de fusão sensorial, das melhores características de cada sensor. O sistema de localização baseia-se principalmente nos dados do GPS, pois este é um sensor de posicionamento absoluto. Porém, os ambientes urbanos, com seus túneis e prédios altos e ambientes com vegetação relativamente densa, são um desafio para o GPS. Nestes ambientes, o GPS perde conexão com os satélites, tornando o sinal fraco e ocasionando até mesmo perda de sinal (quando o número de satélites conectados é menor que quatro). Por isso, a presença de outros sensores como a IMU para medir aceleração, o encoder da roda para medir velocidade e o encoder do volante para medir o ângulo da roda, é necessária para se obter o posicionamento do veículo. 1.3 Motivação O desenvolvimento de um veículo autônomo terrestre apresenta diversas justificativas, sendo possível destacar, no contexto do presente trabalho, as sociais, econômicas e ambientais, por considerar-se estas as mais relevantes. Segundo informa a ABETRAN [Ferreira, 2007], 90% dos acidentes de trânsito são causados por falhas humanas, o que gera uma despesa de 8 bilhões por ano aos cofres do INSS [PortaldoTransito, 2011]. Tais estatísticas podem ser drasticamente reduzidas com a introdução de veículos autônomos terrestres no trânsito brasileiro, visto que, os sistemas autônomos aumentam a confiabildade quando comparados com motoristas humanos. Também podemos acrescentar que os veículos autônomos não violam as leis de trânsito, o que evita consideralvemente a chance de colisão. Outro ponto importante que pode-se ressaltar com a introdução de veículos autônomos é a redução dos congestionamentos. A redução dos dispositivos de segurança presentes na via (tais como quebra-molas, semáforos e etc.) e a melhor capacidade de lidar com o fluxo do trânsito são vantagens do uso 4

15 de veículos autônomos. Além disso, tais veículos podem otimizar o percurso, escolhendo o menor caminho e o mais rápido, podendo evitar regiões com fluxo de trânsito intenso. Dessa forma, há uma redução do consumo de combustível, o que é um fator determinante para os altos custos do transporte rodoviário. Por último, esta otimização do tráfego acarreta na diminuição da poluição do ar e assim teremos uma melhora significativa na qualidade de vida das pessoas. Por sua vez, as técnicas de localização se justificam e mostram-se necessárias para garantir a confiabilidade e segurança da própria locomoção autônoma, permitindo que o veículo autônomo tome decisões fundamentadas em informações dotadas de considerável certeza e precisão. Para que ele possa guiar-se pelas regras de trânsito corretas e chegar aos lugares determinados passando pelo menor e melhor caminho. O que acarreta a economia de combustível pois evita trechos engarrafados, interditados ou que apresentam obstáculos. 1.4 Contribuições O presente trabalho foi desenvolvido no laboratório de Sistemas de Computação e Robótica da Universidade Federal de Minas Gerais (CORO/UFMG). Este laboratório possui ligação com o Grupo de Pequisa e Desenvolvimento de Veículos Autônomos (PDVA) e desenvolve, ainda, projetos na área de Veículos Aéreos Não-Tripulados, além de realizar pesquisas na área de visão computacional e inteligência artificial. O CORO/UFMG desenvolve hoje pesquisas sobre veículos autônomos, possuindo um automóvel da marca Chevrolet modelo Astra 2003/2004 equipado com GPS, Sistema de Medição Inercial e Encoder. Destes sensores podem ser retiradas informações como posição, orientação, aceleração e velocidade do veículo. Tal plataforma é denominada Carro Autônomo da UFMG (CADU), que é um veículo de passeio adaptado para ter algumas de suas funções automatizadas. A Figura 1.1 apresenta o automóvel utilizado. 5

16 Figura 1.1: Carro Autônomo da UFMG. Tendo em vista o objetivo de desenvolvimento de um veículo autônomo pelo CORO/UFMG, o presente estudo busca contribuir para o projeto do Laboratório por meio da implementação de um sistema de localização automática. Assim, a fim de estabelecer tal sistema, o trabalho faz uso de técnicas de fusão sensorial para que as medições de cada sensor sejam combinadas de forma que a confiabilidade em termos de robustez e precisão seja aumentada. Neste trabalho, são definidos na seção 3, dois modelos para representar o carro, bem como os referenciais necessários para realizar a localização. Também são apresentados e descritos na seção 3.2 todos os sensores instalados no carro que são usados para fazer a fusão. O algoritmo da fusão sensorial tem como principal componente o Filtro de Kalman Estendido (Extended Kalman Filter), descrito na seção 2.4. No capítulo 4.3 são coletados os dados 6

17 dos sensores, que são processados e convertidos para o Sistema Internacional de Unidades (S.I.). Através do processamento destes dados é feita uma análise dos erros associados a estes sensores. Dessa forma, define-se na seção 4.1, as configurações que associam sensores e modelos que são implementados no MatLab para realizar a fusão. Todos os testes foram feitos realizando trajetórias com o CADU dentro do campus da UFMG, trajetórias as quais são reconstruídas na seção??. Além da reconstrução da trajetória, são utilizadas métricas como, o traço da matriz de covariância, para definir qual a melhor configuração a ser implementada no algoritmo de fusão sensorial em tempo real, utilizado no carro. 1.5 Organização do Texto Além deste, esta monografia é dividida em outros quatro capítulos que serão distribuídos conforme a seguir. O Capítulo 2 apresentará uma revisão do que existe na literatura a respeito da localização de veículos autônomos, fusão sensorial e abordará, ainda, o Filtro de Kalman, fazendo uma breve compilação das informações obtidas e buscando estabelecer uma conexão entre este e outros trabalhos relacionados. Por sua vez, o Capítulo 3 tratará da modelagem do sistema, realizando um apanhado dos sensores envolvidos na fusão sensorial. Posteriormente, o Capítulo 4 discutirá as diferentes configurações para fusão sensorial, os critérios de desempenho e os resultados experimentais. Finalmente, o Capítulo 5 apresenta os principais resultados alcançados e os discute, sugerindo ainda trabalhos futuros sobre o tema abordado. 7

18 Capítulo 2 Revisão Bibliográfica O uso de veículos autônomos em vias públicas ainda é proibido na maioria dos países. Porém, podem-se encontrar grupos de estudos envolvidos com o desenvolvimento de tais veículos nas principais universidades e montadoras de automóveis do mundo. Os diversos sistemas que compõem os carros autônomos e os desafios para que estes funcionem, fazem com que equipes multi-disciplinares unam esforços e façam a engenharia evoluir neste sentido. Este capítulo começa falando dos principais projetos de veículos autônomos em desenvolvimento no mundo e no Brasil. Depois trata especificamente do problema de localização dos veículos autônomos e das técnicas de fusão sensorial para que esta seja implementada. Por fim, faz-se uma revisão de trabalhos relacionados à localização automática através de fusão sensorial. 8

19 2.1 Veículos Autônomos Um estudo da J.D. Power and Associates [Yvkoff, 2012] apontou que 30% dos compradores de carro estariam interessados em comprar um veículo autônomo. Frente a tal demanda de mercado, os principais nomes da indústria automobilística como General Motors, Ford, Mercedes-Benz, Volkswagen, Audi, BMW, Volvo, Cadillac, entre outras, estão desenvolvendo sistemas que tornam os carros cada vez mais autônomos. A Volvo, sempre pioneira em sistemas de segurança, esta desenvolvendo o projeto SARTRE (Safe Road Trains for the Environment) [Robinson et al., 2010]. Tal projeto consiste na implementação de um sistema de comboios no qual diversos veículos são guiados autonomamente por um veículo líder, tal formação é denominada Platoon [Bergenhem et al., 2010], que é conduzido por um motorista. Um esquema deste sistema pode ser visto na Figura 2.1. As principais montadoras da indústria automobilística, já estão produzindo veículos equipados com sistemas semi-autônomos para auxiliar na condução dos carros, e já anunciam para a próxima década a produção de veículos totalmente autônomos. Dentre estes sistemas podemos citar o Temporary Auto Pilot (TAP) [PhysOrg, 2010], desenvolvido pela Volkswagen, o qual permite que o automóvel, à velocidade limite de 80 milhas por hora, navegue com pouca intervenção humana em rodovias. Tal sistema executa funções como diminuir a velocidade em curvas, manter uma velocidade de cruzeiro, respeitar as bordas da pista e seguir no centro, além de respeitar limites de velocidade do trecho e efetuar paradas de emergência, tudo isso sendo supervisionado pelo motorista. Também podemos citar o Traffic Jam Assistant [Yvkoff, ] lançado pela Audi e pela Mercedes que permite que o carro dirija sozinho a baixas velocidades em engarrafamentos. Mas o desenvolvimento de veículos autônomos encontra sua verdadeira vanguarda nas universidades. A tecnologia de ponta exigida para que um veículo possua locomoção autônoma é o objeto de estudo de diversos labo- 9

20 Figura 2.1: Projeto SARTRE, desenvolvido pela VOLVO. ratórios de pesquisa, nas maiores universidades do mundo. Dentre os principais projetos sendo desenvolvidos podemos citar o MadeInGermany da Freie Universität Berlin [Lab., 2012] (Figura 2.2). Tal veículo é um Volkswagen Passat modificado com a inserção de diversos sensores, como pode ser visto na Figura 2.2. Desenvolvido pelo Autonomous Lab, o veículo possui a parte eletromecânica e a integração dos sensores implementada por engenheiros da Volkswagen. Assim, a equipe da Freie Universität Berlin fica somente responsável pelo software de Inteligência Artificial. A locomoção autônoma do veículo já foi colocada em teste no tráfego de Berlin e o carro obteve certificado de autorização para circular autonomamente pelo Estado de Berlin. O automóvel já é utilizado no serviço de táxi autônomo, que pode ser solicitado por um dispositivo tablet. O receptor GPS no tablet localiza o passageiro e então o veículo se locomove autonomamente até ele e, depois, o passageiro determina o destino através do mesmo GPS. 10

21 Figura 2.2: Veículo MadeInGermany da Freie Universität Berlin. [Lab., 2012] 2.2 Sistema de Localizaçao A localização em determinado ambiente depende do tipo de mapa que é usado para representá-lo. Segundo Jonathan Michael Webster [Webster, 2007] existem dois tipos principais de mapas: os métricos e os topológicos. Os mapas métricos são representados por medidas quantitativas do ambiente que eles representam. Este tipo de mapa determina a posição do veículo com relação a um sistema de coordenadas. O sistema de coordenadas UTM(Universal Transverse Mercador) é a representação em mapa métrico mais usualmente utilizada e será a representação adotada neste trabalho, conforme será detalhado mais à frente. Já a representação topológica, usa "landmarks" para descrever o ambiente. Estas marcações são detectadas por sensores de visão para prover a localização principalmente em ambientes internos, já que em 11

22 ambientes externos tal representação se torna difícil pela ausência de padrões Localização Absoluta A localização absoluta refere-se ao posicionamento global do veículo. Exitem vários meios para alcançar a localização absoluta, entre eles podemos citar a detecção visual de landmarks. Nesta técnica tem-se a priori marcas de posições globais já conhecidas, então detecta-se estas posições e a distância até elas para assim obter a localização. A primeira dificuldade para a localização por landmarks é a detecção das marcas. A visão computacional é a principal tecnologia para essa detecção. O primeiro passo para o sensor de visão é reconhecer os landmarks baseando na comparação com padrões. O segundo passo é determinar a distância do veículo ao landmark. Outra dificuldade deste método, que é citada no trabalho Single Landmark Based Self-Localization of Mobile Robots [Bais et al., 2006], é o surgimento de múltiplas soluções ambíguas. Depois que o landmark foi localizado e sua distância foi determinada, ainda assim existe um círculo de raio igual a distância obtida, como possível posição do veículo. Para que a solução seja única, deve-se obter informação sobre a orientação por alguma característica da marca que indique a direção que o objeto está sendo visto. A localização por marcas, possui a desvantagem de necessitar de um conhecimento prévio das marcas presentes no terreno. Além disso, a distância entre duas marcas pode ser grande, necessitando outra técnica de localização para o veículo navegar entre duas marcas. O GPS(Global Positioning System) tornou-se o sensor preferido para localização absoluta. Similar ao GPS, também já está em operação o Sistema Russo de Navegação por Satélite (GLONASS), que possui maior precisão para uso civil. Este sensor obtém sua posição através de diversos satélites em órbita. Através do tempo entre envio do sinal pelo satélite e recepção do sinal pelo receptor GPS obtêm-se, com o uso de técnicas de triangulação, a posição 12

23 do veículo. Como todo sensor de posição absoluta, o GPS tem suas limitações. Para que este funcione é necessário que sua antena esteja à céu aberto, dificultando assim seu uso em ambientes urbanos com muitos prédios ou túneis, ambientes conhecidos como "canyons urbanos" e também em ambientes com vegetação densa. Tal dificuldade é uma das motivações para o desenvolvimento neste trabalho de técnicas de fusão sensorial para obter a localização do veículo. Também como mencionado no estudo Navigation: Principles of Positioning and Guidance [Hofmann-Wellenhof et al., 2003], para obter a localização é necessário que pelo menos 4 satélites estejam conectados ao GPS Localização Relativa Este tipo de localização é baseado em um ponto de partida conhecido. A partir do ponto inicial as medidas são integradas para obter-se as mudanças na posição do veículo. Existem dois tipos principais de localização relativa: a odometria e a navegação inercial. A odometria consiste na integração da medida de velocidade da roda do veículo ao longo do tempo. A integração é feita com o objetivo de estimar a posição, porém, neste caso além da velocidade é necessário o conhecimento do ângulo ψ (ângulo de guinada do carro) para realizar esta estimativa. Este método possui como ponto fraco, o acúmulo de erros à medida em que a velocidade vai sendo integrada. Podemos citar dois erros atrelados à odometria: os erros sistemáticos e os erros não sistemáticos. O erros sistemáticos são os erros relacionados ao modelo cinemático do veículo e das rodas e podem ser corrigidos com calibração. Já os erros não sistemáticos ocorrem quando há falta de contato entre as rodas e o chão ou algum deslocamento lateral. Os erros desta natureza são mais difíceis de serem corrigidos e geralmente usa-se uma medida de posição absoluta como etapa de atualização para corrigi-los, estratégia que é usada neste trabalho. Já com a Navegação Inercial pode-se obter as medidas de aceleração e variação angular. A aceleração é integrada duas vezes para obter a posição 13

24 e a variação angular uma vez para obter o ângulo do veículo. Tais medidas são providas pela IMU(Inertial Measurement Unit), que possui três eixos ortogonais, com acelerômetro, girômetro e magnetômetro, fornecendo assim, aceleração, velocidade angular e ângulo de orientação em 3 dimensões. A obtenção da posição relativa com o uso da IMU é uma solução que tem se tornado cada vez mais barata, além de ser imune ao problema de deslizamento das rodas. Contudo, o grande problema consiste no acúmulo de erros no processo de integração das variáveis. O erro de medição irá crescer indefinidamente com o movimento do veículo. 2.3 Fusão Sensorial A determinação da posição e da velocidade de um objeto, baseado na informação de múltiplos sensores, é um problema relativamente antigo. Segundo [Hall and Linas, 2001], podemos definir Fusão Sensorial como O processo que lida com a associação, correlação e combinação de dados e informações de uma ou mais fontes para alcançar posicionamento refinado e estimativas de identidade. Existem, basicamente, três tipos de arquitetura de processamento para realizar a fusão: (1) Fusão direta dos dados do sensor Figura 2.3. (2) Representação dos dados do sensor por um vetor de características que depois será fundido Figura 2.4. (3) combinação em alto nível das informações de cada sensor. Figura 2.3: Fusão direta dos dados do sensor. [Hall and Linas, 2001] 14

25 Figura 2.4: Fusão dos dados do sensor através de vetor de características. [Hall and Linas, 2001] Quando os dados dos sensores tem entre si medidas comuns, os dados podem ser combinados de forma direta. Para realizar a fusão direta podemos utilizar métodos clássicos de estimação como o Filtro de Kalman, o qual será descrito detalhadamente na seção a seguir. Mas se os dados dos sensores não tiverem medidas em comum é necessário que a fusão seja feita pelo vetor de estados/características [Antony, 1995]. 2.4 O Filtro de Kalman O Filtro de Kalman é baseado na discretização no domínio do tempo de sistemas dinâmicos lineares, e aborda o problema geral de se tentar estimar um estado x R n de um processo controlado discretamente, o qual é regido pela equação linear de diferença: X k = Ax k 1 + Bu k 1 + W k 1. (2.1) e com medição z R m Z k = Hx k + V k. (2.2) 15

26 As variáveis aleatórias W k e V k representam o ruído de processo e o ruído de medição, respectivamente. É assumido que estas variáveis sejam independentes, brancas e com distribuição normal de probabilidade: p(w ) = N(0, Q). (2.3) p(v ) = N(0, R). (2.4) A matriz Q representa a covariância do ruído de processo e a matriz R representa a covariância do ruído de medição. A matriz A m n, relaciona o estado no passo anterior (k-1) com o estado no passo atual k. A matriz B n l relaciona a entrada de controle u R l ao estado x. A matriz H m n da Equação 2.2 relaciona os estados a medição de Z k. Deve-se também definir as seguintes variáveis: X k Rn : estado estimado a priori no passo k dado o conhecimento do processo no instante k. X k R n : estado estimado a posteriori no passo k dadas as medidas Z k. Para achar as equações do filtro de Kalman, deve-se computar a estimativa a posteriori Xk, como uma combinação linear da estimativa a priori X k e a diferença ponderada entre a medida Z k e a predição da medida H X k. X k = X k + K(Z k H X k ). (2.5) A diferença (Z k H X k ) é chamada de inovação da medição ou resíduo. A inovação reflete a discrepância entre a medição predita H X k e a medida atual Z k. A matriz K m n é composta pelos ganhos que minimizam a covariância do erro a posteriori. 16

27 K k = P k HT (HP k HT + R) 1. (2.6) O filtro de Kalman faz estimativas utilizando uma forma de controle realimentado. O filtro estima os estados do processo em um certo instante de tempo e então realimenta as medidas ruidosas [Welch and Bishop, 2007]. As equações do filtro de Kalman podem ser divididas em dois grupos: equações de predição e equações de medição. As equações de predição são responsáveis por projetar à frente (no tempo) o estado atual e a estimativa da covariância do erro, com a finalidade de obter a estimativa a priori para o próximo passo no tempo. Já as equações de medição são responsáveis pela alimentação, pois incorporam uma nova medida na estimativa a priori a fim de obter uma estimativa a posteriori. Assim, o algoritmo do filtro de Kalman pode ser pensado como um conjunto de equações preditoras-corretoras. As equações discretas de predição e correção podem ser vistas nas Tabelas 2.1 e 2.2. Tabela 2.1: Filtro de Kalman discreto - Equações de Predição Etapa de Predição = A X k 1 + Bu k 1 = AP k 1A T + Q X k P k As equações de predição projetam o estado e a covariância estimada do passo k-1 no passo k. Tabela 2.2: Filtro de Kalman discreto - Equações de Medição Etapa de Medição K k = P k HT (HP k HT + R) 1 X k = X k + K(Z k H X k ) P k = (I K k H)P k A primeira tarefa na etapa de medição é o cálculo do ganho de Kalman. O próximo passo é realizar a medição Z k e gerar o estado estimado a posteriori 17

28 através da incorporação da medição. O passo final é obter a estimativa da covariância do erro a posteriori. Já o filtro de Kalman Estendido, Extended Kalman Filter, ou EKF, é usado para estimar processos, os quais não há relação linear entre o processo e as medições. Assim, este filtro lineariza em torno da média e da covariância. Assume-se que o vetor de estados do processo x R é governado por equações de diferença não-lineares, representadas nas equações 2.7 e 2.8. x k = f(x k 1, u k 1, w k 1 ). (2.7) com medição z R m : Z k = h(x k, v k ). (2.8) As variáveis aleatórias w k e v k são os ruídos de processo e de medição, respectivamente. Neste caso, a função não-linear f na equação de diferença, relaciona o estado no passo anterior k-1, com o estado no passo atual k. Incluindo como parâmetros a função u k 1 e o ruído de processo de média zero w k. Já a função não-linear h na equação de medição, relaciona o estado x k à medição z k. Na prática, não sabemos os valores de w k e v k, contudo, pode-se aproximar o estado e a medição sem estes valores: x k = f( x k 1, u k 1, 0). (2.9) z k = h( x k, 0). (2.10) Para estimar um processo relacionando diferenças não-lineares e medições, devemos escrever novas equações que linearizam e estimam as anterio- 18

29 res: x k = x k + A(x k 1 x k 1 ) + W wk 1. (2.11) z k = z k + H(x k x k ) + V vk. (2.12) onde x k e z k são o estado atual e o vetor de medição. x k e z k são o estado aproximado e o vetor de medições das equações 2.9 e x k é a estimativa a posteriori do estado no passo k. w k e v k representam os ruídos de processo e de medição. Como não temos os valores de w k e v k, não serão consideradas as suas Jacobianas W e V, respectivamente. Assim: A é a matriz Jacobiana das derivadas parciais de f com relação a x. A[i, j] = f[i] x[j] ( x k 1, u k 1, 0). (2.13) H é a matriz Jacobiana das derivadas parciais de h em relação a x. e 2.4 H[i, j] = h[i] x[j] ( x k, 0). (2.14) O conjunto completo de equações do EKF, pode ser visto nas Tabelas

30 Tabela 2.3: Filtro de Kalman Estendido(EKF) - Equações de Predição Etapa de Predição X k = f( x k 1, u k 1, 0) = A kp k 1 A T k + Q P k Tabela 2.4: Filtro de Kalman Estendido(EKF) - Equações de Medição Etapa de Medição K k = P k HT k (H kp k HT + R k ) 1 X k = X k + K k(z k h( X k, 0)) P k = (I K k H k )P k No presente trabalho, o algoritmo usado para a fusão sensorial é a versão estendida do Filtro de Kalman (EKF), pois a estimativa dos estados do processo, relaciona-se de forma não-linear com as medições. Dessa forma, na próxima seção é feita uma revisão das técnicas utilizadas em outros trabalhos para realizar a fusão sensorial. 2.5 Trabalhos Relacionados O uso da combinação GPS/IMU com uma bússola digital é a principal configuração encontrada como solução para o problema de navegação de veículos autônomos. Em [Zhang et al., 2005], destaca-se que podemos encontrar o uso de sensores baratos e de baixa precisão para realizar a fusão sensorial. Neste trabalho, o autor compensa a baixa precisão dos sensores com o uso de um modelo do veículo mais sofisticado e faz uso do filtro de Kalman Unscented, que é mais adequado que a versão Extended quando as equações de estado e as medição são altamente não lineares. Como neste trabalho há uma certa linearidade no modelo, o filtro usado é o EKF apresentado na seção 2.4. O GPS tem sido cada vez mais usado para navegação em ambientes urba- 20

31 nos. Contudo, a localização nas cidades através deste sensor enfrenta grandes problemas diante dos chamados Canyons Urbanos, como já destacado anteriormente. Grandes construções, túneis e viadutos bloqueiam a recepção do sinal dos satélites ou podem refleti-lo, causando atraso na recepção e assim erros de localização. Este é o principal problema do sistema de localização do CADU, sendo que uma possível solução para este problema é encontrada em [Cui and Ge, 2003]. Os autores deste trabalho propõem uma modelagem aproximada do caminho, através de segmentos de reta. Também é proposto o uso do Sistema Global de Posicionamento Russo (GLONASS) [Agency, 2012], com o objetivo de aumentar o número de satélites disponíveis. Além disso, é proposto o uso de referências externas como relógios de precisão e altímetros. Em 2007, a Defense Advanced Research Projects Agency (DARPA) promoveu o Urban Challenge [Challenge, 2007], uma competição na qual os veículos autônomos deveriam percorrer 96 Km em uma área urbana. O trajeto deveria ser completado em menos de 6 horas e os veículos deveriam respeitar todas as regras de tráfego, além de interagir com os outros competidores, de forma a respeitar a precedência em um cruzamento. Times foram formados por parcerias entre montadoras, universidades e empresas de tecnologia, e assim, grande parte dos trabalhos desenvolvidos nas áreas de fusão sensorial e localização autônoma de veículos são provenientes deste desafio. Um dos competidores, o carro Odin da equipe VictorTango[Tech, 2007] foi o único entre os três primeiros colocados que desenvolveu uma solução própria para o sistema de localização (Figura 2.5). A equipe implementou um filtro de posição absoluta e um filtro de posição relativa. O filtro de posição absoluta utiliza GPS, IMU, encoder das rodas e câmeras, as quais detectam landmarks para obter o posicionamento global. Tal filtro consegue prover sozinho a localização, porém, a estimativa da posição irá divergir quando o número de satélites for menor que quatro. O filtro também tem a capacidade de ajustar como a estimativa de posição irá reagir às diferentes taxas de atualização do GPS. Dessa forma, o filtro 21

32 Figura 2.5: Veículo Odin da equipe VictorTango. [Tech, 2007] de posição relativa tem a função de garantir a localização nos momentos de descontinuidade do GPS, que ocorrem com frequência em ambientes urbanos, como foi discutido anteriormente. Neste capítulo foi feita uma síntese sobre o uso de veículos autônomos e os sistemas de localização usados nestes veículos. O que levou a discussão sobre as técnicas de fusão sensorial existentes, como o algoritmo do Filtro de Kalman, que funde informações de sensores para predizer algum estado do sistema. Por fim, foram apresentados alguns trabalhos relacionados ao uso de fusão para solucionar o problema da navegação de veículos autônomos. No Capítulo 3 serão discutidas as metodologias usadas para realizar a fusão. 22

33 Capítulo 3 Metodologia Este capítulo começa descrevendo a modelagem matemática do veículo. Serão discutidos dois modelos cinemáticos para o veículo, bem como os referenciais necessários para a localização e navegação. Por fim, uma descrição da plataforma robótica e dos sensores que a compõem é apresentada. 3.1 Modelagem do Sistema Para a definição da modelagem cinemática, primeiro devemos definir um referencial para o veículo. Existem quatro referenciais que devemos considerar quando estamos falando da navegação de robôs. O referencial inercial, o referencial da terra, o referencial de navegação e o referencial da plataforma [Farrell, 1998]. O referencial inercial é onde as leis de Newton se aplicam e é relativo a ele que os sensores realizam suas medições. O referencial da terra tem sua origem no centro da terra. Já o referencial de navegação é definido em algum ponto da terra que seja conveniente para medições locais. Finalmente, o referencial da plataforma é fixado no corpo do carro, geralmente no seu centro de gravidade. Neste trabalho é considerado que o referencial de navegação não se move com relação ao referencial da terra e que qualquer movimento relacionado ao referencial da plataforma será projetado no 23

34 referencial de navegação. Para o referencial do veículo é usado o sistema RPY(Roll, Pitch, Yaw), o qual é usado para descrever a orientação de corpos rígidos. O modo como este sistema foi afixado ao veículo pode ser visto na Figura 3.1. Figura 3.1: Sistema de Coordenadas fixado ao carro. Para o referencial da terra, foi escolhido o sistema cartesiano ENU (East, North, Up), que é o mais intuitivo para análise de problemas de localização de robôs. A coordenada local ENU é formada por um plano tangente a superfície da terra em um local específico. Por definição, o eixo leste é chamado de X, o norte de Y e para cima de Z. O sistema de coordenadas ENU pode ser visto na Figura 3.2. Como o presente trabalho limita-se a considerar apenas os eixos X e Y, então, a posição do veículo independe de variações verticais. Ou seja, deve- 24

35 Figura 3.2: Sistema de Coordenadas ENU(Leste, Norte, Cima). se projetar a posição na superfície da terra em um sistema de coordenadas bi-dimensional. Para isso foi usado o Sistema Universal Transverso de Mercator (UTM). Tal projeção divide a terra entre as latitudes 80 S e 84 N em 60 zonas de largura de 6 cada. Esta projeção não é única, assim, dentro de cada zona é feita a projeção secante transversa de Mercator. A mapeamento da terra feito com a projeção de Mercator pode ser visto na Figura 3.3. Depois das definições dos referenciais, agora serão propostos os modelos cinemáticos do veículo. Serão apresentados dois modelos, o primeiro, conhecido como unicycle model é descrito em [Luca and Oriolo, 2004]. O segundo modelo é conhecido como modelo de Ackerman, e é descrito em [Choset et al., 2005]. Os modelos são descritos na forma discreta. O primeiro modelo, denominado unicycle model, assume que um veículo de quatro rodas pode ser representado por um veículo de duas rodas, como representado na Figura

36 Figura 3.3: Zonas do Sistema de Coordenadas UTM. Figura 3.4: Modelo cinemático do tipo uniciclo para o carro. [Zhanga et al., 2007] 26

37 Neste modelo, são considerados quatro estados principais: a posição x e y do centro de massa do automóvel, a velocidade v e o ângulo de orientação θ em relação ao eixo z. As entradas para este modelo são: a aceleração A x no eixo x, e a velocidade angular ω. As equações discretas que descrevem os estados são: x(k) = p x (k) p y (k) v(k) θ(k) = p x (k 1) + v(k 1) cos [θ(k 1)] t p y (k 1) + v(k 1) sin [θ(k 1)] t v(k 1) + u 1 (k 1) t θ(k 1) + u 2 (k 1) t (3.1) E as entradas são: u(k) = [ a x (k) ω(k) ] (3.2) Já o segundo modelo, denominado Ackerman, é representado por quatro rodas, sendo que as rodas de traseiras são fixas e as dianteiras controladas pelo volante. Este modelo pode ser visto na Figura 3.5. Aqui, o ângulo das rodas φ representa o ângulo de uma roda média fictícia. Este ângulo é a média dos ângulos das rodas dianteiras. A velocidade v também é a média das velocidades das duas rodas. Porém, neste trabalho, a medição da velocidade é feita nas rodas traseiras, assim, a velocidade medida é efetivamente a velocidade do carro. Portanto, o modelo de Ackerman usado foi alterado no que diz respeito à dependência entre velocidade e ângulo da roda. A distância L entre os eixos, também deve considerada. Os estados do modelo são as posições x e y, a velocidade v do veículo e o ângulo θ de orientação do carro. As entradas do modelo são o ângulo φ da roda dianteira e a aceleração a x no eixo x. Então as equações de estado são: 27

38 Figura 3.5: Modelo de Ackerman para o carro. [Laumond, 1998] x(k) = p x (k) p y (k) v(k) θ(k) = p x (k 1) + v(k 1) cos [θ(k 1)] t p y (k 1) + v(k 1) sin [θ(k 1)] t v(k 1) + u 2 (k 1) t θ(k 1) + v(k 1) sin[u 1(k 1)] t L (3.3) E as entradas são: u(k) = [ φ(k) a x (k) ] (3.4) 28

39 3.2 Sensores do Sistema de Localização A plataforma robótica foi montada sobre um automóvel Chevrolet Astra Sedan 2003/2004, motor 2.4 de 16 válvulas, equipado com câmbio automático e direção hidráulica. Além disso o carro tem diversos sensores instalados, os quais serão usados para obter a localização. Dentre eles podemos citar: o encoder, para medir velocidade, instalado na roda traseira; um GPS que fornece a posição absoluta; uma unidade de medição inercial; e também um outro encoder para medir o ângulo do volante. Assim, foi determinado na modelagem cinemática que devemos obter através de instrumentos de medição, as seguintes variáveis: posição, aceleração e velocidade linear, orientação, velocidade angular e ângulo do volante que será convertido em ângulo da roda. A seguir, serão apresentados os sensores usados para obter essas variáveis GPS Durante os últimos anos o Global Positioning System (GPS) tornou-se a solução preferida para localização absoluta. Isso deve-se ao fato de que a cada dia, estes instrumentos se tornam mais precisos, sendo que podemos encontrar aparelhos com precisão de centímetros e com uma taxa de atualização cada vez maior. Toda essa precisão se deve ao uso de Differential Global Positioning System (DGPS), tecnologia que consiste na utilização de estações terrestres, para efetuar correções em possíveis atrasos dos sinais dos satélites. Assim, o GPS diferencial pode alcançar uma precisão menor que 0,6 m com intervalo de confiança de 95%, em condições de baixa diluição do sinal e elevado número de satélites. Contudo, como todo sensor de localização absoluto, o GPS possui suas limitações. A primeira delas é o fato de que este só funciona quando está a céu aberto, ou seja, a antena precisa estar conectada a pelo menos quatro satélites para que a triangulação possa ser feita [Hofmann-Wellenhof et al., 2003]. Podemos citar também o fato de que, em ambientes urbanos, a grande quantidade de prédios, faz com que o sinal seja refletido e leve um tempo maior 29

40 para alcançar o receptor, causando erros de posição [Cui and Ge, 2003]. No presente estudo, o GPS usado no veículo CADU é da marca Hemisphere modelo R120 [Hemisphere, 2006]. Uma foto do GPS usado pode ser vista na Figura 3.6. Figura 3.6: GPS Hemisphere R120. [Hemisphere, 2006] Tal GPS possui correção diferencial, com precisão de submedição menor que 0,6m com 95% de confiança e taxa de atualização de até 20Hz. A entrada e a saída do instrumento são feitas segundo o protocolo 0183 da National Marine Electronics Association (NMEA) [NMEA, 2012], que permite que diversos tipos de mensagens sejão requeridas do GPS Unidade de Medição Inercial A Unidade de Medição Inercial, também conhecida como Inertial Measurement Unit (IMU) é um dispositivo que combina três acelerômetros, três giroscópios e três magnetrômetros de forma ortogonal. Tal dispositivo instalado na plataforma robótica irá fornecer a aceleração angular, a velocidade angular e os ângulos de Euler (leia-se Roll, Pitch e Yaw). O dispositivo usado no carro autônomo do presente trabalho, é o 3DM-GX1 R da MicroStrain 30

41 [MicroStrain, 2006]. Este instrumento possui movimento angular de 360 nos três eixos e também uma taxa de amostragem de até 350Hz. Além de ter processamento on-board e filtro para as saídas, possui também calibração para distorções no campo magnético causadas por materiais ferro-magnéticos presentes perto do sensor. Porém, a posição angular é obtida através da integração da medida de variação do ângulo, e assim, com o tempo, o acúmulo do erro de posição aumenta, tornando a navegação inercial boa somente para pequenas distâncias, razão pela qual torna-se necessário o uso do sistema de GPS. A Unidade de Medição Inercial pode ser vista na Figura 3.7 Figura 3.7: Unidade de Medição Inercial. [MicroStrain, 2006] Sensor de Velocidade das Rodas Com o intuito de medir a velocidade do carro, foi instalado na roda traseira um encoder. Tal instrumento pode ser visto na Figura 3.8. O encoder é da marca YUMO [SparkFun, 2012], possui 1024 pulsos por rotação, a sua saída pode ser interpretada por qualquer microcontrolador, através do uso de código Gray, podendo ser obtido o número de rotações e a direção. O sensor é alimentado com tensão de 5 a 12 Volts e pode medir rotações de até rpm. 31

42 Figura 3.8: Encoder da roda traseira. [SparkFun, 2012] Encoder do Volante É intuitivo pensar que a orientação das rodas dianteiras está diretamente relacionada com o ângulo do volante, ou seja, que o ângulo do volante é proporcional ao ângulo das rodas. Por isso, o carro possui instalado em seu volante um motor da Maxon de corrente contínua. Neste motor está acoplado um encoder incremental, pelo qual é possível obter a posição do volante. Para ter controle de posição, velocidade e corrente do motor, temos associado a ele um controlador, modelo EPOS 24/5, de mesma marca que lê os dados do encoder a uma taxa de 33Hz. Novamente, foi realizado um procedimento de calibração em [Santos, 2009] para estimação do ângulo das rodas a partir do ângulo do volante medido pelo encoder. A média dos ângulos das rodas foi utilizada, como em Principles of Robot Motion [Choset et al., 2005]. O motor Maxon que atua no volante pode ser visto na Figura 3.9 e o controlador EPOS pode ser visto na Figura 3.10 Neste capítulo foram definidos os referenciais de navegação para o veículo autônomo, bem como os modelos usados para representá-lo. Por último, e não menos importante, foram apresentados e descritos os sensores que compõem a plataforma robótica. Tais sensores, combinados com os modelos discutidos na seção 3, formam as configurações usadas para prover a fusão sensorial, as quais serão apresentadas no capítulo seguinte. 32

43 Figura 3.9: Motor RE 40 - Maxon para controle do volante. [Santos, 2009] Figura 3.10: Controlador EPOS 24/5 do motor. [Sabbagh, 2009] 33