(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210339011.7 (22)申请日 2022.04.01 (71)申请人 东南大学 地址 210096 江苏省南京市玄武区四牌楼 2 号 (72)发明人 王庆　陈晓宇　严超　王怀虎　 杨锐　 (74)专利代理 机构 南京众联专利代理有限公司 32206 专利代理师 蒋昱 (51)Int.Cl. G06T 17/05(2011.01) G06T 7/80(2017.01) G06K 9/62(2022.01) G01S 17/89(2020.01)G01S 17/42(2006.01) G06V 10/762(2022.01) G06V 10/44(2022.01) (54)发明名称 基于四足机器人的强度信息匹配快速激光 SLAM算法 (57)摘要 基于四足机器人的强度信息匹配快速激光 SLAM算法。 在提取特征点方面加入了强度信息， 使得特征点的提取在两帧之间更容易区分相同 特征， 提高了提取特征值及特征值匹配的精度， 同时在帧图匹配过程， 抛弃了采用迭代方法进行 帧图匹配采用畸变二次补偿的方法进行建图， 提 高了计算效率使得在四足机器人平台上能够更 好的实现实时建图。 本发明除了利用几何信息 之 外， 提出将 激光的强度作为特征匹配中的重要信 息， 强度信息中包含环境结构 的反射率， 该环境 结构显示了不同对象 的分布， 因此 强度信息有助 于识别多个帧中的相同特征； 同时将算法运动估 计模块做了改进， 在得到位姿估计后， 再次计算 畸变， 计算后未畸变的特征更新到地图中， 提高 相同地点的定位精度。 权利要求书5页 说明书10页 附图1页 CN 114782640 A 2022.07.22 CN 114782640 A 1.基于四足机器人的强度信息匹配快速 激光SLAM算法， 包括， 其特 征在于： 多线激光雷达扫描区域后， 生成点云数据后将点云数据作为输入项， 利用点云数据进 行构建三维地图， 实现实时定位； 利用所述的基于强度信息的激光SLAM算法， 其中包括以下步骤： S1.对点云数据滤除噪声并区分地 面点和非地 面点； 所述的步骤S1中， 包括以下步骤： S11.采用BFS聚类方法滤除点云数据噪声； S12.对点云进行分割； S13.对点云数据进行降维处 理； S14.对降维处 理后的点云数据拟合 直线 S15.筛选地面点和非地 面点； S2.强度标定及点云数据进行提取 特征点； 所述的步骤S2中， 包括以下步骤： S21.计算 点云的强度信息； S22.提取平面 点和边缘点； S3.运动估计和畸变补偿； 所述的步骤S3中， 包括以下步骤： S31.对姿态变换矩阵进行求解， 激光雷达每旋转一周为一帧， 构建帧与帧之间的约束 关系； S32.校正 点云； S4.位姿估计； 所述的步骤S4中， 包括以下步骤： S41.构建点到边及点到面的关系； S42.计算 点到边及点到面的最小权 重及位姿估计； S5.畸变第二次补偿及建立全局地图； 所述的步骤S5中， 包括以下步骤： S51.对优化后的姿态变换矩阵重新计算； S52.将未畸变的点云更新到由每帧点云形成的地图中。 2.根据权利要求1所述的基于四足机器人的强度信息匹配快速激光SLAM算法， 其特征 在于： 所述的步骤S1具体如下； 首先， 对激光雷达获取的点云数据进行聚类分析， 采用BFS聚类方法， 剔除少于30个点 的聚类， 进 而滤除噪声点云； 之后， 将滤除噪声后的点云数据投影到平面x ‑y坐标系上， 将x ‑y平面视为被Δα划分为 M个扇形的圆面， 因此将平面分割为M块： 为了识别将投影到x ‑y平面的点定位到哪一 块， 采用下式进行处 理：权　利　要　求　书 1/5 页 2 CN 114782640 A 2其中， pi是一个点， 其 坐标为xi， yi， zi， 确定每个点在x‑y平面的分割块； 之后， 对投影后的坐标进行降维处理， 由于投影后损失了垂直方向的信息， 因此将三维 坐标点变为二维坐标点， 即(x， y， z)＝(d， z)， 其中 降维后的坐标既保留了垂 直方向信息又不损失平面信息； 在得到降维后的点云数据后， 构建d ‑z平面离散点， 针对这些离散点在每一个分割块中 采用y＝mx+b进行线性拟合， 拟合的直线均方根误差应小于阈值TRMSE,通过设定两个阈值 来区分地 面点： 进行地面拟合斜率要保证在阈值内， 不在阈值内将会呈现垂直结构， 同时当斜率m趋近 于0时， 截距b的绝对值也不应超过阈值Tb， 斜率很小时说明地面平整， 截距代表了该点距离 地面的高度， 设定阈值Tb， 就能保证不会把高出地面的点云认为是地面， 最后根据拟合数据 进行区分地 面点和非地 面点。 3.根据权利要求1所述的基于四足机器人的强度信息匹配快速激光SLAM算法， 其特征 在于： 所述的步骤S2具体如下； 对接收的强度信息进行 标定， 标定结果应用于特 征点提取； 1)强度标定； 强度值为反射信号接收的能量与发射的激光功率之间的比率， 其中， 接收功率Pr为： Pe是发射功率， Dr是接收器的孔径， ηsys是系统传递系数， ηatm是大气传递系数， α 是物体 表面与激光束的入射角， ρ 是物体的材料反射率， R是测量距离， 入射角α 是非本征参数， ηsys 和 ηatm是常量参数； 强度测量 I则为： ηall是一个常数， 所以有： 激光扫描测量距离， 因此， 分析局部法线来估计入射角， 找两个最近点p1和p2来计算法 线， 如下式所示： 入射角则为：权　利　要　求　书 2/5 页 3 CN 114782640 A 3