基于语义和多物体误差最小动态SLAM方法及机器人与流程

基于语义和多物体误差最小动态slam方法及机器人
技术领域
1.本发明涉及移动机器人视觉定位和建图技术领域，尤其涉及基于语义和多物体误差最小动态slam方法及机器人。


背景技术：

2.同步定位和建图(slam)技术主要分为激光slam和视觉slam两大类。其中，视觉slam是指在没有先验信息的情况下，利用搭载在机器人上的相机感知周围环境，在运动过程中建立环境模型，同时估计自身位置。相机的成本低廉，结构简单，采集得到的信息量更大，应用范围更广。目前很多产品都是应用视觉slam技术进行定位和建图，例如无人车、虚拟现实、无人机和家用扫地机器人等。
3.视觉slam的框架有很多，但大多都是基于静态环境假设的前提去构建。传统的orb-slam2系统是一个集成了单目、双目、深度相机的综合系统，其运行过程是将环境假设为静态场景且场景的主要部分的变化是由摄像头运动造成的。然而在实际环境中，不可避免地存在移动的物体，如室内的人、室外行驶的车辆等。虽然orb-slam2采用了随机抽样一致性检测(ransac)的方法来去除匹配过程中的离群点来提高在动态环境下的鲁棒性，但当环境中动态物体占优时，这种方法往往难以区分出外点，导致相机位姿估计误差较大。
4.针对以上问题，中国专利(cn112308921a)公开了一种基于语义和几何联合优化动态slam的方法。该方法使用实例分割网络mask-rcnn对物体提取语义信息并生成语义二进制掩膜，并对图像进行几何分割生成几何二进制掩膜，基于两种掩膜的动静态信息来计算特征点权重，进而判断特征点的动静态属性。
5.上述专利公开的slam方法主要是通过融合语义和几何两种途径的信息来确定特征点动静态属性，在融合过程中通过设置不同的加权系数和阈值来计算特征点的属性。但由于在运动过程中误差的存在，以及加权系数设置对整个系统的干扰较大，所以该方案在实际动态环境应用过程中鲁棒性较低。导致使用几何分割和语义分割最终融合结果发生错误，从而导致计算相机轨迹和位姿时出现较大偏差，对整个slam系统造成严重的影响。


技术实现要素：

6.本发明旨在至少解决现有技术中存在的技术问题，提供一种基于语义和多物体误差最小动态slam方法及机器人。
7.为了实现本发明的上述目的，根据本发明的第一个方面，本发明提供了一种基于语义和多物体误差最小动态slam方法，获取连续rgb图像帧，对于相邻两帧rgb图像执行步骤s1到步骤s5获得帧间最优相机姿态矩阵，基于所有相邻两帧rgb图像的帧间最优相机姿态矩阵获得相机运动轨迹，其中：步骤s1，获取参考帧rgb图像和当前帧rgb图像；步骤s2，基于参考帧rgb图像获取参考帧静态图像，基于当前帧rgb图像获取当前帧静态图像，所述参考帧静态图像/当前帧静态图像中不包括先验的动态的物体区域；步骤s3，获取参考帧静态图像中每个物体区域对应的相机位姿估计矩阵；步骤s4，对于每个相机位姿估计矩阵，求取
参考帧静态图像中除该相机位姿估计矩阵对应的物体区域之外的物体区域基于该相机位姿估计矩阵在当前帧rgb图像上的重投影误差，融合参考帧静态图像中除该相机位姿估计矩阵对应的物体区域之外的所有物体区域的重投影误差获得该相机位姿估计矩阵的综合投影误差；步骤s5，选取综合投影误差最小的相机位姿估计矩阵作为帧间最优相机姿态矩阵。
8.为了实现本发明的上述目的，根据本发明的第二个方面，本发明提供了一种基于语义和多物体误差最小动态slam系统，包括：相机运动轨迹获取模块，获取连续rgb图像帧，对于相邻两帧rgb图像输入图像获取模块、静态图像获取模块、相机位姿估计矩阵获取模块、综合投影误差获取模块和帧间最优相机姿态矩阵选取模块获得帧间最优相机姿态矩阵，基于所有相邻两帧rgb图像的帧间最优相机姿态矩阵获得相机运动轨迹；图像获取模块，获取参考帧rgb图像和当前帧rgb图像；静态图像获取模块，基于参考帧rgb图像获取参考帧静态图像，基于当前帧rgb图像获取当前帧静态图像，所述参考帧静态图像/当前帧静态图像中不包括先验的动态的物体区域；相机位姿估计矩阵获取模块，获取参考帧静态图像中每个物体区域对应的相机位姿估计矩阵；综合投影误差获取模块，对于每个相机位姿估计矩阵，求取参考帧静态图像中除该相机位姿估计矩阵对应的物体区域之外的物体区域基于该相机位姿估计矩阵在当前帧rgb图像上的重投影误差，融合参考帧静态图像中除该相机位姿估计矩阵对应的物体区域之外的所有物体区域的重投影误差获得该相机位姿估计矩阵的综合投影误差；帧间最优相机姿态矩阵选取模块，选取综合投影误差最小的相机位姿估计矩阵作为帧间最优相机姿态矩阵。
9.为了实现本发明的上述目的，根据本发明的第三个方面，本发明提供了一种计算机可读存储介质，所述存储介质中存储有至少一条指令、至少一段程序、代码集或指令集，所述至少一条指令、所述至少一段程序、所述代码集或指令集由处理器加载并执行以实现如本发明第一方面所述的基于语义和多物体误差最小动态slam方法。
10.为了实现本发明的上述目的，根据本发明的第四个方面，本发明提供了一种移动机器人，包括移动机器人本体和设于所述移动机器人本体上的视觉设备和处理器，所述处理器接收视觉设备拍摄的连续帧rgb图像，并执行本发明第一方面所述的基于语义和多物体误差最小动态slam方法的步骤。
11.本发明主要应用于动态环境下的相机定位和建图，通过参考帧静态图像和当前帧静态图像来剔除绝对动态物体对后续求取相机姿态矩阵(即定位)精度的影响，利用分割出的每个物体区域来分别计算相机位姿估计矩阵，获取每个相机位姿估计矩阵在其他物体区域的重投影误差，选择综合投影误差最小的相机位姿估计矩阵作为最优的相机姿态矩阵，进一步提高相机位姿估计精度和建图的准确性，获得的相机运动轨迹更加精确。
附图说明
12.图1是本发明实施例1中基于语义和多物体误差最小动态slam方法流程示意图；
13.图2是本发明实施例1中参考帧图像中特征点空间三维坐标计算示意图；
14.图3是本发明实施例1一种应用场景的全局流程示意图。
具体实施方式
15.下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，仅用于解释本发明，而不能理解为对本发明的限制。
16.在本发明的描述中，需要理解的是，术语“纵向”、“横向”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。
17.在本发明的描述中，除非另有规定和限定，需要说明的是，术语“安装”、“相连”、“连接”应做广义理解，例如，可以是机械连接或电连接，也可以是两个元件内部的连通，可以是直接相连，也可以通过中间媒介间接相连，对于本领域的普通技术人员而言，可以根据具体情况理解上述术语的具体含义。
18.实施例1
19.本实施例公开了一种基于语义和多物体误差最小动态slam方法，获取连续rgb图像帧，对于相邻两帧rgb图像执行步骤s1到步骤s5获得帧间最优相机姿态矩阵，基于所有相邻两帧rgb图像的帧间最优相机姿态矩阵获得相机运动轨迹，如图1所示，其中：
20.步骤s1，获取参考帧rgb图像和当前帧rgb图像；参考帧rgb图像和当前帧rgb图像为连续的相邻两帧rgb图像，参考帧rgb图像优选但不限于为当前帧rgb图像的前一帧rgb图像。
21.步骤s2，基于参考帧rgb图像获取参考帧静态图像，基于当前帧rgb图像获取当前帧静态图像。参考帧静态图像不包括先验的动态的物体区域，当前帧静态图像不包括先验的动态的物体区域，先验的动态的物体区域是指根据在先经验知识确认为绝对动态的物体区域。参考帧静态图像/当前帧静态图像中包括先验静态物体区域和背景。
22.在本实施例中，具体的，通过去除参考帧rgb图像中的先验动态物体区域获得参考帧静态图像，通过去除当前帧rgb图像中的先验动态物体区域获得当前帧静态图像。
23.步骤s3，获取参考帧静态图像中每个物体区域对应的相机位姿估计矩阵。设j为物体区域索引，j为正整数，优选地，获取参考帧静态图像的第j个物体区域对应的相机位姿估计矩阵，具体包括：
24.步骤s31，从第j个物体区域中选取n个特征点并获取n个特征点在当前帧静态图像中对应的n个匹配特征点；n为正整数；
25.步骤s32，获取参考帧静态图像的第j个物体区域中n个特征点的空间三维坐标；优选地，当视觉设备(如移动机器人上的视觉设备)为rgbd相机或双目相机时，可通过深度图或双目深度测量原理获取每个特征点的深度值，根据特征点的深度值和在参考帧静态图像中的二维坐标能获取到该特征点的空间三维坐标。
26.步骤s33，建立目标函数：
27.其中，x
i'
表示参考帧静态图像的第j个物体区域中第i个特征点在当前帧静态图像中对应的匹配特点i'的二维坐标；r表示旋转矩阵，t表示平移矩阵；xi表示第i个特征点的
空间三维坐标；π(
·
)表示向当前帧静态图像投影的投影函数；表示求取达到最小值时的相机位姿估计矩阵t，将此时的相机位姿估计矩阵作为最优相机位姿估计矩阵t
*
，r
*
表示最优旋转矩阵，t
*
表示最优平移矩阵；表示二范数。
28.步骤s34，求解目标函数获得最优相机位姿估计矩阵t
*
作为参考帧静态图像的第j个物体区域对应的相机位姿估计矩阵。引入bund le adjustment(光束法平差算法)通过最小化图像帧中第j个物体区域内的所有特征点的重投影误差总和来进行求解。优选但不限于采用现有的列文伯格-马夸尔特方法求解目标函数获得最优相机位姿估计矩阵t
*
。
29.步骤s4，对于每个相机位姿估计矩阵，求取参考帧静态图像中除该相机位姿估计矩阵对应的物体区域之外的物体区域基于该相机位姿估计矩阵在当前帧rgb图像上的重投影误差，融合参考帧静态图像中除该相机位姿估计矩阵对应的物体区域之外的所有物体区域的重投影误差获得该相机位姿估计矩阵的综合投影误差。按照步骤s4求取所有相机位姿估计矩阵的综合投影误差。
30.步骤s5，选取综合投影误差最小的相机位姿估计矩阵作为帧间最优相机姿态矩阵。
31.在本实施例中，为了实现自动化获取参考帧静态图像/当前帧静态图像，在步骤s2中，获取参考帧静态图像/当前帧静态图像的具体过程包括：
32.步骤s21,将参考帧rgb图像/当前帧rgb图像输入训练好的实例分割网络获得物体表面掩膜；实例分割网络优选但不限于采用solov2网络。物体表面掩膜上包括实例分割网络根据先验知识识别的动态物体区域和静态物体区域，实例分割网络优选但不限于采用二进制掩膜mask，比如二进制掩膜mask中值为0的像素为语义动态的物体像素，值为1的像素为语义非动态的物体像素，非动态的物体像素包括静态的物体像素和背景。
33.步骤s22,利用物体表面掩膜去除参考帧rgb图像/当前帧rgb图像中的先验动态物体的像素点获得参考帧静态图像/当前帧静态图像。具体的，如将物体表面掩膜中像素值为0的像素去除，对像素值为1的区域中的各物体区域进行编号。
34.在本实施例中，由于实例分割网络检测到的物体信息与前期所采用的训练数据集有关，实际应用时存在无法检测到的物体(如因先验知识不足)以及背景的相关信息，则将剩余的实例分割网络识别的所有非物体区域定义为背景区域。为弥补实例分割网络的不足，避免遗漏静态物体区域，影响后期slam定位精度，进一步优选地，在步骤s3中，在参考帧静态图像和当前帧静态图像中均将各自的背景区域作为一个物体区域，参与后续相机姿态评估定位。
35.在本实施例中，优选地，实例分割网络的训练过程包括：
36.(1)构建样本集，样本为rgb图像，每个rgb图像样本对应一个物体表面掩膜，物体表面掩膜可通过人工先验对rgb图像样本上的物体识别和分割处理获得，物体表面掩膜上每个物体(可为静态物体和动态物体)设置了语义标签，该语义标签用于标记物体是动态的
还是静态的。将样本集划分为训练集、验证集和测试集。
37.(2)构建实例分割网络，优选但不限于为solov2网络，利用训练集训练实例分割网络，分别利用验证集和测试集对训练后的实例分割网络进行验证和测试，获得训练后的实例分割网络。
38.(3)将当前帧rgb图像输入训练后的实例分割网络，获得物体掩膜以及物体掩膜上每个物体的语义标签。
39.在本实施例中，在步骤s32中，当用于slam的视觉设备为单目相机时，特征点通过帧间基础矩阵获得三维空间坐标，为提高特征点三维空间坐标的准确性，对帧间基础矩阵进行初始化，确保帧间基础矩阵通过参考帧静态图像中最静态的物体区域计算获得，以提高帧间基础矩阵准确性和后期相机姿态估计(定位)的准确度，因此，进一步优选地，获取参考帧静态图像中特征点的空间三维坐标的过程包括：
40.步骤s321，在参考帧静态图像中遍历所有物体区域更新参考帧静态图像，具体为：判断每个物体区域在当前帧静态图像中是否存在与该物体区域具有至少8对匹配特征点对的匹配物体区域，若存在，则保留该物体区域，若不存在，则舍弃该物体区域。具体的，令帧间基础矩阵为f，f矩阵描述了空间中的点在两个平面中的坐标对应的关系。f矩阵的大小为3x3矩阵，有9个未知数即9个自由度，但由于其尺度等价性，所以自由度为8。连续两帧图像之间最少存在8对匹配的特征点即可求出帧间基础矩阵f，因此对各物体区域进行遍历，判断物体区域内是否存在8对及以上的特征点数目。
41.进一步优选地，当大于8对匹配特征点对时，为滤除离群值，提高帧间基础矩阵鲁棒性，采用ransac算法滤除离群值，具体包括：首先任意取物体区域内匹配的8对点，可通过现有的8点法计算f矩阵。通过区域内的其它特征点来计算该f矩阵的误差，迭代k次取误差最小时模型的f矩阵为该步的计算值，同理代入其它物体，最终求得初始化的f矩阵，k为正整数。
42.步骤s322，基于更新后的参考帧静态图像中每个物体区域的匹配特征点对获取该物体区域的帧间基础矩阵，即依次对各物体区域求解对应的帧间基础矩阵f。
43.步骤s323，基于每个物体区域的帧间基础矩阵获取该物体区域在匹配物体区域的所有匹配特征点到极线l2的极线距离总和；具体的，如图2所示，如对于更新后的参考帧静态图像中第一个物体区域的求解过程为：令参考帧静态图像中第一物体区域m1内特征点x1，当前帧静态图像fc中第一物体区域对应的匹配物体区域内与特征点x1相匹配的匹配特征点为x
1'
，特征点和匹配特征点对应的归一化坐标如下：
44.x1＝[u
1 v
1 1]
t
，x
1'
＝[u
1' v
1' 1]
t
；
[0045]
u1、v1分别表示特征点x1的归一化横坐标和纵坐标，u
1'
、v
1'
分别表示匹配特征点x
1'
的归一化横坐标和纵坐标。
[0046]
根据对极约束有：x
1't
fx1＝0；
[0047]
极线l1可以表示为:l1＝[x y z]
t
＝fx1＝f[u
1 v
1 1]
t
；
[0048]
则匹配特征点x
1'
到其极线l2的极线距离为:
[0049]
[0050]
其中，x、y、z分别表示特征点的三维空间x轴、y轴、z轴坐标值。
[0051]
步骤s324，将极线距离总和最小的物体区域的帧间基础矩阵作为初始化帧间基础矩阵；
[0052]
步骤s325，基于初始化帧间基础矩阵获取参考帧静态图像中特征点的空间三维坐标。
[0053]
在本实施例中，为准确计算重投影误差，进一步优选地，在步骤s4中，求取参考帧静态图像中除该相机位姿估计矩阵对应的物体区域之外的物体区域基于该相机位姿估计矩阵在当前帧rgb图像上的重投影误差的步骤包括：
[0054]
步骤s41，设求取第j个物体区域基于相机位姿估计矩阵在当前帧rgb图像上的重投影误差，相机位姿估计矩阵为不与第j个物体区域对应的相机位姿估计矩阵，与第j个物体区域对应的相机位姿估计矩阵可表示为j'≠j，n、j均为为正整数；设第j个物体区域包括n个特征点，基于相机位姿估计矩阵分别获取n个特征点在当前帧rgb图像中的估计匹配特征点坐标；
[0055]
步骤s42，将估计匹配特征点坐标和实际匹配特征点坐标分别在归一化平面投影获得归一化估计坐标和归一化实际坐标，求取匹配特征点的归一化估计坐标与归一化实际坐标的距离，将所述距离记为匹配特征点的误差距离；设第i个特征点求取的归一化估计坐标和归一化实际坐标分别表示为和(ui,vi)，则第i个特征点的误差距离为：|| ||表示取绝对值模长。
[0056]
步骤s43，求取第j个物体区域内所有匹配特征点的误差距离的平均值将该平均值d_fin作为第j个物体区域基于相机位姿估计矩阵在当前帧rgb图像上的重投影误差。
[0057]
在本实施例中，在步骤s4中，获取综合投影误差的融合方式优选但不限于为求取融合参考帧静态图像中除该相机位姿估计矩阵对应的物体区域之外的所有物体区域的重投影误差的平均值或最大最小值差值等。
[0058]
在本实施例中，为更全面的评估所有重投影误差，进一步优选地，在步骤s4中，在融合参考帧静态图像中除该相机位姿估计矩阵对应的物体区域之外的所有物体区域的重投影误差获得该相机位姿估计矩阵的综合投影误差步骤中，综合投影误差为参考帧静态图像中除该相机位姿估计矩阵对应的物体区域之外的所有物体区域的重投影误差的累加和。具体的，设相机位姿估计矩阵对应第j个物体区域，则参考帧静态图像中除相机位姿估计矩阵对应的第j个物体区域之外的所有物体区域的重投影误差的累加和表示为：
[0059]
sj＝d_fin1+...+d_fin
j-1
+d_fin
j+1
+...+d_finj；
[0060]
j表示参考帧静态图像中物体区域的总数量，sj表示相机位姿估计矩阵的综合投影误差。按照上述过程求取所有的相机位姿估计矩阵的综合投影误差：s1、s2、s3、...、sj，从s1、s2、s3、...、sj中选择最小者对应的相机位姿估计矩阵作为最优相机姿态估计矩阵，根
据最优相机姿态估计矩阵能够获得相机位姿，实现相机以及机器人定位。
[0061]
在本实施例的一种应用场景中，如图3所示，采用solov2网络获取物体表面掩膜，即原始蒙版。连续帧图像依次按照上述步骤s1到步骤s5处理获取到一系列帧间最优相机位姿矩阵，进而生成完整的相机运动轨迹曲线。在连续图像帧选取追踪效果较好的rgb图像帧作为关键帧(优选但不限于为连续几帧中综合投影误差最小的帧、或者前后差距差异大的帧)，将关键帧输入slam系统后端，完成局部建图和回环检测等环节。
[0062]
本实施例提供了一种结合实例分割算法和多物体重投影误差最小化的slam方法，可用于提高动态环境下系统定位和建图的精度。其中实例分割算法可对输入图像序列进行实例分割，得到各物体在图像中所占区域，将语义为先验动态的物体区域直接剔除。在实际场景中，基于各独立刚体物体几乎不会相对相机朝向一致的方向运动的前提，所以针对实例分割出的其余各物体分别进行计算。首先通过其中一个物体上的特征点计算出该物体相应的位姿矩阵，将此位姿矩阵代入其它物体中，计算估计匹配点和实际匹配点之间的误差，将所有物体误差值累加并归一化作为该物体位姿矩阵的综合投影误差值。各物体依次迭代，最终取综合投影误差最小的相机位姿矩阵作为帧间最优位姿矩阵来计算相机的位姿，即可提高相机位姿估计精度和建图的准确性。
[0063]
与现有slam方法相比，本技术发明重点研究动态环境下的相机定位和建图问题，综合分析基于深度学习的slam系统和传统slam系统的优势和不足。采用实例分割网络结合多物体计算位姿矩阵最终使整体误差最小化的方法来提高slam方法的精度。充分利用实例分割结果，剔除绝对动态物体的同时。本实施例提出利用分割出的其余物体分别来计算帧间位姿估计矩阵，并将矩阵结果代入其它物体和背景中，通过计算估计匹配点和实际匹配点之间距离误差来作为该矩阵质量的衡量参数，各物体依次迭代，最终找到帧间最优位姿估计矩阵。
[0064]
实施例2
[0065]
本实施例公开了一种基于语义和多物体误差最小动态slam系统，包括：相机运动轨迹获取模块，获取连续rgb图像帧，对于相邻两帧rgb图像输入图像获取模块、静态图像获取模块、相机位姿估计矩阵获取模块、综合投影误差获取模块和帧间最优相机姿态矩阵选取模块获得帧间最优相机姿态矩阵，基于所有相邻两帧rgb图像的帧间最优相机姿态矩阵获得相机运动轨迹；图像获取模块，获取参考帧rgb图像和当前帧rgb图像；静态图像获取模块，基于参考帧rgb图像获取参考帧静态图像，基于当前帧rgb图像获取当前帧静态图像，参考帧静态图像/当前帧静态图像中不包括先验的动态的物体区域；相机位姿估计矩阵获取模块，获取参考帧静态图像中每个物体区域对应的相机位姿估计矩阵；综合投影误差获取模块，对于每个相机位姿估计矩阵，求取参考帧静态图像中除该相机位姿估计矩阵对应的物体区域之外的物体区域基于该相机位姿估计矩阵在当前帧rgb图像上的重投影误差，融合参考帧静态图像中除该相机位姿估计矩阵对应的物体区域之外的所有物体区域的重投影误差获得该相机位姿估计矩阵的综合投影误差；帧间最优相机姿态矩阵选取模块，选取综合投影误差最小的相机位姿估计矩阵作为帧间最优相机姿态矩阵。
[0066]
在本实施例中，每个模块与实施例1中的步骤对应，在此不再赘述。
[0067]
实施例3
[0068]
本实施例公开了一种计算机可读存储介质，存储介质中存储有至少一条指令、至
少一段程序、代码集或指令集，至少一条指令、所述至少一段程序、代码集或指令集由处理器加载并执行以实现实施例1提供的基于语义和多物体误差最小动态slam方法。
[0069]
实施例4
[0070]
本实施例公开了一种移动机器人，包括移动机器人本体和设于所述移动机器人本体上的视觉设备和处理器，所述处理器接收视觉设备拍摄的连续帧rgb图像，并执行实施例1提供的基于语义和多物体误差最小动态slam方法的步骤
[0071]
在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。
[0072]
尽管已经示出和描述了本发明的实施例，本领域的普通技术人员可以理解：在不脱离本发明的原理和宗旨的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由权利要求及其等同物限定。