一种自适应利用直线和平面特征的特征拟合与约束构建方法

本发明提出了一种基于迭代误差状态卡尔曼滤波的紧密耦合激光雷达‑惯性里程计方法。属于激光Lidar定位领域。包括以下步骤：首先通过计算激光雷达点的真实测量方向和真实距离的得到的地面真值点位置，根据真值点位置与其测量值的关系得到激光雷达点的测量噪声；再通过计算体素中激光雷达点的协方差矩阵实现PCA(主成分分析，PrincipalComponent Analysis)方法来判断点‑线，点‑面拟合关系；然后通过拟合阈值的自适应调节策略，根据环境的噪声条件自适应更改拟合阈值；随后通过建立双阈值判定机制来拟合直线特征与平面特征；最后通过预测点在世界坐标系下对应的根体素中的结构化特征构建对应的直线或平面约束。本发明提出的一种自适应利用直线和平面特征的特征拟合与约束构建方法，使系统对复杂场景的理解能力显著提升，提高系统定位精度。