(19)中华 人民共和国 国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202111648485.1 (22)申请日 2021.12.2 9 (71)申请人 北京航空航天大 学 地址 100191 北京市海淀区学院路37号 (72)发明人 崔林艳　郭政航　 (74)专利代理 机构 北京科迪生专利代理有限责 任公司 1 1251 代理人 安丽　邓治平 (51)Int.Cl. G06T 7/73(2017.01) G06V 10/26(2022.01) G06V 10/762(2022.01) G06K 9/62(2022.01) (54)发明名称 一种面向动态场景的点线融合语义SLAM方 法 (57)摘要 本发明涉及一种面向动态场景的点线融合 语义SLAM方法， 包括以下步骤： (1)将RGBD相机采 集到的RGB图像， 通过语义分割网络分割出场景 中的动态物体； 同时对采集得到的深度图像进行 聚类， 得到当前图像的语义分割网络分割结果和 深度图聚类结果； (2)对步骤(1)中获取的RGB图 像同时提取点特征和线特征， 对 特征点进行完整 保留不做处理， 同时对线特征进行预处理， 得到 点特征和预处理后的线特征； (3)结合步骤(1)中 获得到的语义分割网络分割结果和深度图聚类 结果， 对步骤(2)中获取的点特征和预处理后的 线特征进行筛选， 将位于动态物体上的点、 线特 征进行剔除， 得到静态点、 线特征； (4)通过步骤 (3)中得到的静态点、 线特征在动态场景下， 进行 比经典方法更稳定、 精确的相机位姿 估计。 权利要求书3页 说明书6页 附图1页 CN 114283199 A 2022.04.05 CN 114283199 A 1.一种面向动态场景的点线融合语义SLAM方法， 其特 征在于， 包括以下步骤： (1)将RGBD相机采集到的RGB图像， 通过语义分割网络分割出场景中的动态物体； 同时 对采集得到的深度图像进 行聚类， 得到 当前图像的语义分割网络分割结果和深度图聚类结 果； (2)对步骤(1)中获取的RGB图像 同时提取点特征和线特征， 对特征点进行完整保留不 做处理， 同时对线特 征进行预处理， 得到点特 征和预处 理后的线特 征； (3)结合步骤(1)中获得到的语义分割网络分割结果和深度图聚类结果， 对步骤(2)中 获取的点特征和预 处理后的线 特征进行筛选， 将位于动态物体上的点、 线 特征进行剔除， 得 到静态点、 线特 征； (4)通过步骤(3)中得到的静态点、 线特征在动态场景下， 进行比经典方法更稳定和精 确的相机位姿估计。 2.根据权利要求1所述的面向动态场景的点线融合语义SLAM方法， 其特征在于： 所述步 骤(1)中， 将RGBD相机采集到的RGB图像， 通过语义分割网络分割出场景中的动态物体， 同时 对采集得到的深度图像进行聚类， 方法如下： 针对RGBD相机同时获取得到的RGB图像和深度图像， 同时开启两个线程处理， 对于RGB 图像， 使用SegNet语义分割网络进行语义分割， 得到像素级别的语义分割结果； 针对RGBD相机采集到 的深度图， 使用K ‑Means算法将得到 的深度图聚类成N个类别。 在 深度图聚类过程中， 深度图像中深度值相 近的区域会被归为同一类别， 记录下深度图中每 一类别的像素位置信息和对应的深度值， 得到深度聚类结果。 3.根据权利要求1所述的面向动态场景的点线融合语义SLAM方法， 其特征在于： 所述步 骤(2)具体实现如下： 对于步骤(1)中获得的RGB图像， 同时提取点特征和线特征， 点特征采用ORB特征， 线特 征采用LSD线 特征， 利用线 段的方向、 线到线距离和端点到端点的距离三个参数来合并近线 和消除短线， 当两条线段端点之间的最小距离和一条线段中点到另一条线段的垂直距离均 小于设定阈值5个像素则认为两个线段合并， 得到一条附近没有近距离干扰线段的线段特 征； 其次， 采用一种稳定快速的线特征筛选方法， 首先计算被检测线段与非零深度值相交 的像素的数量， 然后采用均匀采样方法在直线上进 行采样， 统计采样点处的像素坐标， 最后 参照采样点的像素坐标， 在深度图的对应位置上读取采样点的深度值， 当采样点深度大于 零， 则认为采样点有效， 否则无效， 设定若线段上的采样点无效比例超过三分之二， 则该线 段为无效线 段， 将会被剔除， 对于无效采样点少于二分之一的线 特征， 优先选择在无效采样 点处断开； 至此完成对点特征和线特征的提取和预处理， 将提取得到的特征点和预处理后的特征 线分别保存。 4.根据权利要求1所述的面向动态场景的点线融合语义SLAM方法， 其特征在于： 所述步 骤(3)具体实现如下： 利用步骤(1)中获得到的语义分割网络分割结果， 筛选出位于语义标签为 “人”区域的 特征点， 并称其为初始动态点。 将初始动态点， 按照每个特征点的像素坐标， 将特征点映射 到对应深度聚类图中相 应的像素坐标位置上， 如果初始动态点落在某一深度聚类区域内，权　利　要　求　书 1/3 页 2 CN 114283199 A 2将该深度聚类区域标记为准动态区域， 计算位于同一准动态区域内所有 特征点的重投影误 差， 并求平均值， 如果大于四个 像素， 则将该准动态区域称为动态区域； 利用动态区域在图像中像素位置的分布， 对步骤(2)中保留的特征点和预处理后的特 征线进行筛选， 将位于动态 区域的特征点剔除， 检验线段特征的端点和中点检验是否位于 动态区域， 如果直线的两个端点和 一个中点有任意一个位于动态 区域， 则该线特征为动态 线特征， 将动态线特 征剔除， 最终得到静态点、 线特 征。 5.根据权利要求1所述的面向动态场景的点线融合语义SLAM方法， 其特征在于： 所述步 骤(4)具体实现如下： 在得到静态点、 线特征之后， 同步开启三个线程： 跟踪线程、 局部建图线程和回环检测 线程， 其中， 跟踪线程通过跟踪局部地图中的点、 线特征获得相机的位姿， 局部建图线程通 过新建或者删除路标点和路标线， 并结合跟踪线程获得的多个相机位姿以及回环检测的信 息， 对相机位姿进 行统一的优化， 得到更精确的相机位姿， 回环检测线程是通过检测相机是 否到达过先前位置， 为优化提供参考， 在跟踪线程和局部建图线程中进行改进创新， 具体 为： 通过深度图将二维点特征和线特征映射到三维空间建立初始化地图， 通过最小化点、 线特征的重投影误差求 解相机位姿， 首 先将空间直线重投影到图像上， 计算投影直线I ′c： 其中n∈R3是由直线和相机光心构成的平面π 的法向量， l1， l2， l3为直线系数， K为投影矩 阵， fx， fy， cx， cy为相机内参； 然后计算投影直线I ′c和图像中的匹配线段Ic的误差， 将线特征的重投影误差el表示 为： 其中pstart和pend分别为图像中线段 特征的两个端点； 点特征的重投影误差epoint表示为： epoint＝z‑h(T,Ppoint) 其中z代表了三维观测点Ppoint在图像中的实际位置， h(T,Ppoint)代表了三维空间点 Ppoint通过世界坐标系到像素坐标系的转换矩阵T得到的像素坐标； 假设在第i帧图像中有j个静态特征点和k个静态线特征， 同时假定点特征和线特征的 观测误差服从高斯分布， 得到最后的代价 函数C为： 其中 与 分别表示点、 线的观测协方差， ρp(·)和ρl(·)为鲁棒核函数， 用来降低 代价函数中的异常项：权　利　要　求　书 2/3 页 3 CN 114283199 A 3