一种改进人工势场算法的多机器人编队控制方法

本发明属于多机器人编队控制领域，尤其涉及一种改进人工势场算法的多机器人编队控制方法。

背景技术：

1、随着现代控制技术的发展和人类科技水平的提高，机器人被寄予更大的期望。但由于真实环境的复杂性和单机器人性能的局限性，仅靠单机器人难以完成复杂任务无法满足实际要求，而多机器人系统在协同控制性能上具备更强的容错性、抗干扰性、鲁棒性以及更高的执行效率，多机器人编队问题是指多机器人在执行某项任务的过程中保持预先规定的队形不变，避障问题可以看作多机器人从起点运动到终点的过程中规划出一条安全且高效的路径。

2、目前，全局路径规划方法主要有a*、d*、快速扩展随机树法等，局部路径规划方法主要有人工势场法、遗传算法、神经网络算法等。多机器人编队保持与路径规划是对单机器人路径规划的扩展，在多机器人编队路径规划的过程中，由于机器人之间的协同避障、位置约束等问题导致规划任务更加困难。

3、人工势场法以其简单的数学形式，计算量小等优点，被广泛应用于机器人编队控制当中。人工势场法是将机器人运动环境模拟为一种抽象的人造力场，目标点对机器人产生引力作用，障碍物对移动机器人产生斥力作用。在引力和斥力共同作用下移动机器人朝着目标点移动。然而传统人工势场法进行移动机器人路径规划时通常存在目标不可达和局部极小值问题。

4、上述缺陷制约了传统人工势场法在机器人编队路径规划中的应用效果。

技术实现思路

1、本发明的目的是提出一种改进人工势场算法的多机器人编队控制方法，为了解决人工势场法容易陷入局部最优和单机器人执行任务效率低下的问题。

2、一种改进人工势场法的多机器人编队控制方法，所述的预测方法包括以下步骤：

3、s1，机器人编队通信结构用无向图g＝(γ,∈)表示，其中γ＝1,2,…,n表示图有n个顶点,∈则表示由顶点组成的边无向图即信息可以双向传递；

4、s2，设定各个机器人的初始位置信息，包括横纵坐标以及朝向角度和协同状态达到稳定时机器人之前的相对位置关系；并且给定目标点的位置信息、速度上限、障碍物的位置信息、机器人的探测距离以及控制周期；

5、s3，采用领航跟随编队结构，设置控制循环次数和控制间隔，每次控制时先计算领航者机器人位置与目标点的距离与障碍物进入到机器人的探测距离，计算引力与斥力，使机器人避免碰撞到障碍物的同时继续向目标点移动；

6、s4，根据一阶编队的协同原理计算领航者与跟随者的误差信息并施加给跟随者速度，使其向协同的位置运动；

7、s5，领航者判断是否找到目标点，如果找到则规划出一条完整路径。

8、进一步的，所述步骤s1中，具体的，设置机器人编队双向通信拓扑图对应的邻接矩阵为

9、进一步的，所述步骤s2中，具体的，设置仿真环境大小为25×25，环境中黑色部分为障碍物区域，白色部分为可通行区域。

10、进一步的，在所述步骤s2中设置领航者出发点的坐标为(7.5,7)，领航者与队形方向夹角为45°。目标点的坐标为(20,20)，控制周期为0.1s，设置循环控制600次。

11、进一步的,在所述步骤s3中使用欧几里得距离公式来计算从当前位置到目标位置的距离；使用反正切函数计算从当前位置到目标位置的方向角；

12、dist＝(goal(1)-posex(n,k))2+(goal(2)-posey(n,k))2

13、th＝atan2(goal(2)-posey(n,k)，goal(1)-posex(n,k))

14、进一步的,在所述步骤s3中使用计算出的距离和角度来更新速度分量。这里kn设置为0.2，表示速度的比例因子；

15、vx(n,k+1)＝kn*dist*cos(th)

16、vy(n,k+1)＝kn*dist*sin(th)

17、进一步的,在所述步骤s3中，当障碍物进入到机器人的探测距离时，计算障碍物给机器人的排斥力repu，使机器人避免碰撞到障碍物的同时继续向目标点移动，并且跟新速度分量：

18、repu＝repulsion([posex(n,k),posey(n,k)],obpose,detectr)

19、使用计算出的排斥力向量来更新速度分量：

20、vx(n,k+1)＝vx(n,k+1)+β·repu(1)

21、vy(n,k+1)＝vy(n,k+1)+β·repu(2)

22、进一步的,在所述步骤s3中，定义引力势场函数uatt为：

23、

24、其中katt为引力增益，dt为机器人到目标点的距离。

25、而改进后的引力场函数为：

26、

27、相比于原始引力势场函数，改进后的引力场函数增加了范围限定，d*给定了一个阈值限定了目标和物体之间的距离。

28、斥力势场的表达式为：

29、

30、其中，krep为斥力增益，p为机器人坐标，pobs为障碍物坐标，ρ(p,pobs)为机器人与障碍物的距离，ρ0为障碍物的影响范围。

31、改进后的斥力场函数为：

32、

33、其中m是一个随机数并且0≤m<1,ρm(p,ptar)是增加的分解因子。

34、人工势场法的合力势场函数为：

35、up＝uatt+urep

36、其中uatt为势场空间中目标点产生的引力势场，urep为障碍物产生的斥力势场，up为以上两种势场的矢量叠加。

37、进一步地，所述步骤s4中计算跟随者误差信息及速度，具体的，遍历全部跟随者，计算跟随机器人在x和y方向上的总偏移量以及边的总权重。

38、进一步地，所述步骤s4中，具体的，由于多机器人编队一直处于行进过程，位置在不断进行变化，对各边的权重和速度进行更新并且计算误差权重进行存储。

39、v_x(i,k+1)＝k0*v_x(n,k)+γ*distance*cos(th)

40、v_y(i,k+1)＝k0*v_y(n,k)+γ*distance*sin(th)

41、进一步地，所述步骤s5中，具体的，画出多机器人编队系统的行进轨迹图以及距离差值。

42、本发明提供的技术方案具有以下有益效果：

43、本发明通过改进传统人工势场法，改进的人工势场算法在原来的斥力势场函数中添加分解因子并将原来的斥力分解为指向目标点的引力分量和背离障碍物的斥力分量以达到减小斥力的目的。本发明打破了移动机器人陷入局部空间时引力和斥力的平衡关系，帮助移动机器人逃离局部空间的约束。并且领航机器人可以进行动态避障，跟随机器人进行轨迹跟踪合作完成整体的编队控制，使机器人在多障碍物的复杂环境中能以最优路径到达目标位置。

技术特征：

1.一种改进人工势场算法的多机器人编队控制方法，其特征在于，包括以下步骤：

2.根据权利要求1所述的一种基于改进人工势场算法的多机器人编队控制划方法，其特征在于，所述步骤s1中包含以下步骤：

3.根据权利要求1所述的一种基于改进人工势场算法的多机器人编队控制方法，其特征在于，所述步骤s3中包含以下步骤：

4.根据权利要求1所述的一种基于改进人工势场算法的多机器人编队控制方法，其特征在于，所述步骤s4中包含以下步骤：

技术总结本发明公开了一种改进人工势场算法的多机器人编队控制方法。多机器人路径规划相比单机器人路径规划更加复杂，在规划时需要考虑多机器人避障与相互协作等问题。本发明针对人工势场算法容易陷入局部最值的问题，提出一种改进的人工势场算法。改进的人工势场算法在原来的引力场函数增加了范围限定，斥力势场函数中添加分解因子并将原来的斥力分解为指向目标点的引力分量和背离障碍物的斥力分量以达到减小斥力的目的。该算法打破了移动机器人陷入局部空间时引力和斥力的平衡关系，帮助移动机器人逃离局部空间的约束。并且领航机器人可以进行动态避障，跟随机器人进行轨迹跟踪合作完成整体的编队控制。技术研发人员：伍锡如,土可心受保护的技术使用者：桂林电子科技大学技术研发日：技术公布日：2024/6/18