(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 20221080712 9.8 (22)申请日 2022.07.12 (71)申请人 烟台艾睿光电科技有限公司 地址 264000 山东省烟台市开发区贵阳 大 街11号 (72)发明人 王承端　王水根　辛润超　 (74)专利代理 机构 烟台上禾知识产权代理事务 所(普通合伙) 37234 专利代理师 苏红红 (51)Int.Cl. G06T 17/05(2011.01) G06T 15/06(2011.01) G06T 3/00(2006.01) G06T 7/593(2017.01) G06T 7/73(2017.01)G06T 7/33(2017.01) G06V 10/762(2022.01) G06V 10/75(2022.01) (54)发明名称 一种同时定位建图的方法、 装置、 系统及存 储介质 (57)摘要 本发明公开了一种同时定位建图方法、 装 置、 系统， 方法包括以下步骤： 根据激光雷达发送 点云数据的频率对IMU传感器接收到的IMU数据 进行预处理， 得到IMU预积分数据； 将激光雷达接 收到的三维的点云数据转换为二维的深度图像； 利用IMU预积分数据与深度图像， 获得相邻两帧 点云数据之间目标的移动位姿信息； 以目标的移 动位姿信息为初始值， 将当前帧的点云数据与三 角网格地图进行匹配， 得到目标在所述三角网格 地图中的当前位姿信息， 完成定位； 基于当前帧 的点云数据和目标的当前位姿信息， 利用三维点 云重建技术生成与 目标的位置周围预设范围对 应的三角形网格， 并更新到三角网格地图中， 完 成建图。 权利要求书2页 说明书10页 附图3页 CN 115131514 A 2022.09.30 CN 115131514 A 1.一种同时定位建图方法， 其特 征在于， 包括以下步骤： 步骤1.根据激光雷达发送点云数据的频率对IMU传感器接收到的IMU数据进行预处理， 得到IMU预积分数据； 其中， 激光雷达和IMU传感器均安装在目标 上； 步骤2.将激光雷达 接收到的三维的点云数据转换为 二维的深度图像； 步骤3.利用所述IMU预积分数据与所述深度图像， 获得相邻两帧点云数据之间目标的 移动位姿信息； 步骤4.以所述目标的移动位姿信 息为初始值， 将当前帧的点云数据与三角网格地图进 行匹配， 得到目标在所述 三角网格地图中的当前位姿信息， 完成定位； 步骤5.基于当前帧的点云数据和目标的当前位姿信息， 利用三维点云重建技术生成与 所述目标的当前位置周围预设范围对应的三角形网格， 并更新到所述三角网格地图中， 完 成建图。 2.根据权利要求1所述的一种同时定位建图方法， 其特征在于， 所述步骤5之后， 还包括 步骤6： 采用多约束因子图算法， 将所述IMU预积分数据和所述点云数据作为因子， 添加至因 子图中， 并执行因子图优化， 以对所述 目标的当前位姿信息和更新后的所述三角网格地图 进行全局优化。 3.根据权利要求2所述的一种同时定位建图方法， 其特征在于， 所述步骤6之后， 还包括 步骤7： 将 每一帧点云数据中目标的当前位姿信息与更新后的所述三角网格地图对应存储， 并生成贝叶斯 树。 4.根据权利要求1所述的一种同时定位建图方法， 其特 征在于， 所述 步骤3包括： 对所述深度图像中的每帧点云数据进行聚类分割处 理； 从聚类分割处 理后的每帧点云数据中提取 特征点信息； 以所述IMU预积分数据为初始值， 将从相邻两帧点云数据中提取的特征点信息进行匹 配， 获得相邻两帧点云数据之间目标的移动位姿信息 。 5.根据权利要求4所述的一种同时定位建图方法， 其特征在于， 所述步骤3中对所述深 度图像中的每帧点云数据进行聚类分割处 理的步骤 包括： 对于任意两个相邻的点云数据， 计算对应两个相邻 激光点构 成的直线与激光雷达和其 中一个激光 点构成的连线之间的夹角 β； 若β 值大于设定的角度阈值， 则认为相邻的两个点云数据处于同一物体上， 否则认为是 处于两个物体； 扫描点云数据中所有相邻点云数据， 将点云数据进行聚类分割， 对于提取物体， 若打在 该物体上的点云数据的数目小于设定的阈值， 则认为该物体为噪声， 将其剔除。 6.根据权利要求5所述的一种同时定位建图方法， 其特征在于， 所述步骤3中从聚类分 割处理后的每帧点云数据中提取 特征点信息的步骤 包括： 从聚类分割处理后的每帧点云数据中提取特征点， 并根据 各点云数据在三维空间中的 曲度， 将聚类分割处 理后每帧点云数据中的特 征点分为 点特征点和面特 征点； 其中， 对于任一特征点， 当对应点云数据的曲度 大于等于预设的平滑度阈值 时， 将该特 征点作为点特征点； 当对应点云数据的曲度小于预设的平滑度阈值时， 将该特征点作为面 特征点； 所述 点特征点和面特 征点构成了聚类分割处 理后每帧点云数据中的特 征点信息 。 7.根据权利要求6所述的一种同时定位建图方法， 其特征在于， 所述步骤3中相邻两帧权　利　要　求　书 1/2 页 2 CN 115131514 A 2点云数据中提取的特 征点信息进行匹配步骤 包括： 采用ICP算法或NDT算法或RANSAC方法。 8.根据权利要求1所述的一种同时定位建图方法， 其特征在于， 所述步骤4中将当前帧 的点云数据与三角网格地图进 行匹配， 得到目标在所述三角网格地图中的当前位姿信息步 骤包括： 根据所述目标的当前位置从三角网格地图中按照距离约束取出预设范围内的三角形 网格， 并将所述目标 的当前位置转换为地图坐标系 下， 以利用所述地图坐标系计算出所述 激光雷达发射出的激光的方向； 计算所述激光雷达发射出的激光是否与 取出的三角形网格相交， 如果相交则记录下对 应的交点 位置， 构成与所述 三角网格地图匹配的约束点云数据； 对所述约束点云数据进行匹配， 得到所述目标在所述三角网格地图中的当前位姿信 息。 9.一种同时定位建图装置， 其特征在于， 包括处理器以及存储器， 所述存储器上存储有 计算机程序， 所述计算机程序被所述处理器执行时， 实现如权利要求 1‑8任一所述的同时定 位建图方法。 10.一种同时定位建图系统， 其特征在于， 包括如权利要求9所述的同时定位建图装置， 还包括激光雷达、 IMU传感器； 所述同时定位建图装置、 激光雷达、 IMU传感器均安装于目标上， 所述激光雷达、 IMU传 感器分别与所述同时定位建图装置电连接； 所述激光雷达用于采集三维的点云数据， 并将所述点云数据发送至所述同时定位建图 装置； 所述IMU传感器用于采集目标的IMU数据， 并将所述IMU数据发送至所述同时定位建图 装置。 11.一种计算机存储介质， 其特征在于， 其上存储有计算机程序， 所述计算机该程序被 处理器执行时， 实现如权利要求1 ‑8任一所述的一种同时定位建图方法。权　利　要　求　书 2/2 页 3 CN 115131514 A 3