AESF-LIO: Adaptive Error-State Fusion LiDAR-Inertial Odometry for Ground Vehicles in Structured Environments

In LiDAR-based Simultaneous Localization and Mapping (SLAM) systems for vehicles, the point-to-plane Iterative Closest Point (ICP) method is widely used for scan matching. This approach incorporates all planar points into a single objective function for optimization, yet does not explicitly distinguish the differences in the constraints that ground and non-ground points impose on the pose variables. Ground points primarily constrain the z translation as well as roll and pitch, whereas non-ground points predominantly constrain horizontal translation and yaw. The constraints provided by non-ground points on the z translation, roll, and pitch depend on their spatial distribution and can be weak for z in environments dominated by vertical structures. When one point type dominates in number while providing weak constraints on certain pose variables, the joint optimization may become biased, resulting in insufficient refinement of the weakly constrained variables and degraded odometry accuracy. To address this issue within a point-to-plane ICP framework, this letter proposes a LiDAR-inertial odometry system that separately minimizes point-to-plane residuals for ground and non-ground points and adaptively fuses their corresponding error states to improve pose accuracy. The system employs ground segmentation to divide the point cloud into ground points and non-ground points; it then computes the error state vectors for ground and non-ground points separately and updates the pose using an adaptive weighted fusion strategy. Experiments in simulation, on public datasets, and in real-vehicle experiments demonstrate that the proposed method significantly improves accuracy compared with advanced SLAM baselines.