考虑到激光雷达采集点云的数据量大，如果直接采用非线性最小二乘方法，会带来迭代次数多、运算量大等问题，因此本文设计了一种基于线面特征的两级LM匹配算法，在保证匹配精度的同时，可以大幅提升匹配的实时性。

（1）特征提取

为了避免直接对大量的点云数据进行匹配计算，也为了能减小点云分布对后续匹配定位的影响，对点云信息进行特征提取，并等量筛选不同方向的特征。

将360°的视角分割为6部分，在每个方向分别选择等量的最优的线特征与面特征，作为后续进行匹配的特征。

（2）两级LM匹配

考虑到不同方向的位置、姿态信息的收敛速度不同，根据所提取特征的特点，设计一种两级LM匹配方案，如图3所示。一方面可以降低迭代次数、提升匹配实时性；另一方面，考虑到激光雷达线数限制，俯仰角、高度精度有限，两级LM匹配方案更便于拓展辅助信息源的水平姿态与高度信息的辅助。