A-star.py

import datetime
import math
import matplotlib.pyplot as plt
import numpy as np
import cmath
import operator
#
# coordinates1 = np.array(
#     [[3979, 4854], [2965, 901], [1844, 1979], [2385, 537], [2156, 2169], [2582, 4561], [2920, 4481], [2746, 1749],
#      [1116, 364], [736, 2568], [1611, 1313], [3674, 4814], [3556, 3696], [1673, 465], [1304, 510], [365, 3962],
#      [2485, 2505], [967, 2414], [4771, 1303], [683, 564], [3876, 2460], [3319, 4193], [3449, 2322], [457, 3422],
#      [2702, 3892], [1778, 3699], [2251, 2849], [2384, 1894], [917, 3749], [878, 835], [1841, 1616], [2538, 1560],
#      [2582, 3891], [817, 1786], [3040, 2736], [1498, 706], [4851, 4512], [2139, 4515], [89, 1686], [4962, 4457],
#      [1275, 5], [1836, 665], [988, 701], [965, 547], [3143, 3909], [1081, 3319], [640, 2566], [1694, 938], [4702, 1536],
#      [2826, 4625]])  # 需求地坐标
# coordinates1goods = np.array(
#     [5, 2, 5, 2, 10, 2, 6, 8, 1, 8, 8, 3, 9, 5, 7, 2, 8, 2, 2, 8, 9, 2, 10, 10, 4, 8, 8, 8, 6, 9, 2, 1, 3, 4, 5, 6, 3,
#      9, 7, 10, 10, 8, 6, 6, 4, 9, 8, 9, 9, 6])
coordinates2 = np.array([[3322, 2500]])
coordinates2goods = np.array([10000])
truck_coordinates = [[4292, 4798, 50], [2403, 1155, 50], [852, 4540, 50], [411, 4568, 50], [4389, 1851, 50]]
coordinates1 = np.array(
    [[677, 3817], [4509, 1407], [4995, 913], [1499, 1294], [2734, 3141], [2700, 3730], [4374, 1215], [4168, 1643],
     [1842, 4170], [742, 1570], [2130, 2102], [3043, 4790], [2706, 1356], [2220, 1358], [4577, 4417], [2361, 1860],
     [284, 2006], [516, 4201], [4312, 291], [1071, 2173], [2967, 2750], [3775, 2902], [2116, 2286], [3995, 871],
     [1732, 532], [82, 1009], [4387, 1882], [1410, 4793], [2197, 4683], [3928, 2729], [3839, 2965], [2833, 4056],
     [569, 1859], [1724, 2066], [3655, 3042], [3129, 4734], [1728, 1122], [4474, 4016], [443, 3143], [3302, 2319],
     [4842, 1429], [1092, 3090], [1586, 1367], [3842, 1897], [1276, 4106], [1764, 3626], [2021, 4553], [431, 4190],
     [1315, 1251], [295, 1353], [393, 2866], [4251, 387], [1879, 1120], [1715, 2760], [4855, 1215], [384, 174],
     [4208, 269], [631, 1093], [2029, 1978], [3330, 4372], [2718, 2484], [4480, 3294], [4826, 3456], [3457, 23],
     [2449, 2563], [390, 4844], [4858, 4148], [1757, 3050], [540, 1430], [1739, 2575], [3551, 904], [4773, 1232],
     [2796, 4313], [195, 4761], [3165, 2833], [800, 2586], [1978, 3532], [4105, 4212], [202, 852], [4372, 3286],
     [3666, 3147], [4954, 2526], [3888, 4395], [1262, 2337], [1654, 2379], [3802, 4694], [2295, 3471], [4164, 2257],
     [551, 39], [3050, 4785], [1172, 4790], [1371, 3117], [736, 36], [682, 2463], [2042, 4230], [2241, 1852],
     [531, 2641], [2576, 1978], [3779, 938], [2946, 4847], [3562, 178], [3011, 4640], [3940, 2062], [2588, 2670],
     [3621, 1470], [2186, 1717], [3771, 347], [877, 1809], [2879, 1906], [3961, 1831], [544, 1860], [131, 3786],
     [4858, 672], [935, 1122], [1610, 4706], [2266, 4571], [1471, 4822], [3552, 4506], [4309, 3394], [4148, 1613],
     [4360, 647], [2858, 2631], [721, 2736], [4006, 4602], [1248, 86], [2831, 1050], [3857, 829], [3411, 1736],
     [40, 2157], [3903, 3437], [4014, 1865], [2594, 3043], [3322, 4400], [1966, 509], [1217, 491], [1229, 1482],
     [3431, 4919], [4019, 3024], [829, 4965], [352, 2890], [1200, 4796], [3215, 1812], [1032, 2706], [869, 3096],
     [497, 4921], [4892, 2033], [1486, 4200], [757, 152], [608, 3699], [2074, 3392], [4663, 2483], [87, 278],
     [908, 127], [4582, 1598], [3861, 3432], [3518, 3034], [1628, 647], [4938, 3408], [3798, 1635], [4932, 4328],
     [3540, 3578], [1701, 2877], [444, 3012], [1173, 1258], [4844, 1673], [1788, 4684], [2542, 996], [1997, 3799],
     [4040, 4657], [2884, 4579], [2155, 2341], [4372, 3071], [552, 4], [2545, 2857], [3818, 3511], [1207, 4325],
     [4829, 526], [1060, 3372], [3011, 1962], [2573, 4722], [3160, 4024], [3066, 430], [4802, 3126], [3827, 2222],
     [3047, 4255], [1851, 1114], [1481, 4350], [2204, 1866], [906, 1405], [1230, 1396], [2833, 3632], [447, 4705],
     [3238, 1045], [4587, 4846], [1307, 239], [4513, 4738], [3870, 2773], [1271, 4465], [4412, 4319], [4875, 2103],
     [303, 4009], [1917, 316], [4408, 4776], [735, 1509], [1469, 2181], [1490, 4345], [2994, 1522], [4575, 3185],
     [3778, 3039], [757, 4427], [3964, 3559], [2185, 1713], [493, 3087], [3813, 612], [2796, 2246], [3276, 1666],
     [4365, 1847], [1359, 4150], [213, 2812], [2129, 2162], [224, 2477], [3618, 3373], [1573, 4500], [1466, 2726],
     [3164, 3100], [777, 1956], [2610, 3231], [4238, 4706], [3546, 3621], [3175, 1383], [439, 4944], [3698, 4765],
     [2744, 4086], [439, 700], [4980, 264], [1103, 3787], [2743, 1041], [1441, 4234], [4960, 778], [3385, 4311],
     [4077, 2352], [2986, 2967], [4979, 2043], [4707, 4429], [265, 652], [3872, 3524], [4551, 1061], [1494, 3615],
     [1582, 1125], [3545, 769], [3283, 229], [1279, 3526], [303, 460], [4994, 1209], [4326, 2754], [869, 3193],
     [774, 4236], [1158, 4632], [1200, 3277], [2248, 2015], [3807, 3143], [2854, 2237], [1176, 3690], [1839, 752],
     [1804, 4334], [4911, 1541], [2966, 3259], [1100, 3782], [3930, 3525], [3252, 3593], [2660, 2442], [3830, 657],
     [4514, 4007], [4383, 834], [4715, 4339], [3521, 1721], [4948, 4091], [3926, 4391], [155, 1478], [3323, 4585],
     [4668, 4040], [4060, 1189], [1419, 1060], [432, 4034], [1290, 4224], [4627, 1757], [2736, 2839], [1098, 3039],
     [4663, 96], [2610, 1796], [4964, 2232], [4503, 4076], [3336, 3850], [1802, 446], [971, 8], [345, 3381],
     [1513, 3207], [1571, 851], [3213, 3650], [1226, 179], [4706, 2735], [2800, 1524], [2577, 1669], [4315, 856],
     [2647, 4936], [594, 2662], [1173, 716], [4837, 2097], [2116, 3220], [1003, 3966], [2220, 4052], [964, 1738],
     [2788, 2824], [940, 4473], [175, 1991], [2752, 495], [848, 969], [3017, 3145], [1470, 1903], [1790, 3712],
     [1270, 580], [4228, 3859], [3218, 2500], [825, 4541], [2764, 3179], [272, 235], [4794, 4739], [1329, 216],
     [4608, 575], [2741, 507], [4037, 3927], [1514, 56], [3838, 1669], [1217, 2235], [1974, 3081], [2511, 1352],
     [1210, 4252], [2774, 450], [3423, 442], [124, 1069], [3552, 4477], [1901, 396], [4681, 3531], [1097, 4144],
     [2725, 4126], [440, 2421], [3557, 3037], [1849, 1280], [415, 3466], [4506, 2096], [636, 2333], [205, 3888],
     [991, 4883], [1672, 3168], [2284, 581], [3027, 4080], [3510, 2057], [1571, 321], [4718, 3292], [3503, 1662],
     [24, 4389], [1621, 3823], [2082, 415], [4814, 2397], [3490, 1626], [4947, 1114], [2879, 2468], [3133, 2473],
     [847, 4915], [3153, 910], [851, 2655], [852, 1222], [378, 3516], [4354, 2227], [2165, 1622], [2603, 638],
     [1174, 2257], [2380, 2632], [1745, 2575], [4620, 3055], [3885, 1568], [4049, 2408], [917, 2594], [1835, 1124],
     [3250, 3675], [4067, 517], [2632, 1510], [3351, 42], [3197, 2709], [4998, 2021], [3948, 2744], [4763, 4739],
     [4179, 177], [527, 4513], [2948, 2449], [2455, 3339], [3951, 971], [533, 3564], [4476, 2978], [3243, 2482],
     [3985, 2572], [4510, 2354], [1769, 1137], [2467, 2580], [1314, 3124], [2167, 4803], [2195, 1508], [629, 499],
     [4760, 768], [2683, 3614], [1752, 3273], [3301, 1056], [1834, 2244], [4302, 3052], [4684, 3888], [2524, 1837],
     [3030, 4035], [4561, 4721], [3685, 689], [3759, 4599], [249, 3169], [4103, 4139], [3659, 3218], [3195, 3444],
     [848, 231], [4074, 2877], [873, 1103], [870, 4216], [3462, 2467], [3954, 2677], [2351, 2133], [318, 4328],
     [3998, 4459], [4509, 4939], [3240, 2924], [3963, 2602], [135, 2538], [1520, 1247], [1586, 3065], [3713, 746],
     [2001, 2217], [1905, 3651], [3629, 1278], [4362, 2602], [1339, 10], [2890, 1775], [438, 4528], [983, 834],
     [4721, 3170], [1153, 4991], [2136, 2788], [3132, 2485], [1700, 1534], [1196, 3636], [586, 4626], [3270, 1828],
     [3104, 4485], [2509, 2706], [3359, 1243], [4657, 3253], [2512, 4388], [980, 23], [814, 2524], [279, 3085],
     [2895, 2552], [935, 334], [4505, 2943], [2601, 1724], [1480, 3734], [584, 492], [4446, 1570], [2325, 4356],
     [348, 986], [2562, 3061], [660, 4833], [4850, 3771], [1825, 4327], [539, 4893], [3554, 3481], [2912, 1325],
     [2870, 3347], [2404, 4982], [4436, 4646], [4336, 3836], [3386, 3021], [676, 1415], [3569, 4110], [717, 1216],
     [683, 4002], [2137, 103], [1513, 128], [385, 2301], [3471, 1546], [1880, 2830], [3836, 1793], [2510, 1539],
     [2243, 4228], [763, 3869], [3093, 1313], [4120, 1094]])  # 需求地坐标
coordinates1goods = np.array(
    [10, 8, 5, 3, 6, 7, 10, 7, 1, 1, 10, 5, 1, 8, 9, 5, 10, 5, 2, 5, 5, 3, 6, 3, 7, 1, 8, 10, 2, 10, 1, 10, 6, 10, 6, 6,
     6, 2, 3, 1, 10, 10, 8, 5, 7, 2, 3, 2, 8, 10, 4, 2, 5, 7, 1, 9, 9, 8, 4, 9, 2, 2, 1, 10, 8, 8, 3, 7, 6, 10, 2, 3, 6,
     5, 9, 5, 7, 6, 6, 9, 3, 5, 7, 3, 5, 4, 6, 5, 4, 5, 2, 5, 10, 4, 10, 9, 8, 3, 1, 4, 9, 6, 3, 3, 8, 2, 5, 5, 6, 4, 8,
     2, 1, 3, 9, 5, 5, 2, 2, 5, 9, 6, 3, 5, 6, 5, 5, 5, 2, 6, 4, 9, 3, 9, 10, 5, 8, 2, 2, 1, 4, 7, 7, 8, 7, 7, 5, 6, 2,
     7, 6, 1, 1, 10, 4, 7, 9, 4, 4, 1, 5, 7, 6, 3, 10, 1, 7, 9, 1, 1, 6, 9, 3, 10, 2, 3, 5, 9, 7, 7, 5, 10, 4, 3, 3, 9,
     10, 2, 9, 2, 6, 3, 8, 6, 8, 9, 8, 10, 4, 5, 2, 5, 5, 7, 9, 9, 5, 6, 3, 4, 8, 1, 6, 9, 2, 1, 4, 3, 8, 5, 10, 9, 1,
     10, 8, 4, 7, 5, 9, 5, 9, 7, 8, 3, 1, 3, 8, 1, 4, 6, 1, 1, 8, 8, 10, 7, 9, 9, 8, 1, 5, 3, 10, 1, 1, 1, 7, 6, 1, 4,
     10, 6, 3, 7, 4, 7, 4, 5, 7, 6, 5, 2, 10, 9, 8, 10, 7, 6, 5, 5, 3, 2, 2, 8, 4, 6, 6, 2, 4, 5, 2, 8, 8, 3, 10, 9, 8,
     7, 9, 3, 2, 7, 4, 7, 5, 8, 2, 10, 6, 3, 2, 2, 10, 2, 9, 8, 5, 1, 8, 10, 5, 9, 1, 9, 5, 9, 7, 9, 2, 7, 6, 5, 6, 10,
     8, 6, 9, 2, 4, 3, 7, 4, 9, 5, 5, 7, 2, 1, 2, 5, 4, 3, 3, 7, 3, 6, 2, 8, 4, 5, 7, 1, 10, 9, 8, 4, 10, 9, 7, 1, 7, 3,
     5, 10, 8, 5, 8, 9, 7, 5, 4, 4, 5, 8, 1, 9, 6, 1, 2, 6, 10, 8, 2, 9, 6, 3, 4, 10, 7, 1, 6, 4, 6, 8, 3, 7, 8, 9, 7,
     8, 9, 1, 3, 7, 10, 4, 8, 3, 9, 1, 9, 8, 7, 6, 10, 5, 3, 6, 9, 8, 4, 2, 6, 1, 6, 3, 10, 4, 6, 1, 5, 1, 10, 4, 9, 10,
     3, 9, 6, 5, 3, 4, 8, 9, 6, 9, 9, 7, 10, 6, 2, 1, 2, 6, 3, 8, 5, 1, 5, 1, 8, 4, 5, 9, 4, 8, 3, 3, 2, 7, 8, 2, 4, 10,
     4, 5, 7, 2, 7, 7, 8, 3, 4, 1, 10, 7, 6, 7, 9, 5])


start_time = datetime.datetime.now()


# 供需地封装成site类
class Site:
    def __init__(self, x, y, ifo, goods):
        self.map = []
        self.x = x
        self.y = y
        self.store = 0
        self.need = 0
        self.description = ifo
        self.angle = 0
        if ifo == "req":
            self.need = goods
        elif ifo == "sup":
            self.store = goods

    def __str__(self):
        return '[{},{}]+{}+{}+{}+{}'.format(self.x, self.y, self.description, self.store, self.need, self.angle)


# sites里面储存了所有的供需地
sites = []
for i in range(len(coordinates1)):
    sites.append(Site(coordinates1[i][0], coordinates1[i][1], "req", coordinates1goods[i]))
for i in range(len(coordinates2)):
    sites.append(Site(coordinates2[i][0], coordinates2[i][1], "sup", coordinates2goods[i]))


# 获取长度
def length(x1, y1, x2, y2):
    return math.sqrt((int(x1) - int(x2)) ** 2 + (int(y1) - int(y2)) ** 2)  # 用于计算路径表


# 初始化每个site类内置的记录距离的地图和用来分组的极坐标系
for i in sites:
    i.angle = cmath.polar(complex(i.x - 3322, i.y - 58))[1]
# 分组操作
sites2 = []
for i in sites:
    if i.store == 0:
        sites2.append(i)
cmpfunc = operator.attrgetter('angle')
sites2.sort(key=cmpfunc)
zone = []
count = 0
group = []
temp = 0
while len(sites2) > 0:
    temp = sites2.pop()
    if count + temp.need > 50:
        ttt = group.copy()
        zone.append(ttt)
        group.clear()
        count = 0
        sites2.append(temp)
    else:
        count += temp.need
        group.append(temp)
        if (len(sites2) == 0):
            zone.append(group)


# 无人机类
class UAV:
    def __init__(self, x, y, volume, num):
        self.x = x
        self.y = y
        self.volume = volume
        self.covered_dis = 0
        self.draw_path = []
        self.capacity = 0
        self.at = Site(0, 0, 0, 0)
        self.number = num

    def __str__(self):
        return '坐标: [{},{}]当前运载的货量: {} 总共走了{}距离 '.format(self.x, self.y, self.capacity, self.covered_dis)

    # 画图操作
    def draw_picture(self):
        color = ['b', 'g', 'r', 'c']
        for k in range(len(coordinates1)):
            plt.plot(coordinates1[k][0], coordinates1[k][1], 'r', marker='o')  # 红色 需求点坐标为o
        for k in range(len(coordinates2)):
            plt.plot(coordinates2[k][0], coordinates2[k][1], 'b', marker='>')  # 蓝色 供应点坐标为>
        for k in range(len(self.draw_path) - 1):
            plt.plot((self.draw_path[k][0], self.draw_path[k + 1][0]),
                     (self.draw_path[k][1], self.draw_path[k + 1][1]),
                     color[self.number % 4])
        plt.title('car: ' + str(self.number), fontsize=30)
        plt.show()
        plt.close()


# 无人机初始化
UAVs = []
for i in range(len(truck_coordinates)):
    UAVs.append(UAV(truck_coordinates[i][0], truck_coordinates[i][1], truck_coordinates[i][2], i))
for i in UAVs:
    i.draw_path.append([i.x,i.y])

# A*启发式函数模块：
def isbest(i, bestpath, p):
    for k in bestpath[1:p + 1]:
        if i == k:
            return 1
    return 0


MAXCOUNT = 500
c = 0
for z in zone:
    Min = float('inf')
    Minf = float('inf')
    num = 0
    g = 0
    f = 0
    h = 0
    bestpath = []
    z.insert(0, Site(3322, 2500, "sup", 10000))
    ff = [0 for x in range(len(z))]
    gg = [0 for x in range(len(z))]
    bestpath = [0 for x in range(len(z))]
    print('_________________')
    for s in z:
        for ss in z:
            s.map.append(length(s.x, s.y, ss.x, ss.y))
    num = len(z)
    for p in range(num - 1):
        print(bestpath)
        for k in range(num):
            ff[k] = float('inf')
        for i in range(1, num):
            if isbest(i, bestpath, p):
                continue
            else:
                gg[i] = g + z[bestpath[p]].map[i]
            for m in range(num):
                if isbest(m, bestpath, p):
                    continue
                for t in range(m + 1, num):
                    if isbest(t, bestpath, p):
                        continue
                    if m != 0 or t != i or p == num - 2:
                        if z[m].map[t] < Min:
                            Min = z[m].map[t]
            h = Min * (num - p - 1)
            ff[i] = gg[i] + h
            Min = float('inf')
        for i in range(num):
            if ff[i] < Minf:
                Minf = ff[i]
                bestpath[p + 1] = i
        Minf = float('inf')
        g = g + z[bestpath[p]].map[bestpath[p + 1]]

    for i in bestpath:
        UAVs[c].draw_path.append([z[i].x, z[i].y])
    print(UAVs[c].draw_path)
    c = (c + 1) % len(UAVs)
end_time = datetime.datetime.now()
print()
print('算法时间:', end_time - start_time)
for i in UAVs:
    for j in range(1, len(i.draw_path)):
        i.covered_dis += length(i.draw_path[j - 1][0], i.draw_path[j - 1][1], i.draw_path[j][0], i.draw_path[j][0])
time_list = []
for i in UAVs:
    time_list.append(i.covered_dis)
    for j in range(len(truck_coordinates)):
        plt.plot(truck_coordinates[j][0], truck_coordinates[j][1], 'black', marker='1')  # 黑色 汽车初始位置
    i.draw_picture()
print(sum(time_list))