多智能体系统编队算法仿真--python3实现

初始条件：

智能体位置随机生成
所有智能体位置全局可知
目标多边形位置给定
所有个体运行相同算法，根据环境来决定自己动作。

目标：形成均匀多边形分布，所谓的 ‘均匀’ 效果如下图：即是多边形上间距相等

问题拆分：

抵达均匀多边形
均匀化分布

1 .抵达均匀多边形：

'''code = 'utf-8'''
'''author = peng'''import copy
import numpy as np
import matplotlib.pyplot as plt
from matplotlib import animation
import timeNUM = 10  #设置无人机数量
MOVE_DISTANCE = 0.3
JianCeError = 0.1
'''根据无人机数量NUM得出边界最大容量数量   :    MAXNUM'''
if ((NUM - 4)/4)%1==0:MAXNUM = (NUM - 4)/4
else:MAXNUM = int((NUM - 4)/4) +1
'''JIANJU是调整单位距离'''
JIANJU = 50 /(MAXNUM+1)
x = np.random.randint(1, 100, NUM)
y = np.random.randint(1, 100, NUM)
# x = [36,37,38,39]
# y = [36,37,38,39]
Point_list = []
for i in range(NUM):Point_list.append([x[i], y[i]])
DING_LIST = [[25, 25], [75, 25], [75, 75], [25, 75]]
DingX, DingY = [], []
for each in DING_LIST:DingX.append(each[0])DingY.append(each[1])
DingX.append(DING_LIST[0][0])
DingY.append(DING_LIST[0][1])
fig, ax = plt.subplots()
ax.set_xlim(0, 100)
ax.set_ylim(0, 100)
sc = ax.scatter(x, y, color='r', alpha=0.7,marker='1',linewidth = 10)
ax.plot(DingX, DingY, color = 'black',linestyle = ':')class Point():MOVE_DISTANCE = MOVE_DISTANCEJianCeError = JianCeErrorMAXNUM = MAXNUMJIANJU = JIANJUtiaozheng_aim = Nonedef __init__(self,id):self.id = iddef decide(self,list = copy.deepcopy(DING_LIST)):if self.tiaozheng_aim == None:   #调整目标定下来就不需要改变了nearest = self.detect_nearest(list) #检测最近顶点ID = self.occupy(nearest)    #检测占领者if ID == self.id :self.update(nearest)pass       #自己占领elif ID == None:self.update(nearest)  #无人占领，往该方向移动else:# self.update([50,50])self.tiaozheng_aim = self.adjust(ID)  #调整目标if self.tiaozheng_aim:     #调整成功self.update(self.tiaozheng_aim)else:                      #调整失败# print(list)list2 = copy.deepcopy(list) #深复制防出错list2.remove(nearest)# print(list)return self.decide(list2)else:self.update(self.tiaozheng_aim) #有调整目标，直接移往该方向def adjust(self,ID):order = obj_list[ID].send() #1,0if order == None:return Nonefor each in DING_LIST:d = self.distance_calculate(each, Point_list[ID])if d < self.JianCeError:identity = DING_LIST.index(each)aim = copy.deepcopy(DING_LIST[identity])count = self.MAXNUM - order  #1,2if count % 2 == 0:  # 偶数顺时针if identity == 3:aim[0] += self.JIANJU * (count / 2)return aimelif identity == 2:aim[1] -= self.JIANJU * (count / 2)return aimelif identity == 1:aim[0] -= self.JIANJU * (count / 2)return aimelse:aim[1] += self.JIANJU * (count / 2)return aimelif identity == 3:  # 奇数逆时针aim[1] -= self.JIANJU * (int((count / 2))+1)return aimelif identity == 2:aim[0] -= self.JIANJU * (int((count / 2))+1)return aimelif identity == 1:aim[1] += self.JIANJU * (int((count / 2))+1)return aimelse:aim[0] += self.JIANJU * (int((count / 2))+1)return aimdef detect_nearest(self,list):init_distance = self.distance_calculate(Point_list[self.id], list[0])count, i = 0, 0for each in list:D = self.distance_calculate(Point_list[self.id], each)if D < init_distance:init_distance = Dcount = ii += 1return list[count]def distance_calculate(self, A, B):  # [1,1],[2,2] 得1.4142135623730951return pow(pow(abs(A[0] - B[0]), 2) + pow(abs(A[1] - B[1]), 2), 0.5)def update(self,aim):self_pot = copy.deepcopy(Point_list[self.id])x = np.array([aim[0] - self_pot[0], aim[1] - self_pot[1]])  # 方向向量y = np.array([1, 0])  # x轴方向Lx = np.sqrt(x.dot(x))  # x.dot(x) 点乘自己，相当于向量模平方Ly = np.sqrt(y.dot(y))if Lx > self.MOVE_DISTANCE:cos_angle = x.dot(y) / (Lx * Ly)angle = np.arccos(cos_angle)  # 0.....piif x[0] >= 0 and x[1] >= 0:self_pot[0] = self_pot[0] + self.MOVE_DISTANCE * abs(np.cos(angle))self_pot[1] = self_pot[1] + self.MOVE_DISTANCE * np.sin(angle)elif x[0] <= 0 and x[1] >= 0:self_pot[0] = self_pot[0] - self.MOVE_DISTANCE * abs(np.cos(angle))self_pot[1] = self_pot[1] + self.MOVE_DISTANCE * np.sin(angle)elif x[0] <= 0 and x[1] <= 0:self_pot[0] = self_pot[0] - self.MOVE_DISTANCE * abs(np.cos(angle))self_pot[1] = self_pot[1] - self.MOVE_DISTANCE * np.sin(angle)else:self_pot[0] = self_pot[0] + self.MOVE_DISTANCE * abs(np.cos(angle))self_pot[1] = self_pot[1] - self.MOVE_DISTANCE * np.sin(angle)Point_list[self.id] = self_potelse:Point_list[self.id] = aimdef occupy(self,nearest):for each in Point_list :d = self.distance_calculate(each,nearest)if d < self.JianCeError:ID = Point_list.index(each)return IDreturn Nonedef send(self):'''self.MAXNUM = 2 ,则输出 1,0'''if self.MAXNUM <= 0:return None  # 告诉询问着索要失败else:self.MAXNUM -= 1return self.MAXNUMobj_list = [Point(i) for i in range(0, NUM)]  # 返回生成的NUM个对象的列表comp_li = None
def gen():  # 绘图函数里面用的数据来源global comp_liwhile True:li = []for i in range(NUM):obj_list[i].decide()for each in Point_list:li.append(each)if comp_li == li:print('抵达边界完成，停留3秒')time.sleep(3)exit()else:comp_li = copy.deepcopy(li)with open('set.py','w') as f:f.write('POINT_LIST = '+ str(li))yield lidef update(N_list):sx, sy = [], []for each in N_list:sx.append(each[0])sy.append(each[1])sc.set_offsets(np.c_[sx, sy])return scani = animation.FuncAnimation(fig, update, frames=gen, interval=1)
plt.show()

初始分布随机生成：

抵达多边形后分布：

均匀化后分布：

均匀化算法思路：

class Point（）
def __init__(id):
self.id = id
def decide():
'''决策函数，执行后更改全局变量 POINT_LIST '''
return None

obj_list = [Point(i) for i in range(0, len(Point_list))] # 返回生成的NUM个对象的列表

while True:
'''依次执行每个个体的决策函数，更改自身位置，迭代器返回全局变量 POINT_LIST'''
for i in range(NUM):
obj_list[i].decide()
yield POINT_LIST

###############################################################################################
关于决策函数：
def decide（）：
   """ mmid 指前后邻点的中点与自己位置相连的中点 """
找到前后邻点，计算出 mmid =((pre + next)/2 + my)/2

'''移到目标点'''
move（mmid）

##################################################################################################
关于位置移动的处理：

idea：为了解决在顶点处移动无人机算法表达难度过大的问题提出将均匀多边形链表化的设想，即是把多边形等距划分，将每一个点位置存入链表中，移动无人机只需考虑其在链表上索引变动即可。

具体实现代码：

'''重写 13 '''
'''基本ok 只差停下函数'''
'''哇 终于TM的停下来了'''import copy
import numpy as np
import matplotlib.pyplot as plt
from matplotlib import animation
import time
from set import POINT_LIST
Move_Distance = 20   # 20 * 0.01 =0.2
Ting_Distance = 3
# POINT_LIST = [[41.66666666666667, 25], [25, 41.66666666666667], [41.66666666666667, 75], [75, 25], [25, 75], [75, 58.33333333333333], [75, 75], [58.33333333333333, 75], [25, 25], [75, 41.66666666666667], [25, 58.33333333333333], [58.33333333333333, 25]]
# Point_list = [[25, 50.0], [75, 43.75], [43.75, 25], [25, 75], [25, 43.75], [75, 68.75], [56.25, 25], [62.5, 75], [50.0, 25], [75, 62.5], [25, 68.75], [31.25, 75], [25, 25], [31.25, 25], [25, 31.25], [75, 50.0], [37.5, 25], [56.25, 75], [75, 25], [75, 75], [75, 31.25], [25, 62.5], [37.5, 75], [68.75, 25], [75, 37.5], [25, 37.5], [25, 56.25], [68.75, 75], [62.5, 25], [43.75, 75]]
# Point_list = [[25, 25], [75, 75], [25, 75], [75, 25], [50, 25]]
# Point_list = [[25, 43.75], [25, 56.25], [50.0, 25], [75, 37.5], [68.75, 75], [43.75, 75], [62.5, 25], [75, 43.75], [25, 75], [25, 25], [56.25, 25], [25, 68.75], [75, 50.0], [31.25, 75], [25, 62.5], [75, 68.75], [31.25, 25], [25, 31.25], [62.5, 75], [75, 62.5], [56.25, 75], [75, 56.25], [37.5, 25], [75, 25], [75, 31.25], [25, 37.5], [68.75, 25], [37.5, 75], [43.75, 25]]
Point_list = POINT_LIST
NUM = len(Point_list)
# print(NUM)
DING_LIST = [[25, 25], [75, 25], [75, 75], [25, 75]]
DingX, DingY, x, y = [], [], [], []
for each in DING_LIST:DingX.append(each[0])DingY.append(each[1])
for each in Point_list:x.append(each[0])y.append(each[1])
DingX.append(DING_LIST[0][0])
DingY.append(DING_LIST[0][1])
fig, ax = plt.subplots()
ax.set_xlim(0, 100)
ax.set_ylim(0, 100)
sc = ax.scatter(x, y, color='r', alpha=0.7,marker='1',linewidth = 10)
ax.plot(DingX, DingY, color = 'black',linestyle = ':')'''以间隔0.01生成齿轮链表'''def chain_make():Tooth_Chain = []Tooth_Chain.append([25, 25])for i in np.arange(25.01, 75, 0.01):Tooth_Chain.append([i, 25])Tooth_Chain.append([75, 25])for i in np.arange(25.01, 75, 0.01):Tooth_Chain.append([75, i])Tooth_Chain.append([75, 75])for i in np.arange(74.99, 25.0, -0.01):Tooth_Chain.append([round(i, 2), 75])Tooth_Chain.append([25, 75])for i in np.arange(74.99, 25, -0.01):Tooth_Chain.append([25, round(i, 2)])return Tooth_Chaindef distance_calculate(A, B):  # [1,1],[2,2] 得1.4142135623730951return pow(pow(abs(A[0] - B[0]), 2) + pow(abs(A[1] - B[1]), 2), 0.5)Tooth_Chain = chain_make()
Tooth_Len = len(Tooth_Chain)
Point_adindex = []
for a in Point_list:for b in Tooth_Chain:d = distance_calculate(a, b)if d <= 0.005:      # Point_list数据有问题a.append(Tooth_Chain.index(b))Point_adindex.append(a)
# print(len(Point_adindex))def takeThird(elem):return elem[2]Point_adindex_sort = copy.deepcopy(Point_adindex)
Point_adindex_sort.sort(key=takeThird)# print(len(Point_adindex_sort))class Point():next_dis = 200001def __init__(self, id):''' self.  pre_id    next_id     id  这三个是在Point_list中的位置'''self.id = idmy_id = Point_adindex_sort.index(Point_adindex[self.id])if my_id == 0:self.pre_id = Point_adindex.index(Point_adindex_sort[NUM - 1])self.next_id = Point_adindex.index(Point_adindex_sort[1])elif my_id == NUM - 1:self.next_id = Point_adindex.index(Point_adindex_sort[0])self.pre_id = Point_adindex.index(Point_adindex_sort[NUM - 2])else:self.pre_id = Point_adindex.index(Point_adindex_sort[my_id - 1])self.next_id = Point_adindex.index(Point_adindex_sort[my_id + 1])def decide(self):pre_chain_index = Point_adindex[self.pre_id][2]next_chain_index = Point_adindex[self.next_id][2]self_chain_index = Point_adindex[self.id][2]if pre_chain_index < next_chain_index:a = pre_chain_indexb = next_chain_indexelse:a = pre_chain_indexb = next_chain_index + 20000if abs(self_chain_index - (a+b)/2 ) < 100 : passelse:if pre_chain_index < next_chain_index:  # 正常情况self.next_dis = next_chain_index - self_chain_indexmmid = ((next_chain_index + pre_chain_index) / 2 + self_chain_index) / 2# print('pre:', pre_chain_index, ' ', 'self', self_chain_index, ' ', 'next:', next_chain_index)else:self.next_dis = next_chain_index - self_chain_index + 20000if self.next_dis>= 20000 :self.next_dis -= 20000mmid = ((next_chain_index + Tooth_Len + pre_chain_index) / 2 + self_chain_index) / 2# print('pre:', pre_chain_index, ' ', 'self', self_chain_index, ' ', 'next:', next_chain_index)if abs(mmid - self_chain_index) <= Ting_Distance:if mmid % 1 == 0:self.move(int(mmid))elif self_chain_index > mmid:  # 在目标顺市针方向self.move(int(mmid) + 1)else:self.move(int(mmid))elif mmid > self_chain_index:self.move(self_chain_index + Move_Distance)else:self.move(self_chain_index - Move_Distance)def move(self, aim):if aim >= Tooth_Len: aim -= Tooth_Lenli = copy.deepcopy(Tooth_Chain[aim])li.append(aim)Point_adindex[self.id] = lidef judge(list):d = 20000/NUMfor each in list :if abs(each - d) > 100:return Falsereturn Truedef gen():while True:# print('$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$')li = []# panduanls=[]# if vari > ? :for i in range(NUM):obj_list[i].decide()# panduanls.append(obj_list[i].next_dis)# else:continue# if judge(panduanls):#     print("均匀化分布算法执行完毕，停留3秒")#     time.sleep(3)#     exit()for each in Point_adindex: li.append(each[:-1])yield lidef update(N_list):sx, sy = [], []for each in N_list:sx.append(each[0])sy.append(each[1])sc.set_offsets(np.c_[sx, sy])return scobj_list = [Point(i) for i in range(0, len(Point_list))]  # 返回生成的NUM个对象的列表
ani = animation.FuncAnimation(fig, update, frames=gen, interval=2)plt.show()###均匀化代码需要数据POINT_LIST,可以用代码中注释掉的数据看效果。

编队过程中碰撞问题解决办法：高度错开

三维效果演示：

三维演示代码已上传。

多智能体均匀多边形编队_哔哩哔哩_bilibili
https://www.bilibili.com/video/BV1PE41117DG
GitHub - ColaForced/AgentsMotionSimulation: 多智能体均匀多边形编队、追逐与合围。