CN109544632B - 一种基于层次主题模型的语义slam对象关联方法 - Google Patents

Abstract

一种基于层次主题模型的语义SLAM对象关联的方法，利用深度学习模型检测关键帧中的物体并预测其位姿，在处理每一帧对象时，根据视角重叠的原则利用吉布斯采样方法采样具有潜在关联对象的真实环境对象集合，为当前帧的每一个物体根据物体关联方法进行计算，根据最大后验概率判断是否关联。对物体、相机和地图点构建因子图，它们之间的观测作为边，优化物体位姿、相机位姿和地图点位置。最后构建一个完整的包含物体信息和相机轨迹的语义地图。本发明能够高精度地实现物体关联，避免了冗余的物体关联；能够促进语义SLAM的相机位姿估计，而优化后的物体位姿能够使得物体关联更准确，从而构建更精确的语义地图。

Description

一种基于层次主题模型的语义SLAM对象关联方法

技术领域

本发明涉及机器人视觉、深度学习、统计学等技术领域，具体一种基于层次主题模型的语义SLAM对象关联方法。

背景技术

Simultaneous localization and mapping(SLAM)是机器人应用中的一个重要的问题，比如自动驾驶、自主导航等领域。构建精准的环境地图是SLAM的具体表现形式，传统的SLAM技术依赖于低级的几何特征，例如点、线、面，这种技术在空旷的或者由重复纹理的环境下容易失效。而语义SLAM利用环境中的高级语义信息，可以有效的弥补传统SLAM的不足，并且能够建立可读的，更具应用价值的语义地图。

物体关联和物体位姿优化是语义SLAM中两个至关重要的组成部分。要建立精确的语义地图，准确的物体关联是前提。准确的物体关联依赖于准确的物体测量，包括物体的类别和位姿。但是实际中，随着机器人的运动，传感器捕获的信息是带有噪声的，仅依靠传感器的信息估计机器人的运动是不可靠的，所以需要多种优化算法的辅助。优化算法是设计一般是基于获取的地标信息和相机之间的几何约束创造的，最大化合理高效的利用已有测量信息是评价优化算法的主要标准。

因此，如何设计一种稳定可靠的语义SLAM中的对象关联方法以及优化方法，使得语义SLAM算法能够构建一个准确的语义地图，是实现有效的语义SLAM需要解决的问题。

发明内容

本发明的目的是为语义SLAM设计一种稳定有效的语义对象关联和优化的方法，解决语义SLAM中的基本问题从而构建准确的语义地图。

为了解决上述技术问题，本发明提供如下的技术方案：

一种基于层次主题模型的语义SLAM对象关联方法，包括以下步骤：

1)对相机进行内参标定得到相机的畸变参数和内参矩阵

其中，[x，y]是归一化平面点的坐标，[x_distorted，y_distorted]是畸变后的坐标，k₁，k₂，k₃，p₁，p₂是畸变项；

P为相机内参矩阵，其中f为相机焦距，[O_x，O_y]为主光轴点；

2)利用Single Shot MultiBox Detector(SSD)和Convolutional NeuralNetwork(CNN)构建深度学习网络，训练深度学习模型，完成物体识别和物体位姿预测任务；

3)基于层次主题模型的语义SLAM是在ORB-SLAM2的基础上改进的，机器人运动过程中，相机会捕捉到一系列的图像I_1：T＝{I₁，...，I_T}，对于每一帧图像I_t，计算相机的位姿x_t以及地图点的三维位置，在此过程中，相机的运动方程表示如下：

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

其中，μ_t是运动测量，在视觉SLAM中没有该测量信息，ω_t是服从均值为0，方差为R_t的高斯分布的噪声，相机的观测方程表示如下：

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

将第一帧图像作为关键帧D₀，之后的图像以前一帧关键帧为参照，将图像信息变化明显的图像帧设为新的关键帧D₁，并添加到关键帧队列，以此类推，假设整个过程中共抽取了k个关键帧，即D_0：k＝{D₀，...，D_k}，D表示关键帧集合；

4)对每一个关键帧D_i作如下操作，0≤i≤k：

a)利用步骤2)深度学习模型识别关键帧对应的图像上的目标物体，并估计其相对于相机的位姿，则得到该帧图像的对象测量

其中M_i表示在该帧图像中检测到的物体个数；

b)从之前的关键帧队列中筛选与关键帧D_i有视角重合的n个关键帧集合，这些关键帧的对象测量的集合记为y_c＝{y_c1，...，y_cn}.对每一个出现在关键帧D_i的对象，将其与y_c中所有同类别的对象测量利用层次主题模型Hierarchical Dirichlet Process计算关联概率，根据概率是否大于阈值来判断两个对象是否关联；

c)对当前系统中的每一个对象，构造其与相关的关键帧还有地图点的因子图，利用基于图优化的库g2o(general Graphic Optimization)对对象位姿、相机位姿和地图点位置进行捆集调整优化；

5)如果检测到关键帧回环，进行回环矫正，并更新相关物体位姿；

6)物体关联操作执行后，实时绘制带有三维位姿物体、相机运动轨迹的地图。

本发明的技术构思为：对关键帧图像利用事先训练好的深度学习模型进行物体类别和位姿检测，获得对象测量。利用层次主题模型Hierarchical Dirichlet Process对每一个关键帧中的物体对象测量建立模型，通过吉布斯采样方法采样获得当前关键帧视角范围内出现的真实环境对象的集合。通过基于层次主题模型的物体关联方法，给关键帧中的每一个物体测量分配唯一物体索引。此外，利用因子图优化物体位姿，相机位姿，地图点的位置。最后实时绘制带有物体位姿信息的语义地图。

本发明的有益效果主要表现在：引入层次主题模型HDP，能够高精度地实现物体关联，避免了冗余的物体关联(即在视角不重合的关键帧的物体地标之间作对象关联)。通过物体关联和物体优化后得到的物体位姿，能够促进语义SLAM的相机位姿估计，而优化后的物体位姿能够使得物体关联更准确，从而构建更精确的语义地图。

附图说明

图1是层次主题模型的产生式图模型。

图2是关键帧视角重合情况的例子。

图3是基于层次主题模型的语义SLAM对象关联方法的流程图。

具体实施方式

下面结合附图对本发明作进一步描述。

参照图1～图3，一种基于层次主题模型的语义SLAM对象关联方法，包括如下步骤：

1)对相机进行内参标定得到相机的畸变参数和内参矩阵

其中，[x，y]是归一化平面点的坐标，[x_distorted，y_distorted]是畸变后的坐标，k₁，k₂，k₃，p₁，p₂是畸变项；

P为相机内参矩阵，其中f为相机焦距，[O_x，O_y]为主光轴点；

2)利用Single Shot MultiBox Detector(SSD)和Convolutional NeuralNetwork(CNN)构建深度学习网络，训练深度学习模型，完成物体识别和物体位姿预测任务；

3)基于层次主题模型的语义SLAM是在ORB-SLAM2的基础上改进的，机器人运动过程中，相机会捕捉到一系列的图像I_1：T＝{I₁，...，I_T}，对于每一帧图像，利用特征匹配计算相机的位姿x_t以及地图点的三维位置，在此过程中，相机的运动方程表示如下：

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

其中，μ_t是运动测量，在视觉SLAM中没有该测量信息，ω_t是服从均值为0，方差为R_t的高斯分布的噪声，相机的观测方程表示如下：

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

将第一帧图像作为关键帧D₀，之后的图像以前一帧关键帧为参照，将图像信息变化明显的图像帧设为新的关键帧D₁，并添加到关键帧队列，以此类推，假设整个过程中共抽取了k个关键帧，即D_0：k＝{D₀，...，D_k}，D表示关键帧集合；

4)对每一个关键帧D_i作如下操作，0≤i≤k：

a)利用深度学习模型识别关键帧对应的图像上的目标物体，并估计其相对于相机的位姿，则可得到该帧图像的对象测量

其中M_i表示在该帧图像中检测到的物体个数，每一个对象测量

m∈[0，M_i]都包括物体的类别信息

和位姿信息

b)从之前的关键帧队列中筛选与关键帧D_i有视角重合的n个关键帧集合，这些关键帧的对象测量的集合记为y_c＝{y_c1，...，y_cn}.对每一个出现在关键帧D_i的对象，将其与y_c中所有同类别的对象测量利用层次主题模型Hierarchical Dirichlet Process(HDP)计算关联概率，根据概率是否大于阈值来判断两个对象是否关联；以关键帧D_i为例：

关键帧D_i中有k个物体测量，根据图1的层次主题HDP的图模型，关键帧D_i中的任一对象地标L_di的条件概率分布如下，

其中α，γ是超参数，H是一个随机的初始基分布，以基分布H和超参数γ构成Dirichlet过程得到分布G₀，即G₀～DP(γ，H)，m..表示目前环境中的实际物体的数目，m._r是表示与第r个物体关联的对象测量的个数，

表示关键帧d中在位置t观测到的物体测量。对于关键帧D_i中的每一个对象，其潜在的关联对象只会是出现在与关键帧D_i视角有重合的关键帧中的真实环境中的对象，设有M_j个，M_j≤K。每一个真实环境中的对象都有其唯一的索引s，s∈(1，M_j)，示意图如图2所示，黄色圆角矩形代表关键帧，菱形表示关键帧中的对象测量，圆形表示每个对象测量和真实环境中的物体(正方形表示)的对应关系，即索引。而数据关联过程就是给每个对象测量分配索引的过程，表明每一个关键帧中的观测对象是对应与真实环境中的第几个实际对象，以概率模型的形式表示物体关联就是找到真实环境中的实际对象的分布

则任一对象测量的物体关联的条件概率如下：

其中

表示关键帧观测到的真实环境中的实例对象，n_d，k表示第d个关键帧中出现对象k的次数，n_k，t表示观测地图点被指定为对象k的数目，n_k表示分配到主体k的所有地图点的数目，-(d,i)表示排除掉当前第d个关键帧中的第i个地图点，V表示地图点字典的大小。

c)对当前系统中的每一个对象，构造其与相关的关键帧还有地图点为节点的因子图，通过g2o库进行对象位姿、相机位姿和地图点位置的优化。优化是基于相机和地标的测量之间的几何约束设计的，特别的是在本语义SLAM系统中，采用物体作为地标之一，物体和相机的位姿约束是语义SLAM优化的主要特点。优化公式如下：

e_oc＝T_wo-T_wc*T_co (4.4)

其中，T_wo表示世界坐标系下物体的位姿，T_wc表示世界坐标系下相机的位姿，T_co表示相机坐标系下物体的位姿。此公式表示物体测量和相机位姿测量约束的误差。相机位姿测量和地图点测量约束的误差可视为重投影误差，表示如下：

e_cp＝z-P(RS+t) (4.5)

其中，z表示地图点S在图像上的观测，R，t表示当前相机的运动旋转矩阵和平移向量，P为步骤1)中的相机内参矩阵；

5)如果检测到关键帧回环，进行回环矫正，并更新相关物体位姿，点的位置和相机位姿；

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

Claims (2)

1.一种基于层次主题模型的语义SLAM对象关联方法，其特征在于，所述方法包括以下步骤：

1)对相机进行内参标定得到相机的畸变参数和内参矩阵

其中，[x,y]是归一化平面点的坐标，[x_distorted,y_distorted]是畸变后的坐标，k₁,k₂,k₃,p₁,p₂是畸变项；

P为相机内参矩阵，其中f为相机焦距，[O_x,O_y]为主光轴点；

2)利用SSD和CNN构建深度学习网络，训练深度学习模型，完成物体识别和物体位姿预测任务；

3)基于层次主题模型的语义SLAM是在ORB-SLAM2的基础上改进得到的，机器人运动过程中，相机会捕捉到一系列的图像I_1:T＝{I₁,…,I_T}，对于每一帧图像，计算相机的位姿x_t以及地图点的三维位置，在此过程中，相机的运动方程表示如下：

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

其中，μ_t是运动测量，ω_t是服从均值为0，方差为R_t的高斯分布的噪声，相机的观测方程表示如下：

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

将第一帧图像作为关键帧D₀，之后的图像以前一帧关键帧为参照，将图像信息变化明显的图像帧设为新的关键帧D₁，并添加到关键帧队列，以此类推，假设整个过程中共抽取了k个关键帧，即D_0:k＝{D₀,…,D_k},D表示关键帧集合；

4)对每一个关键帧D_i作如下操作，0≤i≤k：

a)利用深度学习模型识别关键帧对应的图像上的目标物体，并估计其相对于相机的位姿，则得到该帧图像的对象测量

其中M_i表示在该帧图像中检测到的物体个数；

b)从之前的关键帧队列中筛选与关键帧D_i有视角重合的n个关键帧集合，这些关键帧的对象测量的集合记为y_c＝{y_c1,…,y_cn}.对每一个出现在关键帧D_i的对象，将其与y_c中所有同类别的对象测量利用层次主题模型计算关联概率，根据概率是否大于阈值来判断两个对象是否关联；

c)对当前系统中的每一个对象，构造其与相关的关键帧还有地图点的因子图，进行对象位姿、相机位姿和地图点位置的优化；

5)如果检测到关键帧回环，进行回环矫正，并更新相关物体位姿；

6)物体关联操作执行后，实时绘制带有三维位姿物体、相机运动轨迹的地图。

2.如权利要求1所述的一种基于层次主题模型的语义SLAM对象关联方法，其特征在于，所述步骤4)中，通过深度学习预测得到的对象测量，利用层次主题模型对对象测量进行建模，建模的过程设计到吉布斯采样方法，根据构造的HDP主题模型进行对象关联计算，并基于对象关联进行相机位姿优化。

Images