Off the Beaten Track: Laterally Weighted Motion Planning for Local Obstacle Avoidance

We extend the behaviour of generic sample-based motion planners to support obstacle avoidance during long-range path following by introducing a new edge-cost metric paired with a curvilinear planning space. The resulting planner generates naturally smooth paths that avoid local obstacles while minimizing lateral path deviation to best exploit prior terrain knowledge from the reference path. In this adaptation, we explore the nuances of planning in the curvilinear configuration space and describe a mechanism for natural singularity handling to improve generality. We then shift our focus to the trajectory generation problem, proposing a novel Model Predictive Control (MPC) architecture to best exploit our path planner for improved obstacle avoidance. Through rigorous field robotics trials over 5 km, we compare our approach to the more common direct path-tracking MPC method and discuss the promise of these techniques for reliable long-term autonomous operations.