Accurate pose estimation of mobile platform in rough terrain: Comparison of multi-sensor fusion SLAM

Abstract

The necessity for accurate Global Navigation Satellite System -free (GNSS) pose estimation in mobile machinery arises in situations, where it is not feasible to use, such as in dense forestry and underground environments or in places, where the signals are under continuous interference. Relative pose estimation systems constructed using alternate sensors, such as Light Detection and Ranging (LiDAR) radars, cameras and Inertial Measurement Units (IMU) represent a large and active field of study. Such systems are also sometimes denoted as Simultaneous Localization and Mapping (SLAM) systems, where the pose of the system is estimated in a map being built of its surrounding environment. Sensor-fusion can be applied to utilize measurements from multiple complementary sensors together to potentially increase performance. Accumulated drift from the incrementally changing odometry can be corrected by detecting a previously visited position and closing a loop. This work aims to evaluate existing solutions built with the aforementioned sensors to find a suitable system, which holds pose estimation accuracy even when traversing rough terrain.

The open-source SLAM systems LIO-SAM, FAST-LIO2, ORB-SLAM3 and VINS-Fusion were selected for experimentation. The underlying system and sensors were configured suitable for SLAM estimation. This process includes time synchronizing and spatially calibrating different sensors with respect to each other. The feature tracking and inertial parameters of the SLAM algorithms involved in the comparison were tuned for accuracy on a part of a private dataset recorded on the mobile machine. After a good configuration of parameters were found for each system, their performance was evaluated on the entire dataset by comparing the Absolute Pose Error (APE) and Relative Pose Error (RPE) of their resulting optimized and odometry trajectories with respect to a ground truth benchmark reference received from Inertial Navigation System - Real-Time Kinematic (INS-RTK) measurements.

The best performing systems were the loosely-coupled LIO-SAM and ORB-SLAM3 in stereo mode. LIO-SAM is powered by the incremental Smoothing and Mapping algorithm, which provides computationally efficient and accurate pose estimates. The downward facing camera system proved also suitable for accurately traversing on rough terrain. The tightly-coupled systems of VINS-Fusion, FAST-LIO2 and ORB-SLAM3 in visual-inertial mode demonstrated worse performance metrics. The degradation can be explained by the uncertainty left in the calibration procedure of the inertial parameters, where errors propagate more heavily in tightly-coupled systems.

Tarve satelliittivapaalle paikkaestimoinille ilmenee tilanteissa, joissa niitä ei ole täysin mahdollista käyttää, kuten tiheissä metsissä, maanalaisissa ympäristöissä tai paikoissa, joissa sateliittijärjestelmien signaalit ovat jatkuvan häirinnän kohteena. Relatiiviset paikkaestimointi systeemit, jotka ovat rakennettu hyödyntäen antureita, kuten lasertutkia, kameroita ja inertia-antureita. Kyseisiä systeemejä saatetaan myös kutsua Simultaneous Localization and Mapping -systeemeiksi (SLAM), joissa allaolevan järjestelmän paikkaa estimoidaan kartassa, jota rakennetaan samanaikaisesti estimointiprosessin kanssa. Anturifuusiota voi soveltaa hyödyntäen mittauksia useista komplementaarisista antureista. Tämä metodiikka voi mahdollisesti parantaa paikkaestimoinnin suorituskykyä. Odometriaan kerääntynyttä paikkavirhettä voi myös korjata tunnistamalla paikan, jossa järjestelmä on aikaisemmin käynyt. Tätä kutsutaan myös silmukan sulkemiseksi. Tämän työn tavoitteena oli tutkia ja toteuttaa kokeita olemassaolevilla SLAM-ratkaisuilla, jotka osasivat hyödyntää aikaisemmin mainittuja antureita ja niiden kombinaatioita. Kokeet fokusoitiin ottamalla operointi raskaassa maastossa huomioon.

Vapaan lähdekoodin SLAM-systeemeistä LIO-SAM, FAST-LIO2, ORB-SLAM3 ja VINS-Fusion valittiin kokeiluun. Allaoleva systeemi ja sen sisältämät anturit konfiguroitiin sopivaksi SLAM-estimoinnille. Tämä prosessi sisälsi antureiden keskeisen aikasynkronoinnin, ja tiettyjen anturien keskeisen spatiaalisen kalibroinnin. SLAM-algoritmien tarjoamat inertia- ja feature-seuraus parametrit viritettiin järjestelmän tarkkuuden parantamiseksi yhdellä osuudella järjestelmällä nauhoitetulla datajoukolla. Sopivien parametrien löytämisen jälkeen, SLAM-systeemien toimintaa evaluoitiin kokonaisella datajoukolla. Suorituskyvyn mittaamiseksi, absoluuttista ja relatiivista positiovirhettä hyödynnettiin vertaamalla SLAM-systeemeistä saatuja ratoja, sateliittipaikannusjärjestelmältä saatuun referenssirataan.

Parhaiten suoriutuneet systeemit olivat löyhästi kytketty LIO-SAM ja ORB-SLAM3 stereo-moodissa. LIO-SAM:in käyttämä inkrementaalinen Smoothing and Mapping (SAM) -algoritmi osoittautui tuottamaan laskennallisesti tehokkaita ja tarkkoja paikkaestimaatteja. Alaspäin suunnattu kamerasysteemi paljastui myös sopivaksi tavaksi estimoida paikkaa raskaassa maastossa, varsinkin kun ORB-SLAM3:n stereopaikoitusta kokeiltiin. Tiivisti kytketyt systeemit, kuten VINS-Fusion, FAST-LIO2 ja ORB-SLAM3 visuaalis-inertia moodissa demonstroivat huomattavasti heikompaa paikannustarkkuutta. Tämä heikentyminen voi osittain selittyä sillä, miten epävarmuus monistuu paljon herkemmin tiivisti kytketyissä systeemeissä.