├── README.md ├── LICENSE ├── Multi_drawmap.py ├── Single_mcts.py ├── Draw_map.py └── Multi_mcts.py /README.md: -------------------------------------------------------------------------------- 1 | # PathPlannig-via-MCTS 2 | 基于蒙特卡洛树搜索算法实现多机器人区域覆盖路径规划，并将覆盖结果可视化 3 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2022 Quark-zc 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /Multi_drawmap.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | import seaborn as sns 4 | from matplotlib import colors 5 | 6 | # 单个机器人覆盖绘图 7 | 8 | def draw_map(rows, cols, pass_point, trails1,trails2,trails3, obstacles): 9 | field = np.ones((rows, cols)) 10 | for i in range(len(pass_point)): 11 | field[pass_point[i][0] - 1, pass_point[i][1] - 1] = 1 # 注释：横纵坐标-1是因为障碍物和通行点的坐标定义是从下标1开始的,但field()函数下标从1开始 12 | for j in range(len(trails1)): 13 | field[trails1[j][0] - 1, trails1[j][1] - 1] = 4 14 | for k in range(len(trails2)): 15 | field[trails2[k][0] - 1, trails2[k][1] - 1] = 5 16 | for l in range(len(trails3)): 17 | field[trails3[l][0] - 1, trails3[l][1] - 1] = 6 18 | for m in range(len(obstacles)): 19 | field[obstacles[m][0] - 1, obstacles[m][1] - 1] = 2 20 | # 显示当前机器人位置 21 | # field[trails[j][0] - 1, trails[j][1] - 1] = 6 22 | 23 | cmap = colors.ListedColormap(['none', 'white', 'black', 'red', 'yellow', 'magenta', 'green', 'cyan', 'blue']) 24 | 25 | plt.figure(figsize=(cols, rows)) 26 | ax = plt.gca() 27 | 28 | sns.heatmap(field, cmap=cmap, vmin=0, vmax=8, linewidths=1.25, linecolor='black', ax=ax, cbar=False) 29 | 30 | # 设置图标题 31 | ax.set_ylabel('rows') 32 | ax.set_xlabel('cols') 33 | 34 | # 将列标签移动到图像上方 35 | ax.xaxis.tick_top() 36 | ax.xaxis.set_label_position('top') 37 | 38 | # 设置图标的数字个数文字，放在plt.show下面能居中 39 | ax.set_xticks(np.arange(rows)) 40 | ax.set_yticks(np.arange(cols)) 41 | 42 | return field 43 | 44 | def draw_path(trails1,trails2,trails3): 45 | # todo 改好绘图函数 46 | draw_map(rows, cols, pass_point, trails1,trails2,trails3, obstacles) 47 | 48 | for i in range(len(trails1)-1): 49 | x = trails1[i][1] # x,y需要反过来 50 | y = trails1[i][0] 51 | dx = trails1[i+1][1] - x 52 | dy = trails1[i+1][0] - y 53 | plt.arrow(x-0.5,y-0.5,dx,dy,width=0.1,head_width=0.1,head_length=0.1) 54 | 55 | for i in range(len(trails2) - 1): 56 | x = trails2[i][1] # x,y需要反过来 57 | y = trails2[i][0] 58 | dx = trails2[i + 1][1] - x 59 | dy = trails2[i + 1][0] - y 60 | plt.arrow(x - 0.5, y - 0.5, dx, dy, width=0.2, head_width=0.1, head_length=0.1) 61 | 62 | for i in range(len(trails3) - 1): 63 | x = trails3[i][1] # x,y需要反过来 64 | y = trails3[i][0] 65 | dx = trails3[i + 1][1] - x 66 | dy = trails3[i + 1][0] - y 67 | plt.arrow(x - 0.5, y - 0.5, dx, dy, width=0.3, head_width=0.1, head_length=0.1) 68 | # plt.arrow(x-0.5,y-0.5,dx,dy,width=0.01) 69 | 70 | 71 | 72 | def main(rows, cols, pass_point, trails1,trails2,trails3, obstacles): 73 | 74 | # plt.savefig('map_10x10.png') 75 | draw_path(trails1,trails2,trails3) 76 | plt.show() 77 | 78 | # plt.arrow(3, 2, -1, -1, width=0.001, head_width=0.005, head_length=0.1) 79 | # plt.show() 80 | 81 | rows,cols=10,10 82 | # 83 | pass_point = [(1, 1), (1, 2), (1, 4), (1, 5), (1, 6), (1, 7), (1, 8), (1, 9), (1, 10), (2, 1), (2, 2), (2, 4), (2, 5), (2, 6), (2, 7), (2, 8), (2, 9), (2, 10), (3, 1), (3, 2), (3, 4), (3, 5), (3, 6), (3, 7), (3, 8), (3, 9), (3, 10), (4, 1), (4, 2), (4, 4), (4, 5), (4, 6), (4, 7), (4, 8), (4, 9), (4, 10), (5, 1), (5, 2), (5, 3), (5, 4), (5, 5), (5, 9), (5, 10), (6, 1), (6, 2), (6, 3), (6, 4), (6, 5), (6, 6), (6, 7), (6, 8), (6, 9), (6, 10), (7, 1), (7, 2), (7, 3), (7, 4), (7, 5), (7, 6), (7, 7), (7, 8), (7, 9), (7, 10), (8, 1), (8, 2), (8, 3), (8, 4), (8, 5), (8, 6), (8, 7), (8, 8), (8, 9), (8, 10), (9, 1), (9, 2), (9, 3), (9, 4), (9, 5), (9, 6), (9, 7), (9, 8), (9, 9), (9, 10), (10, 1), (10, 2), (10, 3), (10, 4), (10, 5), (10, 6), (10, 7), (10, 8), (10, 9), (10, 10)] 84 | obstacles = [(1,3), (2,3), (3,3), (4,3), (5,6), (5,7), (5,8)] 85 | # pass_point = [(1, 1), (1, 2), (1, 3), (1, 4), (1, 5), (1, 6), (1, 7), (1, 8), (1, 9), (1, 10), (2, 1), (2, 2), (2, 3), (2, 4), (2, 5), (2, 6), (2, 7), (2, 8), (2, 9), (2, 10), (3, 1), (3, 2), (3, 3), (3, 4), (3, 5), (3, 6), (3, 7), (3, 8), (3, 9), (3, 10), (4, 1), (4, 2), (4, 3), (4, 4), (4, 5), (4, 6), (4, 7), (4, 8), (4, 9), (4, 10), (5, 1), (5, 2), (5, 3), (5, 4), (5, 5), (5, 6), (5, 7), (5, 8), (5, 9), (5, 10), (6, 1), (6, 2), (6, 3), (6, 4), (6, 5), (6, 6), (6, 7), (6, 8), (6, 9), (6, 10), (7, 1), (7, 2), (7, 3), (7, 4), (7, 5), (7, 6), (7, 7), (7, 8), (7, 9), (7, 10), (8, 1), (8, 2), (8, 3), (8, 4), (8, 5), (8, 6), (8, 7), (8, 8), (8, 9), (8, 10), (9, 1), (9, 2), (9, 3), (9, 4), (9, 5), (9, 6), (9, 7), (9, 8), (9, 9), (9, 10), (10, 1), (10, 2), (10, 3), (10, 4), (10, 5), (10, 6), (10, 7), (10, 8), (10, 9), (10, 10)] 86 | # obstacles = [] 87 | 88 | 89 | # rows, cols =5,5 90 | # pass_point = [(1, 1), (1, 2), (1, 4), (1, 5), (2, 1), (2, 2) ,(2, 4), (2, 5), (3, 1), (3, 2), (3, 3), (3, 4), (3, 5), (4, 1), (4, 2), (4, 3), (4, 4), (4, 5), (5, 1), (5, 2), (5, 3), (5, 4), (5, 5)] 91 | # obstacles = [(1,3),(2,3)] 92 | # coveraged = [(1,1),(5,2),(1,5)] 93 | 94 | # trails1=[(1, 1), (2, 1), (3, 1), (4, 1), (5, 1), (6, 1), (7, 1), (8, 1), (9, 1), (10, 1), (10, 2), (10, 3), (9, 3), (8, 3), (7, 3), (6, 3), (5, 3), (6, 3), (7, 3), (8, 3), (9, 3), (10, 3), (10, 2), (9, 2), (8, 2), (7, 2), (6, 2), (5, 2), (4, 2), (3, 2), (2, 2), (1, 2)] 95 | # trails2= [ (4, 6), (4, 7), (4, 8), (3, 8), (3, 7), (3, 6), (3, 5), (4, 5), (5, 5), (5, 4), (4, 4), (3, 4), (2, 4), (2, 5), (2, 6), (2, 7), (2, 8), (2, 9), (3, 9), (4, 9), (5, 9), (5, 10), (4, 10), (3, 10), (2, 10), (1, 10), (1, 9), (1, 8), (1, 7), (1, 6), (1, 5),(1,4)] 96 | # trails3=[(8,8),(8, 7), (8, 6), (7, 6), (7, 7), (7, 8), (7, 9), (8, 9), (9, 9), (9, 8), (9, 7), (9, 6), (9, 5), (8, 5), (7, 5), (6, 5), (6, 4), (7, 4), (8, 4), (9, 4), (10, 4), (10, 5), (10, 6), (10, 7), (10, 8), (10, 9), (10, 10), (9, 10), (8, 10), (7, 10), (6, 10), (6, 9), (6, 8), (6, 7), (6, 6), (6, 5)] # 机器人3 97 | 98 | # trails1= [(2, 1), (3, 1), (3, 2), (4, 1), (4, 2), (5, 1), (5, 2), (5, 3), (5, 4), (5, 5), (6, 1), (6, 2), (6, 3), (6, 4), (6, 5), (6, 6), (7, 1), (7, 2), (7, 3), (7, 4), (7, 5), (8, 1), (8, 2), (8, 3), (8, 4), (8, 5), (9, 1), (9, 2), (9, 3), (9, 4), (10, 1), (10, 2), (10, 3), (10, 4)] 99 | # trails2= [(1, 1), (1, 2), (1, 4), (1, 5), (1, 6), (1, 7), (1, 8), (1, 9), (1, 10), (2, 2), (2, 4), (2, 5), (2, 6), (2, 7), (2, 8), (2, 9), (2, 10), (3, 4), (3, 5), (3, 6), (3, 7), (3, 8), (3, 9), (3, 10), (4, 4), (4, 5), (4, 6), (4, 7), (4, 8), (4, 9)] 100 | # trails3= [(4, 10), (5, 9), (5, 10), (6, 7), (6, 8), (6, 9), (6, 10), (7, 6), (7, 7), (7, 8), (7, 9), (7, 10), (8, 6), (8, 7), (8, 8), (8, 9), (8, 10), (9, 5), (9, 6), (9, 7), (9, 8), (9, 9), (9, 10), (10, 5), (10, 6), (10, 7), (10, 8), (10, 9), (10, 10)] 101 | 102 | # trails1= [(10, 4), (10, 4), (9, 4), (8, 4), (7, 4), (6, 4), (5, 4), (4, 4), (3, 4), (2, 4), (1, 4), (1, 5), (1, 6), (1, 7), (1, 8), (1, 9), (1, 10), (2, 10), (3, 10), (4, 10), (5, 10), (6, 10), (7, 10), (8, 10), (9, 10), (10, 10), (10, 9), (10, 8), (10, 7), (10, 6), (10, 5), (10, 4), (10, 3), (10, 2), (10, 1), (9, 1), (8, 1), (7, 1), (6, 1), (5, 1), (4, 1), (3, 1), (2, 1)] 103 | # # trails2= [(2, 24), (2, 24), (2, 23), (2, 22), (2, 21), (2, 20), (2, 19), (2, 18), (2, 17), (2, 16), (2, 15), (2, 16), (2, 17), (2, 18), (2, 19), (2, 20), (2, 21), (2, 21), (2, 21), (2, 21), (2, 21), (2, 21), (2, 21), (2, 21), (2, 21), (2, 21), (2, 20), (2, 19), (2, 18), (2, 17), (2, 16), (2, 15), (2, 14), (2, 13), (2, 12), (2, 11), (2, 10), (2, 9), (2, 8), (2, 7), (2, 6), (2, 5), (2, 4)] 104 | # # trails3= [(21, 11), (21, 11), (20, 11), (19, 11), (18, 11), (17, 11), (16, 11), (15, 11), (14, 11), (13, 11), (12, 11), (11, 11), (10, 11), (9, 11), (8, 11), (7, 11), (6, 11), (7, 11), (8, 11), (9, 11), (10, 11), (11, 11), (12, 11), (13, 11), (14, 11), (15, 11), (15, 10), (15, 9), (15, 8), (15, 7), (15, 6), (15, 5), (15, 4), (15, 3), (15, 2), (14, 2), (13, 2), (12, 2), (11, 2), (10, 2), (9, 2), (8, 2), (7, 2)] 105 | # trails2,trails3 =[],[] 106 | 107 | # trails1= [(3, 1), (3, 1), (4, 1), (5, 1), (5, 2), (5, 3), (5, 4), (4, 4), (3, 4), (2, 4), (1, 4)] 108 | # trails2= [(5, 2), (5, 2), (4, 2), (3, 2), (2, 2), (1, 2), (1, 1), (2, 1), (3, 1), (3, 2), (3, 3)] 109 | # trails3= [(5, 5), (5, 5), (4, 5), (3, 5), (2, 5), (1, 5), (2, 5), (3, 5), (3, 4), (3, 3), (4, 3)] 110 | 111 | # trails1= [(1, 4), (1, 4), (2, 4), (3, 4), (4, 4), (5, 4), (6, 4), (7, 4), (8, 4), (9, 4), (10, 4), (9, 4), (8, 4), (7, 4), (6, 4), (5, 4), (4, 4), (3, 4), (3, 5), (3, 6), (3, 7), (3, 8), (4, 8), (4, 7), (4, 6), (3, 6), (2, 6), (2, 7), (2, 8), (2, 9), (3, 9), (4, 9), (5, 9), (5, 10), (6, 10), (7, 10), (8, 10), (9, 10), (10, 10), (10, 9), (10, 8), (10, 7), (10, 6), (10, 5), (10, 4), (10, 3), (10, 2), (10, 1), (10, 2), (10, 3), (10, 4), (10, 5), (10, 6), (10, 7), (10, 8), (10, 9), (10, 10), (10, 9), (10, 8), (10, 7), (10, 6), (10, 5), (10, 4), (10, 3), (10, 2), (9, 2), (8, 2), (7, 2), (6, 2), (5, 2), (4, 2), (3, 2), (2, 2), (1, 2), (1, 1), (1, 2), (1, 1), (1, 2), (2, 2), (3, 2), (4, 2), (4, 1), (5, 1), (6, 1), (7, 1), (8, 1), (8, 2), (8, 3), (8, 4), (7, 4), (7, 3), (7, 2), (8, 2), (9, 2), (9, 3), (9, 4), (9, 5), (8, 5), (7, 5), (6, 5), (5, 5), (5, 4), (5, 3), (5, 2), (5, 1), (4, 1), (3, 1)] 112 | # trails2= [(4, 10), (4, 10), (3, 10), (2, 10), (1, 10), (2, 10), (3, 10), (4, 10), (5, 10), (6, 10), (7, 10), (8, 10), (9, 10), (10, 10), (10, 9), (10, 8), (10, 7), (10, 6), (10, 5), (10, 4), (10, 3), (10, 2), (10, 1), (10, 2), (10, 3), (10, 4), (10, 5), (10, 6), (10, 7), (10, 8), (10, 9), (10, 10), (10, 9), (10, 8), (10, 7), (10, 6), (10, 5), (10, 4), (10, 3), (10, 2), (10, 1), (9, 1), (8, 1), (7, 1), (6, 1), (5, 1), (4, 1), (3, 1), (2, 1), (1, 1), (2, 1), (3, 1), (4, 1), (5, 1), (6, 1), (7, 1), (8, 1), (9, 1), (10, 1), (10, 2), (10, 3), (10, 4), (10, 5), (10, 6), (10, 7), (10, 8), (10, 9), (10, 10), (10, 9), (10, 8), (10, 7), (10, 6), (10, 5), (10, 4), (10, 3), (10, 2), (10, 1), (9, 1), (8, 1), (7, 1), (6, 1), (6, 2), (6, 3), (6, 4), (6, 5), (6, 6), (6, 7), (6, 8), (6, 9), (6, 10), (5, 10), (4, 10), (3, 10), (2, 10), (1, 10), (1, 9), (1, 8), (1, 7), (1, 6), (1, 5), (2, 5), (3, 5), (4, 5), (5, 5), (5, 4), (4, 4), (3, 4)] 113 | # trails3= [(3, 10), (3, 10), (4, 10), (5, 10), (6, 10), (7, 10), (8, 10), (9, 10), (10, 10), (10, 9), (10, 8), (10, 7), (10, 6), (10, 5), (10, 4), (10, 3), (10, 2), (10, 1), (9, 1), (8, 1), (7, 1), (6, 1), (5, 1), (4, 1), (3, 1), (2, 1), (1, 1), (2, 1), (3, 1), (4, 1), (5, 1), (6, 1), (7, 1), (8, 1), (9, 1), (10, 1), (9, 1), (8, 1), (7, 1), (6, 1), (5, 1), (4, 1), (3, 1), (2, 1), (1, 1), (1, 2), (1, 1), (2, 1), (3, 1), (4, 1), (5, 1), (6, 1), (7, 1), (8, 1), (9, 1), (10, 1), (9, 1), (8, 1), (7, 1), (6, 1), (5, 1), (4, 1), (3, 1), (2, 1), (1, 1), (2, 1), (3, 1), (4, 1), (5, 1), (6, 1), (7, 1), (8, 1), (8, 2), (8, 3), (8, 4), (8, 5), (8, 6), (8, 7), (8, 8), (8, 9), (9, 9), (9, 8), (9, 7), (9, 6), (8, 6), (7, 6), (7, 7), (7, 8), (7, 9), (7, 10), (8, 10), (9, 10), (10, 10), (10, 9), (10, 8), (10, 7), (10, 6), (10, 5), (10, 4), (10, 3), (10, 2), (10, 1), (9, 1), (8, 1), (7, 1), (6, 1), (6, 2)] 114 | 115 | # trails1= [(2, 10), (2, 10), (3, 10), (4, 10), (5, 10), (6, 10), (7, 10), (8, 10), (9, 10), (10, 10), (9, 10), (8, 10), (7, 10), (6, 10), (5, 10), (4, 10), (3, 10), (2, 10), (1, 10), (1, 9), (1, 8), (1, 7), (1, 6), (1, 5), (1, 4), (1, 3), (1, 2), (1, 1), (1, 2), (1, 3), (1, 4), (1, 5), (1, 6), (1, 7), (1, 8), (1, 9), (1, 10), (2, 10), (3, 10), (4, 10), (5, 10), (6, 10), (7, 10), (8, 10), (8, 9), (8, 8), (8, 7), (9, 7), (9, 8), (9, 9), (8, 9), (7, 9), (7, 8), (7, 7), (7, 6), (8, 6), (9, 6), (10, 6), (10, 5), (10, 4), (10, 3), (10, 2), (10, 1), (9, 1), (8, 1), (7, 1), (6, 1), (5, 1), (4, 1), (3, 1)] 116 | # trails2= [(2, 1), (2, 1), (3, 1), (4, 1), (5, 1), (6, 1), (7, 1), (8, 1), (9, 1), (10, 1), (9, 1), (8, 1), (7, 1), (6, 1), (5, 1), (4, 1), (3, 1), (3, 2), (3, 3), (3, 4), (3, 5), (3, 6), (3, 7), (3, 8), (3, 9), (3, 10), (3, 9), (3, 8), (3, 7), (3, 6), (3, 5), (3, 4), (2, 4), (2, 5), (2, 6), (2, 7), (2, 8), (2, 9), (3, 9), (4, 9), (4, 8), (4, 7), (4, 6), (4, 5), (4, 4), (4, 3), (3, 3), (2, 3), (1, 3), (1, 2), (2, 2), (3, 2), (4, 2), (5, 2), (5, 3), (5, 4), (5, 5), (5, 6), (5, 7), (5, 8), (5, 9), (5, 10), (4, 10), (3, 10), (2, 10), (1, 10), (1, 9), (1, 8), (1, 7), (1, 6)] 117 | # trails3= [(6, 10), (6, 10), (5, 10), (4, 10), (3, 10), (2, 10), (1, 10), (2, 10), (3, 10), (4, 10), (5, 10), (6, 10), (7, 10), (8, 10), (9, 10), (10, 10), (9, 10), (8, 10), (7, 10), (6, 10), (5, 10), (4, 10), (3, 10), (2, 10), (1, 10), (2, 10), (3, 10), (4, 10), (5, 10), (6, 10), (6, 9), (6, 8), (6, 7), (6, 6), (6, 5), (6, 4), (6, 3), (6, 2), (6, 1), (7, 1), (8, 1), (8, 2), (8, 3), (8, 4), (8, 5), (9, 5), (9, 4), (9, 3), (9, 2), (8, 2), (7, 2), (7, 3), (7, 4), (7, 5), (7, 6), (8, 6), (9, 6), (10, 6), (10, 7), (10, 8), (10, 9), (10, 10), (9, 10), (8, 10), (7, 10), (6, 10), (6, 9), (6, 8), (6, 7), (6, 6)] 118 | 119 | # trails1= [(2, 10), (2, 10), (3, 10), (4, 10), (5, 10), (6, 10), (7, 10), (8, 10), (9, 10), (10, 10), (9, 10), (8, 10), (7, 10), (6, 10), (5, 10), (4, 10), (3, 10), (2, 10), (1, 10), (1, 9), (1, 8), (1, 7), (1, 6), (1, 5), (1, 4), (1, 3), (1, 2), (1, 1), (1, 2), (1, 3), (1, 4), (1, 5), (1, 6), (1, 7), (1, 8), (1, 9), (1, 10), (2, 10), (3, 10), (4, 10), (5, 10), (6, 10), (7, 10), (8, 10), (8, 9), (8, 8), (8, 7), (9, 7), (9, 8), (9, 9), (8, 9), (7, 9), (7, 8), (7, 7), (7, 6), (8, 6), (9, 6), (10, 6), (10, 5), (10, 4), (10, 3), (10, 2), (10, 1), (9, 1), (8, 1), (7, 1), (6, 1), (5, 1), (4, 1), (3, 1)] 120 | # trails2= [(2, 1), (2, 1), (3, 1), (4, 1), (5, 1), (6, 1), (7, 1), (8, 1), (9, 1), (10, 1), (9, 1), (8, 1), (7, 1), (6, 1), (5, 1), (4, 1), (3, 1), (3, 2), (3, 3), (3, 4), (3, 5), (3, 6), (3, 7), (3, 8), (3, 9), (3, 10), (3, 9), (3, 8), (3, 7), (3, 6), (3, 5), (3, 4), (2, 4), (2, 5), (2, 6), (2, 7), (2, 8), (2, 9), (3, 9), (4, 9), (4, 8), (4, 7), (4, 6), (4, 5), (4, 4), (4, 3), (3, 3), (2, 3), (1, 3), (1, 2), (2, 2), (3, 2), (4, 2), (5, 2), (5, 3), (5, 4), (5, 5), (5, 6), (5, 7), (5, 8), (5, 9), (5, 10), (4, 10), (3, 10), (2, 10), (1, 10), (1, 9), (1, 8), (1, 7), (1, 6)] 121 | # trails3= [(6, 10), (6, 10), (5, 10), (4, 10), (3, 10), (2, 10), (1, 10), (2, 10), (3, 10), (4, 10), (5, 10), (6, 10), (7, 10), (8, 10), (9, 10), (10, 10), (9, 10), (8, 10), (7, 10), (6, 10), (5, 10), (4, 10), (3, 10), (2, 10), (1, 10), (2, 10), (3, 10), (4, 10), (5, 10), (6, 10), (6, 9), (6, 8), (6, 7), (6, 6), (6, 5), (6, 4), (6, 3), (6, 2), (6, 1), (7, 1), (8, 1), (8, 2), (8, 3), (8, 4), (8, 5), (9, 5), (9, 4), (9, 3), (9, 2), (8, 2), (7, 2), (7, 3), (7, 4), (7, 5), (7, 6), (8, 6), (9, 6), (10, 6), (10, 7), (10, 8), (10, 9), (10, 10), (9, 10), (8, 10), (7, 10), (6, 10), (6, 9), (6, 8), (6, 7), (6, 6)] 122 | 123 | trails1= [(1, 9), (1, 9), (1, 8), (1, 7), (1, 6), (1, 5), (1, 4), (1, 5), (1, 6), (1, 7), (1, 8), (1, 9), (1, 10), (2, 10), (3, 10), (4, 10), (5, 10), (6, 10), (7, 10), (8, 10), (9, 10), (10, 10), (9, 10), (8, 10), (8, 9), (8, 8), (8, 7), (8, 6), (8, 5), (8, 4), (8, 3), (7, 3), (7, 4), (7, 5), (7, 6), (7, 7), (7, 8), (7, 9), (7, 10), (8, 10), (9, 10), (10, 10), (10, 9), (10, 8), (10, 7), (10, 6), (10, 5), (10, 4), (10, 3), (10, 2), (10, 1), (9, 1), (8, 1), (7, 1), (6, 1), (5, 1), (4, 1), (3, 1)] 124 | trails2= [(1,1),(2, 1), (2, 1), (3, 1), (4, 1), (5, 1), (6, 1), (7, 1), (8, 1), (9, 1), (10, 1), (10, 2), (10, 3), (10, 4), (10, 5), (10, 6), (10, 7), (10, 8), (10, 9), (10, 10), (9, 10), (8, 10), (7, 10), (6, 10), (5, 10), (4, 10), (3, 10), (2, 10), (1, 10), (1, 9), (2, 9), (3, 9), (4, 9), (5, 9), (5, 10), (4, 10), (3, 10), (2, 10), (1, 10), (2, 10), (2, 9), (2, 8), (2, 7), (2, 6), (2, 5), (2, 4), (3, 4), (3, 5), (3, 6), (3, 7), (3, 8), (3, 9), (3, 10), (2, 10), (1, 10), (1, 9), (1, 8), (1, 7), (1, 6)] 125 | trails3= [(1, 5), (1, 5), (2, 5), (3, 5), (4, 5), (5, 5), (6, 5), (6, 4), (6, 3), (6, 2), (5, 2), (4, 2), (3, 2), (2, 2), (1, 2), (2, 2), (3, 2), (4, 2), (4, 1), (5, 1), (6, 1), (7, 1), (8, 1), (8, 2), (8, 3), (8, 4), (8, 5), (8, 6), (8, 7), (8, 8), (8, 9), (9, 9), (9, 8), (9, 7), (9, 6), (9, 5), (9, 4), (9, 3), (9, 2), (8, 2), (7, 2), (6, 2), (5, 2), (5, 3), (5, 4), (4, 4), (4, 5), (4, 6), (4, 7), (4, 8), (4, 9), (4, 10), (5, 10), (6, 10), (6, 9), (6, 8), (6, 7), (6, 6)] 126 | 127 | 128 | 129 | # pass_point = [(1, 1), (1, 2), (2, 1), (2, 2), (3, 1), (3, 2), (4, 1), (4, 2), (5, 1), (5, 2), (5, 3), (6, 1), (6, 2), (6, 3), (7, 1), (7, 2), (7, 3), (8, 1), (8, 2), (8, 3), (9, 1), (9, 2), (9, 3), (10, 1), (10, 2), (10, 3)] 130 | # obstacles = [(1,3),(2,3),(3,3),(4,3)] # 机器人1 131 | 132 | # draw_map(rows, cols, pass_point, trails1,trails2,trails3, obstacles) 133 | # draw_path(trails1,trails2,trails3) 134 | # plt.show() 135 | 136 | 137 | # trails = [(1,1),(2,1),(3,1),(4,1),(5,1),(5,2),(4,2),(3,2),(2,2),(1,2)] # 测试绘图效果 138 | # main(rows, cols, pass_point, trails, obstacles) 139 | main(rows, cols, pass_point,trails1,trails2,trails3, obstacles) 140 | -------------------------------------------------------------------------------- /Single_mcts.py: -------------------------------------------------------------------------------- 1 | """ 2 | 5.14 单机器人MCTS覆盖算法仿真 3 | 4 | """ 5 | import sys 6 | 7 | from treelib import Tree, Node 8 | import random 9 | import math 10 | import matplotlib.pyplot as plt 11 | import time 12 | 13 | 14 | ''' 15 | 1.主函数部分 16 | ''' 17 | 18 | # todo 覆盖过的栅格标志位设置为0.5， 未覆盖的为0 ， 障碍物为1 19 | # todo 算法效果差主要是随机策略设计的有问题 20 | 21 | def tree_update(node_id, avail_move): # 更新树:输入父节点id和可行动作集合,随机扩展一个子节点并返回其id 22 | 23 | choice = random.choice(avail_move) 24 | while choice == 0: 25 | choice = random.choice(avail_move) 26 | 27 | x = tree.get_node(nid=node_id).data[0][0] 28 | y = tree.get_node(nid=node_id).data[0][1] 29 | 30 | sub_node_id = 0 31 | 32 | if (choice == 1): 33 | sub_node = Node(tag='straight', data=[(x - 1, y), 0, 10e-5, 1,1e5]) 34 | tree.add_node(sub_node, parent=node_id) 35 | sub_node_id = sub_node.identifier 36 | coveraged.append((x - 1, y)) 37 | 38 | if (choice == 2): 39 | sub_node = Node(tag='back', data=[(x + 1, y), 0, 10e-5, 2,1e5]) 40 | tree.add_node(sub_node, parent=node_id) 41 | sub_node_id = sub_node.identifier 42 | coveraged.append(((x + 1, y))) 43 | 44 | if (choice == 3): 45 | sub_node = Node(tag='turn left', data=[(x, y - 1), 0, 10e-5, 3,1e5]) 46 | tree.add_node(sub_node, parent=node_id) 47 | sub_node_id = sub_node.identifier 48 | coveraged.append((x, y - 1)) 49 | 50 | if (choice == 4): 51 | sub_node = Node(tag='turn right', data=[(x, y + 1), 0, 10e-5, 4,1e5]) 52 | tree.add_node(sub_node, parent=node_id) 53 | sub_node_id = sub_node.identifier 54 | coveraged.append((x, y + 1)) 55 | 56 | return (sub_node_id) 57 | 58 | def default_policy(node_id): # 根据内螺旋策略向下选择一个节点进行仿真，遇障碍物或者已覆盖栅格右转 59 | 60 | # todo move = tree.parent(node_id).data[3] 61 | move = tree.get_node(node_id).data[3] 62 | all_moves = get_moni_move(node_id) 63 | if all_moves == [0,0,0,0]: # 陷入死区 64 | all_moves = get_simulation_move(node_id) 65 | 66 | if move in all_moves: 67 | sub_id = tree_update(node_id, [move]) 68 | else: 69 | move = random.choice(all_moves) 70 | while move == 0: 71 | move = random.choice(all_moves) 72 | sub_id = tree_update(node_id,[move]) 73 | 74 | return sub_id 75 | 76 | def is_end(leaf_id): # 判断当前叶子节点表示状态是否为结束局面 77 | 78 | pass 79 | 80 | 81 | def get_expand_move(node_id): # 获得可扩展的操作 82 | 83 | children = tree.children(nid=node_id) 84 | expand_avail_move = [1, 2, 3, 4] 85 | 86 | for i in range(len(children)): # 将已经扩展的节点置零 87 | if children[i].tag == 'straight': 88 | expand_avail_move[0] = 0 89 | elif children[i].tag == 'back': 90 | expand_avail_move[1] = 0 91 | elif children[i].tag == 'turn left': 92 | expand_avail_move[2] = 0 93 | elif children[i].tag == 'turn right': 94 | expand_avail_move[3] = 0 95 | 96 | x = tree.get_node(nid=node_id).data[0][0] # 将不在通行区域的操作置零 97 | y = tree.get_node(nid=node_id).data[0][1] 98 | if (((x - 1, y) in pass_point) == 0): 99 | expand_avail_move[0] = 0 100 | if (((x + 1, y) in pass_point) == 0): 101 | expand_avail_move[1] = 0 102 | if (((x, y - 1) in pass_point) == 0): 103 | expand_avail_move[2] = 0 104 | if (((x, y + 1) in pass_point) == 0): 105 | expand_avail_move[3] = 0 106 | 107 | return (expand_avail_move) 108 | 109 | 110 | def get_simulation_move(node_id): # 获得可模拟的操作 111 | 112 | simulation_avail_move = [1, 2, 3, 4] 113 | x = tree.get_node(nid=node_id).data[0][0] 114 | y = tree.get_node(nid=node_id).data[0][1] 115 | 116 | if (((x - 1, y) in pass_point) == 0): 117 | simulation_avail_move[0] = 0 118 | if (((x + 1, y) in pass_point) == 0): 119 | simulation_avail_move[1] = 0 120 | if (((x, y - 1) in pass_point) == 0): 121 | simulation_avail_move[2] = 0 122 | if (((x, y + 1) in pass_point) == 0): 123 | simulation_avail_move[3] = 0 124 | 125 | return (simulation_avail_move) 126 | 127 | def get_moni_move(node_id): # 获得可模拟的操作 128 | 129 | simulation_avail_move = [1, 2, 3, 4] 130 | x = tree.get_node(nid=node_id).data[0][0] 131 | y = tree.get_node(nid=node_id).data[0][1] 132 | 133 | if (((x - 1, y) in pass_point) == 0): 134 | simulation_avail_move[0] = 0 135 | if (((x + 1, y) in pass_point) == 0): 136 | simulation_avail_move[1] = 0 137 | if (((x, y - 1) in pass_point) == 0): 138 | simulation_avail_move[2] = 0 139 | if (((x, y + 1) in pass_point) == 0): 140 | simulation_avail_move[3] = 0 141 | 142 | if (((x - 1, y) in coveraged) == 1): 143 | simulation_avail_move[0] = 0 144 | if (((x + 1, y) in coveraged) == 1): 145 | simulation_avail_move[1] = 0 146 | if (((x, y - 1) in coveraged) == 1): 147 | simulation_avail_move[2] = 0 148 | if (((x, y + 1) in coveraged) == 1): 149 | simulation_avail_move[3] = 0 150 | 151 | return (simulation_avail_move) 152 | 153 | def p_cal(node_id): 154 | # todo reward和p的计算肯定有问题 155 | coveraged = [] 156 | parent_id = tree.parent(nid=node_id).identifier 157 | while parent_id != 'root': 158 | coveraged.append(tree.get_node(nid=parent_id).data[0]) 159 | parent_id = tree.parent(nid=parent_id).identifier 160 | 161 | coordinate = tree.get_node(nid=node_id).data[0] 162 | if coordinate in coveraged: 163 | p = 0 164 | else: 165 | p = 1 166 | 167 | return p 168 | 169 | def is_turn(node_id): # 判断机器人在这一步是否转向 , 计算转向损失值rk 170 | parent_move = tree.get_node(node_id).data[3] 171 | rk = 0 172 | 173 | parent_id = tree.parent(node_id).identifier 174 | grandpa_move = tree.get_node(parent_id).data[3] 175 | 176 | if parent_move != grandpa_move: # 转向rk=1 后退rk=10 177 | rk = 0.2 178 | if ((parent_move+grandpa_move) ==7) or ((parent_move+grandpa_move) ==3): 179 | rk = 0.5 180 | # todo 181 | 182 | return rk 183 | 184 | # def reward_cal(simulation_id,expand_id): # reward = Σ[p/(tk+1)^2] 185 | # 186 | # # todo reward计算过程错了 187 | # dt, k = 0.1, 0 188 | # reward = 0 189 | # node_id = simulation_id 190 | # 191 | # while node_id != expand_id: 192 | # p = p_cal(node_id) 193 | # k = tree.depth(tree.get_node(nid=node_id))-tree.depth(tree.get_node(nid=expand_id)) 194 | # tk = dt * k 195 | # rk = 0 #is_turn(node_id) 196 | # reward += (p-rk)/((tk+1)**2) 197 | # node_id = tree.parent(node_id).identifier 198 | # 199 | # return reward 200 | 201 | def reward_cal(node_id): # reward = Σ[p/(tk+1)^2] 202 | 203 | dt, k = 0.1, 0 204 | reward = 0 205 | 206 | while node_id != 'root': # 计算路径上的奖励值 207 | p = p_cal(node_id) 208 | k = tree.depth(tree.get_node(nid=node_id)) 209 | tk = dt * k 210 | rk = 0 #is_turn(node_id) 211 | reward += (p-rk)/((tk+1)**2) 212 | node_id = tree.parent(node_id).identifier 213 | 214 | # print('reward:',reward) 215 | 216 | return reward 217 | 218 | 219 | def UCT_cal(node_id): 220 | 221 | reward = tree.get_node(nid=node_id).data[1] 222 | visit = tree.get_node(nid=node_id).data[2] 223 | parent_id=tree.parent(nid=node_id).identifier 224 | parent_visit=tree.get_node(nid=parent_id).data[2] 225 | 226 | # todo 改变c值 227 | # c=1/(math.sqrt(2)) 228 | c = 0.00001 229 | 230 | left = reward / visit 231 | right = c*math.sqrt(2*math.log(parent_visit)/visit) 232 | 233 | # print('q:',left,'ucb:',right) 234 | 235 | UCT = left +right 236 | #todo 237 | data = tree.get_node(node_id).data 238 | tree.update_node(nid=node_id,data=[data[0],data[1],data[2],data[3],UCT]) #更新UCT值 239 | 240 | return UCT 241 | 242 | def coverage_cal(node_id): #计算有效的覆盖栅格数 243 | 244 | coverage = 0 245 | # if (tree.get_node(nid=node_id).is_leaf()): 246 | 247 | explore_trails=[(1,1)] 248 | 249 | while (tree.get_node(nid=node_id).is_root()==False): # 判断当前节点是否是根节点 250 | trail=tree.get_node(nid=node_id).data[0] 251 | explore_trails.append(trail) 252 | node_id=tree.parent(nid=node_id).identifier 253 | 254 | l1=explore_trails 255 | for el in range(len(l1) - 1, -1, -1): 256 | if l1.count(l1[el]) > 1: 257 | l1.pop(el) 258 | 259 | coverage = len(l1) 260 | 261 | return (coverage) 262 | 263 | def select(node_id): # 输入当前节点，选择最优的子节点id 264 | 265 | children = tree.children(node_id) 266 | children_UCT = [0, 0, 0, 0] 267 | for i in range(len(children)): 268 | 269 | children_UCT[i] = UCT_cal(children[i].identifier) 270 | 271 | best_j = 0 272 | 273 | max_UCT = children_UCT[0] 274 | # todo 改进UCT选择过程 275 | for j in range(len(children)): 276 | if children_UCT[j] >= max_UCT: 277 | max_UCT = children_UCT[j] 278 | best_j = j 279 | 280 | return (children[best_j].identifier) 281 | 282 | 283 | 284 | def backup(leaf_node_id, reward): # 从叶子节点出发，回溯更新整棵树 285 | node_id = leaf_node_id 286 | parent_id = tree.parent(nid=node_id).identifier 287 | while (tree.get_node(nid=parent_id).is_root() == 0): # 判断当前节点的父节点是否是根节点 288 | tree.get_node(nid=parent_id).data[1] += reward # 更新reward 289 | tree.get_node(nid=parent_id).data[2] += 1 # 更新visit_times 290 | node_id = tree.parent(nid=node_id).identifier 291 | parent_id = tree.parent(nid=parent_id).identifier 292 | tree.get_node(nid='root').data[1] += reward 293 | tree.get_node(nid='root').data[2] += 1 294 | 295 | 296 | ''' 297 | 2.MCTS过程 298 | ''' 299 | def MCTS_select(root_id): # 输入根节点id,清空select_node_trails[],重新计算并输出根节点到当前扩展层的最优节点序列select_node_trails 300 | 301 | node_id = root_id 302 | root_node = tree.get_node(nid=root_id) 303 | 304 | select_coordinate_trails = [(1, 1)] # 坐标序列 305 | select_node_trails = [root_node] # 节点序列 306 | 307 | while (tree.get_node(nid=node_id).is_leaf() == 0): # 当前节点是叶子节点,选择过程结束 308 | select_node_trail = tree.get_node(nid=select(node_id)) 309 | select_node_trails.append(select_node_trail) 310 | 311 | select_coordinate_trail = tree.get_node(nid=select(node_id)).data[0] 312 | select_coordinate_trails.append(select_coordinate_trail) 313 | 314 | node_id = select_node_trail.identifier 315 | 316 | select_trails = [select_node_trails, select_coordinate_trails] 317 | return (select_trails) 318 | 319 | 320 | def MCTS_expand(node_id): 321 | sub_id = tree_update(node_id, get_expand_move(node_id)) 322 | return (sub_id) # 返回子节点id 323 | ''' 324 | 模拟过程结束判定： 325 | 1.整个树的层数达到200或者仿真结束 326 | 模拟过程策略： 327 | 随机策略 vs 内螺旋策略 328 | ''' 329 | def MCTS_simulation(node_id): # 从扩展得到的节点出发向下模拟 330 | simulation_trails_id = [] # 仿真过程的节点id序列 331 | simulation_trails_coordinate = [] # 仿真过程的节点坐标序列 332 | 333 | depth = tree.depth(node=tree.get_node(node_id)) 334 | while depth <= 500: # 保证模拟过程后，树的总层数为200 335 | sub_id = default_policy(node_id) 336 | simulation_trails_id.append(sub_id) 337 | simulation_trails_coordinate.append(tree.get_node(nid=sub_id).data[0]) 338 | node_id = sub_id 339 | depth = tree.depth(node=tree.get_node(node_id)) 340 | mcts_coverage = coverage_cal(sub_id) / len(pass_point) 341 | # print('仿真覆盖率:', mcts_coverage) 342 | if mcts_coverage == 1.0: 343 | print('程序结束') 344 | explore_trails = [(1, 1)] 345 | 346 | node_id = sub_id 347 | while (tree.get_node(nid=node_id).is_root() == False): # 判断当前节点是否是根节点 348 | trail = tree.get_node(nid=node_id).data[0] 349 | explore_trails.append(trail) 350 | node_id = tree.parent(nid=node_id).identifier 351 | print('仿真得到的机器人路径：', explore_trails) 352 | print('重复率：', len(explore_trails) / len(pass_point)) 353 | sys.exit(0) 354 | 355 | 356 | 357 | # print('仿真节点数：',len(simulation_trails_coordinate)) 358 | 359 | simulation_trails = [simulation_trails_id, simulation_trails_coordinate] 360 | return (simulation_trails) 361 | 362 | 363 | # def MCTS_backup(simulation_id, expand_id): 364 | # reward = reward_cal(simulation_id,expand_id) 365 | # backup(expand_id, reward) 366 | def MCTS_backup(simulation_id, expand_id): 367 | reward = reward_cal(simulation_id) - reward_cal(expand_id) 368 | backup(expand_id, reward) 369 | 370 | 371 | def main(root_node_id, expect_coverage): # 输入起始节点和期望覆盖率,输出最优路径 372 | 373 | node_id = root_node_id 374 | mcts_coverage = 0 375 | step = 1 376 | x,y=[],[] 377 | Expect_coverage = expect_coverage 378 | select_trails = [] 379 | 380 | while (mcts_coverage < Expect_coverage): 381 | 382 | ex_avail_move = get_expand_move(node_id) 383 | while (ex_avail_move != [0, 0, 0, 0]): 384 | expand_node_id = MCTS_expand(node_id) 385 | ex_avail_move = get_expand_move(node_id) 386 | 387 | simulation_trails = MCTS_simulation(expand_node_id)[0] # 模拟得到的节点队列 388 | if len(simulation_trails) == 0: 389 | backup(expand_node_id, 0) 390 | 391 | # todo 随机报错 392 | elif coverage_cal(simulation_trails[-1]) < 0.5: 393 | backup(expand_node_id ,0) 394 | else: 395 | simulation_leaf_node_id = simulation_trails[-1] # 模拟过程的叶子节点 396 | 397 | MCTS_backup(simulation_leaf_node_id, expand_node_id) # 从模拟得到的叶子节点开始回溯 398 | 399 | 400 | # 仿真覆盖率计算 401 | # mcts_coverage = coverage_cal(simulation_leaf_node_id) / len(pass_point) 402 | # print('仿真覆盖率:', mcts_coverage) 403 | # if mcts_coverage == 1.0: 404 | # print('程序结束') 405 | # explore_trails = [(1, 1)] 406 | # 407 | # node_id = simulation_leaf_node_id 408 | # while (tree.get_node(nid=node_id).is_root() == False): # 判断当前节点是否是根节点 409 | # trail = tree.get_node(nid=node_id).data[0] 410 | # explore_trails.append(trail) 411 | # node_id = tree.parent(nid=node_id).identifier 412 | # print('仿真得到的机器人路径：',explore_trails) 413 | # print('重复率：',len(explore_trails)/len(pass_point)) 414 | # sys.exit(0) 415 | 416 | ''' 417 | 仿真结束后去除simulation过程创造的节点,使得蒙特卡洛树只保留expand得到的节点 418 | ''' 419 | expand_child_id = tree.children(nid=expand_node_id)[0].identifier 420 | tree.remove_subtree(nid=expand_child_id) 421 | 422 | # tree.show() 423 | # todo print('树节点数量：',len(tree.all_nodes())) 424 | # todo print(tree.children('root')) 425 | 426 | z = MCTS_select(root_node_id) 427 | select_id = z[0][-1].identifier 428 | select_trails =z[1] 429 | 430 | # print(z[1]) 431 | 432 | # tree.show() 433 | 434 | # print(tree.depth(tree.get_node(nid=select_id))) # 测试用，用后即删 435 | # print(tree.parent(nid=select_id)) 436 | 437 | mcts_coverage = coverage_cal(select_id) / len(pass_point) 438 | print('覆盖率:', mcts_coverage) 439 | 440 | # x.append(step) 441 | # y.append(mcts_coverage) 442 | # step += 1 443 | 444 | node_id = select_id # 更新mcts过程的开始节点 445 | 446 | # todo 绘制单机器人实时覆盖率 447 | # plt.plot(x,y) 448 | # plt.rcParams['font.sans-serif'] = ['SimHei'] 449 | # plt.title('单机器人实时覆盖率',loc='center') 450 | # plt.xlabel('机器人覆盖步长/（步）') 451 | # plt.ylabel('覆盖率/（无单位）') 452 | # plt.savefig("单机器人实时覆盖率.png") 453 | # plt.clf() 454 | # plt.cla() 455 | # plt.close('all') 456 | 457 | return (select_trails) 458 | 459 | # 初始化 460 | AVAILABLE_CHOICES = [1, 2, 3, 4] # 定义可选操作集合--->映射到机器人四个可选动作 上下左右 461 | 462 | pass_point = [(1, 1), (1, 2), (1, 4), (1, 5), (1, 6), (1, 7), (1, 8), (1, 9), (1, 10), (2, 1), (2, 2), (2, 4), (2, 5), (2, 6), (2, 7), (2, 8), (2, 9), (2, 10), (3, 1), (3, 2), (3, 4), (3, 5), (3, 6), (3, 7), (3, 8), (3, 9), (3, 10), (4, 1), (4, 2), (4, 4), (4, 5), (4, 6), (4, 7), (4, 8), (4, 9), (4, 10), (5, 1), (5, 2), (5, 3), (5, 4), (5, 5), (5, 9), (5, 10), (6, 1), (6, 2), (6, 3), (6, 4), (6, 5), (6, 6), (6, 7), (6, 8), (6, 9), (6, 10), (7, 1), (7, 2), (7, 3), (7, 4), (7, 5), (7, 6), (7, 7), (7, 8), (7, 9), (7, 10), (8, 1), (8, 2), (8, 3), (8, 4), (8, 5), (8, 6), (8, 7), (8, 8), (8, 9), (8, 10), (9, 1), (9, 2), (9, 3), (9, 4), (9, 5), (9, 6), (9, 7), (9, 8), (9, 9), (9, 10), (10, 1), (10, 2), (10, 3), (10, 4), (10, 5), (10, 6), (10, 7), (10, 8), (10, 9), (10, 10)] 463 | obstacles = [(1,3),(2,3),(3,3),(4,3),(5,6),(5,7),(5,8)] 464 | # pass_point = [(1, 1), (1, 2), (1, 4), (1, 5), (2, 1), (2, 2) ,(2, 4), (2, 5), (3, 1), (3, 4), (3, 5), (4, 1), (4, 2), (4, 3), (4, 4), (4, 5), (5, 1), (5, 2), (5, 3), (5, 4), (5, 5)] 465 | # obstacles = [(1,3),(2,3),(3,2),(3,3)] 466 | coveraged = [(1,1)] 467 | # pass_point= [(1, 1), (1, 2), (1, 3), (1, 4), (1, 5), (1, 6), (2, 2), (2, 3), (2, 6), (3, 1), (3, 2), (3, 3), (3, 5), (3, 6), (3, 7), (4, 1), (4, 2), (4, 3), (4, 4), (4, 5), (4, 6), (4, 7), (5, 1), (5, 2), (5, 3), (5, 5), (5, 6), (5, 7), (6, 1), (6, 2), (6, 3), (6, 4), (6, 5), (6, 6), (6, 7), (7, 1), (7, 2), (7, 3), (7, 4), (7, 5), (7, 6), (7, 7)] 468 | 469 | # obstacles=[(8, 8), (5, 10), (2, 3), (8, 5), (6, 2), (5, 4), (10, 1), (10, 6), (10, 3), (6, 1), (2, 5), (8, 4), (10, 9), (5, 7), (4, 4), (4, 8), (3, 4), (10, 8), (2, 4), (7, 4)] 470 | # pass_point=[(1, 1), (1, 2), (1, 3), (1, 4), (1, 5), (1, 6), (1, 7), (1, 8), (1, 9), (1, 10), (2, 1), (2, 2), (2, 6), (2, 7), (2, 8), (2, 9), (2, 10), (3, 1), (3, 2), (3, 3), (3, 5), (3, 6), (3, 7), (3, 8), (3, 9), (3, 10), (4, 1), (4, 2), (4, 3), (4, 5), (4, 6), (4, 7), (4, 9), (4, 10), (5, 1), (5, 2), (5, 3), (5, 5), (5, 6), (5, 8), (5, 9), (6, 3), (6, 4), (6, 5), (6, 6), (6, 7), (6, 8), (6, 9), (6, 10), (7, 1), (7, 2), (7, 3), (7, 5), (7, 6), (7, 7), (7, 8), (7, 9), (7, 10), (8, 1), (8, 2), (8, 3), (8, 6), (8, 7), (8, 9), (8, 10), (9, 1), (9, 2), (9, 3), (9, 4), (9, 5), (9, 6), (9, 7), (9, 8), (9, 9), (9, 10), (10, 2), (10, 4), (10, 5), (10, 7), (10, 10)] 471 | 472 | 473 | 474 | # 树结构定义 475 | tree = Tree() 476 | tree.create_node(tag='root', identifier='root', parent=None, 477 | data=[(1, 1), 0, 1, 1, 1]) # data[0]-坐标 data[1]-reward data[2]-visit data[3]-parent_move data[4]-UCT 478 | 479 | expect_coverage = 1.0 480 | round = 1 481 | 482 | start = time.perf_counter() 483 | robot_path = main('root', expect_coverage) # 机器人覆盖路径 484 | end = time.perf_counter() 485 | print('程序运行时间：', end - start) 486 | 487 | repeat = len(robot_path) / (len(pass_point) * expect_coverage) # 机器人重复覆盖率 488 | length = len(robot_path) 489 | print('机器人路径：', robot_path) 490 | print('重复覆盖率：', repeat) 491 | print('覆盖步长:', length) 492 | 493 | tree.save2file(filename='tree') 494 | 495 | -------------------------------------------------------------------------------- /Draw_map.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | import seaborn as sns 4 | from matplotlib import colors 5 | 6 | # 单个机器人覆盖绘图 7 | 8 | def draw_map(rows, cols, pass_point, trails, obstacles): 9 | field = np.ones((rows, cols)) 10 | for i in range(len(pass_point)): 11 | field[pass_point[i][0] - 1, pass_point[i][1] - 1] = 1 # 注释：横纵坐标-1是因为障碍物和通行点的坐标定义是从下标1开始的,但field()函数下标从1开始 12 | for j in range(len(trails)): 13 | field[trails[j][0] - 1, trails[j][1] - 1] = 4 14 | for k in range(len(obstacles)): 15 | field[obstacles[k][0] - 1, obstacles[k][1] - 1] = 2 16 | # 显示当前机器人位置 17 | # field[trails[j][0] - 1, trails[j][1] - 1] = 6 18 | 19 | cmap = colors.ListedColormap(['none', 'white', 'black', 'red', 'yellow', 'magenta', 'green', 'cyan', 'blue']) 20 | 21 | plt.figure(figsize=(cols, rows)) 22 | ax = plt.gca() 23 | 24 | sns.heatmap(field, cmap=cmap, vmin=0, vmax=8, linewidths=1.25, linecolor='black', ax=ax, cbar=False) 25 | 26 | # 设置图标题 27 | ax.set_ylabel('rows') 28 | ax.set_xlabel('cols') 29 | 30 | # 将列标签移动到图像上方 31 | ax.xaxis.tick_top() 32 | ax.xaxis.set_label_position('top') 33 | 34 | # 设置图标的数字个数文字，放在plt.show下面能居中 35 | ax.set_xticks(np.arange(rows)) 36 | ax.set_yticks(np.arange(cols)) 37 | 38 | return field 39 | 40 | def draw_path(path): 41 | # todo 改好绘图函数 42 | draw_map(rows, cols, pass_point, trails, obstacles) 43 | for i in range(len(path)-1): 44 | 45 | 46 | x = path[i][1] # x,y需要反过来 47 | y = path[i][0] 48 | dx = path[i+1][1] - x 49 | dy = path[i+1][0] - y 50 | plt.arrow(x-0.5,y-0.5,dx,dy,width=0.1,head_width=0.1,head_length=0.1) 51 | # plt.arrow(x-0.5,y-0.5,dx,dy,width=0.01) 52 | 53 | 54 | 55 | def main(rows, cols, pass_point, trails, obstacles): 56 | 57 | # plt.savefig('map_10x10.png') 58 | draw_path(trails) 59 | plt.show() 60 | 61 | # plt.arrow(3, 2, -1, -1, width=0.001, head_width=0.005, head_length=0.1) 62 | # plt.show() 63 | 64 | # trails = [] 65 | # rows, cols = 10, 10 66 | # obstacles=[(8, 8), (5, 10), (2, 3), (8, 5), (6, 2), (5, 4), (10, 1), (10, 6), (10, 3), (6, 1), (2, 5), (8, 4), (10, 9), (5, 7), (4, 4), (4, 8), (3, 4), (10, 8), (2, 4), (7, 4)] 67 | # pass_point=[(1, 1), (1, 2), (1, 3), (1, 4), (1, 5), (1, 6), (1, 7), (1, 8), (1, 9), (1, 10), (2, 1), (2, 2), (2, 6), (2, 7), (2, 8), (2, 9), (2, 10), (3, 1), (3, 2), (3, 3), (3, 5), (3, 6), (3, 7), (3, 8), (3, 9), (3, 10), (4, 1), (4, 2), (4, 3), (4, 5), (4, 6), (4, 7), (4, 9), (4, 10), (5, 1), (5, 2), (5, 3), (5, 5), (5, 6), (5, 8), (5, 9), (6, 3), (6, 4), (6, 5), (6, 6), (6, 7), (6, 8), (6, 9), (6, 10), (7, 1), (7, 2), (7, 3), (7, 5), (7, 6), (7, 7), (7, 8), (7, 9), (7, 10), (8, 1), (8, 2), (8, 3), (8, 6), (8, 7), (8, 9), (8, 10), (9, 1), (9, 2), (9, 3), (9, 4), (9, 5), (9, 6), (9, 7), (9, 8), (9, 9), (9, 10), (10, 2), (10, 4), (10, 5), (10, 7), (10, 10)] 68 | 69 | # obstacles= [(5, 8), (8, 7), (6, 1), (10, 2), (7, 7), (6, 2), (6, 10), (9, 7), (1, 4), (3, 10), (6, 4), (10, 8), (8, 3), (6, 5), (4, 4), (3, 7), (4, 3), (2, 10), (2, 7), (5, 5), (4, 1), (2, 6), (6, 9), (1, 3), (10, 4)] 70 | # pass_point= [(1, 1), (1, 2), (1, 5), (1, 6), (1, 7), (1, 8), (1, 9), (1, 10), (2, 1), (2, 2), (2, 3), (2, 4), (2, 5), (2, 8), (2, 9), (3, 1), (3, 2), (3, 3), (3, 4), (3, 5), (3, 6), (3, 8), (3, 9), (4, 2), (4, 5), (4, 6), (4, 7), (4, 8), (4, 9), (4, 10), (5, 1), (5, 2), (5, 3), (5, 4), (5, 6), (5, 7), (5, 9), (5, 10), (6, 3), (6, 6), (6, 7), (6, 8), (7, 1), (7, 2), (7, 3), (7, 4), (7, 5), (7, 6), (7, 8), (7, 9), (7, 10), (8, 1), (8, 2), (8, 4), (8, 5), (8, 6), (8, 8), (8, 9), (8, 10), (9, 1), (9, 2), (9, 3), (9, 4), (9, 5), (9, 6), (9, 8), (9, 9), (9, 10), (10, 1), (10, 3), (10, 5), (10, 6), (10, 7), (10, 9), (10, 10)] 71 | 72 | # obstacles= [(4, 5), (2, 7), (7, 3), (4, 8), (1, 1), (3, 4), (10, 7), (8, 8), (10, 1), (1, 8), (3, 1), (7, 1), (10, 3), (9, 6), (3, 7)] 73 | # pass_point= [(1, 2), (1, 3), (1, 4), (1, 5), (1, 6), (1, 7), (1, 9), (1, 10), (2, 1), (2, 2), (2, 3), (2, 4), (2, 5), (2, 6), (2, 8), (2, 9), (2, 10), (3, 2), (3, 3), (3, 5), (3, 6), (3, 8), (3, 9), (3, 10), (4, 1), (4, 2), (4, 3), (4, 4), (4, 6), (4, 7), (4, 9), (4, 10), (5, 1), (5, 2), (5, 3), (5, 4), (5, 5), (5, 6), (5, 7), (5, 8), (5, 9), (5, 10), (6, 1), (6, 2), (6, 3), (6, 4), (6, 5), (6, 6), (6, 7), (6, 8), (6, 9), (6, 10), (7, 2), (7, 4), (7, 5), (7, 6), (7, 7), (7, 8), (7, 9), (7, 10), (8, 1), (8, 2), (8, 3), (8, 4), (8, 5), (8, 6), (8, 7), (8, 9), (8, 10), (9, 1), (9, 2), (9, 3), (9, 4), (9, 5), (9, 7), (9, 8), (9, 9), (9, 10), (10, 2), (10, 4), (10, 5), (10, 6), (10, 8), (10, 9), (10, 10)] 74 | 75 | # obstacles=[(8, 8), (5, 10), (2, 3), (8, 5), (6, 2), (5, 4), (10, 1), (10, 6), (10, 3), (6, 1), (2, 5), (8, 4), (10, 9), (5, 7), (4, 4), (4, 8), (3, 4), (10, 8), (2, 4), (7, 4)] 76 | # pass_point=[(1, 1), (1, 2), (1, 3), (1, 4), (1, 5), (1, 6), (1, 7), (1, 8), (1, 9), (1, 10), (2, 1), (2, 2), (2, 6), (2, 7), (2, 8), (2, 9), (2, 10), (3, 1), (3, 2), (3, 3), (3, 5), (3, 6), (3, 7), (3, 8), (3, 9), (3, 10), (4, 1), (4, 2), (4, 3), (4, 5), (4, 6), (4, 7), (4, 9), (4, 10), (5, 1), (5, 2), (5, 3), (5, 5), (5, 6), (5, 8), (5, 9), (6, 3), (6, 4), (6, 5), (6, 6), (6, 7), (6, 8), (6, 9), (6, 10), (7, 1), (7, 2), (7, 3), (7, 5), (7, 6), (7, 7), (7, 8), (7, 9), (7, 10), (8, 1), (8, 2), (8, 3), (8, 6), (8, 7), (8, 9), (8, 10), (9, 1), (9, 2), (9, 3), (9, 4), (9, 5), (9, 6), (9, 7), (9, 8), (9, 9), (9, 10), (10, 2), (10, 4), (10, 5), (10, 7), (10, 10)] 77 | # rows,cols=10,10 78 | # trails = pass_point 79 | 80 | 81 | # pass_point = [(1, 1), (1, 2), (1, 3), (1, 4), (1, 5), (2, 1), (2, 2), (2, 3), (2, 4), (2, 5), (3, 1), (3, 2), (3, 3), (3, 4), (3, 5), (4, 1), (4, 2), (4, 3), (4, 4), (4, 5), (5, 1), (5, 2), (5, 3), (5, 4), (5, 5)] 82 | # obstacles = [] 83 | # rows, cols =5, 5 84 | # obstacles=[(8, 8), (5, 10), (2, 3), (8, 5), (6, 2), (5, 4), (10, 1), (10, 6), (10, 3), (6, 1), (2, 5), (8, 4), (10, 9), (5, 7), (4, 4), (4, 8), (3, 4), (10, 8), (2, 4), (7, 4)] 85 | # pass_point=[(1, 1), (1, 2), (1, 3), (1, 4), (1, 5), (1, 6), (1, 7), (1, 8), (1, 9), (1, 10), (2, 1), (2, 2), (2, 6), (2, 7), (2, 8), (2, 9), (2, 10), (3, 1), (3, 2), (3, 3), (3, 5), (3, 6), (3, 7), (3, 8), (3, 9), (3, 10), (4, 1), (4, 2), (4, 3), (4, 5), (4, 6), (4, 7), (4, 9), (4, 10), (5, 1), (5, 2), (5, 3), (5, 5), (5, 6), (5, 8), (5, 9), (6, 3), (6, 4), (6, 5), (6, 6), (6, 7), (6, 8), (6, 9), (6, 10), (7, 1), (7, 2), (7, 3), (7, 5), (7, 6), (7, 7), (7, 8), (7, 9), (7, 10), (8, 1), (8, 2), (8, 3), (8, 6), (8, 7), (8, 9), (8, 10), (9, 1), (9, 2), (9, 3), (9, 4), (9, 5), (9, 6), (9, 7), (9, 8), (9, 9), (9, 10), (10, 2), (10, 4), (10, 5), (10, 7), (10, 10)] 86 | rows,cols=10,10 87 | # # pass_point = [(1, 1), (1, 2), (1, 4), (1, 5), (1, 6), (1, 7), (1, 8), (1, 9), (1, 10), (2, 1), (2, 2), (2, 4), (2, 5), (2, 6), (2, 7), (2, 8), (2, 9), (2, 10), (3, 1), (3, 2), (3, 4), (3, 5), (3, 6), (3, 7), (3, 8), (3, 9), (3, 10), (4, 1), (4, 2), (4, 4), (4, 5), (4, 6), (4, 7), (4, 8), (4, 9), (4, 10), (5, 1), (5, 2), (5, 3), (5, 4), (5, 5), (5, 6), (5, 7), (5, 8), (5, 9), (5, 10), (6, 1), (6, 2), (6, 3), (6, 4), (6, 5), (6, 6), (6, 7), (6, 8), (6, 9), (6, 10), (7, 1), (7, 2), (7, 3), (7, 4), (7, 5), (7, 6), (7, 7), (7, 8), (7, 9), (7, 10), (8, 1), (8, 2), (8, 3), (8, 4), (8, 5), (8, 6), (8, 7), (8, 8), (8, 9), (8, 10), (9, 1), (9, 2), (9, 3), (9, 4), (9, 5), (9, 6), (9, 7), (9, 8), (9, 9), (9, 10), (10, 1), (10, 2), (10, 3), (10, 4), (10, 5), (10, 6), (10, 7), (10, 8), (10, 9), (10, 10)] 88 | # # obstacles = [(1,3),(2,3),(3,3),(4,3)] 89 | pass_point = [(1, 1), (1, 2), (1, 4), (1, 5), (1, 6), (1, 7), (1, 8), (1, 9), (1, 10), (2, 1), (2, 2), (2, 4), (2, 5), (2, 6), (2, 7), (2, 8), (2, 9), (2, 10), (3, 1), (3, 2), (3, 4), (3, 5), (3, 6), (3, 7), (3, 8), (3, 9), (3, 10), (4, 1), (4, 2), (4, 4), (4, 5), (4, 6), (4, 7), (4, 8), (4, 9), (4, 10), (5, 1), (5, 2), (5, 3), (5, 4), (5, 5), (5, 9), (5, 10), (6, 1), (6, 2), (6, 3), (6, 4), (6, 5), (6, 6), (6, 7), (6, 8), (6, 9), (6, 10), (7, 1), (7, 2), (7, 3), (7, 4), (7, 5), (7, 6), (7, 7), (7, 8), (7, 9), (7, 10), (8, 1), (8, 2), (8, 3), (8, 4), (8, 5), (8, 6), (8, 7), (8, 8), (8, 9), (8, 10), (9, 1), (9, 2), (9, 3), (9, 4), (9, 5), (9, 6), (9, 7), (9, 8), (9, 9), (9, 10), (10, 1), (10, 2), (10, 3), (10, 4), (10, 5), (10, 6), (10, 7), (10, 8), (10, 9), (10, 10)] 90 | obstacles = [(1,3), (2,3), (3,3), (4,3), (5,6), (5,7), (5,8)] 91 | # # trails= [(3, 6), (3, 7), (3, 8), (4, 8), (4, 7), (4, 6), (4, 5), (3, 5), (2, 5), (2, 6), (2, 7), (2, 8), (2, 9), (3, 9), (4, 9), (5, 9), (5, 10), (6, 10), (7, 10), (8, 10), (9, 10), (10, 10), (10, 9), (10, 8), (10, 7), (10, 6), (10, 5), (10, 4), (10, 3), (10, 2), (10, 1), (9, 1), (8, 1), (7, 1), (6, 1), (6, 2), (6, 3), (6, 4), (6, 5), (6, 6), (6, 7), (6, 8), (7, 8), (8, 8), (9, 8), (10, 8), (9, 8), (8, 8), (7, 8), (7, 7), (8, 7), (9, 7), (9, 8), (9, 9), (8, 9), (7, 9), (6, 9), (6, 8), (6, 7), (6, 6), (7, 6), (8, 6), (9, 6), (10, 6), (10, 7), (10, 8), (10, 9), (10, 10), (9, 10), (8, 10), (7, 10), (6, 10), (5, 10), (4, 10), (3, 10), (2, 10), (1, 10), (1, 9), (1, 8), (1, 7), (1, 6), (1, 5), (1, 4), (2, 4), (3, 4), (4, 4), (5, 4), (6, 4), (7, 4), (8, 4), (9, 4), (10, 4), (9, 4), (8, 4), (7, 4), (6, 4), (6, 3), (7, 3), (8, 3), (9, 3), (10, 3), (10, 4), (10, 5), (9, 5), (8, 5), (7, 5), (6, 5), (5, 5), (5, 4), (5, 3), (5, 2), (4, 2), (3, 2), (2, 2), (1, 2), (1, 1), (2, 1), (3, 1), (4, 1), (5, 1), (6, 1), (7, 1), (8, 1), (9, 1), (10, 1), (10, 2), (9, 2), (8, 2), (7, 2), (6, 2), (5, 2), (4, 2), (3, 2), (2, 2), (1, 2)] 92 | # 93 | # # trails = [(1, 1), (2, 1), (1, 1), (2, 1), (1, 1), (1, 2), (1, 3), (1, 2), (2, 2), (2, 1), (3, 1), (3, 2), (4, 2), (5, 2), (4, 2), (4, 1), (5, 1), (5, 2), (5, 3), (5, 4), (4, 4), (5, 4), (5, 5), (5, 4), (5, 5), (5, 4), (5, 3), (4, 3), (4, 4), (3, 4), (3, 3), (2, 3), (2, 4), (1, 4), (2, 4), (2, 5), (1, 5), (2, 5), (3, 5), (4, 5)] 94 | # 95 | # # trails = [(1, 1), (2, 1), (3, 1), (2, 1), (2, 2), (3, 2), (4, 2), (4, 1), (3, 1), (2, 1), (2, 2), (3, 2), (3, 1), (3, 2), (3, 1), (2, 1), (3, 1), (4, 1), (4, 2), (4, 1), (5, 1), (5, 2), (4, 2), (4, 1), (3, 1), (2, 1), (3, 1), (2, 1), (2, 2), (2, 1), (2, 2), (3, 2), (4, 2), (3, 2), (3, 1), (3, 2), (4, 2), (5, 2), (5, 1), (4, 1), (3, 1), (2, 1), (3, 1), (2, 1), (1, 1), (1, 2), (1, 1), (2, 1), (3, 1), (4, 1), (4, 2), (5, 2), (4, 2), (5, 2), (4, 2), (4, 3), (4, 2), (3, 2), (4, 2), (3, 2), (2, 2), (3, 2), (2, 2), (3, 2), (4, 2), (3, 2), (3, 3), (4, 3), (3, 3), (3, 2), (2, 2), (3, 2), (3, 3), (3, 2), (2, 2), (3, 2), (4, 2), (5, 2), (5, 1), (4, 1), (5, 1), (5, 2), (4, 2), (5, 2), (5, 3), (6, 3), (6, 4), (6, 5), (7, 5), (6, 5), (7, 5), (6, 5), (6, 6), (7, 6), (6, 6), (5, 6), (4, 6), (4, 5), (3, 5), (4, 5), (4, 6), (5, 6), (6, 6), (6, 7), (6, 6), (6, 5), (7, 5), (6, 5), (6, 4), (6, 3), (6, 4), (6, 3), (7, 3), (7, 2), (7, 1), (8, 1), (7, 1), (8, 1), (8, 2), (9, 2), (10, 2), (9, 2), (8, 2), (9, 2), (9, 1), (9, 2), (9, 1), (8, 1), (9, 1), (8, 1), (7, 1), (7, 2), (8, 2), (8, 1), (8, 2), (7, 2), (8, 2), (7, 2), (8, 2), (8, 3), (8, 2), (7, 2), (8, 2), (9, 2), (9, 1), (9, 2), (10, 2), (9, 2), (8, 2), (7, 2), (8, 2), (8, 1), (8, 2), (8, 3), (9, 3), (9, 2), (8, 2), (9, 2), (9, 1), (8, 1), (7, 1), (8, 1), (8, 2), (8, 3), (9, 3), (8, 3), (9, 3), (9, 2), (9, 1), (8, 1), (8, 2), (9, 2), (10, 2), (9, 2), (9, 1), (8, 1), (9, 1), (9, 2), (9, 1), (8, 1), (7, 1), (7, 2), (7, 1), (8, 1), (8, 2), (7, 2), (7, 3), (8, 3), (9, 3), (8, 3), (8, 2), (8, 1), (9, 1), (9, 2), (8, 2), (9, 2), (10, 2), (9, 2), (8, 2), (8, 1), (9, 1), (8, 1), (7, 1), (7, 2), (8, 2), (8, 3), (8, 2), (9, 2), (8, 2), (7, 2), (7, 1), (7, 2), (7, 3), (6, 3), (6, 4), (6, 3), (6, 4), (6, 3), (5, 3), (6, 3), (7, 3), (8, 3), (9, 3), (9, 4), (10, 4), (9, 4), (10, 4), (10, 5), (9, 5), (9, 6), (9, 7), (8, 7), (8, 6), (9, 6), (8, 6), (9, 6), (9, 7), (10, 7), (9, 7), (8, 7), (9, 7), (9, 8), (9, 7), (8, 7), (7, 7), (7, 6), (7, 5), (7, 6), (8, 6), (9, 6), (9, 5), (9, 6), (9, 7), (9, 6), (9, 5), (10, 5), (10, 4), (10, 5), (10, 4), (9, 4), (9, 5), (9, 6), (9, 7), (9, 8), (9, 9), (8, 9), (8, 10), (8, 9), (7, 9), (6, 9), (5, 9), (6, 9), (6, 10), (6, 9), (5, 9), (6, 9), (6, 10), (7, 10), (7, 9), (7, 8), (7, 9), (7, 8), (7, 7), (8, 7), (8, 6), (9, 6), (8, 6), (8, 7), (8, 6), (8, 7), (8, 6), (7, 6), (8, 6), (8, 7), (8, 6), (7, 6), (7, 7), (7, 8), (7, 9), (7, 10), (8, 10), (8, 9), (8, 10), (7, 10), (8, 10), (9, 10), (9, 9), (9, 8), (9, 7), (8, 7), (8, 6), (9, 6), (8, 6), (8, 7), (9, 7), (9, 8), (9, 7), (9, 6), (8, 6), (7, 6), (7, 5), (7, 6), (8, 6), (9, 6), (9, 5), (10, 5), (10, 4), (9, 4), (9, 5), (10, 5), (10, 4), (10, 5), (10, 4), (10, 5), (10, 4), (9, 4), (9, 5), (10, 5), (9, 5), (9, 4), (9, 5), (9, 6), (9, 5), (10, 5), (10, 4), (9, 4), (9, 5), (9, 4), (9, 3), (9, 4), (9, 5), (9, 4), (9, 3), (9, 2), (9, 3), (9, 4), (9, 5), (9, 4), (9, 3), (8, 3), (9, 3), (9, 4), (10, 4), (10, 5), (10, 4), (9, 4), (10, 4), (9, 4), (9, 3), (8, 3), (9, 3), (9, 2), (9, 3), (9, 2), (8, 2), (8, 1), (9, 1), (9, 2), (10, 2), (9, 2), (9, 3), (9, 2), (9, 3), (9, 2), (9, 1), (9, 2), (10, 2), (9, 2), (8, 2), (9, 2), (8, 2), (8, 1), (8, 2), (7, 2), (7, 3), (6, 3), (6, 4), (6, 3), (7, 3), (7, 2), (7, 3), (6, 3), (5, 3), (6, 3), (6, 4), (6, 3), (7, 3), (6, 3), (7, 3), (8, 3), (9, 3), (9, 4), (9, 3), (8, 3), (8, 2), (8, 3), (8, 2), (7, 2), (8, 2), (8, 1), (7, 1), (7, 2), (8, 2), (7, 2), (7, 1), (7, 2), (7, 3), (7, 2), (7, 3), (6, 3), (5, 3), (5, 2), (4, 2), (4, 1), (3, 1), (3, 2), (3, 3), (3, 2), (4, 2), (5, 2), (5, 3), (4, 3), (4, 2), (4, 3), (4, 2), (4, 3), (5, 3), (6, 3), (6, 4), (6, 3), (6, 4), (6, 3), (6, 4), (6, 5), (6, 4), (6, 5), (6, 6), (6, 5), (6, 6), (7, 6), (7, 7), (6, 7), (6, 6), (7, 6), (7, 7), (8, 7), (7, 7), (6, 7), (6, 8), (5, 8), (5, 9), (5, 8), (6, 8), (7, 8), (7, 9), (7, 10), (7, 9), (7, 8), (7, 7), (8, 7), (9, 7), (9, 8), (9, 7), (9, 8), (9, 9), (8, 9), (7, 9), (6, 9), (6, 8), (5, 8), (6, 8), (6, 7), (7, 7), (8, 7), (8, 6), (7, 6), (6, 6), (7, 6), (8, 6), (8, 7), (7, 7), (7, 8), (6, 8), (5, 8), (6, 8), (6, 7), (6, 6), (6, 5), (5, 5), (6, 5), (5, 5), (6, 5), (7, 5), (7, 6), (6, 6), (6, 7), (6, 8), (7, 8), (7, 9), (6, 9), (7, 9), (8, 9), (9, 9), (9, 10), (9, 9), (9, 10), (8, 10), (7, 10), (8, 10), (9, 10), (9, 9), (8, 9), (8, 10), (8, 9), (7, 9), (7, 10), (6, 10), (7, 10), (8, 10), (8, 9), (9, 9), (9, 10), (9, 9), (8, 9), (8, 10), (7, 10), (7, 9), (7, 10), (8, 10), (9, 10), (10, 10), (9, 10), (8, 10), (7, 10), (6, 10), (6, 9), (6, 10), (7, 10), (8, 10), (9, 10), (10, 10), (9, 10), (9, 9), (9, 8), (9, 9), (9, 10), (9, 9), (9, 8), (9, 9), (9, 10), (8, 10), (9, 10), (9, 9), (8, 9), (8, 10), (7, 10), (6, 10), (6, 9), (6, 8), (6, 9), (6, 10), (7, 10), (8, 10), (7, 10), (7, 9), (7, 8), (7, 7), (6, 7), (6, 8), (6, 9), (5, 9), (5, 8), (5, 9), (4, 9), (5, 9), (6, 9), (6, 10), (7, 10), (6, 10), (6, 9), (5, 9), (6, 9), (5, 9), (5, 8), (6, 8), (7, 8), (7, 7), (7, 8), (6, 8), (6, 9), (6, 10), (6, 9), (5, 9), (4, 9), (5, 9), (4, 9), (3, 9), (2, 9), (3, 9), (2, 9), (1, 9), (1, 10), (2, 10), (1, 10), (1, 9), (1, 10), (1, 9), (2, 9), (2, 8), (3, 8), (2, 8), (2, 7), (2, 8), (1, 8), (1, 7), (2, 7), (1, 7), (2, 7), (1, 7), (2, 7), (1, 7), (1, 8), (1, 9), (2, 9), (2, 8), (1, 8), (2, 8), (2, 7), (2, 8), (2, 9), (1, 9), (2, 9), (2, 10), (3, 10), (4, 10), (4, 9), (3, 9), (4, 9), (5, 9), (6, 9), (7, 9), (7, 8), (7, 7), (8, 7), (7, 7), (6, 7), (6, 6), (6, 5), (7, 5), (6, 5), (6, 6), (6, 7), (6, 8), (6, 9), (5, 9), (4, 9), (4, 10), (4, 9), (3, 9), (2, 9), (2, 10), (3, 10), (4, 10), (3, 10), (3, 9), (4, 9), (4, 10), (3, 10), (3, 9), (3, 10), (3, 9), (3, 10), (2, 10), (2, 9), (2, 8), (2, 7), (2, 8), (1, 8), (1, 7), (2, 7), (1, 7), (1, 8), (1, 7), (2, 7), (2, 6), (3, 6), (2, 6), (3, 6), (3, 7), (4, 7), (3, 7), (3, 6), (4, 6), (4, 5), (4, 6), (4, 7), (4, 6), (4, 5), (5, 5), (4, 5), (3, 5), (3, 6), (4, 6), (3, 6), (3, 5), (4, 5), (5, 5), (6, 5), (6, 6), (6, 7), (6, 8), (6, 9), (6, 8), (7, 8), (7, 9), (7, 8), (7, 9), (8, 9), (9, 9), (9, 8), (9, 9), (9, 8), (9, 7), (8, 7), (7, 7), (7, 8), (7, 7), (7, 8), (7, 9), (7, 8), (6, 8), (6, 7), (6, 8), (6, 7), (6, 6), (7, 6), (6, 6), (5, 6), (4, 6), (3, 6), (2, 6), (1, 6), (1, 7), (2, 7), (2, 6), (3, 6), (2, 6), (2, 7), (1, 7), (1, 8), (1, 7), (1, 6), (1, 5), (1, 6), (1, 5), (1, 4)] 96 | # obstacles = [] 97 | trails = [(1, 1), (1, 2), (1, 3), (1, 4), (1, 5), (1, 6), (1, 7), (1, 8), (1, 9), (1, 10), (2, 10), (3, 10), (4, 10), (5, 10), (6, 10), (7, 10), (8, 10), (9, 10), (10, 10), (10, 9), (10, 8), (10, 7), (10, 6), (10, 5), (10, 4), (10, 3), (10, 2), (10, 1), (9, 1), (8, 1), (7, 1), (6, 1), (5, 1), (4, 1), (3, 1), (2, 1), (2, 2), (2, 3), (2, 4), (2, 5), (2, 6), (2, 7), (2, 8), (2, 9), (3, 9), (4, 9), (5, 9), (6, 9), (7, 9), (8, 9), (9, 9), (9, 8), (9, 7), (9, 6), (9, 5), (9, 4), (9, 3), (9, 2), (8, 2), (7, 2), (6, 2), (5, 2), (4, 2), (3, 2), (3, 3), (3, 4), (3, 5), (3, 6), (3, 7), (3, 8), (4, 8), (5, 8), (6, 8), (7, 8), (8, 8), (8, 7), (8, 6), (8, 5), (8, 4), (8, 3), (7, 3), (6, 3), (5, 3), (4, 3), (4, 4), (4, 5), (4, 6), (4, 7), (5, 7), (6, 7), (7, 7), (7, 6), (7, 5), (7, 4), (6, 4), (5, 4), (5, 5), (5, 6), (6, 6),(6, 5)] 98 | 99 | # trails = [(2, 1), (3, 1), (4, 1), (5, 1), (6, 1), (7, 1), (8, 1), (9, 1), (10, 1), (9, 1), (8, 1), (8, 2), (8, 3), (8, 4), (8, 5), (8, 6), (8, 7), (8, 8), (7, 8), (7, 7), (7, 6), (7, 5), (8, 5), (9, 5), (9, 6), (9, 7), (9, 8), (9, 9), (8, 9), (7, 9), (6, 9), (6, 8), (6, 7), (6, 6), (6, 5), (6, 4), (5, 4), (4, 4), (3, 4), (2, 4), (1, 4), (2, 4), (3, 4), (3, 5), (3, 6), (3, 7), (3, 8), (4, 8), (4, 7), (4, 6), (4, 5), (3, 5), (2, 5), (2, 6), (2, 7), (2, 8), (2, 9), (3, 9), (4, 9), (5, 9), (5, 8), (5, 7), (5, 6), (5, 5), (5, 4), (5, 3), (6, 3), (7, 3), (8, 3), (9, 3), (9, 4), (8, 4), (7, 4), (6, 4), (5, 4), (4, 4), (3, 4), (2, 4), (1, 4), (1, 5), (1, 6), (1, 7), (1, 8), (1, 9), (1, 10), (2, 10), (3, 10), (4, 10), (5, 10), (6, 10), (7, 10), (8, 10), (9, 10), (10, 10), (10, 9), (10, 8), (10, 7), (10, 6), (10, 5), (10, 4), (10, 3), (10, 2), (9, 2), (8, 2), (7, 2), (6, 2), (5, 2), (4, 2), (3, 2), (2, 2), (1, 2),(1, 1)] 100 | 101 | 102 | # trails=[(1, 1), (2, 1), (3, 1), (4, 1), (5, 1), (6, 1), (7, 1), (8, 1), (9, 1), (10, 1), (10, 2), (10, 3), (9, 3), (8, 3), (7, 3), (6, 3), (5, 3), (6, 3), (7, 3), (8, 3), (9, 3), (10, 3), (10, 2), (9, 2), (8, 2), (7, 2), (6, 2), (5, 2), (4, 2), (3, 2), (2, 2), (1, 2)] 103 | # pass_point = [(1, 1), (1, 2), (2, 1), (2, 2), (3, 1), (3, 2), (4, 1), (4, 2), (5, 1), (5, 2), (5, 3), (6, 1), (6, 2), (6, 3), (7, 1), (7, 2), (7, 3), (8, 1), (8, 2), (8, 3), (9, 1), (9, 2), (9, 3), (10, 1), (10, 2), (10, 3)] 104 | # obstacles = [(1,3),(2,3),(3,3),(4,3)] # 机器人1 105 | 106 | # pass_point = [(1, 4), (1, 5), (1, 6), (1, 7), (1, 8), (1, 9), (1, 10), (2, 4), (2, 5), (2, 6), (2, 7), (2, 8), (2, 9), (2, 10), (3, 4), (3, 5), (3, 6), (3, 7), (3, 8), (3, 9), (3, 10), (4, 4), (4, 5), (4, 6), (4, 7), (4, 8), (4, 9), (4, 10), (5, 4), (5, 5), (5, 9), (5, 10)] 107 | # obstacles = [(5, 6), (5, 7), (5, 8)] # 机器人2 108 | # trails = [ (4, 6), (4, 7), (4, 8), (3, 8), (3, 7), (3, 6), (3, 5), (4, 5), (5, 5), (5, 4), (4, 4), (3, 4), (2, 4), (2, 5), (2, 6), (2, 7), (2, 8), (2, 9), (3, 9), (4, 9), (5, 9), (5, 10), (4, 10), (3, 10), (2, 10), (1, 10), (1, 9), (1, 8), (1, 7), (1, 6), (1, 5),(1,4)] 109 | 110 | # pass_point = [(6, 4), (6, 5), (6, 6), (6, 7), (6, 8), (6, 9), (6, 10), (7, 4), (7, 5), (7, 6), (7, 7), (7, 8), (7, 9), (7, 10), (8, 4), (8, 5), (8, 6), (8, 7), (8, 8), (8, 9), (8, 10), (9, 4), (9, 5), (9, 6), (9, 7), (9, 8), (9, 9), (9, 10), (10, 4), (10, 5), (10, 6), (10, 7), (10, 8), (10, 9), (10, 10)] 111 | # obstacles = [] 112 | # trails = [(8,8),(8, 7), (8, 6), (7, 6), (7, 7), (7, 8), (7, 9), (8, 9), (9, 9), (9, 8), (9, 7), (9, 6), (9, 5), (8, 5), (7, 5), (6, 5), (6, 4), (7, 4), (8, 4), (9, 4), (10, 4), (10, 5), (10, 6), (10, 7), (10, 8), (10, 9), (10, 10), (9, 10), (8, 10), (7, 10), (6, 10), (6, 9), (6, 8), (6, 7), (6, 6), (6, 5)] # 机器人3 113 | 114 | # pass_point = [(1, 1), (1, 2), (1, 4), (1, 5), (1, 6), (1, 7), (1, 8), (1, 9), (1, 10), (2, 1), (2, 2), (2, 4), (2, 5), (2, 6), (2, 7), (2, 8), (2, 9), (2, 10), (3, 1), (3, 2), (3, 4), (3, 5), (3, 6), (3, 7), (3, 8), (3, 9), (3, 10), (4, 1), (4, 2), (4, 4), (4, 5), (4, 6), (4, 7), (4, 8), (4, 9), (4, 10), (5, 1), (5, 2), (5, 3), (5, 4), (5, 5), (5, 9), (5, 10), (6, 1), (6, 2), (6, 3), (6, 4), (6, 5), (6, 6), (6, 7), (6, 8), (6, 9), (6, 10), (7, 1), (7, 2), (7, 3), (7, 4), (7, 5), (7, 6), (7, 7), (7, 8), (7, 9), (7, 10), (8, 1), (8, 2), (8, 3), (8, 4), (8, 5), (8, 6), (8, 7), (8, 8), (8, 9), (8, 10), (9, 1), (9, 2), (9, 3), (9, 4), (9, 5), (9, 6), (9, 7), (9, 8), (9, 9), (9, 10), (10, 1), (10, 2), (10, 3), (10, 4), (10, 5), (10, 6), (10, 7), (10, 8), (10, 9), (10, 10)] 115 | # obstacles = [(1,3),(2,3),(3,3),(4,3),(5,6),(5,7),(5,8)] 116 | # obstacles = [] 117 | # trails = [(1, 1), (1, 2), (1, 1), (2, 1), (1, 1), (2, 1), (2, 2), (1, 2), (1, 3), (1, 4), (2, 4), (1, 4), (1, 5), (1, 4), (1, 5), 118 | # (2, 5), (3, 5), (2, 5), (3, 5), (4, 5), (4, 4), (4, 5), (4, 4), (4, 3), (5, 3), (5, 4), (4, 4), (4, 3), (5, 3), (4, 3), 119 | # (4, 2), (4, 3), (5, 3), (5, 2), (5, 1), (4, 1), (4, 2), (3, 2), (4, 2), (3, 2), (3, 1), (3, 2), (3, 3), (2, 3), (2, 4), 120 | # (2, 3), (3, 3), (3, 4), (4, 4), (5, 4), (5, 5)] 121 | # trails = [(1, 1), (2, 1), (3, 1), (2, 1), (3, 1), (2, 1), (1, 1), (2, 1), (3, 1), (4, 1), (3, 1), (4, 1), (5, 1), (5, 2), (4, 2), (4, 1), (4, 2), (5, 2), (5, 3), (6, 3), (7, 3), (8, 3), (7, 3), (7, 2), (6, 2), (6, 3), (6, 4), (7, 4), (7, 5), (7, 4), (7, 5), (7, 4), (8, 4), (9, 4), (10, 4), (10, 3), (10, 2), (9, 2), (10, 2), (10, 1), (10, 2), (9, 2), (9, 1), (8, 1), (8, 2), (9, 2), (8, 2), (7, 2), (6, 2), (6, 1), (5, 1), (5, 2), (6, 2), (6, 3), (5, 3), (5, 2), (6, 2), (5, 2), (4, 2), (5, 2), (6, 2), (5, 2), (6, 2), (7, 2), (7, 3), (7, 4), (8, 4), (9, 4), (8, 4), (7, 4), (7, 5), (8, 5), (8, 6), (8, 5), (8, 6), (9, 6), (9, 5), (10, 5), (9, 5), (10, 5), (9, 5), (9, 6), (9, 7), (10, 7), (9, 7), (10, 7), (9, 7), (9, 8), (9, 7), (9, 8), (10, 8), (9, 8), (9, 9), (9, 8), (9, 9), (9, 8), (9, 9), (8, 9), (9, 9), (9, 10), (8, 10), (7, 10), (8, 10), (7, 10), (6, 10), (7, 10), (8, 10), (7, 10), (7, 9), (8, 9), (8, 10), (8, 9), (8, 10), (8, 9), (9, 9), (10, 9), (9, 9), (8, 9), (8, 10), (7, 10), (7, 9), (7, 8), (7, 9), (6, 9), (5, 9), (4, 9), (4, 8), (3, 8), (3, 7), (2, 7), (2, 8), (2, 9), (3, 9), (2, 9), (2, 8), (1, 8), (2, 8), (3, 8), (3, 7), (2, 7), (1, 7), (1, 6), (2, 6), (3, 6), (2, 6), (1, 6), (1, 5), (2, 5), (1, 5), (1, 4), (1, 5), (1, 6), (1, 5), (1, 6), (2, 6), (2, 5), (2, 4), (1, 4), (1, 5), (1, 6), (2, 6), (3, 6), (4, 6), (3, 6), (3, 7), (2, 7), (2, 6), (1, 6), (1, 5), (1, 6), (2, 6), (2, 7), (3, 7), (3, 6), (2, 6), (2, 5), (1, 5), (1, 4), (1, 5), (2, 5), (2, 4), (2, 5), (1, 5), (1, 4), (2, 4), (3, 4), (3, 5), (2, 5), (3, 5), (3, 4), (2, 4), (2, 5), (2, 6), (1, 6), (2, 6), (2, 7), (3, 7), (4, 7), (4, 6), (3, 6), (4, 6), (3, 6), (4, 6), (3, 6), (4, 6), (4, 5), (3, 5), (2, 5), (1, 5), (1, 6), (1, 5), (1, 4), (2, 4), (1, 4), (1, 5), (2, 5), (3, 5), (2, 5), (2, 4), (2, 5), (2, 6), (2, 7), (3, 7), (4, 7), (4, 8), (4, 7), (3, 7), (3, 6), (4, 6), (4, 5), (4, 4), (4, 5), (4, 4), (3, 4), (3, 5), (4, 5), (3, 5), (4, 5), (5, 5), (4, 5), (3, 5), (3, 6), (3, 7), (3, 6), (3, 5), (3, 4), (3, 5), (2, 5), (3, 5), (4, 5), (5, 5), (4, 5), (5, 5), (5, 4), (6, 4), (6, 5), (6, 4), (6, 3), (6, 2), (6, 3), (6, 2), (6, 3), (7, 3), (7, 4), (7, 3), (7, 4), (8, 4), (8, 3), (8, 2), (9, 2), (9, 3), (9, 2), (9, 3), (9, 2), (9, 3), (8, 3), (8, 4), (8, 5), (9, 5), (10, 5), (9, 5), (9, 6), (9, 7), (10, 7), (10, 8), (10, 7), (9, 7), (10, 7), (10, 6)] 122 | # trails = [(1,1),(2,1),(3,1),(4,1),(5,1),(5,2),(4,2),(3,2),(2,2),(1,2)] # 测试绘图效果 123 | 124 | # trails = [(1, 4), (1, 4), (2, 4), (3, 4), (4, 4), (5, 4), (6, 4), (7, 4), (8, 4), (9, 4), (10, 4), (9, 4), (8, 4), (7, 4), (6, 4), (5, 4), (4, 4), (3, 4), (3, 5), (3, 6), (3, 7), (3, 8), (4, 8), (4, 7), (4, 6), (3, 6), (2, 6), (2, 7), (2, 8), (2, 9), (3, 9), (4, 9), (5, 9), (5, 10), (6, 10), (7, 10), (8, 10), (9, 10), (10, 10), (10, 9), (10, 8), (10, 7), (10, 6), (10, 5), (10, 4), (10, 3), (10, 2), (10, 1), (10, 2), (10, 3), (10, 4), (10, 5), (10, 6), (10, 7), (10, 8), (10, 9), (10, 10), (10, 9), (10, 8), (10, 7), (10, 6), (10, 5), (10, 4), (10, 3), (10, 2), (9, 2), (8, 2), (7, 2), (6, 2), (5, 2), (4, 2), (3, 2), (2, 2), (1, 2), (1, 1), (1, 2), (1, 1), (1, 2), (2, 2), (3, 2), (4, 2), (4, 1), (5, 1), (6, 1), (7, 1), (8, 1), (8, 2), (8, 3), (8, 4), (7, 4), (7, 3), (7, 2), (8, 2), (9, 2), (9, 3), (9, 4), (9, 5), (8, 5), (7, 5), (6, 5), (5, 5), (5, 4), (5, 3), (5, 2), (5, 1), (4, 1), (3, 1)] 125 | # trails = [(4, 10), (4, 10), (3, 10), (2, 10), (1, 10), (2, 10), (3, 10), (4, 10), (5, 10), (6, 10), (7, 10), (8, 10), (9, 10), (10, 10), (10, 9), (10, 8), (10, 7), (10, 6), (10, 5), (10, 4), (10, 3), (10, 2), (10, 1), (10, 2), (10, 3), (10, 4), (10, 5), (10, 6), (10, 7), (10, 8), (10, 9), (10, 10), (10, 9), (10, 8), (10, 7), (10, 6), (10, 5), (10, 4), (10, 3), (10, 2), (10, 1), (9, 1), (8, 1), (7, 1), (6, 1), (5, 1), (4, 1), (3, 1), (2, 1), (1, 1), (2, 1), (3, 1), (4, 1), (5, 1), (6, 1), (7, 1), (8, 1), (9, 1), (10, 1), (10, 2), (10, 3), (10, 4), (10, 5), (10, 6), (10, 7), (10, 8), (10, 9), (10, 10), (10, 9), (10, 8), (10, 7), (10, 6), (10, 5), (10, 4), (10, 3), (10, 2), (10, 1), (9, 1), (8, 1), (7, 1), (6, 1), (6, 2), (6, 3), (6, 4), (6, 5), (6, 6), (6, 7), (6, 8), (6, 9), (6, 10), (5, 10), (4, 10), (3, 10), (2, 10), (1, 10), (1, 9), (1, 8), (1, 7), (1, 6), (1, 5), (2, 5), (3, 5), (4, 5), (5, 5), (5, 4), (4, 4), (3, 4)] 126 | # trails= [(3, 10), (3, 10), (4, 10), (5, 10), (6, 10), (7, 10), (8, 10), (9, 10), (10, 10), (10, 9), (10, 8), (10, 7), (10, 6), (10, 5), (10, 4), (10, 3), (10, 2), (10, 1), (9, 1), (8, 1), (7, 1), (6, 1), (5, 1), (4, 1), (3, 1), (2, 1), (1, 1), (2, 1), (3, 1), (4, 1), (5, 1), (6, 1), (7, 1), (8, 1), (9, 1), (10, 1), (9, 1), (8, 1), (7, 1), (6, 1), (5, 1), (4, 1), (3, 1), (2, 1), (1, 1), (1, 2), (1, 1), (2, 1), (3, 1), (4, 1), (5, 1), (6, 1), (7, 1), (8, 1), (9, 1), (10, 1), (9, 1), (8, 1), (7, 1), (6, 1), (5, 1), (4, 1), (3, 1), (2, 1), (1, 1), (2, 1), (3, 1), (4, 1), (5, 1), (6, 1), (7, 1), (8, 1), (8, 2), (8, 3), (8, 4), (8, 5), (8, 6), (8, 7), (8, 8), (8, 9), (9, 9), (9, 8), (9, 7), (9, 6), (8, 6), (7, 6), (7, 7), (7, 8), (7, 9), (7, 10), (8, 10), (9, 10), (10, 10), (10, 9), (10, 8), (10, 7), (10, 6), (10, 5), (10, 4), (10, 3), (10, 2), (10, 1), (9, 1), (8, 1), (7, 1), (6, 1), (6, 2)] 127 | 128 | 129 | main(rows, cols, pass_point, trails, obstacles) 130 | -------------------------------------------------------------------------------- /Multi_mcts.py: -------------------------------------------------------------------------------- 1 | """ 2 | 5.14 单机器人MCTS覆盖算法仿真 3 | 4 | """ 5 | import sys 6 | 7 | from treelib import Tree, Node 8 | import random 9 | import math 10 | import matplotlib.pyplot as plt 11 | import time 12 | import itertools 13 | 14 | ''' 15 | 1.主函数部分 16 | ''' 17 | 18 | 19 | # todo 覆盖过的栅格标志位设置为0.5， 未覆盖的为0 ， 障碍物为1 20 | # todo 算法效果差主要是随机策略设计的有问题 21 | # data=[[1,1],[1,1],[1,1],0,0.1,[a,b,c],0] 22 | 23 | def tree_update(node_id, move): # 更新树:输入父节点id和可行动作集合,扩展子节点添加到树上并返回其id 24 | 25 | r1x = tree.get_node(nid=node_id).data[0][0] 26 | r1y = tree.get_node(nid=node_id).data[0][1] 27 | 28 | r2x = tree.get_node(nid=node_id).data[1][0] 29 | r2y = tree.get_node(nid=node_id).data[1][1] 30 | 31 | r3x = tree.get_node(nid=node_id).data[2][0] 32 | r3y = tree.get_node(nid=node_id).data[2][1] 33 | 34 | data = tree.get_node(node_id).data 35 | 36 | if move[0] == 1: 37 | data[0] = (r1x - 1, r1y) 38 | elif move[0] == 2: 39 | data[0] = (r1x + 1, r1y) 40 | elif move[0] == 3: 41 | data[0] = (r1x, r1y - 1) 42 | elif move[0] == 4: 43 | data[0] = (r1x, r1y + 1) 44 | 45 | if move[1] == 1: 46 | data[1] = (r2x - 1, r2y) 47 | elif move[1] == 2: 48 | data[1] = (r2x + 1, r2y) 49 | elif move[1] == 3: 50 | data[1] = (r2x, r2y - 1) 51 | elif move[1] == 4: 52 | data[1] = (r2x, r2y + 1) 53 | 54 | if move[2] == 1: 55 | data[2] = (r3x - 1, r3y) 56 | elif move[2] == 2: 57 | data[2] = (r3x + 1, r3y) 58 | elif move[2] == 3: 59 | data[2] = (r3x, r3y - 1) 60 | elif move[2] == 4: 61 | data[2] = (r3x, r3y + 1) 62 | 63 | sub_node = Node(tag='sub', data=[data[0], data[1], data[2], 0, 10e-5, move, 1e5]) 64 | # todo ！！！初始ucb的需要改一下 65 | tree.add_node(sub_node, parent=node_id) 66 | sub_node_id = sub_node.identifier 67 | 68 | coveraged.append(sub_node.data[0]) 69 | coveraged.append(sub_node.data[1]) 70 | coveraged.append(sub_node.data[2]) 71 | 72 | return (sub_node_id) 73 | 74 | 75 | # def default_policy(node_id): # 根据割草策略向下选择一个节点进行仿真，遇障碍物或者已覆盖栅格右转 76 | # 77 | # # todo move = tree.parent(node_id).data[3] 需要改 78 | # # move = [a,b,c] data[5] 79 | # robots_move = [0,0,0] 80 | # rid = 1 # 机器人编号 81 | # while rid <= 3: 82 | # 83 | # move = tree.get_node(node_id).data[5][rid-1] 84 | # 85 | # all_moves = get_moni_move(node_id, rid) 86 | # if all_moves == [0, 0, 0, 0]: # 陷入死区 87 | # all_moves = get_simulation_move(node_id, rid) 88 | # 89 | # if move in all_moves: 90 | # move = move 91 | # 92 | # else: 93 | # move = random.choice(all_moves) 94 | # while move == 0: 95 | # move = random.choice(all_moves) 96 | # 97 | # robots_move[rid-1] = move 98 | # 99 | # rid += 1 100 | # 101 | # sub_id = tree_update(node_id,robots_move) 102 | # 103 | # return sub_id 104 | 105 | def default_policy(node_id): # 根据割草策略向下选择一个节点进行仿真，遇障碍物或者已覆盖栅格右转 106 | 107 | # todo move = tree.parent(node_id).data[3] 需要改 108 | # move = [a,b,c] data[5] 109 | 110 | # print('坐标1', tree.get_node(node_id).data[0]) 111 | # print('坐标2', tree.get_node(node_id).data[1]) 112 | # print('坐标3', tree.get_node(node_id).data[2]) 113 | 114 | 115 | # all_moves = get_moni_move(node_id) 116 | 117 | 118 | # 改为每个机器人单独判断是否陷入死区 119 | all_moves = [0, 0, 0] 120 | rid = 1 121 | while rid <= 3: 122 | 123 | z = rid -1 124 | move = tree.get_node(node_id).data[5][z] 125 | moves = get_moni_move(node_id, rid) 126 | if moves == []: # 避免陷入死区 127 | moves = get_simulation_move(node_id, rid) 128 | # print(tree.get_node(node_id).data[0]) 129 | # print(moves) 130 | if move in moves: 131 | move = move 132 | # print(tree.get_node(node_id).data[0]) 133 | # print(moves) 134 | else: 135 | move = random.choice(moves) 136 | # print(tree.get_node(node_id).data[0]) 137 | # print(moves) 138 | 139 | all_moves[z] = move 140 | 141 | rid += 1 142 | 143 | sub_id = tree_update(node_id, all_moves) 144 | 145 | return sub_id 146 | 147 | 148 | def get_expand_move(node_id): # 输入节点id，返回可扩展操作集合，形式[a,b,c] 149 | # todo 产生一个[10,-1]的错误节点 150 | # all_moves = [(1, 1, 1), (1, 1, 2), (1, 1, 3), (1, 1, 4), (1, 2, 1), (1, 2, 2), (1, 2, 3), (1, 2, 4), (1, 3, 1), 151 | # (1, 3, 2), (1, 3, 3), (1, 3, 4), (1, 4, 1), (1, 4, 2), (1, 4, 3), (1, 4, 4), (2, 1, 1), (2, 1, 2), 152 | # (2, 1, 3), (2, 1, 4), (2, 2, 1), (2, 2, 2), (2, 2, 3), (2, 2, 4), (2, 3, 1), (2, 3, 2), (2, 3, 3), 153 | # (2, 3, 4), (2, 4, 1), (2, 4, 2), (2, 4, 3), (2, 4, 4), (3, 1, 1), (3, 1, 2), (3, 1, 3), (3, 1, 4), 154 | # (3, 2, 1), (3, 2, 2), (3, 2, 3), (3, 2, 4), (3, 3, 1), (3, 3, 2), (3, 3, 3), (3, 3, 4), (3, 4, 1), 155 | # (3, 4, 2), (3, 4, 3), (3, 4, 4), (4, 1, 1), (4, 1, 2), (4, 1, 3), (4, 1, 4), (4, 2, 1), (4, 2, 2), 156 | # (4, 2, 3), (4, 2, 4), (4, 3, 1), (4, 3, 2), (4, 3, 3), (4, 3, 4), (4, 4, 1), (4, 4, 2), (4, 4, 3), 157 | # (4, 4, 4)] 158 | 159 | rid = 1 # 删除不合法的节点 160 | moves = [[1, 2, 3, 4], [1, 2, 3, 4], [1, 2, 3, 4]] 161 | while rid <= 3: 162 | z = rid - 1 163 | 164 | x = tree.get_node(nid=node_id).data[z][0] 165 | y = tree.get_node(nid=node_id).data[z][1] 166 | # print('仿真x:', x, '仿真y:', y) 167 | 168 | if (((x - 1, y) in pass_point) == 0): # 当前机器人动作1不合法 169 | # moves[z][0] = 0 170 | moves[z].remove(1) 171 | if (((x + 1, y) in pass_point) == 0): 172 | # moves[z][1] = 0 173 | moves[z].remove(2) 174 | if (((x, y - 1) in pass_point) == 0): 175 | # moves[z][2] = 0 176 | moves[z].remove(3) 177 | if (((x, y + 1) in pass_point) == 0): 178 | # moves[z][3] = 0 179 | moves[z].remove(4) 180 | 181 | rid += 1 182 | 183 | 184 | # print(moves) 185 | 186 | all_moves = list(itertools.product(moves[0], moves[1], moves[2])) 187 | 188 | # print(all_moves) 189 | children = tree.children(node_id) 190 | # print(len(children)) 191 | for i in range(len(children)): # 删除已经扩展的节点 192 | move = children[i].data[5] 193 | if move in all_moves: 194 | # print(move) 195 | all_moves.remove(move) 196 | # all_moves.append((0, 0, 0)) 197 | 198 | 199 | # for j in range(len(all_moves)): 200 | # if 0 in all_moves[j]: 201 | # all_moves[j] = (0, 0, 0) 202 | 203 | # print('当前节点：',node_id) 204 | # print('可扩展的操作集合：', all_moves) 205 | # print(all_moves) 206 | 207 | return all_moves 208 | 209 | 210 | def get_simulation_move(node_id, rid): # 输入节点id,返回该机器人所有合法操作，[a,b,c] 211 | 212 | moves = [1, 2, 3, 4] 213 | z = rid - 1 214 | 215 | x = tree.get_node(nid=node_id).data[z][0] 216 | y = tree.get_node(nid=node_id).data[z][1] 217 | # if rid == 2: 218 | # print('x:', x, 'y:', y) 219 | 220 | if (((x - 1, y) in pass_point) == 0): # 当前机器人动作1不合法 221 | # moves[z][0] = 0 222 | moves.remove(1) 223 | if (((x + 1, y) in pass_point) == 0): 224 | # moves[z][1] = 0 225 | moves.remove(2) 226 | if (((x, y - 1) in pass_point) == 0): 227 | # moves[z][2] = 0 228 | moves.remove(3) 229 | if (((x, y + 1) in pass_point) == 0): 230 | # moves[z][3] = 0 231 | moves.remove(4) 232 | 233 | if moves == []: 234 | print('机器人坐标：',(x,y)) 235 | 236 | return moves 237 | 238 | 239 | # def get_moni_move(node_id): # 输入节点id和要访问的机器人编号，输出该机器人内螺旋过程的可选操作 240 | # 241 | # # todo 242 | # 243 | # rid = 1 # 删除不合法的节点 244 | # moves = [[1, 2, 3, 4], [1, 2, 3, 4], [1, 2, 3, 4]] 245 | # while rid <= 3: 246 | # z = rid - 1 247 | # 248 | # x = tree.get_node(nid=node_id).data[z][0] 249 | # y = tree.get_node(nid=node_id).data[z][1] 250 | # if rid ==2: 251 | # print('x:', x, 'y:', y) 252 | # 253 | # if (((x - 1, y) in pass_point) == 0): # 当前机器人动作1不合法 254 | # # moves[z][0] = 0 255 | # moves[z].remove(1) 256 | # if (((x + 1, y) in pass_point) == 0): 257 | # # moves[z][1] = 0 258 | # moves[z].remove(2) 259 | # if (((x, y - 1) in pass_point) == 0): 260 | # # moves[z][2] = 0 261 | # moves[z].remove(3) 262 | # if (((x, y + 1) in pass_point) == 0): 263 | # # moves[z][3] = 0 264 | # moves[z].remove(4) 265 | # print(moves) 266 | # 267 | # # todo 改完后加上 268 | # # if (((x - 1, y) in coveraged) == 0): # 当前机器人动作1不合法 269 | # # # moves[z][0] = 0 270 | # # moves[z].remove(1) 271 | # # if (((x + 1, y) in coveraged) == 0): 272 | # # # moves[z][1] = 0 273 | # # moves[z].remove(2) 274 | # # if (((x, y - 1) in coveraged) == 0): 275 | # # # moves[z][2] = 0 276 | # # moves[z].remove(3) 277 | # # if (((x, y + 1) in coveraged) == 0): 278 | # # # moves[z][3] = 0 279 | # # moves[z].remove(4) 280 | # 281 | # rid += 1 282 | # 283 | # all_moves = list(itertools.product(moves[0], moves[1], moves[2])) 284 | # # for j in range(len(all_moves)): 285 | # # if 0 in all_moves[j]: 286 | # # all_moves[j] = (0, 0, 0) 287 | # 288 | # return all_moves 289 | 290 | def get_moni_move(node_id, rid): # 输入节点id和要访问的机器人编号，输出该机器人内螺旋过程的可选操作 291 | 292 | # todo 293 | 294 | moves = [1, 2, 3, 4] 295 | z = rid - 1 296 | 297 | x = tree.get_node(nid=node_id).data[z][0] 298 | y = tree.get_node(nid=node_id).data[z][1] 299 | # if rid == 2: 300 | # print('x:', x, 'y:', y) 301 | 302 | if (((x - 1, y) in pass_point) == 0): # 当前机器人动作1不合法 303 | # moves[z][0] = 0 304 | moves.remove(1) 305 | if (((x + 1, y) in pass_point) == 0): 306 | # moves[z][1] = 0 307 | moves.remove(2) 308 | if (((x, y - 1) in pass_point) == 0): 309 | # moves[z][2] = 0 310 | moves.remove(3) 311 | if (((x, y + 1) in pass_point) == 0): 312 | # moves[z][3] = 0 313 | moves.remove(4) 314 | # print(moves) 315 | 316 | # todo 改完后加上 317 | if (((x - 1, y) in coveraged) == 1) and (1 in moves): # 当前机器人动作1已经被其他机器人走过 318 | # moves[z][0] = 0 319 | moves.remove(1) 320 | if (((x + 1, y) in coveraged) == 1) and (2 in moves): 321 | # moves[z][1] = 0 322 | moves.remove(2) 323 | if (((x, y - 1) in coveraged) == 1) and (3 in moves): 324 | # moves[z][2] = 0 325 | moves.remove(3) 326 | if ((((x, y + 1) in coveraged) == 1) and (4 in moves)): 327 | # moves[z][3] = 0 328 | # print((((x, y + 1) in coveraged) == 0)) 329 | moves.remove(4) 330 | 331 | # print('合法模拟动作moni:', moves) 332 | return moves 333 | 334 | 335 | def p_cal(node_id): # 计算当前节点所有机器人新覆盖的栅格数 336 | # todo reward和p的计算肯定有问题 337 | coveraged = [] 338 | parent_id = tree.parent(nid=node_id).identifier 339 | while parent_id != 'root': 340 | coveraged.append(tree.get_node(nid=parent_id).data[0]) 341 | coveraged.append(tree.get_node(nid=parent_id).data[1]) 342 | coveraged.append(tree.get_node(nid=parent_id).data[2]) 343 | parent_id = tree.parent(nid=parent_id).identifier 344 | 345 | data = tree.get_node(node_id).data 346 | coordinate = [data[0], data[1], data[2]] 347 | p = 0 348 | for i in range(len(coordinate)): 349 | if coordinate[i] in coveraged: 350 | p = 0 351 | else: 352 | p = 1 353 | 354 | return p 355 | 356 | 357 | def reward_cal(node_id): # reward = Σ[p/(tk+1)^2] 358 | 359 | dt, k = 0.1, 0 360 | reward = 0 361 | 362 | while node_id != 'root': # 计算路径上的奖励值 363 | p = p_cal(node_id) 364 | k = tree.depth(tree.get_node(nid=node_id)) 365 | tk = dt * k 366 | rk = 0 # is_turn(node_id) 367 | reward += (p - rk) / ((tk + 1) ** 2) 368 | node_id = tree.parent(node_id).identifier 369 | 370 | # print('reward:',reward) 371 | 372 | return reward 373 | 374 | 375 | def UCT_cal(node_id): 376 | reward = tree.get_node(nid=node_id).data[3] 377 | visit = tree.get_node(nid=node_id).data[4] 378 | parent_id = tree.parent(nid=node_id).identifier 379 | parent_visit = tree.get_node(nid=parent_id).data[4] 380 | 381 | # todo 改变c值 382 | # c=1/(math.sqrt(2)) 383 | c = 0.0001 384 | 385 | left = reward / visit 386 | right = c * math.sqrt(2 * math.log(parent_visit) / visit) 387 | 388 | # print('q:',left,'ucb:',right) 389 | 390 | UCT = left + right 391 | # print('uct:',UCT) 392 | # todo 393 | data = tree.get_node(node_id).data 394 | tree.update_node(nid=node_id, data=[data[0], data[1], data[2], data[3], data[4], data[5], UCT]) # 更新UCT值 395 | 396 | return UCT 397 | 398 | 399 | def coverage_cal(node_id): # 计算有效的覆盖栅格数 400 | 401 | coverage = 0 402 | # if (tree.get_node(nid=node_id).is_leaf()): 403 | 404 | explore_trails = [(1, 1)] 405 | 406 | while (tree.get_node(nid=node_id).is_root() == False): # 判断当前节点是否是根节点 407 | trail1 = tree.get_node(nid=node_id).data[0] 408 | trail2 = tree.get_node(nid=node_id).data[1] 409 | trail3 = tree.get_node(nid=node_id).data[2] 410 | explore_trails.append(trail1) 411 | explore_trails.append(trail2) 412 | explore_trails.append(trail3) 413 | node_id = tree.parent(nid=node_id).identifier 414 | 415 | l1 = explore_trails 416 | for el in range(len(l1) - 1, -1, -1): 417 | if l1.count(l1[el]) > 1: 418 | l1.pop(el) 419 | 420 | coverage = len(l1) 421 | 422 | return (coverage) 423 | 424 | 425 | def select(node_id): # 输入当前节点，选择最优的子节点id 426 | 427 | children = tree.children(node_id) 428 | children_UCT = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 429 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] 430 | for i in range(len(children)): 431 | children_UCT[i] = UCT_cal(children[i].identifier) 432 | # print(children_UCT) 433 | best_j = 0 434 | 435 | max_UCT = children_UCT[0] 436 | # todo 改进UCT选择过程 437 | for j in range(len(children)): 438 | if children_UCT[j] >= max_UCT: 439 | max_UCT = children_UCT[j] 440 | best_j = j 441 | 442 | return (children[best_j].identifier) 443 | 444 | 445 | def backup(leaf_node_id, reward): # 从叶子节点出发，回溯更新整棵树 446 | node_id = leaf_node_id 447 | parent_id = tree.parent(nid=node_id).identifier 448 | while (tree.get_node(nid=parent_id).is_root() == 0): # 判断当前节点的父节点是否是根节点 449 | tree.get_node(nid=parent_id).data[3] += reward # 更新reward 450 | tree.get_node(nid=parent_id).data[4] += 1 # 更新visit_times 451 | node_id = tree.parent(nid=node_id).identifier 452 | parent_id = tree.parent(nid=parent_id).identifier 453 | tree.get_node(nid='root').data[3] += reward 454 | tree.get_node(nid='root').data[4] += 1 455 | 456 | 457 | ''' 458 | 2.MCTS过程 459 | ''' 460 | 461 | 462 | def MCTS_select(root_id): # 输入根节点id,清空select_node_trails[],重新计算并输出根节点到当前扩展层的最优节点序列select_node_trails 463 | 464 | node_id = root_id 465 | root_node = tree.get_node(nid=root_id) 466 | 467 | r1p = [] # 坐标序列 468 | r2p = [] 469 | r3p = [] 470 | select_node_trails = [root_node] # 节点序列 471 | 472 | while tree.get_node(nid=node_id).is_leaf() == 0: # 当前节点是叶子节点,选择过程结束 473 | 474 | select_node_trail = tree.get_node(nid=select(node_id)) 475 | select_node_trails.append(select_node_trail) 476 | 477 | data = tree.get_node(node_id).data 478 | r1p.append(data[0]) 479 | r2p.append(data[1]) 480 | r3p.append(data[2]) 481 | 482 | node_id = select_node_trail.identifier 483 | 484 | robot_trails = [r1p, r2p, r3p] 485 | 486 | select_trails = [select_node_trails, robot_trails] 487 | return select_trails 488 | 489 | 490 | def MCTS_expand(node_id): 491 | # print(node_id) 492 | all_moves = get_expand_move(node_id) 493 | if all_moves != []: 494 | # print(all_moves) 495 | moves = (0, 0, 0) 496 | while moves == (0, 0, 0): 497 | # todo 用随机速度很慢 498 | moves = random.choice(all_moves) 499 | # moves = all_moves[0] 500 | # print('选择动作',moves) 501 | 502 | sub_id = tree_update(node_id, moves) 503 | # print(tree.get_node(sub_id).data) 504 | return sub_id # 返回子节点id 505 | 506 | 507 | ''' 508 | 模拟过程结束判定： 509 | 1.整个树的层数达到200或者仿真结束 510 | 模拟过程策略： 511 | 随机策略 vs 内螺旋策略 512 | ''' 513 | 514 | 515 | def MCTS_simulation(node_id): # 从扩展得到的节点出发向下模拟 516 | simulation_trails_id = [] # 仿真过程的节点id序列 517 | simulation_trails_coordinate = [] # 仿真过程的节点坐标序列 518 | 519 | depth = tree.depth(node=tree.get_node(node_id)) 520 | 521 | while depth <= 100: # 保证模拟过程后，树的总层数为200 522 | sub_id = default_policy(node_id) 523 | # print(tree.get_node(sub_id).data) 524 | data = tree.get_node(node_id).data 525 | simulation_trails_id.append(sub_id) 526 | simulation_trails_coordinate.append(data[0]) 527 | simulation_trails_coordinate.append(data[1]) 528 | simulation_trails_coordinate.append(data[2]) 529 | node_id = sub_id 530 | depth = tree.depth(node=tree.get_node(node_id)) 531 | mcts_coverage = coverage_cal(sub_id) / len(pass_point) 532 | # print('仿真覆盖率:', mcts_coverage) 533 | if mcts_coverage == 1.0: 534 | print('程序结束') 535 | 536 | explore_trails = [] 537 | trails1, trails2, trails3 = [], [], [] 538 | 539 | node_id = sub_id 540 | while (tree.get_node(nid=node_id).is_root() == False): # 判断当前节点是否是根节点 541 | trail1 = tree.get_node(nid=node_id).data[0] 542 | trail2 = tree.get_node(nid=node_id).data[1] 543 | trail3 = tree.get_node(nid=node_id).data[2] 544 | explore_trails.append(trail1) 545 | explore_trails.append(trail2) 546 | explore_trails.append(trail3) 547 | trails1.append(trail1) 548 | trails2.append(trail2) 549 | trails3.append(trail3) 550 | 551 | node_id = tree.parent(nid=node_id).identifier 552 | print('仿真得到的机器人1路径：', trails1) 553 | print('仿真得到的机器人2路径：', trails2) 554 | print('仿真得到的机器人3路径：', trails3) 555 | print('重复率：', len(explore_trails) / len(pass_point)) 556 | sys.exit(0) 557 | 558 | # print('仿真节点数：', len(simulation_trails_id)) 559 | 560 | simulation_trails = [simulation_trails_id, simulation_trails_coordinate] 561 | return (simulation_trails) 562 | 563 | 564 | # def MCTS_backup(simulation_id, expand_id): 565 | # reward = reward_cal(simulation_id,expand_id) 566 | # backup(expand_id, reward) 567 | def MCTS_backup(simulation_id, expand_id): 568 | reward = reward_cal(simulation_id) - reward_cal(expand_id) 569 | backup(expand_id, reward) 570 | 571 | 572 | def main(root_node_id, expect_coverage): # 输入起始节点和期望覆盖率,输出最优路径 573 | 574 | node_id = root_node_id 575 | mcts_coverage = 0 576 | 577 | Expect_coverage = expect_coverage 578 | select_trails = [] 579 | 580 | while (mcts_coverage < Expect_coverage): 581 | 582 | all_moves = get_expand_move(node_id) 583 | 584 | # while all_moves != [(0, 0, 0), (0, 0, 0), (0, 0, 0), (0, 0, 0), (0, 0, 0), (0, 0, 0), (0, 0, 0), (0, 0, 0), (0, 0, 0), 585 | # (0, 0, 0), (0, 0, 0), (0, 0, 0), (0, 0, 0), (0, 0, 0), (0, 0, 0), (0, 0, 0), (0, 0, 0), (0, 0, 0), 586 | # (0, 0, 0), (0, 0, 0), (0, 0, 0), (0, 0, 0), (0, 0, 0), (0, 0, 0), (0, 0, 0), (0, 0, 0), (0, 0, 0), 587 | # (0, 0, 0), (0, 0, 0), (0, 0, 0), (0, 0, 0), (0, 0, 0), (0, 0, 0), (0, 0, 0), (0, 0, 0), (0, 0, 0), 588 | # (0, 0, 0), (0, 0, 0), (0, 0, 0), (0, 0, 0), (0, 0, 0), (0, 0, 0), (0, 0, 0), (0, 0, 0), (0, 0, 0), 589 | # (0, 0, 0), (0, 0, 0), (0, 0, 0), (0, 0, 0), (0, 0, 0), (0, 0, 0), (0, 0, 0), (0, 0, 0), (0, 0, 0), 590 | # (0, 0, 0), (0, 0, 0), (0, 0, 0), (0, 0, 0), (0, 0, 0), (0, 0, 0), (0, 0, 0), (0, 0, 0), (0, 0, 0), 591 | # (0, 0, 0)]: 592 | while all_moves != []: 593 | 594 | expand_node_id = MCTS_expand(node_id) 595 | all_moves = get_expand_move(node_id) 596 | # print('扩展动作集合',all_moves) 597 | 598 | simulation_trails = MCTS_simulation(expand_node_id)[0] # 模拟得到的节点队列 599 | # print('仿真得到的坐标：',MCTS_simulation(expand_node_id)[1]) 600 | if len(simulation_trails) == 0: 601 | backup(expand_node_id, 0) 602 | 603 | # todo 随机报错 604 | elif coverage_cal(simulation_trails[-1]) < 0.5: 605 | backup(expand_node_id, 0) 606 | else: 607 | simulation_leaf_node_id = simulation_trails[-1] # 模拟过程的叶子节点 608 | 609 | MCTS_backup(simulation_leaf_node_id, expand_node_id) # 从模拟得到的叶子节点开始回溯 610 | 611 | ''' 612 | 仿真结束后去除simulation过程创造的节点,使得蒙特卡洛树只保留expand得到的节点 613 | ''' 614 | expand_child_id = tree.children(nid=expand_node_id)[0].identifier 615 | tree.remove_subtree(nid=expand_child_id) 616 | 617 | # tree.show() 618 | # todo print('树节点数量：',len(tree.all_nodes())) 619 | # todo print(tree.children('root')) 620 | # print(len(tree.children('root'))) 621 | # print('扩展结束') 622 | z = MCTS_select(root_node_id) 623 | # print(z) 624 | select_id = z[0][-1].identifier 625 | # robot1_path = z[1][0] 626 | # robot2_path = z[1][1] 627 | # robot3_path = z[1][2] 628 | select_trails = z[1] 629 | 630 | # tree.show() 631 | 632 | # print(tree.depth(tree.get_node(nid=select_id))) # 测试用，用后即删 633 | # print(tree.parent(nid=select_id)) 634 | 635 | mcts_coverage = coverage_cal(select_id) / len(pass_point) 636 | print('覆盖率:', mcts_coverage) 637 | 638 | # x.append(step) 639 | # y.append(mcts_coverage) 640 | # step += 1 641 | 642 | node_id = select_id # 更新mcts过程的开始节点 643 | 644 | # todo 绘制单机器人实时覆盖率 645 | # plt.plot(x,y) 646 | # plt.rcParams['font.sans-serif'] = ['SimHei'] 647 | # plt.title('单机器人实时覆盖率',loc='center') 648 | # plt.xlabel('机器人覆盖步长/（步）') 649 | # plt.ylabel('覆盖率/（无单位）') 650 | # plt.savefig("单机器人实时覆盖率.png") 651 | # plt.clf() 652 | # plt.cla() 653 | # plt.close('all') 654 | 655 | return (select_trails) 656 | 657 | 658 | # 初始化 659 | AVAILABLE_CHOICES = [1, 2, 3, 4] # 定义可选操作集合--->映射到机器人四个可选动作 上下左右 660 | 661 | pass_point = [(1, 1), (1, 2), (1, 4), (1, 5), (1, 6), (1, 7), (1, 8), (1, 9), (1, 10), (2, 1), (2, 2), (2, 4), (2, 5), 662 | (2, 6), (2, 7), (2, 8), (2, 9), (2, 10), (3, 1), (3, 2), (3, 4), (3, 5), (3, 6), (3, 7), (3, 8), (3, 9), 663 | (3, 10), (4, 1), (4, 2), (4, 4), (4, 5), (4, 6), (4, 7), (4, 8), (4, 9), (4, 10), (5, 1), (5, 2), (5, 3), 664 | (5, 4), (5, 5), (5, 9), (5, 10), (6, 1), (6, 2), (6, 3), (6, 4), (6, 5), (6, 6), (6, 7), (6, 8), (6, 9), 665 | (6, 10), (7, 1), (7, 2), (7, 3), (7, 4), (7, 5), (7, 6), (7, 7), (7, 8), (7, 9), (7, 10), (8, 1), (8, 2), 666 | (8, 3), (8, 4), (8, 5), (8, 6), (8, 7), (8, 8), (8, 9), (8, 10), (9, 1), (9, 2), (9, 3), (9, 4), (9, 5), 667 | (9, 6), (9, 7), (9, 8), (9, 9), (9, 10), (10, 1), (10, 2), (10, 3), (10, 4), (10, 5), (10, 6), (10, 7), 668 | (10, 8), (10, 9), (10, 10)] 669 | obstacles = [(1, 3), (2, 3), (3, 3), (4, 3), (5, 6), (5, 7), (5, 8)] 670 | 671 | coveraged = [(1, 1), (1, 4), (6, 4)] 672 | 673 | # pass_point = [(1, 1), (1, 2), (2, 1), (2, 2), (3, 1), (3, 2), (4, 1), (4, 2), (5, 1), (5, 2), (5, 3), (6, 1), (6, 2), (6, 3), (7, 1), (7, 2), (7, 3), (8, 1), (8, 2), (8, 3), (9, 1), (9, 2), (9, 3), (10, 1), (10, 2), (10, 3)] 674 | # obstacles = [(1,3),(2,3),(3,3),(4,3)] # 机器人1 675 | # 676 | # pass_point = [(1, 4), (1, 5), (1, 6), (1, 7), (1, 8), (1, 9), (1, 10), (2, 4), (2, 5), (2, 6), (2, 7), (2, 8), (2, 9), (2, 10), (3, 4), (3, 5), (3, 6), (3, 7), (3, 8), (3, 9), (3, 10), (4, 4), (4, 5), (4, 6), (4, 7), (4, 8), (4, 9), (4, 10), (5, 4), (5, 5), (5, 9), (5, 10)] 677 | # obstacles = [(5, 6), (5, 7), (5, 8)] #机器人2 678 | # 679 | # pass_point = [(6, 4), (6, 5), (6, 6), (6, 7), (6, 8), (6, 9), (6, 10), (7, 4), (7, 5), (7, 6), (7, 7), (7, 8), (7, 9), (7, 10), (8, 4), (8, 5), (8, 6), (8, 7), (8, 8), (8, 9), (8, 10), (9, 4), (9, 5), (9, 6), (9, 7), (9, 8), (9, 9), (9, 10), (10, 4), (10, 5), (10, 6), (10, 7), (10, 8), (10, 9), (10, 10)] 680 | # obstacles = [] # 机器人3 681 | # 682 | # # pass_point = [(1, 1), (1, 2), (1, 4), (1, 5), (2, 1), (2, 2) ,(2, 4), (2, 5), (3, 1), (3, 2), (3, 3), (3, 4), (3, 5), (4, 1), (4, 2), (4, 3), (4, 4), (4, 5), (5, 1), (5, 2), (5, 3), (5, 4), (5, 5)] 683 | # # obstacles = [(1,3),(2,3)] 684 | # coveraged = [(1,4)] 685 | # pass_point= [(1, 1), (1, 2), (1, 3), (1, 4), (1, 5), (1, 6), (2, 2), (2, 3), (2, 6), (3, 1), (3, 2), (3, 3), (3, 5), (3, 6), (3, 7), (4, 1), (4, 2), (4, 3), (4, 4), (4, 5), (4, 6), (4, 7), (5, 1), (5, 2), (5, 3), (5, 5), (5, 6), (5, 7), (6, 1), (6, 2), (6, 3), (6, 4), (6, 5), (6, 6), (6, 7), (7, 1), (7, 2), (7, 3), (7, 4), (7, 5), (7, 6), (7, 7)] 686 | 687 | # obstacles=[(8, 8), (5, 10), (2, 3), (8, 5), (6, 2), (5, 4), (10, 1), (10, 6), (10, 3), (6, 1), (2, 5), (8, 4), (10, 9), (5, 7), (4, 4), (4, 8), (3, 4), (10, 8), (2, 4), (7, 4)] 688 | # pass_point=[(1, 1), (1, 2), (1, 3), (1, 4), (1, 5), (1, 6), (1, 7), (1, 8), (1, 9), (1, 10), (2, 1), (2, 2), (2, 6), (2, 7), (2, 8), (2, 9), (2, 10), (3, 1), (3, 2), (3, 3), (3, 5), (3, 6), (3, 7), (3, 8), (3, 9), (3, 10), (4, 1), (4, 2), (4, 3), (4, 5), (4, 6), (4, 7), (4, 9), (4, 10), (5, 1), (5, 2), (5, 3), (5, 5), (5, 6), (5, 8), (5, 9), (6, 3), (6, 4), (6, 5), (6, 6), (6, 7), (6, 8), (6, 9), (6, 10), (7, 1), (7, 2), (7, 3), (7, 5), (7, 6), (7, 7), (7, 8), (7, 9), (7, 10), (8, 1), (8, 2), (8, 3), (8, 6), (8, 7), (8, 9), (8, 10), (9, 1), (9, 2), (9, 3), (9, 4), (9, 5), (9, 6), (9, 7), (9, 8), (9, 9), (9, 10), (10, 2), (10, 4), (10, 5), (10, 7), (10, 10)] 689 | 690 | ''' 691 | 主函数入口 692 | ''' 693 | # 树结构定义 694 | tree = Tree() 695 | tree.create_node(tag='root', identifier='root', parent=None, 696 | data=[(1, 1), (1, 4), (6, 4), 0, 1, (1, 1, 1), 1]) 697 | # tree.create_node(tag='z', identifier='z', parent='root', 698 | # data=[(2, 1), (2, 4), (7, 4), 0, 1, (2, 2, 2), 1]) 699 | # 700 | expect_coverage = 1.0 701 | round = 1 702 | 703 | start = time.perf_counter() 704 | robot_path = main('root', expect_coverage) # 机器人覆盖路径 705 | end = time.perf_counter() 706 | print('程序运行时间：', end - start) 707 | 708 | repeat = len(robot_path) / (len(pass_point) * expect_coverage) # 机器人重复覆盖率 709 | length = len(robot_path) 710 | print('机器人路径：', robot_path) 711 | print('重复覆盖率：', repeat) 712 | print('覆盖步长:', length) 713 | 714 | tree.save2file(filename='tree') 715 | 716 | # print(get_moni_move('root',1)) 717 | --------------------------------------------------------------------------------