(19)国家知识产权局 (12)发明 专利 (10)授权公告 号 (45)授权公告日 (21)申请 号 202210356985.6 (22)申请日 2022.04.07 (65)同一申请的已公布的文献号 申请公布号 CN 114429432 A (43)申请公布日 2022.05.03 (73)专利权人 科大天工智能装备技 术 （天津） 有 限公司 地址 300000 天津市东 丽区华明 高新技术 产业区华丰路6号G座1号楼 (72)发明人 张超　张波　 (51)Int.Cl. G06T 5/00(2006.01) G06T 7/246(2017.01) G06T 7/277(2017.01) G06T 3/40(2006.01) G06K 9/62(2022.01) G01S 17/89(2020.01) G01S 17/86(2020.01) G01C 21/00(2006.01)G06V 10/80(2022.01) (56)对比文件 CN 114120075 A,202 2.03.01 CN 111812649 A,2020.10.23 CN 114013449 A,202 2.02.08 CN 111045017 A,2020.04.21 CN 108052103 A,2018.0 5.18 CN 111583663 A,2020.08.25 CN 111665826 A,2020.09.15 CN 111798505 A,2020.10.20 CN 110084272 A,2019.08.02 CN 110297491 A,2019.10.01 CN 109934871 A,2019.0 6.25 CN 106056664 A,2016.10.26 CN 112435325 A,2021.0 3.02 CN 10909 9901 A,2018.12.28 CN 113379910 A,2021.09.10 CN 113837277 A,2021.12.24 CN 112347840 A,2021.02.09 审查员 邵娜娜 (54)发明名称 一种多源信息分层融合方法、 装置及 存储介 质 (57)摘要 本申请涉及一种多源信息分层融合方法、 装 置及存储介质， 方法包括获取点云数据、 单目图 像数据和位姿信息； 对获取的点 云数据进行预处 理， 以去除点云数据中的噪点并去除运动畸变； 对获取的单目图像数据进行去噪处理并提取单 目图像数据的SIFT特征点； 为单目图像关联点 云 数据的深度值； 将进行关联深度值的单目图像和 预处理后的点云数据进行融合， 得到局部里程 计， 并将局部里程计转换成里程计因子； 对位姿 信息进行预积分处理， 得到预积分因子； 利用因 子图模型优化里程计因子、 预积分因子和回环检 测因子， 以得到无人移动平台的运动轨迹以及地图。 该方法能提高多源信息的利用率， 降低累计 误差。 权利要求书1页 说明书5页 附图4页 CN 114429432 B 2022.06.21 CN 114429432 B 1.一种多源信息分层融合方法， 应用于无人移动平台， 所述无人移动平台设置有单目 相机、 点云扫描装置以及惯性测量单 元， 其特征在于， 包括： 获取所述点云扫描装置得到的点云数据、 所述单目相机得到的单目图像数据和所述惯 性测量单 元得到的所述无 人移动平台的位姿信息， 所述 位姿信息包括加速度和角速度； 对获取的所述 点云数据进行 预处理， 以去除点云数据中的噪点并去除运动畸变； 对获取的所述单目图像数据进行去噪处 理并提取 单目图像数据的SIFT特 征点； 为单目图像关联 所述点云数据的深度值； 将进行关联深度值的单目图像和预处理后的点云数据进行融合， 得到局部里程计， 并 将所述局部里程计转换成里程计因子； 对所述位姿信息进行 预积分处 理， 得到预积分因子； 利用因子 图模型优化所述里程计因子、 所述预积分因子和回环检测 因子， 以得到所述 无人移动平台的运动轨 迹以及地图。 2.根据权利要求1所述的多源信息分层融合方法， 其特 征在于， 还 包括： 确定相邻点云数据间存在多个时刻的位姿信息， 则提取点云数据的关键帧， 所述关键 帧为不同时刻的点云数据； 对任意两个所述关键帧对应的第 一时刻和第 二时刻间的位姿信 息进行预积分处理， 得 到第一时刻相对于第二时刻的相对变化 量。 3.根据权利要求1所述的多源信 息分层融合方法， 其特征在于， 所述为单目图像关联所 述点云数据的深度值， 具体包括： 根据点云帧和视觉帧的时间戳进行数据对齐， 把SIFT特征点和点云点投影到以单目相 机为圆心的单位球上， 对单位球上 的点进行降采样并利用极坐标进行保存， 利用极坐标确 定SIFT特征点周围最近的三个点云点， 单位球圆心和所述SIFT 特征点的连线的长度为所述 SIFT特征点的深度值。 4.根据权利要求1所述的多源信息分层融合方法， 其特征在于， 还包括： 构建因子 图模 型。 5.根据权利要求1所述的多源信 息分层融合方法， 其特征在于， 采用半径滤波器对点云 数据进行噪声 去除， 并采用线性插值的方法去除运动畸变。 6.根据权利要求1所述的多源信 息分层融合方法， 其特征在于， 利用拓展卡尔曼滤波算 法将关联深度值的单目图像和预处 理后的点云数据进行融合。 7.根据权利要求1所述的多源信 息分层融合方法， 其特征在于， 采用中值滤波算法去除 单目图像的噪声。 8.根据权利要求1至7任一所述的多源信息分层融合方法， 其特征在于， 所述点云扫描 装置为激光雷达， 所述惯性测量单 元为惯性传感器。 9.一种多源信息分层融合装置， 其特 征在于， 包括： 存储器， 用于存 储程序指令； 处理器， 用于调用所述存储器 中存储的所述程序指令以实现如权利要求1至8中任一项 所述的多源信息分层融合方法。 10.一种计算机可读存储介质， 其特征在于， 所述计算机可读存储介质存储有程序代 码， 所述程序代码用于实现如权利要求1至8中任一项所述的多源信息分层融合方法。权　利　要　求　书 1/1 页 2 CN 114429432 B 2一种多源信息分层融合 方法、 装置及存储介质 技术领域 [0001]本申请涉及机器人技 术领域， 尤其涉及一种多源信息分层融合方法。 背景技术 [0002]即时定位与地图构建 （Simultaneous  Localization  And Mapping， SLAM） 技术是 移动机器人需要解决 的关键问题之一。 经过几十年的发展， 目前已经取得诸多成果。 在移动 机器人上， 往往搭载多种传感器， 采集数据并进 行计算， 得到自身位置姿态的定位和 场景地 图信息的系统。 [0003]目前最常见的SLAM为激光SLAM算法和视觉SLAM算法。 随着场景任务日益复杂， 单 一的传感器往往难以满足需求， 因此多传感器融合算法就很有必要。 如何充分利用多传感 器的信息仍是目前的难点之一。 发明内容 [0004]为了解决上述背景技术提到的技术问题或者至少部分地解决上述技术问题， 本申 请提供一种多源信息 分层融合方法、 装置及 存储介质， 以提高多源信息的利用率， 降低累计 误差。 [0005]第一方面， 本申请提供了一种多源信息分层融合方法， 应用于无人移动 平台， 所述 无人移动平台设置有单目相机、 点云扫描装置以及惯性测量单 元， 方法包括： [0006]获取所述点云扫描装置得到的点云数据、 所述单目相机得到的单目图像数据和所 述惯性测量单元得到的所述无人移动平台的位姿信息， 所述位姿信息包括加速度和角速 度； [0007]对获取的所述 点云数据进行 预处理， 以去除点云数据中的噪点并去除运动畸变； [0008]对获取的所述单目图像数据进行去噪处 理并提取 单目图像数据的SIFT特 征点； [0009]为单目图像关联 所述点云数据的深度值； [0010]将进行关联深度值的单目图像和预处理后的点云数据进行融合， 得到局部里程 计， 并将所述局部里程计转换成里程计因子； [0011]对所述位姿信息进行 预积分处 理， 得到预积分因子； [0012]利用因子图模型优化所述里程计因子、 所述预积分因子和回环检测因子， 以得到 所述无人移动平台的运动轨 迹以及地图。 [0013]在此方案中， 回环检测用于判断载体是否再次经过曾经到过的位置， 目的在于增 加历史帧约束， 增加数据丰富度。 在载体运行过程中， 会将扫描帧不断储存构成历史帧集， 当前帧会和历史帧集中的所有激光帧进行匹配 （即当前帧的所有激光点和历史帧的所有激 光点计算物理距离残差， 转换成最小二乘问题， 通过迭代求解得到最小残差和） ， 若当前帧 和历史帧最小残差和小于某个阈值时， 则认定产生回环， 将该残差和视为回环检测因子， 进 行下一步联合优化。 [0014]在此方案中， 可以采用预设的回环检测算法进行回环检测， 得到回环检测因子， 回说　明　书 1/5 页 3 CN 114429432 B 3