Moving Horizon Estimation SLAM for agile vehicles in 3-D environments

Abstract

The ability for a robot to be able to construct a map of the environment and recognize its position on it was one of the biggest developments in robotics. It gives them the possibility of being autonomous and safe, while creating new deployment opportunities where they were previously not feasible nor skillful enough to operate. The Simultaneous Localization and Mapping framework builds onto the perception of the robot, giving it the possibility to online calculate its trajectory and avoid obstacles. Because of it, there is now a large range of scenarios where robots can be used, ranging from a ship on open waters to a ground vehicle on mars. Moreover, the continuous development of processing units has given the possibility for previously hardware exhausting solutions to be used as an option for the localization and mapping problem. With this in mind, the dissertation work is focused on developing a Simultaneous Localization and Mapping (SLAM) solution for a 6 Degrees of Freedom (DoF) vehicle operating on a 3D environment using Moving Horizon Estimation (MHE). Throughout the document it is presented relevant concepts to the modelling of 6 DoF vehicles as well as other approaches to the SLAM problem. It is also tested the applicability of the proposed solution, based on MHE, in a simulation environment of a 3D square-shaped corridor with stationary landmarks, whilst comparing the obtained results with other probabilistic approaches, the Extended Kalman Filter (EKF), which is commonly used but loses stability on extremely nonlinear dynamics, and the Linear Kalman Filter (LKF) sensor-based, which can also deal with the non-linearities of the system by characterizing it on the sensors frame. Each of the algorithms is simulated in MATLAB and their performance was compared considering two different Scenarios.

A capacidade de um robô ser capaz de construir um mapa do ambiente que o envolve e reconhecer a sua posição no mesmo foi um dos maiores desenvolvimentos na área da robótica. Esta capacidade possibilita a que estes sejam autónomos e mais seguros, fomentando novas oportunidades de uso, para as quais a sua aplicação não era viável. A localização e mapeamento simultâneos (com acrónimo em inglês, SLAM) possibilita a perceção do robô, dando-lhe a capacidade de calcular, no momento, a sua trajetória evitando assim obstáculos. Por causa disto, há um grande leque de cenários onde os robôs podem ser aplicados, desde um navio em águas abertas até um veículo terrestre na superfície de Marte. Para além disto, o contínuo desenvolvimento de unidades de processamento possibilitou que soluções antigamente consideradas computacionalmente pesadas possam ser consideradas como solução para o problema de SLAM. Com isto em mente, este trabalho de dissertação foca-se no desenvolvimento de uma solução SLAM baseada em estimação de horizonte movel (com acrónimo em inglês, MHE) para um veículo com 6 graus de liberdade (DoF) que opera num ambiente 3D. Ao longo do documento são apresentados conceitos relevantes para o desenvolvimento de modelos de veículos de 6 DoF bem como outras abordagens para o problema SLAM. Também é testada a aplicabilidade da solução MHE proposta num ambiente de simulação de um corredor 3D em forma de quadrado com marcos estacionários. A performance desta simulação é comparada aos resultados obtidos em outras duas abordagens probabilísticas, o filtro de kalman estendido (EKF) que é largamente utilizada mas que tem uma performance limitada aquando se encontra na presença de dinâmicas extremamente não lineares, e um filtro de kalman linear baseado nos sensores (sensor-based LKF), uma abordagem interessante que lida com as não linearidades do sistema caracterizando-o a partir do refe- rencial do sensor. Cada um dos algoritmos é simulado no MATLAB e o seu desempenho foi comparado considerando dois cenários diferentes.