(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210963410.0 (22)申请日 2022.08.11 (71)申请人 七腾机器人有限公司 地址 401123 重庆市渝北区卉竹路2号7幢 21-1号 (72)发明人 朱冬　杨易　方向明　张建　宋雯　 唐国梅　 (74)专利代理 机构 重庆西南 华渝专利代理有限 公司 50270 专利代理师 陈香兰 (51)Int.Cl. G01C 21/20(2006.01) G01C 21/32(2006.01) G06T 7/246(2017.01) G06T 7/73(2017.01) (54)发明名称 基于语义和多物体误差最小动态SLAM方法 及机器人 (57)摘要 本发明提供了一种基于语义和多物体误差 最小动态SLA M方法及机器 人， SLAM方法包括： S1， 获取参考帧RGB图像和当前帧RGB图像； S2， 获取 参考帧静态图像和当前帧静态图像； S3， 获取每 个物体区域对应的相机位姿估计矩阵； S4， 对于 每个相机位姿估计矩阵， 求取其他物体区域基于 该相机位姿估计矩阵的重投影误差， 融合所有物 体区域的重投影误差获得该相机位姿估计矩 阵 的综合投影误差； S5， 选取综合投影误差最小的 相机位姿估计矩阵为帧间最优相机姿态矩阵。 获 取每个相机位姿估计矩 阵在其他物体区域的重 投影误差， 选择综合投影误差最小的相机位姿估 计矩阵作为帧间最优相机姿态矩阵， 提高相机位 姿估计精度和建图的准确性。 权利要求书3页 说明书8页 附图2页 CN 115326073 A 2022.11.11 CN 115326073 A 1.基于语义和多物体误差最小动态SLAM方法， 其特征在于， 获取连续RGB图像帧， 对于 相邻两帧RGB图像执行步骤S1到步骤S 5获得帧间最优相机姿态矩阵， 基于所有相邻两帧RGB 图像的帧间最优相机姿态 矩阵获得相机运动轨 迹， 其中： 步骤S1， 获取参 考帧RGB图像和当前帧RGB图像； 步骤S2， 基于参考帧RGB图像获取参考帧静态图像， 基于当前帧RGB图像获取当前帧静 态图像， 所述 参考帧静态图像/当前帧静态图像中不包括先验的动态的物体区域； 步骤S3， 获取参 考帧静态图像中每 个物体区域对应的相机位姿估计矩阵； 步骤S4， 对于每个相机位姿估计矩阵， 求取参考帧静态图像中除该相机位姿估计矩阵 对应的物体区域之外的物体区域基于该相机位姿估计矩阵在当前帧RGB图像上的重投影误 差， 融合参考帧静态图像中除该相机位姿估计矩阵对应的物体区域之外的所有物体区域的 重投影误差获得 该相机位姿估计矩阵的综合投影误差； 步骤S5， 选取综合投影误差最小的相机位姿估计矩阵作为帧间最优相机姿态 矩阵。 2.如权利要求1所述的基于语义和多物体误差最小动态SLAM方法， 其特征在于， 在步骤 S2中， 获取参 考帧静态图像/当前帧静态图像的具体过程包括： 将参考帧RGB图像/当前帧RGB图像输入训练好的实例分割网络获得物体表面掩膜； 利用物体表面掩膜去除参考帧RGB图像/当前帧RGB图像 中的先验动态物体的像素点获 得参考帧静态图像/当前帧静态图像。 3.如权利要求1或2所述的基于语义和多物体误差最小动态SLAM方法， 其特征在于， 在 步骤S3中， 在参考帧静态图像和当前帧静态图像中均将各自的背景区域作为一个物体区 域。 4.如权利要求3所述的基于语义和多物体误差最小动态SLAM方法， 其特征在于， 在所述 步骤S3中， 获取参 考帧静态图像的第j个物体区域对应的相机位姿估计矩阵具体包括： 步骤S31， 从第j个物体区域中选取n个特征点并获取n个特征点在当前帧静态图像中对 应的n个匹配特 征点； 所述 n为正整数； j为物体区域索引， j为 正整数； 步骤S32， 获取参 考帧静态图像的第j个物体区域中n个特 征点的空间三维坐标； 步骤S33， 建立目标函数： 其中， xi'表示参考帧静态图像的第j个物体区域中第i个特征点在当前帧静态图像中对 应的匹配特点i'的二维坐标； R表示旋转矩阵， t表示平移矩阵； Xi表示第i个特征点 的空间 三维坐标 ； π( ·) 表示向当前帧静态图 像投影的投影函数 ； 表示求取 达到最小值时的相机位姿估计矩阵T， 将此时的相机位姿估计矩阵作 为最优相机位姿估计矩阵T*， R*表示最优旋转矩阵， t*表示最优平移矩阵； 步骤S34， 求解目标函数获得最优相机位姿估计矩阵T*作为参考帧静态图像的第j个物 体区域对应的相机位姿估计矩阵。 5.如权利要求4所述的基于语义和多物体误差最小动态SLAM方法， 其特征在于， 当用于权　利　要　求　书 1/3 页 2 CN 115326073 A 2SLAM的视觉设备为单目相机时， 获取参考帧静态图像中特征点的空间三维坐标的过程包 括： 在参考帧静态图像中遍历所有物体区域更新参考帧静态图像， 具体为： 判断每个物体 区域在当前帧静态图像中是否存在与该物体区域具有至少8对匹配特征点对的匹配物体区 域， 若存在， 则保留该物体区域， 若不存在， 则舍弃 该物体区域； 基于更新后的参考帧静态图像中每个物体区域的匹配特征点对获取该物体区域的帧 间基础矩阵； 基于每个物体区域的帧间基础矩阵获取该物体区域在匹配物体区域的所有匹配特征 点到极线L2的极线距离总和； 将极线距离总和最小的物体区域的帧间基础矩阵作为初始化帧间基础矩阵； 基于初始化帧间基础矩阵获取参 考帧静态图像中特 征点的空间三维坐标。 6.如权利 要求1或2或4或5所述的基于语义和多物体误差最小动态SLAM方法， 其特征在 于， 在所述步骤S4中， 所述求取参考帧静态图像中除该相 机位姿估计矩阵对应的物体区域 之外的物体区域基于该相机位姿估计矩阵在当前帧RGB图像上的重投影误差的步骤 包括： 设求取第j个物体区域基于相机位姿估计矩阵 在当前帧RGB图像上的重投影误差， j' ≠j， 设第j个物体区域包括n个特征点， 基于相机位姿估计矩阵 分别获取n个特征点在当 前帧RGB图像中的估计匹配特 征点坐标； 将估计匹配特征点坐标和实际匹配特征点坐标分别在归一化平面投影获得归一化估 计坐标和归一化实际坐标， 求取匹配特征点的归一化估计坐标与归一化实际坐标的距离， 将所述距离记为匹配特 征点的误差距离； 求取所有匹配特征点的误差距离的平均值， 所述平均值作为第j个物体区域基于相机 位姿估计矩阵 在当前帧RGB图像上的重投影误差 。 7.如权利要求6所述的基于语义和多物体误差最小动态SLAM方法， 其特征在于， 在步骤 S4中， 所述融合参考帧静态图像中除该相机位姿估计矩阵对应的物体区域之外的所有物体 区域的重投影误差获得该相机位姿估计矩阵的综合投影误差步骤中， 所述综合投影误差为 参考帧静态图像中除该相机位姿估计矩阵对应的物体区域之外的所有物体区域的重投影 误差的累加 和。 8.一种基于语义和多物体误差最小动态SLAM系统， 其特 征在于， 包括： 相机运动轨迹获取模块， 获取连续RGB图像帧， 对于相邻两帧RGB图像输入图像获取模 块、 静态图像获取模块、 相机位姿估计矩阵获取模块、 综合投影误差获取模块和帧间最优相 机姿态矩阵选取模块获得帧间最优相机姿态矩阵， 基于所有相邻两帧RGB图像的帧间最优 相机姿态 矩阵获得相机运动轨 迹； 图像获取模块， 获取参 考帧RGB图像和当前帧RGB图像； 静态图像获取模块， 基于参考帧RGB图像获取参考帧静态图像， 基于当前帧RGB图像获 取当前帧静态图像， 所述参考帧静态图像/当前帧静态图像中不包括先验的动态的物体区 域； 相机位姿估计矩阵获取模块， 获取参考帧静态图像中每个物体区域对应的相机位姿估 计矩阵；权　利　要　求　书 2/3 页 3 CN 115326073 A 3