一种基于迭代误差状态卡尔曼滤波的紧密耦合激光雷达-惯性里程计方法

本发明提出了一种基于迭代误差状态卡尔曼滤波的紧密耦合激光雷达‑惯性里程计方法。属于激光Lidar定位领域。包括以下步骤：首先通过预处理Lidar点云和IMU数据进行运动补偿来初始化Lidar点云；其次通过哈希表存储并增量更新平面参数或统计信息来构建全局地图；再通过体素中平面特征和统计信息进行特征拟合，并根据环境特征自适应地利用这些信息进行点云配准；然后通过迭代误差状态卡尔曼滤波(IESKF)进行状态估计，同时结合点对平面和点对分布的约束，提高定位精度；最后通过新的Lidar扫描数据增量更新体素地图中的平面参数和统计信息。本发明提出的一种基于迭代误差状态卡尔曼滤波的紧密耦合激光雷达‑惯性里程计方法，使基于该紧密耦合激光雷达‑惯性里程计方法的激光雷达SLAM算法在定位精度和鲁棒性上较其他先进算法有大幅提升。