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

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


背景技术：

2.随着科技的快速发展，移动机器人已经被广泛的应用于人们的生产生活中。例如在地面自主导航、资源勘探开发、抢险救灾等多个领域，移动机器人可以代替或协助人类完成各项复杂工作。同时随着移动机器人应用场景和服务模式的不断扩展，人们对移动机器人的智能化程度要求也越来越高，而移动机器人的智能化核心技术是路径规划技术。路径规划是指移动机器人在复杂空间环境中探寻一条从起点到终点的高质量无碰撞路径，一个好的路径规划技术可以有效地提高移动机器人对资源的利用率，降低移动机器人的损耗，延长移动机器人的使用寿命。
3.目前移动机器人路径规划的主流算法分为两大类：传统算法与智能仿生学算法。其中，传统算法主要有a*算法、人工势场法等。智能仿生学算法主要有遗传算法、蚁群算法、粒子群算法、免疫算法等。传统算法在简单的地图环境中性能较好，但不适用于复杂的地图环境。而智能仿生学算法则存在过早收敛、全局路径质量差、易陷入局部极值等问题。同时，在实际应用环境中广泛存在半未知环境，例如抗震救灾、抢险救援等实际场景通常为半未知环境，相较于已知环境，半未知环境的复杂度更高、不确定性更强。但目前的大多路径规划算法仅适用于已知环境，而对于半未知环境的适用性则较差。因此研究半未知环境下的路径规划技术是非常有必要的。
4.在实际环境中，由于某些工作区域的具体环境信息是未知的，而已知环境下的路径规划技术大多不适用于半未知环境，这将对移动机器人的正常工作带来极大挑战，因此需要一种半未知环境下的路径规划技术。半未知环境下的路径规划技术是指在缺少大量全局信息的情况下，利用传感器所获取到的实时局部信息和相应的路径规划机制使得移动机器人安全、高效、快捷的到达目的地的技术。针对半未知环境下的路径规划技术，专家学者们进行了深入的研究。于宁波等提出了一种用于半未知环境下机器人探测的路径规划算法，该算法利用有限状态机迭代推导出机器人在无先验地图可用的情况下规划环境的动作，在保持效率的同时实现完全规划。但是该算法并未考虑动态障碍物的存在，移动机器人可能会与动态障碍物发生碰撞。王石荣等提出了一种基于粒子群优化的地形辅助路径规划算法。该算法将地形识别策略和避障策略集成在一种基于粒子群的路径规划算法中，能够在半未知地形下规划出合适的路径。然而该算法仅仅适用于障碍物分布较为简单的半未知环境，对于复杂度较高的半未知环境并不适用。徐昌等提出了一种基于深度神经网络的实时在线路径规划方法。该方法适用于杂乱半未知环境且在效率、成功率和路径质量方面都具有显著的提升。但该算法并未考虑凹形区域等特殊地形，这将会使移动机器人陷入停滞或者震荡状态。


技术实现要素：

5.因此，本发明的目的在于提供一种基于蚂蚁爬行机制的半未知环境下移动机器人路径探索方法，利用垂直映射法在栅格地图上生成待选引导点集合；在待选引导点集合的基础上，利用蚂蚁爬行机制生成引导路径；在移动机器人沿引导路径行进的过程中进行动态避障等。提高了移动机器人在半未知环境下到达终点的成功率，而且还能够改善移动机器人探索路径的质量，降低移动机器人探索路径所需的能耗。
6.为了实现上述目的，本发明的一种基于蚂蚁爬行机制的半未知环境下移动机器人路径探索方法，包括以下步骤：
7.s1、采用栅格法建立空间环境模型；
8.s2、将目标点向已知环境中的非障碍物区域做垂直投影生成映射点；
9.s3、在映射点左右两侧扩展多个栅格，生成待选区域；在待选区域内删除超出地图边界的栅格和障碍物栅格，将其他栅格形成待选引导点集合；
10.s4、采用蚂蚁爬行机制生成移动机器人当前位置到待选引导点集合中各点的待选路径；检查是否存在当前待选引导点集合与历史待选引导点集合完全重复的情况，若存在转入s5，否则转入s6；
11.s5、移动机器人将传感器的探测范围增至最大，然后在目标点左侧或者右侧，以n个栅格长度为间距生成虚拟目标点，并用虚拟目标点替换当前目标点，转入s2；
12.s6、采用路径评价机制对各待选路径的质量进行评价，选择待选路径中质量最优的路径做为引导路径，将引导路径所对应的待选引导点视为当前引导点，检查是否存在虚拟目标点vgi，若存在则撤销vgi，并恢复目标点g点，转入s7；
13.s7、移动机器人沿引导路径行驶至当前引导点i点并实时更新地图环境信息，在行驶途中，当移动机器人预测到即将和动态障碍物发生碰撞时，采取避障策略进行紧急避障；
14.s8、判断已知环境中是否出现目标点g点，若g点出现在已知环境中，则移动机器人利用蚂蚁爬行机制生成当前位置到目标点g点的引导路径并沿着该路径行驶至g点；若g点没有出现在已知环境中，则转入s2再次进入循环；
15.进一步优选的，在s1中，采用栅格法建立空间环境模型时，包括以下步骤：
16.s101、空间环境划分为若干个大小相同的正方形栅格；
17.s102、按照从左到右、从上到下的顺序由1开始为栅格添加编号直到添加至右下角的最后一个栅格；
18.s103、将各栅格按照预设的坐标转换公式，利用中心点坐标表示。
19.进一步优选的，所述预设的坐标转换公式包括
[0020][0021]
y＝n+0.5-ceil(m/n) (2)
[0022]
m＝floor(l-y)*n+ceil(x) (3)
[0023]
其中，公式(1)和公式(2)用于把栅格编号转化成栅格坐标；公式(3)把栅格坐标转化成栅格编号；mod()为取余函数，floor()为向下取整函数，ceil()为向上取整函数；m为栅格编号，n为栅格总列数，l为栅格总行数，(x,y)为栅格的横、纵坐标。
[0024]
进一步优选的，在s2中，所述目标点向已知环境中的非障碍物区域做垂直投影生
成的映射点，采用如下公式进行计算：
[0025]
i0＝g-h1*l-h2*l；
[0026]
其中，g为目标点的栅格编号，h1为垂直方向上未知环境的总行数，h2为垂直方向上与未知环境相连的障碍物区域的总行数，ｌ为每行的总栅格数。
[0027]
进一步优选的，s4中，所述采用蚂蚁爬行机制生成移动机器人当前位置到待选引导点集合中各点的待选路径，包括以下步骤：
[0028]
s401、初始化信息素δ
μv
＝c(c》0)并将路径表l置空，将移动机器人当前位置c_point存入路径表l中，并将c_point设为蚂蚁的当前位置；
[0029]
s402、蚂蚁从当前位置出发搜索路径，根据式(4)计算从当前栅格点转移到各相邻无障碍栅格点的可行度i
μv
，并根据最大可行度选取下一个栅格点；
[0030][0031]
其中，i
μv
为蚂蚁从路径点μ到路径点v之间的可行度，j(μ)为路径点μ的下一个可行路径点集合，δ
μv
为路径点μ到路径点v之间的信息素浓度，为路径点μ到路径点v之间的启发式信息，表示路径点μ与ｖ距离的倒数，ｋ为信息素启发式因子，γ为期望启发因子。
[0032]
s403、将选择的下一个栅格点的编号存入路径表ｌ中，并将该栅格点设为蚂蚁的当前位置。判断当前位置的编号与待选引导点的编号是否一致，若不一致，则转至s402。若一致，流程结束，路径表ｌ中的路径即为移动机器人当前位置ｃ_ｐｏint到待选引导点的待选路径；
[0033]
进一步优选的，在s6中，采用路径评价机制对待选路径的质量进行评价时，路径质量的评价指标包括路径可达性、路径安全性、路径长度以及路径平滑度。
[0034]
进一步优选的，所述路径质量的计算式如下式所示：
[0035]
q＝ω
１
pf+ω2ps+ω3p
l
+ω4p
m (5)
[0036]
其中，q表示路径质量，pf表示路径可达性，ps表示路径安全性，p
l
表示路径长度，pm表示路径平滑度；ω1、ω2、ω3、ω4分别为路径可达性、路径安全性、路径长度、路径平滑度的权重系数且ω1+ω2+ω3+ω4＝1，ω1≥ω2≥ω3≥ω4。
[0037]
进一步优选的，在s7中，移动机器人预测到即将和动态障碍物发生碰撞时，预测碰撞的具体过程为：
[0038]
将不同的碰撞方式将碰撞类型分为四类：正向碰撞、侧向碰撞、停滞碰撞以及追及碰撞；
[0039]
假设移动机器人当前位置rc的坐标为(xc,yc)，下一个位置rn的坐标为(xn,yn)；动态障碍物当前位置oc的坐标为(rc,cc)，下一个位置on的坐标为(rn,cn)；
[0040]
当移动机器人与动态障碍物的坐标满足以下关系式(11)时，判断移动机器人与动态障碍物发生正向碰撞，有公共碰撞点；
[0041][0042]
当移动机器人与动态障碍物的坐标满足以下关系式(12)时，判断移动机器人与动态障碍物发生正向碰撞时没有公共碰撞点
[0043][0044]
当移动机器人与动态障碍物的坐标满足以下关系式(13)时，判断移动机器人与动态障碍物发生侧向碰撞；
[0045]
xn＝rn&yn＝c
n (13)
[0046]
当移动机器人与动态障碍物的坐标满足以下关系式(14)时，判断移动机器人与动态障碍物发生停滞碰撞；
[0047]
xn＝rn&yn＝cn，rn＝rc&cn＝c
c (14)
[0048]
当移动机器人与动态障碍物的坐标满足以下关系式(14)时，判断移动机器人与动态障碍物发生追及碰撞；
[0049][0050]
进一步优选的，在s7中，所述采取避障策略进行紧急避障中，所述避障策略分别为转向行为策略、原地等待策略以及局部路径重规划策略。
[0051]
进一步优选的，所述转向行为策略用于当移动机器人感知到即将与动态障碍物发生正向面碰撞、正向点碰撞或追及碰撞时，移动机器人通过左转向或者右转向的方式来避开动态障碍物；
[0052]
所述原地等待策略用于当移动机器人感知到即将与动态障碍物发生侧向碰撞时，移动机器人通过原地暂停的方式来避开动态障碍物。
[0053]
进一步优选的，所述局部路径重规划策略，用于当动态障碍物停滞在机器人的行进路径上时，移动机器人利用蚂蚁爬行机制重新规划当前路径点i到第i+2个路径点之间的局部路径的方式来避开动态障碍物；当移动机器人通过上述方式避开动态障碍物后，移动机器人将会重新返回引导路径，然后沿着引导路径继续行驶。
[0054]
本技术公开的基于蚂蚁爬行机制的半未知环境下移动机器人路径探索方法具有如下有益效果：第一，本发明所采用的路径评价机制不仅可以提高移动机器人在半未知环境下到达终点的成功率，而且还能够改善移动机器人探索路径的质量，降低移动机器人探索路径所需的能耗。第二，本发明所采用的蚂蚁爬行机制可以有效应对凹形区域等各种复杂障碍物环境，确保移动机器人不会陷入停滞或者震荡状态。第三，移动机器人可以在引导路径和避障策略之间自由切换，这样既可以保证移动机器人能够成功躲避动态障碍物，又可以保证移动机器人行驶路径的质量。第四，本发明探讨了移动机器人与动态障碍物之间的碰撞类型并提供了相应的检测方法。该检测方法可以大幅降低移动机器人与动态障碍物误撞的概率。第五，本发明提供的避障策略充分利用了行为避障和局部路径重归划策略以应对各种潜在的碰撞情况。该避障策略可以大幅改善移动机器人的避障时间并且降低移动机器人的避障代价。
附图说明
[0055]
图1为本发明提供的基于蚂蚁爬行机制的半未知环境下移动机器人路径探索方法的工作原理示意图。
[0056]
图2为本发明提供的采用栅格法建立空间环境模型示意图。
[0057]
图3(a)为本发明提供的移动机器人与动态障碍物有公共点的正向碰撞的示意图。
[0058]
图3(b)为本发明没有公共点时的正向碰撞的示意图。
[0059]
图3(c)为本发明提供的移动机器人与动态障碍物的侧向碰撞的示意图。
[0060]
图3(d)为本发明提供的移动机器人与动态障碍物的追及碰撞的示意图。
具体实施方式
[0061]
以下通过附图和具体实施方式对本发明作进一步的详细说明。
[0062]
如图1所示，本发明一方面实施例提供的基于蚂蚁爬行机制的半未知环境下移动机器人路径探索方法包括按顺序进行的下列步骤：
[0063]
s1、采用栅格法建立空间环境模型(如图2所示)。栅格法是指将整个二维空间环境划分为若干个大小相同的正方形栅格。其中，白色栅格a代表自由区域，黑色栅格b代表静态障碍物，灰色栅格c代表动态障碍物，黑色栅格d代表未知环境区域。地图从左到右、从上到下由1开始为栅格添加编号直到添加至右下角的最后一个栅格。栅格地图中各栅格的坐标由其中心点坐标表示，栅格编号与栅格坐标之间的转换计算为：
[0064][0065]
y＝n+0.5-ceil(m/n) (2)
[0066]
m＝floor(l-y)*n+ceil(ｘ) (3)
[0067]
其中，式(1)、(2)把栅格编号转化成栅格坐标。式(3)把栅格坐标转化成栅格编号。ｍｏｄ()为取余函数，ｆlｏｏr()为向下取整函数，ceiｌ()为向上取整函数。式中，ｍ为栅格编号，n为栅格总列数，ｌ为栅格总行数，(x,y)为栅格的横、纵坐标；
[0068]
s2：目标点g点(虚拟目标点vgi点)向已知环境中的非障碍物区域做垂直投影生成映射点i0；i0＝g-h1*l-h2*l，其中g为目标点的栅格编号，h1为垂直方向上未知环境的总行数，h2为垂直方向上与未知环境相连的障碍物区域的总行数，l为每行的总栅格数；
[0069]
s3：在i0点左右两侧各扩展n个栅格长度以生成待选区域，故待选区域内包含有2n+1个栅格。去除掉该待选区域内超出地图边界的栅格及障碍物栅格，其余栅格为待选引导点集合i_set；
[0070]
s4：采用蚂蚁爬行机制生成移动机器人当前位置c_point到i_set集合中各点的待选路径。检查是否存在当前i_set集合与历史i_set集合完全重复的情况，若存在转入步骤五，否则转入步骤六；
[0071]
s5：移动机器人将传感器的探测范围增至最大，然后在g(vgi)点左侧或者右侧，以n个栅格长度为间距生成虚拟目标点vgi。vgi暂时替换g点，转入s2；
[0072]
s6：采用路径评价机制对各待选路径的质量进行评价，选择待选路径中最好的路径做为引导路径，该引导路径所对应的待选引导点即为当前引导点i点。检查是否存在vgi，若存在则撤销vgi，同时恢复g点；
[0073]
需要说明的是，步骤s4的作用在于检查移动机器人是否因为地形的缘故陷入停滞状态。正常来讲如果移动机器人没有陷入停滞状态，那么就转入步骤s6，此处不存在虚拟目标点，因此就无需撤销虚拟目标点。如果移动机器人陷入停滞状态，则转入步骤s5生成虚拟目标点，虚拟目标点将会取代原来的目标点重新回到步骤s2，一直执行到步骤s4，如果移动
机器人没有陷入停滞状态，就转入步骤s6。此时因为参与流程的是虚拟目标点而非原来的目标点。因此就需要把虚拟目标点撤销，恢复到原来的目标点。
[0074]
s7：移动机器人沿引导路径行驶至当前引导点i点并实时更新地图环境信息。在行驶途中，当移动机器人预测到即将和动态障碍物发生碰撞时，采取避障策略进行紧急避障；
[0075]
s8：判断已知环境中是否出现g点。若ｇ点出现在已知环境中，则移动机器人利用蚂蚁爬行机制生成当前位置ｃ_ｐｏｉｎｔ到目标点ｇ点的引导路径并沿着该路径行驶至g点，流程结束。若g点没有出现在已知环境中，则转入s2；
[0076]
上述实施过程中s7和s8为并列步骤，实施时，先后顺序没有严格限制。
[0077]
上述实施过程中，所述s4中采用蚂蚁爬行机制生成移动机器人当前位置c_pｏｉnt到待选引导点的待选路径的具体过程为：
[0078]
s401、初始化信息素δ
μv
＝c(c》0)并将路径表l置空。将移动机器人当前位置c_point存入路径表l中，并将c_point设为蚂蚁的当前位置；
[0079]
s402、蚂蚁从当前位置出发搜索路径，根据式(4)计算从当前栅格点转移到各相邻无障碍栅格点的可行度i
μv
，并根据最大可行度选取下一个栅格点；
[0080][0081]
其中，i
μv
为蚂蚁从路径点μ到路径点v之间的可行度，j(μ)为路径点μ的下一个可行路径点集合，δ
μv
为路径点μ到路径点v之间的信息素浓度，为路径点μ到路径点v之间的启发式信息，表示路径点μ与v距离的倒数，k为信息素启发式因子，γ为期望启发因子。
[0082]
s403、将选择的下一个栅格点的编号存入路径表l中，并将该栅格点设为蚂蚁的当前位置。判断当前位置的编号与待选引导点的编号是否一致，若不一致，则转至s402。若一致，流程结束，路径表l中的路径即为移动机器人当前位置c_point到待选引导点的待选路径；
[0083]
上述实施过程中，所述s6中采用路径评价机制对待选路径的质量进行评价的具体过程为：
[0084]
为使移动机器人在半未知环境下探索一条可行的、高质量的路径，本发明设计一种基于多性能指标的路径评价机制。路径质量的评价指标包括路径可达性、路径安全性、路径长度以及路径平滑度。路径质量的计算式如式(5)所示
[0085]
q＝ω1pf+ω2ps+ω3p
l
+ω4p
m (5)
[0086]
其中q表示路径质量，pf表示路径可达性，ps表示路径安全性，p
l
表示路径长度，pm表示路径平滑度。ω1、ω2、ω3、ω4分别为路径可达性、路径安全性、路径长度、路径平滑度的权重系数且ω1+ω2+ω3+ω4＝1。
[0087]
针对上述四个路径质量评价指标，本发明采用德尔菲赋权法对各路径指标的权重系数进行评估，从而得到一组较好的权重系数。首先，根据实际需求指定这四个指标的重要性顺序。考虑到半未知环境的特殊性，四个指标的重要性顺序依次为：路径可达性、路径安全性、路径长度、路径平滑度，即优先保证探索路径的可达性和安全性，再考虑路径长度和路径平滑度，则有ω1≥ω2≥ω3≥ω4。
[0088]
重要性比率定义如式(6)所示：
[0089][0090]
则有ik≥1，ik越大代表前者比后者越重要。
[0091]
定义重要性比率意义。当ik＝1时，表示前者和后者重要程度相同；当ik＝1.3时，表示前者比后者重要程度略高；当ik＝1.6时，表示前者比后者重要程度高；当ik＝1.9时，表示前者比后者重要程度远高；
[0092]
根据所定义的重要性比率ik，ω4的计算过程如式(7)-(9)所示。
[0093][0094][0095][0096]
本发明指定i1＝1.3，i2＝1.9，i3＝1.6。即在半未知环境下，路径可达性的重要程度略高于路径安全性，路径安全性的重要程度远高于路径长度，路径长度的重要程度高于路径平滑度。则可解得：
[0097][0098]
即路径可达性、路径安全性、路径长度、路径平滑度的权重系数依次为：0.4117、0.3171、0.1669、0.1043。
[0099]
上述实施过程中，所述步骤七中当移动机器人预测到即将和动态障碍物发生碰撞时，采取避障策略进行紧急避障的具体过程为：
[0100]
本发明按照不同的碰撞方式将碰撞类型分为四类：正向碰撞、侧向碰撞、停滞碰撞以及追及碰撞。其中，正向碰撞又分为正向点碰撞和正向面碰撞。正向碰撞是指移动机器人与动态障碍物因运动方向共线且相反而产生的面对面碰撞的情况。假设移动机器人当前位置rc的坐标为(xc,yc)，下一个位置rn的坐标为(xn,yn)。动态障碍物当前位置oc的坐标为(rc,cc)，下一个位置on的坐标为(rn,cn)。
[0101]
若移动机器人与动态障碍物发生正向碰撞时有公共碰撞点，则这种碰撞情况被称为正向点碰撞(如图3(a)所示)。即
[0102][0103]
若移动机器人与动态障碍物发生正向碰撞时没有公共碰撞点，即移动机器人与动态障碍物的下一步互为行进路径点。在这种情况下虽无碰撞点但两者也会发生碰撞，这时判断这种碰撞情况为正向面碰撞(如图3(b)所示)。即
[0104]
[0105]
侧向碰撞是指移动机器人与动态障碍物运动方向不共线，但在某一时刻两者恰好产生公共碰撞点的情况(如图3(c)所示)。即
[0106]
xn＝＝rn&&yn＝＝c
n (13)
[0107]
停滞碰撞是指动态障碍物由于某种原因恰好停留在了引导路径上，导致移动机器人与障碍物必定会发生碰撞的情况。即
[0108]
xn＝＝rn&&yn＝＝cn，rn＝＝rc&&cn＝＝c
c (14)
[0109]
追及碰撞是指移动机器人与动态障碍物运动共线且同向，但由于两者速度不匹配在某一时刻产生公共碰撞点的情况(如图3(d)所示)。即
[0110][0111]
本发明共设计三种避障策略，分别为转向行为策略、原地等待策略以及局部路径重规划策略。转向行为策略是指当移动机器人感知到即将与动态障碍物发生正向面碰撞、正向点碰撞或追及碰撞时，移动机器人通过左转向或者右转向的方式来避开动态障碍物。
[0112]
原地等待策略是指当移动机器人感知到即将与动态障碍物发生侧向碰撞时，移动机器人通过原地暂停的方式来避开动态障碍物。
[0113]
局部路径重规划策略是指当动态障碍物停滞在机器人的行进路径上时，机器人利用蚂蚁爬行机制重新规划当前路径点(第i个路径点)到第i+2个路径点之间的局部路径的方式来避开动态障碍物。当移动机器人通过上述方式避开动态障碍物后，移动机器人将会重新返回引导路径，然后沿着引导路径继续行驶。
[0114]
因此，当移动机器人与动态障碍物即将发生正向碰撞或追及碰撞时，采用转向行为策略躲避；当移动机器人与动态障碍物即将发生侧向碰撞时，采用原地等待策略躲避；当移动机器人与动态障碍物即将发生停滞碰撞时，采用局部路径重规划策略躲避。
[0115]
显然，上述实施例仅是为清楚地说明所作的举例，而并非对实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。而由此所引伸出的显而易见的变化或变动仍处于本发明创造的保护范围之中。

技术特征：
1.一种基于蚂蚁爬行机制的半未知环境下移动机器人路径探索方法，其特征在于，包括以下步骤：s1、采用栅格法建立空间环境模型；s2、将目标点向已知环境中的非障碍物区域做垂直投影生成映射点；s3、在映射点左右两侧扩展多个栅格，生成待选区域；在待选区域内删除超出地图边界的栅格和障碍物栅格，将其他栅格形成待选引导点集合；s4、采用蚂蚁爬行机制生成移动机器人当前位置到待选引导点集合中各点的待选路径；检查是否存在当前待选引导点集合与历史待选引导点集合完全重复的情况，若存在转入s5，否则转入s6；s5、移动机器人将传感器的探测范围增至最大，然后在目标点左侧或者右侧，以n个栅格长度为间距生成虚拟目标点，并用虚拟目标点替换当前目标点，转入s2；s6、采用路径评价机制对各待选路径的质量进行评价，选择待选路径中质量最优的路径做为引导路径，将引导路径所对应的待选引导点视为当前引导点，检查是否存在虚拟目标点vg
i
，若存在则撤销vg
i
，并恢复目标点g点，转入s7；s7、移动机器人沿引导路径行驶至当前引导点i点并实时更新地图环境信息，在行驶途中，当移动机器人预测到即将和动态障碍物发生碰撞时，采取避障策略进行紧急避障；s8、判断已知环境中是否出现目标点g点，若g点出现在已知环境中，则移动机器人利用蚂蚁爬行机制生成当前位置到目标点g点的引导路径并沿着该路径行驶至g点；若g点没有出现在已知环境中，则转入s2再次进入循环。2.根据权利要求1所述的基于蚂蚁爬行机制的半未知环境下移动机器人路径探索方法，其特征在于，在s1中，采用栅格法建立空间环境模型时，包括以下步骤：s101、空间环境划分为若干个大小相同的正方形栅格；s102、按照从左到右、从上到下的顺序由1开始为栅格添加编号直到添加至右下角的最后一个栅格；s103、将各栅格按照预设的坐标转换公式，利用中心点坐标表示。3.根据权利要求2所述的基于蚂蚁爬行机制的半未知环境下移动机器人路径探索方法，其特征在于，所述预设的坐标转换公式包括y＝n+0.5-ceil(m/n)
ꢀꢀꢀꢀ
(2)m＝floor(l-y)*n+ceil(x)
ꢀꢀꢀꢀ
(3)其中，公式(1)和公式(2)用于把栅格编号转化成栅格坐标；公式(3)把栅格坐标转化成栅格编号；mod()为取余函数，floor()为向下取整函数，ceil()为向上取整函数；m为栅格编号，n为栅格总列数，l为栅格总行数，(x，y)为栅格的横、纵坐标。4.根据权利要求1所述的基于蚂蚁爬行机制的半未知环境下移动机器人路径探索方法，其特征在于，在s2中，所述目标点向已知环境中的非障碍物区域做垂直投影生成的映射点，采用如下公式进行计算：i0＝g-h1*l-h2*l；其中，g为目标点的栅格编号，h1为垂直方向上未知环境的总行数，h2为垂直方向上与未
知环境相连的障碍物区域的总行数，2为每行的总栅格数。5.根据权利要求1所述的基于蚂蚁爬行机制的半未知环境下移动机器人路径探索方法，其特征在于，s4中，所述采用蚂蚁爬行机制生成移动机器人当前位置到待选引导点集合中各点的待选路径，包括以下步骤：s401、初始化信息素δ
μv
＝c(c＞0)并将路径表l置空，将移动机器人当前位置c_point存入路径表l中，并将c_point设为蚂蚁的当前位置；s402、蚂蚁从当前位置出发搜索路径，根据式(4)计算从当前栅格点转移到各相邻无障碍栅格点的可行度i
μv
，并根据最大可行度选取下一个栅格点；其中，i
μv
为蚂蚁从路径点μ到路径点v之间的可行度，j(μ)为路径点μ的下一个可行路径点集合，δ
μv
为路径点μ到路径点v之间的信息素浓度，为路径点μ到路径点v之间的启发式信息，表示路径点μ与v距离的倒数，k为信息素启发式因子，γ为期望启发因子。s403、将选择的下一个栅格点的编号存入路径表l中，并将该栅格点设为蚂蚁的当前位置。判断当前位置的编号与待选引导点的编号是否一致，若不一致，则转至s402。若一致，流程结束，路径表l中的路径即为移动机器人当前位置c_point到待选引导点的待选路径。6.根据权利要求1所述的基于蚂蚁爬行机制的半未知环境下移动机器人路径探索方法，其特征在于，在s6中，采用路径评价机制对待选路径的质量进行评价时，路径质量的评价指标包括路径可达性、路径安全性、路径长度以及路径平滑度。7.根据权利要求6所述的基于蚂蚁爬行机制的半未知环境下移动机器人路径探索方法，其特征在于，所述路径质量的计算式如下式所示：q＝ω1p
f
+ω2p
s
+ω3p
l
+ω4p
m
ꢀꢀꢀꢀ
(5)其中，q表示路径质量，p
f
表示路径可达性，p
s
表示路径安全性，p
l
表示路径长度，p
m
表示路径平滑度；ω1、ω2、ω3、ω4分别为路径可达性、路径安全性、路径长度、路径平滑度的权重系数且ω1+ω2+ω3+ω4＝1，ω1≥ω2≥ω3≥ω4。8.根据权利要求1所述的基于蚂蚁爬行机制的半未知环境下移动机器人路径探索方法，其特征在于，在s7中，移动机器人预测到即将和动态障碍物发生碰撞时，预测碰撞的具体过程为：将不同的碰撞方式将碰撞类型分为四类：正向碰撞、侧向碰撞、停滞碰撞以及追及碰撞；假设移动机器人当前位置r
c
的坐标为(x
c
，y
c
)，下一个位置r
n
的坐标为(x
n
，y
n
)；动态障碍物当前位置o
c
的坐标为(r
c
，c
c
)，下一个位置o
n
的坐标为(r
n
，c
n
)；当移动机器人与动态障碍物的坐标满足以下关系式(11)时，判断移动机器人与动态障碍物发生正向碰撞，有公共碰撞点；当移动机器人与动态障碍物的坐标满足以下关系式(12)时，判断移动机器人与动态障碍物发生正向碰撞时没有公共碰撞点
当移动机器人与动态障碍物的坐标满足以下关系式(13)时，判断移动机器人与动态障碍物发生侧向碰撞；x
n
＝r
n
&y
n
＝c
n
ꢀꢀꢀꢀ
(13)当移动机器人与动态障碍物的坐标满足以下关系式(14)时，判断移动机器人与动态障碍物发生停滞碰撞；x
n
＝r
n
&y
n
＝c
n
，r
n
＝r
c
&c
n
＝c
c
ꢀꢀꢀꢀ
(14)当移动机器人与动态障碍物的坐标满足以下关系式(14)时，判断移动机器人与动态障碍物发生追及碰撞；9.根据权利要求1所述的基于蚂蚁爬行机制的半未知环境下移动机器人路径探索方法，其特征在于，在s7中，所述采取避障策略进行紧急避障中，所述避障策略分别为转向行为策略、原地等待策略以及局部路径重规划策略。10.根据权利要求9所述的基于蚂蚁爬行机制的半未知环境下移动机器人路径探索方法，其特征在于，所述转向行为策略用于当移动机器人感知到即将与动态障碍物发生正向面碰撞、正向点碰撞或追及碰撞时，移动机器人通过左转向或者右转向的方式来避开动态障碍物；所述原地等待策略用于当移动机器人感知到即将与动态障碍物发生侧向碰撞时，移动机器人通过原地暂停的方式来避开动态障碍物。所述局部路径重规划策略，用于当动态障碍物停滞在机器人的行进路径上时，移动机器人利用蚂蚁爬行机制重新规划当前路径点i到第i+2个路径点之间的局部路径的方式来避开动态障碍物；当移动机器人通过上述方式避开动态障碍物后，移动机器人将会重新返回引导路径，然后沿着引导路径继续行驶。

技术总结
本发明公开了一种基于蚂蚁爬行机制的半未知环境下移动机器人路径探索方法。利用垂直映射法在栅格地图上生成待选引导点集合；在待选引导点集合的基础上，利用蚂蚁爬行机制生成引导路径；在移动机器人沿引导路径行进的过程中进行动态避障等。当移动机器人预测到即将和动态障碍物发生碰撞时，移动机器人采取避障策略进行紧急避障。在避障行为完成后，移动机器人重新回归引导路径，然后移动机器人沿着引导路径继续行驶,提高了移动机器人在半未知环境下到达终点的成功率，而且还能够改善移动机器人探索路径的质量，降低移动机器人探索路径所需的能耗。需的能耗。需的能耗。


技术研发人员：郝琨 张慧杰 李志圣 刘永磊 李成
受保护的技术使用者：天津城建大学
技术研发日：2023.03.07
技术公布日：2023/8/23