一种基于有向质点蚁群的水下机器人路径规划方法及设备

技术领域

本发明涉及路径规划技术领域，更具体的说是涉及一种基于有向质点蚁群的水下机器人路径规划方法及设备。

背景技术

蚁群算法作为路径规划时的一种智能规划方法，不仅非常适用于地面机器人，对水下机器人路径规划同样具有参考价值，其适用范围广，理解相对简单，因此得到了广泛的应用。

但是在一般情况下，蚁群算法不能直接对真实地图进行规划，而是需要先将地图信息栅格化，再运用栅格地图进行操作。在这个过程中，通常将载体看作为栅格地图中一个无方向的质点，也即认为载体在任意当前时刻都可以前往任意无障碍物的方向，不存在转弯代价。对于一般陆地机器人来说，原地调转前进方向是一项普遍具有的基本功能，其所付出能耗代价微乎其微，可忽略不计。而在水下机器人领域，因受水流阻力与水流波动等影响，其存在一定的转弯代价。在依赖螺旋桨推进的传统水下机器人领域，载体可以通过直接控制螺旋桨电机正反转的方式实现调转方向或是回退的功能，虽然其转弯过程较为迅速，但即便如此，其仍然需要付出一定的转弯代价。而对于一些新兴水下机器人，尤其是仿生类水下机器人，以仿生机器海豚为例，因受自身运动方式的影响，其无法像陆地机器人与传统水下机器人一样轻松完成较大偏转以及后退动作，这也意味着载体将具有不可忽视的转弯代价。而传统的蚁群算法路径规划并未考虑到这点，其规划出的路径甚至可能在并非最优的前提下进一步加大载体的能量消耗。此外，水下机器人在行进过程中往往面对阻力，这也是传统蚁群算法未考虑到的方面。以仿生机器海豚为例，其在水下进行运动所面对的转弯代价与水阻力不可忽视，同时在实际工程项目当中，海洋中的环境错综复杂，仿生机器海豚执行任务时往往具有较高能耗指标，过度的能耗将会大大降低仿生机器海豚的效率，甚至导致载体过早耗尽能量而沉底的严重后果。

传统的蚁群算法思想来源于模拟蚂蚁觅食的行为。在自然界中，蚂蚁通过一种名为“信息素”的物质实现蚁群之间的通信，并将信息素遗留到其所经过的路径上。个体蚂蚁到达终点时所经过的路径越短，其所留下的信息素浓度就越高。通过多次迭代搜索，较短路径上的信息素浓度便会不断累加，从而提高下一只蚂蚁选择该条路径的概率，最终帮助找到一条高质量路径。在现有技术中的蚁群算法中，例如基于精英蚂蚁的蚁群算法，基于最大-最小蚂蚁的蚁群算法等，从而实现了不同的路径规划效果。但上述工作都并未对质点蚂蚁的方向性进行改进，也即都将蚂蚁看错一个无方向的质点，其前往任何方向都不具有转弯代价，并且也都没有针对水下机器人运动时所产生的水阻力进行优化改进。

因此，如何以合理、高效、安全的方式进行水下机器人路径规划是本领域技术人员亟需解决的问题。

发明内容

有鉴于此，本发明提供了一种基于有向质点蚁群的水下机器人路径规划方法及设备，用以解决上述现有技术中存在的技术问题。

为了实现上述目的，本发明提供如下技术方案：

一种基于有向质点蚁群的水下机器人路径规划方法，包括：

S100：路径规划总过程开始时，构建水下环境模型，并确定起点与终点；每一轮迭代开始时将所有蚂蚁置于所述起点位置；每只蚂蚁开始行动时，将蚂蚁构建为有向质点，获取蚂蚁当前方向与当前可访问节点，计算得到所有可访问节点的转弯代价的集合；

S200：根据所述转弯代价改进轮盘赌算法实现蚂蚁寻路；

S300：判断蚂蚁是否抵达预设终点，若否则返回S100，更新有向质点方向，重新计算所有可访问节点的转弯代价；若是，则：

S400：计算当前寻路路径的总水阻力代价，并将当前寻路路径的总水阻力代价作为水阻力信息素与路径信息素同步更新；

S500：判断是否完成所有迭代轮次，若否则返回S100；若是则输出所有迭代轮次中路径代价最小的那只蚂蚁所经过的路径作为最终的规划路径并输出。

优选的，所述S100：将蚂蚁构建为有向质点，获取蚂蚁当前方向与当前可访问节点计算所有可访问节点的转弯代价，包括以下步骤：

S110：定义蚂蚁当前方向信息，确定当前蚂蚁处于栅格图中所有可能的方向；

S120：确定载体的极限转弯能力；

S130：根据当前蚂蚁处于栅格图中所有可能的方向以及载体的极限转弯能力，定义转弯代价函数；

S140：获取蚂蚁当前位置信息与所有可访问节点信息；

S150：基于蚂蚁当前位置信息与所有可访问节点信息，通过转弯代价函数计算转弯代价；

S160：输出所有可访问节点转弯代价的集合。

优选的，所述S200：根据所述转弯代价改进轮盘赌蚂蚁寻路，包括以下步骤：

S210：记录栅格图中所有可访问节点到终点的距离的倒数，构成启发式信息四维矩阵，记录栅格图中所有节点之间的信息素，构成信息素多维矩阵；

S220：基于所有可访问节点转弯代价的集合、启发式信息四维矩阵以及信息素多维矩阵计算蚂蚁到每个可访问节点的转移概率，并得到所有可访问节点的转移概率数组集合；

S230：基于所有可访问节点的转移概率数组集合，通过轮盘赌机制选择前进节点；

S240：更新蚂蚁位置与可访问节点信息；

S250：输出更新后的蚂蚁位置信息与可访问节点信息。

优选的，所述S220：基于所有可访问节点转弯代价的集合、启发式信息四维矩阵以及信息素多维矩阵计算蚂蚁到每个可访问节点的转移概率，并得到所有可访问节点的转移概率数组集合，公式为：

式中，

遍历所有可访问节点，得到所有可访问节点的转移概率数组集合

优选的，所述S220：基于所有可访问节点转弯代价的集合、启发式信息四维矩阵以及信息素多维矩阵计算蚂蚁到每个可访问节点的转移概率，并得到所有可访问节点的转移概率数组集合后，还包括：

在所有可访问节点的转移概率数组集合

其中，若

优选的，所述S240：更新蚂蚁位置与可访问节点信息，则

；

式中，

优选的，所述S400：计算当前寻路路径的总水阻力代价，并将当前寻路路径的总水阻力代价作为水阻力信息素与路径信息素同步更新，包括以下步骤：

S410：获取载体速度信息与运动路径信息；

S420：基于所述载体速度信息与运动路径信息，计算蚂蚁在当前寻路路径上所留下的与速度相关的水阻力信息素以及与路程相关的水阻力信息素；

S430：判断是否所有蚂蚁完成当前轮次迭代任务，若否，则执行S440，若是，则执行S450；

S440：选择下一只蚂蚁，返回S100，继续迭代任务；

S450：更新节点上的信息素浓度。

优选的，所述S420：基于所述载体速度信息与运动路径信息，计算蚂蚁在当前寻路路径上所留下的与速度相关的水阻力信息素以及与路程相关的水阻力信息素，包括：

第

；

信息素增量中与速度相关的水阻力信息素表达式为：

；

信息素增量中与路程相关的水阻力信息素表达式为：

；

式中，

优选的，所述S450：更新节点上的信息素浓度，包括：

当所有蚂蚁都完成了一轮迭代任务，更新节点

；

式中，

一种电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现基于有向质点蚁群的水下机器人路径规划方法的步骤。

经由上述的技术方案可知，与现有技术相比，本发明公开提供了一种基于有向质点蚁群的水下机器人路径规划方法及设备，将蚂蚁构建为有向质点，实现了蚁群算法中的实时转弯代价判断，并引入了水阻力信息素进行迭代更新，将仿生机器海豚的能耗特点和运动特点与蚁群算法相结合，使蚁群算法在路径规划的过程中同时兼顾路程短、能耗少，适用于真实工程应用的三项指标，实现了以合理、高效，安全的方式进行水下机器人路径规划。具体有益效果为：

1）克服了现有技术方案中只能使用传统基于无向质点蚂蚁的蚁群算法进行路径规划的技术难题，本发明可以使整个路径规划过程更加智能，极大增强蚁群算法在水下机器人应用中的适应性，使其能够根据转弯代价与水阻因素优化路径。

2）本发明对仿生机器海豚的路径规划具有优越性，考虑到了仿生机器海豚的转弯能耗与运动特点，并通过将仿生机器海豚尾鳍摆动频率与幅度作为一种衡量载体速度指标，最终可以使仿生机器海豚在路径规划的过程中有效避免未来可能会出现的不必要能量损耗，使仿生机器海豚的跟踪路径更加高效节能。

3）具有通用性，不仅适用于仿生机器海豚，同样适用于一般水下机器人与陆地特种机器人，如泥地勘探机器人，其运动时同样存在转弯代价，前进时同样受到阻力。

4）通过基于有向质点蚁群算法的水下机器人路径规划，将蚂蚁看做栅格图中的有向质点，根据蚂蚁当前的方向与可访问节点信息输出转移可能性，然后利用改进轮盘赌的方法选择蚂蚁的下一前进节点，当蚂蚁完成搜索任务时，根据水阻力影响对信息素进行更新迭代。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据提供的附图获得其他的附图。

图1为本发明的方法总流程示意图；

图2为有向质点方向更新与转弯代价计算流程示意图；

图3为有向质点方向更新与转弯代价计算中所使用的栅格化后的地图；

图4为栅格化后的地图中蚂蚁所有可能方向示意图；

图5为改进轮盘赌蚂蚁寻路流程图；

图6为水阻力代价计算与信息素更新流程示意图。

具体实施方式

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

本发明的目的是提供一种基于有向质点蚁群的水下机器人路径规划方法及设备，该方法包括：S100：路径规划总过程开始时，构建水下环境模型，并确定起点与终点；每一轮迭代开始时将所有蚂蚁置于所述起点位置；每只蚂蚁开始行动时，将蚂蚁构建为有向质点，获取蚂蚁当前方向与当前可访问节点，计算得到所有可访问节点的转弯代价的集合；S200：根据所述转弯代价改进轮盘赌算法实现蚂蚁寻路；S300：判断蚂蚁是否抵达预设终点，若否则返回S100，更新有向质点方向，重新计算所有可访问节点的转弯代价；若是，则：S400：计算当前寻路路径的总水阻力代价，并将当前寻路路径的总水阻力代价作为水阻力信息素与路径信息素同步更新；S500：判断是否完成所有迭代轮次，若否则返回S100；若是则输出所有迭代轮次中路径代价最小的那只蚂蚁所经过的路径作为最终的规划路径并输出。为实现合理、高效，安全的方式进行水下机器人路径规划提供解决方案。

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。

如图1所示，本发明实施例公开了一种基于有向质点蚁群的水下机器人路径规划方法，主要步骤为：使用蚂蚁当前方向与当前可访问节点计算所有可访问节点的转弯代价，从而将蚂蚁视作有向质点。根据已经计算出的转弯代价进行改进轮盘赌寻路。判断蚂蚁是否抵达预设终点，若否则返回，更新有向质点方向，重新计算所有可访问节点的转弯代价；若最终蚂蚁抵达了终点，则计算此次寻路路径的总水阻力代价，将其作为水阻力信息素与路径信息素一起同步更新；判断是否完成所有迭代轮次，若否则返回重新计算所有可访问节点的转弯代价；若是则输出所有迭代轮次中路径代价最小的那只蚂蚁所经过的路径作为最终的规划路径。

具体的，本实施例假设栅格化后的地图为N阶的0/1矩阵，其中0代表可通行位置，1代表障碍物位置。

具体的，S100：路径规划总过程开始时，构建水下环境模型，并确定起点与终点；每一轮迭代开始时将所有蚂蚁置于所述起点位置；每只蚂蚁开始行动时，将蚂蚁构建为有向质点，获取蚂蚁当前方向与当前可访问节点计算得到所有可访问节点的转弯代价的集合。

在一个具体实施例中，为了将蚂蚁构建为有向质点，并获取蚂蚁每个可访问节点的转弯代价，需要确定每只蚂蚁可能存在的方向情况，并确定载体在水下运动时的真实转弯能力。其中，每只蚂蚁可能存在的方向情况与栅格图的定义有关，并且通常为

在一个具体实施例中，如图2所示，有向质点方向更新与转弯代价计算的步骤如下：

S110：定义蚂蚁当前方向信息，确定蚂蚁所有可能方向情况。

具体的，在基于栅格图的蚁群算法路径规划中，过于精细的方向划分将使蚂蚁无法准确对准栅格，会大幅度增加算法复杂度。同时在真实工程环境中，水下机器人载体通常受限于自身结构，也无法将偏转流程执行得过于精确，太过精细的路径划分也不具有实际工程意义。将蚂蚁基于惯性坐标系下的偏航角为0情况定义为蚂蚁的右方向，如图3所示，此时假设蚂蚁处在5号位置。将蚂蚁当前方向信息定义为

；

式中，

S120：确定载体极限转弯能力。

具体的，本实施例以仿生机器海豚为例，仿生机器海豚依靠尾鳍摆动作为动力前行，也即载体不存在原地转弯能力，故蚂蚁前往任意斜侧方都存在转弯代价，并随偏转角度的增大而增大。此外，仿生机器海豚不存在后退功能，故蚂蚁后退的转弯代价最大；

S130：根据当前蚂蚁处于栅格图中所有可能的方向以及载体的极限转弯能力，定义转弯代价函数。

具体的，根据S110-S120所确定的公式特征与预先设置好的栅格地图特征，定义转弯代价函数。

具体的，一般水下机器人存在单位时间最大偏转角度

S140：获取蚂蚁当前位置信息与所有可访问节点信息。

具体的，起始状态，输入蚂蚁当前位置信息与所有可访问节点信息。其中，当前位置信息包括蚂蚁在大地坐标系下的坐标

。

S150：转弯代价计算。

具体的，根据蚂蚁当前坐标

；

则第

；

式中，

具体的，

重复上述过程，直至遍历完当前所有可访问节点，并得到所有可访问节点转弯代价集合

S160：输出所有可访问节点转弯代价集合

在一个具体实施例中，通过将蚂蚁设置为有向质点的方式将转弯代价引入路径规划的流程中。转弯代价具有显著优势，蚂蚁将有意识地减少转弯运动，更倾向于选择那些转弯代价较小的路径进行搜索，从而减少能量消耗。每只蚂蚁进行搜索的路径都可能不同，故不同方向信息的蚂蚁针对同一搜索过的栅格的转弯代价亦不相同，故转弯代价不能直接用于信息素的更新，而是需要融入蚂蚁每一步轮盘赌寻路时的过程，对轮盘赌过程进行改进优化。

在一个具体实施例中，如图5所示，轮盘赌蚂蚁寻路的步骤如下：

S210：记录栅格图中所有可访问节点到终点的距离的倒数，构成启发式信息四维矩阵，记录栅格图中所有节点之间的信息素，构成信息素多维矩阵；

具体的，起始状态，输入所有可访问节点转弯代价集合

S220：计算蚂蚁到每个可访问节点的转移可能。

具体的，其中，

；

其中，

S230：轮盘赌选择前进节点。

具体的，根据S220可得，蚂蚁转移到第

；

遍历所有可访问节点，得到所有可访问节点的转移概率数组集合

取随机数

；

若

S240：更新蚂蚁位置与可访问节点信息，则

S250：输出更新后的蚂蚁位置信息与可访问节点信息；

在一个具体实施例中，考虑水阻力的主要目的是衡量载体的能量消耗，而并非关注水阻力的具体数值，但仍要对影响水阻力大小的关键参数进行理解。

水阻力计算公式为：

；

其中，

在一般工程情况下，当载体的形状固定时，影响水阻力的关键因素为

如图6所示，水阻力代价计算与信息素更新的步骤如下：

S410：获取载体速度信息与运动路径信息。

具体的，起始状态，输入载体速度信息与运动路径信息。需要注意的是，仿生机器海豚与陆地机器人和传统水下机器人不同，其主要通过尾鳍的摆动来实现前行，故仿生机器海豚无法做到匀速运动，而是以一种震荡摆动式的速度类匀速前行，此时载体的速度信息并不是载体的线速度

因此，本发明实施例提出一种将仿生机器海豚尾鳍摆动的频率

S420：信息素遗留。

具体的，第

；

总信息素增量中与水下机器人速度相关的水阻力信息素增量：

；

总信息素增量中与蚂蚁所走路程长度相关的水阻力信息增素：

；

式中，

具体的，在本发明中，所运用到的信息素不止一种，它们一起组成了总的信息素。也就是在总体信息素中，其中存在一种与速度相关的水阻力信息素，也存在一种与路程相关，也即规划出的路径总体路程长度相关的水阻力信息素，具体表达式如上所述。

S430：判断是否所有蚂蚁完成迭代任务。

具体的，如果所有蚂蚁都已完成迭代任务，则执行S450。否则执行S440；

S440：选择下一只蚂蚁，返回S100，继续执行迭代任务；

S450：信息素更新。具体的，当所有蚂蚁都完成了一轮迭代任务，此时更新节点

；

式中，

具体的，

S500：判断是否完成所有迭代轮次，若否则返回S100；若是则输出所有迭代轮次中路径代价最小的那只蚂蚁所经过的路径作为最终的规划路径。

具体的，判断是否完成所有迭代轮次，若否则返回S100，继续线性执行。蚁群算法存在蚂蚁数量和迭代次数两种参数，假设有50只蚂蚁，迭代次数为50。那么第1次迭代开始，先选择第一只蚂蚁，开始搜索，当第1只蚂蚁结束任务后，选择第2只蚂蚁，继续进行搜索任务，以此类推，直到50只蚂蚁全部探索完毕，视为第1次迭代结束。此时根据流程图，判断是否完成50次迭代，如果为否，那么继续开始第2次迭代，如此重复，直至完成所有迭代轮次，输出所有迭代轮次中路径代价最小的那只蚂蚁所经过的路径作为最终的规划路径。

通过上述技术方案，本发明使用有向质点对蚁群算法进行改进，并融入转弯代价与水阻代价，通过蚂蚁当前的方向信息与当前可访问节点信息对转移可能性进行优化，可以有效增强蚁群算法路径规划时的智能型与适应性，并且更加符合工程场景。本发明结合了仿生机器海豚在水下运动时的能耗特点与运动特点，将仿生机器海豚的尾鳍摆动频率与幅度作为速度考量，同时将其水阻力考虑其中，给出了相应的信息素迭代函数，在基本蚁群算法基础上提供了一种将转弯代价与水阻力代价融合，并与仿生机器海豚功耗限制结合起来的水下机器人路径规划方法。

另一方面，本发明公开了一种电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，处理器执行计算机程序时实现基于有向质点蚁群的水下机器人路径规划方法的步骤。

在本文中，载体是一种通俗的叫法，可以代指水下机器人。因为本发明具有一定通用性，因此在通用性领域，也可把载体看作其他种类机器人。

本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的装置而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。

对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。