程序员经验分享-hwhale

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

2023-05-16

初始条件：

  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: 多智能体均匀多边形编队、追逐与合围。

本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)

多智能体系统编队算法仿真--python3实现 的相关文章

  • windows平台相关命令收集

    1 获取IP地址命令 xff1a ipconfig 2 获取MAC地址命令 xff1a ipconfig all 3 查看所有端口信息 netstat nal findstr 8080

  • C/C++中比较好用的HTTP开源库

    以下是一些常用的C C 43 43 开源HTTP库及其对应的开源协议和链接 xff1a 1 libcurl xff1a 使用MIT X开源协议 xff0c 支持多种协议和多种数据传输方式 xff0c 被广泛应用于各种开发场景 官网链接 xf

  • 为Ubuntu网页设置稳定的数据隧道

    网站作为互联网世界中的一个个门户 xff0c 是我们对外表达的窗口 xff0c 无数个人和企业通过这个窗口传递着信息 xff0c 这个传递信息的窗口必须稳定持续存在 xff0c 才能让更多访客看到 xff0c 为我们带来更多的流量 而使用c

  • VisualStudio配置与Matlab混合编译问题总结

    VisualStudio配置与Matlab混合编译问题总结 写这篇博客记录一下自己在配置过程中遇到的种种问题 xff0c 也做一个小小总结 xff0c 试图把整个过程讲明白 xff0c 希望对面临同样问题的人有所帮助 网上此类教程很多 xf

  • C++与C的区别（作业）

    头文件的区别 1 C 43 43 中创建源文件后缀为 cpp xff0c C语言中创建源文件后缀为 c xff0c 头文件保持不变 xff0c 后缀为 h 2 包含自己的头文件 xff0c C语言与C 43 43 无区别 包含库目录 xff

  • C++和C语言区别（二）附二级指针的动态内存申请

    结构体区别 1 定义结构体与C语言一致 2 定义变量可省略关键字struct 3 C 43 43 结构体中允许函数存在 xff08 C 43 43 在没有写构造函数和权限限定的时候 xff0c 用法和C语言一致 xff09 xff08 1

  • 自制芯片 http转串口通信模块实现手机控制加红外控制51单片机实现双控制 28BYJ-48步进电机( 13003步进电机驱动器 STC89C52RC ESP8266)

    首先你有这样的8266 这种8266自身带2个按键和烧录芯片方便调试 xff0c 综合性价比较高 还需要有一个51单片机 或者其他单片机都行 还有就是需要这样的13003步进电机驱动器 43 28BYJ 48步进电机和红外接收头以及遥控器

  • C++类和对象（附C语言与C++链表）

    类和对象的基本概念 1 类 xff1a 一系列事物的抽象 xff0c 万物皆可为类 xff08 1 xff09 属性 xff1a 事物的特征 数据类型的描述 xff08 2 xff09 行为 xff1a 事物的操作 函数描述 2 对象 xf

  • C++类的组合

    1 以另一个类的对象为数据成员 xff0c 必须采用初始化参数列表的写法 include lt iostream gt include lt string gt using namespace std class Boy public Bo

  • C++模板

    函数模板 1 把类型当作未知量 2 语法 template lt typename Ty gt Ty function Ty a Ty b return a gt b a b template lt class Ty1 class Ty2

  • C++STL容器（一）

    定长数组 include lt iostream gt include lt string gt include lt array gt using namespace std template lt class Ty size t siz

  • C++STL容器（二）

    链表 include lt iostream gt include lt string gt include lt list gt include lt functional gt 仿函数less和greater头文件 using name

  • C++正则表达式

    regex match regex replace regex search include lt iostream gt include lt regex gt include lt string gt using namespace s

  • C++时间管理和随机数

    时间段 include lt chrono gt duration TYPEDEFS typedef duration lt long long nano gt nanoseconds 纳秒 typedef duration lt long

  • C++文件系统

    filesystem include lt filesystem gt include lt iostream gt using namespace std int main 打开文件夹 filesystem path url 34 fil

  • arduino之旅——L298N驱动小车前进后退

    今天开始组装小车 xff0c 但是把小车底板 xff0c arduino mega 2560芯片 xff0c L298N xff0c 减速电机 xff0c 还有电源相应地连接好后 xff0c 从网上找了如下程序 xff08 源程序来自 xf

  • ODROID XU4 内核编译

    准备材料 一个64位的Linux系统 一份内核源码 交叉编译工具链 添加交叉编译工具链 下载链接 sudo mkdir opt toolchains sudo cp arm eabi 4 6 tar gz opt toolchains cd

  • 红外遥控器解码串口输出模块结合51单片机+oled屏幕实现遥控器红外解锁( STC89C52RC)

    需要 1 单片机 2 红外遥控器解码串口输出模块 3 oled屏幕 4 电子锁 5 继电器 有了这些我们开始吧 xff01 1 先看一段视频效果演示 xff0c 再来介绍实现步骤 红外遥控器解码串口输出模块结合51单片机 43 oled屏幕

  • 关于STM32串口收发，以及数据类型的任意转换及识别字符命令进行相应赋值。**

    关于STM32串口收发 xff0c 以及数据类型的任意转换及识别字符命令进行相应赋值 1 关于STM32串口收发 xff0c 以及数据类型的任意转换及识别字符命令进行响应赋值 对于STM32而言 xff0c 串口这个外设是十分重要的 xff

  • WPS-Word : 页首的段落段前间距失效/不起作用/无效，段前一行无效没反应

    一 问题描述 下面的方案选择与结构设计 xff0c 段前间距是24磅 xff0c 但是实际排版情况却是0磅 二 解决思路 我这里参考了文章 他的原文如下引用部分 1 前一页的分页符改成分节符 2 这是因为第一个标题与上一页有分节 xff0c

随机推荐

热门标签