Multi-Stage Optimization for Stable WMR Motion Planning in Slope Environments

In this work, we propose a trajectory planning framework for wheeled mobile robot (WMR) navigation in environments with sloped terrain. The framework adopts a hybrid strategy that combines sampling-based and optimization-based methods to plan stable trajectory for the robot on a known elevation map. It adopts a two-layer hierarchical structure to ensure fast and stable convergence of the planning process. In the first layer, a traversability probabilistic roadmap (T-PRM) is constructed to perform a coarse-level feasible route search, which serves to identify traversable regions and determine the task sequence. In the second layer, the planning problem is formulated as a multi-stage optimal control problem (MOCP), taking into account obstacle constraints, terrain constraints, and kinematic constraints. Notably, the coarse trajectory generated in the first layer defines the connectivity of the MOCP, significantly accelerating the convergence of the optimization. Finally, extensive results demonstrate the effectiveness of the proposed framework.