基于概率推断的语义SLAM数据关联方法

陈渊博，袁 亮，2，周德勤，于海群，何 丽

(1.新疆大学，乌鲁木齐 830000； 2.北京化工大学，北京 100000； 3.北京煜邦电力技术股份有限公司，北京 100000)

0 引言

精确的定位和建图是机器人自主执行可靠导航任务的基础[1]。传统视觉SLAM(Simultaneous Localization and Mapping)在几何特征不明显或环境变化显著情况下易匹配错误。基于语义信息的SLAM方法借助语义路标实现稳定的匹配和地图构建，近年来得到广泛研究[2-4]。

图像的采集是连续的，而语义路标的提取是离散的，语义数据关联实现环境一致性[5]。语义数据关联的研究多基于低噪声假设或静态扰动，而真实场景中光照、阴影和运动物体的干扰会产生显著的观测噪声和里程计噪声，将导致对语义路标的数据关联产生较大的关联错误，干扰机器人定位并使构建的地图失效[6-8]。

在较大噪声干扰和语义路标数量未知时，本文提出一种改进的非参数推断方法，使用YOLO检测获取语义信息[9]，结合MCMC (Markov Chain Monte Carlo)方法和狄利克雷过程模型进行语义路标的数据关联，关联结果帮助SLAM构建环境的语义地图[10]。针对大噪声扰动下关联模糊问题，该算法使用吉布斯方法对后验概率分布迭代采样，利用随机近似推断优化语义路标间的关联约束，降低语义路标间关联模糊性。本文实验在仿真和KITTI数据集上进行[11]，结果表明，该方法有效提高了语义路标关联的准确性，并实现了机器人的准确定位。

1 语义SLAM模型

1.1 语义视觉SLAM

与几何SLAM模型相比，语义视觉SLAM利用相机获取的语义信息进行位姿估计与优化，相机的位姿信息用x表示，则集合XT表示机器人在全部时间步长T的轨迹，相机在t时间的运动方程为

xt=f(xt-1,ut)+eoeo～N(0,Q)

(1)

式中：ut为运动测量；eo为运动噪声；Q为相机姿态噪声矩阵。当相机位姿为x时，观测到语义路标测量L和特征点测量y，em为观测噪声，R为观测噪声的协方差矩阵，则相机在t时刻的观测方程为

zt=h(xt,yt,Lt)+emem～N(0,R)。

(2)

(3)

(4)

当语义路标数据关联随时间累加，相机的姿态和语义路标位姿不断更新。新的语义路标被检测时，多个类别和外观相似的语义路标受噪声扰动会产生匹配误差，造成数据关联错误，使相机姿态、语义路标姿态估计产生累积误差，因此，提高语义路标数据关联的准确性能够有效提高机器人的定位精度。

1.2 语义数据关联

机器人在环境中运动时，路标类别和数量先验未知，文献[8]提出利用狄利克雷过程(DP)模型进行非参数推断。假定机器人在t时刻观测到M个语义路标，每个语义路标测量Lt,k与语义路标i关联的先验概率为

(5)

式中：mi为语义路标i的已有关联对象数；狄利克雷先验α表示将观测对象作为新语义路标的概率参数。则语义路标的先验关联概率服从狄利克雷分布，有

dt,k～DP(α)

(6)

在语义信息的获取中，获取的语义信息不是绝对可靠的，通过类别概率分布减小关联误差。对于语义观测Lt,k，类别分布模型用π表示，类别分布的狄利克雷先验参数为β，有语义路标类别分布模型

(7)

πLt,k～Dir(βLt,k)。

(8)

(9)

2 语义数据关联算法

2.1 算法整体流程

针对语义路标数据关联中离散路标估计和连续位姿优化问题，本文通过改进概率推断方法实现机器人语义路标的数据关联。根据狄利克雷先验和观测似然得到目标后验概率分布，在关联时利用吉布斯方法进行迭代优化，得到最优关联目标并优化机器人位姿。概率推断数据关联整体算法流程如图1所示。

图1 整体算法流程

2.2 概率推断数据关联算法

语义路标在低噪声假设中的里程计测量和物体观测误差小，语义路标关联的后验概率分布中存在唯一的最大似然目标。真实环境中，里程计和图像观测噪声扰动大，遮挡、重叠和光照变化使存在多个语义对象的后验概率相近，关联的模糊性增加。针对这一问题，本文提出利用吉布斯采样原理对后验概率分布进行迭代处理，该算法通过构造满足遍历理论的马尔可夫链，对状态空间中的变量x进行随机采样，样本分布随累计趋于平滑，得到平稳的目标分布，样本函数的均值趋于函数的期望值，即

(10)

式中,MC(p)表示满足马尔可夫原理的马尔可夫链。语义路标的概率数据关联针对多目标变量，吉布斯采样方法通过固定其他维度信息对单维对象构造马尔可夫链，利用多次迭代得到逼近真实后验概率中的最优特征值，采样原理为

(11)

di,k=AML(Pi)。

(12)

通过吉布斯方法进行迭代采样，优先关联模糊度小的语义对象，利用语义目标关联信息增加约束，降低模糊目标错误关联概率，得到趋近目标的联合分布和相对准确的关联，具体概率数据关联算法如下。

Input：相机位姿X，相机观测Z

Output：关联状态D，语义路标数量M和语义路标测量L(Lc,Lp)

1 begin

3 for每个吉布斯采样步长T

4 for 每个语义观测，执行

5 式(9)得到后验概率分布Pi

6 执行式(12)得到近似关联对象

7 end for

8 执行式(4)和式(5)更新关联状态D和语义路标测量L

9 end for。

3 实验

本文实验在PowerEdge R640上进行，处理器为Intel®Xeon®Silver 4110 CPU @2.1 GHz，内存为16GiB DDR4 RDIMM，实验操作系统为Ubuntu 16.04。仿真数据集实验中，使用文献[8]中构建的数据集并构建新的具有高模糊度的数据集。在KITTI数据集实验中，将提出的SLAM算法与DP算法、ORB SLAM2算法在道路场景下进行实验验证。

3.1 仿真实验及分析

仿真数据集包含15个属于5类语义类别的路标，初始1098个语义路标随机分布在二维平面内。假定噪声服从高斯分布，观测噪声为m～N(0,0.12)，里程计噪声为o～N(0,0.022)时，本文算法和DP算法的数据关联结果如图2和图3所示。图2(e)表示环境真实位姿(Ground Truth,GT)，该实验条件下提出的算法经过3次迭代能够收敛到15个语义路标，如图2(d)所示。DP方法平均需要4次，收敛结果如图3(e)所示。通过图2和图3迭代结果和原始数据比较可得，本文算法在低噪声扰动下有准确的关联和优化结果。

图2 本文算法仿真数据集数据关联结果

图3 DP算法仿真数据集数据关联结果

为评估所提算法在相似场景下的性能，构建含有多个相似语义路标的数据集。此数据集包含15个属于同一类别的语义路标，相机轨迹和语义路标位姿的真实分布如图4(a)所示，图4(b)为观测噪声m～N(0,0.12)和里程计噪声o～N(0,0.022)情况下的初始观测数据，包含937个语义路标获取。该数据集实验中，本文算法能在两次迭代后收敛到15个语义路标，如图4(d)所示。

图4 本文算法在创建的数据集下的数据关联结果

为验证提出算法的鲁棒性，设定多组里程计噪声和观测噪声进行对比实验，9次迭代的结果如图5所示。蓝色实线是本文算法的迭代结果，红色和黄色分别表示DP和HDP(Hierarchical Dirichlet Process)算法[13]得到的结果，绿色虚线表示语义路标真实数量。

结果表明，提出的算法能更快地实现稳定收敛。综上所述，本文算法在SLAM语义数据关联中具有较高的准确性和鲁棒性，在不同的噪声扰动下关联准确性高。从仿真数据集实验的关联结果图可以看出，语义路标在数据关联过程中优化位姿，并帮助机器人提高定位精度。

3.2 KITTI数据集实验及分析

在真实场景实验中，使用KITTI中的单目数据进行验证。所选道路场景包含足够的语义物体，场景中出现的车辆作为进行数据关联的语义路标，使用YOLO检测提取语义路标信息，语义路标检测结果如图6所示。

图6 语义路标检测结果

KITTI数据集运行轨迹均方根误差(RMSE)对比如表1所示。对比算法为基于DP算法的语义SLAM算法和基于几何特征的ORB SLAM2算法，实验结果使用绝对轨迹误差(Absolute Trajectory Error，ATE)评估。其中,RMSE越小，说明运行的轨迹越接近真实轨迹，即机器人定位精度越高。

表1 KITTI数据集运行轨迹误差对比Table 1 Comparison of trajectory errors in KITTI dataset

从表1结果可知，本文算法的均方根误差在不同序列数据集上均有所提升，表明本文算法具有更高的轨迹一致性和定位精度。

在序列00，05，07和10中，存在足够语义路标实现数据关联，准确的关联帮助机器人优化定位。从表1可得，本文算法在序列00，05，07和10上对机器人运动估计的精度分别提高了10.77%，21.30%，10.53%和6.07%。轨迹估计结果如图7所示。以上实验结果表明本文算法的运行结果与真实轨迹一致性高，本文语义数据关联方法消除了相机位姿偏差，提高了机器人定位精度。

图7 本文算法、DP和ORB SLAM2算法运行轨迹

4 结束语

针对噪声扰动下语义SLAM数据关联模糊性问题，采用随机近似推断方法降低关联的不确定性。本文提出的语义SLAM方法结合了Dirichlet方法和吉布斯采样关联语义路标。静态仿真环境中，该方法在不同扰动下能准确关联语义路标。在KITTI数据集实验中，该方法得到的关联结果提高了机器人和语义路标的定位精度。随着语义信息的增加，语义约束条件增多，数据关联的模糊性降低。当机器人在真实场景中运动时，环境中包含多类移动语义对象，下一步工作将针对多类运动语义路标提取展开，构建用于导航的高精度语义地图。