Efficient Trajectory Generation in 3D Environments with Multi-Level Map Construction

TL;DR

This work proposes a robust and efficient framework to generate global trajectories for ground robots in complex 3D environments that demonstrates higher robustness to point cloud noise, enabling the generation of high quality trajectory while maintaining high computational efficiency.

Abstract

We propose a robust and efficient framework to generate global trajectories for ground robots in complex 3D environments. The proposed method takes point cloud as input and efficiently constructs a multi-level map using triangular patches as the basic elements. A kinematic path search is adopted on the patches, where motion primitives on different patches combine to form the global min-time cost initial trajectory. We use a same-level expansion method to locate the nearest obstacle for each trajectory waypoint and construct an objective function with curvature, smoothness and obstacle terms for optimization. We evaluate the method on several complex 3D point cloud maps. Compared to existing methods, our method demonstrates higher robustness to point cloud noise, enabling the generation of high quality trajectory while maintaining high computational efficiency. Our code will be publicly available at https://github.com/ck-tian/MLMC-planner.

Figures (11)

• Figure 1: The result of map construction and trajectory generation using the point cloud library (PCL) rusu20113d to visualize. The triangular patches divided into traversable and untraversable are the basic elements of the map. The red dots represent the waypoints of primitives which are generated based on patches. The yellow line represents the inital trajectory consisting of primitives located on traversable patches and the blue line represents the final trajectory after optimization.
• Figure 2: (a) and (b) represent the process of multi-level map construction. ${\rm{(}}{{\bf{x}}_{{\rm{wf}}}},{{\bf{y}}_{{\rm{wf}}}},{{\bf{z}}_{{\rm{wf}}}}{\rm{)}}$ represents the gravity-aligned world frame. ${\rm{(}}{{\bf{x}}_{{\rm{pf}}}},{{\bf{y}}_{{\rm{pf}}}},{{\bf{z}}_{{\rm{pf}}}}{\rm{)}}$ represents the patch frame. Red dots represent the point cloud after clustering. ${{\bf{S}}_{{\rm{(}}m{\rm{,}}n{\rm{,}}i{\rm{)}}}}$ is the $i$-th slice within ${\bf{mapcel}}{{\bf{l}}_{{\rm{(}}m{\rm{,}}n{\rm{)}}}}$. $\bf{meshcell}$ stores the patches whose projections on the ${{\bf{x}}_{{\rm{wf}}}}$-${{\bf{y}}_{{\rm{wf}}}}$ plane fall within it. In (c), $surface_{\rm{1}}$ is connected to $surface_{\rm{2}}$ with wall. The $surface_{\rm{3}}$ is an overhang and the height variation from $surface_{\rm{1}}$ to it is not continuous.
• Figure 3: The black lines represent the raw point cloud data. The green lines represent the results of map construction. The regions between two adjacent dashed lines represent map cells. In (a), each slice uses ${z_{{\rm{max}}}}$ as the $z$ value of the representative point, which results in a vertical direction bias in regions with gentle slope variation, such as regions ① and ③. In (b), each slice uses ${z_{{\rm{avg}}}}$. While there is no vertical direction bias in regions with gentle slope variation, the height information in regions with steep slope variation, such as area ②, is lost.
• Figure 4: The red dot represents the graph node currently being expanded and the yellow dots lines represent the motion primitives. The motion primitive ${\bf{c}}$ transitions from patch ① to ② and expands to a new graph node. The voxels discretize the 3D space and each voxel contains only one graph node by pruning.
• Figure 5: The red dot represents a trajectory waypoint located on patch ①. Except for patch ② and ③, all other patches are at the same level as the patch ①. Mesh cells marked in yellow are obstacles identified through the same-level expansion. Both parts of ${\bf{meshcel}}{{\bf{l}}_{{\rm{(}}m{\rm{,}}n + 3{\rm{)}}}}$ contain a patch at the same level while untraversable. Both parts of ${\bf{meshcel}}{{\bf{l}}_{{\rm{(}}m + 2{\rm{,}}n{\rm{)}}}}$ don't contain patches at the same level. Only one part of ${\bf{meshcel}}{{\bf{l}}_{{\rm{(}}m - 3{\rm{,}}n{\rm{)}}}}$ contains traversable patch at the same level. The nearest obstacle to the trajectory waypoint is the ${\bf{meshcel}}{{\bf{l}}_{{\rm{(}}m + 2{\rm{,}}n{\rm{)}}}}$.