CN116203975A

CN116203975A - 一种机器人路径规划的方法

Info

Publication number: CN116203975A
Application number: CN202211450970.2A
Authority: CN
Inventors: 秦剑; 胡资江
Original assignee: Guangzhou University
Current assignee: Guangzhou University
Priority date: 2022-11-18
Filing date: 2022-11-18
Publication date: 2023-06-02

Abstract

本发明涉及水下机器人路径规划领域，且公开了一种机器人路径规划的方法，包括以下步骤：基于二次采样和双向扩展策略以及路径缓存策略得到多策略改进的RRT*算法；引入基于优化启发函数和信息素更新规则策略的改进蚁群算法对搜索过程进行优化；改进的RTT*算法与改进后的蚁群算法相结合，形成多策略改进的RRT*算法，实现水下机器人三维空间多目标路径规划方法。该机器人路径规划的方法，将多种改进策略与RRT*算法相结合，形成改进的RRT*算法，可快速获得起始路径并且减少冗余，同时保证了准确度，可以通过改进的RRT*算法获得最优的规划路径。这些优点大大提高了水下机器人的路径规划效率，对建立高效的水下路径工程很有益处。

Description

一种机器人路径规划的方法

技术领域

本发明涉及水下机器人路径规划领域，具体为一种机器人路径规划的方法。

背景技术

现有的水下机器人路径规划设计方法只能解决计算量大、同一环境下多条规划路径结果不一致、搜索空间过大、路径搜索过程随机性太大和搜索过程收敛速度慢等问题的其中一个或者几个，在动态障碍物的情况下，路径规划成为一个复杂且耗时的过程，使工作过程变得繁琐且效率低，也不利于水下机器人路径规划的研究；

现有技术存在的缺点

由于使用统一随机抽样进行路径搜索的RRT算法稳定性较差，因此很难保证得到的结果是全局最优路径。

RRT*是一种基于统一随机抽样的搜索方法。虽然该算法可以在一定程度上获得最佳路径，但随着随机树大小的增加，计算量和内存消耗将以更快的速度增加。计算速度大大降低。RRT*算法可以看作是以牺牲一定的计算成本为代价对开关路径进行更好的优化。

本发明旨在将多种策略改进的多种算法相结合，限制算法的采样区域，压缩其无用的随机采样区域，建立水下机器人路径规划模型，利用该模型可以在三维障碍空间中完成最优路径的规划。利用MATLAB环境下的三维水下模拟空间对模型进行了充分论证，为此我们提出了一种机器人路径规划的方法。

发明内容

(一)解决的技术问题

针对现有技术的不足，本发明提供了一种机器人路径规划的方法，解决了上述的问题。

(二)技术方案

为实现上述所述目的，本发明提供如下技术方案：一种机器人路径规划的方法，包括以下步骤：

第一步：基于二次采样和双向扩展策略以及路径缓存策略得到多策略改进的RRT*算法；

第二步：引入基于优化启发函数和信息素更新规则策略的改进蚁群算法对搜索过程进行优化；

第三步：改进的RTT*算法与改进后的蚁群算法相结合，形成多策略改进的RRT*算法，实现水下机器人三维空间多目标路径规划方法。

优选的，所述第一步中的二次采样和双向扩展策略包括以下内容：

从第二个节点开始，向后检查起始节点与当前节点之间的连接是否存在障碍物干扰；

如果有干扰，则删除节点；

如果没有干扰，则保留节点；

检查过程结束后，可以有效地删除路径中的冗余节点，同时，通过双向扩展随机树搜索策略查找初始路径的基础上，引入Dijkstra算法对扩展随机树进行二次采样以搜索节点矩阵，如果引入Dijkstra算法后优化路径的总成本较小，则更新路径并将其作为最优解，从而将初始路径更新为最优路径。

优选的，所述路径缓存策略包括以下内容：

采用路径缓存作为随机树的生长引导函数，从节点x_near生长新节点的改进的生长引导函数F(x_near)为：

F(x_near)＝R(x_near)+G(x_near)+P(x_near)……(1)；

R(x_near)为节点的随机采样函数，G(x_near)为目标偏置函数，P(x_near)为路径缓存偏置函数，目标向量x_goal对x_near的引力可以表示为：

G＝k_g·||x_goal-x_near||……(2)；

向量x_goal为目标点的位置，||x_goal-x_near||表示节点与目标点之间的距离，ρ表示搜索步长，k_g表示重力系数，获得以下公式作为目标偏差函数

路径缓存偏移量函数构造为：

k_p为路径缓存偏移系数，x_path，i为路径缓存中第i点的位置向量，||x_path，i-x_near||，表示点到节点的欧几里得距离，基本RRT*算法的随机展开函数为：

将公式(3)、(4)、(5)代入公式(1)，得到：

生成新节点的公式进一步得到如下：

定义了p_goal、p_path和p_rand三种概率，其中p_goal表示根据公式(5)(6)(7)选择目标点作为新节点，即随机树向目标生长的概率，p_path表示选择路径缓存点作为新节点的概率，p_rand表示选择一个随机点作为新节点的概率；

p_goal+p_path+p_rand＝1……(8)；

抽样公式为：

其中是x_goal目标点的坐标，x_path，i是路径缓存中第i个点的坐标，x_free是采样空间中随机点的坐标。

优选的，所述第二步中的具体内容如下：

改进蚁群算法公式所示：

d_sj为起点s到下一个节点j的距离，值越大，距离起始点s越远，η_ij越大，节点j被选中的概率越大，d_ij表示当前节点i与下一个节点j之间的距离，d_jG表示下一个节点j与目标点G之间的距离，两者之和越小，η_ij则越大，节点j被选中的概率越大；

信息素更新规则为：

L_mid表示完成一次迭代后所有蚂蚁路径总长度的平均值，L_k表示蚂蚁k本次所走路径的长度，A表示一个常数，与算法的迭代次数有关。

优选的，所述第三步中的具体内容如下：

S1：计算每个目标点之间的初始路径代价；

S2：三维空间中的多目标路径规划问题等价于已知路径代价的普通旅行商问题，通过蚁群算法迭代得到迭代中遍历所有目标点的最优环路，并在迭代过程中记录通过路径每段的蚂蚁数量；

S3：比较路径每段的蚂蚁数量和阈值N，如果超过阈值，调用RRT*更新该段的路径开销；

S4：迭代结束时，调用RRT*，更新包含在最优解中的每段路径的路径代价；

S5：确定已经执行的迭代次数，如果没有达到预设的迭代总次数，则重复步骤BCDE，否则执行步骤F；

S6：将每次迭代的最优解中路径总代价最小的解作为最终解。

优选的，所述S1中选择目标点之间的欧氏距离即直线距离作为目标点之间的初始路径代价，并将该路径代价用于蚁群优化的初始迭代。

优选的，所述S3中蚂蚁的数量经过的每个部分路径相对于阈值N，如果蚂蚁经过的路径超过阈值，并且它的路径成本不变，则调用RRT*计算部分，计算路径代价，并将计算出的路径代价作为这段路径的修正路径代价。

优选的，所述S3中阈值设置规则N:N＝m*iter*2/n，其中m为蚂蚁数量，iter为总迭代次数，n为目标点个数。

优选的，所述S4中在一次迭代结束时，如果这次迭代的最优解路径的总代价低于前几次迭代的最优解，则在本次迭代最优解中包含每个元素。对于一条路径，如果它的路径代价没有被纠正，则调用RRT*计算该路径，并将被计算路径的路径代价作为该路径的修正路径的代价。

(三)有益效果

与现有技术相比，本发明提供了一种机器人路径规划的方法，具备以下有益效果：

1、该机器人路径规划的方法，将多种改进策略与RRT*算法相结合，形成改进的RRT*算法，可快速获得起始路径并且减少冗余，同时保证了准确度，可以通过改进的RRT*算法获得最优的规划路径。这些优点大大提高了水下机器人的路径规划效率，对建立高效的水下路径工程很有益处。

2、该机器人路径规划的方法，可以快速获得初始路径并且削减冗余路径，并且避免随机采样，同时提高路径搜索过程的收敛速度，再利用该方法获得路径规划，从而高效准确地计算水下机器人规划路径。

3、该机器人路径规划的方法，常用的路径规划算法在均匀随机采样过程中有计算量大、同一环境下多条规划路径结果不一致等问题，并且对于多个问题无法同时解决，需要多次利用不同算法解决每次解决问题时所出现的新问题，非常繁琐不利于实际工程的路径规划。实验结果表明，与基本RRT*算法、蚁群算法和粒子群算法相比，改进算法在水下机器人路径规划中同时具有节点少、收敛速度快、精度高等优点。

附图说明

图1为双向扩展和二次采样RRT*算法总体流程示意图；

图2为融合两策略改进的RRT*算法流程示意图；

图3为初始化路径规划环境示意图；

图4为三种算法的路径规划结果示意图；

图5为两种算法路径规划的节点使用情况示意图；

图6为三种算法的收敛曲线示意图；

图7为本文算法在两种地形下的路径规划示意图；

图8为三种算法的路径规划结果示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

请参阅图1-8，一种机器人路径规划的方法，包括以下内容：

第一步：算法的改进

S1：多策略改进的RRT*算法

A.二次采样和双向扩展策略

在水下机器人路径规划过程中，一个优秀的路径规划策略可以提高路径搜索的效率和质量。但是，由于基本RRT*算法是均匀采样的，因此在大多数情况下，搜索树中会产生许多冗余分支。由于搜索的路径可能不是最优的，因此它将包含许多噪声节点，使搜索的路径不够平滑。

因此，提出一种基于双向扩展的RRT*算法来完成水下路径规划。为了过滤尽可能多的冗余路径节点，从第二个节点开始，向后检查起始节点与当前节点之间的连接是否存在障碍物干扰。如果有干扰，则删除节点；如果没有干扰，则保留节点，检查过程结束后，可以有效地删除路径中的冗余节点。同时，在通过双向扩展随机树搜索策略查找初始路径的基础上，引入Dijkstra算法对扩展随机树进行二次采样以搜索节点矩阵。如果引入Dijkstra算法后优化路径的总成本较小，则更新路径并将其作为最优解，从而将初始路径更新为最优路径。基于双向扩展RRT*算法的水下路径规划总体流程如附图1所示；

基本RRT*算法的采样策略是在每次搜索中仅使用初始节点的随机采样来将随机树向前移动。为了提高在包含障碍物的状态空间中路径搜索的效率，扩展待探索的空间，双向扩展RRT*算法从起点和目标点同时生长两棵快速扩展的随机树来搜索状态空间，提高了搜索效率，快速求解了初始规划路径。与基本的RRT*算法相比，两棵随机树的并行搜索不仅提高了求解效率，而且基于两棵随机树的交替吸引，可以快速获得更短的初始规划路径。

B.路径缓存策略

在动态环境中，由于动态障碍物或新障碍物的出现，需要对RRT*算法的随机树进行适当的调整。当遇到原地图没有的障碍物时，部分原路线将被放弃，及时重新规划。新规划的路线既要避开当前的障碍物，又要尽可能保持原路线的走向。为了使重新规划的新路径偏向原路径，需要在方案中设置路径缓存，以保存事先静态规划得到的原路径上的节点。当新的路径规划开始采样时，有一定概率会选择这些点作为随机点来引导新生成的节点，进而引导随机树的生长。

因此，在考虑目标重力偏差的影响时，采用路径缓存作为随机树的生长引导函数。从节点x_near生长新节点的改进的生长引导函数F(x_near)为：

F(x_near)＝R(x_near)+G(x_near)+P(x_near)……(1)

其中，R(x_near)为节点的随机采样函数，G(x_near)为目标偏置函数，P(x_near)为路径缓存偏置函数。目标向量x_goal对x_near的引力可以表示为：

G＝k_g·||x_goal-x_near||……(2)

其中，向量x_goal为目标点的位置，||x_goal-x_near||表示节点与目标点之间的距离，ρ表示搜索步长，k_g表示重力系数。接下来，可以获得以下公式作为目标偏差函数

类似地，路径缓存偏移量函数可以构造为：

其中，k_p为路径缓存偏移系数，x_path，i为路径缓存中第i点的位置向量，||x_path，i-x_near||，表示点到节点的欧几里得距离，基本RRT*算法的随机展开函数为

将公式(3)、(4)、(5)代入公式(1)，可以得到：

生成新节点的公式进一步得到如下：

定义了p_goal、p_path和p_rand三种概率，其中p_goal表示根据公式(5)(6)(7)选择目标点作为新节点，即随机树向目标生长的概率；p_path表示选择路径缓存点作为新节点的概率；p_rand表示选择一个随机点作为新节点的概率，其中

P_goal+p_path+P_rand＝1……(8)

三个概率中p_goal和P_path的确定更为关键，需要根据不同的具体情况进行多次仿真实验来确定。使用p_goal＝0.3，P_path＝0.6作为调试的初始参数。

这部分主要修改了RRT*算法中新增的节点选择函数。该操作首先选择一个随机数P和一个随机整数i，其中P的选择范围为0到1，i的选择范围为1到N，其中N为路径缓存中的节点数。如果P小于p_goal，则向目标点生成一个新节点；如果P大于p_goal且小于p_goal+p_path，则向路径缓存路径中的第i个点生成一个新节点；否则，向随机点生成一个新节点。由此，抽样公式为

其中是x_goal目标点的坐标，x_path，i是路径缓存中第i个点的坐标，x_free是采样空间中随机点的坐标。从公式(8)可以看出，三个概率之和是一定值，所以目标偏差概率和路径缓存偏差概率决定了随机采样的概率。当随机树以某个目标为导向生长时，其对无用节点的探索必然会减少。因此，如何引导随机树向更有利于找到有效路径的方向生长就显得尤为关键。本章以目标点和预先规划的路径节点作为有效方向，指导随机树的生长，从而限制随机抽样，减少无用节点的计算。融合两策略改进的RRT*算法流程图如下：

附图2融合两策略改进的RRT*算法流程图

S2：蚁群算法的改进

为了使减少无用节点的搜索过程收敛，提高搜索效率，引入基于优化启发函数和信息素更新规则策略的改进蚁群算法对搜索过程进行优化。

在基本蚁群算法，启发式函数只考虑当前节点的下一个节点，而不考虑起始点和目标点，这将导致算法陷入僵局在选择下一个节点，使算法停滞不前。因此，有必要对启发式函数进行改进，使蚂蚁在选择下一个节点时远离起点而接近目标点。其改进形式如公式所示:

式中:d_sj为起点s到下一个节点j的距离。值越大，距离起始点s越远，η_ij越大，节点j被选中的概率越大。d_ij表示当前节点i与下一个节点j之间的距离，d_jG表示下一个节点j与目标点G之间的距离，两者之和越小，η_ij则越大，节点j被选中的概率越大。

基本蚁群算法会在每次迭代结束时对所有走过的路径进行信息素更新，但在前几次迭代，对最优路径的搜索仍处于探索阶段，蚂蚁难免会走一些弯路。如果每次都在与最优路径无关的路径上进行信息素更新，可能会误导后续蚂蚁，使最短路径曲线收敛非常缓慢，甚至根本找不到最优路径。因此，在搜索的后期阶段，放弃差路径，减少差路径上的信息素量，使蚂蚁不选择该路径，提高了算法的效率。信息素更新规则为:

式中:L_mid表示完成一次迭代后所有蚂蚁路径总长度的平均值；L_k表示蚂蚁k本次所走路径的长度；A表示一个常数，与算法的迭代次数有关。

第二步：水下机器人路径规划模型的设计

S1：设计方法

水下机器人导航的规划路径与地面任务的规划路径不同，是三维空间中的典型问题。

测试过程中，在MATLAB环境下对基本RRT*算法和本文改进算法进行了比较。具体测试环境为：Intel Core i7 10700；2.90GHz处理器；内存16GB；操作系统为Win10。本实验设置搜索步长为1，循环终止步数为4000步，地图大小为100*100*100，起点为(5,5,5)，终点为(95,95,95)。为了使实验结果更具可比性，相同的地图用于不同算法的验证。初始化路径规划环境如附图3。

附图3初始化路径规划环境图

S11：规划流程

本文提出将改进的RTT*算法与改进后的蚁群算法相结合，形成多策略改进的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*计算该路径，并将被计算路径的路径代价作为该路径的修正路径的代价。

附图4(a)为基本RRT*算法的路径规划结果，附图4(b)为双向扩展并二次采样策略改进后的RRT*算法的路径规划结果，附图4(c)为双向扩展并二次采样以及路径缓存策略改进后的RRT*算法的路径规划结果。

由于采样的随机性，即使地图上障碍物的位置和属性完全相同，搜索到的路径也不相同。本实验中，搜索步长为1。棕色圆圈为障碍物，黑色高亮路线为最终路径，绿色为节点，红色细线为节点扩展时RRT*的新节点与其他节点的连接。

从图4(a)中可以看出，由于RRT*算法基于随机均匀采样，因此在采样过程中获得的随机点几乎遍布整个自由空间。显然，大多数采样点对最终计算路径没有贡献。RRT*算法对每个采样点执行父节点优化。随着随机树的增长，此优化过程将在新节点附近有更多的点。因此，这些无用的节点不仅不能帮助算法找到最终的路径，而且使算法执行大量无用的计算，消耗更多的计算时间和内存。

从图5中可以看出，通过子采样策略优化的输出路径具有较少的采样节点和较短的路径。从图5中可以看出，采用改进的路径缓存策略后，节点利用率和路径长度都有了显著的提高。

表1显示了当障碍物的数量和位置相同，起点和终点相同时，MATLAB仿真环境中两种不同方法的性能比较。对于每种方法，在MATLAB仿真环境中执行了50次运行。获取50组路径长度和计划时间值，并取平均值。从表I.可以看出，基本的RRT*算法不仅需要很长时间的规划，而且路径质量也不理想。基于子采样策略的改进RRT*算法减少了规划时间，提高了搜索效率和规划路径的质量，并大大缩短了规划路径的长度。

表1两种方法生成的路径长度和规划时间的比较

因为基本的RRT*算法很难保证在相同的约束下可以多次规划完全相同的路径。使用路径缓存策略缓存第一个路径规划的结果作为下一个规划的路径缓存，可以防止每个规划以完全随机的方式启动，并且可以进一步避免出现无用的节点。此外，RRT*算法可以在当前随机树下找到最优解，因此结合路径缓存策略，重新规划的路径可以更接近全局最优路径。为了验证路径缓存策略的有效性，在静态环境中使用路径缓存策略对同一地图执行多个计算。生成的路径和节点使用情况如图4(c)和表2所示。

表2使用路径缓存策略的RRT*算法的节点使用情况

从图5和表II.的分析可以看出，使用路径缓存策略的路径规划在获得的有效路径中具有很大程度的一致性。由于采样过程中的随机性降低，对无用节点的探索也减少了。但是，路径节点不会随着重新规划的次数而单调减少，这由扩展步骤和扩展距离决定。当步长固定时，相同条件下的路径节点和路径长度不能随着计算次数的无限减少而无限减少。当路径趋于最优时，路径节点的数量会围绕最优解波动，但由于使用的节点数量的减少，节点利用率得到提高。

表3显示了基于本文算法的路径规划方法与其他3种水下机器人路径探索方法的比较实验。

表3水下机器人路径探索的对比实验

方法1是基于基本RRT*算法的路径规划方法。方法2是基于RTT*算法的路径规划方法，该算法通过双向扩展和子采样策略进行了改进。方法3是一种基于RTT*算法的路径规划方法，通过双向扩展和子采样策略以及路径缓存策略进行改进。

从表3可以看出，改进的RRT*算法和改进的蚁群算法相结合，显著减少了水下机器人路径探索的迭代次数。与前三种方法相比，本文的方法分别减少了88％、83％和71％，拐点数量分别减少了82％、74％和38％。

在确定改进算法的有效性后，为了进一步研究改进算法的稳定性和其他参数，分别采用3种策略对100个随机障碍物环境下的基本RRT*算法和改进RRT*算法进行了仿真实验。并记录节点使用情况。图6显示了所执行的100次模拟中节点使用情况的折线图。

从图6可以看出，由于改进算法引入了路径缓存偏移，随机采样的概率降低了。因此，需要计算的节点数量是有限的，这减少了冗余计算和内存消耗。总体而言，计算时间与迭代次数有很大的相关性，因此在减少使用的节点数后，运行时间也减少了。由于RRT*算法会带来更多的冗余计算，引入路径缓冲区偏移采样，在一定程度上抵消了这部分计算时间，改进后的算法节省了约60％的计算时间。这也说明了节点数量与计算时间之间的相关性，并且由于RRT*的计算量也与随机树的大小有关，因此节点数量的减少也进一步减小了随机树的大小。

为了进一步验证，在同一模拟场景中，比较了本文算法的拐点数和迭代次数，比较了蚁群算法和粒子群算法。图7显示了三种算法的收敛曲线。从图7可以看出，基本的蚁群算法在18次迭代后收敛，粒子群算法在13次迭代后收敛。本文算法只需要8次迭代即可收敛。

表4基于本文算法、蚁群算法和粒子群算法的路径规划方法，对水下机器人路径探索进行了对比实验。

表4三种算法结果的比较

通过对比表4中三种算法的仿真结果可以看出，本文算法能够快速有效地搜索全局最优路径。与蚁群算法和粒子群算法相比，路径长度分别减小了31％和10％，收敛速度最快，全局优化能力提高。本文算法在最小路径长度、迭代次数和拐点数等方面具有一定的优势。

在MATLABR2016环境中随机生成两个不同的地形环境，本文中的算法在相同的参数下运行。仿真结果如附图7(a)和附图7(b)所示。仿真结果表明，该算法在特定地形环境下具有良好的适应性。

附图8显示了三种算法在同一地形下的路径规划结果。附图8(a)是基本蚁群算法的路径规划结果。附图8(b)是粒子群算法路径规划的结果，附图8(c)是本文算法路径规划的结果

可以看出，本文算法在路径规划的路径长度和拐点数上取得了一定的优势。实验结果表明，本文优化的启发式函数和信息素更新规则策略可以有效克服蚁群基本算法收敛速度慢、容易落入局部最优解的缺点。它可以有效地降低水下机器人在实际应用中的消耗，具有良好的工程意义。

结果说明，针对RRT*算法在同一环境下计算量大、路径规划结果不一致的问题，提出一种改进的RRT*算法。首先，采用双向扩展策略进行全局快速搜索，得到连接起点和目标点的初始路径，然后引入子采样策略对路径进行优化，计算路径节点中的最短距离，消除初始路径上的冗余节点，优化初始路径，得到最优路径。同时，为避免算法随机抽样导致多个规划结果不一致，引入路径缓存策略进行校正。最后，将基于改进启发函数的蚁群算法与信息素更新规则策略相结合，形成改进的RRT*算法。通过MATLAB仿真实验验证了双向扩展和子采样策略、路径缓存策略、优化的启发式函数和信息素更新规则策略的有效性。

尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由所附权利要求及其等同物限定。

Claims

1.一种机器人路径规划的方法，其特征在于，包括以下步骤：

第三步：改进的RTT*算法与改进后的蚁群算法相结合，形成多策略改进的RRT*算法，实现水下机器人三维空间多目标路径规划方法。。

2.根据权利要求1所述的一种机器人路径规划的方法，其特征在于：所述第一步中的二次采样和双向扩展策略包括以下内容：

如果有干扰，则删除节点；

如果没有干扰，则保留节点；

检查过程结束后，可以有效地删除路径中的冗余节点，同时，通过双向扩展随机树搜索策略查找初始路径的基础上，引入Dijkstra算法对扩展随机树进行二次采样以搜索节点矩阵，如果引入Dijkstra算法后优化路径的总成本较小，则更新路径并将其作为最优解，从而将初始路径更新为最优路径。。

3.根据权利要求1所述的一种机器人路径规划的方法，其特征在于：所述路径缓存策略包括以下内容：

F(x_near)＝R(x_near)+G(x_near)+P(x_near)......(1)；

G＝k_g·||x_goal-x_near||......(2)；

路径缓存偏移量函数构造为：

k_p为路径缓存偏移系数，x_path,i为路径缓存中第i点的位置向量，||x_path,i-x_near||，表示点到节点的欧几里得距离，基本RRT*算法的随机展开函数为：

将公式(3)、(4)、(5)代入公式(1)，得到：

生成新节点的公式进一步得到如下：

p_goal+p_path+p_rand＝1......(8)；

抽样公式为：

其中是x_goal目标点的坐标，x_path,i是路径缓存中第i个点的坐标，x_free是采样空间中随机点的坐标。

4.根据权利要求1所述的一种机器人路径规划的方法，其特征在于：所述第二步中的具体内容如下：

改进蚁群算法公式所示：

信息素更新规则为:

5.根据权利要求1所述的一种机器人路径规划的方法，其特征在于：所述第三步中的具体内容如下：

S1：计算每个目标点之间的初始路径代价；

6.根据权利要求5所述的一种机器人路径规划的方法，其特征在于：所述S1中选择目标点之间的欧氏距离即直线距离作为目标点之间的初始路径代价，并将该路径代价用于蚁群优化的初始迭代。

7.根据权利要求5所述的一种机器人路径规划的方法，其特征在于：所述S3中蚂蚁的数量经过的每个部分路径相对于阈值N，如果蚂蚁经过的路径超过阈值，并且它的路径成本不变，则调用RRT*计算部分，计算路径代价，并将计算出的路径代价作为这段路径的修正路径代价。

8.根据权利要求5所述的一种机器人路径规划的方法，其特征在于：所述S3中阈值设置规则N:N＝m*iter*2/n，其中m为蚂蚁数量，iter为总迭代次数，n为目标点个数。

9.根据权利要求5所述的一种机器人路径规划的方法，其特征在于：所述S4中在一次迭代结束时，如果这次迭代的最优解路径的总代价低于前几次迭代的最优解，则在本次迭代最优解中包含每个元素。对于一条路径，如果它的路径代价没有被纠正，则调用RRT*计算该路径，并将被计算路径的路径代价作为该路径的修正路径的代价。