一种基于启发式偏置采样的室内环境机器人探索方法与流程

1.本发明涉及机器人技术领域，特别涉及一种基于启发式偏置采样的室内环境机器人探索方法。


背景技术：

2.随着计算机软硬件技术的发展以及人工智能技术的突破，服务型移动机器人的研究取得了瞩目的成绩，智能公共服务机器人应用场景和服务模式不断拓展，带动我国服务机器人市场规模高速增长。移动型机器人自主探索是指机器人在没有任何先验知识的情况下，在一个新的环境中通过移动而建立完整环境地图的过程。
3.目前，主流的自主探索算法是基于边界点的探索方法，边界将探测空间分为已知区域和未知区域，引导机器人向未知区域探索可以逐步完成对整个环境的建模。在火灾救援、矿井探测等时间要求苛刻的场合，快速完成对整个环境的建模，可以有效帮助施救、开采等任务的进行。
4.为了更快速的完成环境建模任务，广大学者的研究重心聚焦于如何探测和选择边界点，边界点的选择直接影响探索效率。然而这类探索策略仅仅考量的是当前时刻的探索路径成本和更新该边界点周围小部分地图带来的的信息收益，而忽略环境结构的潜在收益。比如，在室内环境下，机器人在进入一个房间探索后，一般会有两种动作：1、将房间区域探索完成再转向房间外探索；2、房间区域还未探索完成就转向了房间外，之后再次进入房间探索。显然，第二种动作更浪费时间，效率更低。
5.基于快速搜索随机树(rrt)的自主探索策略的核心思想是一种贪婪策略，且边界点的信息增益模型也不设计对周围环境的考量，那么机器人在每一时刻会无视周围已探索的环境结构而选取收益值最大的边界点作为目标点。比如，机器人在一个房间区域探索时，如果房间外的边界点的收益值较大，机器人就转向房间外探索，之后绕回把房间区域探索完毕，这便产生了回溯现象；其次，由于rrt算法本身的随机性，在对边界点的提取时，往往不能及时的将机器人前进区域的边界点提取出去，那么机器人也会出现选择另一个区域探索，又回头探索的现象。导致了机器人探索慢、效率低的问题。


技术实现要素：

6.本发明要解决的技术问题是提供一种探索快、探索效率高的基于启发式偏置采样的室内环境机器人探索方法。
7.为了解决上述问题，本发明提供了基于启发式偏置采样的室内环境机器人探索方法，其包括：
8.a、初始化；
9.b、进行定位和建图；
10.c、在地图的空闲区域生长两棵快速搜索随机树，利用所述两棵快速生长随机树进行边界点的提取，记为rrt边界点；进行启发式物体的识别并获取启发式物体的位置信息，
以启发式物体的位置构造先验区域，在所述先验区域内通过偏置采样快速搜索随机树算法提取边界点，记为房间边界点；剔除rrt边界点和房间边界点中的无效边界点；
11.d、当房间边界点存在时，机器人优先选择收益值最大的房间边界点作为目标点进行探索，并在所有房间边界点探索完成后对rrt边界点进行探索，且优先选择收益值最大的rrt边界点作为目标点进行探索；
12.e、引导机器人向目标点导航并更新地图；
13.f、当先验区域内检测不到边界点时，销毁先验区域；
14.g、循环步骤a
‑
f，直至完成整个环境探索。
15.作为本发明的进一步改进，步骤b具体包括：通过slam模块接收传感器数据，更新部分地图为已知的区域，同时通过slam模块构建的地图反过来修正机器人的位姿。
16.作为本发明的进一步改进，所述两棵快速搜索随机树分为全局树和局部树，全局树和局部树均通过以下步骤进行rrt边界点的提取；具体包括：
17.c11、将起点添加到树结构中作为根节点，两棵快速搜索随机树的起点都是人为在地图的空闲区域内设定的；
18.c12、在地图区域内随机撒点作为候选点；
19.c13、若该候选点在已知区域内，则遍历树结构上的所有已有的节点，选取距离候选点最近的节点作为最邻近点，以最邻近点到候选节点的连线作为生长方向；如果最邻近点与候选节点的距离超过预先设定的步长，则由最邻近点沿着生长方向生长一个步长，到达的点作为生长点；如果距离不超过步长，则该候选点作为生长点；
20.若候选点在未知区域内，则先找到该候选点的最邻近节点，最邻近点到候选点的连线作为生长方向，然后由最邻近点沿着生长方向向前生长，到达边界的地方作为边界点；
21.c14、将生长点和候选节点的连线在地图上做碰撞检测，通过遍历这条直线上所有的栅格点，然后判断该栅格点的栅格状态，如果该栅格点的状态是被障碍物占据的，则碰撞检测不通过，返回步骤c12重新进行采样；如果连线没有碰到障碍物，就将该候选点、生长点和候选节点的连线添加到树结构中；
22.其中，当局部树探测到边界点后，局部树会被清除并在机器人当前位置重新生长。
23.作为本发明的进一步改进，在所述先验区域内通过偏置采样快速搜索随机树算法提取边界点，记为房间边界点；具体包括：
24.c21、将启发式物体的位置添加到树结构中作为根节点；
25.c22、偏置采样快速搜索随机树算法每次使用一个随机函数来决定采用哪一种采样方式；所述采样方式包括均匀随机采样和偏置采样；
26.c23、遍历树结构上的所有已有的节点，选取距离候选点最近的节点作为最邻近点，最邻近点到候选节点的连线作为生长方向；如果最邻近点与候选节点的距离超过预先设定的步长，则由最邻近点沿着生长方向生长一个步长，到达的点作为生长点；如果距离不超过步长，则该候选点作为生长点；
27.c24、将生长点和候选节点的连线在地图上做碰撞检测，碰撞测试通过则进入步骤c25，否则返回步骤c22；
28.c25、判断通过碰撞测试的生长点是否为边界点，并改变部分边界点的栅格状态；
29.c26、根据当前时刻的边界点数目与总边界点数目之比计算覆盖率。
30.作为本发明的进一步改进，步骤c24具体包括：遍历生长点和候选节点的连线所在直线上的所有的栅格点，然后判断该栅格点的栅格状态，如果该栅格点的状态是被占据的或者是未知的，则碰撞检测不通过，返回步骤c22重新进行采样；如果连线没有碰到障碍物和未知区域，则将该候选点、生长点和候选节点的连线添加到树结构中。
31.作为本发明的进一步改进，所述剔除rrt边界点和房间边界点中的无效边界点，具体包括：
32.c31、将所述rrt边界点和房间边界点通过mean
‑
shift聚类算法聚类后得到质心点；
33.c32、实时检测该边界点的栅格状态以及在costmap中的值，若该栅格状态是空闲的且costmap中值超过某一阈值，则该栅格点也作为无效点剔除。
34.作为本发明的进一步改进，所述以启发式物体的位置构造先验区域，具体包括：
35.当机器人识别到启发式物体时，若机器人位置在启发式物体的下方，则所述先验区域在启发式物体的位置的上方；当机器人位置在启发式物体的上方时，则所述先验区域在启发式物体的位置的下方。
36.作为本发明的进一步改进，所述先验区域的长度分别为启发式物体的位置向两侧延伸长度a，先验区域的宽度为启发式物体的位置向后延伸长度2b；其中，2≤a≤10,2≤b≤10。
37.作为本发明的进一步改进，步骤e具体包括：使用a*全局路径规划算法在已知的环境中规划出一条由机器人当前位置到目标点的路径，并结合dwa局部路径规划算法使机器人很好的利用局部环境信息完成避障，将两者相结合引导机器人向目标点导航并更新地图。
38.作为本发明的进一步改进，在步骤a中，整个环境都是未知的，机器人处于环境中的某一位置，机器人通过自身携带的传感器获取周围环境信息的数据。
39.本发明的有益效果：
40.本发明基于启发式偏置采样的室内环境机器人探索方法通过采样启发式先验信息探索，当机器人识别到启发式物体后，优先探索先验区域内的环境，可以使机器人优先探索完一个房间区域再转向其它区域，有效减少探索过程中的回溯现象，提升探索效率。
41.上述说明仅是本发明技术方案的概述，为了能够更清楚了解本发明的技术手段，而可依照说明书的内容予以实施，并且为了让本发明的上述和其他目的、特征和优点能够更明显易懂，以下特举较佳实施例，并配合附图，详细说明如下。
附图说明
42.图1是本发明优选实施例中基于启发式偏置采样的室内环境机器人探索方法的流程图；
43.图2是本发明优选实施例中构造的先验区域的示意图；
44.图3是本发明优选实施例中实验场景一的示意图；
45.图4是本发明优选实施例中实验场景二的示意图；
46.图5是本发明优选实施例中实验场景三的示意图。
具体实施方式
47.下面结合附图和具体实施例对本发明作进一步说明，以使本领域的技术人员可以更好地理解本发明并能予以实施，但所举实施例不作为对本发明的限定。
48.如图1所示，为本发明优选实施例中的基于启发式偏置采样的室内环境机器人探索方法，包括以下步骤：
49.a、初始化；整个环境都是未知的，机器人处于环境中的某一位置，机器人通过自身携带的传感器获取周围环境信息的数据。
50.b、进行定位和建图；具体包括：通过slam模块接收传感器数据，更新部分地图为已知的区域，同时通过slam模块构建的地图反过来修正机器人的位姿。
51.c、在地图的空闲区域生长两棵快速搜索随机树，利用所述两棵快速生长随机树进行边界点的提取，记为rrt边界点；进行启发式物体的识别并获取启发式物体的位置信息，以启发式物体的位置构造先验区域，在所述先验区域内通过偏置采样快速搜索随机树算法提取边界点，记为房间边界点；剔除rrt边界点和房间边界点中的无效边界点。
52.其中，所述两棵快速搜索随机树分为全局树和局部树，全局树和局部树均通过以下步骤进行rrt边界点的提取；具体包括：
53.c11、将起点添加到树结构中作为根节点，两棵快速搜索随机树的起点都是人为在地图的空闲区域内设定的；
54.c12、在地图区域内随机撒点作为候选点；
55.c13、若该候选点在已知区域内，则遍历树结构上的所有已有的节点，选取距离候选点最近的节点作为最邻近点，以最邻近点到候选节点的连线作为生长方向；如果最邻近点与候选节点的距离超过预先设定的步长，则由最邻近点沿着生长方向生长一个步长，到达的点作为生长点；如果距离不超过步长，则该候选点作为生长点；
56.若候选点在未知区域内，则先找到该候选点的最邻近节点，最邻近点到候选点的连线作为生长方向，然后由最邻近点沿着生长方向向前生长，到达边界的地方作为边界点，这一过程在算法方面的体现是从最邻近点沿生长方向开始遍历，栅格状态为未知的作为边界点；
57.c14、将生长点和候选节点的连线在地图上做碰撞检测，通过遍历这条直线上所有的栅格点，然后判断该栅格点的栅格状态，如果该栅格点的状态是被障碍物占据的，则碰撞检测不通过，返回步骤c12重新进行采样；如果连线没有碰到障碍物，就将该候选点、生长点和候选节点的连线添加到树结构中；
58.其中，当局部树探测到边界点后，局部树会被清除并在机器人当前位置重新生长。
59.可选的，进行启发式物体的识别并获取启发式物体的位置信息，具体包括：
60.s1、使用一种基于深度学习的目标检测方法进行启发式物体的识别，它是在yolov4_tiny的基础上改进的轻量级网络。这个轻量级网络主要由卷积层、逆残差块、池化层和spp(spatial pyramid pooling)层组成，共42层，输出简化为两层。与yolov4_tiny相比，这一轻量级网络的检测准确率提高了19.2％，接近yolov4，但速度几乎是yolov4的4倍。总的来说，这一轻量级网络在速度和精度方面都是非常有效的，它适合于启发式目标识别。
61.s2、当使用以上网络检测到启发式物体后，就可以获取到启发式物体在像素平面下的坐标信息。接下来实现从图像坐标系到相机坐标系再到世界坐标系的转换，假设输出
的启发式物体的中心点p在图像坐标系下的坐标为(u,v)，其在相机坐标系下的坐标为(x
c
,y
c
,z
c
)，在世界坐标系下的坐标为(x,y,z)，它们之间存在如下映射关系:
[0062][0063][0064][0065]
为了方便实际转换计算，本发明用齐次坐标表示相机参数，所以二维点到三维点的转换可表示为:
[0066][0067]
其中，相机内参(f
x
,f
y
,c
x
,c
y
)可通过订阅深度相机在ros下发布的话题得到。
[0068]
在本实施例中，以启发式物体的位置为基准构造先验区域，此区域是遵从人类的感知习惯，门后所在的区域是一个房间，区域的大小是人为经验设定的，且保证在大多数场合略大于实际区域大小。具体包括：当机器人识别到启发式物体时，若机器人位置在启发式物体的下方，则预估的先验区域在启发式物体的位置的上方；同理，当机器人位置在启发式物体的上方时，预估的先验区域在启发式物体的位置的下方。可选的，预估的先验区域的长度分别为启发式物体的位置向两侧延伸长度a，预估区域的宽度为启发式物体的位置向后延伸长度2b。本实施例中，其中，2≤a≤10,2≤b≤10；可选的，a和b均设置为6。参照图2。
[0069]
其中，在所述先验区域内通过偏置采样快速搜索随机树算法提取边界点，记为房间边界点；具体包括：
[0070]
c21、将启发式物体的位置添加到树结构中作为根节点；
[0071]
c22、偏置采样快速搜索随机树算法(bias_rrt)每次使用一个随机函数来决定采用哪一种采样方式；所述采样方式包括均匀随机采样和偏置采样；其中，所述均匀随机采样即在给定的地图区域每一个位置采样的概率相同；所述偏置采样只选择地图中的边界位置采样。地图的边界位置包含空闲区域与障碍物区域的交界，空闲区域和未知区域的交界，可以直接使用opencv(计算机视觉中经典的专用库)中图像轮廓的提取方法提取出这些边界，并计算出此时的边界点数目，记为总边界点数目。本发明bias_rrt算法每次都使用一个随机函数来决定采用哪一种采样方式；
[0072]
c23、遍历树结构上的所有已有的节点，选取距离候选点最近的节点作为最邻近点，最邻近点到候选节点的连线作为生长方向；如果最邻近点与候选节点的距离超过预先设定的步长，则由最邻近点沿着生长方向生长一个步长，到达的点作为生长点；如果距离不超过步长，则该候选点作为生长点；
[0073]
c24、将生长点和候选节点的连线在地图上做碰撞检测，碰撞测试通过则进入步骤c25，否则返回步骤c22；其中，碰撞测试的原理是遍历这条直线上所有的栅格点，然后判断
该栅格点的栅格状态，如果该栅格点的状态是被占据的或者是未知的，则碰撞检测不通过，返回步骤c22重新进行采样；如果连线没有碰到障碍物和未知区域，就将该候选点、生长点和候选节点的连线添加到树结构中；(对比rrt算法提取边界点的过程，本发明bias_rrt算法在碰撞检测时将被占据的和未知的栅格状态都看成障碍物，这样可以快速进行树结构的建立)；
[0074]
c25、判断通过碰撞测试的生长点是否为边界点，并改变部分边界点的栅格状态；
[0075]
具体的，判定方法为检查在以生长点为中心的正方形区域内是否有栅格状态为被占据的栅格点和未知的栅格点，若正方形区域内有被占据的栅格点，则说明树生长到了障碍物附近，同时改变这些正方形区域内栅格点的状态为已知的，表明这一块区域已被树结构覆盖，那么步骤c22中的总边界点数目经过此处理将会减少；若正方形区域内有未知的栅格点，则说明树生长到了未知区域附近，同时改变正方形区域内栅格点的状态为已知的，表明这一块区域已被树结构覆盖，并将该点作为房间边界点发布出去，这些房间边界点将作为目标点引导机器人向着房间区域探索。同样的，因为改变了正方形区域内栅格点的状态，步骤c22中的总边界点数目会再次减少；
[0076]
c26、根据当前时刻的边界点数目与总边界点数目之比计算覆盖率。
[0077]
具体的，覆盖率计算公式为：
[0078][0079]
其中，n
now
为当前时刻的边界点数目，n
total
为总边界点数目。每次覆盖率计算都会和预先设定的阈值进行比较，如果小于预先设定的阈值，则表示树结构已经覆盖完整个先验区域，这一标志会作为先验区域是否探索完成的重要指标，本实施例中阈值设置为0.1。由于rrt算法本身的随机性，在某些极端情况，不能及时提取所有的边界点，所以将覆盖率和先验区域没有边界点两者共同作为先验区域探索完成的标志，可以有效使机器人优先探索完先验区域，避免回溯现象。
[0080]
其中，所述剔除rrt边界点和房间边界点中的无效边界点，具体包括：
[0081]
c31、将所述rrt边界点和房间边界点通过mean
‑
shift聚类算法聚类后得到质心点；
[0082]
c32、实时检测该边界点的栅格状态以及在costmap中的值，若该栅格状态是空闲的且costmap中值超过某一阈值，则该栅格点也作为无效点剔除。
[0083]
具体的，将探测到的边界点通过mean
‑
shift聚类算法聚类后得到质心点，这样可以过滤掉一部分边界点，减少计算消耗。同时在每一时刻，都检测该边界点的栅格状态以及在costmap(代价地图)中的值(costmap将每一栅格值划分在0～255之间，白色值为255，代表空闲状态；黑色值为0，代表障碍物；之间的值是灰色，代表未知的)，若该栅格状态是空闲的(表明该栅格点已被探索)且costmap中值超过某一阈值，则表明该栅格点所在区域已被探索，该栅格点也应该作为无效点剔除。
[0084]
d、当房间边界点存在时，机器人优先选择收益值最大的房间边界点作为目标点进行探索，并在所有房间边界点探索完成后对rrt边界点进行探索，且优先选择收益值最大的rrt边界点作为目标点进行探索。
[0085]
具体的，机器人优先选择房间边界点探索，使机器人在识别到启发式物体后，优先
进入先验区域探索。当房间边界点存在时，表示先验区域还未探索完成，然后将房间边界点探索完后才会选择rrt边界点进行探索。这样机器人便能按照人为的设想，将一个区域探索完毕再转向别的区域探索；
[0086]
当有多个边界点存在时，我们为这些边界点设计了收益函数，房间边界点收益函数的计算公式同rrt边界点相同，但是这两类边界点分开比较，即当房间边界点存在时，选择收益值最大的房间边界点作为目标点；当没有房间边界点存在时，则选取最大的rrt边界点作为目标点；边界点的收益函数如下：
[0087]
r1
f
＝w1*i
f
‑
w2*n
f
[0088]
其中，i
f
为信息增益：即在质心点的信息增益半径r＝1内未知的栅格的个数；n
f
为路径成本：即机器人当前位置与质心点位置的欧式距离；本实施例中w1设定为3，w2设定为1。
[0089]
e、引导机器人向目标点导航并更新地图；具体包括：使用a*全局路径规划算法在已知的环境中规划出一条由机器人当前位置到目标点的路径，并结合dwa局部路径规划算法使机器人很好的利用局部环境信息完成避障，将两者相结合引导机器人向目标点导航并更新地图。
[0090]
f、当先验区域内检测不到边界点时，销毁先验区域；具体的，当先验区域内检测不到边界点时，表示该区域已经探索完成，则销毁先验区域模型以便下一个启发式区域的形成。
[0091]
g、循环步骤a
‑
f，直至完成整个环境探索。
[0092]
为充分证明本发明的有效性，本发明与基于快速搜索随机树的自主探索算法(以下用“rrts算法”表示)在两个仿真场景下进行了对比实验。对于每一个实验环境，总共进行了20次实验。包括10次使用rrts算法，10次我们改进后的算法。实验对比的指标是探索整个环境所使用的时间以及行驶的路径长度。
[0093]
实验数据如表1、表2和表3所示，分别对应实验场景一、二和三。在第一个实验场景中(如图3)，本发明的方法相比于rrts算法，探索时间减少了31.2％，探索路径长度减少了21.8％。在第二个实验场景中(如图4)，我们的方法相比于rrts算法，探索时间减少了13.4％，探索路径长度减少了17.2％。在第三个实验场景中(如图5)，我们的方法相比于rrts算法，探索时间减少了21.9％，探索路径长度减少了22.7％。
[0094]
表一
[0095][0096]
表二
[0097][0098][0099]
表三
[0100][0101]
本发明基于启发式偏置采样的室内环境机器人探索方法通过采样启发式先验信息探索，当机器人识别到启发式物体后，优先探索先验区域内的环境，可以使机器人优先探索完一个房间区域再转向其它区域，有效减少探索过程中的回溯现象，提升探索效率。
[0102]
以上实施例仅是为充分说明本发明而所举的较佳的实施例，本发明的保护范围不限于此。本技术领域的技术人员在本发明基础上所作的等同替代或变换，均在本发明的保护范围之内。本发明的保护范围以权利要求书为准。