devbox
Guff_9hys
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

各类路径规划算法python 代码_python全覆盖路径规划模板

作者：Guff_9hys | 2024-07-19 01:52:22

赞

踩

python全覆盖路径规划模板

一、人工势场法

  1. # 初始化参数设置
  2. import numpy as np
  3. import matplotlib.pyplot as plt
  4. import copy
  5. from celluloid import Camera  # 保存动图时用，pip install celluloid
  6. %matplotlib qt5
  7. ## 初始化车的参数
  8. d = 3.5  #道路标准宽度
  9.  
  10. W = 1.8  #  汽车宽度
  11.  
  12. L = 4.7  # 车长
  13.  
  14. P0 = np.array([0, - d / 2, 1, 1]) #车辆起点位置，分别代表x,y,vx,vy
  15.  
  16. Pg = np.array([99, d / 2, 0, 0]) # 目标位置
  17.  
  18. # 障碍物位置
  19. Pobs = np.array([
  20.     [15, 7 / 4, 0, 0],    
  21.     [30, - 3 / 2, 0, 0],
  22.     [45, 3 / 2, 0, 0], 
  23.     [60, - 3 / 4, 0, 0], 
  24.     [80, 3/2, 0, 0]])
  25.  
  26. P = np.vstack((Pg,Pobs))  # 将目标位置和障碍物位置合放在一起
  27.  
  28. Eta_att = 5  # 引力的增益系数
  29.  
  30. Eta_rep_ob = 15  # 斥力的增益系数
  31.  
  32. Eta_rep_edge = 50   # 道路边界斥力的增益系数
  33.  
  34. d0 = 20  # 障碍影响的最大距离
  35.  
  36. num = P.shape[0] #障碍与目标总计个数
  37.  
  38. len_step = 0.5 # 步长
  39.  
  40. n=1
  41.  
  42. Num_iter = 300  # 最大循环迭代次数
  43.  
  44. # 数据存储变量定义
  45. path = []  # 保存车走过的每个点的坐标
  46. delta = np.zeros((num,2)) # 保存车辆当前位置与障碍物的方向向量，方向指向车辆；以及保存车辆当前位置与目标点的方向向量，方向指向目标点
  47. dists = [] # 保存车辆当前位置与障碍物的距离以及车辆当前位置与目标点的距离
  48. unite_vec = np.zeros((num,2)) #  保存车辆当前位置与障碍物的单位方向向量，方向指向车辆；以及保存车辆当前位置与目标点的单位方向向量，方向指向目标点
  49.  
  50. F_rep_ob = np.zeros((len(Pobs),2))  # 存储每一个障碍到车辆的斥力,带方向
  51. v=np.linalg.norm(P0[2:4]) # 设车辆速度为常值
  52.  
  53. ## ***************初始化结束，开始主体循环******************人工势场法核心代码
  54. Pi = P0[0:2]  # 当前车辆位置
  55. # count=0
  56. for i in range(Num_iter):
  57.     if ((Pi[0] - Pg[0]) ** 2 + (Pi[1] - Pg[1]) ** 2) ** 0.5 < 1:
  58.         break
  59.     dists=[]
  60.     path.append(Pi)
  61.     # print(count)
  62.     # count+=1
  63.     #计算车辆当前位置与障碍物的单位方向向量
  64.     for j in range(len(Pobs)):
  65.         delta[j]=Pi[0:2] - Pobs[j, 0:2]
  66.         dists.append(np.linalg.norm(delta[j]))
  67.         unite_vec[j]=delta[j]/dists[j]
  68.     #计算车辆当前位置与目标的单位方向向量
  69.     delta[len(Pobs)]=Pg[0:2] - Pi[0:2]
  70.     dists.append(np.linalg.norm(delta[len(Pobs)]))
  71.     unite_vec[len(Pobs)] = delta[len(Pobs)]/dists[len(Pobs)]
  72.     
  73.     ## 计算引力
  74.     F_att = Eta_att*dists[len(Pobs)]*unite_vec[len(Pobs)]
  75.     
  76.     ## 计算斥力
  77.     # 在原斥力势场函数增加目标调节因子（即车辆至目标距离），以使车辆到达目标点后斥力也为0
  78.     for j in  range(len(Pobs)):
  79.         if dists[j] >= d0:
  80.             F_rep_ob[j] = np.array([0, 0])
  81.         else:
  82.             # 障碍物的斥力1，方向由障碍物指向车辆
  83.             F_rep_ob1_abs = Eta_rep_ob * (1 / dists[j] - 1 / d0) * (dists[len(Pobs)])**n / dists[j] ** 2  # 斥力大小
  84.             F_rep_ob1 = F_rep_ob1_abs*unite_vec[j]  # 斥力向量
  85.             # 障碍物的斥力2，方向由车辆指向目标点
  86.             F_rep_ob2_abs = n/2 * Eta_rep_ob * (1 / dists[j] - 1 / d0) **2 *(dists[len(Pobs)])**(n-1) # 斥力大小
  87.             F_rep_ob2 = F_rep_ob2_abs * unite_vec[len(Pobs)]  # 斥力向量
  88.             # 改进后的障碍物合斥力计算
  89.             F_rep_ob[j] = F_rep_ob1 + F_rep_ob2
  90.     
  91.     
  92.     # 增加道路边界斥力势场，根据车辆当前位置，选择对应的斥力函数
  93.     if Pi[1] > - d + W / 2 and Pi[1] <= - d / 2:
  94.          F_rep_edge = [0, Eta_rep_edge * v * np.exp(-d / 2 - Pi[1])]  # 下道路边界区域斥力势场，方向指向y轴正向
  95.     elif Pi[1] > - d / 2 and Pi[1] <= - W / 2:
  96.         F_rep_edge = np.array([0, 1 / 3 * Eta_rep_edge * Pi[1] ** 2])
  97.     elif Pi[1] > W / 2 and Pi[1] <= d / 2:
  98.         F_rep_edge = np.array([0, - 1 / 3 * Eta_rep_edge * Pi[1] ** 2])
  99.     elif Pi[1] > d / 2 and Pi[1] <= d - W / 2:
  100.         F_rep_edge = np.array([0, Eta_rep_edge * v * (np.exp(Pi[1] - d / 2))])
  101.     
  102.     
  103.     ## 计算合力和方向
  104.     F_rep = np.sum(F_rep_ob, axis=0)+F_rep_edge
  105.     
  106.     F_sum = F_att+F_rep
  107.     
  108.     UnitVec_Fsum = 1 / np.linalg.norm(F_sum) * F_sum
  109.     #计算车的下一步位置
  110.     Pi = copy.deepcopy(Pi+ len_step * UnitVec_Fsum)
  111.     # Pi[0:2] = Pi[0:2] + len_step * UnitVec_Fsum
  112.     # print(Pi)
  113.  
  114. path.append(Pg[0:2]) # 最后把目标点也添加进路径中
  115. path=np.array(path) # 转为numpy
  116.  
  117. ## 画图
  118. fig=plt.figure(1)
  119. # plt.ylim(-4, 4)
  120. plt.axis([-10,100,-15,15])
  121. camera = Camera(fig)
  122. len_line = 100
  123. # 画灰色路面图
  124. GreyZone = np.array([[- 5, - d - 0.5], [- 5, d + 0.5],
  125.             [len_line, d + 0.5], [len_line, - d - 0.5]])
  126. for i in range(len(path)):
  127.      
  128.      plt.fill(GreyZone[:, 0], GreyZone[:, 1], 'gray')
  129.      plt.fill(np.array([P0[0], P0[0], P0[0] - L, P0[0] - L]), np.array([- d /
  130.           2 - W / 2, - d / 2 + W / 2, - d / 2 + W / 2, - d / 2 - W / 2]), 'b')
  131.      # 画分界线
  132.      plt.plot(np.array([- 5, len_line]), np.array([0, 0]), 'w--')
  133.  
  134.      plt.plot(np.array([- 5, len_line]), np.array([d, d]), 'w')
  135.  
  136.      plt.plot(np.array([- 5, len_line]), np.array([- d, - d]), 'w')
  137.  
  138.      # 设置坐标轴显示范围
  139.      # plt.axis('equal')
  140.      # plt.gca().set_aspect('equal')
  141.      # 绘制路径
  142.      plt.plot(Pobs[:,0],Pobs[:,1], 'ro') #障碍物位置
  143.  
  144.      plt.plot(Pg[0],Pg[1], 'gv')  # 目标位置
  145.  
  146.      plt.plot(P0[0],P0[1], 'bs')  # 起点位置
  147.      # plt.cla()
  148.      plt.plot(path[0:i,0],path[0:i,1], 'k')  # 路径点
  149.      plt.pause(0.001)
  150. #      camera.snap()
  151. # animation = camera.animate()
  152. # animation.save('trajectory.gif')
  153.

参考：https://blog.csdn.net/weixin_42301220/article/details/125155505

二、D*算法

  1. # 首次搜索
  2. # coding=utf-8
  3. import matplotlib.pyplot as plt
  4. import numpy as np
  5. import math
  6.  
  7. map_grid = [[1 for j in range(0, 8)] for i in range(0, 8)]  # 定义列表
  8. map_grid = np.array(map_grid)  # 将列表转化为数组，因为只有数组才有维度的概念，方便切片
  9. map_grid[3:6, 1] = 0  # 障碍物
  10. map_grid[3:6, 5] = 0
  11. map_grid[0, 3] = 5  # 起点
  12. map_grid[7, 3] = 6  # 终点
  13.  
  14.  
  15. def draw_effect(map_grid,second_path):
  16.     plt.imshow(map_grid, cmap=plt.cm.hot, interpolation='nearest', vmin=0, vmax=10)  # 绘制热力图
  17.     plt.colorbar()
  18.     plt.xlim(-1, 8)  # x轴的范围
  19.     plt.ylim(-1, 8)
  20.     my_x_ticks = np.arange(0, 8, 1)  # x轴标号的范围
  21.     my_y_ticks = np.arange(0, 8, 1)
  22.     plt.xticks(my_x_ticks)
  23.     plt.yticks(my_y_ticks)
  24.     second_path = np.array(second_path)
  25.     plt.plot(second_path[:, 1:2],second_path[:, 0:1],'-')
  26.     # plt.grid(True)  # 开启栅格  可以不开启
  27.     plt.show()  # 可视化
  28.  
  29.  
  30. open_list = [[7, 3, 0, 0, None, None]]  # 将终点置于open列表中列表中分别有x0,y0坐标,h值,父节点X、Y坐标
  31. close_list = []
  32.  
  33.  
  34. # draw_effect(map_grid)
  35. # 将邻域放入open_list中
  36. def open_list_append(x0, y0, x1, y1, h0, h1, map_grid, open_list):
  37.     if 0 <= x1 <= 7 and 0 <= y1 <= 7 and map_grid[x1, y1] != 4 and map_grid[x1, y1] != 0:  # 左边没有越界并且没有在closelist里面
  38.         if map_grid[x1, y1] == 3:  # 如果是在open_list中,h要更新
  39.             open_list = np.array(open_list)
  40.             if (h1 + h0) < open_list[np.where((open_list[:, 0] == x1) & (open_list[:, 1] == y1)), 2]:
  41.                 h = h1 + h0
  42.                 k = h1 + h0
  43.                 open_list[np.where((open_list[:, 0] == x1) & (open_list[:, 1] == y1)), 2] = h
  44.                 open_list[np.where((open_list[:, 0] == x1) & (open_list[:, 1] == y1)), 3] = k
  45.                 open_list[np.where((open_list[:, 0] == x1) & (open_list[:, 1] == y1)), 4] = x0
  46.                 open_list[np.where((open_list[:, 0] == x1) & (open_list[:, 1] == y1)), 4] = y0
  47.             open_list = list(open_list.tolist())
  48.  
  49.         else:  # 是new节点
  50.             h = h1 + h0
  51.             k = h1 + h0
  52.             # open_list = list(open_list)
  53.             open_list.append([x1, y1, h, k, x0, y0])
  54.             map_grid[x1, y1] = 3
  55.  
  56.     return open_list
  57.  
  58.  
  59. # 首次搜索
  60. def first_search(open_list, close_list, map_grid):  # 给出终点坐标，完成首次遍历
  61.     # 采用D算法遍历
  62.     # 选openlist中h最小的,将openlist按照h排序，取第一个，并删除第一个，将它放到close_list里面
  63.     open_list = list(open_list)
  64.     open_list.sort(key=lambda x: x[2])
  65.     # open_list.pop(0)
  66.     insert_list = open_list[0]  # 引入中间列表，用来存储每一次被选中的遍历的点
  67.     x0 = int(insert_list[0])
  68.     y0 = int(insert_list[1])
  69.     open_list.pop(0)
  70.     close_list.append(list(insert_list))
  71.     map_grid[x0, y0] = 4  # 被加入到close_list里面
  72.  
  73.     # 找insert_list的邻域 ----->寻找顺序：从左边开始逆时针
  74.     h0 = int(insert_list[2])
  75.  
  76.     x1 = x0
  77.     y1 = y0 - 1
  78.     h1 = 10
  79.     open_list = open_list_append(x0, y0, x1, y1, h0, h1, map_grid, open_list)
  80.  
  81.     x1 = x0 - 1
  82.     y1 = y0 - 1
  83.     h1 = 14
  84.     open_list = open_list_append(x0, y0, x1, y1, h0, h1, map_grid, open_list)
  85.  
  86.     x1 = x0 - 1
  87.     y1 = y0
  88.     h1 = 10
  89.     open_list = open_list_append(x0, y0, x1, y1, h0, h1, map_grid, open_list)
  90.  
  91.     x1 = x0 - 1
  92.     y1 = y0 + 1
  93.     h1 = 14
  94.     open_list = open_list_append(x0, y0, x1, y1, h0, h1, map_grid, open_list)
  95.  
  96.     x1 = x0
  97.     y1 = y0 + 1
  98.     h1 = 10
  99.     open_list = open_list_append(x0, y0, x1, y1, h0, h1, map_grid, open_list)
  100.  
  101.     x1 = x0 + 1
  102.     y1 = y0 + 1
  103.     h1 = 14
  104.     open_list = open_list_append(x0, y0, x1, y1, h0, h1, map_grid, open_list)
  105.  
  106.     x1 = x0 + 1
  107.     y1 = y0
  108.     h1 = 10
  109.     open_list = open_list_append(x0, y0, x1, y1, h0, h1, map_grid, open_list)
  110.  
  111.     x1 = x0 + 1
  112.     y1 = y0 - 1
  113.     h1 = 14
  114.     open_list = open_list_append(x0, y0, x1, y1, h0, h1, map_grid, open_list)
  115.  
  116.     return [open_list, close_list, map_grid]
  117.  
  118.  
  119. while map_grid[0, 3] != 4 and open_list != []:
  120.     [open_list, close_list, map_grid] = first_search(open_list, close_list, map_grid)
  121.  
  122. # 首次搜索完成
  123. first_path = []
  124. close_list = np.array(close_list)
  125. xn = 0
  126. yn = 3
  127. while xn != 7 or yn != 3:
  128.     list1 = list(close_list[np.where((close_list[:, 0] == xn) & (close_list[:, 1] == yn))][0])
  129.     xn = int(list1[4])
  130.     yn = int(list1[5])
  131.     first_path.append(list1)
  132.  
  133. first_path.append([7, 3, 0, 0, None, None])
  134.  
  135. # 第二次搜索
  136. # 通过上面的程序已经找到了一条路径，完成了第一次的搜索，此时每个节点的h和k是相等的。此时开始点在close list里面，最短路径在firstpath中。
  137. # 可以看出，每个节点的父节点都是该节点的八个邻节点中k值最小的哪个。
  138. # 当出现动态变化时，我们可以利用这个图尽快修正我们的路径，而不是重新规划。
  139. # 当我们检测到某点被阻碍了：1、修改这个点的h值，h变为inf，把它放入openlist中。注意此时该节点的k还是小值，是原来哪个h的值，因此它将立即被取出
  140. # 2、把这个修改扩散出去，直到kmin >=h
  141. # 设置一个突然出现的障碍
  142. map_grid[3, 3] = 0
  143.  
  144. close_list[np.where((close_list[:, 4] == 3) & (close_list[:, 5] == 3)), 2] = math.inf
  145. close_list[np.where((close_list[:, 0] == 3) & (close_list[:, 1] == 3)), 2] = math.inf
  146. insertRow = list(close_list[np.where((close_list[:, 4] == 3) & (close_list[:, 5] == 3))][0])
  147. x = int(insertRow[0])
  148. y = int(insertRow[1])
  149. open_list.append(insertRow)  # ->>>>>>open_list是列表格式
  150. map_grid[x, y] = 3
  151. close_list = list(close_list.tolist())  # ----->>>>>close_list是列表格式
  152. close_list.remove(insertRow)
  153. open_list.sort(key=lambda x: x[3])  # 先排序，选择k最小的节点
  154. k_min = open_list[0][3]  #
  155. hx = open_list[0][2]
  156.  
  157.  
  158. # 接下来把这个点扩散出去
  159.  
  160. def find_neighbor(x0, y0, x1, y1, k_old, hx, h1, close_list):
  161.     close_list = np.array(close_list)
  162.     hy = close_list[np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 2][0]
  163.     if (hy <= k_old) and (hx > hy + h1):
  164.         close_list[np.where((close_list[:, 0] == x0) & (close_list[:, 1] == y0)), 4] = x1
  165.         close_list[np.where((close_list[:, 0] == x0) & (close_list[:, 1] == y0)), 5] = y1
  166.         close_list[np.where((close_list[:, 0] == x0) & (close_list[:, 1] == y0)), 2] = hy + h1
  167.         hx = hy + h1
  168.     return [hx, list(close_list.tolist())]
  169.  
  170. def find_neighbor2(x0, y0, x1, y1, k_old, hx, h1, close_list, open_list, map_grid):
  171.     close_list = np.array(close_list)
  172.     hy = close_list[np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 2][0]
  173.     if (close_list[np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 4] == x0 and close_list[
  174.         np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 5] == y0 and (hy != hx + h1)) or (
  175.             (hy > hx + h1) and (
  176.             (close_list[np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 4] != x0) or (
  177.             close_list[np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 5] != y0))):
  178.         close_list[np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 4] = x0
  179.         close_list[np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 5] = y0
  180.         close_list[np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 2] = hx + h1
  181.         Y = list(close_list[np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1))][0])
  182.         # 把Y放入open_list中
  183.         close_list = list(close_list.tolist())
  184.         close_list.remove(Y)
  185.         open_list.append(Y)
  186.         map_grid[x1, y1] = 3
  187.     return [open_list, close_list, map_grid]
  188.  
  189. def find_neighbor3(x0, y0, x1, y1, k_old, hx, h1, close_list, open_list, map_grid):
  190.     close_list = np.array(close_list)
  191.     hy = close_list[np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 2][0]
  192.     if (close_list[np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 4] == x0 and close_list[
  193.         np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 5] == y0 and (hy != hx + h1)):
  194.         close_list[np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 4] = x0
  195.         close_list[np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 5] = y0
  196.         close_list[np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 2] = hx + h1
  197.         Y = list(close_list[np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1))][0])
  198.         # 把Y放入open_list中
  199.         close_list = list(close_list.tolist())
  200.         close_list.remove(Y)
  201.         open_list.append(Y)
  202.         map_grid[x1, y1] = 3
  203.     else:
  204.         if ((close_list[np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 4] != x0 or close_list[
  205.             np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 5] != y0) and (hy > hx + h1)):
  206.             #print(list(close_list[np.where((close_list[:, 0] == x0) & (close_list[:, 1] == y0))][0]))
  207.             if map_grid[x0,y0]!=3:
  208.                 X = list(close_list[np.where((close_list[:, 0] == x0) & (close_list[:, 1] == y0))][0])
  209.                 close_list = list(close_list.tolist())
  210.                 close_list.remove(X)
  211.                 open_list.append(X)
  212.             else:
  213.                 open_list = np.array(open_list)
  214.                 X = list(open_list[np.where((open_list[:, 0] == x0) & (open_list[:, 1] == y0))][0])
  215.                 open_list = list(open_list.tolist())
  216.         #     # 把Y放入open_list中
  217.             map_grid[x0, y0] = 3
  218.         else:
  219.             if ((close_list[np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 4] != x0 or close_list[
  220.                 np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 5] != y0) and (hx > hy + h1)) and \
  221.                     map_grid[x1, y1] == 4 and hy > k_old:
  222.                 if map_grid[x1, y1] != 3:
  223.                     Y = list(close_list[np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1))][0])
  224.                     close_list = list(close_list.tolist())
  225.                     close_list.remove(Y)
  226.                     open_list.append(Y)
  227.                 else:
  228.                     open_list = np.array(open_list)
  229.                     Y = list(open_list[np.where((open_list[:, 0] == x1) & (open_list[:, 1] == y1))][0])
  230.                     open_list = list(open_list.tolist())
  231.                 # 把Y放入open_list中
  232.                 map_grid[x1, y1] = 3
  233.  
  234.     return [open_list, close_list, map_grid]
  235.  
  236. # 扩散程序 process-state:先弹出openlist列表中k最小的节点，并删除这个节点。然后分类处理：
  237. def process_state(open_list, close_list, map_grid):
  238.     # 修改这个点的h值
  239.     open_list.sort(key=lambda x: x[3])  # 先排序，选择k最小的节点
  240.     X = open_list[0]  # X表示k最小的节点
  241.     x0 = int(X[0])
  242.     y0 = int(X[1])
  243.     close_list.append(X)  # 将它放入closelist
  244.     map_grid[x0, y0] = 4
  245.     open_list.remove(X)
  246.     # 从openlist中删除这个节点
  247.     # 分类处理：（该节点处于lower状态，该节点处于lower状态）
  248.     k_old = X[3]
  249.     hx = X[2]
  250.     # print(close_list)
  251.  
  252.     if k_old < hx:  # k_old是上升状态
  253.         x1 = x0
  254.         y1 = y0 - 1
  255.         h1 = 10
  256.         [hx, close_list] = find_neighbor(x0, y0, x1, y1, k_old, hx, h1, close_list)
  257.  
  258.         x1 = x0 - 1
  259.         y1 = y0 - 1
  260.         h1 = 14
  261.         [hx, close_list] = find_neighbor(x0, y0, x1, y1, k_old, hx, h1, close_list)
  262.  
  263.         x1 = x0 - 1
  264.         y1 = y0
  265.         h1 = 10
  266.         [hx, close_list] = find_neighbor(x0, y0, x1, y1, k_old, hx, h1, close_list)
  267.  
  268.         x1 = x0 - 1
  269.         y1 = y0 + 1
  270.         h1 = 14
  271.         [hx, close_list] = find_neighbor(x0, y0, x1, y1, k_old, hx, h1, close_list)
  272.  
  273.         x1 = x0
  274.         y1 = y0 + 1
  275.         h1 = 10
  276.         [hx, close_list] = find_neighbor(x0, y0, x1, y1, k_old, hx, h1, close_list)
  277.  
  278.         x1 = x0 + 1
  279.         y1 = y0 + 1
  280.         h1 = 14
  281.         [hx, close_list] = find_neighbor(x0, y0, x1, y1, k_old, hx, h1, close_list)
  282.  
  283.         x1 = x0 + 1
  284.         y1 = y0
  285.         h1 = 10
  286.         [hx, close_list] = find_neighbor(x0, y0, x1, y1, k_old, hx, h1, close_list)
  287.  
  288.         x1 = x0 + 1
  289.         y1 = y0 - 1
  290.         h1 = 14
  291.         [hx, close_list] = find_neighbor(x0, y0, x1, y1, k_old, hx, h1, close_list)
  292.         # 找它的邻节点，看能不能让它的h降低
  293.         #print(hx)
  294.  
  295.     # if k_old == hx:  # 该节点x处于lower状态
  296.     #     x1 = x0
  297.     #     y1 = y0 - 1
  298.     #     h1 = 10
  299.     #     [open_list, close_list, map_grid] = find_neighbor2(x0, y0, x1, y1, k_old, hx, h1, close_list, open_list,
  300.     #                                                        map_grid)
  301.     #
  302.     #     x1 = x0 - 1
  303.     #     y1 = y0 - 1
  304.     #     h1 = 14
  305.     #     [open_list, close_list, map_grid] = find_neighbor2(x0, y0, x1, y1, k_old, hx, h1, close_list, open_list,
  306.     #                                                        map_grid)
  307.     #
  308.     #     x1 = x0 - 1
  309.     #     y1 = y0
  310.     #     h1 = 10
  311.     #     [open_list, close_list, map_grid] = find_neighbor2(x0, y0, x1, y1, k_old, hx, h1, close_list, open_list,
  312.     #                                                        map_grid)
  313.     #
  314.     #     x1 = x0 - 1
  315.     #     y1 = y0 + 1
  316.     #     h1 = 14
  317.     #     [open_list, close_list, map_grid] = find_neighbor2(x0, y0, x1, y1, k_old, hx, h1, close_list, open_list,
  318.     #                                                        map_grid)
  319.     #
  320.     #     x1 = x0
  321.     #     y1 = y0 + 1
  322.     #     h1 = 10
  323.     #     [open_list, close_list, map_grid] = find_neighbor2(x0, y0, x1, y1, k_old, hx, h1, close_list, open_list,
  324.     #                                                        map_grid)
  325.     #
  326.     #     x1 = x0 + 1
  327.     #     y1 = y0 + 1
  328.     #     h1 = 14
  329.     #     [open_list, close_list, map_grid] = find_neighbor2(x0, y0, x1, y1, k_old, hx, h1, close_list, open_list,
  330.     #                                                        map_grid)
  331.     #
  332.     #     x1 = x0 + 1
  333.     #     y1 = y0
  334.     #     h1 = 10
  335.     #     [open_list, close_list, map_grid] = find_neighbor2(x0, y0, x1, y1, k_old, hx, h1, close_list, open_list,
  336.     #                                                        map_grid)
  337.     #
  338.     #     x1 = x0 + 1
  339.     #     y1 = y0 - 1
  340.     #     h1 = 14
  341.     #     [open_list, close_list, map_grid] = find_neighbor2(x0, y0, x1, y1, k_old, hx, h1, close_list, open_list,
  342.     #                                                        map_grid)
  343.     #
  344.     # else:
  345.     #     x1 = x0
  346.     #     y1 = y0 - 1
  347.     #     h1 = 10
  348.     #     [open_list, close_list, map_grid] = find_neighbor3(x0, y0, x1, y1, k_old, hx, h1, close_list, open_list,
  349.     #                                                        map_grid)
  350.     #
  351.     #     x1 = x0 - 1
  352.     #     y1 = y0 - 1
  353.     #     h1 = 14
  354.     #     [open_list, close_list, map_grid] = find_neighbor3(x0, y0, x1, y1, k_old, hx, h1, close_list, open_list,
  355.     #                                                        map_grid)
  356.     #     #
  357.     #     x1 = x0 - 1
  358.     #     y1 = y0
  359.     #     h1 = 10
  360.     #     [open_list, close_list, map_grid] = find_neighbor3(x0, y0, x1, y1, k_old, hx, h1, close_list, open_list,
  361.     #                                                        map_grid)
  362.     #     #
  363.     #     x1 = x0 - 1
  364.     #     y1 = y0 + 1
  365.     #     h1 = 14
  366.     #     [open_list, close_list, map_grid] = find_neighbor3(x0, y0, x1, y1, k_old, hx, h1, close_list, open_list,
  367.     #                                                        map_grid)
  368.     #     #
  369.     #     x1 = x0
  370.     #     y1 = y0 + 1
  371.     #     h1 = 10
  372.     #     [open_list, close_list, map_grid] = find_neighbor3(x0, y0, x1, y1, k_old, hx, h1, close_list, open_list,
  373.     #                                                        map_grid)
  374.     #     x1 = x0 + 1
  375.     #     y1 = y0 + 1
  376.     #     h1 = 14
  377.     #     [open_list, close_list, map_grid] = find_neighbor3(x0, y0, x1, y1, k_old, hx, h1, close_list, open_list,
  378.     #                                                        map_grid)
  379.     #
  380.     #     x1 = x0 + 1
  381.     #     y1 = y0
  382.     #     h1 = 10
  383.     #     [open_list, close_list, map_grid] = find_neighbor3(x0, y0, x1, y1, k_old, hx, h1, close_list, open_list,
  384.     #                                                        map_grid)
  385.     #     x1 = x0 + 1
  386.     #     y1 = y0 - 1
  387.     #     h1 = 14
  388.     #     [open_list, close_list, map_grid] = find_neighbor3(x0, y0, x1, y1, k_old, hx, h1, close_list, open_list,
  389.     #                                                    map_grid)
  390.  
  391.     open_list.sort(key=lambda x: x[3])  # 先排序，选择k最小的节点
  392.     k_min = open_list[0][3]  #
  393.  
  394.     return [open_list, list(close_list), map_grid,k_min,hx]
  395.  
  396. while k_min<hx:
  397.     [open_list, close_list, map_grid,k_min,hx] = process_state(open_list, close_list, map_grid)
  398.  
  399.  
  400. #避障
  401. second_path = []
  402. close_list = np.array(close_list)
  403. xn = 0
  404. yn = 3
  405. while xn != 7 or yn != 3:
  406.     list1 = list(close_list[np.where((close_list[:, 0] == xn) & (close_list[:, 1] == yn))][0])
  407.     xn = int(list1[4])
  408.     yn = int(list1[5])
  409.     second_path.append(list1)
  410.  
  411. second_path.append([7, 3, 0, 0, None, None])
  412. draw_effect(map_grid,second_path)
  413. print("Find it")
'
运行

三、遗传算法

  1.  
  2. import numpy as np
  3. import matplotlib.pyplot as plt
  4. from matplotlib import cm
  5. from mpl_toolkits.mplot3d import Axes3D
  6.  
  7. DNA_SIZE = 24
  8. POP_SIZE = 200
  9. CROSSOVER_RATE = 0.8
  10. MUTATION_RATE = 0.005
  11. N_GENERATIONS = 50
  12. X_BOUND = [-3, 3]
  13. Y_BOUND = [-3, 3]
  14.  
  15.  
  16. def F(x, y):
  17. 	return 3*(1-x)**2*np.exp(-(x**2)-(y+1)**2)- 10*(x/5 - x**3 - y**5)*np.exp(-x**2-y**2)- 1/3**np.exp(-(x+1)**2 - y**2)
  18.  
  19. def plot_3d(ax):
  20.  
  21. 	X = np.linspace(*X_BOUND, 100)
  22. 	Y = np.linspace(*Y_BOUND, 100)
  23. 	X,Y = np.meshgrid(X, Y)
  24. 	Z = F(X, Y)
  25. 	ax.plot_surface(X,Y,Z,rstride=1,cstride=1,cmap=cm.coolwarm)
  26. 	ax.set_zlim(-10,10)
  27. 	ax.set_xlabel('x')
  28. 	ax.set_ylabel('y')
  29. 	ax.set_zlabel('z')
  30. 	plt.pause(3)
  31. 	plt.show()
  32.  
  33.  
  34. def get_fitness(pop): 
  35.     x,y = translateDNA(pop)
  36. 	pred = F(x, y)
  37. 	return (pred - np.min(pred)) + 1e-3 #减去最小的适应度是为了防止适应度出现负数，通过这一步fitness的范围为[0, np.max(pred)-np.min(pred)],最后在加上一个很小的数防止出现为0的适应度
  38.  
  39.  
  40. def translateDNA(pop): #pop表示种群矩阵，一行表示一个二进制编码表示的DNA，矩阵的行数为种群数目
  41. 	x_pop = pop[:,1::2]#奇数列表示X
  42. 	y_pop = pop[:,::2] #偶数列表示y
  43. 	
  44. 	#pop:(POP_SIZE,DNA_SIZE)*(DNA_SIZE,1) --> (POP_SIZE,1)
  45. 	x = x_pop.dot(2**np.arange(DNA_SIZE)[::-1])/float(2**DNA_SIZE-1)*(X_BOUND[1]-X_BOUND[0])+X_BOUND[0]
  46. 	y = y_pop.dot(2**np.arange(DNA_SIZE)[::-1])/float(2**DNA_SIZE-1)*(Y_BOUND[1]-Y_BOUND[0])+Y_BOUND[0]
  47. 	return x,y
  48.  
  49. def crossover_and_mutation(pop, CROSSOVER_RATE = 0.8):
  50. 	new_pop = []
  51. 	for father in pop:		#遍历种群中的每一个个体，将该个体作为父亲
  52. 		child = father		#孩子先得到父亲的全部基因（这里我把一串二进制串的那些0，1称为基因）
  53. 		if np.random.rand() < CROSSOVER_RATE:			#产生子代时不是必然发生交叉，而是以一定的概率发生交叉
  54. 			mother = pop[np.random.randint(POP_SIZE)]	#再种群中选择另一个个体，并将该个体作为母亲
  55. 			cross_points = np.random.randint(low=0, high=DNA_SIZE*2)	#随机产生交叉的点
  56. 			child[cross_points:] = mother[cross_points:]		#孩子得到位于交叉点后的母亲的基因
  57. 		mutation(child)	#每个后代有一定的机率发生变异
  58. 		new_pop.append(child)
  59.  
  60. 	return new_pop
  61.  
  62. def mutation(child, MUTATION_RATE=0.003):
  63. 	if np.random.rand() < MUTATION_RATE: 				#以MUTATION_RATE的概率进行变异
  64. 		mutate_point = np.random.randint(0, DNA_SIZE*2)	#随机产生一个实数，代表要变异基因的位置
  65. 		child[mutate_point] = child[mutate_point]^1 	#将变异点的二进制为反转
  66.  
  67. def select(pop, fitness):    # nature selection wrt pop's fitness
  68.     idx = np.random.choice(np.arange(POP_SIZE), size=POP_SIZE, replace=True,
  69.                            p=(fitness)/(fitness.sum()) )
  70.     return pop[idx]
  71.  
  72. def print_info(pop):
  73. 	fitness = get_fitness(pop)
  74. 	max_fitness_index = np.argmax(fitness)
  75. 	print("max_fitness:", fitness[max_fitness_index])
  76. 	x,y = translateDNA(pop)
  77. 	print("最优的基因型：", pop[max_fitness_index])
  78. 	print("(x, y):", (x[max_fitness_index], y[max_fitness_index]))
  79.  
  80.  
  81. if __name__ == "__main__":
  82. 	fig = plt.figure()
  83. 	ax = Axes3D(fig)	
  84. 	plt.ion()#将画图模式改为交互模式，程序遇到plt.show不会暂停，而是继续执行
  85. 	plot_3d(ax)
  86.  
  87. 	pop = np.random.randint(2, size=(POP_SIZE, DNA_SIZE*2)) #matrix (POP_SIZE, DNA_SIZE)
  88. 	for _ in range(N_GENERATIONS):#迭代N代
  89. 		x,y = translateDNA(pop)
  90. 		if 'sca' in locals(): 
  91. 			sca.remove()
  92. 		sca = ax.scatter(x, y, F(x,y), c='black', marker='o');plt.show();plt.pause(0.1)
  93. 		pop = np.array(crossover_and_mutation(pop, CROSSOVER_RATE))
  94. 		#F_values = F(translateDNA(pop)[0], translateDNA(pop)[1])#x, y --> Z matrix
  95. 		fitness = get_fitness(pop)
  96. 		pop = select(pop, fitness) #选择生成新的种群
  97. 	
  98. 	print_info(pop)
  99. 	plt.ioff()
  100. 	plot_3d(ax)

参考;https://blog.csdn.net/ha_ha_ha233/article/details/91364937 

四、粒子群算法

  1. import numpy as np
  2. import matplotlib.pyplot as plt
  3. from mpl_toolkits.mplot3d import Axes3D
  4.  
  5.  
  6. def fit_fun(x):  # 适应函数
  7.     return sum(100.0 * (x[0][1:] - x[0][:-1] ** 2.0) ** 2.0 + (1 - x[0][:-1]) ** 2.0)
  8.  
  9.  
  10. class Particle:
  11.     # 初始化
  12.     def __init__(self, x_max, max_vel, dim):
  13.         self.__pos = np.random.uniform(-x_max, x_max, (1, dim))  # 粒子的位置
  14.         self.__vel = np.random.uniform(-max_vel, max_vel, (1, dim))  # 粒子的速度
  15.         self.__bestPos = np.zeros((1, dim))  # 粒子最好的位置
  16.         self.__fitnessValue = fit_fun(self.__pos)  # 适应度函数值
  17.  
  18.     def set_pos(self, value):
  19.         self.__pos = value
  20.  
  21.     def get_pos(self):
  22.         return self.__pos
  23.  
  24.     def set_best_pos(self, value):
  25.         self.__bestPos = value
  26.  
  27.     def get_best_pos(self):
  28.         return self.__bestPos
  29.  
  30.     def set_vel(self, value):
  31.         self.__vel = value
  32.  
  33.     def get_vel(self):
  34.         return self.__vel
  35.  
  36.     def set_fitness_value(self, value):
  37.         self.__fitnessValue = value
  38.  
  39.     def get_fitness_value(self):
  40.         return self.__fitnessValue
  41.  
  42.  
  43. class PSO:
  44.     def __init__(self, dim, size, iter_num, x_max, max_vel, tol, best_fitness_value=float('Inf'), C1=2, C2=2, W=1):
  45.         self.C1 = C1
  46.         self.C2 = C2
  47.         self.W = W
  48.         self.dim = dim  # 粒子的维度
  49.         self.size = size  # 粒子个数
  50.         self.iter_num = iter_num  # 迭代次数
  51.         self.x_max = x_max
  52.         self.max_vel = max_vel  # 粒子最大速度
  53.         self.tol = tol  # 截至条件
  54.         self.best_fitness_value = best_fitness_value
  55.         self.best_position = np.zeros((1, dim))  # 种群最优位置
  56.         self.fitness_val_list = []  # 每次迭代最优适应值
  57.  
  58.         # 对种群进行初始化
  59.         self.Particle_list = [Particle(self.x_max, self.max_vel, self.dim) for i in range(self.size)]
  60.  
  61.     def set_bestFitnessValue(self, value):
  62.         self.best_fitness_value = value
  63.  
  64.     def get_bestFitnessValue(self):
  65.         return self.best_fitness_value
  66.  
  67.     def set_bestPosition(self, value):
  68.         self.best_position = value
  69.  
  70.     def get_bestPosition(self):
  71.         return self.best_position
  72.  
  73.     # 更新速度
  74.     def update_vel(self, part):
  75.         vel_value = self.W * part.get_vel() + self.C1 * np.random.rand() * (part.get_best_pos() - part.get_pos()) \
  76.                     + self.C2 * np.random.rand() * (self.get_bestPosition() - part.get_pos())
  77.         vel_value[vel_value > self.max_vel] = self.max_vel
  78.         vel_value[vel_value < -self.max_vel] = -self.max_vel
  79.         part.set_vel(vel_value)
  80.  
  81.     # 更新位置
  82.     def update_pos(self, part):
  83.         pos_value = part.get_pos() + part.get_vel()
  84.         part.set_pos(pos_value)
  85.         value = fit_fun(part.get_pos())
  86.         if value < part.get_fitness_value():
  87.             part.set_fitness_value(value)
  88.             part.set_best_pos(pos_value)
  89.         if value < self.get_bestFitnessValue():
  90.             self.set_bestFitnessValue(value)
  91.             self.set_bestPosition(pos_value)
  92.  
  93.     def update_ndim(self):
  94.  
  95.         for i in range(self.iter_num):
  96.             for part in self.Particle_list:
  97.                 self.update_vel(part)  # 更新速度
  98.                 self.update_pos(part)  # 更新位置
  99.             self.fitness_val_list.append(self.get_bestFitnessValue())  # 每次迭代完把当前的最优适应度存到列表
  100.             print('第{}次最佳适应值为{}'.format(i, self.get_bestFitnessValue()))
  101.             if self.get_bestFitnessValue() < self.tol:
  102.                 break
  103.  
  104.         return self.fitness_val_list, self.get_bestPosition()
  105.  
  106. if __name__ == '__main__':
  107.     # test 香蕉函数
  108.     pso = PSO(4, 5, 10000, 30, 60, 1e-4, C1=2, C2=2, W=1)
  109.     fit_var_list, best_pos = pso.update_ndim()
  110.     print("最优位置:" + str(best_pos))
  111.     print("最优解:" + str(fit_var_list[-1]))
  112.     plt.plot(range(len(fit_var_list)), fit_var_list, alpha=0.5)
'
运行

参考：https://zhuanlan.zhihu.com/p/378904190 

五、蚁群算法

  1. # -*- coding: utf-8 -*-
  2. import random
  3. import copy
  4. import time
  5. import sys
  6. import math
  7. import tkinter #//GUI模块
  8. import threading
  9. from functools import reduce
  10.  
  11.  
  12. # 参数
  13. '''
  14. ALPHA:信息启发因子，值越大，则蚂蚁选择之前走过的路径可能性就越大
  15.       ，值越小，则蚁群搜索范围就会减少，容易陷入局部最优
  16. BETA:Beta值越大，蚁群越就容易选择局部较短路径，这时算法收敛速度会
  17.      加快，但是随机性不高，容易得到局部的相对最优
  18. '''
  19. (ALPHA, BETA, RHO, Q) = (1.0,2.0,0.5,100.0)
  20. # 城市数，蚁群
  21. (city_num, ant_num) = (50,50)
  22. distance_x = [
  23.     178,272,176,171,650,499,267,703,408,437,491,74,532,
  24.     416,626,42,271,359,163,508,229,576,147,560,35,714,
  25.     757,517,64,314,675,690,391,628,87,240,705,699,258,
  26.     428,614,36,360,482,666,597,209,201,492,294]
  27. distance_y = [
  28.     170,395,198,151,242,556,57,401,305,421,267,105,525,
  29.     381,244,330,395,169,141,380,153,442,528,329,232,48,
  30.     498,265,343,120,165,50,433,63,491,275,348,222,288,
  31.     490,213,524,244,114,104,552,70,425,227,331]
  32. #城市距离和信息素
  33. distance_graph = [ [0.0 for col in range(city_num)] for raw in range(city_num)]
  34. pheromone_graph = [ [1.0 for col in range(city_num)] for raw in range(city_num)]
  35.  
  36.  
  37.  
  38. #----------- 蚂蚁 -----------
  39. class Ant(object):
  40.  
  41.     # 初始化
  42.     def __init__(self,ID):
  43.         
  44.         self.ID = ID                 # ID
  45.         self.__clean_data()          # 随机初始化出生点
  46.  
  47.     # 初始数据
  48.     def __clean_data(self):
  49.     
  50.         self.path = []               # 当前蚂蚁的路径           
  51.         self.total_distance = 0.0    # 当前路径的总距离
  52.         self.move_count = 0          # 移动次数
  53.         self.current_city = -1       # 当前停留的城市
  54.         self.open_table_city = [True for i in range(city_num)] # 探索城市的状态
  55.         
  56.         city_index = random.randint(0,city_num-1) # 随机初始出生点
  57.         self.current_city = city_index
  58.         self.path.append(city_index)
  59.         self.open_table_city[city_index] = False
  60.         self.move_count = 1
  61.     
  62.     # 选择下一个城市
  63.     def __choice_next_city(self):
  64.         
  65.         next_city = -1
  66.         select_citys_prob = [0.0 for i in range(city_num)]  #存储去下个城市的概率
  67.         total_prob = 0.0
  68.  
  69.         # 获取去下一个城市的概率
  70.         for i in range(city_num):
  71.             if self.open_table_city[i]:
  72.                 try :
  73.                     # 计算概率：与信息素浓度成正比，与距离成反比
  74.                     select_citys_prob[i] = pow(pheromone_graph[self.current_city][i], ALPHA) * pow((1.0/distance_graph[self.current_city][i]), BETA)
  75.                     total_prob += select_citys_prob[i]
  76.                 except ZeroDivisionError as e:
  77.                     print ('Ant ID: {ID}, current city: {current}, target city: {target}'.format(ID = self.ID, current = self.current_city, target = i))
  78.                     sys.exit(1)
  79.         
  80.         # 轮盘选择城市
  81.         if total_prob > 0.0:
  82.             # 产生一个随机概率,0.0-total_prob
  83.             temp_prob = random.uniform(0.0, total_prob)
  84.             for i in range(city_num):
  85.                 if self.open_table_city[i]:
  86.                     # 轮次相减
  87.                     temp_prob -= select_citys_prob[i]
  88.                     if temp_prob < 0.0:
  89.                         next_city = i
  90.                         break
  91.  
  92.         # 未从概率产生，顺序选择一个未访问城市
  93.         # if next_city == -1:
  94.         #     for i in range(city_num):
  95.         #         if self.open_table_city[i]:
  96.         #             next_city = i
  97.         #             break
  98.  
  99.         if (next_city == -1):
  100.             next_city = random.randint(0, city_num - 1)
  101.             while ((self.open_table_city[next_city]) == False):  # if==False,说明已经遍历过了
  102.                 next_city = random.randint(0, city_num - 1)
  103.  
  104.         # 返回下一个城市序号
  105.         return next_city
  106.     
  107.     # 计算路径总距离
  108.     def __cal_total_distance(self):
  109.         
  110.         temp_distance = 0.0
  111.  
  112.         for i in range(1, city_num):
  113.             start, end = self.path[i], self.path[i-1]
  114.             temp_distance += distance_graph[start][end]
  115.  
  116.         # 回路
  117.         end = self.path[0]
  118.         temp_distance += distance_graph[start][end]
  119.         self.total_distance = temp_distance
  120.         
  121.     
  122.     # 移动操作
  123.     def __move(self, next_city):
  124.         
  125.         self.path.append(next_city)
  126.         self.open_table_city[next_city] = False
  127.         self.total_distance += distance_graph[self.current_city][next_city]
  128.         self.current_city = next_city
  129.         self.move_count += 1
  130.         
  131.     # 搜索路径
  132.     def search_path(self):
  133.  
  134.         # 初始化数据
  135.         self.__clean_data()
  136.  
  137.         # 搜素路径，遍历完所有城市为止
  138.         while self.move_count < city_num:
  139.             # 移动到下一个城市
  140.             next_city =  self.__choice_next_city()
  141.             self.__move(next_city)
  142.  
  143.         # 计算路径总长度
  144.         self.__cal_total_distance()
  145.  
  146. #----------- TSP问题 -----------
  147.         
  148. class TSP(object):
  149.  
  150.     def __init__(self, root, width = 800, height = 600, n = city_num):
  151.  
  152.         # 创建画布
  153.         self.root = root                               
  154.         self.width = width      
  155.         self.height = height
  156.         # 城市数目初始化为city_num
  157.         self.n = n
  158.         # tkinter.Canvas
  159.         self.canvas = tkinter.Canvas(
  160.                 root,
  161.                 width = self.width,
  162.                 height = self.height,
  163.                 bg = "#EBEBEB",             # 背景白色 
  164.                 xscrollincrement = 1,
  165.                 yscrollincrement = 1
  166.             )
  167.         self.canvas.pack(expand = tkinter.YES, fill = tkinter.BOTH)
  168.         self.title("TSP蚁群算法(n:初始化 e:开始搜索 s:停止搜索 q:退出程序)")
  169.         self.__r = 5
  170.         self.__lock = threading.RLock()     # 线程锁
  171.  
  172.         self.__bindEvents()
  173.         self.new()
  174.  
  175.         # 计算城市之间的距离
  176.         for i in range(city_num):
  177.             for j in range(city_num):
  178.                 temp_distance = pow((distance_x[i] - distance_x[j]), 2) + pow((distance_y[i] - distance_y[j]), 2)
  179.                 temp_distance = pow(temp_distance, 0.5)
  180.                 distance_graph[i][j] =float(int(temp_distance + 0.5))
  181.  
  182.     # 按键响应程序
  183.     def __bindEvents(self):
  184.  
  185.         self.root.bind("q", self.quite)        # 退出程序
  186.         self.root.bind("n", self.new)          # 初始化
  187.         self.root.bind("e", self.search_path)  # 开始搜索
  188.         self.root.bind("s", self.stop)         # 停止搜索
  189.  
  190.     # 更改标题
  191.     def title(self, s):
  192.  
  193.         self.root.title(s)
  194.  
  195.     # 初始化
  196.     def new(self, evt = None):
  197.  
  198.         # 停止线程
  199.         self.__lock.acquire()
  200.         self.__running = False
  201.         self.__lock.release()
  202.  
  203.         self.clear()     # 清除信息 
  204.         self.nodes = []  # 节点坐标
  205.         self.nodes2 = [] # 节点对象
  206.  
  207.         # 初始化城市节点
  208.         for i in range(len(distance_x)):
  209.             # 在画布上随机初始坐标
  210.             x = distance_x[i]
  211.             y = distance_y[i]
  212.             self.nodes.append((x, y))
  213.             # 生成节点椭圆，半径为self.__r
  214.             node = self.canvas.create_oval(x - self.__r,
  215.                     y - self.__r, x + self.__r, y + self.__r,
  216.                     fill = "#ff0000",      # 填充红色
  217.                     outline = "#000000",   # 轮廓白色
  218.                     tags = "node",
  219.                 )
  220.             self.nodes2.append(node)
  221.             # 显示坐标
  222.             self.canvas.create_text(x,y-10,              # 使用create_text方法在坐标（302，77）处绘制文字
  223.                     text = '('+str(x)+','+str(y)+')',    # 所绘制文字的内容
  224.                     fill = 'black'                       # 所绘制文字的颜色为灰色
  225.                 )
  226.             
  227.         # 顺序连接城市
  228.         #self.line(range(city_num))
  229.         
  230.         # 初始城市之间的距离和信息素
  231.         for i in range(city_num):
  232.             for j in range(city_num):
  233.                 pheromone_graph[i][j] = 1.0
  234.                 
  235.         self.ants = [Ant(ID) for ID in range(ant_num)]  # 初始蚁群
  236.         self.best_ant = Ant(-1)                          # 初始最优解
  237.         self.best_ant.total_distance = 1 << 31           # 初始最大距离
  238.         self.iter = 1                                    # 初始化迭代次数 
  239.             
  240.     # 将节点按order顺序连线
  241.     def line(self, order):
  242.         # 删除原线
  243.         self.canvas.delete("line")
  244.         def line2(i1, i2):
  245.             p1, p2 = self.nodes[i1], self.nodes[i2]
  246.             self.canvas.create_line(p1, p2, fill = "#000000", tags = "line")
  247.             return i2
  248.         
  249.         # order[-1]为初始值
  250.         reduce(line2, order, order[-1])
  251.  
  252.     # 清除画布
  253.     def clear(self):
  254.         for item in self.canvas.find_all():
  255.             self.canvas.delete(item)
  256.  
  257.     # 退出程序
  258.     def quite(self, evt):
  259.         self.__lock.acquire()
  260.         self.__running = False
  261.         self.__lock.release()
  262.         self.root.destroy()
  263.         print (u"\n程序已退出...")
  264.         sys.exit()
  265.  
  266.     # 停止搜索
  267.     def stop(self, evt):
  268.         self.__lock.acquire()
  269.         self.__running = False
  270.         self.__lock.release()
  271.         
  272.     # 开始搜索
  273.     def search_path(self, evt = None):
  274.  
  275.         # 开启线程
  276.         self.__lock.acquire()
  277.         self.__running = True
  278.         self.__lock.release()
  279.         
  280.         while self.__running:
  281.             # 遍历每一只蚂蚁
  282.             for ant in self.ants:
  283.                 # 搜索一条路径
  284.                 ant.search_path()
  285.                 # 与当前最优蚂蚁比较
  286.                 if ant.total_distance < self.best_ant.total_distance:
  287.                     # 更新最优解
  288.                     self.best_ant = copy.deepcopy(ant)
  289.             # 更新信息素
  290.             self.__update_pheromone_gragh()
  291.             print (u"迭代次数：",self.iter,u"最佳路径总距离：",int(self.best_ant.total_distance))
  292.             # 连线
  293.             self.line(self.best_ant.path)
  294.             # 设置标题
  295.             self.title("TSP蚁群算法(n:随机初始 e:开始搜索 s:停止搜索 q:退出程序) 迭代次数: %d" % self.iter)
  296.             # 更新画布
  297.             self.canvas.update()
  298.             self.iter += 1
  299.  
  300.     # 更新信息素
  301.     def __update_pheromone_gragh(self):
  302.  
  303.         # 获取每只蚂蚁在其路径上留下的信息素
  304.         temp_pheromone = [[0.0 for col in range(city_num)] for raw in range(city_num)]
  305.         for ant in self.ants:
  306.             for i in range(1,city_num):
  307.                 start, end = ant.path[i-1], ant.path[i]
  308.                 # 在路径上的每两个相邻城市间留下信息素，与路径总距离反比
  309.                 temp_pheromone[start][end] += Q / ant.total_distance
  310.                 temp_pheromone[end][start] = temp_pheromone[start][end]
  311.  
  312.         # 更新所有城市之间的信息素，旧信息素衰减加上新迭代信息素
  313.         for i in range(city_num):
  314.             for j in range(city_num):
  315.                 pheromone_graph[i][j] = pheromone_graph[i][j] * RHO + temp_pheromone[i][j]
  316.  
  317.     # 主循环
  318.     def mainloop(self):
  319.         self.root.mainloop()
  320.  
  321. #----------- 程序的入口处 -----------
  322.                 
  323. if __name__ == '__main__':
  324.  
  325.     print (u""" 
  326. --------------------------------------------------------
  327.     程序：蚁群算法解决TPS问题程序 
  328.     作者：许彬 
  329.     日期：2015-12-10
  330.     语言：Python 2.7 
  331. -------------------------------------------------------- 
  332.     """)
  333.     TSP(tkinter.Tk()).mainloop()

参考;https://blog.csdn.net/fanxin_i/article/details/80380733

 

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/Guff_9hys/article/detail/848813
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号