Augmenting GRIPS with Heuristic Sampling for Planning Feasible Trajectories of a Car-Like Robot

Brian Angulo, Konstantin Yakovlev, Ivan Radionov

Abstract

This paper presents an enhanced version of the Gradient-informed Path Smoothing (GRIPS) algorithm, specifically optimized for car-like robots. By integrating heuristic sampling at critical bottlenecks and streamlining waypoint processing, the proposed method significantly improves planning success rates and computational efficiency. Experimental results demonstrate a success rate increase from 50% to 90% and a reduction in computation time by up to 80% compared to the original GRIPS framework.

Introduction

Planning trajectories for car-like robots presents a significant "kinodynamic" challenge. Unlike holonomic robots that can move in any direction, car-like robots are restricted by non-holonomic constraints, such as a minimum turning radius and steering angle limits. A common strategy to address this is a two-stage process: first, generating a simple geometric path (often a series of line segments), and second, smoothing that path into a feasible trajectory that respects the robot's physical constraints.

While the Gradient-informed Path Smoothing (GRIPS) algorithm is a fast solution for the second stage, it often fails when encountering sharp turns that exceed the robot's kinematic capabilities. This failure occurs because the original algorithm only operates on the existing waypoints, lacking the flexibility to deviate significantly from the initial geometric path to find a wider, feasible arc.

Kinematic Constraints

The complexity of this planning problem stems from the robot's underlying physics. The steering function must account for Euclidean distance and heading angles to ensure that any proposed path can be executed by the hardware.

Kinematic model of a car-like robot defining the non-holonomic constraints including heading angles and steering geometry.

Methodology

The authors propose three primary modifications to the GRIPS framework to overcome these limitations:

  1. Greedy Skipping (SkipStates): Instead of building a complex graph to identify shortcuts, the algorithm greedily attempts to connect the current state to the furthest possible future waypoint using a steering function. This approach significantly accelerates the pruning process.
  2. Horizon Searching (ReachNextStates): If a direct connection to the immediate next waypoint is blocked or infeasible, the algorithm searches ahead within a predefined "horizon." This allows the robot to bypass local obstacles or difficult maneuvers that would otherwise stall the planner.
  3. Heuristic Sampling (ExtraStates): This is the core innovation. When the robot cannot connect two waypoints, the algorithm deterministically samples new states perpendicular to the geometric path. This provides the robot with the "elbow room" needed to "swing wide" on tight turns, creating a feasible arc where the original geometric path was too restrictive.

Experimental Evaluation

The researchers evaluated the modified algorithm using four distinct 2D grid maps (Map1–Map4) from indoor, outdoor, and standard Moving AI benchmarks. Each map was tested with 200 randomized tasks. Furthermore, high-fidelity 3D simulations were conducted in CoppeliaSim using a model of a Clearpath Husky wheeled robot integrated with ROS.

Trajectory planning results illustrating how heuristic sampling allows the robot to deviate from the geometric path (yellow) to find feasible maneuvers (colored curves) through bottlenecks.

Key Results and Discussion

The augmented GRIPS algorithm achieved a 90% success rate, a 40% improvement over the original version in complex scenarios. In terms of efficiency, the algorithm proved to be up to 80% faster than the baseline. While other methods like Theta*-RRT might achieve slightly higher success rates in specific edge cases, the modified GRIPS produces shorter, more efficient trajectories and is significantly less computationally intensive.

In 3D simulation environments, the planner operated at 10Hz, demonstrating its capability for real-time re-planning on physical robotic hardware. The shift from random sampling (as seen in RRT) to deterministic, geometrically informed heuristic sampling ensures that computational resources are focused only where the planner is struggling.

Conclusion

By introducing off-path flexibility through heuristic sampling and adopting a greedy approach to waypoint processing, this research successfully adapts GRIPS for the demanding constraints of car-like robots. Future work will explore extending these techniques to dynamic environments and further refining the steering functions to approach mathematical optimality while maintaining real-time performance.

Summary updated

Abstract

Kinodynamic motion planning for non-holomonic mobile robots is a challenging problem that is lacking a universal solution. One of the computationally efficient ways to solve it is to build a geometric path first and then transform this path into a kinematically feasible one. Gradient-informed Path Smoothing (GRIPS) [1] is a recently introduced method for such transformation. GRIPS iteratively deforms the path and adds/deletes the waypoints while trying to connect each consecutive pair of them via the provided steering function that respects the kinematic constraints. The algorithm is relatively fast but, unfortunately, does not provide any guarantees that it will succeed. In practice, it often fails to produce feasible trajectories for car-like robots with large turning radius. In this work, we introduce a range of modifications that are aimed at increasing the success rate of GRIPS for car-like robots. The main enhancement is adding an additional step that heuristically samples the waypoints along the bottleneck parts of the geometric paths (such as sharp turns). The results of the experimental evaluation provide a clear evidence that the success rate of the suggested algorithm is up to 40% higher compared to the original GRIPS and hits the bar of 90%, while its runtime is lower.
BibTeX
@inproceedings{angulo2021augmenting,
  author = {Brian Angulo and Konstantin Yakovlev and Ivan Radionov},
  title = {{Augmenting GRIPS with Heuristic Sampling for Planning Feasible Trajectories of a Car-Like Robot}},
  booktitle = {ECMR},
  year = {2021},
  url = {https://openreview.net/forum?id=Tnb6D78mVH},
}
Figure