Light Detection and Ranging(LiDAR)-inertial simultaneous localization and mapping (SLAM) is a critical component in multi-sensor autonomous navigation systems, providing both accurate pose estimation and detailed environmental understanding. Despite its importance, existing optimization-based LiDAR-inertial SLAM methods often face key limitations: unreliable feature extraction, sensitivity to noise and sparsity, and the inclusion of redundant or low-quality feature correspondences. These weaknesses hinder their performance in complex or dynamic environments and fail to meet the reliability requirements of autonomous systems. To overcome these challenges, we propose a novel and accurate LiDAR-inertial SLAM framework with three major contributions. First, we employ a robust multi-category feature extraction method based on principal component analysis (PCA), which effectively filters out noisy and weakly structured points, ensuring stable feature representation. Second, to suppress outlier correspondences and enhance pose estimation reliability, we introduce a coarse-to-fine two-stage feature correspondence selection strategy that evaluates geometric consistency and structural contribution. Third, we develop an adaptive weighted pose estimation scheme that considers both distance and directional consistency, improving the robustness of feature matching under varying scene conditions. These components are jointly optimized within a sliding-window-based factor graph, integrating LiDAR feature factors, IMU pre-integration, and loop closure constraints. Extensive experiments on public datasets (KITTI, M2DGR) and a custom-collected dataset validate the proposed method’s effectiveness. Results show that our system consistently outperforms state-of-the-art approaches in accuracy and robustness, particularly in scenes with sparse structure, motion distortion, and dynamic interference, demonstrating its suitability for reliable real-world deployment.

Autonomous navigation in complex environments requires efficient and reliable road-network representations for fast path planning. However, traditional grid and skeleton-based approaches often suffer from high computational cost and limited path quality. This paper proposes a Hierarchical Topology-Metric Road Graph (HTMRG) framework for autonomous navigation of unmanned ground vehicles (UGVs). The method automatically constructs a hierarchical road graph from grid maps by identifying key intersection structures and generating smooth corridor and intersection connections. In addition, a dedicated start–goal insertion strategy is developed to enable efficient graph-based path planning in previously unexplored scenarios. Extensive simulations and real-world experiments demonstrate that the proposed method can automatically construct hierarchical road graphs and generate smooth, high-quality paths with improved planning efficiency and robustness. The HTMRG framework has also been successfully integrated into a UGV system, validating its effectiveness and practicality in real-world navigation scenarios.