CN109871021B

CN109871021B - 一种基于粒子群优化算法的机器人导航方法

Info

Publication number: CN109871021B
Application number: CN201910204602.1A
Authority: CN
Inventors: 王瑞; 郭星
Original assignee: Anhui University
Current assignee: Anhui University
Priority date: 2019-03-18
Filing date: 2019-03-18
Publication date: 2022-04-15
Anticipated expiration: 2039-03-18
Also published as: CN109871021A

Abstract

本发明提出的一种基于粒子群优化算法的机器人导航方法，包括：建立二维坐标网格模型，在二维坐标网格模型上绘制出发点到目的地点的连线作为导向直线，并在导向直线上设置n个节点用于将导向直线分割为n+1段；根据网格点平移各被障碍物覆盖的节点，然后根据粒子群优化算法获取出发点与相邻的节点之间的优化路径、在横坐标上相邻的两个节点之间的优化路径以及目的地与最接近的节点之间的优化路径；串联所有优化路径获取出发点与目的地点之间的导航路径。本发明有利于提高计算速度和精确度，并保证最终获取的导航路径在导向直线附件浮动，从而有利于缩短导航路径，优化机器人行走路线，提高工作效率。

Description

一种基于粒子群优化算法的机器人导航方法

技术领域

本发明涉及路线导航技术领域，尤其涉及一种基于粒子群优化算法的机器人导航方法。

背景技术

运动路径的规划是机械人工智能的一个重要组成部分，它可以给予机器人智能运动并躲避障碍的能力。一开始我们给定机器人在障碍物众多的2D环境中的开始点位置和终点位置，路径导航的基本目标是在避免与障碍物接触的同时，为机器人生成朝向目标点的最短路径。根据可用信息量的大小，路径导航可分成离线路径导航和在线路径导航。在离线路径导航中，机器人具有关于环境的先验信息，而在线路径导航中，机器人没有关于环境的先验信息。一般路径导航问题被认为是NP-难的(非确定性多项式)，因为解决这类问题所需的计算时间随着问题的大小或维度的增加以指数速率增加，当问题中的条件较为复杂时，就很难在有限时间里得出答案。

自从N.J.尼尔森在1960年后期[3,4]提出的开创性工作以来，人们对运动规划算法进行了大量的研究，如能见度图、Voronoi图、概率路线图、快速探索随机树、势场和许多其他算法，这些经典运动规划方法存在计算量大、时间长等缺点。

相对于经典运动规划方法计算量大、时间长等缺点，启发式算法在求解路径导航问题时具有易于实现等优点，因此引起了研究者们的广泛关注。粒子群算法是一种非常简单，但却是非常强大的启发式优化技术，在许多复杂的优化问题中被证明是非常有效的。与遗传算法和其他启发式技术相比，粒子群优化算法具有许多优点，如鲁棒性高、快速收敛、需要修改的参数少。且粒子群优化算法是一种简单同时也是非常强大的智能算法，已被证明非常适合应用于复杂程度比较高的多维优化难题。

粒子群优化算法中，粒子群是通过给粒子群中的每个粒子一个任意的坐标和速度来进行初始化。粒子群里的每个成员都需要被随机搁置在问题解的全集里面。用适应函数来评价每个粒子的适应值，每个粒子的适应度最大值存储在Pbest中，整个群中的全局最大值存储在Gbest中。在下一次迭代中，使用(1)和(2)将这些粒子移动到新的位置，并且给予每个粒子一个新的速度。粒子通过相互交流单个粒子全局最佳位置和整个粒子群的全局最佳位置逐渐达到全局最佳位置。以上的过程重复进行，直到所有粒子收敛到同一点或者重复的次数到达了之前设定的一个阈值。

PSO(粒子群优化算法)主要运用以下两个公式(1)、(2)来更新每个粒子的V(速度)和X(位置)：

(1) V_i＝wV_i-1+c₁r₁(P_best-X_i-1)+c₂r₂(G_best-X_i-1)

(2) X_i＝X_i-1+V_i

其中，c1和c2分别表示个体学习率和群体学习率，并且它们的值通常被设定为2，r1和r2是在[0-1]范围内均匀分布的随机数。参数c1和c2表示粒子自身最佳位置与相邻粒子最佳位置的相对重要性，W是惯性权重因子，用于提高搜索稳定性。为了让粒子能够更精确、有效地收敛，可以通过减小W的大小来达到目标。为了加快该群的全局最优解的探索速度，可以使用较大的W值，如果是为了加快局部最优解寻找速度，优选使用较小的W值。全局最优解的探索与局部最优解的探索之间的平衡可以通过常用的线性降低惯性权重策略来实现，如公式(3)所示。

其中Wmax和Wmin分别是惯性权重的初始值和最终值，itmax是迭代的最大次数。通常使用Wmax＝0.9和Wmin＝0.4的值。用均匀分布函数[Xmin,Xmax]和[Vmin,Vmax]对粒子群中每个粒子的位置和速度进行随机初始化，如公式(4)、(5)所示。

(4) X_i＝X_min+δ₁(X_max-X_min)

(5) v_i＝v_min+δ₁(v_max-v_min)

其中WMAX和Wmin表示从0到1的随机数。PSO算法可以使用以下步骤来实现：

通过粒子群算法获取优化路径在现有技术中已经相当成熟，具体步骤如下：

1.输入初始化粒子群规模N和最大重复次数it_max

2.对于N个粒子进行初始化，第i个粒子随机获得位置Xi和速度Vi

3.通过适应度函数f(x)计算每个粒子的适应度,然后根据式(1)和式(2)跟新每个粒子的速度和位置，跟新pbest和gbest。

4.将步骤3重复it_max次后退出循环并得到gbest。

为了完成不同的任务，机器人需要在我们周围进行移动。这种需要使得机器人运动导航成为机器人学的一个非常重要的部分。一个没有办法在环境中安全移动的机器人，不仅无法完成任务，甚至会对人类的生命和财产造成比较大的伤害。

假设一个刚性机器人A在2D欧几里得工作空间F中移动。让工作空间F填充刚性障碍物O，其中O表示位于障碍物内部的F中的所有点的集合。A允许在工作空间F中移动，而障碍物集合O保持固定。给定A在F中的起点坐标和终点坐标，路径导航的目标是在A的起点坐标和终点坐标之间生成最短的路径，同时避免与O的任何接触。实际运动环境中，障碍物的分布和形状复杂多样，现有的粒子群算法通过穷举搜索获取最终路径，计算量大，复杂度高，不利于推广。

发明内容

基于背景技术存在的技术问题，本发明提出了一种基于粒子群优化算法的机器人导航方法。

本发明提出的一种基于粒子群优化算法的机器人导航方法，包括以下步骤：

S1、获取机器人工作环境的二维地图，二维地图包括：出发点、目的地点和障碍物，并基于二维地图建立二维坐标网格模型；

S2、在二维坐标网格模型上绘制出发点到目的地点的连线作为导向直线，并在导向直线上设置n个节点用于将导向直线分割为n+1段；

S3、根据网格点平移各被障碍物覆盖的节点，然后根据粒子群优化算法获取出发点与相邻的节点之间的优化路径、在横坐标上相邻的两个节点之间的优化路径以及目的地与最接近的节点之间的优化路径；

S4、串联所有优化路径获取出发点与目的地点之间的导航路径。

优选的，步骤S3中获取的每一条优化路径均在横坐标方向或者纵坐标方向上缩短机器人与目的地之间的距离。

优选的，步骤S3具体包括以下步骤：

S31、将出发点、n个节点和目的地点按照在导向直线上的顺序排列；

S32、将出发点作为起始点，并将与起始点相邻的节点作为目标点；

S33、判断目标点是否被障碍物覆盖；否，则将目标点作为有效目标点；

S34、是，则获取与目标点最接近的位于障碍物外的网格点作为有效目标点；

S35、根据粒子群算法获取起始点与有效目标点之间的优化路径；

S36、将起始点更新为有效目标点，然后判断各节点是否遍历结束；

S37、否，则将目标点更新为与起始点相邻的节点，然后返回步骤S33；

S38、如果步骤S36中，各节点遍历结束，则获取起始点与目的点之间的优化路径，然后执行步骤S4。

优选的，步骤S34中获取的有效目标点的横坐标值位于出发点横坐标值与目的地点横坐标值之间或者纵坐标值位于出发点纵坐标值与目的地点纵坐标值之间；

步骤S36具体为：将起始点更新为有效目标点，并判断是否存在横坐标值位于起始点横坐标值与目的地点横坐标值之间或者纵坐标值位于起始点点纵坐标值与目的地点纵坐标值之间的节点；

步骤S37具体为：存在，则将目标点更新为横坐标值位于起始点横坐标值与目的地点横坐标值之间或者纵坐标值位于出发点纵坐标值与目的地点纵坐标值之间的节点，然后返回步骤S33；

步骤S38具体为：不存在，则获取起始点与目的点之间的优化路径，然后执行步骤S4。

优选的，步骤S37中，将有效目标点更新为距离起始点最近且横坐标值位于起始点横坐标值与目的地点横坐标值之间或者纵坐标值位于出发点纵坐标值与目的地点纵坐标值之间的节点。

优选的，步骤S34中，有效目标点根据最小化的目标函数获取，最小化目标函数为：

其中，sp表示出发点，sp-wp₁表示出发点与相邻的第一个路径点之间的距离，wp_i表示第i个路径点，wp_i-1-wp_i表示两个相邻的路径点之间的距离；路径点为导航路径上各优化路径的端点，m为路径点的数量，1≦m≦n；x_i为第i个路径点的横坐标，y_i为第i个路径点的纵坐标。

优选的，步骤S1中，二维坐标网络模型中，各障碍物的映射模型由障碍物的等比例模型与围绕在所述等比例模型外周的防护区域组成，防护区域的宽度等于机器人半径根据二维坐标网络模型等比例缩放的值。

优选的，步骤S3中，结合机器人半径根据粒子群优化算法获取没相邻两个节点之间的优化路径。

优选的，步骤S2中，n个节点将导向直线均分为n+1份。

优选的，n的取值与出发点到目的地点之间的距离以及粒子群部落的大小成正比。

本发明提出的一种基于粒子群优化算法的机器人导航方法，通过对机器人出发点和目的地点之间的导向直线分段，将粒子群优化算法的计算对象由出发点到目的地点分割为多个小段，从而降低了粒子群算法的计算量，有利于提高计算速度和精确度。而且，本发明中，基于导向直线进行节点即路径点的选择和平移，为导航路径的获取定下了基准方向，有利于保证最终获取的导航路径在导向直线附件浮动，从而有利于缩短导航路径，优化机器人行走路线，提高工作效率。

本发明中，建立二维地图网格模型，通过网格点确定被障碍物覆盖的节点的移动方向，为被障碍物覆盖的节点的移动缩小了范围，有利于进一步提高工作效率。

附图说明

图1为本发明实施例1提出的一种基于粒子群优化算法的机器人导航方法流程图；

图2为实施例1中节点平移示意图；

图3为本发明实施例2提出的一种基于粒子群优化算法的机器人导航方法流程图；

图4为本发明实施例3提出的一种基于粒子群优化算法的机器人导航方法局部流程图；

图5为实施例3中节点平移示意图；

图6为实施例3中二维坐标网格模型；

图7为实施例3中导向流程分段示意图。

图8为环境1中导航路径示意图；

图9为环境2中导航路径示意图；

图10为环境3中导航路径示意图；

图11为环境4中导航路径示意图。

具体实施方式

实施例1

参照图1，本发明提出的一种基于粒子群优化算法的机器人导航方法，包括以下步骤。

S1、获取机器人工作环境的二维地图，二维地图包括：出发点、目的地点和障碍物，并基于二维地图建立二维坐标网格模型。

如图2所示为一个具体的二维坐标网络模型，其中SP和GP分别表示出发点和目的地点，图中麻点多边形表示障碍物。

S2、在二维坐标网格模型上绘制出发点到目的地点的连线作为导向直线，并在导向直线上设置n个节点用于将导向直线分割为n+1段。导向直线为图1中虚线段SP-GP所示。

本实施方式汇总，n个节点将导向直线均分为n+1份，具体实施时，也可在导向直线上随机选择n各节点。n的取值与出发点到目的地点之间的距离以及粒子群部落的大小成正比。

S3、根据网格点平移各被障碍物覆盖的节点，然后根据粒子群优化算法获取出发点与相邻的节点之间的优化路径、在横坐标上相邻的两个节点之间的优化路径以及目的地与最接近的节点之间的优化路径。

具体的，结合图2，导向直线上设有三个节点wp₁、wp₂和wp₃，其中wp₂位于障碍物内，此时可结合网格线将节点wp₂平移到与障碍物外部自由空间内距离节点wp₂最近的位置，例如图2中对节点wp₂进行平移获得节点wp₂'。此时，图2所示实施例中获得了导航路径上的节点wp₁、wp₂'和wp₃，然后通过离子群优化算法分别获取出发点SP与节点wp₁之间的优化路径1、节点wp₁与节点wp₂'之间的优化路径2、节点wp₂'与节点wp₃之间的优化路径3、节点wp₃与目的地点GP之间的优化路径4。

S4、串联所有优化路径获取出发点与目的地点之间的导航路径。本步骤中，最终获取的导航路径为图2中多段线SP-wp₁-wp₂'-wp₃-GP所示。

具体实施时，为了避免机器人旋转即避免机器人在一段路线上往复运动造成运动路线的延长，步骤S3中获取的每一条优化路径均在横坐标方向或者纵坐标方向上缩短机器人与目的地之间的距离。即，步骤S3中，在对障碍物覆盖的节点wp_i进行平移时，平移后的节点wp_i'的横坐标必须位于前一个节点wp_i-1的横坐标与后一个节点wp_i+1的横坐标之间，或者，平移后的节点wp_i'的纵坐标必须位于前一个节点wp_i-1的纵坐标与后一个节点wp_i+1的纵坐标之间。如此，以保证最终获得的优化路劲的单向性。具体的，节点wp₁的前一个节点为SP，最后一个节点wp_n的后一个节点为GP。

具体的，本实施方式中，在平移节点时，必须考虑机器人半径。具体实施时，可在建立二维坐标网格模型时，将机器人半径补偿到障碍物模型上，即步骤S1中，二维坐标网络模型中，各障碍物的映射模型由障碍物的等比例模型与围绕在所述等比例模型外周的防护区域组成，防护区域的宽度等于机器人半径根据二维坐标网络模型等比例缩放的值。本方法中，放区域的宽度与机器人半径的比值等于障碍物的映射模型与障碍物的比值。

具体实施时，还可通过网格点的选择约束补偿机器人半径，即在平移被障碍物覆盖的节点时，保证平移后的节点即被选中的网格点与相邻的障碍物的最短距离均大于机器人半径。即步骤S3中结合机器人半径获取优化路径。且，本实施方式中为了保证机器人无障碍运动避免碰撞，步骤S3中，结合机器人半径根据粒子群优化算法获取没相邻两个节点之间的优化路径。

实施例2

本实施方式中，步骤S3具体包括以下步骤：

S31、将出发点、n个节点和目的地点按照在导向直线上的顺序排列。

S32、将出发点作为起始点，并将与起始点相邻的节点作为目标点。

S33、判断目标点是否被障碍物覆盖；否，则将目标点作为有效目标点。

S34、是，则获取与目标点最接近的位于障碍物外的网格点作为有效目标点。如此，通过目标点到有效目标点的变换，将被障碍物覆盖的目标点变换为自由空间内的有效目标点，实现了导航路径上障碍物的规避。本步骤中，通过网格点平移目标点，明确了目标点的移动范围，从而有利于提高工作效率，实现路线的快速规划。

S35、根据粒子群算法获取起始点与有效目标点之间的优化路径。

S36、将起始点更新为有效目标点，然后判断各节点是否遍历结束。

S37、否，则将目标点更新为与起始点相邻的节点，然后返回步骤S33。

实施例3

本实施例3基于上述实施例2进行改进，本实施例3对比上述实施例2，具有以下区别点。

本实施例中，步骤S34中获取的有效目标点的横坐标值位于出发点横坐标值与目的地点横坐标值之间或者纵坐标值位于出发点纵坐标值与目的地点纵坐标值之间。

步骤S36具体为：将起始点更新为有效目标点，并判断是否存在横坐标值位于起始点横坐标值与目的地点横坐标值之间或者纵坐标值位于起始点点纵坐标值与目的地点纵坐标值之间的节点。

步骤S37具体为：存在，则将目标点更新为横坐标值位于起始点横坐标值与目的地点横坐标值之间或者纵坐标值位于出发点纵坐标值与目的地点纵坐标值之间的节点，然后返回步骤S33。

具体的，结合图2，本实施例在具体实施时，步骤如下：

步骤S1中，建立如图6所示的二维坐标网格模型。本实施例中，图6中所示的障碍物即各麻点多边形为实体障碍物的映射模型，故而，在后续获取优化路径时需要考虑机器人半径。

步骤S2中，建立的导向直线和节点的设置如图7所示，本实施例中，导向直线山随机选择四个节点wp₁、wp₂、wp₃、wp₄。

步骤S32中，首先设置起始点作为出发点SP，获取节点wp₁作为目标点。

SP点为起始点，节点wp₁为目标点时，执行步骤S33-S35，由于节点wp₁被障碍物覆盖，故而，将节点wp₁平移到网格点wp₁'的位置，获取有效目标点wp₁'，然后获取SP点与wp₁'之间的优化路径。wp₁'与障碍物的最短距离大于机器人半径。

然后，将wp₁'作为起始点，并通过步骤S36-S37，获得wp₁'对应的目标点wp₂。

wp₁'作为起始点，wp₂作为目标点时，由于wp₂位于自由空间内，直接以wp₂为有效目标点，并获取wp₁'与wp₂之间的优化路径。然后，将wp₂作为起始点，并通过步骤S36-S37，获得wp₂对应的目标点wp₃。

wp₂点为起始点，wp₃为目标点时，执行步骤S33-S35，由于节点wp₃被障碍物覆盖，故而，将节点wp₃平移到网格点wp₃'的位置，获取有效目标点wp₃'，然后获取wp₂点与wp₃'之间的优化路径。wp₃'与障碍物的最短距离大于机器人半径。

然后，将wp₃'作为起始点，并通过步骤S36-S37，获得wp₃'对应的目标点wp₄。

wp₃'作为起始点，wp₄作为目标点时，由于wp₄位于自由空间内，直接以wp₄为有效目标点，并获取wp₃'与wp₄之间的优化路径。

最后执行执行步骤S36-S38，获得wp₄与目的地点GP之间的优化路径。

然后，串联优化路径SP-wp₁'、wp₁'-wp₂、wp₂-wp₃'、wp₃'-wp₄、wp₄-GP组成导航路径。

具体的，本实施例的步骤S34中，有效目标点可根据最小化的目标函数获取，最小化目标函数为：

其中，sp表示出发点，sp-wp₁表示出发点与相邻的第一个路径点之间的距离，wp_i表示第i个路径点，wp_i-1-wp_i表示两个相邻的路径点之间的距离。路径点为导航路径上各优化路径的端点，m为路径点的数量，1≦m≦n。x_i为第i个路径点的横坐标，y_i为第i个路径点的纵坐标。

实施例4

本实施例与实施例1相比，提供了步骤S2中节点的设置模型，模型具体为：

其中，1≦i≦n，V_i表示第i个节点的速度，X_i表示第i个节点的横坐标值，c1和c2分别表示个体学习率和群体学习率，r1和r2为[0-1]范围内均匀分布的随机数，Pbest为适应度最大值，Gbest整个粒子群中的全局最大值。

以下提供多个仿真环境对本发明提供的导航方法的优化结果进行验证。

本实施例中，参数设置为：群大小N＝500，最大迭代次数Itmax＝100，最大惯性权重Wmax＝0.9，最小惯性权重Wmin＝0.4，最大速度Vmax＝200，最小速度Vmin＝0，社会学习因子C1＝2，认知学习因子C2＝2，路径点数n＝100。

本实施例中，由于机器人本身在现实世界中具有一些尺寸，因此将障碍物的尺寸增加一个固定的值(机器人本身的半径)，以保持机器人与障碍物之间的安全距离就可以保证机器人与环境之间没有碰撞。

环境1中在不同位置设置了九个凸多边形障碍物，这九个凸多边形形状各异。机器人的出发点和目的地点分别取为(0，0)和(3.5，9)。图8所示的多段线L1表示由该导航方法生成的导向路径，而黑色阴影多边形表示障碍。

环境2中在不同位置设置了七个凸多边形障碍物，机器人的出发点和目的地点分别取为(0，0)和(7.8，9.2)。通过本发明找到出发点和目的地点之间的无碰撞路径L2如图9所示。

环境3分布着八个凸多边形障碍物。机器人的出发点和目的地点分别取(0，0)和(10，6.5)，最终获得的导航路径如图10中所示的多段线L3所示。

环境4中，机器人的起点和目的地点分别取为(-3，11)和(8，-2)。环境障碍物分布非常密集，有十六个圆形障碍物排成4×4的方阵。该环境4中生成的导航路径如图11中多段线L4所示。

以上所述，仅为本发明涉及的较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，根据本发明的技术方案及其发明构思加以等同替换或改变，都应涵盖在本发明的保护范围之内。

在多种含有不同数量和不同布局的凸多边形障碍物的环境中，得到了非常不错的实验结果，证明了该改进算法的效果。

Claims

1.一种基于粒子群优化算法的机器人导航方法，其特征在于，包括以下步骤：

S4、串联所有优化路径获取出发点与目的地点之间的导航路径；

所述的步骤S3具体包括以下步骤：

S38、如果步骤S36中，各节点遍历结束，则获取起始点与目的点之间的优化路径，然后执行步骤S4；

所述步骤S34中获取的有效目标点的横坐标值位于出发点横坐标值与目的地点横坐标值之间或者纵坐标值位于出发点纵坐标值与目的地点纵坐标值之间；

步骤S36具体为：将起始点更新为有效目标点，并判断是否存在横坐标值位于起始点横坐标值与目的地点横坐标值之间或者纵坐标值位于起始点纵坐标值与目的地点纵坐标值之间的节点；

步骤S38具体为：不存在，则获取起始点与目的点之间的优化路径，然后执行步骤S4；

所述步骤S37中，将有效目标点更新为距离起始点最近且横坐标值位于起始点横坐标值与目的地点横坐标值之间或者纵坐标值位于出发点纵坐标值与目的地点纵坐标值之间的节点；

所述的步骤S34中，有效目标点根据最小化的目标函数获取，最小化目标函数为：

，其中，sp表示出发点，表示出发点与相邻的第一个路径点之间的距离，表示第i个路径点，表示两个相邻的路径点之间的距离；路径点为导航路径上各优化路径的端点，m为路径点的数量，1≦m≦n；为第i个路径点的横坐标，为第i个路径点的纵坐标。

2.如权利要求1所述的基于粒子群优化算法的机器人导航方法，其特征在于，步骤S3中获取的每一条优化路径均在横坐标方向或者纵坐标方向上缩短机器人与目的地之间的距离。

3.如权利要求1所述的基于粒子群优化算法的机器人导航方法，其特征在于，步骤S1中，二维坐标网络模型中，各障碍物的映射模型由障碍物的等比例模型与围绕在所述等比例模型外周的防护区域组成，防护区域的宽度等于机器人半径根据二维坐标网络模型等比例缩放的值。

4.如权利要求1所述的基于粒子群优化算法的机器人导航方法，其特征在于，步骤S3中，结合机器人半径根据粒子群优化算法获取每相邻两个节点之间的优化路径。

5.如权利要求1所述的基于粒子群优化算法的机器人导航方法，其特征在于，步骤S2中，n个节点将导向直线均分为n+1份。

6.如权利要求1或5所述的基于粒子群优化算法的机器人导航方法，其特征在于，n的取值与出发点到目的地点之间的距离以及粒子群部落的大小成正比。