一种基于人工势场算法的多智能体集群避障方法

1.本发明属于智能体的路径规划技术领域，具体涉及一种基于人工势场算法的多智能体集群避障方法。


背景技术：

2.避障问题一直是路径规划领域中需要解决的重要问题，要想实现多智能体协同追击的目标，运动过程中的集群与避障问题的解决成为了一个重要的研究方向。目前关于多智能体集群与避障问题的研究主要集中在多无人机协同制导避障方面，并取得了一定的研究成果。但较少有关于多智能体方面的相关研究。而一些智能体存在一定的特殊需求，即施加在智能体上的控制力只能沿着其速度的法向方向，而在沿速度切向方向上没有控制力，这对避障算法提出了一定的要求。
3.无人机体积小、飞行灵活、易于控制，而本专利所针对的智能体是上述特殊需求的智能体，存在一定条件的约束，增加了相关研究的难度。
4.本专利所研究的智能体人工势场法是一种应用广泛的避障方法，其原理简单，易于实现。但多智能体的集群避障任务的实现不同于普通智能体的避障，主要体现在以下几个方面：
5.1.多智能体协同。在多智能体执行任务过程中，不同的智能体不是作为单独的个体，而是以集群的方式飞行，这就要求多个智能体在飞行过程中要始终保持一定的距离约束，既不能相距过远，同时智能体之间不能发生碰撞；
6.2.缺少控制力。本发明中所针对的智能体在避障过程中，无法提供沿智能体速度切向的控制力，因此智能体只能通过速度法向的控制力改变速度方向，进而避开障碍物；
7.3.人工势场法失效问题。由于缺少速度切向的控制力，当智能体、目标点、障碍物处于同一直线时，人工势场法会失效，导致智能体与障碍物碰撞。
8.基于以上几个问题，传统的人工势场算法无法直接应用于多智能体集群避障的实现，需要进一步改进。


技术实现要素：

9.本发明的目的在于提出一种单纯施加法向控制力的三维人工势场算法，来实现多智能体集群避障任务，使得智能体集群能够在行进过程中躲避障碍物，成功到达目标点。
10.本发明所采取的技术方案是：一种基于人工势场算法的多智能体集群避障方法，包括以下步骤：
11.s1.明确初始信息，包括地图信息和智能体初始信息；
12.s2.设计人工势场函数；
13.s3.利用坐标转换，设计单纯施加法向控制力的人工势场算法；
14.s4.增加一个速度负反馈；
15.s5.进行仿真试验。
16.本发明与现有技术相比具有以下有益效果：
17.本发明对传统的人工势场法做出了改进，设计的虚拟中心引力势场和智能体斥力势场的方法实现了多智能体集群的距离约束控制；设计的单纯施加法向控制力的人工势场算法实现了缺少速度切向方向控制力的智能体的避障问题的解决；设计的控制力中增加附加系数的方法解决了特殊情况下人工势场算法失效的问题。实现了在缺少速度切向控制力的情况下，能够完成多智能体集群避障飞行，同时解决了在特殊情况下人工势场法可能会失效的问题。
附图说明
18.图1是本发明避障方法的算法设计流程图；
19.图2是人工势场法原理示意图；
20.图3是智能体受力示意图；
21.图4是智能体集群受力示意图；
22.图5是地面坐标系与弹道坐标系间的关系；
23.图6是智能体集群仿真结果；
24.图7是各智能体之间距离；
25.图8是虚拟中心与各智能体距离；
26.图9是智能体与障碍物间距离。
具体实施方式
27.为了更好地了解本发明的目的、结构及功能，下面结合附图，对本发明的做进一步详细的描述。
28.本发明的算法流程为：在未遇到障碍物时，多枚智能体通过所受控制力的作用，彼此之间保持一定距离约束，作为一个集群朝向目标点运动；在遇到障碍物时，由于障碍物斥力的作用，智能体各自绕开障碍物，此时集群间距增大；在避开障碍物后，智能体受虚拟中心引力的作用重新保持距离约束，继续朝向目标点运动。
29.具体参照图1所示，本发明的一种基于人工势场算法的多智能体集群避障方法，包括以下步骤：
30.s1、明确初始信息。包括地图信息和智能体初始信息，具体包括：目标点坐标qg，障碍物个数及其坐标qr，智能体个数及其起始坐标q，各智能体初速度v0。
31.s2、设计人工势场函数。
32.根据人工势场算法原理，设计各部分势场函数u，并根据势场函数的负梯度，即得到各部分对应的势场力f。
33.如图2所示，人工势场法的原理是在智能体的运动空间中人为构造一个虚拟势场，该势场由目标点产生的引力势场和空间中障碍物产生的斥力势场组成。
34.在本发明中，智能体即为智能体，智能体在空间中受到目标点引力，从而朝向目标点运动，同时在遇到障碍物时，由于障碍物斥力作用绕开障碍物，实现避障。
35.受力示意图如图3所示。
36.为了实现对多智能体的距离约束控制，除了设计传统人工势场算法所具有的目标
点引力势场和障碍物斥力势场外，新增虚拟中心引力势场和智能体斥力势场。
37.虚拟中心为所有智能体坐标的平均坐标，位于智能体集群的中心处，是一个认为创造的虚拟点，该点产生一个引力势场，使得所有智能体均有向该虚拟中心运动的趋势。
38.同时为每个智能体设置一个斥力势场，由此，每个智能体相对于其他智能体而言是一个动态障碍物，当智能体靠近其他智能体时会受斥力作用而远离。
39.这样，在只考虑智能体集群的情况下，单个体智能体在朝向集群中心运动的同时，由于受到其他智能体斥力的原因，又不会与其他智能体相碰，从而实现了智能体集群的距离约束。
40.受力示意图如图4所示。
41.综上因此所需要设计的人工势场共包括四部分：目标点引力势场函数u
att
、障碍物斥力势场函数u
rep
、虚拟中心引力势场函数u
attmid
、智能体斥力势场函数u
repi
；
42.人工势场算法中的人工势场函数设计步骤如下：
43.s21.目标点引力势场函数u
att
：
44.目标点对智能体有引力作用，且智能体距目标点的距离越大，对应的引力势能越大，距离越小，对应的引力势能越小，当智能体于目标点足够接近时，引力势能为零，认为智能体到达目标点。由此设计出的目标点的引力势场函数为：
45.u
att
＝k
att
×
(q-qg)2/2；
46.其中：u
att
为目标点的引力势场；k
att
为引力系数；q为当前智能体的位置；qg为目标点的位置。
47.由公式可以看到，引力势场u
att
与智能体和目标点之间的距离(q-qg)成平方关系，引力势场随着智能体和目标点之间的距离增加而单调递增，方向为智能体与目标点的连线指向目标点。引力系数k
att
决定引力势场的比例增益，可以通过人为调整来改变引力势场的强弱。
48.相应的吸引力f
att
为引力势场u
att
的负梯度，是引力势场u
att
对智能体和目标点之间的距离的导数，即：
[0049][0050]
由上式可知，引力函数f
att
随着智能体和目标点之间的距离增加而单调递增，方向为智能体与目标点连线指向目标点。
[0051]
s22.障碍物斥力势场
[0052]
智能体运动中受到的来自障碍物的斥力同样与距离有关：智能体与障碍物距离越近，受到的斥力越大，距离越远，斥力越小，以此来保证不会与障碍物相撞。由此设计的障碍物斥力势场函数为：
[0053][0054]
其中，k
rep
为斥力系数；qr为障碍物的位置；dr为障碍物的斥力影响距离。当智能体运动过程中进入障碍物的斥力影响范围时，会受到比较大的斥力影响，当智能体离开障碍物的斥力影响范围后，障碍物对其的斥力为零，由此避免距离智能体较远的障碍物对其运动产生影响。
[0055]
相应的斥力为斥力势场u
rep
的负梯度。即：
[0056][0057]
由上式可知，斥力函数f
rep
随着智能体和障碍物之间的距离减小而单调递增，方向为智能体与目标点连线指向智能体。
[0058]
s23.虚拟中心引力势场函数
[0059]
同目标点的引力势场一样，智能体距离虚拟中心越近，所受引力越小，距离越远，所受引力越大。得到虚拟中心的引力势场函数为：
[0060]uattmid
＝k
att
×
(q-q
mid
)2/2；
[0061]
其中：u
attmid
为虚拟中心的引力场；k
att
为引力系数；q为当前智能体的位置；q
mid
为虚拟中心的位置。
[0062]
虚拟中心取为所有智能体的中心点，即：
[0063][0064]
对应的受到虚拟中心的引力为：
[0065][0066]
s24.智能体斥力势场函数
[0067]
同障碍物斥力势场一样，智能体距离其他智能体越近，所受斥力越大，距离越小，所受斥力越小。由此得到智能体的斥力势场函数：
[0068][0069]urepi
为智能体斥力势场，qi为施加斥力的智能体的位置；d0为智能体的影响距离。
[0070]
斥力f
repi
由斥力势场函数的负梯度得到：
[0071][0072]
s25.优先距离约束任务的改进
[0073]
多智能体在运动过程中受到目标点的引力与单智能体人工势场法设计中的引力相同，但多智能体的运动增加了距离约束，因此多智能体向虚拟中心运动的任务的优先级应高于向目标运动，即当多智能体运动过程中存在智能体与虚拟中心的距离大于某一值时，需要减小目标点对智能体的引力，使智能体优先满足距离约束。
[0074]
即智能体受到目标点的引力为：
[0075][0076]
其中d为设定的优先进行距离约束的距离。
[0077]
s3、单纯施加法向控制力的人工势场法设计(利用坐标转换，将依照s2人工势场算
法所得到的控制力转换为单纯施加法向控制力的人工势场算法)。
[0078]
为了方便三维人工势场法的设计，需要利用坐标转换。最终的设计需求是将沿智能体速度方向的控制力改为0，只施加法向的控制力，因此引入地面坐标系和弹道坐标系，下面给出定义：
[0079]
1.地面坐标系axyz
[0080]
地面坐标系axyz是与地球表面固连的坐标系。坐标系原点a通常选取在智能体运动起始点上(即运动起始瞬时智能体的质心上)；ax轴指向可以是任意的；ay轴沿垂线向上，az轴与其他两轴垂直并构成右手坐标系。
[0081]
地面坐标系作为惯性参考系，主要用来确定智能体质心在空间的坐标位置和智能体在空间的姿态等参考基准。
[0082]
2.弹道坐标系ox2y2z2[0083]
弹道坐标系的原点o取在智能体的瞬时质心上；ox2轴与智能体速度矢量v重合；oy2轴位于包含速度矢量v的铅垂面内垂直于ox2轴，指向上为正；oz2轴垂直于其他两轴并构成右手坐标系。弹道坐标系于智能体速度矢量v固连，是动坐标系。
[0084]
设计思路是将在地面坐标系下的控制力矢量的坐标表示转换到弹道坐标系的坐标表示，并将ox2轴上(即沿速度矢量v方向上)的分量设为0，再将新的坐标转换到地面坐标系下。为此需要地面坐标系和弹道坐标系的坐标转换(如图5所示)，引入两个角度：
[0085]
1.弹道倾角θ：
[0086]
智能体的速度矢量v(即ox2轴)与水平面间的夹角。速度矢量指向水平面上方，θ角为正；反之为负。
[0087]
2.弹道偏角
[0088]
智能体速度矢量v在水平面内投影(即图5中ax')与地面坐标系的ax轴间的夹角。迎角平面(即迎ay轴俯视)观察，若由ax轴至ax'轴是逆时针旋转，则角为正；反之为负。
[0089]
地面坐标系到弹道坐标系的转换的矩阵形式表示为：
[0090][0091]
其中
[0092]
l(θ)和l(φv)分别为由地面坐标系转换到弹道坐标系的两步旋转的旋转矩阵。
[0093]
由此得到人工势场法的控制力在弹道坐标系上的坐标表示为：
[0094]
[0095]
其中，为控制力在地面坐标系的坐标表示，为控制力在弹道坐标系上的坐标表示。
[0096]
将沿速度矢量方向的控制力分量f
x2
设为0，得到新的弹道坐标系的坐标表示
[0097]
从而得到改进后的人工势场法的控制力：
[0098][0099]
在某些特殊情况，可能出现控制力恰好与速度矢量方向重合的情况，此时采用单纯法向控制力的人工势场法会导致人工势场法失效，因此增加一个附加系数k将改进后的人工势场法更新为：
[0100][0101]
s4、为增加智能体运行稳定性，为控制力部分增加一个速度负反馈，最终智能体运动过程中所受合力为：
[0102]
f＝f
att
f
rep
f
attmid
f
repi-kv*v；
[0103]
其中，v为智能体速度，kv为速度反馈系数。
[0104]
s5、进行仿真试验。运行仿真程序，得到智能体集群运动结果。
[0105]
实施例：
[0106]
下面通过介绍一个具有一定代表性的实施例，来进一步说明本发明技术方案的相关设计。
[0107]
s1、明确初始信息。
[0108]
目标点坐标为qg＝(10 10 10)(km)；
[0109]
障碍物个数为5个，坐标分别为q
r1
＝(4.0 3.3 4.0)(km)，q
r2
＝(4.0 4.0 3.0)(km)，q
r3
＝(4.4 4.2 3.4)(km)，q
r4
＝(3.6 4.6 4)(km)，q
r1
＝(2.5 3.0 3.5)(km)；
[0110]
智能体个数为3个，起始坐标分别为q1＝(0 0 1.0)(km)，q2＝(0 1.0 0)(km)，q3＝(1.0 0 0)(km)；
[0111]
各智能体初速度相同，均为v0＝(1 1 1)(km/s)；
[0112]
s2、设计人工势场函数。
[0113]
s21.目标点引力势场函数u
att
：
[0114]
设引力系数k
att
＝2；
[0115]
则目标点的引力势场函数为：
[0116]uatt
＝(q-qg)2；
[0117]
目标点引力为：
[0118]fatt
＝-2(q-qg)；
[0119]
s22.障碍物斥力势场：
[0120]
设斥力系数为k
rep
＝0.5，障碍物斥力影响距离为dr＝2(km)；
[0121]
则障碍物斥力势场函数为：
[0122][0123]
障碍物斥力为：
[0124][0125]
s23.虚拟中心引力势场函数：
[0126]
引力系数不变，为k
att
＝2；
[0127]
虚拟中心引力势场函数为：
[0128]uattmid
＝(q-q
mid
)2；
[0129]
虚拟中心引力为：
[0130]fattmid
＝-2(q-q
mid
)；
[0131]
s24.智能体斥力势场函数：
[0132]
斥力系数不变，为k
rep
＝0.5，智能体斥力影响距离为d0＝2(km)；
[0133]
则智能体斥力势场函数为：
[0134][0135]
智能体斥力为：
[0136][0137]
s25.优先距离约束任务的改进：
[0138]
设优先进行距离约束任务的距离d＝0.5(km)；
[0139]
则智能体受到目标点的引力为：
[0140][0141]
s3、单纯施加法向控制力的人工势场法设计。
[0142]
设附加系数k＝0.001；
[0143]
则更新后的控制力为：
[0144][0145]
s4、为增加智能体运行稳定性，为控制力部分增加一个速度负反馈。
[0146]
设速度反馈系数kv＝5；
[0147]
则最终智能体运动过程中所受合力为：
[0148]
f＝f
att
f
rep
f
attmid
f
repi-5*v
[0149]
根据以上算法设计，运行仿真程序，得到智能体集群运动结果如图6、图7、图8、图9所示。
[0150]
可以理解，本发明是通过一些实施例进行描述的，本领域技术人员知悉的，在不脱离本发明的精神和范围的情况下，可以对这些特征和实施例进行各种改变或等效替换。另外，在本发明的教导下，可以对这些特征和实施例进行修改以适应具体的情况及材料而不会脱离本发明的精神和范围。因此，本发明不受此处所公开的具体实施例的限制，所有落入本技术的权利要求范围内的实施例都属于本发明所保护的范围内。