一种实时的物体级语义地图构建和更新方法及装置

1.本发明属于人工智能中机器人的定位和建图技术领域，具体涉及一种实时的物体级语义地图构建及更新方法。


背景技术：

2.机器人或其它移动感知智能体的落地应用需要满足长期部署的要求，即能够在每日变化的场景中长期稳定的感知定位。此外，移动机器人要实现对未知环境的理解，需要明确环境中各种物体的属性，以便能够和环境做进一步的交互。目前研究存在以下两点问题：
3.(1)物体级定位感知系统的实时性依赖于成本高昂的硬件，否则实时性难以保证
4.视觉定位感知系统的主要成本开销在于传感器和运算平台。实时性主要取决于算法和运算平台的算力。视觉传感器可分为深度相机和普通的单目相机。深度相机如双目相机，通过视差原理获取深度信息，但逐个像素视差的计算需要消耗较多算力；深度相机如rgb
‑
d相机，可以直接获取深度信息，但价格数十倍于单目相机。单目相机低廉轻巧，需要多视图才能计算深度信息，相比深度相机更具挑战性。算力与成本是正相关的，定位感知功能仅仅是智能终端的底层功能，通常不希望该功能占据系统全部资源，以便留给上层应用足够的算力。
5.(2)机器人长期部署中，环境特征变化导致维护复用地图困难
6.在机器人的长期部署中，环境的变化难以避免，造成原有地图的定位特征发成变化，导致依赖于该特征的重定位发生偏差，长期的环境变化往往造成基于原有的地图无法重新定位，以至于地图无法重复使用。重新建图又将会花费大量的物力财力。


技术实现要素：

7.针对现有技术中存在的缺陷，本发明的目的是提供一种实时的物体级语义地图构建及更新方法，该方法能够满足物体级感知需求，同时能够实现机器人地图的更新以便长期复用。
8.为达到以上目的，本发明采用的技术方案是：
9.一种实时的物体级语义地图构建及更新方法，所述方法通过实时物体级语义地图构建及更新系统实现，所述系统包括实物平台和仿真平台，每个所述平台包括操作系统层、驱动层和硬件层；所述实物平台作为下位机是独立的数据处理中心，所述仿真平台位于上位机，用于下位机的数据远程可视化和环境仿真；
10.所述实物平台的操作系统层包括追踪线程模块、语义线程模块、重定位线程模块和地图更新模块，通过将所述追踪线程模块得到的三维地图点信息和所述语义线程模块得到的二维语义框融合，实现物体级语义地图的构建；通过所述重定位线程模块和地图更新模块实现物体级语义地图的复用和更新；
11.所述实物平台的硬件层包括单目相机、移动底盘和交互屏幕，所述单目相机设置在所述移动底盘上，通过所述移动底盘带动所述单目相机运动获取周围环境的图像信息；
12.所述实物平台的驱动层包括图像数据采集模块和底盘运动控制模块，通过所述图像数据采集模块驱动所述单目相机，通过所述底盘运动控制模块控制所述移动底盘的运动；
13.所述操作系统层和驱动层，所述驱动层和硬件层之间通信连接；
14.所述系统通过以下步骤实现物体级语义地图的构建：
15.s1、对二维语义框进行缩放，消除语义框的重叠情况，具体步骤包括：
16.s11、通过yolo3算法得到场景物体的矩形语义框；
17.s12、根据语义框的类别，设置固定的颜色以便可视化；
18.s13、计算语义框的对角长度，根据所述对角长度对语义框进行缩放以消除语义框的重叠情况；
19.s2、对同一帧图像中的物体地图点进行分类，对不同帧图像的地图点进行数据关联，具体步骤包括：
20.s21、对通过orb
‑
slam2算法提取的特征点进行语义标记；
21.s22、根据特征点与地图点的映射关系，索引到对应的地图点并进行语义标记；
22.s23、判断每一帧图像的语义地图点归属于地图中的某个物体或判定为新物体，具体判断方法为：
23.采用当前帧地图点与已有物体地图点的重合程度判断该帧物体与已有物体的差异性，如果差异性小于设定阈值，则归为同一物体，如果差异性大于设定阈值或者为不曾出现的类别，则判定为一个新物体；
24.所述重合程度的计算方法为：
25.当前帧中的一个物体obj
x
的n个地图点集合为obj
x
＝[p
x1
,p
x2
,
…
,p
xn
]，其中p＝[x,y,z]
t
为地图点的三维坐标，地图中已有同类物体m个{obj1,obj2,...obj
m
}，s
m
为已有第m个物体与obj
x
中重合地图点的个数，则重合程度中重合地图点的个数，则重合程度
[0026]
s3、对地图中同个物体被拆分为多个物体的地图点进行合并，具体步骤包括：
[0027]
s31、计算物体地图点的形心和半径，其中形心的计算方法为：
[0028][0029]
半径r的计算方法为：
[0030][0031]
然后对r
i
进行升序排列，取物体外围点的平均半径r
*
作为物体的半径，此时
[0032][0033]
其中，n表示物体地图点的数量，一个地图点记做所有地图点记做其中，|p|＝size(p)，表示取向量p的维数；
[0034]
s32、遍历同类物体的半径，用同类物体的平均半径作为该类物体的半径；
[0035]
s33、根据物体的半径，对邻近的同类物体进行判断，对需要合并的物体地图点进行合并，所述判断方法为：
[0036]
计算同类物体形心之间的距离，当距离小于设定半径阈值时，则合并该相邻的物体地图点为一个物体；
[0037]
所述系统通过以下步骤实现物体级语义地图的更新：
[0038]
s4、根据关键帧的共视关系，统计局部地图中的物体，具体步骤包括：
[0039]
s41、根据关键帧的共视关系得到局部地图，局部地图由每一关键帧对应的地图点构成；
[0040]
s42、对局部地图中每一关键帧中的物体进行统计，并建立关键帧与其对应物体的索引；
[0041]
s5、计算当前帧所对应的物体点云与局部地图中已有同类物体的相似程度，具体步骤包括：
[0042]
s51、遍历当前帧的物体，计算该物体的地图点与地图中同类物体地图点重合数量；
[0043]
s52、根据重合数量得到相似程度，并根据所述相似程度进行相似性检验，其中所述相似程度的计算方法为：
[0044]
相似程度＝m/n
[0045]
其中，n表示物体包含的地图点数量，m表示未被观测到的地图点个数；
[0046]
所述相似性检验的具体步骤为：
[0047]
对相似程度大于阈值的两个物体点云对相似程度大于阈值的两个物体点云进行判断，其中|p
a
|＝size(p
a
)，表示p
a
的维数，即物体点云p
a
包含的地图点个数；
[0048]
如果p
a
、p
b
属于同一个物体，则p
a
、p
b
服从相同的概率分布，合并p
a
、p
b
的地图点，记混合样本对样本地图点的三个维度分别升序排列并编秩，对于样本p
x
定义统计量则有：
[0049][0050][0051]
其中，r为混合样本的秩；
[0052]
w＝min(w
a
,w
b
)服从高斯分布n(μ,σ2)，其均值μ和方差σ2的计算方法如下：
[0053]
[0054][0055]
其中，τ∈p
a
∩p
b
；
[0056]
假设p
a
、p
b
属于同一个物体，若该假设成立，则概率函数f满足：f(w)≥f(z
r
)＝f(z
l
)＝α/2，其中α为分位数，置信区间为[z
l
,z
r
]；否则不是同一个物体，即不被当前帧观测；
[0057]
s6、根据物体的变化情况以整个物体对应的地图点为单位对地图进行更新，具体步骤包括：
[0058]
s61、根据s5的检验结果，判断原位置物体地图点是否被当前帧观测，并记录未被观测次数；
[0059]
s62、根据观测次数的记录对多次评定为不能被观测到的物体，将其对应的地图点从地图中清除。
[0060]
本发明的有益技术效果在于：
[0061]
1、本发明采用轻巧的单目相机作为传感器获取图像，避免使用昂贵的深度相机，节约成本，有利于在消费级设备上部署；
[0062]
2、本发明通过多线程技术维护稀疏的点云地图，使得系统更加高效和轻量，以便达到实时的要求；
[0063]
3、本发明同时具有物体级的感知功能，可以满足更高级的场景交互任务；
[0064]
4、本发明的语义感知与追踪采用模块化设计，语义感知的精度和范围可以根据需求组合；
[0065]
5、本发明创新性的利用物体语义信息辅助地图的更新，来保证重定位的精度和地图的时效性。
附图说明
[0066]
图1是本发明具体实施方式中提供的实时的物体级语义地图构建及更新方法流程框图；
[0067]
图2是图1所述方法中系统的结构框图；
[0068]
图3是图2所述系统的软件工作原理图；
[0069]
图4是图1所述方法中数据融合原理图。
具体实施方式
[0070]
下面结合附图和具体实施方式对本发明作进一步描述。
[0071]
图1示出了本发明具体实施方式中提供的实时的物体级语义地图构建及更新方法流程框图，图2示出了用于实现图1中所述方法的系统结构框图，所述系统包括实物平台和仿真平台，每个平台包括操作系统层、驱动层和硬件层，实物平台作为下位机是独立的数据处理中心，所述仿真平台位于上位机，上位机用于下位机的数据远程可视化和环境仿真。
[0072]
图3示出了图2所述系统的软件工作原理图，实物平台的操作系统层运行的算法采用多线程技术，包括追踪线程模块、语义线程模块、重定位线程模块和地图更新模块。通过
将追踪线程模块得到的三维地图点信息和语义线程模块得到的二维语义框融合，实现物体级语义地图的构建，通过所述重定位线程模块和地图更新模块实现物体级语义地图的复用和更新。本实施例中，追踪线程模块基于orb
‑
slam2算法，语义线程模块基于yolo3算法。
[0073]
实物平台的硬件层包括单目相机、移动底盘和交互屏幕(比如触摸屏)，单目相机设置在移动底盘上，单目相机用于获取环境图像，移动底盘用于承载单目相机采集环境信息，交互屏幕用于地图数据的操作及可视化。
[0074]
实物平台的驱动层包括图像数据采集模块和底盘运动控制模块，图像数据采集模块用于驱动单目相机，底盘运动控制模块用于控制移动底盘的运动。
[0075]
操作系统层和驱动层，驱动层和硬件层之间通信连接。
[0076]
本实施例中，实物平台基于kobuki通用轮式移动底盘，搭载了nvidia jetson agx xavier arm处理器、ubuntu18操作系统、8核cpu和32g内存，其内置一颗低性能的512核心volta gpu，使得实时的语义slam成为可能。
[0077]
本实施例中，仿真平台基于d
‑
h参数法建立了机器人的模型并在rviz下可视化，基于ros_control配置了底盘的差速控制器，在gazebo仿真平台建立了机器人的物理仿真模型和场景。
[0078]
本发明的主要创新性在于语义线程与追踪线程的数据融合和地图更新线程，实时性体现在程序对cpu、gpu资源的低占用率和对硬件平台的低要求，有效性体现在语义地图的融合效果以及地图更新方法对重定位的成功率的提升。
[0079]
本发明所述的实时物体级语义地图构建及更新方法包括地图构建方法和地图更新方法两大部分，下面分别对这两部分的技术方案进行详细描述。
[0080]
地图构建方法是对语义线程得到的二维语义框与追踪线程得到的三维地图点信息进行融合，从而得到带有语义信息地图点。地图构建方法包括以下步骤：
[0081]
s1、对二维语义框进行缩放，消除语义框的重叠情况。
[0082]
本步骤中具体步骤为：
[0083]
s11、通过yolo3算法得到场景物体的语义框。
[0084]
语义框是物体在二维图像上的位置，记做r，本实施例中语义框采用矩形，也可以为其他形状。
[0085]
s12、根据语义框的类别，设置固定的颜色以便可视化。
[0086]
物体类别记做id，根据物体id生成固定的颜色以便可视化。
[0087]
s13、根据语义框的对角长度，对语义框进行缩放消除语义框的重叠情况。
[0088]
首先遍历当前帧的语义框，定义x、y为矩形语义框r的边长，计算语义框对角线的长度
[0089]
根据语义框的对角长度l，对语义框进行缩放。例如，以语义框r1为参考，考虑语义框r2与r1的重叠情况，定义x1、y1和x2、y2分别为r1和r2的边长，dx、dy分别为矩形重叠区域的边长，根据l1、l2和dx、dy的相对大小对语义框进行缩放，缩放方案有以下几种：
[0090][0091]
语义框的缩放保证了在同一帧图像中，不同像素语义信息的唯一性，该方案在保证语义信息唯一性的情况下尽可能多的保留了语义信息。
[0092]
s2、对同一帧图像中的物体地图点进行分类，对不同帧图像的地图点进行数据关联。
[0093]
本步骤中具体步骤为：
[0094]
s21、对通过orb
‑
slam2算法提取的特征点进行语义标记。
[0095]
遍历当前帧通过orb
‑
slam2算法提取的特征点，对落入相应语义框的特征点标记为相应类别。
[0096]
s22、根据特征点与地图点的映射关系，索引到对应的地图点并进行语义标记。
[0097]
对于每一帧图像其中一个地图点，在不同帧间语义分类存在歧义时，以被最多次认定的语义类别作为该地图点的最终分类。
[0098]
s23、确定每一帧图像的语义地图点归属于地图中的某个物体或判定为新物体。
[0099]
数据融合过程是指对一帧图像察到的不同物体与其它帧的物体进行关联融合的过程，得到每个地图点归属于哪一个物体。图4示出了数据融合的原理，图中，id表示物体的类型(如id＝39表示瓶子，id＝56表示椅子，id＝75表示花瓶)。采用当前帧地图点与已有物体地图点的重合程度判断该帧物体与已有物体的差异性，如果差异性小，则归为同一物体，如果差异性大或者为不曾出现的类别，则作为一个新物体处理，如图4中id＝75。
[0100]
重合程度的计算方法为：当前帧中的一个物体obj
x
的n个地图点集合为obj
x
＝[p
x1
,p
x2
,
…
,p
xn
]，其中p＝[x,y,z]
t
为地图点的三维坐标，地图中已有同类物体m个{obj1,obj2,...obj
m
}，s
m
为已有第m个物体与obj
x
中重合地图点的个数，则重合程度
[0101]
数据关联和优化方法利用物体点云的空间关系，通过严格的差异性(重合程度)评判，保证能够区分邻近物体。
[0102]
s3、对地图中同个物体被拆分为多个物体的地图点进行合并。
[0103]
本步骤中具体步骤为：
[0104]
s31、计算物体地图点的形心和半径。
[0105]
首先计算物体地图点的形心，对于有n个地图点的物体，其中一个地图点记做所有地图点记做其中，|p|＝size(p)，表示取向量p的维数，如|p|表示该物体地图点的维数，即地图点的个数。形心的计算方法为：
[0106][0107]
半径r的计算方法为：
[0108][0109]
然后对r
i
进行升序排列，取物体外围点的平均半径r
*
作为物体的半径，此时
[0110][0111]
s32、遍历同类物体的半径，用同类物体的平均半径作为该类物体的半径。
[0112]
s33、根据物体的半径，对邻近的同类物体进行判断，对需要合并的物体地图点进行合并。
[0113]
计算同类物体形心之间的距离，当距离小于半径阈值，则合并该相邻的物体地图点为一个物体。
[0114]
基于两个物体中心距离的方法判断两个物体的差异性，基于物体半径的方法确定合并阈值，保证了拆分开的物体能够被融合。
[0115]
下面对地图更新方法进行详细描述。
[0116]
地图更新是指删除地图中在场景里消失的物体，增加新出现的物体，从而使地图与环境保持一致。本发明利用语义信息通过地图点的对比来发现地图的变化情况，从物体的层面整体考虑该物体所在区域的变化情况。变化情况包括物体的替换、移动和消失。物体的替换指原位置物体的消失和原位置新物体的出现，物体的移动指原位置物体的消失和新位置物体的出现，新出现的地图点直接添加到地图中，因此地图的更新只需删除消失的物体地图点。地图更新方法包括以下步骤：
[0117]
s4、根据关键帧的共视关系，统计局部地图中物体。
[0118]
本步骤中具体步骤为：
[0119]
s41、根据关键帧的共视关系得到局部地图，局部地图由每一关键帧对应的地图点构成。
[0120]
两个至少具有15个共同观测地图点的关键帧被认为满足共视关系。更新最近观测到的物体，最近观测到物体指当前帧及之前最近10个关键帧所包含的物体，统计最近观测到的物体编号。
[0121]
s42、对局部地图中每一关键帧中的物体进行统计，并建立关键帧与其对应物体的索引。
[0122]
s5、计算当前帧所对应的物体点云与局部地图中已有同类物体的相似程度。
[0123]
本步骤中具体步骤为：
[0124]
s51、遍历当前帧的物体，计算该物体的地图点与地图中同类物体地图点重合数量。
[0125]
遍历当前帧的物体，与局部地图中物体进行对比，对于局部地图中不被观测的物体，计算该物体的地图点与当前帧同类物体地图点的重合数量，对于包含了n个地图点物体，统计未被观测到的地图点个数，记为m。
[0126]
s52、根据重合数量得到相似程度，据此进一步的进行相似性检验。
[0127]
根据重合数量得到相似程度m/n，相似程度作为先验信息，进一步的对相似程度大于阈值的两个物体点云于阈值的两个物体点云进行判
断，其中|p
a
|＝size(p
a
)，表示p
a
的维数，即物体点云p
a
包含的地图点个数。如果p
a
、p
b
属于同一个物体，则p
a
、p
b
服从相同的概率分布，合并p
a
、p
b
的地图点，记混合样本对样本地图点的三个维度分别升序排列并编秩，对于样本p
x
定义统计量
[0128][0129]
其中r为混合样本的秩。
[0130]
w＝min(w
a
,w
b
)服从高斯分布n(μ,σ2)，其均值μ和方差σ2计算方法如下：
[0131][0132]
其中，τ∈p
a
∩p
b
。
[0133]
假设p
a
、p
b
属于同一个物体，若该假设成立，则概率函数f满足：f(w)≥f(z
r
)＝f(z
l
)＝α/2，其中α为分位数，置信区间为[z
l
,z
r
]。否则不是同一个物体，即不被当前帧观测。
[0134]
s6、根据物体的变化情况以整个物体对应的地图点为单位对地图进行更新。
[0135]
本步骤中具体步骤为：
[0136]
s61、根据s50的检验结果，判断原位置物体地图点是否被当前帧观测，并记录未被观测次数，未被观测次数记为{k1,k2,...k
m
}。
[0137]
s62、根据观测次数的记录对多次评定为不能被观测到的物体，将其对应的地图点从地图中清除。
[0138]
本发明采用成本远低于深度相机的单目相机，实时构建轻量的物体级稀疏点云地图，并通过语义信息来辅助地图的更新，满足物体级感知需求的同时，最大限度的降低对平台算力的要求，对于实现机器人本地实时的定位感知、长期自治和实际应用具有重要意义。
[0139]
上述实施例只是对本发明的举例说明，本发明也可以以其它的特定方式或其它的特定形式实施，而不偏离本发明的要旨或本质特征。因此，描述的实施方式从任何方面来看均应视为说明性而非限定性的。本发明的范围应由附加的权利要求说明，任何与权利要求的意图和范围等效的变化也应包含在本发明的范围内。