适用于复杂林下环境的鲁棒实时定位与建图方法及系统

摘要文本

本发明属于定位与建图技术领域，公开了一种适用于复杂林下环境的鲁棒实时定位与建图方法及系统，包括：原始数据测量；数据预处理；提取角度特征点；连续性约束；点‑面ICP配准；点云地图构建。首先，进行原始数据采集，并对IMU数据进行预积分，用于点云数据运动畸变校正处理和提供位姿变换初值。发明了一种基于角度的特征点提取方法。随后，把角度特征点投影为二维图像，在二维图像中剔除不稳定点。接着，使用点‑面ICP配准方法计算相邻帧点云间的位姿变换参数，将点云转换到全局坐标系。最后，基于增量式K‑D Tree数据结构，提出一种改进的局部地图构建方式，提高动态更新局部地图的效率。