基于混合前沿采样和分层规划的自主探索方法及系统

本发明属于机器人路径规划领域，具体涉及基于混合前沿采样和分层规划的自主探索方法及系统。

背景技术：

1、机器人在未知环境下进行自主探索是无人平台系统研究的主要课题之一。在一些任务中，如三维重建、抢险救援、目标搜寻等，人类操作员并不总是能够控制机器人的运动并实时完成任务。这样的场景要求机器人具有较高的自主性，能够独立决定下一步的前进目标，完成环境绘制等任务。同时尽可能的提高探索效率，避免不必要的路线折返，以尽可能短的时间探索最多的区域。

2、自主探索技术的主流框架主要包括基于前沿的方法和基于采样的方法。两种方法本质上是在当前已绘制的地图上进行搜索或采样以获取边界区域，并将边界作为备选探索区域，通过评估备选边界的收益，选择收益最大的边界作为机器人的下一个探索区域，直到整个环境被完全建模。然而，这些方法仍然有一些局限性，首先，大多数方法都需要在已绘制的地图中搜索前沿区域或执行大量采样以获取视点。这对机器人搭载的边缘计算设备来说是巨大的负担，并可能导致机器人无法快速响应。尤其是当探索范围增大时，采样效率降低，无法实时生成新的前沿或有效视点。其次，常见的收益评估方法是最小化机器人到边界的距离，或最大化边界的信息增益。它们都忽略了有效的全局路径覆盖，使得机器人存在短视行为，导致低效率的回溯探索。

3、因此，在机器人自主探索中，快速地从环境中提取有效的前沿，减少探索过程中的折返运动，生成高效的全局覆盖路径，是提高自主探索效率的重要研究内容。

技术实现思路

1、本发明为了解决背景技术中存在探索方法计算负担大，探索效率低等技术问题，目的在于提供了基于混合前沿采样和分层规划的自主探索方法及系统，应用于地面轮式机器人，计算负担小、探索效率高。

2、为了解决技术问题，本发明的技术方案是：

3、基于混合前沿采样和分层规划的自主探索方法，所述方法包括：

4、步骤s1，利用激光雷达传感器和slam算法对周围已知环境进行建图，同时估计机器人的当前位姿；

5、步骤s2，设置采样条件和搜索范围，利用激光雷达点云数据和机器人周围区域的局部地图信息，获取潜在的前沿点集；

6、步骤s3，根据预设的过滤条件，对所述前沿点集进行过滤，得到有效前沿点集合；

7、步骤s4，将已绘制的地图划分为多个子区域，将所述有效前沿点集合中的点根据坐标位置划分至各自的子区域中，对子区域的访问顺序进行优化排序，并确定当前时刻应当访问的子区域；

8、步骤s5，对当前时刻应当访问的子区域内所有候选前沿点进行收益评估，选择收益最高的前沿点作为探索目标；

9、步骤s6，将探索目标点发送至路径规划模块中，并利用步骤s1获取的当前机器人位姿，驱动机器人前往目标区域；

10、步骤s7，检查前沿点集合中是否还存在前沿点，若不存在，则结束探索，否则继续重复上述步骤。

11、进一步，在所述步骤s1中，使用激光雷达传感器的点云数据作为slam算法的输入，使用slam算法根据当前点云数据生成环境的地图，地图的数据形式为二维栅格地图，每个栅格包括空闲、未知和占据信息，同时完成对机器人的位姿估计，位姿包括机器人在环境中的位置和姿态。

12、进一步，所述步骤s2具体包括：

13、s21：基于雷达点云的前沿点采样：首先将当前时刻下的点云数据转换至柱坐标系下，选取所有在指定高度上的点，将多个点根据从0°-360°的极角顺序进行排序，构成一组有序点云集合；在点云集合中，对于任意两个连续点，满足如下两个条件之一的：①|ri+1-ri|≥rgap，②θi+1-θi≥θinf，在两个点之间插入一个前沿点；其中，r和θ各自表示某一点在极坐标下的极径和极角，i表示该点在点云集合中的序列，rgap和θinf为阈值参数，可根据实际环境条件设定；

14、s22：基于图像处理的前沿点采样：首先将以机器人为中心，宽度和高度为k的二维栅格地图数据转化为图像数据；在ros(机器人操作系统)中，二维栅格地图的数据结构为一维数组，数组中的每一个元素值的范围为-1至100，对应该栅格的状态信息，值-1表示该地图栅格为未知状态，值0表示该栅格为空闲状态，其余值表示栅格的占据概率；使用opencv中cv_8s mat存储上述地图数据，按照从左下到右上的顺序，根据一维数组中的元素值，利用如下规则转化并存储在二维图像矩阵中，具体的储存规则的方式如下：

15、

16、其中，m(x,y)为二维图像矩阵在(x,y)上的像素值，a(x,y)为一维数组中对应位置(x,y)的栅格状态信息，将上述储存的图像称为原始图像，然后在原始图像进行下列操作，提取前沿点：

17、①对原始图像进行二值化处理，将m(x,y)值为0的像素设置为255，将其余像素值设置为0，得到二值图像；

18、②对原始图像进行边缘检测，得到边缘图像；

19、③对所述二值图像执行轮廓检测，并将检测到的轮廓绘制在二值图像上；

20、④对步骤③取得的二值图像取反操作，即将白色像素变黑色像素，黑色像素变白色像素，然后与边缘图像进行按位与操作，得到前沿区域的轮廓；

21、⑤对按位与操作后的图像进行轮廓检测，并获取轮廓的中心点像素坐标，并将像素坐标转换至世界坐标系下，即可获得基于图像处理方法采样的前沿点；

22、s23：将上述两种采样方法获取的前沿点进行汇总，得到初步的前沿点即潜在的前沿点集。

23、进一步，所述预设的过滤条件包括：该前沿点与障碍物距离小于特定值、该前沿点的视野范围内存在其他相邻的前沿点；同时在探索过程中应及时删除机器人已经接近过的前沿点和无法到达的前沿点。

24、进一步，在所述步骤s4中，子区域分割及访问顺序排序，包括：

25、分割子区域：根据当前已经绘制的地图，获取地图的最大矩形包围框，然后根据矩形包围框的大小，在矩形的长和宽方向上进行均等划分，得到n个子区域{sr1,sr2,...,srn}；

26、过滤子区域：将所述步骤s2中过滤后的所有前沿点按照其坐标位置，分配至各自属于的子区域中，并将内部没有前沿点的子区域移除，得到剩余m个子区域{sr1,sr2,...,srm},m≤n；

27、动态更新子区域：当已绘制的地图更新后，再次执行划分和过滤子区域步骤；

28、区域访问顺序排序：对当前所有过滤后的子区域，按照路径优化公式安排访问顺序：

29、

30、d(prob,pi)＝λ3·e(prob,p0)+e(prob,p0)+…+e(pi-1,pi)；

31、其中，g＝[sr1,sr2,...,srm]为剩余子区域所有可能的访问顺序的序列，d(prob,pi)为机器人位置prob到子区域中心位置pi的累计距离，λ为调节因子，并引入动态时间扭曲(dtw)方法计算全局路径序列g与上一次迭代中选择的路径序列之间的相似度；在上式中，e(pi-1,pi)表示两个子区域中心之间的欧氏距离，并引入λ3因子，使机器人从靠近自身的子区域开始探索，进一步地，上式在所有可能的访问顺序的序列中，选择得分最高的序列作为本次迭代的子区域访问顺序，并按照访问顺序，选择当前时刻应当访问的子区域。

32、进一步，在所述步骤s5中，对当前时刻应当访问的子区域内所有前沿点进行收益评估，评估函数为：

33、gain＝α1·||gji||-α2·||gjd||-α3·||gjo||，其中，表示为距离代价，使用机器人当前位置与待评估前沿点的欧式距离进行计算；表示为朝向代价，具体地可计算为其中θori为机器人速度方向与机器人和待评估前沿点之间连线的夹角；为信息增益代价，可表示为以待评估前沿点为中心、半径r范围内，栅格地图中未知区域的面积大小；α1,α2,α3为权重因子，将上述三种代价因素执行最小-最大归一化操作后再进行加权，得到收益分数gain，选择收益最大的前沿点作为下一个探索目标点。

34、进一步，在所述步骤s6中，将探索目标点发送至路径规划模块，路径规划模块规划出一条从机器人到达目标点的路径，并在行驶过程中避开障碍物；具体地，全局路径规划使用a*算法，利用定位信息和已经绘制的二维栅格地图完成全局路径规划；局部路径规划使用点云数据得到障碍物位置信息，结合时间弹性带teb算法实现局部避障，使机器人安全平稳地到达目标区域。

35、进一步，所述步骤s3中过滤后的所有前沿点集合在整个探索过程中维护并不断更新，当所有前沿区域已经探索完毕，即前沿点集合为空集且在环境中没有采样到新的前沿点时，结束探索过程；否则将继续重复上述操作。

36、基于混合前沿采样和分层规划的自主探索系统，所述系统包括：

37、一个或多个处理器；

38、存储器，用于存储一个或多个程序；

39、当所述一个或多个程序被所述一个或多个处理器执行时，使得所述一个或多个处理器执行上述中任一所述的基于混合前沿采样和分层规划的自主探索方法。

40、与现有技术相比，本发明的优点在于：

41、本发明提出了一种基于混合前沿采样的分层规划探索方法及系统。首先，采用了混合前沿采样的方法，使用当前帧的点云和局部范围地图中提取环境中的前沿点，这种方法无需遍历整个地图以搜索前沿区域，因此前沿采样效率不受环境规模或地图大小的影响；同时，利用图像处理技术在局部小范围地图中搜索前沿，能及时发现并检测到一些较小的前沿区域，提高了探索完成度。

42、其次，引入了分层规划策略，该策略首先将环境分割为若干个子区域，利用路径优化方法并对子区域的访问顺序进行排序，再决定下一步应该访问的子区域。由于路径优化公式考虑了访问这些子区域所需的总体路径长度，因此能够更合理地安排各个区域的访问顺序。相比于直接对每一个前沿点进行评估以决定下一个目标点，分层规划能够为机器人提供更全面的全局视角，有效减少探索的总体距离，缓解了探索过程中的短视问题。

43、再次，将混合前沿采样方法和分层规划策略进行结合，使探索系统更方便地响应环境变化，并能够实时更新勘探策略。通过划分区域，对整个勘探过程进行管理和优化，使机器人能够适应各种类型和规模的环境。

44、最后，在多个现实环境中进行了实机实验，实验结果体现了本发明在探索未知环境过程中的可行性与高效性。