Driving on Surfelgrid: Motion Planning and Trajectory Optimization in Uneven Environments

Abstract

In this paper, we present a navigation framework for wheeled robots operating in uneven environments. Considering the sparsity of the moving surface along the Z-axis, we introduce a novel map representation, namely Surfelgrid. Surfelgrid uses surfels to represent geometric features of the local area and uses lists to efficiently store multi-layer surfaces, which is suitable for most wheeled robot scenarios. On this basis, we devise a hierarchical planning approach by introducing milestones. Specifically, we search for an initial path that satisfies the kinematic constraints by extending Hybrid A* to the surfel state space. Additionally, we improve the smoothness and clearance of the trajectory with B-spline optimization, which incorporates surfel geometric information to meet the landing constraints of wheeled robots. Finally, by representing the final trajectory as a non-uniform B-spline, an iterative time adjustment method is adopted to guarantee the kinematic feasibility of the trajectory. We validate the proposed method in various complex simulation environments and the real world. We release our code as an open-source package.