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

1.一种基于层次主题模型的语义SLAM对象关联方法，其特征在于，所述方法包括以下
步骤：
1)对相机进行内参标定得到相机的畸变参数和内参矩阵
其中，[x,y]是归一化平面点的坐标，[xdistorted,ydistorted]是畸变后的坐标，k1,k2,k3,p1,
p2是畸变项；
P为相机内参矩阵，其中f为相机焦距，[Ox,Oy]为主光轴点；
2)利用SSD和CNN构建深度学习网络，训练深度学习模型，完成物体识别和物体位姿预
测任务；
3)基于层次主题模型的语义SLAM是在ORB‑SLAM2的基础上改进得到的，机器人运动过
程中，相机会捕捉到一系列的图像I1:T＝{I1,…,IT}，对于每一帧图像，计算相机的位姿xt以
及地图点的三维位置，在此过程中，相机的运动方程表示如下：
xt＝f(xt‑1,μt)+ωt,ωt～N(0,Rt)              (3.1)
其中，μt是运动测量，ωt是服从均值为0，方差为Rt的高斯分布的噪声，相机的观测方程
表示如下：
zt＝h(xt,yt)+vt,vt～N(0,Qt)                 (3.2)
将第一帧图像作为关键帧D0，之后的图像以前一帧关键帧为参照，将图像信息变化明显
的图像帧设为新的关键帧D1，并添加到关键帧队列，以此类推，假设整个过程中共抽取了k
个关键帧，即D0:k＝{D0,…,Dk},D表示关键帧集合；
4)对每一个关键帧Di作如下操作，0≤i≤k：
a)利用深度学习模型识别关键帧对应的图像上的目标物体，并估计其相对于相机的位
姿，则得到该帧图像的对象测量 其中Mi表示在该帧图像中检测到的物体
个数；
b)从之前的关键帧队列中筛选与关键帧Di有视角重合的n个关键帧集合，这些关键帧的
对象测量的集合记为yc＝{yc1,…,ycn}.对每一个出现在关键帧Di的对象，将其与yc中所有
同类别的对象测量利用层次主题模型计算关联概率，根据概率是否大于阈值来判断两个对
象是否关联；
c)对当前系统中的每一个对象，构造其与相关的关键帧还有地图点的因子图，进行对
象位姿、相机位姿和地图点位置的优化；
5)如果检测到关键帧回环，进行回环矫正，并更新相关物体位姿；
6)物体关联操作执行后，实时绘制带有三维位姿物体、相机运动轨迹的地图。
2.如权利要求1所述的一种基于层次主题模型的语义SLAM对象关联方法，其特征在于，
所述步骤4)中，通过深度学习预测得到的对象测量，利用层次主题模型对对象测量进行建
模，建模的过程设计到吉布斯采样方法，根据构造的HDP主题模型进行对象关联计算，并基
于对象关联进行相机位姿优化。

一种基于层次主题模型的语义SLAM对象关联方法
技术领域
[0001] 本发明涉及机器人视觉、深度学习、统计学等技术领域，具体一种基于层次主题模
型的语义SLAM对象关联方法。
背景技术
[0002] Simultaneous localization and mapping(SLAM)是机器人应用中的一个重要的
问题，比如自动驾驶、自主导航等领域。构建精准的环境地图是SLAM的具体表现形式，传统
的SLAM技术依赖于低级的几何特征，例如点、线、面，这种技术在空旷的或者由重复纹理的
环境下容易失效。而语义SLAM利用环境中的高级语义信息，可以有效的弥补传统SLAM的不
足，并且能够建立可读的，更具应用价值的语义地图。
[0003] 物体关联和物体位姿优化是语义SLAM中两个至关重要的组成部分。要建立精确的
语义地图，准确的物体关联是前提。准确的物体关联依赖于准确的物体测量，包括物体的类
别和位姿。但是实际中，随着机器人的运动，传感器捕获的信息是带有噪声的，仅依靠传感
器的信息估计机器人的运动是不可靠的，所以需要多种优化算法的辅助。优化算法是设计
一般是基于获取的地标信息和相机之间的几何约束创造的，最大化合理高效的利用已有测
量信息是评价优化算法的主要标准。
[0004] 因此，如何设计一种稳定可靠的语义SLAM中的对象关联方法以及优化方法，使得
语义SLAM算法能够构建一个准确的语义地图，是实现有效的语义SLAM需要解决的问题。
发明内容
[0005] 本发明的目的是为语义SLAM设计一种稳定有效的语义对象关联和优化的方法，解
决语义SLAM中的基本问题从而构建准确的语义地图。
[0006] 为了解决上述技术问题，本发明提供如下的技术方案：
[0007] 一种基于层次主题模型的语义SLAM对象关联方法，包括以下步骤：
[0008] 1)对相机进行内参标定得到相机的畸变参数和内参矩阵
[0009]
[0010] 其中，[x，y]是归一化平面点的坐标，[xdistorted，ydistorted]是畸变后的坐标，k1，k2，
k3，p1，p2是畸变项；
[0011]
[0012] P为相机内参矩阵，其中f为相机焦距，[Ox，Oy]为主光轴点；
[0013] 2)利用Single Shot MultiBox Detector(SSD)和Convolutional Neural 
Network(CNN)构建深度学习网络，训练深度学习模型，完成物体识别和物体位姿预测任务；
[0014] 3)基于层次主题模型的语义SLAM是在ORB‑SLAM2的基础上改进的，机器人运动过
程中，相机会捕捉到一系列的图像I1：T＝{I1，...，IT}，对于每一帧图像It，计算相机的位姿
xt以及地图点的三维位置，在此过程中，相机的运动方程表示如下：
[0015] xt＝f(xt‑1，μt)+ωt，ωt～N(0，Rt)   (3.1)
[0016] 其中，μt是运动测量，在视觉SLAM中没有该测量信息，ωt是服从均值为0，方差为Rt
的高斯分布的噪声，相机的观测方程表示如下：
[0017] zt＝h(xt，yt)+vt，vt～N(0，Qt)   (3.2)
[0018] 将第一帧图像作为关键帧D0，之后的图像以前一帧关键帧为参照，将图像信息变
化明显的图像帧设为新的关键帧D1，并添加到关键帧队列，以此类推，假设整个过程中共抽
取了k个关键帧，即D0：k＝{D0，...，Dk}，D表示关键帧集合；
[0019] 4)对每一个关键帧Di作如下操作，0≤i≤k：
[0020] a)利用步骤2)深度学习模型识别关键帧对应的图像上的目标物体，并估计其相对
于相机的位姿，则得到该帧图像的对象测量 其中Mi表示在该帧图像中检
测到的物体个数；
[0021] b)从之前的关键帧队列中筛选与关键帧Di有视角重合的n个关键帧集合，这些关
键帧的对象测量的集合记为yc＝{yc1，...，ycn}.对每一个出现在关键帧Di的对象，将其与yc
中所有同类别的对象测量利用层次主题模型Hierarchical Dirichlet Process计算关联
概率，根据概率是否大于阈值来判断两个对象是否关联；
[0022] c)对当前系统中的每一个对象，构造其与相关的关键帧还有地图点的因子图，利
用基于图优化的库g2o(general Graphic Optimization)对对象位姿、相机位姿和地图点
位置进行捆集调整优化；
[0023] 5)如果检测到关键帧回环，进行回环矫正，并更新相关物体位姿；
[0024] 6)物体关联操作执行后，实时绘制带有三维位姿物体、相机运动轨迹的地图。
[0025] 本发明的技术构思为：对关键帧图像利用事先训练好的深度学习模型进行物体类
别和位姿检测，获得对象测量。利用层次主题模型Hierarchical Dirichlet Process对每
一个关键帧中的物体对象测量建立模型，通过吉布斯采样方法采样获得当前关键帧视角范
围内出现的真实环境对象的集合。通过基于层次主题模型的物体关联方法，给关键帧中的
每一个物体测量分配唯一物体索引。此外，利用因子图优化物体位姿，相机位姿，地图点的
位置。最后实时绘制带有物体位姿信息的语义地图。
[0026] 本发明的有益效果主要表现在：引入层次主题模型HDP，能够高精度地实现物体关
联，避免了冗余的物体关联(即在视角不重合的关键帧的物体地标之间作对象关联)。通过
物体关联和物体优化后得到的物体位姿，能够促进语义SLAM的相机位姿估计，而优化后的
物体位姿能够使得物体关联更准确，从而构建更精确的语义地图。
附图说明
[0027] 图1是层次主题模型的产生式图模型。
[0028] 图2是关键帧视角重合情况的例子。
[0029] 图3是基于层次主题模型的语义SLAM对象关联方法的流程图。
具体实施方式
[0030] 下面结合附图对本发明作进一步描述。
[0031] 参照图1～图3，一种基于层次主题模型的语义SLAM对象关联方法，包括如下步骤：
[0032] 1)对相机进行内参标定得到相机的畸变参数和内参矩阵
[0033]
[0034] 其中，[x，y]是归一化平面点的坐标，[xdistorted，ydistorted]是畸变后的坐标，k1，k2，
k3，p1，p2是畸变项；
[0035]
[0036] P为相机内参矩阵，其中f为相机焦距，[Ox，Oy]为主光轴点；
[0037] 2)利用Single Shot MultiBox Detector(SSD)和Convolutional Neural 
Network(CNN)构建深度学习网络，训练深度学习模型，完成物体识别和物体位姿预测任务；
[0038] 3)基于层次主题模型的语义SLAM是在ORB‑SLAM2的基础上改进的，机器人运动过
程中，相机会捕捉到一系列的图像I1：T＝{I1，...，IT}，对于每一帧图像，利用特征匹配计算
相机的位姿xt以及地图点的三维位置，在此过程中，相机的运动方程表示如下：
[0039] xt＝f(xt‑1，μt)+ωt，ωt～N(0，Rt)   (3.1)
[0040] 其中，μt是运动测量，在视觉SLAM中没有该测量信息，ωt是服从均值为0，方差为Rt
的高斯分布的噪声，相机的观测方程表示如下：
[0041] zt＝h(xt，yt)+vt，vt～N(0，Qt)   (3.2)
[0042] 将第一帧图像作为关键帧D0，之后的图像以前一帧关键帧为参照，将图像信息变
化明显的图像帧设为新的关键帧D1，并添加到关键帧队列，以此类推，假设整个过程中共抽
取了k个关键帧，即D0：k＝{D0，...，Dk}，D表示关键帧集合；
[0043] 4)对每一个关键帧Di作如下操作，0≤i≤k：
[0044] a)利用深度学习模型识别关键帧对应的图像上的目标物体，并估计其相对于相机
的位姿，则可得到该帧图像的对象测量 其中Mi表示在该帧图像中检测到
的物体个数，每一个对象测量 m∈[0，Mi]都包括物体的类别信息 和位姿信息
[0045] b)从之前的关键帧队列中筛选与关键帧Di有视角重合的n个关键帧集合，这些关
键帧的对象测量的集合记为yc＝{yc1，...，ycn}.对每一个出现在关键帧Di的对象，将其与yc
中所有同类别的对象测量利用层次主题模型Hierarchical Dirichlet Process(HDP)计算
关联概率，根据概率是否大于阈值来判断两个对象是否关联；以关键帧Di为例：
[0046] 关键帧Di中有k个物体测量，根据图1的层次主题HDP的图模型，关键帧Di中的任一
对象地标Ldi的条件概率分布如下，
[0047]
[0048]
[0049] 其中α，γ是超参数，H是一个随机的初始基分布，以基分布H和超参数γ构成
Dirichlet过程得到分布G0，即G0～DP(γ，H)，m..表示目前环境中的实际物体的数目，m.r是
表示与第r个物体关联的对象测量的个数， 表示关键帧d中在位置t观测到的物体测量。
对于关键帧Di中的每一个对象，其潜在的关联对象只会是出现在与关键帧Di视角有重合的
关键帧中的真实环境中的对象，设有Mj个，Mj≤K。每一个真实环境中的对象都有其唯一的索
引s，s∈(1，Mj)，示意图如图2所示，黄色圆角矩形代表关键帧，菱形表示关键帧中的对象测
量，圆形表示每个对象测量和真实环境中的物体(正方形表示)的对应关系，即索引。而数据
关联过程就是给每个对象测量分配索引的过程，表明每一个关键帧中的观测对象是对应与
真实环境中的第几个实际对象，以概率模型的形式表示物体关联就是找到真实环境中的实
际对象的分布 则任一对象测量的物体关联的条件概率如下：
[0050]
[0051] 其中 表示关键帧观测到的真实环境中的实例对象，nd，k表示第d个关键帧中出现
对象k的次数，nk，t表示观测地图点被指定为对象k的数目，nk表示分配到主体k的所有地图
点的数目，‑(d,i)表示排除掉当前第d个关键帧中的第i个地图点，V表示地图点字典的大
小。
[0052] c)对当前系统中的每一个对象，构造其与相关的关键帧还有地图点为节点的因子
图，通过g2o库进行对象位姿、相机位姿和地图点位置的优化。优化是基于相机和地标的测
量之间的几何约束设计的，特别的是在本语义SLAM系统中，采用物体作为地标之一，物体和
相机的位姿约束是语义SLAM优化的主要特点。优化公式如下：
[0053] eoc＝Two‑Twc*Tco   (4.4)
[0054] 其中，Two表示世界坐标系下物体的位姿，Twc表示世界坐标系下相机的位姿，Tco表
示相机坐标系下物体的位姿。此公式表示物体测量和相机位姿测量约束的误差。相机位姿
测量和地图点测量约束的误差可视为重投影误差，表示如下：
[0055] ecp＝z‑P(RS+t)   (4.5)
[0056] 其中，z表示地图点S在图像上的观测，R，t表示当前相机的运动旋转矩阵和平移向
量，P为步骤1)中的相机内参矩阵；
[0057] 5)如果检测到关键帧回环，进行回环矫正，并更新相关物体位姿，点的位置和相机
位姿；
[0058] 6)随着物体关联操作和优化操作的执行，实时更新绘制带有三维位姿的物体、相
机运动轨迹的地图。