Path Planning on Multi-level Point Cloud with a Weighted Traversability Graph

The gist This article proposes a new path planning method for addressing multi-level terrain situations.

ML-SkiMap Generation

** The proposed method includes innovations in three aspects: 1) the pre-processing of point cloud maps with a multi-level skip-list structure and data-slimming algorithm for well-organized and simplified map formalization and management, 2) the direct acquisition of local traversability indexes through vehicle and point cloud interaction analysis, which saves work in surface fitting, and 3) the assignment of traversability indexes on a multi-level connectivity graph to generate a weighted traversability graph for generally search-based path planning.<ref:2504.216221,the proposed method includes innovations in three aspects: 1) the pre-processing of point cloud maps with a multi-level skip-list structure and data-slimming algorithm for well-organized and simplified map formalization and management, 2) the direct acquisition of local traversability indexes through vehicle and point cloud interaction analysis, which saves work in surface fitting, and 3) the assignment of traversability indexes on a multi-level connectivity graph to generate a weighted traversability graph for generally search-based path planning.**

** The ML-SkiMap structure is designed to store sparse point clouds efficiently. SkiMap is a mapping framework proposed in [11], which is specifically designed to store sparse point clouds in 3D space, having the combined advantages of a 3D grid in computational complexity and of an Octree in storage size.<ref:2504.216223,SkiMap is a mapping framework proposed in [11], which is specifically designed to store sparse point clouds in 3D space, having the combined advantages of a 3D grid in computational complexity and of an Octree in storage size.**

** The ML-SkiMap adds a layer to indicate the levels of the cells in the vertical direction. The cells in the z-direction are partitioned to be different groups, which embody the vertical structure information at a specific XY location.<ref:2504.216223,The major difference of ML-SkiMap with the original SkiMap structure is that we add a layer to indicate the levels of the cells in the vertical direction. The cells in the z-direction are partitioned to be different groups, which embody the vertical structure information at a specific XY location.**

** Variable resolution map generation is achieved by estimating surface curvature numerically for each voxel based on Principal Component Analysis (PCA). The number of data points to be preserved in a voxel is determined by the local curvature. For example, using coefficients a1 = 900, b1 = 3, c1 = 1, the result shows that the resolution for the flat area is largely decreased, and the complex shapes are preserved.<ref:2504.2162243,The number of data points Nl i j k to be preserved in V l i j k is then determined by the local curvature Curvl i j k, as in Eq. (4). For example, using coefficients a1 = 900, b1 = 3, c1 = 1, the result in Figure 1(c) shows that the resolution for the flat area is largely decreased, and the complex shapes are preserved.**

Weighted Traversability Graph (WTG)

** A weighted traversability graph (WTG) is proposed to include information both on driving safety and surface connectivity in the multi-level map. The geometric connectivity graph between neighbor cells represents surface continuity properties.<ref:2504.216223,We propose a weighted traversability graph (WTG) to include information both on driving safety and surface connectivity in the multi-level map. The geometric connectivity graph between neighbor cells represents surface continuity properties.**

** Bi-directional traversability indexes are assigned on the graph’s edges. With the WTG defined, graph search methods or sample-based methods can find the feasible path between any given start and target locations.<ref:2504.216223,Bi-directional traversability indexes are assigned on the graph’s edges. With the WTG defined, graph search methods or sample-based methods can find the feasible path between any given start and target locations.**

** The local WTG on ai,j is completed as shown in Figure 3(e). The connectivity value C l1→l2 i j→i+1 j is set to be 0 if the distance in Z direction k1 − k2 is larger than a specified value cmax, meaning voxels are considered on different levels and thus unconnected.<ref:2504.216223,if there are large disparities between the height of two surface cells, the relationship between the two is marked as unconnected, and vice versa.**

Calculation of Traversability Weights

** The method to calculate F l i j is similar to [15], with some modifications to accommodate the multi-level map structures. The driving safety of a grid on the x-y plane is evaluated at multiple layers.<ref:2504.216223,In this section, we improve the single-layer analysis to be an ML-SkiMap version, which combines the vehicle-environment interacted analysis method in [15]. The driving safety of a grid on the x-y plane is evaluated at multiple layers.**

** Tip-over and slip hazards are addressed by adopting static analysis. The vehicle is simplified with critical geometric parameters such as the distance between the left and right wheels 2W, the distance between the front and rear wheels 2L, the radius of the wheel R, and the height of the chassis H.<ref:2504.2162233,The vehicle is simplified as shown in Figure 4, with critical geometric parameters as the distance between the left and right wheels 2W, the distance between the front and rear wheels 2L, the radius of the wheel R, and the height of the chassis H.**

** The angle αw between the vertical vector and nw is used to estimate vehicle inclination. The larger this angle αw, the more dangerous it is for driving at that location and direction.<ref:2504.2162238,The angle αw between the vertical vector and nw is used to estimate vehicle inclination. The larger this angle αw, the more dangerous the vehicle is at the current location and driving direction.**

** The traversability cost of a corresponding pose is set as a very large value η if hazards like falling in an unknown area, flip over, or chassis collision could happen.<ref:2504.2162231,When the hazards of falling in an unknown area, flip over, or chassis collision could happen, the traversability cost of the corresponding pose is set as a very large value η to prevent vehicles from passing by.**

Graph-based Path Search

** The A**

algorithm is modified to utilize the traversability graph to generate a short and safe path. The travel cost at a candidate voxel V p m n is not only determined by itself, but also by its parent voxel V k i j.<ref:2504.2162231,The travel cost at a candidate voxel V p m n is not only determined by itself, but also by its parent voxel V k i j.<ref:2504.2162231,f(V p m n) = g p m n + h p m n + λ · F k i j→m n (11), where V p m n is an adjacent node to V k i j; f(V p m n) is the estimated total travel cost of V p m n; g p m n is the actual travel cost from the start voxel to V p m n; h p m n is the heuristic estimated distance cost from V p m n to the goal (the Euclidean distance is used here); F k i j→m n is the F-map value at V k i j in the direction from k th level cell of (i, j) to p th level cell of (m, n); λ is a constant value to balance the distance cost and the traversability cost, which should be tuned according to the resolution of the traversability map.

** The modified A**

algorithm for path planning is presented as in Algorithm. 2, which terminates when the target point is reached or there are no paths eligible.<ref:2504.2162231,The corresponding modified A∗ algorithm for path planning is presented as in Algorithm. 2, which terminates when the target point is reached, or there are no paths eligible.</ref:2504.2162231

Experimental Results and Discussions

** The results demonstrate that the proposed method can properly address 3D path planning problems for ground vehicles in a wide range of situations.<ref:2504.216221,The results demonstrate that the proposed method can properly address 3D path planning problems for ground vehicles in a wide range of situations.**

** The article proves that the method is robust to various environments with different terrain surfaces and structures.<ref:2504.216221,The article proves that the method is robust to various environments with different terrain surfaces and structures.

1. Bold header: Multi-level Data Structuring for Point Clouds

The proposed ML-SkiMap structure, which stores only the occupied cells in 3D space using a SkipList tree, combined with level indices, allows for efficient management of multi-level terrain data, addressing the challenge of differentiate their vertical structures when evaluating the traversability.

2. Bold header: Dynamic Traversability Weighting

The system generates a weighted traversability graph (WTG) to include information both on driving safety and surface connectivity, where edge weights are derived from local traversability indexes, ensuring that path planning considers not just geometric adjacency but also driving safety and surface connection conditions.

3. Bold header: Adaptive A-Star Path Planning

The modified A-star algorithm calculates the travel cost as a combination of accumulated path cost, heuristic distance, and the graph's traversal cost: f(V p,m,n) = g p,m n + h p,m n + λ · F k,i,j→m,n, enabling it to find a short and safe path by balancing distance cost with traversability constraints.

4. Bold header: Enhanced Hazard Avoidance

The system incorporates a conservative strategy for occlusions by setting the traversability cost to a very large value η when any of the footprints of the four wheels are at the area of the occlusion, effectively preventing vehicles from entering areas where the terrain information is missing due to occlusions.

5. Bold header: Robust Vehicle-Terrain Interaction Modeling

The calculation of traversability costs explicitly models vehicle dynamics by fitting planes to point clouds and calculating an inclination angle αw, which is used to determine danger, as stated in the paper: the larger the angle αw, the more dangerous the vehicle is at the current location.