一种基于A*与改进人工势场法的编队路径规划控制方法技术

本发明专利技术公开了一种基于A*与改进人工势场法的编队路径规划控制方法，主要由全局路径规划和局部路径两部分组成，采用A*算法作为全局路径规划算法，生成最优路径，引导多机器人编队安全、快速的到达目标点。将最优路径划分为多个子目标点，构成子目标点序列，采用改进的人工势场法作为局部路径规划算法，将每一个子目标点作为人工势场法的虚拟目标点，在静态地图中有效避开障碍物，引导多机器人编队安全通过障碍物区域，保证路径规划的顺利进行，解决传统人工势场法目标点不可达的问题。并且缩短了多机器人系统编队路径规划的运动时间和运动距离。动距离。动距离。

【技术实现步骤摘要】
一种基于A*与改进人工势场法的编队路径规划控制方法


[0001]本专利技术属于路径规划控制
，涉及多机器人系统的编队路径规划控制方法，具体涉及了一种基于A*与改进人工势场法的编队路径规划控制方法。

技术介绍

[0002]当今社会科技飞速发展，物流业、服务业、制造业等多个行业在工业2.0的新时代中与时俱进、同步发展。因此，移动机器人被广泛应用于采矿、救援和农业等领域。但是，单个机器人的执行能力有限，独立完成复杂任务较为困难。通常需要多个机器人相互配合，多机器人系统协同可以克服单个机器人的功能缺陷，特别是在一些特定任务中，保持正确的编队不仅可以提高任务执行效率，还可以增强整个系统的鲁棒性和容错性，同时增加运行时的适应性，使机器人更容易成功完成任务。多机器人系统编队路径规划在多机器人编队系统中起着承上启下的作用，决定着整个系统是否能够顺利执行，有效的编队路径规划方法能够在短时间内生成最优路径，它不仅允许多机器人系统在避开障碍物的过程中调整编队轨迹，而且防止了编队中机器人之间发生碰撞。
[0003]编队路径规划是一项复杂的任务，可以看作是一个多目标优化问题，规划轨迹应在总距离、导航时间和能耗等方面进行优化。A*算法是目前主要用于在静态地图环境中求解最短路径，是最流行的一种寻找最短路径的图搜索算法。A*算法使用启发式搜索，更加重视接近目标的最优路径，但是该算法主要运用在求解全局最短路径中，无法处理动态避障问题，实时性较差。传统人工势场法通过目标点的虚拟引力场和障碍物的虚拟斥力场产生合力，牵引机器人向目标点移动，该算法非常适用于在局部路径规划中安全避开障碍物，但是由于人工势场法是由虚拟势场构建而来，因此机器人在运动过程中极易陷入局部极小值。局部极小值主要分为三种情况：(1)引力大于斥力，导致碰撞障碍物；(2)引力等于斥力，导致合力为零，机器人停止或振荡；(3)斥力大于引力，导致目标点不可达。因此，选择合适的编队路径规划算法是多机器人系统编队路径规划最重要的一步。

技术实现思路

[0004]针对现有技术的不足，本专利技术提出了一种基于A*与改进人工势场法的编队路径规划控制方法，引入距离因子，并采用多子目标划分的方法对传统的人工势场法进行改进，然后再融合A*算法，解决了传统人工势场法易陷入局部极小值或出现运动震荡的问题。
[0005]一种基于A*与改进人工势场法的编队路径规划控制方法，具体包括以下步骤：
[0006]步骤一：指定一个机器人为编队领导者，然后采用代数图论法进行多机器人系统的编队。
[0007]所述代数图论法具体为：设g＝(G,E,V)是一个N阶有向加权图，其中V表示节点集合V＝{v1,v2,v3,
……
v
N
}。E表示有向边集合，有向边e
ij
由有序节点(v
i
,v
j
)表示。在有向加权图g中，从节点v
i
到节点v
j
的有向路径是有向网络中的一系列边(v
i
,v
j
)，当且仅当其子图是有向生成树时，有向加权图g包含有向生成树。用一个对角矩阵D∈R
N
来表示度矩阵，用来描
述连接到每个节点或顶点的链接数，度矩阵D为：
[0008][0009]同样的，用一个方阵A∈R
N
来表示邻接矩阵，用来描述节点之间的信息流或连接关系，邻接矩阵A为：
[0010][0011]计算得到拉普拉斯矩阵L：
[0012]L＝D
‑
A
ꢀꢀꢀ
(3)
[0013]对于n个节点，动力学公式为：
[0014][0015]其中x(t)∈R，v(t)∈R,分别表示当前节点在t时刻的位置和速度。x(0)＝x0,v(0)＝v0,x＝[x1,x2,x3,
……
x
N
]T
并且v＝[v1,v2,v3,
……
v
N
]T
。u(t)∈R表示当前节点的在t时刻的力输入，x(t)、v(t)、u(t)三者关系为：
[0016]u(t)＝
‑
Lx(t)
‑
Lv(t)
ꢀꢀꢀ
(5)
[0017]对于多机器人系统的共识协议，必须满足以下条件，即所有节点必须收敛到与其他相邻节点的相同状态，收敛表达式为：
[0018][0019]根据动力学公式将多机器人系统的二阶共识改写为：
[0020][0021]其中，s＝(s
1T
,s
2T
,
……
s
NT
)
T
，w＝(w
1T
,w
2T
,
……
w
NT
)
T
，并且第i个节点位置的纵坐标Sy
i
＝(s
iT
，w
iT
)
T
。然后，将网络改写为紧密的矩阵形式：
[0022][0023]其中，s
y
(t)表示当前节点在t时刻位置的纵坐标，是matlab中的Kronecker积，I
n
是一个n阶单位矩阵。代数图论法具有易于实现大规模机器人编队并且稳定性和收敛性好的特点。
[0024]步骤二：以编队中领导者的机器人位置作为父节点，通过A*算法规划出全局最优路径。
[0025]步骤三：在步骤二得到的全局最优路径中划分多个子目标点，子目标点的个数可以根据环境的复杂程度和障碍物的疏密程度进行更改。短距离下分割的子目标点可以逐步吸引领导者机器人向最终的目标点移动，以此大大降低机器人陷入局部极小值的概率，完成不同环境下的路径规划。将子目标点作为虚拟目标点，多机器人系统在改进人工势场法
的引导下向该子目标点移动。
[0026]s3.1、在改进人工势场法的引导下，编队领导者向虚拟目标点移动，系统中的其余机器人跟随编队领导者的路径移动。当系统中的任意一个机器人到达虚拟目标点时，下一个子目标点出现。否则，直至本次程序迭代运行完成后，下一个子目标点出现。
[0027]s3.2、在多机器人系统朝子目标点运动的过程中，如果系统中任意一个机器人与障碍物的距离小于阈值d，编队临时解散，所有机器人在改进人工势场法的作用下避开障碍物，然后继续向虚拟目标点运动；在到达虚拟目标点后恢复编队。
[0028]s3.3、重复步骤s3.1、s3.2直至多机器人系统到达最后一个子目标点即全局目标点。
[0029]所述改进人工势场法具体为：
[0030]设计如下的障碍物斥力势场函数U
rep
(q)：
[0031][0032]其中，0≤n＜1；β＞0，表示斥力系数，ρ(q,q
obs
)表示机器人位置q与障碍物位置q
obs
之间本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于A*与改进人工势场法的编队路径规划控制方法，其特征在于：具体包括以下步骤：步骤一：指定一个机器人为编队领导者，然后采用代数图论法进行多机器人系统的编队；步骤二：以编队中领导者的机器人位置作为父节点，通过A*算法规划出全局最优路径；步骤三：在步骤二得到的全局最优路径中划分多个子目标点，然后将子目标点作为虚拟目标点，多机器人系统在改进人工势场法的引导下向该子目标点移动；所述改进人工势场法在人工势场法的障碍物斥力势场函数中加入了机器人位置与障碍物位置之间的相对距离作为调节因子，使机器人驶向虚拟目标点的过程中，斥力分量逐渐减小，而引力分量逐渐增大，当机器人到达目标点时，引力分量达到最大，斥力分量逐渐减至0；s3.1、在改进人工势场法的引导下，编队领导者向虚拟目标点移动，系统中的其余机器人跟随编队领导者的路径移动；当系统中的任意一个机器人到达虚拟目标点时，下一个子目标点出现；否则，直至本次程序迭代运行完成后，下一个子目标点出现；s3.2、在多机器人系统朝子目标点运动的过程中，如果系统中任意一个机器人与障碍物的距离小于阈值d，编队临时解散，所有机器人在改进人工势场法的作用下避开障碍物，然后继续向虚拟目标点运动；在到达虚拟目标点后恢复编队；s3.3、重复步骤s3.1、s3.2直至多机器人系统到达最后一个子目标点即全局目标点。2.如权利要求1所述一种基于A*与改进人工势场法的编队路径规划控制方法，其特征在于：所述代数图论法具体为：设g＝(G，E，V)是一个N阶有向加权图，其中V表示节点集合V＝{v1，v2，v3，
……
v
N
}；E表示有向边集合，有向边e
ij
由有序节点(v
i
，v
j
)表示；在有向加权图g中，从节点v
i
到节点v
j
的有向路径是有向网络中的一系列边(v
i
，v
j
)，当且仅当其子图是有向生成树时，有向加权图g包含有向生成树；度矩阵D用于描述连接到每个节点或顶点的链接数：邻接矩阵A用于描述节点之间的信息流或连接关系：计算得到拉普拉斯矩阵L：L＝D
‑
A
ꢀꢀꢀꢀ
(3)对于n个节点，动力学公式为：其中x(t)∈R，v(t)∈R，分别表示当前节点在t时刻的位置和速度；x(0)＝x0，v(0)＝v0，x＝[x1，x2，x3，......

【专利技术属性】
技术研发人员：童啸龙，余善恩，
申请(专利权)人：杭州电子科技大学，
类型：发明
国别省市：