Towards Efficient Trajectory Generation for Ground Robots beyond 2D Environment

Jingping Wang, Long Xu, Haoran Fu, Zehui Meng, Chao Xu, Yanjun Cao, Ximin Lyu, Fei Gao

Abstract

As robotics technology advances, ground robots are increasingly required to operate in environments that extend beyond simple planar surfaces. These robots encounter passive height variations due to complex, rugged terrain and may possess active height control capabilities through specialized mechanical structures. Traditional navigation frameworks often fail to account for both types of vertical motion simultaneously, limiting the operational scope of ground robots. This paper presents an optimization-based planning framework designed for ground robots that considers both active and passive height changes in the z-axis. The proposed planner utilizes a Valid Ground Filter (VGF) and a safety penalty field defined in $\mathbb{R}^3$, ensuring a continuous optimal solution space and high-quality, smooth trajectories. By incorporating custom z-axis constraints, the framework is adaptable to various robot configurations. Simulations and real-world experiments demonstrate the efficiency and trajectory quality of the proposed algorithm.

Introduction

Most conventional ground robot navigation systems operate under the assumption that the world is a flat, 2D manifold. While this simplification reduces computational complexity, it is insufficient for real-world applications involving rugged terrain, multi-layered structures like parking garages, or robots with the mechanical freedom to adjust their own height.

Existing solutions typically address either passive height changes (following the terrain's shape) or ignore 3D geometry entirely. They rarely integrate active height control—where a robot, such as a wheeled-bipedal platform, might need to crouch to navigate under an obstacle. Furthermore, many existing 3D planners are computationally intensive, hindering their use in real-time, agile maneuvers. This research addresses these gaps by proposing a unified framework for 3D trajectory generation.

Methodology

The proposed planning pipeline consists of three primary components designed to balance safety, traversability, and computational efficiency.

1. Valid Ground Filter (VGF)

To manage the complexity of 3D environments, the framework employs a Valid Ground Filter. Rather than treating all detected points as obstacles, the VGF identifies "traversable" points based on surface orientation and local flatness. A point is deemed valid if it resides on an upper surface where the robot can maintain stability. This process effectively prunes the search space, allowing the pathfinder to focus only on viable regions of the environment.

2. Safety Penalty Field in $\mathbb{R}^3$

To facilitate smooth optimization, the authors construct a penalty field that encodes collision risks and terrain hazards, such as steep slopes. A core innovation is the Downward Projection Strategy. This strategy projects the robot's 3D position onto the nearest identified ground surface to estimate its posture (tilt) and the sharpness of terrain changes. This allows the optimizer to account for ground constraints even when the trajectory is not perfectly aligned with the surface, providing necessary gradients for the optimization process.

3. Spatio-temporal Trajectory Optimization

The framework utilizes the MINCO (Minimum Control Effort Polynomial) trajectory class to formulate an unconstrained optimization problem. The objective function minimizes a weighted sum of costs, including:

  • Smoothness: Minimizing jerk for fluid motion.
  • Time: Optimizing for total travel duration.
  • Safety: Maintaining distance from obstacles.
  • Traversability: Ensuring the terrain is suitable for the robot's chassis.

For robots with active degrees of freedom, a custom penalty term is added to ensure the robot's height remains within a safe and feasible range relative to both the floor and any overhead obstructions.

Experimental Evaluation

The system was evaluated through both high-fidelity simulations and real-world hardware tests.

Simulation Results

Using Unity-based environments featuring uneven forests and multi-layered viaducts, the proposed method was compared against state-of-the-art benchmarks (Liu’s and Krusi’s methods).

  • Efficiency: In an "Abandoned Station" scenario, the proposed method achieved a terrain assessment time ($t_a$) of 1.28s, significantly faster than Liu’s method (50.7s) and Krusi’s (2.9s).
  • Trajectory Quality: The generated paths were shorter and smoother. In the "Uneven Forest" test, the mean curvature ($\kappa_m$) was 0.042, compared to 0.174 and 0.136 for the benchmarks.

Real-World Hardware

Tests were conducted using the "Diablo" direct-drive self-balancing wheeled bipedal robot. Equipped with a Livox Mid-70 LiDAR, the robot successfully navigated complex environments by choosing barrier-free ramps over stairs and actively crouching to pass under low-hanging tables—behaviors that traditional 2D or passive-3D planners cannot replicate.

Discussion and Conclusion

This research demonstrates that treating the z-axis as a controllable degree of freedom is essential for the next generation of morphing ground robots. By constructing a continuous optimization space in $\mathbb{R}^3$, the framework produces higher-quality trajectories than discrete search methods.

While the current system is highly efficient, it relies on a 3D grid map which can be memory-intensive for large-scale environments. Additionally, some pre-processing steps are currently performed offline. Future work will focus on optimizing memory utilization and transitioning the entire pipeline to an online, real-time setup to better handle dynamic obstacles and rapidly changing environments.

Summary updated

Abstract

With the development of robotics, ground robots are no longer limited to planar motion. Passive height variation due to complex terrain and active height control provided by special structures on robots require a more general navigation planning framework beyond 2D. Existing methods rarely considers both simultaneously, limiting the capabilities and applications of ground robots. In this paper, we proposed an optimization-based planning framework for ground robots considering both active and passive height changes on the z-axis. The proposed planner first constructs a penalty field for chassis motion constraints defined in R3 such that the optimal solution space of the trajectory is continuous, resulting in a high-quality smooth chassis trajectory. Also, by constructing custom constraints in the z-axis direction, it is possible to plan trajectories for different types of ground robots which have z-axis degree of freedom. We performed simulations and realworld experiments to verify the efficiency and trajectory quality of our algorithm.
BibTeX
@inproceedings{wang2023towards,
  author = {Jingping Wang and Long Xu and Haoran Fu and Zehui Meng and Chao Xu and Yanjun Cao and Ximin Lyu and Fei Gao},
  title = {{Towards Efficient Trajectory Generation for Ground Robots beyond 2D Environment}},
  booktitle = {IEEE International Conference on Robotics and Automation},
  year = {2023},
  url = {https://openreview.net/forum?id=KpN48qtbLm},
}
Figure