Terrain-Aware Kinodynamic Planning with Efficiently Adaptive State Lattices for Mobile Robot Navigation in Off-Road Environments

TL;DR

The paper addresses safe, terrain-aware navigation for mobile robots on non-flat terrain by integrating kinodynamic constraints into a recombinant state lattice. It introduces KEASL, which encodes attitude, velocity, and velocity-constraint profiles along graph edges loaded from a precomputed edge library, and computes edge durations via bidirectional Eulerian integration. The approach provides higher-fidelity motion representations than traditional 2D cost maps, and real-world experiments with a Warthog UGV show KEASL delivers more efficient routes under terrain-induced constraints in a large set of planning problems, albeit with longer planning times in some cases. The work demonstrates significant potential for safer, more reliable off-road navigation and highlights avenues for adaptive selection between KEASL and prior methods and for integrating learned guidance into global planning.

Abstract

To safely traverse non-flat terrain, robots must account for the influence of terrain shape in their planned motions. Terrain-aware motion planners use an estimate of the vehicle roll and pitch as a function of pose, vehicle suspension, and ground elevation map to weigh the cost of edges in the search space. Encoding such information in a traditional two-dimensional cost map is limiting because it is unable to capture the influence of orientation on the roll and pitch estimates from sloped terrain. The research presented herein addresses this problem by encoding kinodynamic information in the edges of a recombinant motion planning search space based on the Efficiently Adaptive State Lattice (EASL). This approach, which we describe as a Kinodynamic Efficiently Adaptive State Lattice (KEASL), differs from the prior representation in two ways. First, this method uses a novel encoding of velocity and acceleration constraints and vehicle direction at expanded nodes in the motion planning graph. Second, this approach describes additional steps for evaluating the roll, pitch, constraints, and velocities associated with poses along each edge during search in a manner that still enables the graph to remain recombinant. Velocities are computed using an iterative bidirectional method using Eulerian integration that more accurately estimates the duration of edges that are subject to terrain-dependent velocity limits. Real-world experiments on a Clearpath Robotics Warthog Unmanned Ground Vehicle were performed in a non-flat, unstructured environment. Results from 2093 planning queries from these experiments showed that KEASL provided a more efficient route than EASL in 83.72% of cases when EASL plans were adjusted to satisfy terrain-dependent velocity constraints. An analysis of relative runtimes and differences between planned routes is additionally presented.

Figures (8)

• Figure 1: A Clearpath Robotics Warthog Unmanned Ground Vehicle in an off-road environment. Kinodynamic Efficiently Adaptive State Lattices (KEASL) integrates velocity acceleration constraints imposed by terrain hazards into the search process to provide better global guidance in off-road environments.
• Figure 2: Illustration of seven KEASL edge library expansions of a state lattice indexed by $\left(x,y,\psi\right)$ starting from node $\left(F,0^{\circ}\right)$. Expansions of the state lattice are performed at $\left(L,270^{\circ}\right)$, $\left(S,0^{\circ}\right)$, $\left(N,90^{\circ}\right)$, $\left(G,0^{\circ}\right)$, $\left(H,0^{\circ}\right)$, $\left(I,0^{\circ}\right)$ and $\left(J,0^{\circ}\right)$.
• Figure 3: A depiction of the steps in the bi-directional Eulerian integration approach used for velocity acceleration profile calculation. (a) depicts the velocity constraints along an example trajectory. (b) shows the forward pass, where the calculations occur from left to right and never exceed the constraint values. (c) illustrates the backward pass where calculations occur from right to left and never exceed the forward pass values. The resulting shape is the velocity profile for the path.
• Figure 4: Velocity limits set by the roll and pitch constraints. These indicate the operating ranges that we deem "safe" for robot traversal for these experiments. Anything exceeding these limits is considered unsafe. The operating range for roll and pitch is indicated by the shaded gray area beneath each line.
• Figure 5: Depiction of the three test sites. The top row of images shows examples of EASL path planning through the environments, and the bottom row shows the KEASL planning. The columns are split into three pairs denoted by (a), (b), and (c), with each pair containing a binary cost map (left) and color-coded height map (right). The height is scaled from low to high elevation with blue to red coloring, respectively. In both the cost maps and the height maps, the paths are colored as a scale for robot velocity from 0m/s to the max allowable speed (2m/s) indicated from blue to red, respectively. The distance from the initial state (green rectangle in the center of each map) to every goal state (red rectangle at the end of each trajectory) is 25m.