基于蚂蚁爬行机制的半未知环境下移动机器人路径探索方法

本发明涉及移动机器人路径规划，尤其涉及一种基于蚂蚁爬行机制的半未知环境下移动机器人路径探索方法。

背景技术：

1、随着科技的快速发展，移动机器人已经被广泛的应用于人们的生产生活中。例如在地面自主导航、资源勘探开发、抢险救灾等多个领域，移动机器人可以代替或协助人类完成各项复杂工作。同时随着移动机器人应用场景和服务模式的不断扩展，人们对移动机器人的智能化程度要求也越来越高，而移动机器人的智能化核心技术是路径规划技术。路径规划是指移动机器人在复杂空间环境中探寻一条从起点到终点的高质量无碰撞路径，一个好的路径规划技术可以有效地提高移动机器人对资源的利用率，降低移动机器人的损耗，延长移动机器人的使用寿命。

2、目前移动机器人路径规划的主流算法分为两大类：传统算法与智能仿生学算法。其中，传统算法主要有a*算法、人工势场法等。智能仿生学算法主要有遗传算法、蚁群算法、粒子群算法、免疫算法等。传统算法在简单的地图环境中性能较好，但不适用于复杂的地图环境。而智能仿生学算法则存在过早收敛、全局路径质量差、易陷入局部极值等问题。同时，在实际应用环境中广泛存在半未知环境，例如抗震救灾、抢险救援等实际场景通常为半未知环境，相较于已知环境，半未知环境的复杂度更高、不确定性更强。但目前的大多路径规划算法仅适用于已知环境，而对于半未知环境的适用性则较差。因此研究半未知环境下的路径规划技术是非常有必要的。

3、在实际环境中，由于某些工作区域的具体环境信息是未知的，而已知环境下的路径规划技术大多不适用于半未知环境，这将对移动机器人的正常工作带来极大挑战，因此需要一种半未知环境下的路径规划技术。半未知环境下的路径规划技术是指在缺少大量全局信息的情况下，利用传感器所获取到的实时局部信息和相应的路径规划机制使得移动机器人安全、高效、快捷的到达目的地的技术。针对半未知环境下的路径规划技术，专家学者们进行了深入的研究。于宁波等提出了一种用于半未知环境下机器人探测的路径规划算法，该算法利用有限状态机迭代推导出机器人在无先验地图可用的情况下规划环境的动作，在保持效率的同时实现完全规划。但是该算法并未考虑动态障碍物的存在，移动机器人可能会与动态障碍物发生碰撞。王石荣等提出了一种基于粒子群优化的地形辅助路径规划算法。该算法将地形识别策略和避障策略集成在一种基于粒子群的路径规划算法中，能够在半未知地形下规划出合适的路径。然而该算法仅仅适用于障碍物分布较为简单的半未知环境，对于复杂度较高的半未知环境并不适用。徐昌等提出了一种基于深度神经网络的实时在线路径规划方法。该方法适用于杂乱半未知环境且在效率、成功率和路径质量方面都具有显著的提升。但该算法并未考虑凹形区域等特殊地形，这将会使移动机器人陷入停滞或者震荡状态。

技术实现思路

1、因此，本发明的目的在于提供一种基于蚂蚁爬行机制的半未知环境下移动机器人路径探索方法，利用垂直映射法在栅格地图上生成待选引导点集合；在待选引导点集合的基础上，利用蚂蚁爬行机制生成引导路径；在移动机器人沿引导路径行进的过程中进行动态避障等。提高了移动机器人在半未知环境下到达终点的成功率，而且还能够改善移动机器人探索路径的质量，降低移动机器人探索路径所需的能耗。

2、为了实现上述目的，本发明的一种基于蚂蚁爬行机制的半未知环境下移动机器人路径探索方法，包括以下步骤：

3、s1、采用栅格法建立空间环境模型；

4、s2、将目标点向已知环境中的非障碍物区域做垂直投影生成映射点；

5、s3、在映射点左右两侧扩展多个栅格，生成待选区域；在待选区域内删除超出地图边界的栅格和障碍物栅格，将其他栅格形成待选引导点集合；

6、s4、采用蚂蚁爬行机制生成移动机器人当前位置到待选引导点集合中各点的待选路径；检查是否存在当前待选引导点集合与历史待选引导点集合完全重复的情况，若存在转入s5，否则转入s6；

7、s5、移动机器人将传感器的探测范围增至最大，然后在目标点左侧或者右侧，以n个栅格长度为间距生成虚拟目标点，并用虚拟目标点替换当前目标点，转入s2；

8、s6、采用路径评价机制对各待选路径的质量进行评价，选择待选路径中质量最优的路径做为引导路径，将引导路径所对应的待选引导点视为当前引导点，检查是否存在虚拟目标点vgi，若存在则撤销vgi，并恢复目标点g点，转入s7；

9、s7、移动机器人沿引导路径行驶至当前引导点i点并实时更新地图环境信息，在行驶途中，当移动机器人预测到即将和动态障碍物发生碰撞时，采取避障策略进行紧急避障；

10、s8、判断已知环境中是否出现目标点g点，若g点出现在已知环境中，则移动机器人利用蚂蚁爬行机制生成当前位置到目标点g点的引导路径并沿着该路径行驶至g点；若g点没有出现在已知环境中，则转入s2再次进入循环；

11、进一步优选的，在s1中，采用栅格法建立空间环境模型时，包括以下步骤：

12、s101、空间环境划分为若干个大小相同的正方形栅格；

13、s102、按照从左到右、从上到下的顺序由1开始为栅格添加编号直到添加至右下角的最后一个栅格；

14、s103、将各栅格按照预设的坐标转换公式，利用中心点坐标表示。

15、进一步优选的，所述预设的坐标转换公式包括

16、

17、y＝n+0.5-ceil(m/n) (2)

18、m＝floor(l-y)*n+ceil(x) (3)

19、其中，公式(1)和公式(2)用于把栅格编号转化成栅格坐标；公式(3)把栅格坐标转化成栅格编号；mod()为取余函数，floor()为向下取整函数，ceil()为向上取整函数；m为栅格编号，n为栅格总列数，l为栅格总行数，(x,y)为栅格的横、纵坐标。

20、进一步优选的，在s2中，所述目标点向已知环境中的非障碍物区域做垂直投影生成的映射点，采用如下公式进行计算：

21、i0＝g-h1*l-h2*l；

22、其中，g为目标点的栅格编号，h1为垂直方向上未知环境的总行数，h2为垂直方向上与未知环境相连的障碍物区域的总行数，ｌ为每行的总栅格数。

23、进一步优选的，s4中，所述采用蚂蚁爬行机制生成移动机器人当前位置到待选引导点集合中各点的待选路径，包括以下步骤：

24、s401、初始化信息素δμv＝c(c>0)并将路径表l置空，将移动机器人当前位置c_point存入路径表l中，并将c_point设为蚂蚁的当前位置；

25、s402、蚂蚁从当前位置出发搜索路径，根据式(4)计算从当前栅格点转移到各相邻无障碍栅格点的可行度iμv，并根据最大可行度选取下一个栅格点；

26、

27、其中，iμv为蚂蚁从路径点μ到路径点v之间的可行度，j(μ)为路径点μ的下一个可行路径点集合，δμv为路径点μ到路径点v之间的信息素浓度，为路径点μ到路径点v之间的启发式信息，表示路径点μ与ｖ距离的倒数，ｋ为信息素启发式因子，γ为期望启发因子。

28、s403、将选择的下一个栅格点的编号存入路径表ｌ中，并将该栅格点设为蚂蚁的当前位置。判断当前位置的编号与待选引导点的编号是否一致，若不一致，则转至s402。若一致，流程结束，路径表ｌ中的路径即为移动机器人当前位置ｃ_ｐｏint到待选引导点的待选路径；

29、进一步优选的，在s6中，采用路径评价机制对待选路径的质量进行评价时，路径质量的评价指标包括路径可达性、路径安全性、路径长度以及路径平滑度。

30、进一步优选的，所述路径质量的计算式如下式所示：

31、q＝ω１pf+ω2ps+ω3pl+ω4pm (5)

32、其中，q表示路径质量，pf表示路径可达性，ps表示路径安全性，pl表示路径长度，pm表示路径平滑度；ω1、ω2、ω3、ω4分别为路径可达性、路径安全性、路径长度、路径平滑度的权重系数且ω1+ω2+ω3+ω4＝1，ω1≥ω2≥ω3≥ω4。

33、进一步优选的，在s7中，移动机器人预测到即将和动态障碍物发生碰撞时，预测碰撞的具体过程为：

34、将不同的碰撞方式将碰撞类型分为四类：正向碰撞、侧向碰撞、停滞碰撞以及追及碰撞；

35、假设移动机器人当前位置rc的坐标为(xc,yc)，下一个位置rn的坐标为(xn,yn)；动态障碍物当前位置oc的坐标为(rc,cc)，下一个位置on的坐标为(rn,cn)；

36、当移动机器人与动态障碍物的坐标满足以下关系式(11)时，判断移动机器人与动态障碍物发生正向碰撞，有公共碰撞点；

37、

38、当移动机器人与动态障碍物的坐标满足以下关系式(12)时，判断移动机器人与动态障碍物发生正向碰撞时没有公共碰撞点

39、

40、当移动机器人与动态障碍物的坐标满足以下关系式(13)时，判断移动机器人与动态障碍物发生侧向碰撞；

41、xn＝rn&yn＝cn (13)

42、当移动机器人与动态障碍物的坐标满足以下关系式(14)时，判断移动机器人与动态障碍物发生停滞碰撞；

43、xn＝rn&yn＝cn，rn＝rc&cn＝cc (14)

44、当移动机器人与动态障碍物的坐标满足以下关系式(14)时，判断移动机器人与动态障碍物发生追及碰撞；

45、

46、进一步优选的，在s7中，所述采取避障策略进行紧急避障中，所述避障策略分别为转向行为策略、原地等待策略以及局部路径重规划策略。

47、进一步优选的，所述转向行为策略用于当移动机器人感知到即将与动态障碍物发生正向面碰撞、正向点碰撞或追及碰撞时，移动机器人通过左转向或者右转向的方式来避开动态障碍物；

48、所述原地等待策略用于当移动机器人感知到即将与动态障碍物发生侧向碰撞时，移动机器人通过原地暂停的方式来避开动态障碍物。

49、进一步优选的，所述局部路径重规划策略，用于当动态障碍物停滞在机器人的行进路径上时，移动机器人利用蚂蚁爬行机制重新规划当前路径点i到第i+2个路径点之间的局部路径的方式来避开动态障碍物；当移动机器人通过上述方式避开动态障碍物后，移动机器人将会重新返回引导路径，然后沿着引导路径继续行驶。

50、本技术公开的基于蚂蚁爬行机制的半未知环境下移动机器人路径探索方法具有如下有益效果：第一，本发明所采用的路径评价机制不仅可以提高移动机器人在半未知环境下到达终点的成功率，而且还能够改善移动机器人探索路径的质量，降低移动机器人探索路径所需的能耗。第二，本发明所采用的蚂蚁爬行机制可以有效应对凹形区域等各种复杂障碍物环境，确保移动机器人不会陷入停滞或者震荡状态。第三，移动机器人可以在引导路径和避障策略之间自由切换，这样既可以保证移动机器人能够成功躲避动态障碍物，又可以保证移动机器人行驶路径的质量。第四，本发明探讨了移动机器人与动态障碍物之间的碰撞类型并提供了相应的检测方法。该检测方法可以大幅降低移动机器人与动态障碍物误撞的概率。第五，本发明提供的避障策略充分利用了行为避障和局部路径重归划策略以应对各种潜在的碰撞情况。该避障策略可以大幅改善移动机器人的避障时间并且降低移动机器人的避障代价。