(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210855355.3 (22)申请日 2022.07.19 (71)申请人 杭州蓝芯科技有限公司 地址 311121 浙江省杭州市余杭区余杭街 道文一西路1818-2号7幢902室 (72)发明人 徐鹏程　 (74)专利代理 机构 北京易捷胜知识产权代理事 务所(普通 合伙) 11613 专利代理师 齐胜杰 (51)Int.Cl. G06V 10/80(2022.01) G06V 10/762(2022.01) G06V 10/764(2022.01) G06V 20/58(2022.01) G06V 10/25(2022.01)G06T 7/50(2017.01) G06T 7/521(2017.01) G01S 17/93(2020.01) G01S 17/86(2020.01) G01S 17/06(2006.01) (54)发明名称 基于RGBD相机融合单线激光雷达的障碍物 定位方法 (57)摘要 本发明涉及一种基于RGBD相机融合单线激 光雷达的障碍物定位方法， 包括： 控制装置周期 接收彩色相机采集的彩色图、 深度相机的深度图 和单线激光雷达的第一数据； 根据障碍物检测策 略选择对彩色图、 深度图和第一数据中的两个以 上进行融合并筛选融合结果， 获得最终的障碍物 定位结果。 上述方法通过场景和障碍物类型划分 的方式实现多数据融合获得定位结果， 提升了定 位精度。 权利要求书3页 说明书11页 附图1页 CN 115205646 A 2022.10.18 CN 115205646 A 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,z c),(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；权　利　要　求　书 1/3 页 2 CN 115205646 A 2以及根据最大值idmax和最小值idmin， 将图像的像素坐标映射到单线激光雷达坐标系 下对应雷达数据的索引范围， 即所有id满足单线激光雷达坐标系下idmin ’<id<idmax ’激光 点云数据的坐标； 对获得的满足idmi n’<id<idmax ’激光点云数据进行点云簇分析， 获取分析 结果； 若分析结果为一个聚类 ‑结果， 则将这个点云簇的中心坐标作为障碍物在单线激光雷 达坐标系下的坐标； 若分析结果为多个聚类结果， 则将多个点云簇的中心坐标和单线激光雷达的距离值排 序， 保留距离值最小的点云簇的中心坐标， 将保留的中心坐标作为障碍物在单线激光雷达 坐标系下的坐标； 障碍物在单线激光雷达坐标系下的坐标转换到机器人坐标系下， 获取障碍物相对机器 人的位置信息 。 5.根据权利要求4所述的障碍物定位方法， 其特征在于， 所述控制装置根据障碍物检测 策略选择对彩色图、 深度图和第一数据中的两个以上进行融合并筛 选融合结果， 包括： 在彩色图和深度图融合时， 障碍物相对机器人的位置信息在预设距离内， 在彩色图和 第一数据融合时， 障碍物相对机器人 的位置信息在预设距离内， 将彩色图和深度图融合时 障碍物相对机器人的位置信息作为控制装置 输出的位置信息； 在彩色图和深度图融合时， 障碍物相对机器人的位置信息在预设距离内， 在彩色图和 第一数据融合时， 障碍物相对机器人 的位置信息在预设距离外， 且彩色图中检测框所属类 别为悬空或者低矮障碍物类别， 则将彩色图和深度图融合时障碍物相对机器人的位置信息 作为控制装置 输出的位置信息； 在彩色图和深度图融合时， 障碍物相对机器人的位置信息无法确定， 在彩色图和第一 数据融合时， 障碍物相对机器人 的位置信息在预设距离内， 且彩色图中检测框所属类别为 黑色障碍物类别或室外场景， 将彩色图和 第一数据融合时障碍物相对机器人的位置信息作 为控制装置 输出的位置信息 。 6.根据权利要求1所述的障碍物定位方法， 其特征在于， 所述控制装置根据障碍物检测 策略选择对彩色图、 深度图和第一数据中的两个以上进行融合并筛 选融合结果， 包括： 在彩色图和深度图融合 时， 障碍物相对机器人的位置信 息无法确定， 或者， 在 彩色图和 深度图融合时， 障碍物相对机器人的位置信息在预设距离 外， 以及 在彩色图和第一数据融合时， 障碍物相对机器人的位置信息在预设距离外， 将彩色图 和第一数据融合时障碍物相对机器人的位置信息作为控制装置 输出的位置信息 。 7.根据权利要求1所述的障碍物定位方法， 其特征在于， 所述控制装置根据障碍物检测 策略选择对彩色图、 深度图和第一数据中的两个以上进行融合并筛 选融合结果， 包括： 彩色图中检测框所属类别为 桌子类别时； 获取检测框对应的单线激光雷达坐标系下的第二数据， 并进行点云聚类， 获取单线激 光雷达坐标系的桌腿坐标PL(xL,yL,zL)， 桌腿坐标转换到彩色相机坐标系下得到PC(xC,yC, zC)， 再转换到像素坐标系 Pp(up,vp)； 桌面的宽度估计值为检测框对应单线激光雷达坐标系 的多个桌腿的最大横向距离的欧式距离； 获取彩色图的检测框的上下边的横坐标ut,ub， 桌腿在检测框中正上方和正下方对应的 像素坐标为(ut,vp)和(ub,vp)；权　利　要　求　书 2/3 页 3 CN 115205646 A 3