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

初始条件：

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

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

问题拆分：

1. 抵达均匀多边形
2. 均匀化分布

1 .抵达均匀多边形：

'''code = 'utf-8'''
'''author = peng'''
 
import copy
import numpy as np
import matplotlib.pyplot as plt
from matplotlib import animation
import time
 
NUM = 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_DISTANCE
    JianCeError = JianCeError
    MAXNUM = MAXNUM
    JIANJU = JIANJU
    tiaozheng_aim = None
    def __init__(self,id):
        self.id = id
    def 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,0
        if order == None:return None
        for 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,2
        if count % 2 == 0:  # 偶数顺时针
            if identity == 3:
                aim[0] += self.JIANJU * (count / 2)
                return aim
            elif identity == 2:
                aim[1] -= self.JIANJU * (count / 2)
                return aim
            elif identity == 1:
                aim[0] -= self.JIANJU * (count / 2)
                return aim
            else:
                aim[1] += self.JIANJU * (count / 2)
                return aim
        elif identity == 3:  # 奇数逆时针
            aim[1] -= self.JIANJU * (int((count / 2))+1)
            return aim
        elif identity == 2:
            aim[0] -= self.JIANJU * (int((count / 2))+1)
            return aim
        elif identity == 1:
            aim[1] += self.JIANJU * (int((count / 2))+1)
            return aim
        else:
            aim[0] += self.JIANJU * (int((count / 2))+1)
            return aim
 
    def detect_nearest(self,list):
        init_distance = self.distance_calculate(Point_list[self.id], list[0])
        count, i = 0, 0
        for each in list:
            D = self.distance_calculate(Point_list[self.id], each)
            if D < init_distance:
                init_distance = D
                count = i
            i += 1
        return list[count]
    def distance_calculate(self, A, B):  # [1,1],[2,2] 得1.4142135623730951
        return 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.....pi
            if 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_pot
        else:
            Point_list[self.id] = aim
    def occupy(self,nearest):
        for each in Point_list :
            d = self.distance_calculate(each,nearest)
            if d < self.JianCeError:
                ID = Point_list.index(each)
                return ID
        return None
    def send(self):
        '''self.MAXNUM = 2 ,则输出 1,0'''
        if self.MAXNUM <= 0:
            return None  # 告诉询问着索要失败
        else:
            self.MAXNUM -= 1
            return self.MAXNUM
 
obj_list = [Point(i) for i in range(0, NUM)]  # 返回生成的NUM个对象的列表
 
comp_li = None
def gen():  # 绘图函数里面用的数据来源
    global comp_li
    while 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 li
 
def 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 sc
 
ani = 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_Chain
 
 
def distance_calculate(A, B):  # [1,1],[2,2] 得1.4142135623730951
    return 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 = 200001
    def __init__(self, id):
        ''' self.  pre_id    next_id     id  这三个是在Point_list中的位置'''
        self.id = id
        my_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_index
            b = next_chain_index
        else:
            a = pre_chain_index
            b = next_chain_index + 20000
        if abs(self_chain_index - (a+b)/2 ) < 100 : pass
        else:
 
 
            if pre_chain_index < next_chain_index:  # 正常情况
                self.next_dis = next_chain_index - self_chain_index
                mmid = ((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 + 20000
                if self.next_dis>= 20000 :
                    self.next_dis -= 20000
                mmid = ((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_Len
        li = copy.deepcopy(Tooth_Chain[aim])
        li.append(aim)
        Point_adindex[self.id] = li
 
def judge(list):
    d = 20000/NUM
    for each in list :
        if abs(each - d) > 100:
            return False
    return True
 
 
def 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 li
 
 
def 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 sc
 
 
 
obj_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,可以用代码中注释掉的数据看效果。

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

三维效果演示：

三维演示代码已上传。

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