Path Planning on Multi-level Point Cloud with a Weighted Traversability Graph
Authors: Yujie Tang, Quan Li, Hao Geng, Yangmin Xie, Yusheng Yang, Hang Shi
Organizations: Areaspace Information Research Institute, Chinese Academy of Sciences, Beijing, China · School of Mechatronics Engineering and Automation, Shanghai University, Shanghai, 200444, China · Shanghai Key Laboratory of Intelligent Manufacturing and Robotics, Shanghai University, Shanghai, 200444, China
This article proposes a new path planning method for addressing multi-level terrain situations. 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 A* algorithm is modified to utilize the traversability graph to generate a short and safe path. The effectiveness and reliability of the proposed method are verified through indoor and outdoor experiments conducted in various environments, including multi-floor buildings, woodland, and rugged mountainous regions. The results demonstrate that the proposed method can properly address 3D path planning problems for ground vehicles in a wide range of situations.
Figures & tables
Fig 1 : Structure of the multi-layered Skimap.
Fig 2 : Point cloud simplification process. (a) Test scene. (b) Original dense point cloud acquired from the highlighted area. (c) Curvature distribution map computed from local surface geometry. (d) Variable-resolution map after simplification.
Fig 3 : Definition of the traversability vector Fi,j .
Fig 4 : Weighted traversability graph construction. (a) the local ML-SkiMap voxels. (b) the extraction of the local surface voxels around some ai.j . (c) connectivity analysis of the multi-level surfaces around ai.j . (d) traversability analysis at ai.j in 8 directions on two levels. (e) combining results of Ci,jl and Fi,jl to get the WTG.
Fig 5 : A simplified geometric model of the vehicle.
Fig 6 : Footprints of the vehicle and the four corresponding fitted planes to estimate the vehicle pose.
Fig 7 : The tilt angle of the vehicle.
Fig 8 : Cases when the vehicle is at the occlusion areas.
Fig 9 : Chassis collides with obstacle points.
Fig 10 : The visualization of the WTG for the testing ground in Figure 3
Fig 11 : The experimental UGV platform.
Fig 12 : The 3D LiDAR system.
Fig 13 : Indoor environment mapping results. (a) Indoor test area with boxes and furniture serving as obstacles. (b) Dense point cloud acquired using a 3D LiDAR sensor. (c) Variable-resolution map generated by the proposed method.
Fig 14 : The traversability map of the indoor scene from: (a) the top view and (b) the side view. The green regions are traversable, and the black regions are unpassable.
Fig 15 : Indoor path planning for two pairs of start and target points. The red lines are the planned path of the vehicle center, and the square shapes are the corresponding vehicle projection to the xy planes.
Fig 16 : The first test scene. (a) The picture of the motto. (b) Its dense point cloud map. (c) Its variable resolution map.
Fig 17 : The traversability map of the motto. (a) From the top view. (b) From side view.
Fig 18 : Path planning for two pairs of start and target points in the motto scene. The red lines are the planned path of the vehicle center, and the square shapes are the corresponding vehicle projection to the planes.
Fig 19 : The second outdoor test scene. (a) The picture of the grove. (b) Its point cloud map.
Fig 20 : The traversability map of the grove from the top view.
Fig 21 : Path planning results in the grove scene. The red lines are the planned path of the vehicle center, and the square shapes are the corresponding vehicle projection to the xy planes.
Gaussian noise deviation (m)
0.01
0.02
0.035
0.06
Success rate
100%
100%
100%
100%
Subsample rate
90%
70%
50%
30%
Success rate
100%
100%
100%
20%
Table 1: Success rate of the proposed path planning method under different measurement conditions.
Category
Description
Symbol
Value
Mapping
Voxel size
d×d×dz
500×500×500 mm
Variant resolution map parameter
[a,b,c]
[900,3,1]
Minimum distance between two layers
hL
3000 mm
Vehicle dimensions
Half of the distance between left and right wheels
W
887 mm
Half of the distance between front and rear wheels
L
1425 mm
The radius of the wheel
R
458.15 mm
Table 2: Path planning related parameters in the tests using Mercedes-Benz G500
Fig 22 : Off-road vehicle, Mercedes-Benz G 500 4×4².
Fig 23 : The original test scenes from the online databases.
Fig 24 : The variable resolution point clouds of the online datasets (shown as the left pictures) and the corresponding WTGs (shown as the right pictures).
Fig 25 : Planned path in Scenario 1.
Fig 26 : Planned path in Scenario 2.
Fig 27 : Planned path in Scenario 3.
Fig 28 : Sensitivity analysis of hyperparameter λ in Scenario 3 (parking garage). Paths are color-coded by λ value: red ( λ=10 ), cyan ( λ=100 ), orange ( λ=1000 ), and purple ( λ=10000 ). Higher λ values produce longer paths that avoid steep inclines, demonstrating the trade-off between path length and traversability. Path statistics: λ=10 : L=318.3 m; λ=100 : L=348.3 m, λ=1000 : L=531.2 m, λ=10000 : L=954.5 m.
Fig 29 : The path planning result in [ 16 ] . Light blue points denote low roughness, gray points denote high roughness, and dark red ones denote untraversable.
Fig 30 : The path planning result using our method with similar choice of starting and target points.
Fig 31 : The traversability map of the bunny.
Fig 32 : Baseline comparison results on Scenarios 2 and 3. The proposed method demonstrates robust performance across multi-level structures, while competing approaches exhibit fundamental limitations: OMDP cannot handle vertical occlusion, and SBSP requires severe data reduction to achieve computational tractability, sacrificing path quality. The red, green, and yellow paths correspond to the trajectories generated by the proposed method, OMDP, and SBSP, respectively. (a) Scenario 2, example 1. (b) Scenario 2, example 2. (c) Scenario 3.
SBSP-planner [ 2 ]
OMDP-planner [ 37 ]
Ours
Scenario 2 over bridge
Path length (m)
/
/
634.9
Computation time (s)
/
/
43.6
Scenario 2 under bridge
Path length (m)
/
202.7
203.2
Computation time (s)
/
1.6
2.55
Table 3: Path planning performance comparison across test scenarios. Planning time includes both map query and graph search. “/” indicates planning failure (no valid path found within timeout or geometrically invalid result).
Surface Retreival time (s), Scenario 2
Map size (MB), Scenario 2
Map size (MB), Scenario 3
Surfel-based Map [ 2 ]
0.103
51.3
9.4
OctoMap [ 37 ]
0.070
43.4
8.1
ML-SkiMap (Ours)
0.032
12.6
8.6
Table 4: Comparison of map retrieval efficiency and memory usage. Surface retrieval time denotes the time required to access all traversable surface elements from the stored map representation. Map size indicates the total memory required to represent the environment, including the original point cloud data and the associated voxel- or surfel-based map structures used for planning.
Faculty of Engineering and Natural Sciences, Tampere University, Finland. · Institute of Technology, University of Tartu, Estonia. · AI and ML Department, Volvo Autonomous Solutions, Sweden.