基于RGBD相机融合单线激光雷达的障碍物定位方法与流程

基于rgbd相机融合单线激光雷达的障碍物定位方法
技术领域
1.本发明涉及机器人技术领域，尤其涉及一种基于rgbd相机融合单线激光雷达的障碍物定位方法。


背景技术：

2.目前，将自动驾驶和移动机器人统称为机器人，在机器人领域中，机器人需要获取周围环境信息，并且对这些原始信息及时有效准确地加以抽象处理，得到简洁的环境信息，这为下一步决策规划奠定基础。因此，一个好的感知模块在机器人上起到至关重要的作用。
3.小型低速移动机器人需要考虑自身实际需要和成本等原因，往往不会使用价格高昂的传感器，如多线激光雷达等，因建图定位需要，往往会配备单线激光雷达。rgbd相机可以获取更加丰富的环境信息，且价格较低廉，随着机器人算力的提升，可以抽象更高级的语义信息。
4.常见的机器人的外部环境感知传感器有激光雷达、声纳、单目相机、双目相机和rgbd相机等。不同的传感器各有特点，如激光雷达又分为单线激光雷达、多线激光雷达、固态激光雷达等，激光雷达有测距远、精度高的特点，室内机器人多用单线激光雷达，主要因为其价格较为低廉，同时相比rgbd相机抗阳光干扰能力强，但其感知范围仅在一个平面上，难以区分障碍物的种类。声纳定位精度低，感知范围小。单目相机没有尺度信息，难以感知障碍物的真实空间位置。双目相机对无纹理的障碍物检测效果差、计算资源耗费高、测量精度随距离增长有大幅度衰减。rgbd相机具有体积小、功耗低、fov范围广，但其有效测距范围有限，且抗阳光干扰弱，室外难以使用。
5.为此，如何基于rgbd相机融合单线激光雷达实现障碍物的定位成为当前亟需解决的技术问题。


技术实现要素：

6.(一)要解决的技术问题
7.鉴于现有技术的上述缺点、不足，本发明提供一种基于rgbd相机融合单线激光雷达的障碍物定位方法。
8.(二)技术方案
9.为了达到上述目的，本发明采用的主要技术方案包括：
10.第一方面，本发明实施例提供一种基于rgbd相机融合单线激光雷达的障碍物定位方法，移动机器人包括：机器人内部的控制装置、安装在机器人前侧的rgbd相机和单线激光雷达；所述rgbd相机和单线激光雷达与控制装置电连接；rgbd相机包括：具有相对位置关系的彩色相机和深度相机；所述方法包括：
11.控制装置周期接收彩色相机采集的彩色图、深度相机的深度图和单线激光雷达的第一数据；
12.所述控制装置根据障碍物检测策略选择对彩色图、深度图和第一数据中的两个以
上进行融合并筛选融合结果，获得最终的障碍物定位结果；
13.障碍物检测策略包括下述的一项或多项组合：
14.确定场景下彩色图与深度图融合的结果；
15.确定场景下彩色图与第一数据融合的结果；
16.确定场景下预设距离内彩色图与深度图融合的结果；
17.确定场景下预设距离外彩色图与第一数据融合的结果；
18.特定障碍物场景下彩色图与第一数据融合的结果；
19.特定障碍物场景下彩色图与第一数据、深度图融合的结果。
20.可选地，针对室内近距离的场景，上述控制装置根据障碍物检测策略选择对彩色图、深度图和第一数据中的两个以上进行融合并筛选融合结果，包括：
21.获取彩色图中的障碍物检测框；
22.将所述检测框映射到深度图中，并获取检测框内各像素点的深度数据及该深度数据距离相机坐标系原点的聚类结果，获取检测框内障碍物的深度信息；
23.根据障碍物的深度信息，获取障碍物相对机器人的位置信息。
24.可选地，根据障碍物的深度信息，获取障碍物相对机器人的位置信息，包括：
25.基于所述障碍物的深度信息和深度相机的内参，获取障碍物相对于rgbd相机的位置；根据rgbd相机在机器人上的系统外参，获取障碍物相对机器人的位置信息；
26.在障碍物定位之前，借助于设置在机器人外部的多个反光柱，通过适应性调整反光柱与机器人的距离，形成系统外参方程式，获取所述系统外参。
27.可选地，针对室内远距离场景和/或室外场景，所述控制装置根据障碍物检测策略选择对彩色图、深度图和第一数据中的两个以上进行融合并筛选融合结果，包括：
28.将单线激光雷达的点云数据即第一数据pnl:{id,(xl,yl,zl)}转换到彩色相机坐标系，得到彩色相机坐标系下的数据pnc:{id,(xc,yc,zc)}，再投影到彩色相机的像素坐标系，得到像素坐标系的数据pnp:{id,(xc,yc,zc),(u,v)}；
29.其中，id为激光点云数据的索引号，(xl,yl,zl)激光点云数据在单线激光雷达坐标系下的坐标，(xc,yc,zc)激光点云数据在彩色相机坐标系下的坐标，(u,v)为激光点云数据在彩色相机的像素坐标系坐标；
30.获取彩色图中的障碍物检测框，并记录检测框的左右两侧的纵坐标vl，vr，基于像素坐标系的数据pnp:{id,(xc,yc,zc),(u,v)}，在坐标v满足vl《v《vr时，获得检测框左右两侧对应的雷达数据索引坐标v的最大值idmax和最小值idmin；
31.以及根据最大值idmax和最小值idmin，将图像的像素坐标映射到单线激光雷达坐标系下对应雷达数据的索引范围(根据图像的像素坐标、rgbd相机的内参、rgbd相机和单线激光雷达直接的相对外参求解出来)，即所有id满足idmin’《id《idmax’激光点云数据的坐标；
32.对获得的满足idmin’《id《idmax’激光点云数据进行点云簇分析，获取分析结果；
33.若分析结果为一个聚类结果，则将这个点云簇的中心坐标作为障碍物在单线激光雷达坐标系下的坐标；
34.若分析结果为多个聚类结果，则将多个点云簇的中心坐标和单线激光雷达的距离值排序，保留距离值最小的点云簇的中心坐标，将保留的中心坐标作为障碍物在单线激光
雷达坐标系下的坐标；
35.障碍物在单线激光雷达坐标系下的坐标转换到机器人坐标系下，获取障碍物相对机器人的位置信息。
36.可选地，所述控制装置根据障碍物检测策略选择对彩色图、深度图和第一数据中的两个以上进行融合并筛选融合结果，包括：
37.在彩色图和深度图融合时，障碍物相对机器人的位置信息在预设距离内，在彩色图和第一数据融合时，障碍物相对机器人的位置信息在预设距离内，将彩色图和深度图融合时障碍物相对机器人的位置信息作为控制装置输出的位置信息；
38.在彩色图和深度图融合时，障碍物相对机器人的位置信息在预设距离内，在彩色图和第一数据融合时，障碍物相对机器人的位置信息在预设距离外，且彩色图中检测框所属类别为悬空或者低矮障碍物类别，则将彩色图和深度图融合时障碍物相对机器人的位置信息作为控制装置输出的位置信息；
39.在彩色图和深度图融合时，障碍物相对机器人的位置信息无法确定，在彩色图和第一数据融合时，障碍物相对机器人的位置信息在预设距离内，且彩色图中检测框所属类别为黑色障碍物类别或室外场景，将彩色图和第一数据融合时障碍物相对机器人的位置信息作为控制装置输出的位置信息。
40.可选地，所述控制装置根据障碍物检测策略选择对彩色图、深度图和第一数据中的两个以上进行融合并筛选融合结果，包括：
41.在彩色图和深度图融合时，障碍物相对机器人的位置信息无法确定，或者，在彩色图和深度图融合时，障碍物相对机器人的位置信息在预设距离外，以及
42.在彩色图和第一数据融合时，障碍物相对机器人的位置信息在预设距离外，将彩色图和第一数据融合时障碍物相对机器人的位置信息作为控制装置输出的位置信息。
43.可选地，所述控制装置根据障碍物检测策略选择对彩色图、深度图和第一数据中的两个以上进行融合并筛选融合结果，包括：
44.彩色图中检测框所属类别为桌子类别时；
45.获取检测框对应的单线激光雷达坐标系下的第二数据，并进行点云聚类，获取单线激光雷达坐标系的桌腿坐标p
l
(x
l
,y
l
,z
l
)，桌腿坐标转换到彩色相机坐标系下得到pc(xc,yc,zc)，再转换到像素坐标系p
p
(u
p
,v
p
)；桌面的宽度估计值为检测框对应单线激光雷达坐标系的多个桌腿的最大横向距离的欧式距离；
46.获取彩色图的检测框的上下边的横坐标u
t
,ub，桌腿在检测框中正上方和正下方对应的像素坐标为(u
t
,v
p
)和(ub,v
p
)；
[0047][0048]
其中x,y,z为相机坐标系下的坐标，f
x
,fy,c
x
,cy为rgbd相机内参，u,v为像素坐标；将u＝u
t
,v＝v
p
带入公式求出y，记为y
t
，同理将u＝u
t
,v＝v
p
带入公式求出yb；获取桌子的估计高度h＝|y
t-yb|；
[0049]
根据桌下的空荡区高度和宽度以及机器人尺寸比较确定桌子是否为当前机器人的障碍物。
[0050]
可选地，所述控制装置周期接收彩色相机采集的彩色图、深度相机的深度图和单线激光雷达的第一数据之前，还包括：
[0051]
采用预先建立的数据集训练彩色图的障碍物检测模型，所述数据集中各图片为预先标注有类别和障碍物检测框的图片；
[0052]
训练后的障碍物检测模型能够对彩色图中的障碍物进行识别，输出障碍物所属的检测框及障碍物的类别。
[0053]
第二方面，本发明实施例还提供一种控制装置，其包括存储器和处理器，所述存储器用于存储计算机程序，所述处理器用于执行所述存储器中存储的计算机程序并执行上述第一方面任一所述的基于rgbd相机融合单线激光雷达的障碍物定位方法的步骤。
[0054]
第三方面，本发明实施例还提供一种移动机器人，其包括上述第二方面所述的控制装置，安装在机器人前侧的rgbd相机和单线激光雷达；所述rgbd相机和单线激光雷达与控制装置电连接。
[0055]
(三)有益效果
[0056]
本发明实施例的方法结合rgbd相机和单线激光雷达的优缺点，提出在不同环境/场景下，两种传感器融合感知的方法，例如rgbd相机融合单线激光雷达的障碍物检测定位，实现对障碍物的精准定位，同时可实现特殊场景中对障碍物体做空间定位。
附图说明
[0057]
图1为本发明一实施例提供的基于rgbd相机融合单线激光雷达的障碍物定位方法的流程示意图；
[0058]
图2为本发明另一实施例提供的基于rgbd相机融合单线激光雷达的障碍物定位方法的流程示意图。
具体实施方式
[0059]
为了更好的解释本发明，以便于理解，下面结合附图，通过具体实施方式，对本发明作详细描述。
[0060]
当前，单线激光雷达和深度相机都有各自的缺陷：单线激光雷达检测距离远40m内，但是检测高度是固定的，很多低矮障碍和悬空障碍不能检测到。深度相机可以检测更大维度的障碍，低矮障碍和悬空障碍可以被检测到，但检测距离较短，2m范围外精度下降很多。同时阳光对深度相机影响很大，在室外定位效果很差。
[0061]
鉴于此，本发明实施例中结合rgbd相机和单线激光雷达的优缺点，提出在不同环境/场景下，两种传感器融合感知的方法，即就rgbd相机融合单线激光雷达的障碍物检测定位，具体地，检测识别出障碍物(rgbd相机的彩色相机利用深度学习得到图片上的检测框和类别)；进一步，可在特殊场景中对障碍物体做空间定位(利用检测框映射到深度图上或激光雷达点云上)，实现对障碍物的精确定位。
[0062]
需要说明的是，本发明实施例中提及的rgbd相机包括普通的彩色相机和深度相机，相机厂家在出厂之前将彩色相机的数据和深度相机的数据对齐，以及提供rgbd相机所属的点云数据，该点云数据和彩色相机与深度相机的相对位置关系。
[0063]
深度相机一般都是通过主动发射和接收红外光线来测距。但对于深色低反射物
体，深度相机接收到的红外光过少，深度相机自身给不出该种障碍的位置。另外有些障碍是中间镂空的，比如一些货架，桌子等，单线激光雷达和深度相机均不能独立感知完整障碍物情况。本发明实施例中通过检测框、类别和单线激光雷达、深度相机数据估计出障碍物的轮廓，防止机器人通过类似桌子下面时候，机器人上方撞到桌子侧面，有效提高了障碍物的精准定位。
[0064]
本发明实施例中的移动机器人包括：机器人内部的控制装置、安装在机器人前侧的rgbd相机和单线激光雷达；所述rgbd相机和单线激光雷达与控制装置电连接；rgbd相机包括：具有相对位置关系的彩色相机和深度相机。
[0065]
实施例一
[0066]
如图1所示，本发明实施例提供一种基于rgbd相机融合单线激光雷达的障碍物定位方法，本实施例的方法的执行主体可为任意机器人的控制设备/移动机器人，具体实现方法包括下述步骤：
[0067]
s10、控制装置周期接收彩色相机采集的彩色图、深度相机的深度图和单线激光雷达的第一数据；
[0068]
s20、控制装置根据障碍物检测策略选择对彩色图、深度图和第一数据中的两个以上进行融合并筛选融合结果，获得最终的障碍物定位结果；
[0069]
障碍物检测策略包括下述的一项或多项组合：
[0070]
确定场景下彩色图与深度图融合的结果；
[0071]
确定场景下彩色图与第一数据融合的结果；
[0072]
确定场景下预设距离内彩色图与深度图融合的结果；
[0073]
确定场景下预设距离外彩色图与第一数据融合的结果；
[0074]
特定障碍物场景下彩色图与第一数据融合的结果；
[0075]
特定障碍物场景下彩色图与第一数据、深度图融合的结果。
[0076]
举例来说，在一种实现场景中，例如室内近距离的检测场景，上述s20可进行彩色图和深度图的融合，具体包括：
[0077]
s21、获取彩色图中的障碍物检测框；
[0078]
在实际应用中，可采用预先建立的数据集训练彩色图的障碍物检测模型，所述数据集中各图片为预先标注有类别和障碍物检测框的图片；训练后的障碍物检测模型能够对彩色图中的障碍物进行识别，输出障碍物所属的检测框及障碍物的类别。
[0079]
本实施例的障碍物检测模型可以为深度学习模型，其不限定深度学习模型，根据实际需要选择。
[0080]
s22、将所述检测框映射到深度图中，并获取检测框内各像素点的深度数据及该深度数据距离相机坐标系原点的聚类结果，获取检测框内障碍物的深度信息；
[0081]
s23、根据障碍物的深度信息，获取障碍物相对机器人的位置信息。
[0082]
举例来说，基于所述障碍物的深度信息和深度相机的内参，获取障碍物相对于rgbd相机的位置；根据rgbd相机在机器人上的系统外参，获取障碍物相对机器人的位置信息；
[0083]
在障碍物定位之前，借助于设置在机器人外部的多个反光柱，通过适应性调整反光柱与机器人的距离，形成系统外参方程式，获取所述系统外参。
[0084]
举例来说，在第二种实现场景中，例如室内远距离检测中或者室外场景的检测中，上述s20可包括：
[0085]
s21a、将单线激光雷达的点云数据即第一数据pnl:{id,(xl,yl,zl)}转换到彩色相机坐标系，得到彩色相机坐标系下的数据pnc:{id,(xc,yc,zc)}，再投影到彩色相机的像素坐标系，得到像素坐标系的数据pnp:{id,(xc,yc,zc),(u,v)}；
[0086]
其中，id为激光点云数据的索引号，(xl,yl,zl)激光点云数据在单线激光雷达坐标系下的坐标，(xc,yc,zc)激光点云数据在彩色相机坐标系下的坐标，(u,v)为激光点云数据在彩色相机的像素坐标系坐标；
[0087]
s22a、获取彩色图中的障碍物检测框，并记录检测框的左右两侧的纵坐标vl，vr，基于像素坐标系的数据pnp:{id,(xc,yc,zc),(u,v)}，在坐标v满足vl《v《vr时，获得检测框左右两侧对应的雷达数据索引中坐标v的最大值idmax和最小值idmin；
[0088]
s23a、以及根据最大值idmax和最小值idmin，将图像的像素坐标映射到单线激光雷达坐标系下对应雷达数据的索引范围(根据图像的像素坐标、相机的内参、相机和单线激光雷达直接的相对外参求解出来)，即所有id满足idmin《id《idmax激光点云数据的坐标；
[0089]
s24a、对获得的满足idmin《id《idmax激光点云数据进行点云簇分析，获取分析结果；
[0090]
若分析结果为一个聚类的结果，则将这个点云簇的中心坐标作为障碍物在单线激光雷达坐标系下的坐标；
[0091]
若分析结果为多个聚类结果，则将多个点云簇的中心坐标和单线激光雷达的距离值排序，保留距离值最小的点云簇的中心坐标，将保留的中心坐标作为障碍物在单线激光雷达坐标系下的坐标；
[0092]
s25a、障碍物在单线激光雷达坐标系下的坐标转换到机器人坐标系下，获取障碍物相对机器人的位置信息。
[0093]
基于上述的两种场景，在实际应用中，可基于下述四种筛选方式选择最终的输出结果：
[0094]
第一种筛选方式：在彩色图和深度图融合时，障碍物相对机器人的位置信息在预设距离内，在彩色图和第一数据融合时，障碍物相对机器人的位置信息在预设距离内，将彩色图和深度图融合时障碍物相对机器人的位置信息作为控制装置输出的位置信息；
[0095]
第二种筛选方式：在彩色图和深度图融合时，障碍物相对机器人的位置信息在预设距离内，在彩色图和第一数据融合时，障碍物相对机器人的位置信息在预设距离外，且彩色图中检测框所属类别为悬空或者低矮障碍物类别，则将彩色图和深度图融合时障碍物相对机器人的位置信息作为控制装置输出的位置信息；
[0096]
第三种筛选方式：在彩色图和深度图融合时，障碍物相对机器人的位置信息无法确定，在彩色图和第一数据融合时，障碍物相对机器人的位置信息在预设距离内，且彩色图中检测框所属类别为黑色障碍物类别或室外场景，将彩色图和第一数据融合时障碍物相对机器人的位置信息作为控制装置输出的位置信息。
[0097]
第四种筛选方式：在彩色图和深度图融合时，障碍物相对机器人的位置信息无法确定，或者，在彩色图和深度图融合时，障碍物相对机器人的位置信息在预设距离外，以及
[0098]
在彩色图和第一数据融合时，障碍物相对机器人的位置信息在预设距离外，将彩
色图和第一数据融合时障碍物相对机器人的位置信息作为控制装置输出的位置信息。
[0099]
实施例二
[0100]
基于上述实施例一的描述，可理解的是，上述方法主要包括下述三部分内容：
[0101]
第一：借助于标定物(如反光柱)对rgbd相机中的彩色相机和单线激光雷达进行标定，得到系统外参。
[0102]
需要说明的是，现在彩色相机和激光雷达的相对外参标定一般都是彩色相机和多线(或固态)激光雷达标定(点云数据相比单线激光雷达更丰富)。本实施例中借助于单线激光雷达和彩色相机实现系统外参的标定。
[0103]
第二、基于rgbd相机中的彩色相机、深度相机和单线激光雷达实现室内外不同场景中障碍物的自适应感应定位。
[0104]
该部分借助于深度学习对彩色相机的图像进行检测，以获取障碍物的检测框；然后根据检测框内障碍物的距离、环境(设定阈值)来选择单线激光雷达还是深度相机的定位结果。这样选择方式可以使得障碍物定位结果更鲁棒。
[0105]
第三、针对室内外不同场景中漏成像的部分，结合rgbd相机中的深度相机进行补充，保证精准定位和障碍物的空间形状的确定。
[0106]
以下结合附图2对上述三部分进行详细说明。
[0107]
第一、系统外参标定过程。
[0108]
a、机器人前侧安装有rgbd相机和单线激光雷达。准备多个高度一致反光柱(如8个0.5m高)，将其竖直自由摆放在机器人前方10m*10m范围内。在机器人静止状态下，人工确保这些反光柱都可以无遮挡地被彩色相机和激光雷达同时观测到，即所有反光柱被彩色相机和单线激光雷达共视。
[0109]
b、获取激光雷达和彩色相机图像中反光柱数目。
[0110]
激光雷达自适应获取反光柱数目。单线激光雷达获取反光柱数目方法：当激光雷达点云同时满足以下两个条件时则判定为反光柱：i、在机器人前方10m*10m范围内点云强度值大于反光柱的检测阈值；ii、对强度值高的点云簇做聚类，用最小二乘法拟合圆弧，圆弧的曲率半径和反光柱实际半径的差值小于阈值，则可以认定为点云簇为反光柱。
[0111]
彩色相机获取反光柱数目方法：用深度学习检测到的反光柱的检测框进行计数。
[0112]
若单线激光雷达或彩色相机检测到的反光柱数目小于8则需要人工重新调整反光柱的位置，直到单线激光雷达或彩色相机检测到的反光柱数目均等于8。
[0113]
c、彩色相机和单线激光雷达分别记录反光柱柱面顶部中心点的坐标值(如图2所示)，具体是记录在单线激光雷达坐标系的3d坐标；记录和彩色相机像素坐标系下的反光柱柱面顶部中心点的2d像素坐标。
[0114]
反光柱柱面顶部中心点在单线激光雷达坐标系的3d坐标获取方法：实际上单线激光雷达只能获取反光柱在激光雷达坐标系下的2d坐标(x,y)，2d坐标为反光柱点云的拟合圆弧的线中点2d坐标。这里3d坐标的高度值为反光柱高度减去雷达距地面的安装高度，如雷达安装高度为0.1m，反光柱高度0.5m，高度差为0.4,则3d坐标为(x,y,0.4)。按照从左到右标记1～8个反光柱，并记录每个反光柱柱面顶部中心点的3d坐标。
[0115]
反光柱柱面顶部中心点在彩色相机像素坐标系下的像素坐标获取方法：将彩色相机图片的反光柱从左到右标注序号1～8，触发反光柱柱面顶部中心点，以使得标定程序记
录该顶部中心点的像素坐标(u，v)，并进一步通过标示方式辅助判断点击的位置是不是反光柱柱面顶部中心点。待标定程序页面跳出是否记录该点的坐标的窗口时，以使用户确定最终记录的该序号的反光柱柱面顶部中心点的坐标。
[0116]
d、人工移动所有反光柱位置，重复上述子步骤a、b、c至少3次，可以获取3组一共24个3d坐标：
[0117]
(((x
11
,y
11
,0.04),(x
12
,y
12
,0.04)...(x
18
,y
18
,0.04)),((x
21
,y
21
,0.04),(x
22
,y
22
,0.04)...(x
28
,y
28
,0.04)),((x
31
,y
31
,0.04),(x
32
,y
32
,0.04)...(x
38
,y
38
,0.04)))和2d像素坐标(((u
11
,v
11
),(u
12
,v
12
)...(u
18
,v
18
)),((u
21
,v
21
),(u
22
,v
22
)...(u
28
,v
28
)),((u
31
,v
31
),(u
32
,v
32
)...(u
38
,v
38
)))。
[0118][0119]
zc为相机坐标系相对单线激光雷达坐标系(o
l
x
lylzl
)的位姿的z分量，公式展开后可以消去zc。k为相机内参，(u,v)为图像坐标系，t为单线激光雷达坐标系下的相对位姿关系。
[0120]
将24个3d坐标和2d坐标带入上式进行svd分解，即可求出t。对t求逆，即可求出单线激光雷达在彩色相机坐标系下的相对位姿关系。
[0121]
第二、障碍物的检测定位过程。
[0122]
1.1、深度学习模型训练阶段：
[0123]
a、人工采集特定类别障碍物如人和机器人的图片数据(如10000张)，人工标注障碍物的检测框和类别。
[0124]
b、根据机器人所属工控机的gpu性能选用合适的目标检测模型，例如工控gpu是intel的集成显卡，则可以选用较小的深度学习网络模型如yolov2-tiny。还可以用深度学习框架(如darknet)训练深度网络模型。
[0125]
c、将训练好的网络模型部署到机器人所属的工控机上，启动目标检测程序，获取障碍物在彩色相机图片上检测框的像素坐标。
[0126]
1.2、机器人启动目标检测程序之后的目标检测阶段：
[0127]
该步骤为数据级融合定位，该步骤分为两部分，彩色图和深度图融合以及彩色图和单线激光雷达融合。
[0128]
(1)彩色相机的彩色图和深度相机的深度图融合：
[0129]
a、彩色图的检测框结果映射到深度图中。
[0130]
需要说明的是，rgbd相机的彩色相机和深度相机的位姿关系是已知的。
[0131]
b、获取对检测框内深度数据的距离，保留距离相机坐标系原点最小的聚类结果，可过滤掉检测框中的非目标障碍物的深度信息，保留障碍物的深度信息。
[0132]
c、根据深度信息以及深度相机的内参(厂家提供或自行使用棋盘格标定)求出障碍物相对相机的位置，根据机器人预设好的各个传感器之间的相对外参，求出障碍物相对机器人的位置。
[0133]
(2)彩色相机的彩色图和单线激光雷达的数据融合：
[0134]
a、将单线激光雷达各点的数据pnl:{id,(xl,yl,zl)}转换到彩色相机坐标系pnc:{id,(xc,yc,zc)}，再投影到彩色相机的像素坐标系pnp:{id,(xc,yc,zc),(u,v)}。id为激光点云数据的索引号，(xl,yl,zl)激光点云数据在单线激光雷达坐标系下的坐标(zl为固定值，为激光雷达的安装高度)，(xc,yc,zc)激光点云数据在彩色相机坐标系下的坐标，(u,v)为激光点云数据在彩色相机的像素坐标系坐标。
[0135]
b、记录彩色图像障碍物检测框的左右两边的纵坐标vl，vr，找到所有像素坐标系雷达数据索引坐标v满足vl《v《vr的点云。找出v的最大值和最小值对应的idmax和idmin。
[0136]
c、根据最大值idmax和最小值idmin，将图像的像素坐标系下索引坐标v映射到单线激光雷达坐标系下对应雷达数据的索引范围，根据映射后的索引范围idmax’和idmin’找到在单线激光雷达坐标系下所有id满足单线激光雷达坐标系下idmin’《id《idmax’激光点云数据的坐标。
[0137]
d、对在idmin’和idmax’范围内点云簇分析，对范围内的点云做聚类。若只有一个聚类结果，则将这个点云簇的中心坐标作为障碍物在雷达坐标系下的坐标。若有多个聚类结果，则将多个点云簇的中心坐标和激光雷达的距离值排序，最终只保留距离值最小的点云簇的中心坐标。(idmin’和idmax’范围内的点云包括障碍物的激光雷达点云，可能也包括该范围内障碍物侧后方的其他激光雷达点云。)
[0138]
e、根据b找到所有检测框内的障碍物在激光雷达坐标系下的坐标，再转换到机器人坐标系下，完成障碍物检测定位。
[0139]
(3)不同环境决策级融合障碍物定位：
[0140]
由于rgbd相机在室外强光照、遇到黑色低反射物质以及远距离时深度相机获取的深度数据不可靠，本实施例会根据相机特性设定阈值，只保留阈值内的深度数据。
[0141]
3.1)近距离(如3m范围内)障碍物检测：
[0142]
a、彩色图片的同一个障碍物检测框下，彩色图与深度图结合的障碍物定位结果和彩色图与激光雷达定位结果都在3m范围内且重合度高，此时以彩色图与深度图定位的结果为准。
[0143]
b、彩色图片的同一个障碍物检测框下，彩色相机与深度相机结合的障碍物定位结果在3m范围、彩色相机与激光雷达定位结果在3m范围外，此时障碍物为近距离的悬空或者低矮障碍物(单个障碍物全部位于在雷达感知平面上方或下方)，以彩色图与深度图定位的结果为准。
[0144]
c、彩色相机与深度相机结合的障碍物定位无结果，彩色相机与激光雷达定位结果在3m范围内，此时障碍物可能是黑色障碍物或者在室外，深度相机感知结果差(检测框内对应的深度图无深度值，或有深度值的有效像素数目低于检测框内所有像素数目的1/5)，此时以彩色相机与激光雷达定位结果为准。
[0145]
3.2)远距离障碍物检测：彩色相机与深度相机结合的障碍物定位无结果或检测距离在3m外，同时彩色相机与激光雷达定位结果在3m外，此时可以判定是远距离障碍物，此时以彩色相机与激光雷达定位结果为准。
[0146]
3.3)特殊障碍物轮廓高度估计：
[0147]
对于一些两边有腿中间为空的障碍物，如门、桌子、高腿货架等(以桌子为例)，rgbd相机深度数据可能失效或者无法感知完整障碍物深度。此时激光雷达也只能检测到桌
腿，机器人路径规划可能会从桌子下面经过，若机器人高度比桌子高，则会发生撞击。因此有必要对特定障碍物做轮廓和高度估计。
[0148]
具体步骤如下：
[0149]
a、深度学习检测类似桌子的特定障碍物，和上述彩色图和单线激光雷达融合过程一样，找到彩色图检测框对应单线激光雷达坐标系下id满足要求的点云。对该范围内点云聚类，找到单线激光雷达坐标系的桌腿坐标p
l
(x
l
,y
l
,z
l
)，并转换到彩色相机坐标系下pc(xc,yc,zc)，zc实际就是彩色相机坐标系的深度。再转换到像素坐标系p
p
(u
p
,v
p
)。桌子的宽度估计值为检测框对应单线激光雷达坐标系的多个桌腿的最大横向距离的欧式距离。
[0150]
b、找到彩色图像的检测框的上下边的横坐标u
t
,ub。则桌腿在检测框中正上方和正下方对应的像素坐标为(u
t
,v
p
)和(ub,v
p
)。
[0151][0152]
其中x,y,z为相机坐标系下的坐标，f
x
,fy,c
x
,cy为相机内参，u,v为像素坐标。将u＝u
t
,v＝v
p
带入公式，可以求出y，记为y
t
。同理将u＝u
t
,v＝v
p
带入公式求出yb。桌子的估计高度h＝|y
t-yb|。
[0153]
c、根据桌下的空荡区高度和宽度以及机器人尺寸比较决定是否把桌子作为障碍物。
[0154]
本实施例结合rgbd相机和单线激光雷达的优缺点，提出在不同环境/场景下，两种传感器融合感知的方法，实现了对障碍物的精准定位。
[0155]
实施例三
[0156]
本实施例还提供一种移动机器人的控制装置，包括：存储器和处理器；所述处理器用于执行所述存储器中存储的计算机程序，以实现执行上述实施例一和实施例二任意所述的基于rgbd相机融合激光雷达的障碍物定位方法的步骤。
[0157]
另一方面，本发明实施例还提供一种计算机可读存储介质，其用于存储有计算机程序，所述计算机程序被处理器执行时实现上述任意实施例的基于rgbd相机融合激光雷达的障碍物定位方法的步骤。
[0158]
应当注意的是，在权利要求中，不应将位于括号之间的任何附图标记理解成对权利要求的限制。词语“包含”不排除存在未列在权利要求中的部件或步骤。位于部件之前的词语“一”或“一个”不排除存在多个这样的部件。本发明可以借助于包括有若干不同部件的硬件以及借助于适当编程的计算机来实现。在列举了若干装置的权利要求中，这些装置中的若干个可以是通过同一个硬件来具体体现。词语第一、第二、第三等的使用，仅是为了表述方便，而不表示任何顺序。可将这些词语理解为部件名称的一部分。
[0159]
此外，需要说明的是，在本说明书的描述中，术语“一个实施例”、“一些实施例”、“实施例”、“示例”、“具体示例”或“一些示例”等的描述，是指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不必须针对的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任一个或多个实施例或示例中以合适的方式结合。此外，在不相互矛盾的情况下，本领域的技术人员可以将本说明书中描述的不同实施例或示例以及不同实
施例或示例的特征进行结合和组合。
[0160]
尽管已描述了本发明的优选实施例，但本领域的技术人员在得知了基本创造性概念后，则可对这些实施例作出另外的变更和修改。所以，权利要求应该解释为包括优选实施例以及落入本发明范围的所有变更和修改。
[0161]
显然，本领域的技术人员可以对本发明进行各种修改和变型而不脱离本发明的精神和范围。这样，倘若本发明的这些修改和变型属于本发明权利要求及其等同技术的范围之内，则本发明也应该包含这些修改和变型在内。