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

技术特征：
1.一种基于rgbd相机融合单线激光雷达的障碍物定位方法，其特征在于，移动机器人包括：机器人内部的控制装置、安装在机器人前侧的rgbd相机和单线激光雷达；所述rgbd相机和单线激光雷达与控制装置电连接；rgbd相机包括：具有相对位置关系的彩色相机和深度相机；所述方法包括：控制装置周期接收彩色相机采集的彩色图、深度相机的深度图和单线激光雷达的第一数据；所述控制装置根据障碍物检测策略选择对彩色图、深度图和第一数据中的两个以上进行融合并筛选融合结果，获得最终的障碍物定位结果；障碍物检测策略包括下述的一项或多项组合：确定场景下彩色图与深度图融合的结果；确定场景下彩色图与第一数据融合的结果；确定场景下预设距离内彩色图与深度图融合的结果；确定场景下预设距离外彩色图与第一数据融合的结果；特定障碍物场景下彩色图与第一数据融合的结果；特定障碍物场景下彩色图与第一数据、深度图融合的结果。2.根据权利要求1所述的障碍物定位方法，其特征在于，所述控制装置根据障碍物检测策略选择对彩色图、深度图和第一数据中的两个以上进行融合并筛选融合结果，包括：获取彩色图中的障碍物检测框；将所述检测框映射到深度图中，并获取检测框内各像素点的深度数据及该深度数据距离相机坐标系原点的聚类结果，获取检测框内障碍物的深度信息；根据障碍物的深度信息，获取障碍物相对机器人的位置信息。3.根据权利要求2所述的障碍物定位方法，其特征在于，根据障碍物的深度信息，获取障碍物相对机器人的位置信息，包括：基于所述障碍物的深度信息和深度相机的内参，获取障碍物相对于rgbd相机的位置；根据rgbd相机在机器人上的系统外参，获取障碍物相对机器人的位置信息；在障碍物定位之前，借助于设置在机器人外部的多个反光柱，通过适应性调整反光柱与机器人的距离，形成系统外参方程式，获取所述系统外参。4.根据权利要求2所述的障碍物定位方法，其特征在于，所述控制装置根据障碍物检测策略选择对彩色图、深度图和第一数据中的两个以上进行融合并筛选融合结果，包括：将单线激光雷达的点云数据即第一数据pnl:{id,(xl,yl,zl)}转换到彩色相机坐标系，得到彩色相机坐标系下的数据pnc:{id,(xc,yc,zc)}，再投影到彩色相机的像素坐标系，得到像素坐标系的数据pnp:{id,(xc,yc,zc),(u,v)}；其中，id为激光点云数据的索引号，(xl,yl,zl)激光点云数据在单线激光雷达坐标系下的坐标，(xc,yc,zc)激光点云数据在彩色相机坐标系下的坐标，(u,v)为激光点云数据在彩色相机的像素坐标系坐标；获取彩色图中的障碍物检测框，并记录检测框的左右两侧的纵坐标vl，vr，基于像素坐标系的数据pnp:{id,(xc,yc,zc),(u,v)}，在检测框左右两侧对应的雷达数据索引坐标v满足vl<v<vr时，获得检测框左右两侧对应的雷达数据索引坐标v的最大值idmax和最小值idmin；
以及根据最大值idmax和最小值idmin，将图像的像素坐标映射到单线激光雷达坐标系下对应雷达数据的索引范围，即所有id满足单线激光雷达坐标系下idmin’<id<idmax’激光点云数据的坐标；对获得的满足idmin’<id<idmax’激光点云数据进行点云簇分析，获取分析结果；若分析结果为一个聚类-结果，则将这个点云簇的中心坐标作为障碍物在单线激光雷达坐标系下的坐标；若分析结果为多个聚类结果，则将多个点云簇的中心坐标和单线激光雷达的距离值排序，保留距离值最小的点云簇的中心坐标，将保留的中心坐标作为障碍物在单线激光雷达坐标系下的坐标；障碍物在单线激光雷达坐标系下的坐标转换到机器人坐标系下，获取障碍物相对机器人的位置信息。5.根据权利要求4所述的障碍物定位方法，其特征在于，所述控制装置根据障碍物检测策略选择对彩色图、深度图和第一数据中的两个以上进行融合并筛选融合结果，包括：在彩色图和深度图融合时，障碍物相对机器人的位置信息在预设距离内，在彩色图和第一数据融合时，障碍物相对机器人的位置信息在预设距离内，将彩色图和深度图融合时障碍物相对机器人的位置信息作为控制装置输出的位置信息；在彩色图和深度图融合时，障碍物相对机器人的位置信息在预设距离内，在彩色图和第一数据融合时，障碍物相对机器人的位置信息在预设距离外，且彩色图中检测框所属类别为悬空或者低矮障碍物类别，则将彩色图和深度图融合时障碍物相对机器人的位置信息作为控制装置输出的位置信息；在彩色图和深度图融合时，障碍物相对机器人的位置信息无法确定，在彩色图和第一数据融合时，障碍物相对机器人的位置信息在预设距离内，且彩色图中检测框所属类别为黑色障碍物类别或室外场景，将彩色图和第一数据融合时障碍物相对机器人的位置信息作为控制装置输出的位置信息。6.根据权利要求1所述的障碍物定位方法，其特征在于，所述控制装置根据障碍物检测策略选择对彩色图、深度图和第一数据中的两个以上进行融合并筛选融合结果，包括：在彩色图和深度图融合时，障碍物相对机器人的位置信息无法确定，或者，在彩色图和深度图融合时，障碍物相对机器人的位置信息在预设距离外，以及在彩色图和第一数据融合时，障碍物相对机器人的位置信息在预设距离外，将彩色图和第一数据融合时障碍物相对机器人的位置信息作为控制装置输出的位置信息。7.根据权利要求1所述的障碍物定位方法，其特征在于，所述控制装置根据障碍物检测策略选择对彩色图、深度图和第一数据中的两个以上进行融合并筛选融合结果，包括：彩色图中检测框所属类别为桌子类别时；获取检测框对应的单线激光雷达坐标系下的第二数据，并进行点云聚类，获取单线激光雷达坐标系的桌腿坐标p
l
(x
l
,y
l
,z
l
)，桌腿坐标转换到彩色相机坐标系下得到p
c
(x
c
,y
c
,z
c
)，再转换到像素坐标系p
p
(u
p
,v
p
)；桌面的宽度估计值为检测框对应单线激光雷达坐标系的多个桌腿的最大横向距离的欧式距离；获取彩色图的检测框的上下边的横坐标u
t
,u
b
，桌腿在检测框中正上方和正下方对应的像素坐标为(u
t
,v
p
)和(u
b
,v
p
)；
其中x,y,z为相机坐标系下的坐标，f
x
,f
y
,c
x
,c
y
为rgbd相机内参，u,v为像素坐标；将u＝u
t
,v＝v
p
带入公式求出y，记为y
t
，同理将u＝u
t
,v＝v
p
带入公式求出y
b
；获取桌子的估计高度h＝|y
t-y
b
|；根据桌下的空荡区高度和宽度以及机器人尺寸比较确定桌子是否为当前机器人的障碍物。8.根据权利要求1所述的障碍物定位方法，其特征在于，所述控制装置周期接收彩色相机采集的彩色图、深度相机的深度图和单线激光雷达的第一数据之前，还包括：采用预先建立的数据集训练彩色图的障碍物检测模型，所述数据集中各图片为预先标注有类别和障碍物检测框的图片；训练后的障碍物检测模型能够对彩色图中的障碍物进行识别，输出障碍物所属的检测框及障碍物的类别。9.一种控制装置，其特征在于，包括存储器和处理器，所述存储器用于存储计算机程序，所述处理器用于执行所述存储器中存储的计算机程序并执行上述权利要求1至8任一所述的基于rgbd相机融合单线激光雷达的障碍物定位方法的步骤。10.一种移动机器人，其特征在于，包括上述权利要求9所述的控制装置，安装在机器人前侧的rgbd相机和单线激光雷达；所述rgbd相机和单线激光雷达与控制装置电连接。

技术总结
本发明涉及一种基于RGBD相机融合单线激光雷达的障碍物定位方法，包括：控制装置周期接收彩色相机采集的彩色图、深度相机的深度图和单线激光雷达的第一数据；根据障碍物检测策略选择对彩色图、深度图和第一数据中的两个以上进行融合并筛选融合结果，获得最终的障碍物定位结果。上述方法通过场景和障碍物类型划分的方式实现多数据融合获得定位结果，提升了定位精度。位精度。位精度。


技术研发人员：徐鹏程
受保护的技术使用者：杭州蓝芯科技有限公司
技术研发日：2022.07.19
技术公布日：2022/10/18