一种基于人造引力场运动模拟的机器人运动规划方法

1.本发明涉及路径规划技术领域，具体涉及一种基于人造引力场运动模拟的机器人运动规划方法。


背景技术：

2.人类进入新世纪以来，以互联网、大数据、人工智能为表示的新技术与制造业加速融合，推动了智能制造的进步与成熟。同时有关机器人的新技术和新产品不断出现，成为了推动新一轮科技革命和产业革命的新的动力，既改变了人们的生活，也为制造业的发展提供了突破口。一个国家智能机器人技术的水平高低象征着该国家自动化、人工智能、自适应、工业控制等多方面的综合实力。路径规划技术作为机器人研究领域的重要组成部分，其发展在一定程度上标志着机器人智能水平的高低，而路径规划方法的优劣直接影响路径规划效果。目前，国内外许多专家学者都在致力于路径规划算法的研究，常用的优化算法主要有人工势场法、免疫算法、蚁群优化算法、神经网络、粒子群优化算法和遗传算法等。人工势场法因其数学分析简单、计算量小、路径光滑等优点被广泛应用在实时避障和路径规划领域。但随着研究的不断深入，应用人工势场法进行机器人路径规划的不足逐渐被发现，例如，局部最优、目标不可达等问题。


技术实现要素：

3.本发明的目的在于提供一种基于人造引力场运动模拟的机器人运动规划方法，以解决现有技术中局部最优、目标不可达的技术问题。
4.为解决上述技术问题，本发明具体提供下述技术方案：
5.一种基于人造引力场运动模拟的机器人运动规划方法，包括以下步骤：
6.步骤s1、在路径搜索空间中基于人工势场法在目标终点处构建对机器人的人造引力场，以及在空间故障点处构建对机器人的人造斥力场；
7.步骤s2、量化机器人运动规划路径的路径曲折度，基于路径曲折度与s型函数为人造引力场和人造斥力场构建势场调节因子，并基于势场调节因子对所述人造引力场和人造斥力场进行组合得到机器人的人造合力场，以避免局部最优和目标终点不可达；
8.步骤s3、基于所述人造合力场在路径搜索空间中规划出搜索起点至目标终点的运动规划路径，并利用b样条曲线对运动规划路径进行平滑优化得到从搜索起点至目标终点的最优运动路径。
9.作为本发明的一种优选方案，所述在路径搜索空间中基于人工势场法在目标终点处构建对机器人的人造引力场，包括：
10.构建目标终点对机器人的引力势场，所述人造引力场的势场函数为：
[0011][0012]
式中，u
attr
表征为人造引力场的势场强度，k
attr
表征为人造引力场的比例系数，dr表征为机器人的位置坐标，dg表征为目标终点的位置坐标，||d
r-dg||表征为机器人和目标终点间的欧几里得距离。
[0013]
作为本发明的一种优选方案，所述在空间故障点处构建对机器人的人造斥力场，包括：
[0014]
依次构建每个空间障碍点对机器人的人造斥力场，所述人造斥力场的势场函数为：
[0015][0016]
式中，u
rep,j
表征为第j个空间障碍点产生的人造斥力场的势场强度，k
rep
表征为人造斥力场的比例系数，dj表征为第j个空间障碍点的位置坐标与机器人的位置坐标间的欧几里得距离，d0表征为空间障碍点的影响距离，j为计量常数；
[0017]
对所有空间障碍点对机器人的人造斥力场进行求和得到空间障碍点对机器人的总人造斥力场，所述总人造斥力场的势场函数为：
[0018][0019]
式中，u
rep
表征为总人造斥力场的势场强度，m表征为空间障碍点的总数目。
[0020]
作为本发明的一种优选方案，所述量化机器人运动规划路径的路径曲折度，包括：
[0021]
获取运动规划路径中最新路径规划节点以及最新路径规划节点的所有前置路径规划节点的位置坐标，并计算出最新路径规划节点以及前置路径规划节点的位置坐标平均值作为最新坐标期望值，所述最新坐标期望值的计算公式为：
[0022][0023]
基于所述最新坐标期望值计算出最新路径规划节点以及前置路径规划节点的位置坐标离散值作为最新路径曲折度，所述最新路径曲折度的计算公式为：
[0024][0025]
式中，dh
new
表征为最新路径曲折度，eh
new
表征为最新坐标期望值，hi表征为最新路径规划节点以及前置路径规划节点中第i个路径规划节点的位置坐标，xi、yi和zi分别表征为所述第i个路径规划节点的位置坐标中的三维x向坐标值、三维y向坐标值和三维z向坐标值，n为最新路径规划节点以及前置路径规划节点中的路径规划节点总数量，i为计量常数。
[0026]
作为本发明的一种优选方案，所述基于路径曲折度与s型函数为人造引力场和人造斥力场构建势场调节因子，包括：
[0027]
基于所述最新路径曲折度和最新坐标期望值构建出最新收敛因子，以量化出在最新路径规划节点处的运动规划路径收敛性，所述最新收敛因子的计算公式为：
[0028][0029]
式中，γ
new
表征为最新收敛因子；
[0030]
将最新收敛因子与s型函数结合构建出在最新路径规划节点处人造引力场和人造斥力场的势场调节因子，所述人造引力场的势场调节因子的计算公式为：
[0031][0032]
所述人造斥力场的势场调节因子的计算公式为：
[0033][0034]
式中，pa
new
表征为人造引力场的势场调节因子，pr
new
表征为人造斥力场的势场调节因子，β表征为常系数，α表征为常系数，c表征为常数项。
[0035]
作为本发明的一种优选方案，所述基于势场调节因子对所述人造引力场和人造斥力场进行组合得到机器人的人造合力场，包括：
[0036]
将最新路径规划节点处人造引力场和人造斥力场的势场调节因子对所述人造引力场的势场函数和人造斥力场的势场函数进行组合得到人造合力场的势场函数，所述人造合力场的势场函数为：
[0037]utotal
＝pa
new
*u
attr
pr
new
*u
rep
；
[0038]
式中，u
total
表征为人造合力场的势场函数。
[0039]
作为本发明的一种优选方案，所述基于所述人造合力场在路径搜索空间中规划出搜索起点至目标终点的运动规划路径，包括：
[0040]
步骤1、计算搜索起点的人造合力场的势场函数值，并提取出势场函数值中的势场方向，依据所述搜索起点沿所述势场方向进行预设步长的拓展得到最新路径规划节点；
[0041]
步骤2、计算最新路径规划节点与目标终点的距离，并将所述最新路径规划节点与目标终点的距离与预设距离比较，其中，
[0042]
若所述最新路径规划节点与目标终点的距离小于或等于预设距离，则从搜索起点开始与依次连接路径规划节点至目标终点处得到运动规划路径；
[0043]
若所述最新路径规划节点与目标终点的距离大于预设距离，则计算最新路径规划节点的人造合力场的势场函数值，并提取出势场函数值中的势场方向，依据所述搜索起点沿所述势场方向进行预设步长的拓展得到下一最新路径规划节点；
[0044]
步骤3、循环执行步骤2，直至得到搜索起点至目标终点的运动规划路径。
[0045]
作为本发明的一种优选方案，在最新路径规划节点执行步骤2前，对所述最新路径规划节点进行碰撞检测，其中，
[0046]
若最新路径规划节点与空间障碍点或空间边界发生碰撞，则将最新路径规划节点沿上一最新路径规划节点的所述势场方向逐次执行反向缩减d0/2长度，直至执行最新路径
规划节点与空间障碍点或空间边界不发生碰撞。
[0047]
作为本发明的一种优选方案，在最新路径规划节点的势场方向为0时，将上一路径规划节点的势场方向作为最新路径规划节点的势场。
[0048]
作为本发明的一种优选方案，对所述运动规划路径执行三次b样条曲线的得到所述最优运动路径。
[0049]
本发明与现有技术相比较具有如下有益效果：
[0050]
本发明对运动规划路径的收敛性进行量化，并基于收敛性构建的人造引力场和人造斥力场的调节因子，实现路径规划初期的路径规划节点距离目标终点距离较远，人造引力场的势场强度减小，人造斥力场的势场强度增大，从而避免了在路径规划节点距离目标终点距离较远时，人造引力场的势场强度过大，导致陷入局部最优问题，路径规划初期的路径规划节点距离目标终点距离较近，人造引力场的势场强度增大，人造斥力场的势场强度减小，从而避免了在路径规划节点距离目标终点距离较近时，人造引力场的势场强度过小，导致陷入目标不可达的问题，全程根据路径规划节点的增加进行自适应，无需人为进行干预。
附图说明
[0051]
为了更清楚地说明本发明的实施方式或现有技术中的技术方案，下面将对实施方式或现有技术描述中所需要使用的附图作简单地介绍。显而易见地，下面描述中的附图仅仅是示例性的，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据提供的附图引伸获得其它的实施附图。
[0052]
图1为本发明实施例提供的机器人运动规划方法流程图；
[0053]
图2为本发明实施例提供的人造合力场结构示意图。
具体实施方式
[0054]
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0055]
如图1和2所示，本发明提供了一种基于人造引力场运动模拟的机器人运动规划方法，包括以下步骤：
[0056]
步骤s1、在路径搜索空间中基于人工势场法在目标终点处构建对机器人的人造引力场，以及在空间故障点处构建对机器人的人造斥力场；
[0057]
在路径搜索空间中基于人工势场法在目标终点处构建对机器人的人造引力场，包括：
[0058]
构建目标终点对机器人的引力势场，人造引力场的势场函数为：
[0059][0060]
式中，u
attr
表征为人造引力场的势场强度，k
attr
表征为人造引力场的比例系数，dr表征为机器人的位置坐标，dg表征为目标终点的位置坐标，||d
r-dg||表征为机器人和目标
终点间的欧几里得距离。
[0061]
在空间故障点处构建对机器人的人造斥力场，包括：
[0062]
依次构建每个空间障碍点对机器人的人造斥力场，人造斥力场的势场函数为：
[0063][0064]
式中，u
rep,j
表征为第j个空间障碍点产生的人造斥力场的势场强度，k
rep
表征为人造斥力场的比例系数，dj表征为第j个空间障碍点的位置坐标与机器人的位置坐标间的欧几里得距离，d0表征为空间障碍点的影响距离，j为计量常数；
[0065]
对所有空间障碍点对机器人的人造斥力场进行求和得到空间障碍点对机器人的总人造斥力场，总人造斥力场的势场函数为：
[0066][0067]
式中，u
rep
表征为总人造斥力场的势场强度，m表征为空间障碍点的总数目。
[0068]
利用传统人工势场法对路径搜索空间进行引力势场和斥力势场的构建得到人造引力场和人造斥力场。
[0069]
步骤s2、量化机器人运动规划路径的路径曲折度，基于路径曲折度与s型函数为人造引力场和人造斥力场构建势场调节因子，并基于势场调节因子对人造引力场和人造斥力场进行组合得到机器人的人造合力场，以避免局部最优和目标终点不可达；
[0070]
量化机器人运动规划路径的路径曲折度，包括：
[0071]
获取运动规划路径中最新路径规划节点以及最新路径规划节点的所有前置路径规划节点的位置坐标，并计算出最新路径规划节点以及前置路径规划节点的位置坐标平均值作为最新坐标期望值，最新坐标期望值的计算公式为：
[0072][0073]
基于最新坐标期望值计算出最新路径规划节点以及前置路径规划节点的位置坐标离散值作为最新路径曲折度，最新路径曲折度的计算公式为：
[0074][0075]
式中，dh
new
表征为最新路径曲折度，eh
new
表征为最新坐标期望值，hi表征为最新路径规划节点以及前置路径规划节点中第i个路径规划节点的位置坐标，xi、yi和zi分别表征为第i个路径规划节点的位置坐标中的三维x向坐标值、三维y向坐标值和三维z向坐标值，n为最新路径规划节点以及前置路径规划节点中的路径规划节点总数量，i为计量常数。
[0076]
基于路径曲折度与s型函数为人造引力场和人造斥力场构建势场调节因子，包括：
[0077]
基于最新路径曲折度和最新坐标期望值构建出最新收敛因子，以量化出在最新路径规划节点处的运动规划路径收敛性，最新收敛因子的计算公式为：
[0078][0079]
式中，γ
new
表征为最新收敛因子；
[0080]
在从搜索起点开始进行运动路径规划时，由于路径规划节点是从搜索起点开始逐点进行规划的，路径规划初期，路径规划节点数量少，eh
new
较大，dh
new
较小，特别是在运动规划路径中仅有搜索起点一个点时dh
new
＝0，eh
new
最大，在路径规划节点增加时，即路径规划节点向目标终点接近时，eh
new
呈下降趋势，dh
new
呈上升趋势，则在路径规划末期，路径规划节点数量多，eh
new
较小，dh
new
较大，从而使得γ
new
从路径规划初期到路径规划末期趋于收敛，并在路径规划初期较大，路径规划末期较小，随着路径规划时期呈自收敛状态。
[0081]
将最新收敛因子与s型函数结合构建出在最新路径规划节点处人造引力场和人造斥力场的势场调节因子，人造引力场的势场调节因子的计算公式为：
[0082][0083]
人造斥力场的势场调节因子的计算公式为：
[0084][0085]
式中，pa
new
表征为人造引力场的势场调节因子，pr
new
表征为人造斥力场的势场调节因子，β表征为常系数，α表征为常系数，c表征为常数项。
[0086]
基于势场调节因子对人造引力场和人造斥力场进行组合得到机器人的人造合力场，包括：
[0087]
将最新路径规划节点处人造引力场和人造斥力场的势场调节因子对人造引力场的势场函数和人造斥力场的势场函数进行组合得到人造合力场的势场函数，人造合力场的势场函数为：
[0088]utotal
＝pa
new
*u
attr
pr
new
*u
rep
；
[0089]
式中，u
total
表征为人造合力场的势场函数。
[0090]
利用γ
new
构建人造引力场和人造斥力场的势场调节因子，使得在路径规划初期，pa
new
较小，pr
new
较大，实现对人造合力场中的u
attr
和u
rep
进行调节，即在路径规划初期拉低u
attr
的值，提升u
rep
的值，使得路径规划初期人造引力场的势场强度减小，人造斥力场的势场强度增大，同时路径规划初期的路径规划节点距离目标终点距离较远，人造引力场的势场强度减小，人造斥力场的势场强度增大，从而避免了在路径规划节点距离目标终点距离较远时，人造引力场的势场强度过大，导致陷入局部最优问题；
[0091]
同理，在路径规划末期，pa
new
较大，pr
new
较小，实现对人造合力场中的u
attr
和u
rep
进行调节，即在路径规划初期提升u
attr
的值，拉低u
rep
的值，使得路径规划初期人造引力场的势场强度增大，人造斥力场的势场强度减小，同时路径规划初期的路径规划节点距离目标
终点距离较近，人造引力场的势场强度增大，人造斥力场的势场强度减小，从而避免了在路径规划节点距离目标终点距离较近时，人造引力场的势场强度过小，导致陷入目标不可达的问题，全程根据路径规划节点的增加进行自适应，无需人为进行干预。
[0092]
步骤s3、基于人造合力场在路径搜索空间中规划出搜索起点至目标终点的运动规划路径，并利用b样条曲线对运动规划路径进行平滑优化得到从搜索起点至目标终点的最优运动路径。
[0093]
基于人造合力场在路径搜索空间中规划出搜索起点至目标终点的运动规划路径，包括：
[0094]
步骤1、计算搜索起点的人造合力场的势场函数值，并提取出势场函数值中的势场方向，依据搜索起点沿势场方向进行预设步长的拓展得到最新路径规划节点；
[0095]
步骤2、计算最新路径规划节点与目标终点的距离，并将最新路径规划节点与目标终点的距离与预设距离比较，其中，
[0096]
若最新路径规划节点与目标终点的距离小于或等于预设距离，则从搜索起点开始与依次连接路径规划节点至目标终点处得到运动规划路径；
[0097]
若最新路径规划节点与目标终点的距离大于预设距离，则计算最新路径规划节点的人造合力场的势场函数值，并提取出势场函数值中的势场方向，依据搜索起点沿势场方向进行预设步长的拓展得到下一最新路径规划节点；
[0098]
步骤3、循环执行步骤2，直至得到搜索起点至目标终点的运动规划路径。
[0099]
在最新路径规划节点执行步骤2前，对最新路径规划节点进行碰撞检测，其中，
[0100]
若最新路径规划节点与空间障碍点或空间边界发生碰撞，则将最新路径规划节点沿上一最新路径规划节点的势场方向逐次执行反向缩减d0/2长度，直至执行最新路径规划节点与空间障碍点或空间边界不发生碰撞。
[0101]
在最新路径规划节点的势场方向为0时，将上一路径规划节点的势场方向作为最新路径规划节点的势场，或由使用者根据实际使用场景进行自定义。
[0102]
对运动规划路径执行三次b样条曲线的得到最优运动路径，。
[0103]
本发明对运动规划路径的收敛性进行量化，并基于收敛性构建的人造引力场和人造斥力场的调节因子，实现路径规划初期的路径规划节点距离目标终点距离较远，人造引力场的势场强度减小，人造斥力场的势场强度增大，从而避免了在路径规划节点距离目标终点距离较远时，人造引力场的势场强度过大，导致陷入局部最优问题，路径规划初期的路径规划节点距离目标终点距离较近，人造引力场的势场强度增大，人造斥力场的势场强度减小，从而避免了在路径规划节点距离目标终点距离较近时，人造引力场的势场强度过小，导致陷入目标不可达的问题，全程根据路径规划节点的增加进行自适应，无需人为进行干预。
[0104]
以上实施例仅为本技术的示例性实施例，不用于限制本技术，本技术的保护范围由权利要求书限定。本领域技术人员可以在本技术的实质和保护范围内，对本技术做出各种修改或等同替换，这种修改或等同替换也应视为落在本技术的保护范围内。