(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202211059723.X (22)申请日 2022.08.30 (71)申请人 华南理工大 学 地址 510640 广东省广州市天河区五山路 381号 (72)发明人 田联房　刘海林　杜启亮　 (74)专利代理 机构 广州市华学知识产权代理有 限公司 4 4245 专利代理师 冯炳辉 (51)Int.Cl. G06V 20/10(2022.01) G06V 10/44(2022.01) G06V 10/75(2022.01) G06T 7/246(2017.01) G06T 7/73(2017.01) (54)发明名称 一种面向动态 环境的单目视 觉SLAM方法 (57)摘要 本发明公开了一种面向动态环境的单目视 觉SLAM方法， 包 括： 1)初始化， 包括读取图像序列 的前两帧、 提取并匹配ORB特征点、 建立世界坐标 系、 设置单目尺度信息、 建立初始地图、 构建关键 帧序列以及关键帧滑动窗口； 2)追踪参考帧并估 计初始位姿； 3)剔除动态特征点并优化位姿； 4) 根据追踪结果， 插入关键帧并追踪参考关键帧， 然后构造地图点并插入地图； 5)剔除滑动窗口上 动态地图点， 并执行局部光束平差优化； 6)剔除 冗余关键帧和地图点； 7)若设备算力充足， 执行 全局光束平差优化； 8)重复步骤2) ‑7)， 直至序列 中图像帧全部处理完。 本发明解决了视觉SLAM方 法应用于动态环境中定位与建图存在鲁棒性弱 及精度差的问题。 权利要求书4页 说明书9页 附图1页 CN 115471748 A 2022.12.13 CN 115471748 A 1.一种面向动态 环境的单目视 觉SLAM方法， 其特 征在于， 包括以下步骤： 1)初始化， 包括读取图像序列的前两帧、 提取并匹配ORB 特征点、 建立世界坐标系、 设置 单目尺度信息、 建立初始地图、 构建 关键帧序列以及关键帧滑动窗口； 2)读取图像序列作为当前帧， 将上一帧设为当前帧的参考帧， 完成当前帧与参考帧的 特征匹配并估计相对位姿 Tk‑1,k， 其中， 当前帧为第k帧， 参 考帧为第k ‑1帧； 3)根据步骤2)中的相对位姿， 比较当前帧与参考帧匹配特征点对的几何信息一致性和 光度信息一致性， 剔除不一致的匹配特征点对以及对应的地图点， 并根据剩余的匹配特征 点对， 通过最小化重投影误差函数优化当前帧与参 考帧的相对位姿； 4)若步骤3)中剩余的匹配特征点对数小于设置的阈值Tm， 或者当前帧与当前关键帧间 隔帧数超过阈值Tn， 则根据当前帧构造新的关键帧； 5)若步骤4)构造了新的关键帧， 则将新的关键帧插入关键帧序列和滑动窗口， 将新的 关键帧设为当前关键帧， 将上一关键帧设为当前关键帧的参考关键帧； 然后根据步骤3)优 化后的相机位姿， 将当前关键帧与参考关键帧进 行特征点匹配， 其中， 对于未初始 化地图点 的匹配特 征点对， 则通过三角化方法构造新的地图点并插 入地图M； 6)根据几何信息一致性， 识别并剔除滑动窗口上的动态地图点， 然后在滑动窗上进行 局部光束平差优化； 7)若滑动窗口中关键帧个 数达到最大值TK， 并且当前关键帧位姿Tn,w与参考关键帧位姿 Tn‑1,w满足 则剔除滑动窗口中的参考关键帧， 若 则剔除滑动窗 口上时间最早 的关键帧， 其中， 运算符|| ·||表示取矩阵范数， δt为设置的阈值； 随后遍历 全部关键帧及地图点， 若关键帧所观测到地图点的90％以上都被至少三个其它关键帧所观 测， 则剔除该关键帧， 若地图点被观测次数少于三次， 则剔除该地图点； 8)完成步骤7)后， 新开启一条线程进行一次全局光束平差优化； 重 复步骤2)至步骤8)， 直到序列中的图像全部处 理完。 2.根据权利要求1所述的一种面向动态环境的单目视觉SLAM方法， 其特征在于， 在步骤 1)中， 将读取的RGB图像转化为灰度图， 将灰度图连续降采样构造图像金字塔， 根据每层图 像分辨率大小提取相应数量的FAST角点， 并计算全部角点的BRIEF描述子， 根据描述子 之间 的汉明距离， 在两帧之间进行特征点匹配； 随后将世界坐标系设为第一帧的位姿， 并通过随 机采样一 致性计算第二帧的位姿， 具体包括以下步骤： 1.1)随机 选取8对匹配的特 征点， 利用八点法计算本质矩阵E12； 1.2)根据步骤1.1)的结果， 检验全部匹配特征点对(qi1,qi2)的对极约束偏差 若ed＞Td， 则将匹配特征点对标记为离群值， 否则标记为内围值， 其 中， qi1和qi2分别是第一帧和第二帧特 征点的齐次坐标， K 是相机内参矩阵， Td是设置的阈值； 1.3)根据 步骤1.2)的结果， 若内围值匹配特征点对数占总匹配特征点对数比例超过阈 值Tr， 0.7＜Tr≤1， 则对步骤1.1)的本质矩阵进行奇异值分解， 并得到第一帧与第 二帧的相 对位姿T12， 其中， 令位姿中的平移向量为单位向量， 即确定了尺度信息； 若内围值比例低于 Tr， 则重复步骤1.1)至1.3)直到内围值比例不低于Tr； 若全部组合选取完， 仍未找到满足内 围值比例大于Tr的组合， 则选择内围值比例最大的组合对应的本质矩阵进行奇异值分解， 并得到T12；权　利　要　求　书 1/4 页 2 CN 115471748 A 21.4)根据步骤1.3)的结果， 将全部内围值进行三角化计算得到三维地 图点， 并构建初 始地图M； 根据第一帧构造关键帧KF1， 插入到关键帧序列和滑动窗口， 将该关键帧设为当前 关键帧。 3.根据权利要求2所述的一种面向动态环境的单目视觉SLAM方法， 其特征在于， 在步骤 2)中， 首先将读取的RGB图转换为灰度图， 连续降采样构造图像金字塔， 根据每层图像的分 辨率大小提取相应数量的FAST角点， 并计算全部角点的BRIEF描述子； 然后将提取的每个特 征点通过BRIEF描述子在 参考帧中寻找最佳匹配， 若最佳匹配的汉明距离小于阈值To， 则保 留该特征点匹配， 否则舍弃该特征点匹配； 最后根据随机采样一致性策略， 利用Epnp算法计 算当前帧与参 考帧之间的相对位姿 Tk‑1,k， 具体包括以下步骤： 2.1)随机 选取4对匹配特 征点， 通过Epnp算法求 解出位姿 Tk‑1,k； 2.2)计算当前帧特 征点重投影误差ei： 式中， i表示地图点的索引号， N表示三维地图点数， Pik‑1为参考帧坐标系地图点， 为 当前帧特 征点， π(·)为去齐次运 算； 2.3)若||ei||＞Tl， 则该匹配特征点对标记为离群值， 否则标记为内围值； 计算内围值 的总占比， 若大于Tn， 则保留步骤2.1)的位姿， 否则， 重复步骤2.1)至2.3)直至内围值总占 比大于Tn； 若选取完全部组合， 仍无法满足条件， 则选择 内围值比例最高的位姿作为估计位 姿， 其中， Tl和Tn是设置的阈值。 4.根据权利要求3所述的一种面向动态环境的单目视觉SLAM方法， 其特征在于， 在步骤 3)中， 为了消除快速运动物体上特征点的影响， 通过光度信息一致性和几何信息一致性进 行动态特 征点识别与剔除， 并优化当前帧与参 考帧的相对位姿， 具体包括以下步骤： 3.1)根据步骤2)中标记为内围值的特征点对， 计算特征点附近图像块的像素值， 若特 征点对的区域像素值差值大于阈值Tg， 则将相应的匹配特征点对标记为动态特征点 并移除 该匹配特 征点对及其对应的地图点； 3.2)对于经过步骤3.1)检验并具有三维地图点的特征点对， 将对应的地图点投影到当 前帧和参考帧中并构 造重投影误差函数， 最后通过最小化重投影误差函数来迭代优化地图 点的空间坐标， 具体表达式如下： 上式通过高斯牛顿法迭代求解； 在优化过程中， 若偏差满足 则 将Plk‑1标记为动态地图点， 进而移除该地图点以及相应匹配特征点对， 式中， l表示检验之 后地图点的索引号， l＝1,2,...,N ′， arg min(·)表示函数取最小值时的参数取值， π( ·) 表示去齐次变换， Tk‑1,k是当前帧与参考帧的相 对位姿， 和 分别是参考帧和当前帧的 特征点， Plk是第k帧坐标系地图点， N ′是检验之后剩余的地图点个数， ΔPlk‑1表示优化前后 地图点Plk‑1位置的变化 量， Ωl为地图点Plk‑1的协方差矩阵， Th为设置的阈值， K 是相机内参； 3.3)根据步骤3.2)过滤后的匹配特征点对， 通过求解重投影误差函数优化相对位姿， 具体表示 为：权　利　要　求　书 2/4 页 3 CN 115471748 A 3