[发明专利]一种基于LeGO-LOAM的分步式帧间位姿估计算法

摘要

本发明公开了一种基于LeGO‑LOAM的分步式帧间位姿估计算法。首先读入激光雷达的点云信息，并将点云划分为地面点和非地面点，再标记小集群点。在对点云进行分类之后，在地面点中提取地面特征点，在非地面点中提取边缘特征点。之后对位姿进行分步求解，步骤一通过地面特点计算得到帧间位姿的翻滚角、俯仰角和竖直方向的位移，并将计算得到的三自由度位姿代入步骤二的边缘特征点计算偏航角以及水平面上位移的过程，在步骤二中对六自由度位姿同时进行优化，最后再与地图点共同优化位姿。应用本发明的方法求解的轨迹精度较LeGO‑LOAM算法的轨迹精度提升最高达45.4％。