TRG-planner: Traversal Risk Graph-Based Path Planning in Unstructured Environments for Safe and Efficient Navigation

TL;DR

Unstructured environments pose navigation safety challenges due to terrain irregularities. The authors introduce Traversal Risk Graph (TRG) and a TRG-planner that builds a direction-aware, reachability-informed graph in real time by sampling around the robot and weighting edges with traversal risk $w_{ij}$ computed from an ellipse PCA model, and planning with a risk-aware cost $C(v_{i+1})=C(v_i)+d_{i+1,i}(\Gamma w_{i+1,i}+1)$. Key contributions include the TRG components (nodes, edges), construction via wavefront sampling, hierarchical TRG management, and planning strategy. Empirical results in simulation and real-world tests, including the ICRA 2023 Quadruped Robot Challenge, show improved safety and distance efficiency, enabling reliable global navigation for quadrupedal platforms.

Abstract

Unstructured environments such as mountains, caves, construction sites, or disaster areas are challenging for autonomous navigation because of terrain irregularities. In particular, it is crucial to plan a path to avoid risky terrain and reach the goal quickly and safely. In this paper, we propose a method for safe and distance-efficient path planning, leveraging Traversal Risk Graph (TRG), a novel graph representation that takes into account geometric traversability of the terrain. TRG nodes represent stability and reachability of the terrain, while edges represent relative traversal risk-weighted path candidates. Additionally, TRG is constructed in a wavefront propagation manner and managed hierarchically, enabling real-time planning even in large-scale environments. Lastly, we formulate a graph optimization problem on TRG that leads the robot to navigate by prioritizing both safe and short paths. Our approach demonstrated superior safety, distance efficiency, and fast processing time compared to the conventional methods. It was also validated in several real-world experiments using a quadrupedal robot. Notably, TRG-planner contributed as the global path planner of an autonomous navigation framework for the DreamSTEP team, which won the Quadruped Robot Challenge at ICRA 2023. The project page is available at https://trg-planner.github.io .

Figures (11)

• Figure 1: (L-R): Our quadruped robot autonomously navigates through a harsh slope terrain during the QRC competition. The proposed planner constructs the traversal risk graph, which contains the reachability of the terrain and the relative risk of the path candidates. The white-to-red gradation of the graph edges visualizes the relative risk of the path candidates. The robot plans a safe and efficient path by optimizing the graph (green line).
• Figure 2: Overview of our proposed path planner framework called TRG-planner. Traversal Risk Graph (TRG) is proposed to capture the geometrical information of the terrain, consisting of nodes, edges, and their relationships (Section \ref{['sec:TRG_components']}). (a) TRG is constructed by sampling nodes from the local elevation map or, optionally, from a prebuilt map (dashed line) and connecting edges based on the relative risk of path candidates. (b) The local graph is extracted from the global graph. It is hierarchically expanded and updated. (c) TRG-planner optimizes the risk-aware cost function to find a safe and distance-efficient path.
• Figure 3: The edge area is approximated to an ellipse plane $\xi$ using principal component analysis (PCA). Nodes $\mathbf{v}_{i}$ and $\mathbf{v}_{j}$ are the focal points of the ellipse, and the minor axis of the ellipse is determined by the radius $r_{\text{robot}}$ of the inscribed circle of the robot. $\mathbf{\hat{n}}_{\text{lon}}$ and $\mathbf{\hat{n}}_{\text{lat}}$ are the longitudinal and latitudinal unit direction vectors along the path, and $\mathbf{\hat{e}^\xi_{\text{lon}}}$ and $\mathbf{\hat{e}^\xi_{\text{lat}}}$ are normalized eigenvectors of the ellipse along each direction, respectively.
• Figure 4: Example of TRG expansion. (Left) Nodes (orange dots) are randomly sampled from a uniform distribution on a circle with a radius of $r_{\text{exp}}$ from the reference node $\mathbf{v}_{\text{ref}}$ (blue dot). (Right) There are three possible cases for the sampled nodes. (a) Sampled nodes are discarded if the corresponding terrain is unstable or if the terrain between the reference node and the sampled node is unsuitable for traversal. (b) If an existing node is within the radius of $r_{\text{robot}}$ from the sampled node, they are merged and an edge is wired (green line) between the reference node and the existing node (green dot). (c) Otherwise, the sampled node is generated (magenta dot), and edges are subsequently wired between the sampled and existing node within $r_{\text{exp}}$ (magenta lines).
• Figure 5: Example of hierarchical graph management. (a) The robot navigates along the planned path (green) to the sub-goal closest to the unknown goal (red). The local graph, shown in the blue dashed box, is extracted from the global graph which is shown in white. The invisible wall, highlighted as a yellow ellipsoid, is outside the local measurement. (b) Some nodes are updated to the invalid states as the unseen wall is detected. Then, replanning is conducted because the previous path is on the invalid node. (c) When the robot moves to the unknown area, the local graph is expanded from the frontier nodes, leading to the global graph update. Finally, the sub-goal becomes the known goal position, and the robot reaches the goal.