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

TL;DR

This adaptation extends the behavior 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 and describes a mechanism for natural singularity handling to improve generality.

Abstract

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.

Figures (25)

• Figure 1: We present an architecture for locally avoiding unmapped obstacles along a reference path by extending sample-based motion planners to encourage trajectories with characteristics that best exploit prior path knowledge. The goal of our planner is to follow the previously established reference path as closely as possible while avoiding any new obstacles that may have appeared along the path. To accomplish this task we use sample-based motion planners deployed in a laterally constrained corridor configuration space to find candidate collision-free paths, and then apply Model Predictive Control (MPC) to generate optimized trajectories for the robot. Our system is validated on an ARGO Atlas J8 robot in real-world scenarios.
• Figure 2: An overview of the proposed obstacle-avoidance system. A change-detection LiDAR perception module identifies previously unmapped structures and updates a locally planar 2D occupancy grid map. The planner finds paths that avoid obstacles using our laterally weighted edge-cost metric and a tracking MPC enforces kinematic constraints on the final trajectory. Velocity commands are then sent to maneuver the robot, at which point we receive new robot state estimates from VTR and pass these states to both the planner and MPC to update plans and complete the feedback cycle.
• Figure 3: Left: A reference path in Euclidean coordinates shown in green, with the longitudinal and lateral components extended in a grid. Right: The corresponding representation of the path in curvilinear coordinates.
• Figure 4: An illustration of the proposed collision checking scheme using curvilinear coordinates. Given an edge in curvilinear space, we can perform a collision check by finely discretizing the line into points and converting each point to Euclidean space using some basic interpolation and trigonometry. We then perform a check to see if any of the points are located inside the current obstacle map, and return a Boolean that indicates if the edge should be created in curvilinear space.
• Figure 5: The informed sampling domain, $\bm{X_{\hat{f}}}$, for the Euclidean distance edge metric (blue ellipse), and the conservatively bounded laterally weighted edge metric (black box) for $\bm{\alpha = 0.5}$. Samples shown in red, were populated using rejection sampling and illustrate the true 'eye-ball' distribution of the informed sampling region. As $\bm{\alpha}$ tends to zero, the domains coincide.