CN113160401A - 一种面向物体的视觉slam轻量化语义地图创建方法 - Google Patents

Abstract

本发明公开了一种面向物体的视觉SLAM轻量化语义地图创建方法，通过几何SLAM和语义目标检测为图像中物体分配对应的三维点云以初始化物体；采用非参数统计和参数统计进行物体数据关联以更新物体数据库；利用对离异点鲁棒的孤立森林算法估计物体初始位置和尺寸，通过线段对齐算法估计物体方向，并同相机位姿一起联合优化得到准确的物体9自由度位姿；采用三维立方框和二次曲面对物体进行建模，创建面向物体的轻量化语义地图。本发明克服了多物体、多类别、无规律环境下的数据关联与物体数据库更新存在的困难，并解决了在相机运动快速、物体遮挡情况下的物***姿估计，极大地提升了机器人对环境的理解能力。

Description

一种面向物体的视觉SLAM轻量化语义地图创建方法

技术领域

本发明属于计算机视觉及机器人领域，具体涉及一种面向物体的视觉SLAM轻量化语义地图创建方法。

背景技术

基于视觉SLAM创建的语义地图是将相机获取的环境三维几何信息与通过语义分割或目标检测方法获取的语义标签叠加而生成的。不同于传统的几何地图仅能满足基本的定位需求，语义地图能为机器人提供语义导航、定位、环境理解等更高级别、更智能化的功能。当前语义地图构建存在诸多挑战：1)数据关联与全局物体数据库更新不准确。数据关联用于确定当前时刻检测到的物体与地图数据库中已存在或其他时刻观测到的物体的对应关系，在多物体、多类别、无规律环境和相机观测视角不佳的情况下的语义数据关联存在很大困难，从而导致全局物体数据库构建与更新不准确。2)物***姿估计不准确。仅用物体点云质心表达物***置的方法的环境表达能力太弱，由于相机快速运动、物体遮挡和噪点的存在使得估计物体的方向和尺寸极为困难。3)地图存储效率低。像素级稠密语义地图创建成本高、存储量大、不易更新且存在大量无用的信息。因此，既能满足环境语义表达又能满足实际应用需求的语义地图应该是面向物体的轻量化地图，其关键问题在于鲁棒的物体级数据关联算法、准确的物***姿估计算法和恰当的物体建模方法。

发明内容

针对以往语义地图创建方法在数据关联、物***姿估计和物体建模上的不足，本发明提出了一种面向物体的视觉SLAM轻量化语义地图构建方法。首先对数据关联与物体数据库阶段的统计量分布进行建模，对服从高斯分布和不服从高斯分布的统计量分别使用参数统计和非参数统计进行关联，以创建物体信息库。然后利用对外点鲁棒的孤立森林算法和线段对齐算法估计物***置、尺寸和方向。最后将物体9自由度位姿用三维立方框和二次曲面表达，创建轻量化物体级语义地图。该方法能实现鲁棒的语义数据关联，避免全局物体数据库的错误和遗漏，并创建面向物体9自由度位姿的语义地图，提升机器人环境感知能力，以服务于导航、抓取、交互等智能化应用场景。

本发明的技术方案是：1)首先根据并行运行Yolo v3目标检测网络和视觉SLAM***给物体分配对应的三维点云和语义标签。2)对每个物体三维点云和三维质心的分布进行建模，利用非参数统计和参数统计进行数据关联以更新全局物体数据库。3)利用孤立森林算法和线段对齐算法确定物体尺寸和方向并进行优化。4)将估计的物体9自由度位姿使用立方框和二次曲面进行建模，以创建面向物体的轻量化语义地图。

面向物体的视觉SLAM轻量化语义地图构建方法，包括如下步骤：

步骤一，几何-语义度量初始化物体：并行执行几何视觉SLAM和Yolo v3语义目标检测，为物体分配三维点云和语义标签。

几何SLAM首先对图像提取二维ORB特征点，通过多视图三角化获得其对应的三维点云，并使用Yolo v3目标检测网络得到物体在图像中的二维检测框，通过检测框和特征点的位置关系为物体分配三维点云，并将点云均值作为物体质心。

步骤二，物体数据库构建与更新：根据物体中点云与质心的统计量分布，采用非参数统计和参数统计进行物体数据关联并更新物体数据库。

数据库构建与更新涉及的统计量如图1所示，包括当前帧观测到的某物体的点云集合

地图数据库中已存在的某物体的点云集合

当前帧观测到的某物体的点云质心

地图数据库中已存在的某物体的质心集合

和地图数据库中两个物体的质心集合C₁，C₂。

1)非参数统计。受相机观测视角、运动、物体遮挡和物体形状的影响，物体所包含的三维点云往往不服从高斯分布，而非参数统计的优势便是能处理统计量不服从高斯分布的情况，所以在本算法中采用非参数统计验证两组分别来自当前帧观测到的物体的点云和地图数据库中已存在的物体的点云P，Q之间的关系。理论上，如果P，Q来自同一个物体，则他们的分布将相同，即f_P＝f_Q，如图1(a)所示。

首先将两组点云混合，得到

构造统计量

其中R(·)是求秩操作。定义Mann-Whitney统计量W＝min(W_p,W_q)，可以证明统计量W服从高斯分布，从而本算法使用高斯统计量来建模非高斯的点云。然后计算统计量W的均值和方差：

m(W)＝(|P|+|Q|)/2 (2)

其中Δ＝|P|+|Q|+1，τ∈P∩Q。

2)单样本t检验。在统计量不服从高斯分布的情况下使用非参数检验更合理，但在统计量服从高斯分布的情况下，参数统计的准确性更高。在本算法中利用单样本t检验验证当前帧观测到的物体质心和地图数据库中已存在的物体质心之间的关系。如图1(b)所示，假设当前帧中物体的质心为c，地图数据库中物体的质心集合为C，对二者构造t统计量：

其中m(·)，σ(·)分别是求均值和方差操作。

3)双样本t检验。由于相机视角、遮挡等原因可能导致前两个步骤存在漏关联的情况。本算法中采用双样本t检验验证地图数据库中已存在的两个物体是否应属于同一个物体。如图1(c)所示，假设两个物体的质心集合分别为C₁，C₂，则对它们构造如下的t统计量：

对于以上三种检验方法，如果构造的统计量在置信区间内，则认为数据关联成功，更新数据库中的就物体，否则关联失败，在数据库中创建一个新物体。

步骤三，物体参数化：数据库更新之后利用孤立森林算法估计物体中心和尺寸，并利用线段对齐算法估计物体方向。

1)估计物体中心和尺寸。孤立森林算法是一种非参数方法，适用于不服从高斯分布的点云，其思想是用随机超平面对数据空间进行分割，将容易被孤立的点视为异常点。如图2所示，图2(a)中矩形的点在分割4次之后被孤立，图2(b)中矩形的点被分割8次被孤立，所以将图2(a)中矩形的点视为异常点。对物体点云集合X中的每个点云x计算其异常值得分s(x)：

其中E(h(x))为点云样本x在孤立树中路径的期望，H(i)＝ln(i)+γ为调和数。剔除得分大于阈值的点云，将剩余点云的均值作为物体中心t，将点云在三个方向上的长度一段作为尺寸s＝(max(X)-min(X))/2，其中max(·)，min(·)分别是求最大值和最小值操作。

2)估计物体方向。首先对图像提取LSD线段，并将落在物体检测框中的线段分配给物体，如图3(a)所示。然后计算每条二维LSD线段的角度α(Z_lsd)。然后将物体点云在空间中构成三维立方框投影到图像中，如图3(e-f)所示，得到立方框投影线段的角度α(Z(θ))，计算二者角度的差值e(θ)作为误差：

e(θ)＝||α(Z(θ))-α(Z_lsd)||² (9)

Z(θ)＝KP_c(R(θ)Z+t) (10)

其中θ表示物体的方向，Z表示立方框的一条三维边，Z(θ)表示立方框的边在图像中的投影，其角度记为α(Z(θ))，K表示相机内参，(P_c,t)表示相机外参。θ偏航角从0度开始，以物体为中心每隔λ度旋转一次，如图3(b-d)所示，每次计算一次角度得分：

其中N_p表示LSD线段平行于投影线段的数量，N_a表示LSD线段的总数，E(e)是一系列e(θ)的均值，ξ为误差阈值。最后将得分最高时的角度θ作为物体的方向。

3)物体尺寸和方向优化。物体角度误差为e(θ)，物体尺寸误差定义为投影边到最边缘的LSD线段的距离e(s)，将物***姿与相机位姿进行联合优化，代价函数为：

步骤四，轻量化建模：估计物***姿之后利用三维立方框和二次曲面建模物体。

一方面为更好地表达物***姿，另一方面为更好地描述物体形状，本发明算法利用通用的立方框和二次曲面建模物体。已知物体中心t，尺寸s和方向θ，可以将立方框和二次曲面参数化为：

P_w＝R(θ)P_o+t (13)

Q_w＝TQ_oT^T (14)

其中

分别表示在物体坐标系和世界坐标系中立方框8个顶点的坐标，顶点与t和s有关。

分别表示在物体坐标系和世界坐标系中二次曲面的参数，且

表示在物体坐标系下三个半轴分别为s_l,s_w,s_h、中心为坐标系原点的二次曲面。物体模型参数化示意图如图4所示。

本发明的有益效果为充分考虑了多物体、多类别、无规律环境下的数据关联与全局物体数据库，并针对相机快速运动、物体遮挡提出鲁棒的外点剔除算法和线段对齐算法以确定物体9自由度位姿，最终利用轻量化的立方框和二次曲面进行建模，构建语义地图。极大地提升了机器人在运动过程中对物体进行位姿估计的准确性和环境理解能力，有助于开展更智能化的语义导航、抓取和交互等工作。

附图说明

图1本发明数据关联与物体数据库更新阶段统计量示意图；(a)非参数检验示意图；(b)单样本t检验示意图；(c)双样本t检验示意图。

图2本发明孤立森林算法示意图；(a)四次分割示意图；(b)八次分割示意图。

图3本发明线段对齐算法估计物体方向示意图；(a)对图像进行目标检测和线段检测；(b、c、d)三维空间中物体绕自身旋转；(e、f、g)物体立方框投影到图像中计算角度误差。

图4本发明物体模型参数化示意图；(a)特征提取与目标检测；(b)物体点云分配。

图5本发明具体实施方式的算法执行流程示意图。

图6本发明具体实施方式的为物体分配三维点云示意图。

图7本发明具体实施方式的物体点云和质心分布图；(a)物体点云分布图；(b)物体质心分布图。

图8本发明具体实施方式的物***姿估计示意图；(a)初始物体3D框；(b)孤立森林算法处理之后的3D框；(c)线段对齐之后的3D框。

图9本发明具体实施方式的面向物体轻量化语义地图创建效果图。(a,b,c)相机运动过程中选取的三个视角的图像；(d)最终创建的轻量化语义地图。

具体实施方式

本实施方式的算法的流程如附图5所示：

步骤一：逐帧对图像进行目标检测和特征提取，根据特征点和检测框的关系，为图像中的每个物体分配三维点云和语义标签，如图6所示。当地图中不存在物体时，用当前帧的第一个物体初始化物体地图数据库，否则进行数据关联更新物体数据库。

步骤二：数据库更新阶段用到的统计量包括当前帧物体的点云集合、物体质心；地图中物体的点云集合、物体质心集合。对统计量的分布进行建模，如图7所示，物体点云不服从高斯分布，物体质心服从高斯分布，分别对其采用非参数统计和参数统计。首先对当前帧中某物体的点云集合和地图中某物体的点云集合根据公式(1)构造非参数Mann-Whitney统计量W，若W满足以下公式则关联成功：

f(W)≥f(r₁)＝f(r₂)＝α/2 (15)

其中α是显著度水平，具体实施时取为0.025，

为置信区间，m,σ分别为均值和方差，s是标准正态分布的α分位数，具体实施时取为1.96。

然后对当前帧中某物体的质心和地图中某物体的质心集合C根据公式(4)构造单样本t统计量，若t满足以下公式则关联成功：

f(t)≥f(t_α/2,v)＝α/2 (16)

其中t_α/2,v是自由度为

时t分布的上α/2分位值，α是显著度水平，具体实施时取为0.05。

最后对地图中两个物体的质心集合C₁,C₂根据公式(5)构造双样本t统计量，若在v＝|C₁|+|C₂|-2时t满足公式(16)，则合并两个物体。

步骤三：准确的物体数据库更新将为基于点云的物***姿估计提供更多的样本。首先利用孤立森林算法，以物体的点云集合X为输入，执行公式(7)，得到每个点云的异常得分，其中孤立树的数量t取为100，采样的大小ψ＝min(256,|X|)。剔除得分大于0.6的点云，将剩余点云的均值作为物体中心，范围作为物体的尺寸。

在确定中心和尺寸之后通过偏航角采样进行线段对齐，输入为一系列图像的LSD线段z₁,z₂,…,z_t和物体的初始角度θ₀＝0°，通过执行公式(9，10，11)得到得分最高时的角度θ作为物体方向，具体实施时每次的旋转角度λ＝5°。

最后通过公式(12)进行联合优化，进一步细化尺寸和方向。如图8(a)为物体初始位姿，图8(b)为使用孤立森林算法之后得到的中心和尺寸，图8(c)为使用线段对齐和优化之后得到的物***姿，其中估计的物***姿用立方框表示，并叠加在半稠密地图中以定性观察建模效果。

步骤四：通过前面的三个步骤获得了物体的中心、尺寸和方向9自由度位姿，分别根据物体形状用三维立方框，公式(13)，以及二次曲面，公式(14)建模，对于具有明确方向且较规则的物体比如书本、键盘、椅子等用立方框表示，对其余方向感不强且形状类似于椭球体的物体比如球、水瓶、杯子等使用二次曲面来表示。如图9为桌面环境中物体的轻量化建图结果。

Claims (5)

1.一种面向物体的视觉SLAM轻量化语义地图创建方法，其特征在于，包括步骤如下：

步骤一，几何-语义度量初始化物体：并行执行几何视觉SLAM和Yolo v3语义目标检测，为物体分配三维点云和语义标签；其中几何SLAM模块关联二维特征点和三维点云，语义检测模块关联二维特征点和目标边界框，从而为语义物体分配几何三维点云，并计算质心，以初始化物体；

步骤二，物体数据库构建与更新：为物体点云集合和质心集合的分布进行验证，采用非参数统计和参数统计关联帧间物体和地图数据库中物体，实现数据库创建与更新，分别采用非参数统计关联当前帧中物体点云和地图数据库中物体点云，采用单样本t检验关联当前帧中物体质心和地图数据库中物体质心，采用双样本t检验融合地图数据库中重复的两个物体，构建准确且鲁棒的全局物体数据库；

步骤三，物体参数化：数据关联之后估计物***姿；对不服从高斯分布的点云采用孤立森林算法计算其得分，剔除离异值，初始化物***置和方向，并采样物体偏航角通过线段对齐算法估计物体方向，最终同相机位姿一起联合优化，得到准确的9自由度位姿；

步骤四，轻量化建模：利用获得的9自由度位姿分别采用三维立方框和二次曲面建模不同形状的物体，创建最终的面向物体的轻量化语义地图。

2.根据权利要求1所述的面向物体的视觉SLAM轻量化语义地图创建方法，其特征在于，步骤一：首先SLAM模块对图像提取二维ORB特征点，通过多视图观测三角化获得其对应的三维点云，并使用Yolo v3目标检测网络得到物体在图像中的二维检测框，通过检测框和特征点的位置关系为物体分配三维点云，并将点云均值作为物体质心。

3.根据权利要求1所述的面向物体的视觉SLAM轻量化语义地图创建方法，其特征在于，步骤二：根据物体中点云与质心的统计量分布，采用非参数统计和参数统计进行数据关联以更新全局地图中的物体数据库；

数据库更新阶段涉及的统计量，包括当前帧观测到的某物体的点云集合

地图数据库中已存在的某物体的点云集合

当前帧观测到的某物体的点云质心

地图数据库中已存在的某物体的质心集合

和地图数据库中两个物体的质心集合C₁，C₂；

1)在统计量不服从高斯分布的情况下使用非参数统计；

首先将来自当前帧观测到的物体的点云和地图中已存在的物体的点云P，Q混合，得到

构造统计量

其中R(·)是求秩操作；定义Mann-Whitney统计量W＝min(W_p,W_q)，可证明统计量W服从高斯分布，使用高斯统计量建模非高斯的点云；然后计算统计量W的均值和方差：

m(W)＝(|P||Q|)/2 (2)

其中Δ＝|P|+|Q|+1，τ∈P∩Q；

2)在统计量服从高斯分布的情况下，采用参数统计；

2-1)单样本t检验；利用单样本t检验验证当前帧观测到的物体质心和地图数据库中已存在的物体质心之间的关系；假设当前帧中物体的质心为c，地图数据库中物体的质心集合为C，对二者构造t统计量：

其中m(·)，σ(·)分别是求均值和方差操作；

2-2)双样本t检验；采用双样本t检验验证地图数据库中已存在的两个物体是否应属于同一个物体；假设两个物体的质心集合分别为C₁，C₂，则对它们构造如下的t统计量：

如果构造的统计量在置信区间内，则认为数据关联成功，更新数据库中的就物体，否则关联失败，在数据库中创建一个新物体。

4.根据权利要求1所述的面向物体的视觉SLAM轻量化语义地图创建方法，其特征在于，步骤三：数据库更新之后利用孤立森林算法估计物体中心和尺寸，并利用线段对齐算法估计物体方向；

1)估计物体中心和尺寸；利用孤立森林算法对物体点云集合X中的每个点云x计算其异常值得分s(x)：

其中E(h(x))为点云样本x在孤立树中路径的期望，H(i)＝ln(i)+γ为调和数；剔除得分大于阈值的点云，将剩余点云的均值作为物体中心t，将点云在三个方向上的长度一段作为尺寸s＝(max(X)-min(X))/2，其中max(·)，min(·)分别是求最大值和最小值操作；

2)估计物体方向；首先对图像提取LSD线段，并将落在物体检测框中的线段分配给物体，然后计算每条二维LSD线段的角度α(Z_lsd)；将物体点云在空间中构成三维立方框投影到图像中，得到立方框投影线段的角度α(Z_θ)，计算二者角度的差值作为误差：

e(θ)＝||α(Z(θ))-α(Z_lsd)||² (9)

Z(θ)＝KP_c(R(θ)Z+t) (10)

其中θ表示物体的方向，Z表示立方框的一条三维边，Z(θ)表示立方框的边在图像中的投影，其角度记为α(Z(θ))，K表示相机内参，(P_c,t)表示相机外参；θ偏航角从0度开始，以物体为中心每隔λ度旋转一次，每次计算一次角度得分：

其中N_p表示LSD线段平行于投影线段的数量，N_a表示LSD线段的总数，E(e)是一系列e(θ)的均值，ξ为误差阈值；最后将得分最高时的角度θ作为物体的方向；

3)物体尺寸和方向优化；将物***姿与相机位姿进行联合优化，代价函数为：

其中物体角度误差为e(θ)，物体尺寸误差定义为投影边到最边缘的LSD线段的距离e(s)。

5.根据权利要求1所述的面向物体的视觉SLAM轻量化语义地图创建方法，其特征在于，步骤四：完成物体参数化之后利用三维立方框和二次曲面建模物体；

已知物体中心t，尺寸s和方向θ，将立方框和二次曲面参数化为：

P_w＝R(θ)P_o+t (13)

Q_w＝TQ_oT^T (14)

其中

分别表示在物体坐标系和世界坐标系中立方框8个顶点的坐标，顶点与t和s有关；

分别表示在物体坐标系和世界坐标系中二次曲面的参数，且

表示在物体坐标系下三个半轴分别为s_l,s_w,s_h、中心为坐标系原点的二次曲面。

Images