CN113793378B - 基于层次分组的语义slam对象关联和位姿更新方法及*** - Google Patents

Abstract

本发明涉及一种基于层次分组的语义SLAM对象关联和位姿更新方法及***，方法包括获取动态物体的运动图像；根据运动图像构建关键帧队列，从关键帧队列中选取关键帧作为关键帧组；对每个关键帧组内的关键帧进行物体关联操作，得到对象路标及其位姿信息；根据对象路标及其位姿信息构建高斯混合模型，利用高斯混合模型判断每个关键帧组中的对象路标是否与其他关键帧组中的每个对象路标关联，得到判断结果；根据判断结果更新地图中的对象路标及其位姿信息。本发明能够高精度地实现对象关联，避免了多个相似且相近的物体产生错误关联，克服现有技术在多个相同对象相距很近的情况下存在对象关联准确性不高、场景泛化能力弱以及位姿优化不足的缺陷。

Description

基于层次分组的语义SLAM对象关联和位姿更新方法及***

技术领域

本发明涉及机器人视觉技术领域，尤其是指一种基于层次分组的语义 SLAM对象关联和位姿更新方法及***。

背景技术

尽管纯视觉SLAM具有较好的鲁棒性能，但在动态场景、快速运动、纹 理丢失、光照变化和其他情况下很容易跟踪失败。因此将传统的SLAM与语 义信息相结合可以提高***的鲁棒性，并且更符合人类对探索未知环境的认 知。但是传统的视觉SLAM在定位与建图中使用很少的语义信息，因此在某 些应用场景中受到限制。

精确的物体关联和实时更新优化的对象位姿是语义SLAM中至关重要的 组成部分。要建立精确的三维语义地图，准确的物体关联和物***姿是前提。 准确的物体关联主要依赖于准确的物体测量，包括物体的类别和位姿。但在 实际情况中，机器人的传感器捕获的信息是带有噪声的，单纯使用传感器的 信息去估计机器人的运动是不可靠的。而且现有技术的语义SLAM对象关联 方法在多个相同对象相距很近的情况下存在对象关联准确性不高、场景泛化 能力弱以及对象位姿优化不足的缺点，从而无法构建准确的三维语义地图。

发明内容

为此，本发明所要解决的技术问题在于克服现有技术中语义SLAM对象 关联方法在多个相同对象相距很近的情况下存在对象关联准确性不高、场景 泛化能力弱以及对象位姿优化不足的缺陷。

为解决上述技术问题，本发明提供一种基于层次分组的语义SLAM对象关 联和位姿更新方法，包括：

获取动态物体的运动图像；

根据所述运动图像构建关键帧队列，并从所述关键帧队列中选取连续的 关键帧作为关键帧组，其中相邻的关键帧组具有重叠的关键帧；

对每个关键帧组内的关键帧进行物体关联操作，得到每个关键帧组的对 象路标及其位姿信息；

根据一个关键帧组的对象路标及其位姿信息构建高斯混合模型，利用所 述高斯混合模型判断每个关键帧组中的每个对象路标是否与其他关键帧组中 的每个对象路标关联，得到判断结果；

根据所述判断结果更新所述地图中的对象路标及其位姿信息。

在本发明的一个实施例中，获取动态物体的运动图像包括：

在所述动态物体运动过程中，利用摄像设备捕捉其运动图像，对于每一 帧图像分别计算所述摄像设备的位姿以及摄像设备在地图上的位置。

在本发明的一个实施例中，在利用摄像设备捕捉动态物体的运动图像前 进行去畸变处理。

在本发明的一个实施例中，根据所述运动图像构建关键帧队列包括：

以第一个图像帧作为关键帧添加至关键帧队列中，并以该关键帧作为参 照选取图像信息变化明显的图像帧设为新的关键帧，将新的关键帧添加至关 键帧队列中，并以新的关键帧作为参照选取其他的关键帧，依次类推，得到 包括多个关键帧的关键帧队列。

在本发明的一个实施例中，对每个关键帧组内的关键帧进行物体关联操 作包括：

利用MOT算法检测得到每个关键帧组内的对象测量，其中对象测量包括 位姿信息，并将每个对象测量关联到对象路标，得到包含有对象路标及其位 姿信息的关键帧组。

在本发明的一个实施例中，将每个对象测量关联到对象路标，得到包含 有对象路标及其位姿信息的关键帧组包括：

首先将与关键帧组中的第一个对象测量关联的对象路标作为第一个对象 路标；然后判断该关键帧组中的第二个对象测量是否与第一个对象路标关联， 若判断结果为是，则将第二个对象测量关联到该第一个对象路标，若判断结 果为否，则生成与第二个对象测量相关联的第二个对象路标；之后再分别判 断该关键帧组中的第三个对象测量是否与第一个对象路标和第二个对象路标 关联，依次类推，得到包含有对象路标及其位姿信息的关键帧组。

在本发明的一个实施例中，每个关键帧组内的关键帧的数量小于等于10。

在本发明的一个实施例中，根据一个关键帧组的对象路标及其位姿信息 构建高斯混合模型，利用所述高斯混合模型判断每个关键帧组中的每个对象 路标是否与其他关键帧组中的每个对象路标关联包括：

S4.1：根据第n个关键帧组中的第i个对象路标及其位姿信息构建高 斯混合模型；

S4.2：将第m个关键帧组中的第j个对象路标及其位姿信息作为所述 高斯混合模型的输入值，计算第m个关键帧组中的第j个对象路标/>与第n 个关键帧组中的第i个对象路标/>的最大相似概率；

S4.3：若最大相似概率大于等于预设的阈值，则判断与/>关联，若 最大相似概率小于预设的阈值，则判断/>与/>不关联，且重复步骤S4.2， 直至找到与/>关联的对象路标。

在本发明的一个实施例中，在判断第m个关键帧组中的第j个对象路标 与第n个关键帧组中的第i个对象路标/>关联后，则得到与/>关联的所 有对象测量均与/>关联。

此外，本发明还提供一种基于层次分组的语义SLAM对象关联和位姿更新 ***，包括：

获取模块，所述获取模块用于获取动态物体的运动图像；

关键帧组构建模块，所述关键帧组构建模块用于根据所述运动图像构建 关键帧队列，并从所述关键帧队列中选取连续的关键帧作为关键帧组，其中 相邻的关键帧组具有重叠的关键帧；

第一层次对象关联模块，所述第一层次对象关联模块用于对每个关键帧 组内的关键帧进行物体关联操作，得到每个关键帧组的对象路标及其位姿信 息；

第二层次对象关联模块，所述第二层次对象关联模块用于根据一个关键 帧组的对象路标及其位姿信息构建高斯混合模型，利用所述高斯混合模型判 断每个关键帧组中的每个对象路标是否与其他关键帧组中的每个对象路标关 联，得到判断结果；

位姿更新模块，所述位姿更新模块用于根据所述判断结果更新所述地图 中的对象路标及其位姿信息。

本发明的上述技术方案相比现有技术具有以下优点：

本发明引入层次分组模型，首先对每个关键帧组内的关键帧进行对象关 联，再对关键帧组间的对象测量进行对象关联，从而能够高精度地实现对象 关联，并基于对象关联进行对象路标位姿的优化，能够促进语义SLAM的相机 位姿估计，而优化后的物***姿能够使得物体关联更准确，从而构建更精确 的语义地图，而且避免了多个相似且相近的物体产生错误关联，克服现有技 术中语义SLAM对象关联方法在多个相同对象相距很近的情况下存在对象关 联准确性不高、场景泛化能力弱以及对象位姿优化不足的缺陷。

附图说明

为了使本发明的内容更容易被清楚的理解，下面根据本发明的具体实施 例并结合附图，对本发明作进一步详细的说明。

图1是本发明一种基于层次分组的语义SLAM对象关联和位姿更新方法 的流程示意图。

图2是本发明一种基于层次分组的语义SLAM对象关联和位姿更新方法 的示意框图。

图3是本发明一种基于层次分组的语义SLAM对象关联和位姿更新方法 的部分示意框图。

图4是本发明一种基于层次分组的语义SLAM对象关联和位姿更新*** 的结构示意图。

附图标记说明：10、获取模块；20、关键帧组构建模块；30、第一层次 对象关联模块；40、第二层次对象关联模块；50、位姿更新模块。

具体实施方式

下面结合附图和具体实施例对本发明作进一步说明，以使本领域的技术 人员可以更好地理解本发明并能予以实施，但所举实施例不作为对本发明的 限定。

实施例一

请参阅图1至图3所示，本实施例提供一种基于层次分组的语义SLAM对 象关联和位姿更新方法，具体包括以下步骤：

S1：获取动态物体的运动图像。

示例地，可以利用摄像设备捕捉其运动图像。作为一个优选的方案，摄 像设备可以是相机，利用相机捕捉动态物体的运动图像，对于每一帧图像分 别计算相机的位姿以及相机在地图上的位置，在此运动过程中，相机的运动 方程表示如下：

x_t＝f(x_t-1，μ_t)+ω_t，ω_t～N(0，R_t)

其中，f(x_t-1，μ_t)表示t-1时刻到t时刻的理想情况下的位姿变化关系，μ_t表示运动测量，ω_t表示服从均值，其值为0，方差R_t表示高斯分布的噪声；还 有相机的观测方程表示如下：

z_t＝h(x_t，y_t)+v_t，v_t～N(0，Q_t)

其中，h(x_t，y_t)表示理想情况t时刻下在位姿x_t下对路标y_t的观测数据，v_t表示服从均值，其值为0，方差Q_t为高斯分布的噪声。

示例地，在利用相机捕捉动态物体的运动图像前进行去畸变处理。具体 处理为：对相机进行内参标定得到相机的畸变参数和内参矩阵如下：

其中，[x，y]表示归一化平面点的坐标，[x_distorted，y_distorted]表示畸变后的 坐标，k₁，k₂，k₃，p₁，p₂表示畸变项，r表示平面上任意一点到坐标系原点之间的 距离，P表示相机内参矩阵，f表示相机焦距，[O_x，O_y]表示主光轴点。

S2：根据运动图像构建关键帧队列，并从关键帧队列中选取连续的关键 帧作为关键帧组，其中相邻的关键帧组具有重叠的关键帧。

示例地，以第一个图像帧作为关键帧添加至关键帧队列中，并以该关键 帧作为参照选取图像信息变化明显的图像帧设为新的关键帧，将新的关键帧 添加至关键帧队列中，并以新的关键帧作为参照选取其他的关键帧，依次类 推，得到包括多个关键帧的关键帧队列。

S3：对每个关键帧组内的关键帧进行物体关联操作，得到每个关键帧组 的对象路标及其位姿信息。

示例地，对每个关键帧组内的关键帧进行物体关联操作包括：利用MOT 算法检测得到每个关键帧组内的对象测量，对象测量包括位姿信息，并将每 个对象测量关联到对象路标，得到包含有对象路标及其位姿信息的关键帧组。 其中将每个对象测量关联到对象路标，得到包含有对象路标及其位姿信息的 关键帧组包括：首先将与关键帧组中的第一个对象测量关联的对象路标作为 第一个对象路标；然后判断该关键帧组中的第二个对象测量是否与第一个对 象路标关联，若判断结果为是，则将第二个对象测量关联到该第一个对象路 标，若判断结果为否，则生成与第二个对象测量相关联的第二个对象路标； 之后再分别判断该关键帧组中的第三个对象测量是否与第一个对象路标和第 二个对象路标关联，依次类推，得到包含有对象路标及其位姿信息的关键帧 组。

S4：根据一个关键帧组的对象路标及其位姿信息构建高斯混合模型，利 用高斯混合模型判断每个关键帧组中的每个对象路标是否与其他关键帧组中 的每个对象路标关联，并根据判断结果更新地图中的对象路标及对象路标的 位姿信息。

在上述S2中，首先将第一帧图像作为关键帧F₁，并将其添加到关键帧队 列中，之后的图像帧以前一帧关键帧为参照，选取图像信息变化明显的图像 帧设为新的关键帧F₂，并将其添加到关键帧队列中，以此类推。假设在t时刻 已有D个关键帧的关键帧组，即F_1：D＝{F₁，…，F_D}。然后在零时刻开始，从关 键帧队列中的第一个关键帧F₁开始选取连续的M帧关键帧作为一组关键帧组， 例如在t时刻时，关键帧队列可以被划分成一组关键帧组G_t＝{g₁，…，g_N}，其 中g_n(1≤n≤N)表示第n个关键帧，N表示关键帧的数量。当然这里的关键 帧组的数量为多个，并且相邻的关键帧组具有重叠的关键帧。

在上述S3中，在每个关键帧组中，利用MOT算法关联关键帧组中的对象 测量，由于每个关键帧组内的关键帧的数量小于等于10，因此本发明可以高 精度地检测和跟踪属于同一个对象路标的对象测量。具体内容如下：

a)对关键帧组中的每一个关键帧F_m(1≤m≤M)使用MOT算法，检测 得到一组对象测量L_m＝{L_m，1，…，L_m，K}，K_m表示一组关键帧组中第m个关键 帧检测得到的对象测量的数量，每个对象测量L_m，k(1≤k≤K)包含许多参数 信息表示如下：

其中，分别表示检测到的物体识别ID、检测 物体所在关键帧的ID、检测物体的中心点x轴的坐标值、检测物体的中心点 y轴的坐标值以及检测物体的边界框的宽度和高度。

b)利用MOT算法使得每个对象测量得到跟踪识别ID，每个关键帧中的对 象测量可以准确的与一个对象路标相关联，它可以跨多个关键帧同时跟踪多 个对象实例，例如在第n个关键帧组中，将第m个关键帧中的第k个对象测 量和第i个对象路标之间的关联表示为/>其中上 标G表示关联在一个关键帧组内，/>表示第n个关键帧组中的第i个对象路 标；如果检测到的对象测量无法与任何先前的对象路标相关联，则该关键帧 组内将生成新的对象路标，即一组关键帧组中包含了一组对象路标集合表示第n个关键帧组中第i个对象路标，I 表示该关键帧组中对象路标的数量。需要说明的是，关键帧组中的对象路标 不是地图中使用的最终对象路标，而是在全局级别上关联的对象路标，即本 发明设计的分层次策略，其具有相同层次的对象关联。

在上述S4中，其具体包括以下内容：

S4.1：根据第n个关键帧组中的第i个对象路标及其位姿信息构建高 斯混合模型。例如可以将第1个关键帧组中的对象路标及其位姿信息作为高 斯混合模型(GMM)建模的初始化值，同时也可以作为地图中最早的对象路标 之一，其他的关键帧组中的对象路标作为检测值输入GMM进行计算。假设对 地图中的对象路标集合O进行建模，输入的检测值是关联Z个对象测量的对 象路标，可以表示如下：

其中，λ_z表示权重系数，φ(O|Θ_z)表示高斯分布密度，的均值为μ_z，方差为/>同时：

对对象路标集合O与Z个对象测量进行关联判断，并且每个对象测量数据 都设置有六个自由变量，即对应3D世界位置和三个方向。

S4.2：将第m个关键帧组中的第j个对象路标及其位姿信息作为高斯 混合模型的输入值，计算第m个关键帧组中的第j个对象路标/>与第n个关 键帧组中的第i个对象路标/>的最大相似概率。例如可以将第2个关键帧组 中的第3个对象路标/>及其位姿信息作为高斯混合模型的输入值，计算第二 个关键帧组中的第3个对象路标/>与第1个关键帧组中的第2个对象路标/>的最大相似概率。

S4.3：若最大相似概率大于等于预设的阈值，则判断与/>关联，且 与/>关联的所有对象测量均与/>关联；若最大相似概率小于预设的阈值， 则判断/>与/>不关联，且重复步骤S4.2，直至找到与/>关联的对象路标， 若是遍历当前地图中所有的对象路标都没有找到关联的对象路标，则将/>作 为新的对象路标加入到地图中，并更新地图中对象路标的相关信息，同时利 用/>的位姿信息对现有的GMM进行扩充。

还有，关键帧组中的对象路标在全局地图中分配唯一的对象路标，其 关系q_p如下：

其中，O_p表示当前地图中第P个对象路标，P表示t时刻地图中的对象路 标的数量。

在上述S5中，更新地图中对象路标的位姿包括以下内容：

每个对象路标O_p中包含多个对象测量，计算第k个对象测量与同一对象路 标相关联的其他对象的平均角度差θ_k和平均距离差φ_k，由于距离和角度的尺 度不同，因此我们需要对其进行归一化，设置最大角度差A和最大距离差B， 然后将归一化的平均角度差和平均距离差/>计算为：

然后，将与对象路标O_p相关的第k个对象测量L_k的位姿差计算为：

其中，α和β分别是角度和位置差的权重，在所有实验中，分别将它们设 置为0.4和0.6，然后对所有位姿差进行排序，并将对象路标的位姿设置为具 有最小位姿差的对象测量的位姿。

如果检测到关键帧回环，进行回环矫正，并更新相关对象路标位姿，点 的位置和相机位姿。

随着物体关联操作和优化操作的执行，实时更新绘制带有三维位姿的对 象路标、相机运动轨迹的地图。

本发明引入层次分组模型，首先对每个关键帧组内的关键帧进行对象关 联，再对关键帧组间的对象测量进行对象关联，从而能够高精度地实现对象 关联，并基于对象关联进行对象路标位姿的优化，能够促进语义SLAM的相机 位姿估计，而优化后的物***姿能够使得物体关联更准确，从而构建更精确 的语义地图，而且避免了多个相似且相近的物体产生错误关联，克服现有技 术中语义SLAM对象关联方法在多个相同对象相距很近的情况下存在对象关联 准确性不高、场景泛化能力弱以及对象位姿优化不足的缺陷。

实施例二

下面对本发明实施例2公开的一种基于层次分组的语义SLAM对象关联和 位姿更新***进行介绍，下文描述的一种基于层次分组的语义SLAM对象关联 和位姿更新***与上文描述的一种基于层次分组的语义SLAM对象关联和位姿 更新方法可相互对应参照。

请参阅图4所示，本发明实施例提供一种基于层次分组的语义SLAM对象 关联和位姿更新***，包括：

获取模块10，获取模块10用于获取动态物体的运动图像；

关键帧组构建模块20，关键帧组构建模块20用于根据所述运动图像构建 关键帧队列，并从所述关键帧队列中选取连续的关键帧作为关键帧组，其中 相邻的关键帧组具有重叠的关键帧；

第一层次对象关联模块30，第一层次对象关联模块30用于对每个关键帧 组内的关键帧进行物体关联操作，得到每个关键帧组的对象路标及其位姿信 息；

第二层次对象关联模块40，第二层次对象关联模块40用于根据一个关键 帧组的对象路标及其位姿信息构建高斯混合模型，利用所述高斯混合模型判 断每个关键帧组中的每个对象路标是否与其他关键帧组中的每个对象路标关 联，得到判断结果；

位姿更新模块50，位姿更新模块50用于根据判断结果更新地图中的对象 路标及对象路标的位姿信息。

本实施例的基于层次分组的语义SLAM对象关联和位姿更新***用于实现 前述的基于层次分组的语义SLAM对象关联和位姿更新方法，因此该***的具 体实施方式可见前文中的基于层次分组的语义SLAM对象关联和位姿更新方法 的实施例部分，所以，其具体实施方式可以参照相应的各个部分实施例的描 述，在此不再展开介绍。

另外，由于本实施例的基于层次分组的语义SLAM对象关联和位姿更新系 统用于实现前述的基于层次分组的语义SLAM对象关联和位姿更新方法，因此 其作用与上述方法的作用相对应，这里不再赘述。

本领域内的技术人员应明白，本申请的实施例可提供为方法、***、或 计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、 或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个 其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘 存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。

本申请是参照根据本申请实施例的方法、设备(***)、和计算机程序 产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程 图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流 程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算 机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使 得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实 现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定 的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理 设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储 器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程 或多个流程和/或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上， 使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现 的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程 图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的 步骤。

显然，上述实施例仅仅是为清楚地说明所作的举例，并非对实施方式的 限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出 其它不同形式变化或变动。这里无需也无法对所有的实施方式予以穷举。而 由此所引申出的显而易见的变化或变动仍处于本发明创造的保护范围之中。

Claims (8)

1.一种基于层次分组的语义SLAM对象关联和位姿更新方法，其特征在于，包括：

获取动态物体的运动图像；

根据所述运动图像构建关键帧队列，并从所述关键帧队列中选取连续的关键帧作为关键帧组，其中相邻的关键帧组具有重叠的关键帧；

对每个关键帧组内的关键帧进行物体关联操作，得到每个关键帧组的对象路标及其位姿信息；

根据一个关键帧组的对象路标及其位姿信息构建高斯混合模型，利用所述高斯混合模型判断每个关键帧组中的每个对象路标是否与其他关键帧组中的每个对象路标关联，得到判断结果；

根据所述判断结果更新地图中的对象路标及其位姿信息；

对每个关键帧组内的关键帧进行物体关联操作包括：

利用MOT算法检测得到每个关键帧组内的对象测量，其中对象测量包括位姿信息，并将每个对象测量关联到对象路标，得到包含有对象路标及其位姿信息的关键帧组；

其中，将每个对象测量关联到对象路标，得到包含有对象路标及其位姿信息的关键帧组包括：

首先将与关键帧组中的第一个对象测量关联的对象路标作为第一个对象路标；然后判断该关键帧组中的第二个对象测量是否与第一个对象路标关联，若判断结果为是，则将第二个对象测量关联到该第一个对象路标，若判断结果为否，则生成与第二个对象测量相关联的第二个对象路标；之后再分别判断该关键帧组中的第三个对象测量是否与第一个对象路标和第二个对象路标关联，依次类推，得到包含有对象路标及其位姿信息的关键帧组。

2.根据权利要求1所述的基于层次分组的语义SLAM对象关联和位姿更新方法，其特征在于：获取动态物体的运动图像包括：

在所述动态物体运动过程中，利用摄像设备捕捉其运动图像，对于每一帧图像分别计算所述摄像设备的位姿以及摄像设备在地图上的位置。

3.根据权利要求2所述的基于层次分组的语义SLAM对象关联和位姿更新方法，其特征在于：在利用摄像设备捕捉动态物体的运动图像前进行去畸变处理。

4.根据权利要求1所述的基于层次分组的语义SLAM对象关联和位姿更新方法，其特征在于：根据所述运动图像构建关键帧队列包括：

以第一个图像帧作为关键帧添加至关键帧队列中，并以该关键帧作为参照选取图像信息变化明显的图像帧设为新的关键帧，将新的关键帧添加至关键帧队列中，并以新的关键帧作为参照选取其他的关键帧，依次类推，得到包括多个关键帧的关键帧队列。

5.根据权利要求1所述的基于层次分组的语义SLAM对象关联和位姿更新方法，其特征在于：每个关键帧组内的关键帧的数量小于等于10。

6.根据权利要求1所述的基于层次分组的语义SLAM对象关联和位姿更新方法，其特征在于：根据一个关键帧组的对象路标及其位姿信息构建高斯混合模型，利用所述高斯混合模型判断每个关键帧组中的每个对象路标是否与其他关键帧组中的每个对象路标关联包括：

S4.1：根据第n个关键帧组中的第i个对象路标及其位姿信息构建高斯混合模型；

S4.2：将第m个关键帧组中的第j个对象路标及其位姿信息作为所述高斯混合模型的输入值，计算第m个关键帧组中的第j个对象路标/>与第n个关键帧组中的第i个对象路标/>的最大相似概率；

S4.3：若最大相似概率大于等于预设的阈值，则判断与/>关联，若最大相似概率小于预设的阈值，则判断/>与/>不关联，且重复步骤S4.2，直至找到与/>关联的对象路标。

7.根据权利要求6所述的基于层次分组的语义SLAM对象关联和位姿更新方法，其特征在于：在判断第m个关键帧组中的第j个对象路标与第n个关键帧组中的第i个对象路标关联后，则得到与/>关联的所有对象测量均与/>关联。

8.一种基于层次分组的语义SLAM对象关联和位姿更新***，其特征在于，包括：

获取模块，所述获取模块用于获取动态物体的运动图像；

关键帧组构建模块，所述关键帧组构建模块用于根据所述运动图像构建关键帧队列，并从所述关键帧队列中选取连续的关键帧作为关键帧组，其中相邻的关键帧组具有重叠的关键帧；

第一层次对象关联模块，所述第一层次对象关联模块用于对每个关键帧组内的关键帧进行物体关联操作，得到每个关键帧组的对象路标及其位姿信息；

第二层次对象关联模块，所述第二层次对象关联模块用于根据一个关键帧组的对象路标及其位姿信息构建高斯混合模型，利用所述高斯混合模型判断每个关键帧组中的每个对象路标是否与其他关键帧组中的每个对象路标关联，得到判断结果；

位姿更新模块，所述位姿更新模块用于根据所述判断结果更新地图中的对象路标及其位姿信息；

对每个关键帧组内的关键帧进行物体关联操作包括：

利用MOT算法检测得到每个关键帧组内的对象测量，其中对象测量包括位姿信息，并将每个对象测量关联到对象路标，得到包含有对象路标及其位姿信息的关键帧组；

其中，将每个对象测量关联到对象路标，得到包含有对象路标及其位姿信息的关键帧组包括：

首先将与关键帧组中的第一个对象测量关联的对象路标作为第一个对象路标；然后判断该关键帧组中的第二个对象测量是否与第一个对象路标关联，若判断结果为是，则将第二个对象测量关联到该第一个对象路标，若判断结果为否，则生成与第二个对象测量相关联的第二个对象路标；之后再分别判断该关键帧组中的第三个对象测量是否与第一个对象路标和第二个对象路标关联，依次类推，得到包含有对象路标及其位姿信息的关键帧组。