PATH PLANNING AND REAL-TIME COLLISION AVOIDANCE BASED ON THE ESSENTIAL VISIBILITY GRAPH