├── .idea
├── .gitignore
├── AgentsMotionSimulation.iml
├── csv-plugin.xml
├── inspectionProfiles
│ ├── Project_Default.xml
│ └── profiles_settings.xml
├── misc.xml
└── modules.xml
├── Done
└── plgsim.py
├── ToDo
├── agent.py
├── main.py
└── polygon.py
├── pics
├── 图2、高度错开性避免碰撞策略流程图.png
└── 基于自组织的均匀多边形编队仿真.gif
└── readme.md
/.idea/.gitignore:
--------------------------------------------------------------------------------
1 | # Default ignored files
2 | /shelf/
3 | /workspace.xml
4 |
--------------------------------------------------------------------------------
/.idea/AgentsMotionSimulation.iml:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 |
5 |
6 |
7 |
8 |
--------------------------------------------------------------------------------
/.idea/csv-plugin.xml:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 |
29 |
30 |
--------------------------------------------------------------------------------
/.idea/inspectionProfiles/Project_Default.xml:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 |
5 |
10 |
11 |
12 |
18 |
19 |
20 |
--------------------------------------------------------------------------------
/.idea/inspectionProfiles/profiles_settings.xml:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 |
5 |
6 |
--------------------------------------------------------------------------------
/.idea/misc.xml:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 |
--------------------------------------------------------------------------------
/.idea/modules.xml:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 |
5 |
6 |
7 |
8 |
--------------------------------------------------------------------------------
/Done/plgsim.py:
--------------------------------------------------------------------------------
1 | import copy
2 | import time
3 |
4 | import matplotlib.pyplot as plt
5 | import numpy as np
6 | from matplotlib import animation
7 |
8 | """无人机设置"""
9 | n = 8 # 个数
10 |
11 | """多边形参数设置"""
12 | m = 5 # 边数
13 | r = 30 # 多边形半径
14 | cx, cy, cz = 50, 50, 200 # 中心位置
15 |
16 | """高度错开收敛所需参数"""
17 | hv = 2 # 高度错开收敛时，纵轴运动速度
18 | hgap = 380 / n
19 |
20 | """抵达边界所需参数"""
21 | step_len = 0.3 # 移动步长
22 | Detect_Error = 0.1 # 允许误差
23 |
24 | """边界目标位置调整所需参数"""
25 | max_n = (n - m) / m if ((n - m) / m) % 1 == 0 else int((n - m) / m) + 1
26 | unit_gap = 2 * r * np.sin(np.pi / m) / (max_n + 1) # 2 * R * np.sin(np.pi/n)是边界长度， unit_gap是调整单位距离
27 |
28 | """均匀化参数"""
29 | Stop_Gap_NUM = 3 # 齿链上，若所有结点两两之间仅仅相小于3个节点，则判断为编队完成
30 | Move_Gap_Num = 20 # 20 * 0.01 =0.2 齿链上，步长转换成节点数
31 |
32 | """随机生成无人机初始位置,记录为Point_List"""
33 | pxs = np.random.randint(1, 100, n)
34 | pys = np.random.randint(1, 100, n)
35 | pzs = [0 for i in range(n)]
36 | Point_List = [[pxs[i], pys[i], 0] for i in range(n)]
37 |
38 | """ 计算多边形顶点位置"""
39 | Vertexs_List = [[50 + r * np.sin(i * 2 * np.pi / m), 50 + r * np.cos(i * 2 * np.pi / m), cz] for i in range(1, m + 1)]
40 | verxs, verys, verzs = [v[0] for v in Vertexs_List], [v[1] for v in Vertexs_List], [v[2] for v in Vertexs_List]
41 | verxs.append(Vertexs_List[0][0])
42 | verys.append(Vertexs_List[0][1])
43 | verzs.append(Vertexs_List[0][2])
44 |
45 | """初始化图像"""
46 | fig = plt.figure()
47 | ax = plt.gca(projection='3d')
48 | ax.set_xlim(0, 100)
49 | ax.set_xlabel('X')
50 | ax.set_ylim(0, 100)
51 | ax.set_ylabel('Y')
52 | ax.set_zlim(0, 400)
53 | ax.set_zlabel('Z')
54 | sc = ax.scatter3D(pxs, pys, pzs, color='r', alpha=0.7)
55 | ax.plot(verxs, verys, verzs, color='black', linestyle=':')
56 |
57 | """均匀化时,链表化节点参数"""
58 | d = 2 * r * np.sin(np.pi / m)
59 | num = int(d / 0.01)
60 | tooth_distance = d / num
61 | Vertexs_Tooth_Chain = []
62 |
63 | """链表化多边形"""
64 | for i in range(0, m):
65 | Vertexs_Tooth_Chain.append(Vertexs_List[i])
66 | base_pos = copy.deepcopy(Vertexs_List[i])
67 | if i == len(Vertexs_List) - 1:
68 | nxt = Vertexs_List[0]
69 | else:
70 | nxt = Vertexs_List[i + 1]
71 | x = np.array([nxt[0] - Vertexs_List[i][0], nxt[1] - Vertexs_List[i][1]]) # 方向向量
72 | y = np.array([1, 0]) # x轴方向
73 | xmd = np.sqrt(x.dot(x)) # x.dot(x) 点乘自己，相当于向量模平方
74 | ymd = np.sqrt(y.dot(y))
75 | cos_angle = x.dot(y) / (xmd * ymd)
76 | angle = np.arccos(cos_angle)
77 | if x[0] >= 0 and x[1] >= 0:
78 | for j in range(1, num):
79 | a = base_pos[0] + j * tooth_distance * abs(np.cos(angle))
80 | b = base_pos[1] + j * tooth_distance * abs(np.sin(angle))
81 | Vertexs_Tooth_Chain.append([a, b, cz])
82 | elif x[0] <= 0 and x[1] >= 0:
83 | for j in range(1, num):
84 | a = base_pos[0] - j * tooth_distance * abs(np.cos(angle))
85 | b = base_pos[1] + j * tooth_distance * abs(np.sin(angle))
86 | Vertexs_Tooth_Chain.append([a, b, cz])
87 | elif x[0] <= 0 and x[1] <= 0:
88 | for j in range(1, num):
89 | a = base_pos[0] - j * tooth_distance * abs(np.cos(angle))
90 | b = base_pos[1] - j * tooth_distance * abs(np.sin(angle))
91 | Vertexs_Tooth_Chain.append([a, b, cz])
92 | else:
93 | for j in range(1, num):
94 | a = base_pos[0] + j * tooth_distance * abs(np.cos(angle))
95 | b = base_pos[1] - j * tooth_distance * abs(np.sin(angle))
96 | Vertexs_Tooth_Chain.append([a, b, cz])
97 |
98 | """------------------------分割线---------------------------------------------"""
99 | """由于智能体策略相对复杂，一个类写下所有状态下相应决策，代码过长，便按照当前状态的不同策略拆分成两个类"""
100 |
101 |
102 | class Point_1():
103 | """第一阶段智能体模型表示: 高度错开 + 移动到多边形上"""
104 |
105 | def __init__(self, id):
106 | self.id = id
107 | self.random = np.random.random() # 随机数，用于高度错开
108 | self.aim_hight = None # 当前阶段运动目标位置
109 | self.aim_adjusted = None # 多边形顶点位置被占领后，重新确定的目标
110 | self.max_n = max_n
111 |
112 | def decide0(self):
113 | """抵达目标高度"""
114 | my_pos = copy.deepcopy(Point_List[self.id])
115 | if abs(cz - my_pos[2]) < hv:
116 | Point_List[self.id][2] = cz
117 | else:
118 | aim = my_pos[2] + hv
119 | Point_List[self.id][2] = aim
120 |
121 | def decide1(self):
122 | """高度错开"""
123 | if self.aim_hight == None:
124 | li1 = [[Obj_List[i].send_random(), i] for i in range(n)]
125 |
126 | li1.sort(key=lambda x: x[0])
127 | my_index = li1.index([self.random, self.id])
128 | self.aim_hight = 10 + (my_index + 1) * hgap # 因为my_index从0 开始取
129 | # 有目标高度后开始更新位置
130 | my_pos = copy.deepcopy(Point_List[self.id])
131 | if abs(my_pos[2] - self.aim_hight) < hv:
132 | Point_List[self.id][2] = self.aim_hight
133 | elif my_pos[2] < self.aim_hight:
134 | Point_List[self.id][2] = my_pos[2] + hv
135 | else:
136 | Point_List[self.id][2] = my_pos[2] - hv
137 |
138 | def decide2(self, list=copy.deepcopy(Vertexs_List)):
139 | """抵达多边形主要逻辑： 抵达顶点 + 顶点被占领后调整目标位置"""
140 | if self.aim_adjusted == None:
141 | nearest = self.detect_nearest(list) # 检测最近顶点
142 | idx = self.occupy(nearest)
143 | if idx == self.id:
144 | self.update(nearest)
145 | elif idx == None:
146 | self.update(nearest)
147 | else:
148 | self.aim_adjusted = self.adjust(idx)
149 | if self.aim_adjusted: # 调整成功
150 | self.update(self.aim_adjusted)
151 | else:
152 | list2 = copy.deepcopy(list)
153 | list2.remove(nearest)
154 | return self.decide2(list2)
155 | else:
156 | self.update(self.aim_adjusted)
157 |
158 | def send_random(self):
159 | return self.random
160 |
161 | def detect_nearest(self, list):
162 | """检测最近的多边形顶点"""
163 | init_distance = self.distance_calculate(Point_List[self.id], list[0])
164 | count, i = 0, 0
165 | for each in list:
166 | D = self.distance_calculate(Point_List[self.id], each)
167 | if D < init_distance:
168 | init_distance = D
169 | count = i
170 | i += 1
171 | return list[count]
172 |
173 | def distance_calculate(self, A, B): # [1,1,?],[2,2,?] 得1.4142135623730951
174 | return pow(pow(abs(A[0] - B[0]), 2) + pow(abs(A[1] - B[1]), 2), 0.5)
175 |
176 | def occupy(self, nearest):
177 | """检测是谁占领的该多边形顶点，返回其id"""
178 | for each in Point_List:
179 | d = self.distance_calculate(each, nearest)
180 | if d < Detect_Error:
181 | id = Point_List.index(each)
182 | return id
183 | return None
184 |
185 | def update(self, aim):
186 | """朝着目标位置运动，更新坐标"""
187 | self_pot = copy.deepcopy(Point_List[self.id])
188 | x = np.array([aim[0] - self_pot[0], aim[1] - self_pot[1]]) # 方向向量
189 | y = np.array([1, 0]) # x轴方向
190 | xmd = np.sqrt(x.dot(x)) # x.dot(x) 点乘自己，相当于向量模平方
191 | ymd = np.sqrt(y.dot(y))
192 | if xmd > step_len:
193 | cos_angle = x.dot(y) / (xmd * ymd)
194 | angle = np.arccos(cos_angle) # 0.....pi
195 | if x[0] >= 0 and x[1] >= 0:
196 | self_pot[0] = self_pot[0] + step_len * abs(np.cos(angle))
197 | self_pot[1] = self_pot[1] + step_len * np.sin(angle)
198 | elif x[0] <= 0 and x[1] >= 0:
199 | self_pot[0] = self_pot[0] - step_len * abs(np.cos(angle))
200 | self_pot[1] = self_pot[1] + step_len * np.sin(angle)
201 | elif x[0] <= 0 and x[1] <= 0:
202 | self_pot[0] = self_pot[0] - step_len * abs(np.cos(angle))
203 | self_pot[1] = self_pot[1] - step_len * np.sin(angle)
204 | else:
205 | self_pot[0] = self_pot[0] + step_len * abs(np.cos(angle))
206 | self_pot[1] = self_pot[1] - step_len * np.sin(angle)
207 | Point_List[self.id] = self_pot
208 | else:
209 | Point_List[self.id][0] = aim[0]
210 | Point_List[self.id][1] = aim[1]
211 |
212 | def adjust(self, idx):
213 | """多边形顶点被占领后，向占领者询问自己的目标位置"""
214 | order = Obj_List[idx].send_order()
215 | if order == None: return None
216 | for each in Vertexs_List:
217 | d = self.distance_calculate(each, Point_List[idx])
218 | if d < Detect_Error:
219 | identity = Vertexs_List.index(each)
220 | aim = copy.deepcopy(Vertexs_List[identity])
221 | count = self.max_n - order # 1,2
222 | if identity == 0:
223 | pre = Vertexs_List[-1]
224 | nxt = Vertexs_List[identity + 1]
225 | elif identity == len(Vertexs_List) - 1:
226 | pre = Vertexs_List[identity - 1]
227 | nxt = Vertexs_List[0]
228 | else:
229 | pre = Vertexs_List[identity - 1]
230 | nxt = Vertexs_List[identity + 1]
231 |
232 | if count % 2 == 0: # 偶数顺时针
233 | x = np.array([pre[0] - aim[0], pre[1] - aim[1]]) # 方向向量
234 | else: # 奇数逆时针
235 | x = np.array([nxt[0] - aim[0], nxt[1] - aim[1]]) # 方向向量
236 | count2 = count / 2 if count % 2 == 0 else int(count / 2) + 1
237 | y = np.array([1, 0]) # x轴方向
238 | xmd = np.sqrt(x.dot(x)) # x.dot(x) 点乘自己，相当于向量模平方
239 | ymd = np.sqrt(y.dot(y))
240 | cos_angle = x.dot(y) / (xmd * ymd)
241 | angle = np.arccos(cos_angle)
242 | if x[0] >= 0 and x[1] >= 0:
243 |
244 | aim[0] = aim[0] + count2 * unit_gap * abs(np.cos(angle))
245 | aim[1] = aim[1] + count2 * unit_gap * np.sin(angle)
246 | elif x[0] <= 0 and x[1] >= 0:
247 | aim[0] = aim[0] - count2 * unit_gap * abs(np.cos(angle))
248 | aim[1] = aim[1] + count2 * unit_gap * np.sin(angle)
249 | elif x[0] <= 0 and x[1] <= 0:
250 | aim[0] = aim[0] - count2 * unit_gap * abs(np.cos(angle))
251 | aim[1] = aim[1] - count2 * unit_gap * np.sin(angle)
252 | else:
253 | aim[0] = aim[0] + count2 * unit_gap * abs(np.cos(angle))
254 | aim[1] = aim[1] - count2 * unit_gap * np.sin(angle)
255 | return aim
256 |
257 | def send_order(self):
258 | if self.max_n <= 0:
259 | return None # 告诉询问着索要失败
260 | else:
261 | self.max_n -= 1
262 | return self.max_n
263 |
264 |
265 | """------------------------分割线---------------------------------------------"""
266 |
267 |
268 | class Point2():
269 | """第二阶段智能体模型： 均匀化分布 + 高度收敛"""
270 | next_dis = len(Vertexs_Tooth_Chain) + 1
271 |
272 | def __init__(self, id):
273 | """初始化,绑定前后智能体关系"""
274 | self.id = id
275 | my_id = PointAddIndexSorted.index(Point_addIndex[self.id])
276 | if my_id == 0:
277 | self.pre_id = Point_addIndex.index(PointAddIndexSorted[n - 1])
278 | self.next_id = Point_addIndex.index(PointAddIndexSorted[1])
279 | elif my_id == n - 1:
280 | self.next_id = Point_addIndex.index(PointAddIndexSorted[0])
281 | self.pre_id = Point_addIndex.index(PointAddIndexSorted[n - 2])
282 | else:
283 | self.pre_id = Point_addIndex.index(PointAddIndexSorted[my_id - 1])
284 | self.next_id = Point_addIndex.index(PointAddIndexSorted[my_id + 1])
285 |
286 | def decide1(self):
287 | """均匀化，往前后节点的中间位置移动； 再有拐角的多边形上，怎么计算中间位置？
288 | 答案： 把多边形等距划开，链表化，看成一条尺链，计算出每一个节点的坐标，
289 | 计算中间位置，就只需要计算所需要移动的节点的索引
290 |
291 | 补充：这只是编程实现手法，现实情况，一个agent，定位前后agent位置，
292 | 加上多边形位置已知，往其中点移动使可实现的，所以不矛盾。
293 | """
294 | pre_chain_index = Point_addIndex[self.pre_id][3]
295 | next_chain_index = Point_addIndex[self.next_id][3]
296 | self_chain_index = Point_addIndex[self.id][3]
297 | if pre_chain_index < next_chain_index: # 正常情况
298 | self.next_dis = next_chain_index - self_chain_index
299 | mmid = ((next_chain_index + pre_chain_index) / 2 + self_chain_index) / 2
300 | else:
301 | self.next_dis = next_chain_index - self_chain_index + len(Vertexs_Tooth_Chain)
302 | if self.next_dis >= len(Vertexs_Tooth_Chain):
303 | self.next_dis -= len(Vertexs_Tooth_Chain)
304 | mmid = ((next_chain_index + len(Vertexs_Tooth_Chain) + pre_chain_index) / 2 + self_chain_index) / 2
305 |
306 | if abs(mmid - self_chain_index) <= Stop_Gap_NUM:
307 | if mmid % 1 == 0:
308 | self.move(int(mmid))
309 | elif self_chain_index > mmid: # 在目标顺时针方向
310 | self.move(int(mmid) + 1)
311 | else:
312 | self.move(int(mmid))
313 | elif mmid > self_chain_index:
314 | self.move(self_chain_index + Move_Gap_Num)
315 | else:
316 | self.move(self_chain_index - Move_Gap_Num)
317 |
318 | def decide2(self):
319 | """高度收敛"""
320 | my_hight = Point_List[self.id][2]
321 | if abs(my_hight - cz) < hv:
322 | Point_List[self.id][2] = cz
323 | elif my_hight > cz:
324 | Point_List[self.id][2] = my_hight - hv
325 | else:
326 | Point_List[self.id][2] = my_hight + hv
327 |
328 | def move(self, aim):
329 | if aim >= len(Vertexs_Tooth_Chain): aim -= len(Vertexs_Tooth_Chain)
330 | li = copy.deepcopy(Vertexs_Tooth_Chain[aim])
331 | Point_List[self.id][0] = copy.deepcopy(Vertexs_Tooth_Chain[aim])[0]
332 | Point_List[self.id][1] = copy.deepcopy(Vertexs_Tooth_Chain[aim])[1]
333 | li.append(aim)
334 | Point_addIndex[self.id] = li
335 |
336 |
337 | """实例化多个对象"""
338 | Obj_List = [Point_1(i) for i in range(0, n)] # 返回生成的n个对象的列表
339 |
340 | """排序后带索引的节点列表"""
341 | PointAddIndexSorted = []
342 |
343 | """---------------------------分割线------------------------"""
344 |
345 |
346 | def gen():
347 | """仿真数据源"""
348 | global PointAddIndexSorted
349 | global Point_addIndex
350 | global Vertexs_Tooth_Chain
351 | state_flag = 0
352 | Point_List2 = [] # 用于比较
353 | init_flag = 0
354 |
355 | def distance_calculate_2D(A, B):
356 | return pow(pow(A[0] - B[0], 2) + pow(A[1] - B[1], 2), 0.5)
357 |
358 | while True:
359 | JudgeList = []
360 | if state_flag == 0:
361 | for i in range(n):
362 | Obj_List[i].decide0()
363 | elif state_flag == 1:
364 | for i in range(n):
365 | Obj_List[i].decide1()
366 | elif state_flag == 2:
367 | for i in range(n):
368 | Obj_List[i].decide2()
369 | elif state_flag == 3:
370 |
371 | if init_flag == 0:
372 | Point_addIndex = []
373 | for i in Point_List:
374 | for j in Vertexs_Tooth_Chain:
375 | if distance_calculate_2D(i, j) <= tooth_distance / 2:
376 | li = copy.deepcopy(i)
377 | li.append(Vertexs_Tooth_Chain.index(j))
378 | Point_addIndex.append(li)
379 | break
380 |
381 | PointAddIndexSorted = copy.deepcopy(Point_addIndex)
382 | PointAddIndexSorted.sort(key=lambda x: x[3])
383 | Obj_List2 = [Point2(i) for i in range(0, n)] # 返回生成的n个对象的列表
384 | init_flag = 1
385 |
386 | for i in range(n):
387 | Obj_List2[i].decide1()
388 | JudgeList.append(Obj_List2[i].next_dis)
389 | if judge(JudgeList):
390 | state_flag += 1
391 | elif state_flag == 4:
392 | for i in range(n):
393 | Obj_List2[i].decide2()
394 |
395 | else:
396 | print("最终编队完成, 10s后退出")
397 | time.sleep(10)
398 | exit()
399 | if Point_List2 == Point_List:
400 | state_flag += 1
401 | else:
402 | pass
403 | Point_List2 = copy.deepcopy(Point_List)
404 | yield Point_List
405 |
406 |
407 | def judge(arg_list):
408 | d = len(Vertexs_Tooth_Chain) / n
409 | for each in arg_list:
410 | if abs(each - d) > 100:
411 | return False
412 | return True
413 |
414 |
415 | def update(arg_list):
416 | """刷新动画"""
417 | li = np.array(arg_list)
418 | sx, sy, sz = [], [], []
419 | for each in arg_list:
420 | sx.append(each[0])
421 | sy.append(each[1])
422 | sz.append(each[2])
423 | sc.set_offsets(li[:, :-1])
424 | sc.set_3d_properties(sz, zdir='z')
425 | return sc
426 |
427 |
428 | ani = animation.FuncAnimation(fig, update, frames=gen, interval=1)
429 | plt.show()
430 |
--------------------------------------------------------------------------------
/ToDo/agent.py:
--------------------------------------------------------------------------------
1 | import random
2 | import numpy as np
3 |
4 | class Agent:
5 | def __init__(self, id, x, y, z):
6 | self.id = id
7 | self.x = x
8 | self.y = y
9 | self.z = z
10 | self.v_1 = 5; self.error_1 = 2
11 | self.v_2 = 3; self.error_2 = 1
12 |
13 |
14 | self.rp = 0 # 高度错开随机参数
15 | self.tph = 0 # 高度错临时高度
16 | self.state = 0
17 | self.isStop = 0
18 | self.agents = None
19 | self.pgn = None
20 | self.flag_1 = False; self.flag_2 = False; self.flag_3 = False
21 | self.flag_4 = False; self.flag_5 = False; self.flag_6 = False
22 | self.edge_pos_arr = []
23 |
24 | # 抵达多边形参数
25 | self.state_2 = 0
26 |
27 | def move_in_longitude_1(self):
28 | """爬升至多边形设定高度"""
29 | if self.flag_1:
30 | return self.gen_random_p()
31 | d = self.pgn.h - self.z
32 | if abs(d) < self.error_1:
33 | self.z = self.pgn.h
34 | self.flag_1 = True
35 | else:
36 | self.z += self.v_1 if d > 0 else -self.v_1
37 |
38 | def gen_random_p(self):
39 | """生成排序所用随机数"""
40 | if self.flag_2:
41 | return self.wait_for_height_sort()
42 | self.rp = random.random()
43 | self.flag_2 = True
44 |
45 | def wait_for_height_sort(self):
46 | """等待所有智能体进入高度排序状态"""
47 | self.flag_5 = True
48 | if self.flag_6: return self.gen_temp_height()
49 |
50 | if self.check_all_flag_5():
51 | self.set_all_flag_6()
52 |
53 |
54 | def gen_temp_height(self):
55 | if self.flag_3:
56 | return self.move_in_longitude_2()
57 | sagents = sorted(self.agents, key=lambda x:x.rp)
58 | idx = sagents.index(self.agents[self.id])
59 | self.tph = self.pgn.h + (idx - len(self.agents)//2) * (350/len(self.agents))
60 | self.flag_3 = True
61 |
62 | def move_in_longitude_2(self):
63 | if self.flag_4: return
64 | d = self.tph - self.z
65 | if abs(d) < self.error:
66 | self.z = self.tph
67 | self.flag_4 = True
68 | if self.check_all_flag_4():
69 | self.set_all_agents_state(1)
70 | else:
71 | self.z += self.v if d > 0 else -self.v
72 |
73 | def move_to_vertex(self):
74 | ix = self.cal_nearest_vertex()
75 | if not ix:
76 | return self.move_to_edge()
77 |
78 |
79 |
80 | def move_to_edge(self):
81 | """移动到多边性边上"""
82 | pass
83 |
84 | def move_on_edge(self):
85 | """多边形上移动"""
86 | pass
87 |
88 | def update_forward(self, aim):
89 | x = np.array([aim[0] - self.x, aim[1] - self.y]) # 方向向量
90 | y = np.array([1, 0]) # x轴方向
91 | mx = np.sqrt(x.dot(x)) # x.dot(x) 点乘自己，相当于向量模平方
92 | my = np.sqrt(y.dot(y))
93 | if mx > self.v_2:
94 | cos_angle = x.dot(y) / (mx * my)
95 | angle = np.arccos(cos_angle) # 0.....pi
96 | if x[0] >= 0 and x[1] >= 0:
97 | self.x = self.x + self.v_2 * abs(np.cos(angle))
98 | self.y = self.y + self.v_2 * np.sin(angle)
99 | elif x[0] <= 0 and x[1] >= 0:
100 | self.x = self.x - self.v_2 * abs(np.cos(angle))
101 | self.y = self.y + self.v_2 * np.sin(angle)
102 | elif x[0] <= 0 and x[1] <= 0:
103 | self.x = self.x - self.v_2 * abs(np.cos(angle))
104 | self.y = self.y - self.v_2 * np.sin(angle)
105 | else:
106 | self.x = self.x + self.v_2 * abs(np.cos(angle))
107 | self.y = self.y - self.v_2 * np.sin(angle)
108 | else:
109 | self.x = aim[0]
110 | self.y = aim[1]
111 |
112 |
113 |
114 | def limited_state_machine(self):
115 | """运动决策有限状态机"""
116 | if self.isStop: return
117 | """0~3表示三种运动决策"""
118 | if self.state == 0:
119 | self.move_in_longitude_1()
120 | elif self.state == 1:
121 | self.move_to_vertex()
122 | elif self.state == 2:
123 | self.move_to_edge()
124 | elif self.state == 3:
125 | self.move_on_edge()
126 |
127 | def get_id(self):
128 | return self.id
129 |
130 | def get_pos(self):
131 | return self.x, self.y, self.z
132 |
133 | def check_all_flag_2(self):
134 | for a in self.agents:
135 | if a.flag_2 == False:
136 | return False
137 | return True
138 |
139 | def check_all_flag_4(self):
140 | for a in self.agents:
141 | if a.flag_4 == False:
142 | return False
143 | return True
144 |
145 | def set_all_agents_state(self, n):
146 | for a in self.agents:
147 | a.state = n
148 |
149 | def check_all_flag_5(self):
150 | for a in self.agents:
151 | if a.flag_5 == False:
152 | return False
153 | return True
154 |
155 | def set_all_flag_6(self):
156 | for a in self.agents:
157 | a.flag_6 = True
158 |
159 | def cal_distance_2d(self, a, b):
160 | return (a[0]-b[0])**2 + (a[1]-b[1])**2
161 |
162 | def cal_nearest_vertex(self):
163 | temp = []
164 | for i in range(len(self.pgn.vertexs)):
165 | d = self.cal_distance_2d([self.pgn.vertexs[i][0],self.pgn.vertexs[i][1]], \
166 | [self.x,self.y])
167 | temp.append([i,d])
168 | temp.sort(temp, key=lambda x:x[1])
169 | for cb in temp:
170 | if not self.pgn.isOccupied[cb[0]]:
171 | return cb[0]
172 | else:
173 | return False
174 |
175 | def inquire_edge_pos(self, id):
176 | return []
177 |
178 |
179 |
180 |
181 |
182 |
183 |
184 |
--------------------------------------------------------------------------------
/ToDo/main.py:
--------------------------------------------------------------------------------
1 | from polygon import Polygon
2 | from agent import Agent
3 | from random import randint, shuffle
4 | import matplotlib.pyplot as plt
5 | from matplotlib import animation
6 | import numpy as np
7 |
8 |
9 | class MultiMotionSimulate:
10 | def __init__(self, n, m, r, h, cx, cy):
11 | self.pgn = Polygon(m, r, h, cx, cy)
12 |
13 | self.n = n # 初始化多n个智能体
14 | self.s = [i for i in range(n)] # 随机启动顺序
15 | xd, xh = 0, 100
16 | yd, yh = 0, 100
17 | self.agents = [Agent(i, randint(xd, xh), randint(yd, yh), 0) for i in range(n)]
18 |
19 | self.fig, self.ax, self.sc = None, None, None
20 | self.init_show()
21 |
22 | for a in self.agents:
23 | a.pgn = self.pgn
24 | a.agents = self.agents
25 |
26 | self.isStop = False
27 |
28 | def init_show(self):
29 | """初始画布,"""
30 | self.fig = plt.figure()
31 | self.ax = plt.gca(projection='3d')
32 | self.ax.set_xlim(0, 100)
33 | self.ax.set_xlabel('X')
34 | self.ax.set_ylim(0, 100)
35 | self.ax.set_ylabel('Y')
36 | self.ax.set_zlim(0, 400)
37 | self.ax.set_zlabel('Z')
38 | vxs, vys, vzs = [v[0] for v in self.pgn.vertexs], [v[1] for v in self.pgn.vertexs], [self.pgn.h] * len(
39 | self.pgn.vertexs) # 多边形顶点坐标
40 | vxs.append(vxs[0]);
41 | vys.append(vys[0]);
42 | vzs.append(vzs[0]) # 线条首尾相连
43 | self.ax.plot(vxs, vys, vzs, color='black', linestyle=':')
44 | pxs, pys, pzs = [p.x for p in self.agents], [p.y for p in self.agents], [p.z for p in self.agents] # agent坐标
45 | self.sc = self.ax.scatter3D(pxs, pys, pzs, color='r', alpha=0.7)
46 |
47 | def observe(self):
48 | """观察所有agents的状态"""
49 | while not self.isStop:
50 | shuffle(self.s)
51 | for c in self.s:
52 | c = randint(0, self.n - 1)
53 | self.agents[c].limited_state_machine()
54 | yield None
55 |
56 | def deal_3d(self, agents):
57 | """3d画图数据设置"""
58 | pzs = [p.z for p in self.agents]
59 | temp = np.array([[p.x, p.y, p.z] for p in self.agents])
60 | self.sc.set_offsets(temp[:, :-1])
61 | self.sc.set_3d_properties(pzs, zdir='z')
62 | return self.sc
63 |
64 | def show(self):
65 | """画图"""
66 | ani = animation.FuncAnimation(self.fig, func = self.deal_3d, frames = self.observe, interval= 1)
67 | plt.show()
68 |
69 |
70 | if __name__ == '__main__':
71 | # 初始化目标多边形，边形为5，半径30，高度200, 中心坐标为50，50
72 | mms = MultiMotionSimulate(10, 5, 30, 200, 50, 50)
73 | mms.show()
74 |
--------------------------------------------------------------------------------
/ToDo/polygon.py:
--------------------------------------------------------------------------------
1 | import numpy as np
2 |
3 |
4 | class Polygon:
5 | def __init__(self, m, r, h, cx = 50, cy = 50) -> object:
6 | self.m = m
7 | self.r = r
8 | self.h = h
9 | self.cx = cx
10 | self.cy = cy
11 | self.vertexs = []
12 | self.gen_ploygen_vertexs()
13 | self.isOccupied = [False for i in range(m)]
14 |
15 | def gen_ploygen_vertexs(self):
16 | for i in range(1, self.m + 1):
17 | x = self.cx + self.r * np.sin(i * 2 * np.pi / self.m)
18 | y = self.cy + self.r * np.cos(i * 2 * np.pi / self.m)
19 | self.vertexs.append([x, y])
20 |
21 |
22 |
23 |
24 |
--------------------------------------------------------------------------------
/pics/图2、高度错开性避免碰撞策略流程图.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/colaforced/AgentsMotionSimulation/b80b15146d47f6567596e4b457d75c2b63193230/pics/图2、高度错开性避免碰撞策略流程图.png
--------------------------------------------------------------------------------
/pics/基于自组织的均匀多边形编队仿真.gif:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/colaforced/AgentsMotionSimulation/b80b15146d47f6567596e4b457d75c2b63193230/pics/基于自组织的均匀多边形编队仿真.gif
--------------------------------------------------------------------------------
/readme.md:
--------------------------------------------------------------------------------
1 | # 基于自组织的多无人机编队算法设计
2 | ## 一、基于自组织的多无人均匀多边形编队控制
3 | ### 1. 基于自组织的多无人机编队问题详解
4 | #### 1.1 初始配置、目标配置和无人机模型定义
5 | **1.1.1、初始配置、目标配置和无人机模型定义**
6 |
7 | 1) 我们把无人机考虑成一个具有一定体积的智能体，不考虑其具体动力学特性，
8 | 仅仅将其视为一个具有一定体积的、可自主移动的圆球。智能体之间不能接
9 | 触、碰撞。
10 | 2) 智能体是同质的、可互换的、匿名的，所有个体性质都是相同的，唯一的不
11 | 同只有其所处的环境不同或者说其运动状态不同，除此之外，没有任何特殊
12 | 性，匿名性也导致智能体无法依靠 ID 分辨其他个体。且个体具有无记忆性，
13 | 无法记录上一步的行为或者决策。
14 | 3) 智能体拥有相同的笛卡尔坐标系统，且目标多边形位置是固定已知的，没个
15 | 无人机均有记录均匀多边形的几个顶点位置。
16 | 4) 忽略通信细节，不考虑通信时延和定位精度问题，每个智能体均可准确知道
17 | 其他任何个体位置，实际上仿真实验中只用到局部其他个体的位置信息。
18 | 5) 智能体有两种状态，静止和激活状态，静止即为保持位置不变化，但仍然会
19 | 与外界保持沟通；激活状态则是智能体主动判断自身所处位置，根据环境做
20 | 出决策，然后执行决策。仿真时，智能体默认保持静止，激活是异步进行的，
21 | 同一时间只考虑激活一个个体，激活个体随机选择，依次达到对现实运行的
22 | 仿真。
23 |
24 |
25 | #### 多边形形成算法设计
26 | **1.2.1、多边形形成控制算法约束条件**
27 |
28 | 算法设计之先，我们需要给出一些智能体运动约束条件：
29 | 1)智能体之间不能碰撞，更不能重合，设定智能体之间最小间距为 d。
30 | 2)本文所设计控制算法不涉及具体控制率，每个个体移动速度相同，表现在算法中则是每个个体每次激活时移动的距离小于等于 D。
31 |
32 |
33 | **步骤一 ：抵达多边形**
34 | ~~~python
35 | """
36 | Vertex_Position 表示多边形顶点坐标列表、detect 表示函数检测最近顶点 detect_occupy 表示
37 | 函数检测顶点是否被占领、move 表示函数移动、get 表示函数通信索要排队序号，adjust 表示目标
38 | """
39 |
40 | def function1(Vertex_Position = Vertex_Position):
41 | nearest_point = detect(Vertex_Position)
42 | #检测最近多边形顶点位置
43 | if ！detect_occupy(nearest_point):
44 | #如果检测到该顶点没被占领
45 | move(nearest_point)
46 | #移向目标顶点
47 | else:
48 | order = get(occupy_ID)
49 | #向占领的智能体索要当前排队序号
50 | if order:
51 | adjust_aim = adjust(order)
52 | #根据排队序号调整自己的目标位置
53 | else:
54 | Vertex_Position = Vertex_Position.remove(nearest_point)
55 | return functions(Vertex_Position)
56 | #索要排序号失败则移除顶点列表中的最近顶点，
57 | #并再次调用决策函数
58 | return None
59 | ~~~
60 |
61 | **步骤二 ：均匀化**
62 | ~~~python
63 | """
64 | def function2():
65 | pre_index ,next_index = detect_neighbor()
66 | #检测前后相邻个体在均匀多边形链表上的索引
67 | aim = ((pre_index+next_index)/2 + my_index)/2
68 | #aim 为前后相邻个体的中点与自身位置的中点
69 | move(aim)
70 | #往目标位置移动
71 |
72 | 注：detect_neighbor()表示函数检测多边形上前后相邻个体
73 | ~~~
74 |
75 | ### 2 仿真结果
76 |
77 | 代码见 ``done/plgsim.py``
78 | 由于本科时候太菜了，自己写的代码自己都不想去维护。
79 | 为了读者阅读方便，尝试重构代码(见``ToDo``),
80 | 基本框架已经搭起来了，但策略迁移工作量太多了，
81 | 又是毕业之际，暂时保留再做，感兴趣的小伙伴也也可以提出代码合并哈。
82 |
83 | 
84 |
85 |
86 |
87 | ### 3: 链接
88 | 1. https://blog.csdn.net/py431382/article/details/89854176?spm=1001.2014.3001.5501
89 | 2. https://www.bilibili.com/video/BV1PE41117m6?spm_id_from=333.999.0.0
90 |
91 |
92 | ## 二、基于自组织的多智能体追逐与合围
93 |
94 |
95 | https://www.bilibili.com/video/BV1PE41117DG
96 |
97 |
98 | 代码待整理上传
--------------------------------------------------------------------------------