一种危险救援机器人路径规划方法与流程

本发明涉及路径规划技术领域，特别是一种危险救援机器人路径规划方法。

背景技术：

长期以来，各种灾难给人类的生产和生活带来了不同程度的破坏，灾难是人类过去、现在、将来所面对的最严峻的挑战之一。因此，如何在灾难发生后进行有效的救援成为人类所关注的一个重要课题。通常情况下，在灾难发生后，环境复杂而且恶劣，救援人员很难深入灾区进行救援，这给救援行动带来了极大的挑战。随着科学技术的进步，采用机器人代替人类完成各种危险的救援任务越来越成为一种趋势。目前，研究机器人救援路径的规划对提高机器人的救援效率，保证救援机器人高效地完成救援任务具有非常重要的作用和意义。该问题可归结为机器人路径规划问题，目前，利用启发式算法(微粒群算法、遗传算法、蚁群算法等)解决此类问题取得了较好的效果。其中，蚁群算法是通过信息素的累积和更新收敛于最优路径上，存在搜索时间长，容易出现早熟收敛，得到的解不是最优解等缺点。

技术实现要素：

本发明的目的是要提供一种危险救援机器人路径规划方法，提出了一种改进的蚁群算法，依据距离和生命强度定义了新的转移规则，依据救援人数定义了信息素更新策略；按照待救人员之间的距离和生命强度常数对最优解附近做微小的扰动，改变最优解的组合方式，寻找质量更优的解；帮助算法避免陷入局部最优，提高了算法的求解质量和收敛速度。

为达到上述目的，本发明是按照以下技术方案实施的：

一种危险救援机器人路径规划方法，包括以下步骤：

s1、读入数据：启动蚁群算法，读入待救人员的人数m，各待救人员的纵横坐标和生命强度常数σ和生命强度阈值设置机器人行驶速度v，机器人救援每个待救人员所消耗的时间tr，设置机器人的出发位置，设置对最优解进行调整的步长阈值c；

s2、参数初始化：令循环次数nc＝0，用来记录步长的变量i＝0，设置最大循环次数ncmax，令蚁群算法涉及的各参数为最佳配置值，信息素的初始值τij(0)＝0，δτij(0)＝0，最大救援人数f设置初始值为0，设置蚂蚁数量n；

s3、循环次数nc++，i++；

s4、对每一只蚂蚁，按照转移规则，得到机器人的行驶路线，计算该路线所救援的人数，如果获救人数大于f，则更新f；

s5、当所有的蚂蚁完成路线的搜索后，当nc<ncmax时，假如i<c，更新各路径上的信息素，转到步骤s3；假如i＝c，重新设置i＝0，对最优解进行调整，即假如出现更优解，更新f，更新各路径上的信息素，转到步骤s3；如果nc>ncmax，执行第步骤s6；

s6、输出最优解即机器人最佳救援路径。

进一步，所述步骤s1中，机器人在救援的过程中，判断某个待救人员是否可救需根据该待救人员的生命强度来进行判断，在机器人出发时，各待救人员的生命强度通过生命强度探测仪测得，待救援人员的生命强度为区间，表示为σ0＝σ±α，其中σ和α为两个常数；生命强度是一个随时间不断衰减的函数，经过时间t后，待救人员i的生命强度表示为：

由于待救人员i的生命强度是一个随时间变化的区间，这将导致机器人在实施救援的过程中难以判断待救人员是否可救，对上述区间加以改进，选取区间上下限的中间值作为此时待救援人员的生命强度，即：假设待救人员i的生命强度阈值为如果表示待救人员i可以得到救援。

进一步，所述步骤s2中保证机器人的救援人数最大的方法为，构建数学模型：假如σij＞δσj,i,j∈m,fij＝1，否则fij＝0，即待救人员j的生命强度大于设定阈值δσj才能得到救援；此外：表示机器人从起始点出发，每个待救人员最多只能得到一次救援；且在方法中，作有如下假设：在救援的过程中，待救人员的位置保持不变；环境现场带有生命强度探测仪，在开始救援之前可以实时给出各待救人员的生命强度值；机器人自身的电量足够完成一次救援任务；机器人救援一个待救人员所消耗的时间tr是相等的；机器人行驶的速度v保持恒定。

进一步，所述步骤s4中的转移规则具体为：对于机器人救援路径规划问题，机器人由待救人员i向下一个待救人员j转移时,需要考虑以下因素：路径上的信息素浓度；路径长度，优先原则为距离较短的待救人员优先；待救人员此时的生命强度σij(t)，优先原则为此时生命强度较小的待救人员优先，时间t为机器人到达待救人员i的时间+机器人救援i的时间+机器人从i到达j的路程消耗时间；

假设τij为机器人所在路径ij上的信息素浓度,，ηij(t)＝1/dij，dij越小，表示i和j之间的距离越短，j具有较高的优先权作为下一个被救援人员，σij(t)越小,表示此时待救人员的生命强度越小，j具有较高的优先权作为下一个被救援人员；

蚂蚁由待救人员i转向待救人员j的概率pij(t)为：

进一步，所述步骤s5中更新各路径上的信息素的具体步骤为：

由于在标准的蚁群算法中，路径(i，j)上的信息量在t+k时刻按如下规则进行调整：

τij(t+k)＝(1-p)·τij(t)+δτij(t)，

δτij(t)表示蚂蚁在本次循环中留在路径(i，j)上的信息量，计算公式如下：

公式中，q表示信息素强度，ls表示第s只蚂蚁在本次循环中所走路径的总长度；

蚂蚁在路径(i，j)上的信息素增量是与最终救援的人数f相关的，因此对信息素增量做如下调整：

fs表示第s只蚂蚁在本次循环中最终救援的人数。

进一步，所述步骤s5中对最优解进行调整具体方法为：

在机器人获得的最优解中，假如未获救人员距离前一个人员比较近，因此，调整未获救人员的位置，保证他们距离前一个队员的距离与其他未获救人员相比都是最近的距离，调整完毕后，假如出现更优解，调整最优解；

在机器人获得的最优解中，假如未获救人员的生命强度常数比较大，将未获救人员按照生命强度常数σ从大到小进行排序，已获救人员位置保持不变，排序完毕后，假如出现更优解，调整最优解。

与现有技术相比，本发明针对机器人救援路径问题提出了一种改进的蚁群算法，结合问题特征，定义了新的转移规则和信息素更新策略；按照待救人员的距离和生命强度常数设计了最优解调整策略，改变最优解的组合方式，在最优解附近做微小的扰动，寻找质量更优的解，帮助算法避免陷入局部最优，提高了算法的求解质量和收敛速度。

附图说明

图1为本发明实施例中1台机器人6个待救援人员救援路径。

具体实施方式

下面结合具体实施例对本发明作进一步描述，在此发明的示意性实施例以及说明用来解释本发明，但并不作为对本发明的限定。

1)问题建模

假设有m个待救人员(目标点)，分别表示为h1，h2，…，hm。为了救援这些被困人员，机器人需要按照一定的顺序进行救援，机器人的出发位置也看成一个目标点p0，救援顺序为p0，s1，…，si…，sm，其中，si∈h，1≤i≤m。图1显示救援场景中包含6个待救援人员，即m＝6，机器人从p0出发，按照p0-h2-h1-h3-h5-h6-h4的顺序进行救援。

机器人在救援的过程中，判断某个待救人员是否可救需根据该待救人员的生命强度来进行判断，在机器人出发时，各待救人员的生命强度可通过生命强度探测仪测得。待救援人员的生命强度为区间，表示为σ0＝σ±α，其中σ和α为两个常数。生命强度是一个随时间不断衰减的函数，经过时间t后，待救人员i的生命强度可以表示为:

由于待救人员i的生命强度是一个随时间变化的区间，这将导致机器人在实施救援的过程中难以判断待救人员是否可救，对上述区间加以改进，选取区间上下限的中间值作为此时待救援人员的生命强度,即：

假设待救人员i的生命强度阈值为如果表示待救人员i可以得到救援。

本发明求解的问题可以描述为:在救援场景中，已知m个待救人员，机器人从固定点出发，为机器人规划一条救援路径，在待救人员的生命期限内将其救起，保证机器人救援的人数最多。

2)在构造数学模型之前，定义相关变量如下:

i,j：机器人的出发点或待救人员序号，出发点序号为0。

dij：待救人员i到待救人员j的距离。

tij：机器人从待救人员i到待救人员j的时间。

σij：机器人从待救人员i到待救人员j时，待救人员j的生命强度。

xij：如果待救人员j在待救人员i之后被救援，xij＝1，否则xij＝0。

fij：如果待救人员j可以被成功救援，且xij＝1，fij＝1,否则fij＝0。

本发明所要建立的目标函数是保证机器人的救援人数最大，构造的数学模型如下:

假如σij＞δσj,i,j∈m,fij＝1，否则fij＝0，即待救人员j的生命强度大于设定阈值δσj才能得到救援。

此外：

公式(4)和公式(5)表示机器人从起始点出发，每个待救人员最多只能得到一次救援。

为了不失一般性，另外作出如下假设:

①在救援的过程中，待救人员的位置保持不变；

②环境现场带有生命强度探测仪，在开始救援之前可以实时给出各待救人员的生命强度值；

③机器人自身的电量足够完成一次救援任务；

④机器人救援一个待救人员所消耗的时间tr是相等的。

⑤机器人行驶的速度v保持恒定。

3)蚁群算法设计

(1)转移规则

对于机器人救援路径规划问题，机器人由待救人员i向下一个待救人员j转移时,需要考虑以下因素:

①路径上的信息素浓度；

②路径长度，优先原则为距离较短的待救人员优先；

③待救人员此时的生命强度σij(t)，优先原则为此时生命强度较小的待救人员优先。时间t应为机器人到达待救人员i的时间+机器人救援i的时间+机器人从i到达j的路程消耗时间；

假设τij为机器人所在路径ij上的信息素浓度,，ηij(t)＝1/dij，dij越小，表示i和j之间的距离越短，j具有较高的优先权作为下一个被救援人员。σij(t)越小,表示此时待救人员的生命强度越小，j具有较高的优先权作为下一个被救援人员。

综合以上因素,机器人(蚂蚁)由待救人员i转向待救人员j的概率pij(t)为:

(2)信息素更新策略

在标准的蚁群算法中，路径(i,j)上的信息量在t+k时刻可按如下规则进行调整：

τij(t+k)＝(1-ρ)·τij(t)+δτij(t)(7)

δτij(t)表示蚂蚁在本次循环中留在路径(i,j)上的信息量,计算公式如下：

公式(8)中，q表示信息素强度，ls表示第s只蚂蚁在本次循环中所走路径的总长度。

对于本发明所针对的机器人救援路径规划问题，蚂蚁在路径(i,j)上的信息素增量是与最终救援的人数f相关的，因此，本文对信息素增量做如下调整：

fs表示第s只蚂蚁在本次循环中最终救援的人数。

(3)最优解调整策略

对于机器人在救援时获得的最优解，为提高最优解的质量，考虑到距离和生命强度的影响，在搜索的过程中，每隔一段步长，在最优解附近做微小的扰动，调整最优解中未获救人员的顺序，有可能帮助算法寻找到更优的解。调整最优解的具体方法为：

1)在机器人获得的最优解中，假如未获救人员距离前一个人员比较近，这可能意味着该队员有可能获救。因此，调整未获救人员的位置，保证他们距离前一个队员的距离与其他未获救人员相比都是最近的距离。调整完毕后，假如出现更优解，调整最优解。例如：假设机器人最佳救援路径为0-3-2-4-5-6-1，在这条路径上，最终获救人员是3、2、4，未获救人员是5、6、1，此时，假如5、6、1中距离4最近的人员是6，则将6排在4的后面，接着假如5、1中距离6最近的人员是1，则将1排在6的后面，最终得到新的救援路径0-3-2-4-6-1-5。

2)在机器人获得的最优解中，假如未获救人员的生命强度常数比较大，这可能意味着该人员如果调整顺序，排在未获救人员靠前的位置，将有可能获救，改善解的质量。因此，可以将未获救人员按照生命强度常数σ从大到小进行排序，已获救人员位置保持不变，排序完毕后，假如出现更优解，调整最优解。

(4)算法具体流程

①读入数据。读入待救人员的人数m，各待救人员的纵横坐标和生命强度常数σ和生命强度阈值设置机器人行驶速度v，机器人救援每个待救人员所消耗的时间tr，设置机器人的出发位置，设置对最优解进行调整的步长阈值c。

②参数初始化。令循环次数nc＝0，用来记录步长的变量i＝0，设置最大循环次数ncmax，令算法涉及的各参数为最佳配置值。信息素的初始值τij(0)＝0，

δτij(0)＝0，最大救援人数f设置初始值为0，设置蚂蚁数量n。

③循环次数nc++，i++。

④对每一只蚂蚁，按照转移规则，得到机器人的行驶路线。计算该路线所救援的人数。如果获救人数大于f，则更新f。

⑤当所有的蚂蚁完成路线的搜索后，当nc<ncmax时，假如i<c,依据(2)小节，更新各路径上的信息素，转到步骤③步；假如i＝c,重新设置i＝0,按照(3)小节对最优解进行调整(假如出现更优解，更新f)。更新各路径上的信息素，转到步骤③步；如果nc>ncmax，执行第⑥步。

⑥输出最优解。

本发明的技术方案不限于上述具体实施例的限制，凡是根据本发明的技术方案做出的技术变形，均落入本发明的保护范围之内。