CN114326726A - 一种基于a*与改进人工势场法的编队路径规划控制方法 - Google Patents

Abstract

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

Description

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

技术领域

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

背景技术

当今社会科技飞速发展，物流业、服务业、制造业等多个行业在工业2.0的新时代中与时俱进、同步发展。因此，移动机器人被广泛应用于采矿、救援和农业等领域。但是，单个机器人的执行能力有限，独立完成复杂任务较为困难。通常需要多个机器人相互配合，多机器人系统协同可以克服单个机器人的功能缺陷，特别是在一些特定任务中，保持正确的编队不仅可以提高任务执行效率，还可以增强整个系统的鲁棒性和容错性，同时增加运行时的适应性，使机器人更容易成功完成任务。多机器人系统编队路径规划在多机器人编队系统中起着承上启下的作用，决定着整个系统是否能够顺利执行，有效的编队路径规划方法能够在短时间内生成最优路径，它不仅允许多机器人系统在避开障碍物的过程中调整编队轨迹，而且防止了编队中机器人之间发生碰撞。

编队路径规划是一项复杂的任务，可以看作是一个多目标优化问题，规划轨迹应在总距离、导航时间和能耗等方面进行优化。A*算法是目前主要用于在静态地图环境中求解最短路径，是最流行的一种寻找最短路径的图搜索算法。A*算法使用启发式搜索，更加重视接近目标的最优路径，但是该算法主要运用在求解全局最短路径中，无法处理动态避障问题，实时性较差。传统人工势场法通过目标点的虚拟引力场和障碍物的虚拟斥力场产生合力，牵引机器人向目标点移动，该算法非常适用于在局部路径规划中安全避开障碍物，但是由于人工势场法是由虚拟势场构建而来，因此机器人在运动过程中极易陷入局部极小值。局部极小值主要分为三种情况：(1)引力大于斥力，导致碰撞障碍物；(2)引力等于斥力，导致合力为零，机器人停止或振荡；(3)斥力大于引力，导致目标点不可达。因此，选择合适的编队路径规划算法是多机器人系统编队路径规划最重要的一步。

发明内容

针对现有技术的不足，本发明提出了一种基于A*与改进人工势场法的编队路径规划控制方法，引入距离因子，并采用多子目标划分的方法对传统的人工势场法进行改进，然后再融合A*算法，解决了传统人工势场法易陷入局部极小值或出现运动震荡的问题。

一种基于A*与改进人工势场法的编队路径规划控制方法，具体包括以下步骤：

步骤一：指定一个机器人为编队领导者，然后采用代数图论法进行多机器人系统的编队。

所述代数图论法具体为：设g＝(G,E,V)是一个N阶有向加权图，其中V表示节点集合V＝{v₁,v₂,v₃,……v_N}。E表示有向边集合，有向边e_ij由有序节点(v_i,v_j)表示。在有向加权图g中，从节点v_i到节点v_j的有向路径是有向网络中的一系列边(v_i,v_j)，当且仅当其子图是有向生成树时，有向加权图g包含有向生成树。用一个对角矩阵D∈R^N来表示度矩阵，用来描述连接到每个节点或顶点的链接数，度矩阵D为：

同样的，用一个方阵A∈R^N来表示邻接矩阵，用来描述节点之间的信息流或连接关系，邻接矩阵A为：

计算得到拉普拉斯矩阵L：

L＝D-A (3)

对于n个节点，动力学公式为：

其中x(t)∈R，v(t)∈R,分别表示当前节点在t时刻的位置和速度。x(0)＝x₀,v(0)＝v₀,x＝[x₁,x₂,x₃,……x_N]^T并且v＝[v₁,v₂,v₃,……v_N]^T。u(t)∈R表示当前节点的在t时刻的力输入，x(t)、v(t)、u(t)三者关系为：

u(t)＝-Lx(t)-Lv(t) (5)

对于多机器人系统的共识协议，必须满足以下条件，即所有节点必须收敛到与其他相邻节点的相同状态，收敛表达式为：

根据动力学公式将多机器人系统的二阶共识改写为：

其中，s＝(s₁ ^T,s₂ ^T,……s_N ^T)^T，w＝(w₁ ^T,w₂ ^T,……w_N ^T)^T，并且第i个节点位置的纵坐标Sy_i＝(s_i ^T，w_i ^T)^T。然后，将网络改写为紧密的矩阵形式：

其中，s_y(t)表示当前节点在t时刻位置的纵坐标，

是matlab中的Kronecker积，I_n是一个n阶单位矩阵。代数图论法具有易于实现大规模机器人编队并且稳定性和收敛性好的特点。

步骤二：以编队中领导者的机器人位置作为父节点，通过A*算法规划出全局最优路径。

步骤三：在步骤二得到的全局最优路径中划分多个子目标点，子目标点的个数可以根据环境的复杂程度和障碍物的疏密程度进行更改。短距离下分割的子目标点可以逐步吸引领导者机器人向最终的目标点移动，以此大大降低机器人陷入局部极小值的概率，完成不同环境下的路径规划。将子目标点作为虚拟目标点，多机器人系统在改进人工势场法的引导下向该子目标点移动。

s3.1、在改进人工势场法的引导下，编队领导者向虚拟目标点移动，系统中的其余机器人跟随编队领导者的路径移动。当系统中的任意一个机器人到达虚拟目标点时，下一个子目标点出现。否则，直至本次程序迭代运行完成后，下一个子目标点出现。

s3.2、在多机器人系统朝子目标点运动的过程中，如果系统中任意一个机器人与障碍物的距离小于阈值d，编队临时解散，所有机器人在改进人工势场法的作用下避开障碍物，然后继续向虚拟目标点运动；在到达虚拟目标点后恢复编队。

s3.3、重复步骤s3.1、s3.2直至多机器人系统到达最后一个子目标点即全局目标点。

所述改进人工势场法具体为：

设计如下的障碍物斥力势场函数U_rep(q)：

其中，0≤n＜1；β＞0，表示斥力系数，ρ(q,q_obs)表示机器人位置q与障碍物位置q_obs之间的相对距离，ρⁿ(q,q_obs)为添加的调节因子，ρ₀表示距离阈值。

计算障碍物斥力势场U_rep(q)的负梯度得到障碍物的斥力函数F_rep为：

其中，F_rep1和F_rep2分别是斥力F_rep在不同方向上的分力：

F_rep1为斥力分量，方向由障碍物指向机器人。

F_rep2为引力分量，方向由机器人指向虚拟目标点。

人工势场法是一种局部路径规划方法，具有成熟度高、效率高、数学计算简单等优点，最大优点在于处理静态地图或者动态地图的避障问题。但是，人工势场法是由虚拟势场构建而来，因此机器人在运动过程中极易陷入局部极小值。改进人工势场法在人工势场法的基础上，增加了机器人与目标点间的相对距离作为调节因子ρⁿ(q,q_obs)，改进了斥力势场函数，使机器人驶向虚拟目标点的过程中，斥力分量逐渐减小，而引力分量逐渐增大，当机器人到达目标点时，引力分量达到最大，斥力分量逐渐减至0，使机器人靠近目标点并稳定在目标点处，从而解决传统人工势场中目标不可达问题。

本发明具有以下有益效果：

采用A*算法作为全局路径规划算法，生成最优路径，引导多机器人编队安全、快速的到达目标点。采用改进的人工势场法作为局部路径规划算法，在静态地图中有效避开障碍物，解决传统人工势场法目标点不可达的问题，引导多机器人编队安全通过障碍物区域，并且缩短了多机器人系统编队路径规划的运动时间和运动距离。

附图说明

图1是改进人工势场法斥力函数的受力分析图；

图2是基于A*与改进人工势场法的编队路径规划控制方法流程图；

图3是多子目标点划分方法流程图；

图4是实施例中在简单环境下的多子目标点划分结果；

图5是实施例中在狭窄空间下的多子目标点划分结果；

图6是实施例中在复杂环境下的多子目标点划分结果；

图7是实施例中在简单环境下编队路径规划结果；

图8是实施例中在狭窄空间下编队路径规划结果；

图9是实施例中在复杂环境下编队路径规划结果。

具体实施方式

以下结合附图对本发明作进一步的解释说明；

一种基于A*与改进人工势场法的编队路径规划控制方法，针对传统人工势场法易陷入局部极小值，导致目标点不可达的问题。对传统的人工势场法进行改进，在传统人工势场法的斥力势场函数中添加了距离因子，有效解决了传统人工势场法在机器人向目标点靠近时斥力大于引力，导致目标点不可达的问题，改进后的斥力受力情况如图1所示。

如图2所示，本方法针对A*算法不能满足实时性要求，以及传统人工势场法易出现机器人穿过障碍物或出现运动振荡，以及路径规划距离长、时间久的问题，将A*算法和改进的人工势场法融合，如图3所示，引入多子目标划分的方式，同时对运动距离和导航时间进行优化，以迭代次数、运动时间以及每个机器人的运动距离作为衡量指标，不仅有效解决了传统人工势场法易穿越障碍物或者出现运动振荡的问题，同时还大大缩短了多机器人系统编队路径规划的运动距离和运动时间。

本实施例在Matlab软件中对该方法进行仿真，分别设置了简单环境、狭窄空间和复杂环境三种仿真环境，简单环境是指障碍物数量较少，且障碍物之间空隙较大。狭窄空间是指仿真中障碍物之间空隙较小，形成一条狭窄通道。复杂空间是指障碍物数量较多，且障碍物之间空隙较小。障碍物在仿真中均以黑色图形表示。如图4、图5、图6所示。图中左下方的点表示编队领导者的起点，右上方的点表示编队领导者的最终目标点，起点与终点之间的曲线表示A*算法规划的全局最优路径，图中画圈的点表示从起点出发的第一个子目标点。结合代数图论法进行多机器人系统编队控制，在A*算法和改进人工势场法的共同作用下完成编队路径规划任务，到达最终目标点，如图7、8、9所示，起点与终点之间的曲线表示四个机器人的运动路径。

此外，在简单环境的仿真中做了50次实验，分别统计了在传统人工势场法和本方法多机器人系统编队路径规划的迭代次数、每个机器人运动的路径长度、以及完成任务的时间，分别取其平均值作为对比数据，对比结果如表1所示：

表1

其中机器人1,2,3,4表示采用传统人工势场法进行编队路径规划的迭代次数、运动时间和路径长度。机器人一、二、三、四表示采用本方法的迭代次数、运动时间和路径长度。对比结果发现，采用多子目标划分与改进的人工势场法，使得多机器人在简单环境中编队路径规划的迭代次数约减少60.23％，完成任务的时间约减少42.9％，一号机器人运动的路径长度约减少44.23％，二号机器人运动的路径长度约减少42.82％，三号机器人运动的路径长度约减少48.65％，四号机器人运动的路径长度约减少55.68％。

以上显示和描述了本发明的基本原理、主要特征和本发明的优点。本行业的技术人员应该了解，本发明不受上述实施例的限制，上述实施例和说明书中描述的只是本发明的原理，在不脱离本发明精神和范围的前提下本发明还会有各种变化和改进，这些变化和改进都落入要求保护的本发明的范围内。本发明要求的保护范围由所附的权利要求书及其等同物界定。

Claims (3)

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＝{v₁，v₂，v₃，……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)＝x₀，v(0)＝v₀，x＝[x₁，x₂，x₃，......x_N]^T并且v＝[v₁，v₂，v₃，......v_N]^T；u(t)∈R表示当前节点的在t时刻的力输入，x(t)、v(t)、u(t)三者关系为：

u(t)＝-Lx(t)-Lv(t) (5)

对于多机器人系统的共识协议，必须满足以下条件，即所有节点必须收敛到与其他相邻节点的相同状态，收敛表达式为：

根据动力学公式将多机器人系统的二阶共识改写为：

其中，s＝(s₁ ^T，s₂ ^T，......s_N ^T)^T，w＝(w₁ ^T，w₂ ^T，......w_N ^T)^T，并且第i个节点位置的纵坐标Sy_i＝(s_i ^T，w_i ^T)^T；然后，将网络改写为紧密的矩阵形式：

其中，s_y(t)表示当前节点在t时刻位置的纵坐标，

是matlab中的Kronecker积，I_n是一个n阶单位矩阵。

3.如权利要求1所述一种基于A*与改进人工势场法的编队路径规划控制方法，其特征在于：所述改进人工势场法具体为：

设计如下的障碍物斥力势场函数U_rep(q)：

其中，0≤n＜1；β＞0，表示斥力系数，ρ(q，q_obs)表示机器人位置q与障碍物位置q_obs之间的相对距离，ρⁿ(q，q_obs)为添加的调节因子，ρ₀表示距离阈值；

计算障碍物斥力势场U_rep(q)的负梯度得到障碍物的斥力函数F_rep为：

其中，F_rep1和F_rep2分别是斥力F_rep在不同方向上的分力：

F_rep1为斥力分量，方向由障碍物指向机器人；

F_rep2为引力分量，方向由机器人指向虚拟目标点。

Images