Real-Time Local Map Generation and Collision-Free Trajectory Planning for Autonomous Vehicles in Dynamic Environments

Abstract

In this paper we propose a method for autonomous navigation in GPS-denied environments that can be deployed in all types of robotic vehicles requiring only a single RGB-D camera. Our algorithm is an integration of localization, mapping and trajectory planning software modules that can handle dynamic environments. Depth measurements and visual odometry are used to (a) create a robocentric Euclidean Signed Distance Fields (ESDF) map in real-time, (b) estimate the position of obstacles surrounding the robot, and (c) calculate the distance and gradient from them. Subsequently, we use an optimization algorithm to plan a collision-free trajectory. We show that a local map can be created faster than with the fixed-size array method used in current optimization methods. Our approach also allows for effective detection of dynamic obstacles by constantly updating the map. As a result, the robot deviates from its initial path only when necessary. We validate our results in simulation by deploying our algorithm in a skid steering rover and a hexacopter in an environment with static and dynamic obstacles.