基于深度摄像头的机器人临时避障方法
【专利摘要】本发明公开了一种基于深度摄像头的机器人临时避障方法，机器人在沿既定的全局导航路径行进的过程中如果检测到障碍物，则采用安装在机器人上的深度摄像头偏转左、右两个角度拍摄得到深度图，处理得到局部环境地图，在局部环境地图中检测障碍物左、右两侧的行进空间，选择宽度较大的一侧作为绕行行进空间，绘制绕行移动范围矩形，在绕行移动范围矩形内生成绕行可行点，从中选择绕行点，生成绕行路径，再将绕行路径映射到全局地图中，将映射后绕行路径的末点与原始全局导航路径上的下一个行进节点连接，完成临时避障。本发明可以高效、准确地完成机器人临时避障，并在绕行避障后返回原始全局导航路径，提高机器人的智能性。
【专利说明】
基于深度摄像头的机器人临时避障方法
技术领域
[0001 ]本发明属于机器人与计算机视觉技术领域，更为具体地讲，涉及一种基于深度摄 像头的机器人临时避障方法。
【背景技术】
[0002] 移动机器人是机器人领域一个重要分支，智能导航与临时避障是移动机器人最核 心的技术之一。机器人导航是指机器人在已知或者未知环境下自主的规划出一条从起点到 目标地点的移动路径，让机器人沿着这条路径能顺利的到达终点；机器人临时避障是指机 器人在移动过程中根据采集到的实时的环境信息，发现环境中新的障碍物，按照一定的方 法有效的避开障碍物，使导航能顺利进行。传统的机器人避障方法主要有可视图法、栅格 法、拓扑法、人工势场法，另外还有基于神经网络、遗传等算法的避障方法。
[0003] 对于已有的机器人避障方法，主要缺陷如下：
[0004] 1)可视图法缺乏灵活性，在障碍物较多的时候搜索时间长，并且要求障碍物形状 不能接近圆形，限制了其实际应用。
[0005] 2)栅格法是目前研究较多的避障方法，但是其中栅格的大小影响环境信息的存储 量和计算时间，栅格越大，分辨率越低，在复杂环境下避障效果越差，栅格越小，避障效果越 好，但计算量增加。
[0006] 3)拓扑法的主要问题在于构建拓扑网络的过程相当复杂，特别是在增加障碍物的 时候如何有效的修改拓扑关系还有待解决。
[0007] 4)人工势场法把移动机器人在环境中的运动视为在一种抽象的人造受力场中的 运动，目标点对机器人有吸引力，障碍物对机器人有排斥力，最终通过合力来控制机器人的 运动，其缺点是忽略了障碍物的结构外形信息，模型的建立容易产生误差并且陷入到局部 最小值。
[0008] 5)对于利用神经网络、遗传等算法进行避障，其主要缺陷在于算法过于复杂，对机 器人计算能力的要求非常高，推广性较弱。
【发明内容】
[0009] 本发明的目的在于克服现有技术的不足，提供一种基于深度摄像头的机器人临时 避障方法，采用深度摄像头获取局部环境地图生成绕行路径，然后映射到全局地图中，高 效、准确地完成机器人临时避障，并在绕行避障后返回原始全局导航路径，提高机器人的智 能性。
[0010] 为实现上述发明目的，本发明基于深度摄像头的机器人临时避障方法，其步骤包 括：
[0011] S1:机器人在沿既定的全局导航路径行进的过程中，对行进方向上的障碍物进行 检测，一旦检测到障碍物，进入步骤S2;
[0012] S2:在检测到障碍物后，机器人上所安装的深度摄像头分别向左、右偏转一定角 度，拍摄得到两幅深度图，将两幅深度图转换为3D点云图，分层压缩得到2D局部点云地图， 将两幅2D局部点云地图进行旋转拼接，得到局部环境地图；
[0013] S3:在步骤S2得到的局部环境地图中，以机器人当前位置为坐标原点、正前方为X 轴、左边为Y轴建立坐标系，然后检测得到障碍物点云的上、下、左、右边界，并且检测得到障 碍物左、右两侧的行进空间；
[0014] S4:判断是否有左、右两侧任意一侧行进空间宽度大于机器人直径D，如果是，进入 步骤S5，否则机器人停止前进；
[0015] S5:选择障碍物左、右两侧行进空间宽度较大的一侧作为绕行行进空间，在局部环 境地图中沿机器人在全局导航路径的行进方向上绘制一个标识绕行移动范围的矩形，该矩 形包含机器人、障碍物和绕行行进空间，且在机器人后方和障碍物前方根据预设尺寸预留 空余范围；
[0016] S6:在绕行移动范围矩形内，采用绕行可行点搜索算法生成绕行可行点；
[0017] S7:在步骤S6得到的绕行可行点中选择绕行点，生成绕行路径；
[0018] S8:根据机器人当前位置在全局地图中的坐标，将步骤S7中得到的绕行路径所经 过的绕行点坐标映射到全局地图中，将映射后绕行路径的末点与原始全局导航路径上的下 一个行进节点连接，机器人根据绕行路径行进。
[0019] 本发明基于深度摄像头的机器人临时避障方法，机器人在沿既定的全局导航路径 行进的过程中如果检测到障碍物，则采用安装在机器人上的深度摄像头偏转左、右两个角 度拍摄得到深度图，处理得到局部环境地图，在局部环境地图中检测障碍物左、右两侧的行 进空间，选择宽度较大的一侧作为绕行行进空间，绘制绕行移动范围矩形，在在绕行移动范 围矩形内生成绕行可行点，从中选择绕行点，生成绕行路径，再将绕行路径映射到全局地图 中，将映射后绕行路径的末点与原始全局导航路径上的下一个行进节点连接，完成临时避 障。
[0020] 本发明具有以下技术效果：
[0021] 1)将避障与导航融合到一起，避障结束之后机器人并不会丢失掉自己原始的导航 路径，而是在避障绕行后回到原始全局导航路径，因此任何新出现的障碍物都不会影响机 器人对整个环境的理解，提高机器人的智能性；
[0022] 2)避障方法简单易行，计算量小，并且所需配置的深度摄像机在机器人上也易于 安装和扩展，有利于工程应用推广。
【附图说明】
[0023] 图1是本发明基于深度摄像头的机器人临时避障方法流程图；
[0024] 图2是本实施例中机器人的全局导航路径示意图；
[0025]图3是本实施例中障碍物示例图；
[0026] 图4是基于作圆法的绕行可行点搜索算法；
[0027] 图5是绕行可行点搜索示例图；
[0028] 图6是本实施例中所获取的绕行可行点示意图；
[0029] 图7是本实施例中绕行路径生成方法的流程图；
[0030] 图8是本实施例中绕行路径映射至全局地图的绕行路径图。
【具体实施方式】
[0031] 下面结合附图对本发明的【具体实施方式】进行描述，以便本领域的技术人员更好地 理解本发明。需要特别提醒注意的是，在以下的描述中，当已知功能和设计的详细描述也许 会淡化本发明的主要内容时，这些描述在这里将被忽略。
[0032] 实施例
[0033] 图1是本发明基于深度摄像头的机器人临时避障方法流程图。如图1所示，本发明 基于深度摄像头的机器人临时避障方法包括以下步骤：
[0034] S101:障碍物检测：
[0035] 机器人在沿既定的全局导航路径行进的过程中，对行进方向上的障碍物进行检 测，一旦检测到障碍物，进入步骤S102。
[0036] 障碍物检测所使用的具体设备和算法可以根据实际需要来选择。本发明中由于机 器人配置了深度摄像头，因此本实施例中采用深度摄像头和超声波传感器结合进行障碍物 检测，其具体方法为：
[0037] 在机器人行进过程中，超声波传感器采用超声波检测机器行进方向上的障碍物， 超声波检测器的检测范围可以根据实际需要来设置，当检测到存在障碍物时，机器人后退 若干米，直到障碍物进行深度摄像对的可视范围，然后由深度摄像头拍摄机器人正前方的 深度图，将深度图转化为3D点云，对于每一帧3D点云图，将其分层压缩成2D局部点云地图， 在2D局部点云地图上建立以机器人当前位置为坐标原点、正前方为X轴、左边为Y轴的坐标 系，判断在{(x，y) |xG (-a，a)，yG (〇，b)}范围内是否有点云，如果在该范围内有点云，贝1J判 断存在障碍物，否则判断不存在障碍物。其中a、b根据实际情况设置，用于标识机器人的通 过路径。
[0038] 图2是本实施例中机器人的全局导航路径示意图。如图2所示，白色实线条为本实 施例中机器人的全局导航路径，是室内路径。
[0039] 图3是本实施例中障碍物示例图。如图3所示，图中灰色实线包围部分为障碍物。根 据图3标示可知，在{(1，7)|1￡(- &，&)，7^(〇，13)}存在代表障碍物的点云，从而检测得到障 碍物。
[0040] S102:获取局部环境地图：
[0041 ]在检测到障碍物后，机器人上所安装的深度摄像头分别向左、右偏转一定角度，拍 摄得到两幅深度图，将两幅深度图转换为3D点云图，分层压缩得到2D局部点云地图，将两幅 2D局部点云地图进行旋转拼接，得到局部环境地图。
[0042]本实施例中深度摄像头安装在机器人的头部，向下倾斜30度，在左、右偏转时，偏 转角度为30度。旋转拼接是图像处理领域的常用技术手段，在本实施例中旋转拼接的过程 如下：
[0043]设点a(x，y)为相机绕Y轴正方向旋转30度得到一个点，此时相机坐标系相对于原 始相机位置坐标系的旋转矩阵为：
[0045]
1将点a(x，y)旋转到原始坐标系下，其对应坐标alx'O为：
[0046] (x*,y*) = (x,y)XM
[0047] 对于相机绕Y轴负方向旋转30所得到的点云， ，同样按照上述公式计算即 可。将两帧2D点云转换到统一坐标系下并进行点云叠加，即可得到包含完整局部信息的局 部环境地图。
[0048] S103:障碍物边界和行进空间检测：
[0049] 为了对当前障碍物的结构信息有一个更为清晰的认识，需要在步骤S2得到的局部 环境地图中，以机器人当前位置为坐标原点、正前方为X轴、左边为Y轴建立坐标系，然后检 测得到障碍物点云的上、下、左、右边界，并且检测得到障碍物左、右两侧的行进空间。本实 施例中采用的边界和行进空间检测的具体方法为：
[0050] 在步骤S102得到的局部环境地图中，以机器人当前位置为坐标原点、正前方为X 轴、左边为Y轴建立坐标系，任意选取障碍物点云中一个点记为H，分别向左、向右按列遍历 局部环境地图，当某一列整列都没有点云时，则将其作为该方向的障碍物边界，将该边界作 为行进空间边界，然后继续遍历，当某一列存在点云时，则将该列作为行进空间的另一侧边 界，获取行进空间的宽度。在左、右边界内，以点H开始，分别向上、向下按行遍历局部环境地 图，如果某一行整列都没有点云时，则将其作为访方向的障碍物边界。
[0051]以图3所示的障碍物为例，A、B、C、D为障碍物点云的端点，xdPx2分别表示障碍物的 左、右边界。
[0052] S104:判断是否有左、右两侧任意一侧行进空间宽度大于机器人直径D，如果是，说 明当前行进空间可以使机器人通过，进入步骤S105,否则进入机器人停止前进，此时机器人 无法绕行，可以根据预设的策略来进行处理，例如将相关消息发送给服务器，然后由服务器 重新规划全局导航路径等等。
[0053] S105:确定绕行移动范围：
[0054]选择障碍物左、右两侧行进空间宽度较大的一侧作为绕行行进空间，在局部环境 地图中沿机器人在全局导航路径的行进方向上绘制一个标识绕行移动范围的矩形，该矩形 包含机器人、障碍物和绕行行进空间，且在机器人后方和障碍物前方根据预设尺寸预留空 余范围。空余范围的大小可以根据实际需要进行确定，主要是为了让机器人有一定的路线 调整空间。
[0055] S106:生成绕行可行点：
[0056] 在绕行移动范围矩形内，采用绕行可行点搜索算法生成绕行可行点。本实施例中 采用基于作圆法的绕行可行点搜索算法来生成绕行可行点。图4是基于作圆法的绕行可行 点搜索算法。如图4所示，基于作圆法的绕行可行点搜索算法的具体步骤为：
[0057] S401:初始化：
[0058] 以机器人当前位置作为圆点、大于机器人半径的预设半径r为半径作圆，机器人半 径是指机器人的最大半径，并将圆进行K等分，对等分点进行编号，将位于绕行移动范围内 的等分点作为第1代候选可行点。本实施例中K=16。令可行点代数i = l，初始化绕行可行点 集合S = 0。：
[0059] S402:搜索第i代绕行可行点：
[0060] 从第i代候选可行点中，依次计算每个候选可行点到最近障碍物点的距离h，如果r <h<Ar，其中A>1，则将该候选可行点作为本代的绕行可行点，放入第i代绕行可行点集合 Sl和绕行可行点集合S，否则不作为绕行可行点。A用于保证绕行可行点与障碍物不至于相 隔太远，以免绕行太远，降低机器人行进效率。根据实验统计可知，A的取值范围为 时可以达到较好的效果。
[0061] S403:判断是否81为空集，即第i代绕行可行点数量为零，如果是，则说明已经无法 搜索到新的绕行可行点，搜索结束，否则进入步骤S404。
[0062] S404:获取第i+1代候选可行点：
[0063] 从第i代绕行可行点集合Sl中取出1个绕行可行点，以该绕行可行点为圆心、该绕 行可行点到最近障碍物点的距离h为半径作圆，并将圆进行K等分，筛选位于绕行移动范围 内、之前所绘制的所有历史圆的覆盖范围以外的等分点作为候选可行点，将绕行可行点集 合S中位于该绕行可行点所绘制圆以内的绕行可行点删除，并将该绕行可行点和位于该绕 行可行点所绘制圆以内的绕行可行点从第i代绕行可行点集合 Sl中删除，然后从第i代绕行 可行点集合Sl中重新取出1个绕行可行点搜索第i + 1代候选可行点，直到第i代绕行可行点 集合Si为空。
[0064] 也就是说，在选择候选可行点的时候，还会对之前产生的绕行可行点进行再次筛 选，从而减少冗余点，提高算法效率。
[0065] S405:令 i = i+l，返回步骤 S402。
[0066] 图5是绕行可行点搜索示例图。如图5所示，方块点表示最近障碍物点，灰色圆点表 示绕行可行点和候选可行点，黑色圆点表示被排除的点。其中（a)表示以机器人当前位置为 圆心的第1个圆，此时假定所有的等分点都在绕行移动范围内，那么所有等分点均作为第1 代候选可行点。在图5中（b)中，该点至最近障碍物点的距离满足要求，因此将该点作为绕行 可行点。然后以该绕行可行点作圆，该圆的等分点中位于第1个圆内的点不作为候选可行 点，而原来第1个圆中位于该绕行可行点所绘制圆内部的可行点也被排除了。在图5中（c) 中，新绘制圆所对应的点至最近障碍物点的距离不满足要求，不能作为绕行可行点。在图5 中（d)中，新绘制圆所对应的点至最近障碍物点的距离满足要求，因此以该绕行可行点作 圆，该圆的等分点中位于第1个圆和第2个圆内的点不作为候选可行点，而原来第1个圆和第 2个圆内中位于该绕行可行点所绘制圆内部的可行点也被排除了。
[0067] 图6是本实施例中所获取的绕行可行点示意图。如图6所示，本实施例中，共计得到 了 10个绕行可行点。
[0068] S107:生成绕行路径：
[0069] 在获取绕行移动范围内的绕行可行点之后，需要在绕行可行点中选择绕行点，生 成绕行路径。
[0070] 图7是本实施例中绕行路径生成方法的流程图。如图7所示，本实施例中所采用的 绕行路径生成方法具体步骤为：
[0071] S701:生成绕行点集合：
[0072]分别求取步骤S109得到的每个绕行可行点与机器人当前位置的距离，根据距离从 小到大对绕行可行点进行排序，即距离越大，编号越靠后，得到排序后的绕行可行点的集合 Po
[0073] S702:初始化参数：
[0074]令绕行点序号k = l，第0个绕行点为机器人当前位置。
[0075] S703:选择第k个绕行点：
[0076]按照顺序依次连接第k_l个绕行点与绕行可行点集合P中序号大于第k_l个绕行点 的绕行可行点，将连接线段按照机器人半径划分为小段，计算每个分段点与其最近障碍物 点的距离，如果所有分段点与最近障碍点的距离都大于机器人半径，则将该绕行可行点放 入集合Z，否则不作任何操作。在集合Z中选择编号最大的绕行可行点作为第k个绕行点。 [0077] S704:判断第k个绕行点是否存在，如果存在，进入步骤S705，否则进入步骤S706。
[0078] S705:令 k = k+l，返回步骤 S702。
[0079] S706:得到绕行路径：
[0080] 将得到的绕行点按照其序号连接，得到机器人的绕行路径。
[0081] 以图6所示的10个绕行可行点为例，按照与机器人当前位置的距离进行排序，得到 PfPn)，假设当前位置所能到达的最远点为P?，那么第1个绕行点即为P?。然后再从P?开始， 判断P 8、P9和P1Q，从P?开始所有到达的距离机器人当前位置最远的点为P8,即第2个绕行点即 为P 8。那么再从P8开始，判断P9和P1Q，从P8开始所能到达的距离机器人当前位置最远的点为 P 1〇,第3个绕行点即为P1Q。那么绕行路径即为机器人从当前位置出发，先直线行进至P7,然后 行进至P 8，最后行进至P1()，从而绕开障碍物。
[0082] S108:绕行路径映射：
[0083]根据机器人当前位置在全局地图中的坐标，将步骤S110中得到的绕行路径所经过 的绕行点坐标映射到全局地图中，将映射后绕行路径的末点与原始全局导航路径上的下一 个行进节点连接，机器人根据绕行路径行进。根据绕行路径，可以指引机器人回到原始全局 导航路径上，完成障碍物绕行。
[0084] 根据本发明中局部环境地图的坐标系建立方式可知，在局部环境地图中机器人当 前位置的坐标为(〇,〇)，假设机器人当前位置在全局地图中的坐标是(X，Y)，那么绕行点在 两个坐标系下的是简单的(X，Y)的平移关系。
[0085] 图8是本实施例中绕行路径映射至全局地图的绕行路径图。由于在全局地图中障 碍物是不可见的，因此图8中没有需要绕行的障碍物点云。如图8所示，原始全局导航路径为 直线，但是由于中间出现了障碍物，需要进行临时避障，采用本发明得到的绕行路径为右侧 所示折线。可见，本发明可以有效实现机器人在行进过程中的临时避障，且能在避障绕行后 回到原始全局导航路径，提高机器人的智能性。
[0086] 尽管上面对本发明说明性的【具体实施方式】进行了描述，以便于本技术领域的技术 人员理解本发明，但应该清楚，本发明不限于【具体实施方式】的范围，对本技术领域的普通技 术人员来讲，只要各种变化在所附的权利要求限定和确定的本发明的精神和范围内，这些 变化是显而易见的，一切利用本发明构思的发明创造均在保护之列。
【主权项】
1. 一种基于深度摄像头的机器人临时避障方法，其特征在于，包括以下步骤： S1:机器人在沿既定的全局导航路径行进的过程中，对行进方向上的障碍物进行检测， 一旦检测到障碍物，进入步骤S2; S2:在检测到障碍物后，机器人上所安装的深度摄像头分别向左、右偏转一定角度，拍 摄得到两幅深度图，将两幅深度图转换为3D点云图，分层压缩得到2D局部点云地图，将两幅 2D局部点云地图进行旋转拼接，得到局部环境地图； S3:在步骤S2得到的局部环境地图中，以机器人当前位置为坐标原点、正前方为X轴、左 边为Y轴建立坐标系，然后检测得到障碍物点云的上、下、左、右边界，并且检测得到障碍物 左、右两侧的行进空间； S4:判断是否有左、右两侧任意一侧行进空间宽度大于机器人直径D，如果是，进入步骤 S5,否则进机器人停止前进； S5:选择障碍物左、右两侧行进空间宽度较大的一侧作为绕行行进空间，在局部环境地 图中沿机器人在全局导航路径的行进方向上绘制一个标识绕行移动范围的矩形，该矩形包 含机器人、障碍物和绕行行进空间，且在机器人后方和障碍物前方根据预设尺寸预留空余 范围； S6:在绕行移动范围矩形内，采用绕行可行点搜索算法生成绕行可行点； S7:在步骤S6得到的绕行可行点中选择绕行点，生成绕行路径； S8:根据机器人当前位置在全局地图中的坐标，将步骤S7中得到的绕行路径所经过的 绕行点坐标映射到全局地图中，将映射后绕行路径的末点与原始全局导航路径上的下一个 行进节点连接，机器人根据绕行路径行进。2. 根据权利要求1所述的机器人临时避障方法，其特征在于，所述步骤S1中障碍物检测 采用基于深度图像的障碍物检测算法，具体方法为:超声波传感器采用超声波检测机器行 进方向上的障碍物，当检测到存在障碍物时，机器人后退若干米，直到障碍物进入深度摄像 头的可视范围，然后由深度摄像头拍摄机器人正前方的深度图，将深度图转化为3D点云，对 于每一帧3D点云图，将其分层压缩成2D局部点云地图，在2D局部点云地图上建立以机器人 当前位置为坐标原点、正前方为X轴、左边为Y轴的坐标系，判断在{( X，y)|XG(-a，a)，yG (0，b)}范围内是否有点云，如果在该范围内有点云，则判断存在障碍物，否则判断不存在障 碍物。3. 根据权利要求1所述的机器人避障方法，其特征在于，所述步骤S6中绕行可行点搜索 算法为基于作圆法的绕行可行点搜索算法，其具体步骤为： S6.1:以机器人当前位置作为圆点、大于机器人半径的预设半径r为半径作圆，并将圆 进行K等分，对等分点进行编号，将位于绕行移动范围内的等分点作为第1代候选可行点;令 可行点代数i = 1，初始化绕行可行点集合$ = 0 ; S6.2:从第i代候选可行点中，依次计算每个候选可行点到最近障碍物点的距离h，如果 r<h<Ar，其中A>1，则将该候选可行点作为本代的绕行可行点，放入第i代绕行可行点集 合^和绕行可行点集合S，否则不作为绕行可行点； S6.3:如果第i代绕行可行点数量为零，搜索结束，否则进入步骤S6.4; S6.4:从第i代绕行可行点集合Sl中取出1个绕行可行点，以该绕行可行点为圆心、该绕 行可行点到最近障碍物点的距离h为半径作圆，并将圆进行K等分，筛选位于绕行移动范围 内、之前所绘制的所有历史圆的覆盖范围以外的等分点作为候选可行点，将绕行可行点集 合S中位于该绕行可行点所绘制圆以内的绕行可行点删除，并将该绕行可行点和位于该绕 行可行点所绘制圆以内的绕行可行点从第i代绕行可行点集合 Sl中删除，然后从第i代绕行 可行点集合Sl中重新取出1个绕行可行点搜索第i+1代候选可行点，直到第i代绕行可行点 集合Si为空； 36.5:令1 = 1+1，返回步骤56.2。4. 根据权利要求3所述的机器人临时避障方法，其特征在于，所述步骤S6.2中A的取值 范围为2<A<5。5. 根据权利要求1所述的机器人临时避障方法，其特征在于，所述步骤S7中绕行路径生 成方法的具体步骤包括： S7.1:分别求取步骤S6得到的每个绕行可行点与机器人当前位置的距离，根据距离从 小到大对绕行可行点进行排序，得到排序后的绕行可行点的集合P; S7.2:令绕行点序号k= 1，第0个绕行点为机器人当前位置； S7.3:按照顺序依次连接第k-1个绕行点与绕行可行点集合P中序号大于第k-1个绕行 点的绕行可行点，将连接线段按照机器人半径划分为小段，计算每个分段点与其最近障碍 物点的距离，如果所有分段点与最近障碍点的距离都大于机器人半径，则将该绕行可行点 放入集合Z，否则不作任何操作;在集合Z中选择编号最大的绕行可行点作为第k个绕行点； S7.4:如果第k个绕行点是否存在，进入步骤S7.5，否则令令k = k+l，返回步骤S7.2; S7.5:将得到的绕行点按照其序号连接，得到机机器人的绕行路径。
【文档编号】G05D1/02GK106054900SQ201610643767
【公开日】2016年10月26日
【申请日】2016年8月8日 公开号201610643767.5, CN 106054900 A, CN 106054900A, CN 201610643767, CN-A-106054900, CN106054900 A, CN106054900A, CN201610643767, CN201610643767.5
【发明人】程洪, 王富奎, 郑亚莉, 陈诗南, 骆佩佩
【申请人】电子科技大学