(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210876326.5 (22)申请日 2022.07.25 (71)申请人 之江实验室 地址 311100 浙江省杭州市余杭区中泰街 道之江实验室南湖总部 (72)发明人 付宜利　顾建军　李恩博　朱世强　 (74)专利代理 机构 杭州浙科专利事务所(普通 合伙) 33213 专利代理师 陈洁 (51)Int.Cl. G01C 21/16(2006.01) G01C 21/00(2006.01) G01S 17/86(2020.01) G01S 17/93(2020.01) G06V 10/26(2022.01)G06V 10/44(2022.01) (54)发明名称 一种针对地面移动机器人的Lidar-IMU融合 的slam方法 (57)摘要 本发明属于同步定位与建图(slam)领域， 公 开了一种针对地面移动机器 人的Lidar ‑IMU融合 的slam方法， 包 括如下步骤： 步骤1： 数据预处理； 步骤2： 构建子地图； 步骤3： 创建地图与优化因子 图； 步骤4： 优化位姿图。 本发明能够实现在有起 伏路面上为机器人添加地面约束， 增加竖直方向 的约束， 能够很好的减少累积误差， 整个实验机 器人轨迹长大约1000m， 与传统算法相比本发明 的方法在精度上有很大的提高， 特别是在z方向 更为明显。 其中loam的平移偏差(均方根误差表 示)为18.461m， lio_sam的平移偏差为15.672m， 而本发明的算法平移偏差为8.331m。 该算法得到 的轨迹精度有了 很大的提高。 权利要求书4页 说明书10页 附图2页 CN 115355904 A 2022.11.18 CN 115355904 A 1.一种针对地面移动机器人的Lidar ‑IMU融合的slam方法， 其特征在于， 包括如下步 骤： 步骤1： 数据预处 理； 包括点云去畸变和特 征提取； 步骤2： 关键帧选择与子地 图构建； 引入视觉slam中的关键帧概念， 使用两个关键帧之 间的激光雷达信息来构造局部 子地图； 步骤3： 关键帧的因子图优化； 通过因子图优化将关键帧与世界坐标系相关联， 构建一 个因子图来联合优化激光雷达关键帧和IMU的预积分； 步骤4： 位姿图优化； 通过位姿图优化得到一个全局一 致的环境 地图。 2.根据权利 要求1所述的针对地面移动 机器人的Lidar ‑IMU融合的slam方法， 其特征在 于， 所述步骤1包括 点云去畸变和特 征提取； 所述点云去畸变： 在因子优化过程中， IMU的预积分提供预积分因子， 而优化过程IMU的 偏差也会作为优化参数参与优化， 利用得到的IMU偏 差纠正的IMU 数据来估计任意时刻载体 的位姿， 将激光 点变换到统一 坐标系下， 进行点云畸变校正； 所述特征提取： 从去畸变的点云中提取特征点云， 提取边点和表面点作为匹配的特征 点， 对于任何点p， 从与p同一行扫描中找到10个连续点， 用S表示， 均分 的位于p的两侧， 则p 的粗糙度可以通过以下公式计算： 式中|S|是S中点的个数， 取10， ri表示第i个点的深度； 选择粗糙度值较小的点作为平面点， 粗糙度值较大的点作为边缘点， 则从任一激光雷 达扫描得到的特 征点表示 为 表示平面 点， 表示边缘点。 3.根据权利 要求1所述的针对地面移动 机器人的Lidar ‑IMU融合的slam方法， 其特征在 于， 所述步骤2包括关键帧的选择和子地图的构建； 所述关键帧的选择： 引入了视觉slam中的关键帧概念， 使用两个关键帧之间的激光雷 达信息来构造局部子地图， 假设地面移动机器人是一个刚体， 相邻激光雷达扫描时刻的机 器人本体坐标系的xy平面的变化可以反映地平面的变化， 当新的激光雷达扫描到来时， 将 当前帧地面法线矢量e3转换到前一 帧坐标系中e ′3i， 则相邻激光雷达扫描的地平面之间的 角度为： 式中 是e3的转置矩阵； 如果θ大于设定的阈值θth， 则认为地平面的斜率发生了变化， 机器人不再与之前的扫描 位于同一 地平面上， 此扫描将被选为关键帧； 所述子地 图的构建： 如果新的Lidar扫描为关键帧， 则创建一个新的子地图， 并将该关 键帧作为子地图的参考帧， 此时机器人的本体坐标系为子地图的坐标系， 用Mi表示， 然后， 构建一个包括激光雷达里程计约束和地面约束的最大后验估计， 优化得到后续的普通雷达 帧相对于 子地图的位姿变换；权　利　要　求　书 1/4 页 2 CN 115355904 A 2当新的激光雷达普通帧Fj到来时， 在预处理后获得其特征点 将其与所在 子地图Mi相匹配， 此时， 本地子地图具有如下激光雷达帧： Mi＝{Li,Fi,0,Fi,1…Fi,j‑1} 式中Li表示子地图Mi的参考关键帧， Fi,k,k＝1,2, …j‑1表示子地图中已有的普通帧， 对 于扫描匹配方法， 使用与loam相同的方法， 首先将此普通 帧的特征点转换为其所在子地图 中得到 初始变换是通过使用来自校正后的IMU的预测获得的， 对于 中的每个特征 点， 在子地图中找到相应的边缘点和平面点， 然后， 最小化特征点到其相应边和平面的距 离， 以获得最 最优的变换： 式中 式中 分别是当前帧 的边缘特征点和面特征点， 是 转换到局部坐 标系后得到 的点， 是局部地图中与 对应的匹配边缘点， 是局部 地图中与 对应的匹配面 点； 子地图中的所有激光雷达帧都位于同一地平面上， 因此， 使用在SE(3)位姿 ‑SE(2)约束 的方法向每个激光雷达普通帧添加平 面约束， 理想情况下， 在子地图中， 机器人在平面中移 动， 将普通帧和子地图之间的变换参数化为SE(2)位姿， 其李代数表示为v＝[v1 v2 φ]， 其 中φ是旋转分量， (v1,v2)并表示平移分量； 在实际环境中， 由于机器人运动中的崎岖地 形和 机器人自身的晃动， 机器人的姿态在SE(2)以外的维度上存在运动干扰， 因此， 不是直接在 SE(2)上参数化机器人姿势， 而是通过李 代数将姿势从SE(2)恢复到SE(3)： ξ ＝[v1 v2 0 0 0 φ]T 同时， 用高斯模型对其他三个维度的运动扰动的误差进行建模， 表示 其他三 维空间中允许的运动扰动振幅， 这是由地形 条件和机器人 结构决定的； 因此， 地平面约束的残差 定义为： 权　利　要　求　书 2/4 页 3 CN 115355904 A 3