Kinodynamic Motion Planning for Mobile Robot Navigation across Inconsistent World Models

TL;DR

The paper tackles motion planning for mobile robots in partially observed, uncertain environments where successive world models disagree due to sensor noise. It extends the KEASL kinodynamic planning framework to a multi-hypothesis search space and analyzes four strategies—VEH, PEH, GEH, and GEGRH—for robust planning under evolving maps. GEGRH, which adds a graph revision step, achieves lower planning times and shorter trajectories than VEH in field tests, while preserving safety under uncertainty; GEGRH also outperforms SH in several scenarios, highlighting the benefits of multi-hypothesis reasoning. The work demonstrates that deterministic graph-search approaches can be made robust to perception uncertainty and suitable for field deployment in off-road robotics.

Abstract

Mobile ground robots lacking prior knowledge of an environment must rely on sensor data to develop a model of their surroundings. In these scenarios, consistent identification of obstacles and terrain features can be difficult due to noise and algorithmic shortcomings, which can make it difficult for motion planning systems to generate safe motions. One particular difficulty to overcome is when regions of the cost map switch between being marked as obstacles and free space through successive planning cycles. One potential solution to this, which we refer to as Valid in Every Hypothesis (VEH), is for the planning system to plan motions that are guaranteed to be safe through a history of world models. Another approach is to track a history of world models, and adjust node costs according to the potential penalty of needing to reroute around previously hazardous areas. This work discusses three major iterations on this idea. The first iteration, called PEH, invokes a sub-search for every node expansion that crosses through a divergence point in the world models. The second and third iterations, called GEH and GEGRH respectively, defer the sub-search until after an edge expands into the goal region. GEGRH uses an additional step to revise the graph based on divergent nodes in each world. Initial results showed that, although PEH and GEH find more optimistic solutions than VEH, they are unable to generate solutions in less than one-second, which exceeds our requirements for field deployment. Analysis of results from a field experiment in an unstructured, off-road environment on a Clearpath Robotics Warthog UGV indicate that GEGRH finds lower cost trajectories and has faster average planning times than VEH. Compared to single-hypothesis (SH) search, where only the latest world model is considered, GEGRH generates more conservative plans with a small increase in average planning time.

Figures (12)

• Figure 1: Top: UGV in an unstructured, off-road environment. Bottom: Three successive planning problems, with the colored cost map overlaid on a faded image of the previous. Relying on the most recent cost map causes the planned trajectory (magenta) to oscillate around the central obstacles (shaded yellow, circled in black).
• Figure 2: Three successive planning problems, with the colored cost map overlaid on a faded image of the previous. Using only the most recent cost map causes the planned trajectory (magenta) to oscillate around the central clustering of obstacles (shaded yellow, circled in black). The multi-hypothesis plan (blue) is consistently to one side of the cluster. Note that at $t=0$ the trajectories are identical because only one world hypothesis exists at this point.
• Figure 3: The solution generated by VEH (blue) avoids all obstacles in all three hypothesized worlds. The solution from SH (red) only accounts for the most recent world hypothesis (green cost map).
• Figure 4: Case 1: An inconsistent obstacle (gray checkerboard) does not exist in the primary world hypothesis, but does in the secondary world hypothesis. It falls within one edge expansion between nodes $n_{0,0}$ and $n_{1,0}$. The "Hypothesis 1" expansion assumes the region is free-space, and the "Hypothesis 2" expansion assumes the region is lethal.
• Figure 5: Case 2: An inconsistent obstacle (gray checkerboard) does not exist in the primary world hypothesis and does in the secondary world hypothesis. The "Hypothesis 1" and "Hypothesis 2" trajectories generated between nodes $n_{0,3}$ and $n_{5,3}$ assume the region is safe and lethal respectively.