Multidimensional Particle Filter for Long-Term Visual Teach and Repeat in Changing Environments

When a mobile robot is asked to navigate intelligently in an environment, it needs to estimate its own and the environment's state. One of the popular methods for robot state and position estimation is particle filtering (PF). Visual Teach and Repeat (VT&R) is a type of navigation that uses a camera to navigate the robot along the previously traversed path. Particle filters are usually used in VT&R to fuse data from odometry and camera to estimate the distance traveled along the path. However, in VT&R, there are other valuable states that the robot can benefit from, especially when moving through changing environments. We propose a multidimensional particle filter to estimate the robot state in VT&R navigation. Apart from the traveled distance, our particle filter estimates lateral and heading deviation from the taught path as well as the current appearance of the environment. This appearance is estimated using maps created across various environmental conditions recorded during the previous traversals. The joint state estimation is based on contrastive neural network architecture, allowing self-supervised learning. This architecture can process multiple images in parallel, alleviating the potential overhead caused by computing the particle filter over the maps simultaneously. We conducted experiments to show that the joint robot/environment state estimation improves navigation accuracy and robustness in a continual mapping setup. Unlike the other frameworks, which treat the robot position and environment appearance separately, our PF represents them as one multidimensional state, resulting in a more general uncertainty model for VT&R.