基于人工势场法的多移动机器人编队避障控制方法

本发明涉及机器人技术领域，特别涉及一种基于人工势场法的多移动机器人编队避障控制方法。

背景技术：

随着人工智能的发展，单个机器人已经不能满足人们的日常生活的需要，而多移动机器人协作可以高效的完成任务和节约成本；在实际运用的场景中，编队难免会遇到障碍物，在多机器人系统中，编队控制与整体避障作为多移动机器人系统的关键问题，也是众多学者一直需要深入研究的问题。

目前多移动机器人编队避障的算法研究不太成熟，编队避障算法主要借鉴传统单个机器人的避障策略，应用于多移动机器人编队避障中，主要的方法有如下几种：人工势场法、a^*算法、遗传算法、预测窗口算法等。编队避障对队形的要求更高，在障碍物的约束环境下，机器人既要维持整体队形，又要合理地避开环境中可能碰到的障碍物。中国发明专利《一种多移动机器人系统半自主编队及避障控制方法》(申请日：2019.10.14；申请号：cn110703751a；公开日：2020.01.17)基于虚拟结构的整体编队结构难以改变，导致编队避障轨迹不平滑且稳定性低。中国发明专利《一种基于图论与势场方法的无人车多车道护航编队方法》(申请日：2019.12.02；申请号：cn201911211093.1；公开日：2020.03.27)需要预定义的信息过多，对通讯要求过高，会导致整体编队效率降低。

针对多移动机器人编队避障过程中队形控制、避障效率低等问题，本文发明了一种基于人工势场法的编队避障方法。首先建立可用公式统一表达的队形数据库，通过势函数得到障碍物区域可供通行的路径与宽度，领航者根据获取的信息，从队形数据库选择合适的编队队形进行合理、高效的避障，当编队中最后一个机器人避开障碍物时，避障队形快速切换到初始队形，继续执行任务。本方法控制编队在复杂环境中，可以确保编队顺利、合理、高效通过障碍物区域，提高了编队避障系统的稳定性和编队避障的整体效率。

技术实现要素：

本发明针对现有技术的缺陷，提供了一种基于人工势场法的多移动机器人编队避障控制方法。

为了实现以上发明目的，本发明采取的技术方案如下：

一种基于人工势场法的多移动机器人编队避障控制方法.包括以下步骤：

(1)建立常用的编队队形数据库；

(2)定义斥场力，得到障碍物与编队领航者之间的距离与斥场力的函数关系，分别得出领航机器人与障碍物的距离，进而得出障碍物区域可供编队通过的队形宽度。

(3)通过步骤(2)得到障碍物区域供编队通过的队形宽度，与编队当前队形进行对比，从步骤(1)建立的队形知识库选择合理的避障队形进行切换，确保编队顺利通过障碍物。

(4)编队最后一个机器人通过障碍物区域时，设计队形快速恢复函数使避障编队快速切换到初始队形。

(5)恢复初始队形，到达目标点。

进一步地，步骤(1)具体如下：

针对编队控制中常用的队形建立队形数据库，采用领航者法进行编队控制，为了表示机器人之间的相互关系及队形的形状参数，定义队形形状的参数信息矩阵通用公式如下：

f^m＝[fx1fx2...fxn]4×n(1)

fxj＝[f1jf2jf3jf4j]^t(2)

j＝1,…，n,f1j＝j,f2j＝i,

其中f^m表示某个队形的具体参数信息矩阵，fxj表示第j个机器人rj的队形信息。fxj具有由4部分构成：f1j为具体队形中跟随机器人rj的编号；f2j表示rj在编队中所跟踪的领航者的编号；f3j表示为跟随机器人rj与其领航者之间需要保持的期望距离；为机器人ri与rj之间需要保持的期望距离；f4j为跟随机器人rj与其领航者之间需要保持的期望方向角；为机器人ri与rj之间需要保持的期望方向角。

进一步地，步骤(2)具体如下：

领航者与障碍物之间的距离通过机器人携带的传感器得到；

斥力场的表达公式：

其中，η为斥力尺度因子，r表物体和障碍物之间的距离，ρ0代表每个障碍物的影响半径。

同时，领航者根据得到f1与f2的大小和θ1与θ2，求两个障碍物之间的距离d，公式如下：

其中，f1与f2分别为机器人在势场受到障碍物的排斥力；r1与r2分别为领航者与障碍物之间的距离，θ1与θ2分别为领航者和目标点连线与障碍物的夹角。

进一步地，步骤(3)具体如下：

根据d与d0大小选择避障策略从队形数据库选择避障队形，设d-d0＝k，当k＜0时，编队需要选择一字型编队通过障碍物；当k＝0时，编队中的机器人需要改变跟随者与领航者的跟随角度来缩小队形，减小队形的宽度；当k＞0时，编队保持初始队形，即可通过障碍物。其中d0为初始编队队形宽度。

进一步地，步骤(4)具体如下：

当编队中的机器人最后一个通过障碍物时，改变编队队形需要从避障队形切换到初始队形，继续执行任务。

设避障后领航者位姿为：

p(t)＝[x1y1φ1]^t(5)

其中，(x1，y1)为领航者的坐标，φ1为领航者的方向角。

设队形快速恢复函数为：

p(t)＝(p0-p∞)e^-μt+p∞(6)

其中，μ＞0，p0为初始值即避障后虚拟领航者位置状态量，p∞为p(t)的稳态值即恢复队形后的位置状态量，μt为收敛系数；p(t)按指数快速递减到p∞的值，可实现队形以指数形式快速恢复。

与现有技术相比，本发明的优点在于：

采用领航者法来控制编队队形，建立了常用队形的队形数据库，利用存在势场得到障碍物区域可供通过的宽度，再与初始队形宽度进行对比，根据对比结果，制定合适的避障策略，并设计了避障后的队形快速恢复，确保编队整体可以快速高效、合理通过障碍，能够顺利完成避障。

附图说明

图1是本发明实施例避障控制方法流程图；

图2是本发明实施例编队避障策略示意图。

具体实施方式

为使本发明的目的、技术方案及优点更加清楚明白，以下根据附图并列举实施例，对本发明做进一步详细说明。

如图1所示，一种基于人工势场法的多移动机器人编队避障控制方法。包括以下步骤：

(1)针对编队控制中常用的队形建立队形数据库，采用领航者法进行编队控制，为了表示机器人之间的相互关系及队形的形状参数，定义队形形状的参数信息矩阵通用公式如下：

f^m＝[fx1fx2...fxn]4×n(1)

fxj＝[f1jf2jf3jf4j]^t(2)

j＝1,…，n,f1j＝j,f2j＝i,

其中f^m表示某个队形的具体参数信息矩阵，fxj表示第j个机器人rj的队形信息。fxj具有由4部分构成：f1j为具体队形中跟随机器人rj的编号；f2j表示rj在编队中所跟踪的领航者的编号；f3j表示为跟随机器人rj与其领航者之间需要保持的期望距离；为机器人ri和rj之间需要保持的期望距离；f4j为跟随机器人rj与其领航者之间需要保持的期望方向角；为机器人ri和rj之间需要保持的期望方向角。

(2)当编队某个机器人检测到障碍物时，进入避障模式，编队整体速度降低；领航者根据势函数判断障碍物的个数，并判断障碍物之间距离，并从(1)中建立的队形数据库匹配合适的队形。

如图2所示，r1与r2的大小可以通过机器人携带的传感器得到，可以通过各个障碍物与领航机器人之间的距离，得到障碍物之间可供机器人通过的宽度。

斥力场的表达公式：

其中，η为斥力尺度因子，r代表物体和障碍物之间的距离，ρ0代表

每个障碍物的影响半径。

同时，领航者可以根据得到f1与f2的大小和θ1与θ2，进一步得出两个障碍物之间的距离d。求d的公式如下：

其中，r1与r2分别为领航者与障碍物之间的距离，θ1与θ2分别为领航者和目标点连线与障碍物的夹角，f1与f2分别为机器人在势场受到障碍物的排斥力。

(3)根据d与d0大小选择避障策略从队形数据库选择避障队形，设d-d0＝k，当k＜0时，编队需要选择一字型编队通过障碍物；当k＝0时，编队中的机器人需要改变跟随者与领航者的跟随角度来缩小队形，减小队形的宽度，则编队可以顺利通过障碍物；当k＞0时，编队保持初始队形，即可通过障碍物。其中d0为初始编队队形宽度。

(4)当编队中的机器人最后一个通过障碍物时，改变编队队形需要从避障队形切换到初始队形，继续执行任务。

设避障后领航者位姿为：

p(t)＝[x1y1φ1]^t(5)

其中，(x1,y1)为领航者的坐标，φ1为领航者的方向角。

设队形快速恢复函数为：

p(t)＝(p0-p∞)e^-μt+p∞(6)

其中，μ＞0，p0为初始值即避障后虚拟领航者位置状态量，p∞为p(t)的稳态值即恢复队形后的位置状态量，μt为收敛系数，p(t)按指数快速递减到p∞的值，可实现队形以指数形式快速恢复。

(5)队形恢复，编队到达目标点。

本领域的普通技术人员将会意识到，这里所述的实施例是为了帮助读者理解本发明的实施方法，应被理解为本发明的保护范围并不局限于这样的特别陈述和实施例。本领域的普通技术人员可以根据本发明公开的这些技术启示做出各种不脱离本发明实质的其它各种具体变形和组合，这些变形和组合仍然在本发明的保护范围内。