GR-LIO: A Local Ground-Aware LiDAR-Inertial Odometry System Using Body-to-Ground Height
Authors: Zhixin Zhang, Yang Song, Liang Zhao, Nathan Shankar, Barry Lennox, Pawel Ladosz
Organizations: Department of Electrical and Electronic Engineering, University of Manchester, Manchester M13 9PL, U.K. · School of Informatics, University of Edinburgh, Edinburgh EH8 9AB, U.K. · Department of Mechanical, Aerospace and Civil Engineering, University of Manchester, Manchester M13 9PL, U.K.
LiDAR-inertial odometry (LIO) is widely used for state estimation in ground-based autonomous mobile robots. However, the geometric constraints provided by the local ground surface remain largely underexploited in existing LIO systems. This paper proposes a filter-based local ground-aware LIO framework that explicitly incorporates local ground plane geometry into the state estimation process to improve both localization accuracy and computational efficiency. Specifically, a local ground plane is parameterized by the robot orientation and the body-to-ground (B-G) height and continuously propagated within the state estimation process. Based on the proposed B-G geometry model, a propagated local ground plane enables efficient and reliable ground segmentation. The segmented ground points are then incorporated into the filter update through point-to-plane geometric constraints, improving both state estimation accuracy and efficiency. Furthermore, a planar motion update is introduced to exploit the propagated local ground plane as an additional geometric constraint, effectively suppressing vertical drift and improving estimation robustness. To address the initially unknown B-G height, an efficient initialization strategy is developed, followed by an online calibration procedure for continuous refinement. The proposed system is evaluated on several public benchmark datasets and self-collected real-world datasets covering diverse operating scenarios. Experimental results demonstrate that the proposed method consistently outperforms representative LIO methods in terms of both localization accuracy and computational efficiency.
Figures & tables
Fig. 1: Geometric parameterization of the local ground plane using the body-to-ground (B-G) height and robot orientation.
Fig. 2: A simple 2-D model of the local ground plane parameterization using the body-to-ground (B-G) height.
Fig. 3: Planar motion residuals used in the proposed state update. Gn and Gd denote the normal vector and offset of the ground plane in the global frame, respectively. rz and rn represent the translational and orientation residuals with respect to the local ground plane.
Fig. 4: System overview of GR-LIO. The blue block indicates the state estimation process, while the red block represents the mapping module.
Fig. 5: Examples of situations where the planar motion assumption is violated. The red highlighted regions indicate areas where the local ground geometry changes and the B-G based local ground model becomes invalid.
Fig. 6: Vertical drift relative to the estimated ground plane during slope transitions and uneven-terrain traversal. The dashed line indicates the threshold used for planar motion checking, while red markers denote samples that exceed the threshold.
Fig. 7: Experimental platform and data collection environments.
Fig. 8: Qualitative comparison of ground segmentation results between GR-LIO and LeGO-LOAM on representative sequences from the M2DGR dataset. For each sequence, the left column shows the segmentation results of GR-LIO, the center column presents an image of the corresponding real-world environment for reference, and the right column shows the segmentation results of LeGO-LOAM. The major differences between the two methods are highlighted with green and red circles to facilitate visual comparison. It can be observed that LeGO-LOAM produces more wromng ground segmentations than GR-LIO.
Sequence
GR-LIO (Ours)
LeGO-LOAM [ 5 ]
hall 01
0.45
7.55
door 01
0.48
9.05
gate 01
0.43
7.89
street 03
0.44
7.98
TABLE I: Average execution time of the ground segmentation module on representative M2DGR sequences (ms).
Sequence
LINS [ 13 ]
LIO-SAM [ 3 ]
FAST-LIO2 [ 4 ]
GR-LIO (FP)
GR-LIO(w/o PU)
GR-LIO
APE t
RPE t
APE t
RPE t
APE t
RPE t
APE t
RPE t
APE t
RPE t
APE t
RPE t
gate 01
0.296
0.353
0.164
0.298
0.179
0.145
0.173
0.147
0.172
0.144
0.172
0.144
gate 02
FAIL
0.438
0.288
0.325
0.142
0.329
0.096
0.328
0.142
0.320
0.140
gate 03
1.127
0.699
0.416
0.275
0.218
0.137
0.222
0.142
0.220
0.137
0.215
0.135
circle 01
FAIL
1.748
0.507
1.340
0.247
1.554
0.166
1.329
0.243
1.332
0.166
circle 02
FAIL
2.579
0.525
1.241
0.178
1.247
0.175
1.238
0.175
1.241
0.159
TABLE II: RMSE comparison of different algorithms and GR-LIO variants on the M2DGR dataset (APE t : m, RPE t : m).
Fig. 9: Trajectory comparison of FAST-LIO2 (green) and our GR-LIO (blue) with the ground truth (black dashed) in room 03 along the x , y , and z axes.
Fig. 10: Representative mapping results and estimated trajectories on the M2DGR dataset. The white curves represent the estimated robot trajectories. Green points indicate segmented local ground points, while red points denote non-ground points.
Sequence
FAST-LIO2 [ 4 ]
GR-LIO (FP)
GR-LIO(w/o PU)
GR-LIO
ATE t
RPE t
ATE t
RPE t
ATE t
RPE t
ATE t
RPE t
Outdoor01
0.244
0.151
17.56
12.74
0.242
0.150
0.216
0.142
Outdoor04
0.826
0.155
0.879
0.276
0.910
0.155
0.640
0.127
Dynamic03
1.144
0.144
0.044
0.008
0.163
0.144
0.138
0.122
Dynamic04
0.297
0.144
0.761
0.477
0.273
0.143
0.123
0.136
Varying-illu03
0.638
0.166
0.812
0.168
0.654
0.171
0.396
0.158
TABLE III: RMSE comparison of FAST-LIO2 and GR-LIO variants on the M3DGR dataset (ATE t : m, RPE t : m).
Fig. 11: Comparison of FAST-LIO2 (blue), GR-LIO without planar motion update (orange), and GR-LIO (green) against the ground truth (black dashed) along the x , y , and z axes in M3DGR Dark02.
Sequence
GR−LIOInit
GR−LIOCalib
GR−LIOGT
APE t
APE r
RPE t
APE t
APE r
RPE t
APE t
APE r
RPE t
gate 01
0.176
2.740
0.145
0.172
2.740
0.144
0.172
2.739
0.144
gate 02
0.341
2.732
0.185
0.326
2.732
0.142
0.320
2.732
0.140
street 01
0.707
2.749
0.137
0.286
2.745
0.136
0.308
2.746
0.136
street 02
3.259
2.744
0.223
2.898
2.743
0.222
2.876
2.743
0.222
hall 01
0.294
2.828
0.920
0.290
2.828
0.920
0.287
2.828
0.920
TABLE IV: RMSE comparison under different B-G height configurations on the M2DGR dataset (APE t : m, APE r : °, RPE t : m). The three configurations correspond to initialization only, online calibration, and ground-truth B-G height.
Fig. 12: Initialization and convergence behavior of the ground height estimate during online calibration on the M2DGR dataset. (a)(b) show indoor sequences, while (c)(d) show outdoor sequences. The red triangle indicates the initialization result, and the red dashed line denotes the ground truth.
Fig. 13: Comparison of FAST-LIO2 and GR-LIO in terms of the number of features used for state update and the corresponding search time at each frame. The blue and orange curves represent FAST-LIO2 and GR-LIO, respectively, while the dashed lines indicate the corresponding mean values.
Method
room 01
hall 01
gate 01
street 03
LINS [ 13 ]
13.07
20.52
30.05
39.59
LIO-SAM [ 3 ]
8.65
23.31
36.01
55.71
FAST-LIO2 [ 4 ]
3.46
8.07
20.76
19.07
GR-LIO (W/O PU)
2.02
5.56
14.57
14.23
GR-LIO
2.24
6.35
15.40
15.73
TABLE V: Average execution time per LiDAR frame, including state update and incremental map update (ms).
Fig. 14: Mapping results and estimated trajectories on the self-collected datasets. The white curves represent the estimated robot trajectories. The left and right columns correspond to GR-LIO and FAST-LIO2, respectively. In the results of our GR-LIO, segmented local ground points are highlighted in green, while non-ground points are shown in red. The coordinate frames indicate the estimated start and end poses of the robot.