LexTOR: Lexicographic teach optimize and repeat based on user preferences

In the last years, many researchers started to consider teach-and-repeat approaches for reliable autonomous navigation. The paradigm, in all its proposed forms, is deeply rooted in the idea that the robot should autonomously follow a route that has been demonstrated by a human during a teach phase. However, human demonstrations are often inefficient in terms of execution time or may cause premature wear of the robot components due to jittery behavior or strong accelerations. In this paper, we propose the concept of teach, optimize and repeat, which introduces a trajectory optimization step between the teach and repeat phases. To address this problem, we further propose LexTOR, a constrained trajectory optimization method for teach and repeat problems, where the constraints are defined according to user preferences. At its core, LexTOR optimizes both the execution time and the trajectory smoothness in a lexicographic sense. The experiments show that LexTOR is very effective, both qualitatively and quantitatively, in terms of execution time, smoothness, accuracy and bound satisfaction.