基于改进人工势场法的机器人路径规划方法

1.本发明涉及机器人路径规划方法技术领域，特别是涉及一种基于改进人工势场法的机器人路径规划方法。


背景技术：

2.机器人如工业机器人、服务机器人、特种机器人等，其研发、制造与应用是衡量一个国家科技创新和制造业水平的重要标志。机器人路径规划作为机器人的一项关键技术，其目的是按照某一性能指标，如距离、时间、能量等，为机器人搜索一条从起始位置到目标位置的最优路径。
3.人工势场法以其数学形式简单、计算量小、规划出的路径平滑等优点，被广泛应用于机器人的避障路径规划。在进行机器人的避障路径规划时，人工势场法将机器人的工作环境抽象为势力场，机器人的目标位置对机器人产生引力，障碍物对机器人产生斥力，机器人在引力和斥力的共同作用下向机器人的目标位置移动。然而，传统人工势场法存在两个难以克服的缺陷，一是机器人易陷入局部极小点位置；二是当机器人的目标位置附近存在障碍物时，机器人到达目标位置附近后无法继续靠近目标位置。上述缺陷制约了传统人工势场法在机器人的避障路径规划中的应用效果。


技术实现要素：

4.基于此，有必要针对上述技术问题，提供一种基于改进人工势场法的机器人路径规划方法。
5.为实现上述目的，本发明提供的一种基于改进人工势场法的机器人路径规划方法具体包括以下步骤：
6.步骤一：初始化人工势场法的参数，确定机器人的起始位置x0、机器人的目标位置xg、障碍物总数m和第j个障碍物的位置xj，j＝1,2,
…
,m；
7.步骤二：定义引力势场函数ua、虚拟势场函数uv和斥力势场函数ur；
8.步骤三：设置机器人的虚拟目标位置xv为机器人的当前位置x；
9.步骤四：分别根据引力势场函数ua、虚拟势场函数uv和斥力势场函数ur计算机器人所受的引力虚拟引力和斥力进而计算机器人所受的合力
10.步骤五：采用粒子群算法优化机器人的移动步长x，根据最优移动步长x
*
确定机器人的下一位置xn；
11.步骤六：判断获得的下一位置xn是否能使机器人到达目标位置xg，若是，则机器人移动至下一位置xn，结束路径规划；否则，执行步骤七；
12.步骤七：判断机器人是否陷入局部极小点位置，若是，则机器人不移动，舍弃获得的下一位置xn，更新机器人的虚拟目标位置xv，返回步骤四；否则，机器人移动至下一位置xn，返回步骤三。
13.进一步地，所述步骤一需要初始化的人工势场法的参数包括：引力系数k
a1
与k
a2
、
虚拟引力系数kv、斥力系数kr、引力作用阈值da、障碍物影响距离dr、虚拟目标位置的偏置距离dv、目标位置判定参数l、机器人的最小移动步长x
min
及最大移动步长x
max
。
14.进一步地，所述步骤二定义引力势场函数ua为：
[0015][0016]
其中，k
a1
、k
a2
为引力系数，x为机器人的当前位置，xg为机器人的目标位置，da为引力作用阈值。
[0017]
进一步地，所述步骤二定义虚拟势场函数uv为：
[0018][0019]
其中，kv为虚拟引力系数，x为机器人的当前位置，xv为机器人的虚拟目标位置。
[0020]
进一步地，所述步骤二定义斥力势场函数ur为：
[0021][0022]
其中，u
r,j
为第j个障碍物产生的斥力势场，kr为斥力系数，x为机器人的当前位置，xg为机器人的目标位置，xj为第j个障碍物的位置，dr为障碍物影响距离。
[0023]
进一步地，所述步骤四计算机器人所受的引力虚拟引力斥力及合力的具体方法为：
[0024]
机器人的目标位置xg对机器人的引力等于引力势场函数ua的负梯度，引力的幅值的表达式为：
[0025][0026]
其中，k
a1
、k
a2
为引力系数，da为引力作用阈值；引力的方向由机器人的当前位置x指向机器人的目标位置xg；
[0027]
机器人的虚拟目标位置xv对机器人的虚拟引力等于虚拟势场函数uv的负梯度，虚拟引力的幅值的表达式为：
[0028][0029]
其中，kv为虚拟引力系数；虚拟引力的方向由机器人的当前位置x指向机器人的虚拟目标位置xv；
[0030]
第j个障碍物对机器人的斥力等于第j个障碍物产生的斥力势场u
r,j
的负梯度，
斥力的幅值的表达式为：
[0031][0032]
其中，kr为斥力系数，dr为障碍物影响距离；第j个障碍物对机器人的斥力的方向由第j个障碍物的位置xj指向机器人的当前位置x；
[0033]
机器人所受的合力由机器人所受的引力虚拟引力及斥力相加得来，其表达式为：
[0034][0035]
进一步地，所述步骤五采用粒子群算法优化机器人的移动步长x的具体步骤为：
[0036]
步骤1：初始化粒子群算法的参数，定义适应度函数，设置初代粒子的位置和更新速度，建立初代粒子群，令迭代次数i＝0；
[0037]
步骤2：根据适应度函数计算初代粒子群中每个粒子的适应度，选出个体最优粒子和全局最优粒子；
[0038]
步骤3：根据个体最优粒子和全局最优粒子计算新一代粒子的更新速度；
[0039]
步骤4：根据当代粒子的位置和新一代粒子的更新速度计算新一代粒子的位置；
[0040]
步骤5：令i＝i+1，根据适应度函数计算新一代粒子群中每个粒子的适应度，选出个体最优粒子和全局最优粒子；
[0041]
步骤6：判断是否满足迭代终止条件，若是，则全局最优粒子的位置即为机器人的最优移动步长x
*
，终止迭代；否则，返回步骤3。
[0042]
进一步地，所述步骤六判断获得的下一位置xn使得机器人到达目标位置xg的条件为：x
g-xn≤l，l为目标位置判定参数。
[0043]
进一步地，所述步骤七判断机器人陷入局部极小点位置的条件为：机器人的当前位置x与之前某一位置的距离小于或等于机器人最小移动步长x
min
的若干整数倍。
[0044]
进一步地，所述步骤七当机器人陷入局部极小点位置时，选取机器人的当前位置与目标位置连线的中垂线上的一点，用该点位置来更新虚拟目标位置xv；更新后的虚拟目标位置xv通过联立下面的方程求解：
[0045][0046]
其中，dv为虚拟目标位置的偏置距离。
[0047]
本发明提供的技术方案具有以下有益效果：
[0048]
本发明通过改进传统人工势场法的引力势场函数和斥力势场函数；引入虚拟目标
位置，定义虚拟势场函数，使得机器人在引力、虚拟引力和斥力的共同作用下向目标位置移动；当机器人陷入局部极小点位置时，更新虚拟目标位置，重新求解机器人的下一位置，从而激励机器人逃离局部极小点位置。本发明克服了传统人工势场法易陷入局部极小点、目标位置不可达的缺陷，使机器人在多障碍物的复杂环境中能以最优路径到达目标位置。
附图说明
[0049]
下面将结合附图及实施例对本发明作进一步说明，附图中：
[0050]
图1为本发明实施例中一种基于改进人工势场法的机器人路径规划方法的流程图；
[0051]
图2为本发明实施例中机器人的运动任务与工作环境的示例图；
[0052]
图3为传统人工势场法下机器人陷入局部极小点位置的示例图；
[0053]
图4为传统人工势场法下机器人陷入局部极小点位置时所受合力的变化曲线图；
[0054]
图5为传统人工势场法下机器人目标位置不可达的示例图；
[0055]
图6为传统人工势场法下机器人目标位置不可达时所受合力的变化曲线图；
[0056]
图7为本发明实施例中某次优化机器人移动步长时全局最优粒子适应度随迭代次数的变化曲线图；
[0057]
图8为本发明实施例中机器人路径规划结果的示例图；
[0058]
图9为本发明实施例中机器人所受合力的变化曲线图。
具体实施方式
[0059]
为了对本发明的技术特征、目的和效果有更加清楚的理解，现对照附图详细说明本发明的具体实施方式。
[0060]
本实施例中，选取的研究对象为某一六自由度机械臂，利用本发明为该机械臂规划一条从起始位置到目标位置、无碰撞的最优路径。
[0061]
本实施例中，一种基于改进人工势场法的机器人路径规划方法，具体步骤如图1所示，包括：
[0062]
步骤一：初始化人工势场法的参数，确定机器人的起始位置x0、机器人的目标位置xg、障碍物总数m和第j个障碍物的位置xj，j＝1,2,
…
,m；
[0063]
步骤二：定义引力势场函数ua、虚拟势场函数uv和斥力势场函数ur；
[0064]
步骤三：设置机器人的虚拟目标位置xv为机器人的当前位置x；
[0065]
步骤四：分别根据引力势场函数ua、虚拟势场函数uv和斥力势场函数ur计算机器人所受的引力虚拟引力和斥力进而计算机器人所受的合力
[0066]
步骤五：采用粒子群算法优化机器人的移动步长x，根据最优移动步长x
*
确定机器人的下一位置xn；
[0067]
步骤六：判断获得的下一位置xn是否能使机器人到达目标位置xg，若是，则机器人移动至下一位置xn，结束路径规划；否则，执行步骤七；
[0068]
步骤七：判断机器人是否陷入局部极小点位置，若是，则机器人不移动，舍弃获得的下一位置xn，更新机器人的虚拟目标位置xv，返回步骤四；否则，机器人移动至下一位置xn，返回步骤三。
[0069]
本实施例中，机器人的运动任务与工作环境如图2所示，机器人的起始位置x0(0,0,0)，机器人的目标位置xg(100,100,100)，障碍物总数m＝6，障碍物的位置分别为x1(20,20,20)、x2(35,28,30)、x3(47,45,47)、x4(58,50,60)、x5(70,75,75)、x6(87,80,80)；障碍物均为球体，半径分别为r1＝3、r2＝3、r3＝3.4、r4＝3.5、r5＝2.7、r6＝5.3。
[0070]
具体地，步骤一需要初始化的人工势场法的参数包括：引力系数k
a1
与k
a2
、虚拟引力系数kv、斥力系数kr、引力作用阈值da、障碍物影响距离dr、虚拟目标位置的偏置距离dv、目标位置判定参数l、机器人的最小移动步长x
min
及最大移动步长x
max
。
[0071]
本实施例中，引力系数k
a1
＝35，k
a2
＝35，虚拟引力系数kv＝20，斥力系数kr＝20，引力作用阈值da＝315，障碍物影响距离dr＝10，虚拟目标的偏置距离dv＝5，目标位置判定参数l＝0.25，机器人的最小移动步长x
min
＝1，最大移动步长x
max
＝2。
[0072]
需要说明的是，上述参数的具体数值仅是本发明的一种优选的示例，在其他实施例中，具体数值可根据实际情况进行调整。
[0073]
具体地，步骤二定义引力势场函数ua为：
[0074][0075]
其中，k
a1
、k
a2
为引力系数，x为机器人的当前位置，xg为机器人的目标位置，da为引力作用阈值。
[0076]
具体地，步骤二定义虚拟势场函数uv为：
[0077][0078]
其中，kv为虚拟引力系数，x为机器人的当前位置，xv为机器人的虚拟目标位置。
[0079]
具体地，步骤二定义斥力势场函数ur为：
[0080][0081]
其中，u
r,j
为第j个障碍物产生的斥力势场，kr为斥力系数，x为机器人的当前位置，xg为机器人的目标位置，xj为第j个障碍物的位置，dr为障碍物影响距离。
[0082]
具体地，步骤四计算机器人所受的引力虚拟引力斥力及合力的具体方法为：
[0083]
机器人的目标位置xg对机器人的引力等于引力势场函数ua的负梯度，引力的幅值的表达式为：
[0084]
[0085]
其中，k
a1
、k
a2
为引力系数，da为引力作用阈值；引力的方向由机器人的当前位置x指向机器人的目标位置xg；
[0086]
机器人的虚拟目标位置xv对机器人的虚拟引力等于虚拟势场函数uv的负梯度，虚拟引力的幅值的表达式为：
[0087][0088]
其中，kv为虚拟引力系数；虚拟引力的方向由机器人的当前位置x指向机器人的虚拟目标位置xv；
[0089]
第j个障碍物对机器人的斥力等于第j个障碍物产生的斥力势场u
r,j
的负梯度，斥力的幅值的表达式为：
[0090][0091]
其中，kr为斥力系数，dr为障碍物影响距离；第j个障碍物对机器人的斥力的方向由第j个障碍物的位置xj指向机器人的当前位置x；
[0092]
机器人所受的合力由机器人所受的引力虚拟引力及斥力相加得来，其表达式为：
[0093][0094]
具体地，步骤五采用粒子群算法优化机器人的移动步长x的具体步骤为：
[0095]
步骤1：初始化粒子群算法的参数，定义适应度函数，设置初代粒子的位置和更新速度，建立初代粒子群，令迭代次数i＝0；
[0096]
步骤2：根据适应度函数计算初代粒子群中每个粒子的适应度，选出个体最优粒子和全局最优粒子；
[0097]
步骤3：根据个体最优粒子和全局最优粒子计算新一代粒子的更新速度；
[0098]
步骤4：根据当代粒子的位置和新一代粒子的更新速度计算新一代粒子的位置；
[0099]
步骤5：令i＝i+1，根据适应度函数计算新一代粒子群中每个粒子的适应度，选出个体最优粒子和全局最优粒子；
[0100]
步骤6：判断是否满足迭代终止条件，若是，则全局最优粒子的位置即为机器人的最优移动步长x
*
，终止迭代；否则，返回步骤3。
[0101]
本实施例中，设粒子群规模为50，最大迭代次数为25，惯性权重为0.8，自我学习因子为0.5，群体学习因子为0.5。初代粒子的位置在区间[x
min
,x
max
]随机产生，初代粒子的更新速度在区间[0.1x
min
,0.1x
max
]随机产生。
[0102]
在其他实施例中，上述具体数值也可根据实际情况调整。
[0103]
具体地，步骤六判断获得的下一位置xn使得机器人到达目标位置xg的条件为：|x
g-xn|≤l，l为目标位置判定参数，本实施例中优选l＝0.25。
[0104]
具体地，步骤七判断机器人陷入局部极小点位置的条件为：机器人的当前位置x与之前某一位置的距离小于等于机器人最小移动步长x
min
的若干整数倍。
[0105]
本实施例中，机器人的当前位置x与当前位置倒数7步所在位置的距离，小于或等于机器人最小移动步长x
min
的4倍，则机器人陷入局部极小点位置。
[0106]
具体地，步骤七当机器人陷入局部极小点位置时，选取机器人的当前位置与目标位置连线的中垂线上的一点，用该点位置来更新虚拟目标位置xv；更新后的虚拟目标位置xv通过联立下面的方程求解：
[0107][0108]
其中，dv为虚拟目标位置的偏置距离。
[0109]
传统人工势场法易使机器人陷入局部极小点位置。在图2所示的机器人的运动任务与工作环境下，利用传统人工势场法进行机器人路径规划，如图3所示，移动30步后，机器人在障碍物1附近陷入局部极小点位置，会在图中三角形标记的两点间往复移动，导致机器人无法逃离局部极小点位置继续向目标位置移动；机器人陷入局部极小点位置时所受合力的幅值变化情况如图4所示，当机器人陷入局部极小点位置后，机器人所受合力的幅值会在两个数值之间来回变化，但方向相反。
[0110]
传统人工势场法的另一缺陷是：当机器人的目标位置附近存在障碍物时，机器人到达目标位置附近后无法继续靠近目标位置。在图2所示的机器人的运动任务与工作环境下，利用本发明方法激励机器人逃离障碍物1附近的局部极小点位置后，继续利用传统人工势场法进行机器人路径规划，如图5所示，由于机器人的目标位置附近存在障碍物6，机器人到达目标位置附近后，会在图中菱形标记的两点间往复移动，无法继续靠近目标位置；机器人目标位置不可达时所受合力的幅值变化情况如图6所示。
[0111]
本发明方法在本实施例中的应用结果如图7-9所示；图7为某一步优化机器人移动步长时全局最优粒子适应度随迭代次数的变化曲线图，全局最优粒子适应度逐渐下降并收敛，说明利用粒子群算法能获得机器人的最优移动步长，使机器人能更快地到达目标位置；在图2所示的机器人的运动任务与工作环境下，利用本发明方法进行机器人路径规划，如图8所示，机器人未在障碍物1附近陷入局部极小点位置，即使机器人的目标位置附近存在障碍物6，机器人仍能到达目标位置；图9显示了机器人移动过程中所受合力的幅值变化情况；综上，本发明能够有效克服传统人工势场法易陷入局部极小点、目标位置不可达的缺陷，使机器人在多障碍物的复杂环境中能以最优路径到达目标位置。
[0112]
需要说明的是，在本文中，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者系统不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者系统所固有的要素。在没有更多限制的情况下，由语句“包括一个
……”
限定的要素，并不排除在包括该
要素的过程、方法、物品或者系统中还存在另外的相同要素。
[0113]
上述本发明实施例序号仅仅为了描述，不代表实施例的优劣。在列举了若干装置的单元权利要求中，这些装置中的若干个可以是通过同一个硬件项来具体体现。词语第一、第二、以及第三等的使用不表示任何顺序，可将这些词语解释为标识。
[0114]
以上仅为本发明的优选实施例，并非因此限制本发明的专利范围，凡是利用本发明说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本发明的专利保护范围内。