一种融合RRT与蚁群算法的三维空间多目标路径规划方法与流程

本发明涉及一种融合rrt与蚁群算法的三维空间多目标路径规划方法，属于机器人路径规划领域。

背景技术：

机器人路径规划一直是机器人智能控制领域的研究热点，而随着无人机在民用和军事上越来越广泛的应用，无人机所在的三维空间中的路径规划问题也变得愈发重要。在无人机民用方向中的电力巡检和光伏发电设备巡检，以及军事方向中的无人机多任务点巡航，都涉及到了三维空间中的多目标路径规划问题，即在三维空间中规划一条无障碍经过所有目标点且要求总路径距离趋于最小的回路。

三维空间中的多目标路径规划主要涉及到两个问题，第一是任意两个目标点之间的三维路径，即以两个目标点为起止点，规划一条避开所有障碍且路径距离趋于最小的路径；第二是各个目标点的经过顺序，即以一个确定的顺序依次经过各个目标点构成一个闭合回路，类似于旅行商问题，区别在于旅行商问题中任意两点间的路径代价是已知的，而三维空间中的多目标路径规划问题中任意两点间的路径代价一开始是未知的，需要规划两点间的路径后才能已知其路径代价。对于三维空间中的多目标路径规划问题，已有的研究是将其拆分成两个独立的问题，三维路径规划和旅行商问题，先用三维空间中的路径规划方法如a*算法、rrt算法和遗传算法等对各个目标点之间的路径进行规划，求出任意两个目标点之间的路径及路径距离，这样就把问题转换成了已知各目标点间路径代价的普通旅行商问题，然后再用动态规划、模拟退火算法和蚁群算法等对该旅行商问题进行求解，这样的方法需要将大量的时间用于各个目标点之间的路径规划，随着目标点数量的增多，各个目标点间的路径规划成了该问题的主要时间消耗，成为庞大的计算负担。如果能找到一种新的方法，既能显著降低计算量和计算时间，又不降低解的优良性的话，将会是对该问题的一大帮助。

技术实现要素：

本发明的目的在于针对现有技术的不足，提供一种融合rrt与蚁群算法的三维空间多目标路径规划方法。

本发明解决上述技术问题的技术方案如下：一种融合rrt与蚁群算法的三维空间多目标路径规划方法，该方法包括以下步骤：

步骤a：计算各目标点之间的初始路径代价；

步骤b：将三维空间中多目标路径规划问题等效于已知路径代价的普通旅行商问题，进行一次蚁群算法迭代，得到该次迭代中遍历所有目标点的最优回路，并在迭代过程中记录各段路径上的蚂蚁经过次数；

步骤c：将各段路径上的蚂蚁经过次数与阈值n比较，如果超过阈值，则调用rrt更新该段路径代价；

步骤d：该次迭代结束时，调用rrt更新最优解所包含的各段路径的路径代价；

步骤e：判断已进行的迭代次数，如果没有达到预设迭代总次数，则重复步骤bcde，否则执行步骤f；

步骤f：将各次迭代的最优解中路径总代价最小的解作为最终解。

进一步地，所述步骤a中，将各目标点之间的欧氏距离即直线距离选取为各目标点之间的初始路径代价，将该路径代价用于蚁群优化的初始迭代。

进一步地，所述步骤c中，将各段路径上的蚂蚁经过次数与阈值n比较，如果该段路径的蚂蚁经过次数超过阈值，且其路径代价未被修正过，则调用rrt计算该段路径代价，并将计算所得路径代价作为该段路径修正后的路径代价。

进一步地，所述步骤c中，阈值n的设定规则：n＝m*iter*2/n，其中m为蚂蚁个数，iter为总迭代次数，n为目标点个数。

进一步地，所述步骤d中，在该次迭代结束时，如果该次迭代的最优解路径总代价低于以往迭代中的最优解，则对于该次迭代的最优解所包含的每一段路径，如果其路径代价未被修正过，则调用rrt计算该段路径，并将计算所得路径的路径代价作为该段路径修正后的路径代价。

本发明的有益效果是：通过对rrt和蚁群算法的融合，以欧氏距离作为初始路径代价，在蚁群优化迭代过程中修正路径代价，避免了用于各目标点间路径规划的大量冗余计算量，显著节省了计算时间，同时保留了最终解的优良性。

附图说明

图1是蚁群算法应用于旅行商问题的流程图；

图2是rrt算法流程图；

图3是确定新节点示意图；

图4是碰撞检测方法示意图；

图5是融合rrt与蚁群算法的三维空间多目标路径规划方法的流程图。

具体实施方式

以下结合附图对本发明的原理和具体实现方法进行描述。

本发明提供的一种融合rrt与蚁群算法的三维空间多目标路径规划方法，该方法可应用于无人机光伏设备巡检或军事目标巡航等无人机多目标路径规划场景中，其具体流程包括：

步骤a：计算各目标点之间的初始路径代价；

步骤b：将三维空间中多目标路径规划问题等效于已知路径代价的普通旅行商问题，进行一次蚁群算法迭代，得到该次迭代中遍历所有目标点的最优回路，并在迭代过程中记录各段路径上的蚂蚁经过次数；

步骤c：将各段路径上的蚂蚁经过次数与阈值n比较，如果超过阈值，则调用rrt更新该段路径代价，执行步骤d，否则直接执行步骤d；

步骤d：该次迭代结束时，调用rrt更新最优解所包含的各段路径的路径代价；

步骤e：判断已进行的迭代次数，如果没有达到预设迭代总次数，则重复步骤bcde，否则执行步骤f；

步骤f：将各次迭代的最优解中路径总代价最小的解作为最终解。

进一步，所述步骤a包括：

计算各目标点之间的欧式距离即直线距离，将其作为各目标点之间的初始路径代价，以此初始路径代价参与蚁群优化的迭代过程。计算公式如下：

其中d为路径代价，x1、y1、z1分别为其中一个目标点的三维空间坐标值，x2、y2、z2为另一个目标点的三维空间坐标值。

进一步，所述步骤b包括：

进行一次蚁群算法迭代，此时三维空间中多目标路径规划问题等效于已知路径代价的普通旅行商问题，蚁群算法用于旅行商问题时，其计算流程如图1所示，一次迭代的计算步骤如下：

1)初次迭代前，初始化算法运行参数，本实施例中，设置蚂蚁数量m＝40，最大迭代次数itermax＝100，信息素重要程度因子α＝1，启发式函数重要程度因子β＝5，信息素挥发因子ρ＝0.1，信息素更新系数q＝1。每一次迭代开始时，对于各个蚂蚁，分别随机选取一个目标点作为其起始点。

2)各个蚂蚁根据路径信息素浓度τij和启发式函数依次选取一个目标点作为下一目标点，记录所选路径上的蚂蚁经过次数，直到每个蚂蚁都选择完所有目标点构成闭合回路。其中，两个目标点之间的启发式函数为该两点间的路径代价dij的倒数，对某一个蚂蚁来说从当前目标点选取下一可选目标点的概率pij计算式如下：

其中求和公式是对该蚂蚁的剩余可选目标点进行计算求和。

3)每个蚂蚁都选择完所有目标点构成闭合回路后，对所有路径上的信息素浓度进行更新，信息素浓度更新包括信息素的挥发和增加，信息素挥发是针对所有路径，其公式如下：

τij＝(1-ρ)τij

信息素增加只针对被蚂蚁选择的路径，其公式如下：

τij＝τij+q/li

其中li为各个蚂蚁所选择回路的路径总代价。

4)比较各个蚂蚁所选择回路的路径总代价li，将路径总代价最小的回路作为该次迭代的最优解，即该次迭代中遍历所有目标点的最优回路。

进一步，所述步骤c包括：

将各段路径上的蚂蚁经过次数与阈值n比较，如果该段路径的蚂蚁经过次数超过阈值，且其路径代价未被修正过，则调用rrt计算该段路径代价，并将计算所得路径代价作为该段路径修正后的路径代价。阈值n的设定规则为n＝m*iter*2/n，其中m为蚂蚁个数，iter为总迭代次数，n为目标点个数。

rrt(快速随机搜索树)算法采用随机采样的规划方法，不需要预处理，搜索速度快，在高维空间中速度优势尤为明显，其算法流程图如图2所示，基本思想就是通过不断生成采样点，根据采样点位置生成新树枝，从而产生一个从起点开始不断生长的搜索树，当该搜索树能连到终点时，即找到了一条从起点到终点的连通路径。rrt算法基本步骤如下：

1)生成采样点。这里生成采样点采用目标偏移型策略，在采样空间即路径规划空间中生成采样点时，除了在采样空间中随机生成一个点作为采样点外，还有一定几率将终点作为采样点，这样可以使树偏向终点生长，加快算法收敛速度。在此实例中选用终点作为采样点的几率为0.6。

2)确定新节点。如图3所示，生成采样点qrand后，在已有的树节点中找到距离采样点最近的树节点qnear，最近的树节点qnear和采样点qrand的连线上距离最近的树节点qnear一定距离lseg的点即作为新节点qnew。在此实例中lseg选为路径规划空间长度最小的维上，规划空间长度的十分之一。

3)碰撞检测。如果qnear和qnew中间有障碍，则放弃该新节点，从生成采样点的步骤重新开始。检测两点q′和q″是否无障碍连通时，如图4所示，在两点间均匀生成一系列中间点，如果每一个中间点都不在障碍范围内，则认为该两点无障碍连通。在此实例中，中间点数目n的设置规则如下：

n＝120dq/d

其中dq为两点间距离，d为路径规划空间距离最远的两点间距离，中间点数目n的下限为7，当n小于7时则设置n为7。

4)添加新树枝。这里采用rrt算法的变种rrt*中的策略，即生成新节点后，对新节点一定半径圆内的已有树节点进行路径代价比较，选择路径代价最低的树节点作为新节点的父节点来生成新树枝，另外，若该新节点一定半径圆内的已有树节点将新节点作为父节点能降低其路径代价的话，则将其父节点改为新节点，这样能够找到趋近于最优解的较优解。在此实例中，圆的半径选取规则为：

其中ntree为已有的树节点数目。

5)判断树是否能够连接到终点，如果不能则重复上述步骤，如果已经连接到终点，则从连接终点的树节点开始不断找出其父节点，直到找到起点，这样就找到一条从起点开始连接终点的路径，最后用贪心算法原则去除该路径上多余的中间点后得到最终路径。

进一步，所述步骤d包括：

在该次迭代结束时，如果该次迭代的最优解路径总代价低于以往迭代中的最优解，则对于该次迭代的最优解所包含的每一段路径，如果其路径代价未被修正过，则调用rrt计算该段路径，rrt计算过程同步骤c中所述，并将计算所得路径的路径代价作为该段路径修正后的路径代价。

进一步，所述步骤e包括：

每一次迭代结束后，将已迭代次数与预设迭代总次数比较，如果没有达到总次数，则继续迭代，重复步骤bcde。

进一步，所述步骤f包括：

迭代次数达到预设迭代总次数后，在各次迭代的最优解中找出路径总代价最小的解，并将其作为最终解，最终解包括各个目标点的经过顺序以及在此经过顺序中任意两个相邻目标点间的三维路径。

由以上说明，可总结出融合rrt与蚁群算法的三维空间多目标路径规划方法的流程图，如图5所示，本发明通过对rrt和蚁群算法的融合，以欧氏距离作为初始路径代价，在蚁群优化迭代过程中修正路径代价，避免了用于各目标点间路径规划的大量冗余计算量，显著节省了计算时间(以40个目标为例可节省约80％用于各目标点间路径规划的时间)，同时保留了最终解的优良性。

上述实施例用来解释说明本发明，而不是对本发明进行限制，在本发明的精神和权利要求的保护范围内，对本发明作出的任何修改和改变，都落入本发明的保护范围。