遗传算法求解无人机路径多目标规划问题（python实现）_遗传算法 多目标路径规划

问题描述

前置背景

该问题是笔者所在学校2021年数学建模的第二次训练赛的第二问

问题一即有3种通信方式D1,D2,D3，他们在三个海拔高度3km，6km，9km的通信包络网存在一个661*621的网格地图中，地图由0和1组成，包络网可以见下图

其中前缀D1代表采用D1通信方式，后缀3000代表其为在海拔3KM处的包络网

问题二

问题二是无人机航线规划多目标问题，总共有九架无人机，所以相当于有9个子问题

如图从上到下依次是九架无人机在各自所能支持的通信方式和飞行海拔下的通信范围网，其中青色代表D1通信方式，左上角是飞行的起点，右下角是飞行的终点。无人机在通信盲区，即白色区域也可以支持飞行。

该问题是如何规划无人机的航线，使得其依次尽可能满足以下要求，试给出最佳方案
1.通信盲区最小
2.航程最短
3.D1优先

遗传算法

可参考知乎上的答案
如何通俗易懂地解释遗传算法？有什么例子？
https://www.zhihu.com/question/23293449

我想让机器人Bob画出我的梦中情人（最优解）。Bob当然不知道我梦中情人长啥样。Bob很无奈，只能一张张试。先画一张，问我像不像？我说不像，Bob就重新画一张。直到我觉得像。然而Bob又不会画画，只会填格子。于是Bob准备了一张1000*1000的格子纸，每个格子可以填黑色或者白色。那么总共有2^1000000种画法（NP-hard问题）。如果我能坚持到宇宙毁灭N次，那可以每张都看，然后找到那个最像的。显然我没这个耐心。于是我只让Bob画10万张，画不出来我就砸了它。Bob很紧张，开始想办法，终于想到了遗传算法。第一轮，Bob随机画了1万张（初始种群）。这1万张里面，肯定各种乱七八糟，有像星空的，像猪的，像石头的，等等。然后Bob让我挑最像的。妈蛋，我强忍怒火，挑出来一堆猪、猴、狗，好歹是哺乳动物。Bob拿着我挑的“猪猴狗们”，如获至宝，开始第二轮，将这些画各种交叉变异一下。比如把一幅画的耳朵跟另一幅的鼻子换一下，或者再随机改个眼睛。然后又生成了1万张图让我挑。我挑出来一堆猴子猩猩，好歹是灵长类动物。如此反复好多轮后，挑出来的开始像人了，虽然有男人、女人、小孩。慢慢地，开始有美女了。再慢慢地，就越来越像了。在画了10万张图后，终于找到一张还不错的。虽然不是梦中情人，但也很不错呐（次优解）。这就是遗传算法。

其是进化算法的一种，是一种现代智能优化算法
其广泛应用于NP-hard问题的求解，尤其是找函数最值，这个在MATLAB等数学软件中都有工具箱
但是在这里，问题求解用到的是遗传算法的思想，不能直接使用工具箱
需要按照他的思想来从底层实现初始化种群选择交叉变异
对于只有三天的数学建模来说，编程难度极高，细节非常多
尤其是如何初始化种群是一个很大的问题

问题的求解

基本思想参考B站基于智能算法的路径规划，可以上去听视频讲解，下面的有些图来自其PPT
文字版：基于遗传算法的移动机器人路径规划
本文问题规模有600*600，很大，那个只有10 * 10，而且问题不完全相同，需要重新设计求解方案

流程

第一步：初始化种群
第二步：计算适应度
第三步：选择
第四步：交叉
第五步：变异
第六步：若未达迭代次数则继续迭代，转向第二步；
若达到迭代次数，输出最优解

初始化种群

采取栅格图法创建地图空间，便于确定直角坐标系。
以5 * 5地图为例，编码如下

种群初始化按他这样子会有一些小问题，该问题中相当于无障碍，只要能够到达就可以

种群初始化是最难的部分，试了快一天，如果在一个点随机采用向下或向上，其形状类似一根直线（虽然解也是像一条直线）
经过很久的尝试后，总算得到了比较完美的随机路径

适应度函数

在本文中主要是直接用经过路径上的D1网格个数，通信盲区网格个数以及总距离的一个加权和作为多目标适应度函数，具体定义可见下

#计算适应度函数
def calcfit(popu, *mpf):
    cl = 100
    mp0 = mpf[0]
    mp1 = mpf[1] if len(mp) > 1 else -1
    global a1,a2,a3
    popu = np.asarray(popu)
    lenth = popu.argmin()+1 if popu[-1] == 0 else len(popu)
    #print(lenth,' ',popu)
    d1 = 0
    d2 = 0
    d5 = 0
    for i in range(lenth):
        y_now = int((popu[i]) // (col))
        x_now = int((popu[i]) % (col))
        y_next = int((popu[i+1]) // (col)) if i + 1 <lenth else y_now
        x_next = int((popu[i+1]) % (col)) if i + 1 <lenth else x_now
        
        #print(mp0, y_now, x_now)
        if mp[mp0].iloc[y_now][x_now] == 1:
            d1 += 1
        elif mp1 != -1 and mp[mp1].iloc[y_now][x_now] == 1:
            d2 += 1
        
        if x_next != 0:
            d5 += np.sqrt((x_next - x_now) * (x_next - x_now) + (y_next - y_now) * (x_next - x_now))
    d3 = lenth - d2 - d1
    
    fit0 = d3 
    fit1 = d5
    #乘cl是为了缩放，防止损失精度，该目标fit2为极大化目标，希望D1尽可能多
    fit2 = cl / d1 if d1 !=0 else 0 
    return a0 * fit0 + a1 * fit1 + a2 * fit2 #越大越好
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33

选择

将适应度值从小到大排序，假设种群中强者的概率是50%，即0.5，并且认为强者都能存活（这里强者的概率其实就是对强者筛选过了），即都能进入下一代。那么剩下的50%就是弱者，而对于弱者，也有一个弱者存活概率比如0.3，之后采取随机存活的方式，产生一个0-1之间的随机小数,若0.3大于这个小数 ,那么认为可以存活，否则直接舍弃。存活下来的强者和弱者构成了一个新的种群，进行下一步的交叉操作。

交叉

首先需要确定一个交叉概率pc，产生0-1之间的一个随机数，并和交叉概率pc比较，若小于pc则进行交叉操作。交叉方式采用的是单点交叉的方式，具体的交叉操作是找出两条路径中所有相同的点，然后随机选择其中的一个点，将之后的路径进行交叉操作。具体的流程图如图4（a）所示，其中n为路径数（即种群数量）。

变异

首先需要确定一个变异概率pm，产生0-1之间的一个随机数，并和变异概率pm比较，若小于pm则进行变异操作。变异方法是随机选取路径中除起点和终点以外的两个栅格，去除这两个栅格之间的路径，然后以这两个栅格为相邻点，使用初始化路径中的第二步将这两个点进行连续操作。此时有可能无法产生连续的路径，则需要重新选择两个栅格执行以上操作，直到完成变异操作。

结果

调节a1,a2,a3的值可以得到不同的轨迹，其实好像就是一条直线？
按需调节参数，可能会有不同结果，目前笔者的参数下，就是一条线

参考文献：
基于智能算法的路径规划
基于遗传算法的移动机器人路径规划
算法学习之遗传算法路径规划

代码

import numpy as np
import pandas as pd
import matplotlib.pyplot as plt
import random

col = 621
row = 661
a1, a2, a3 = 1, 1, 1#fit函数比重
DEBUG = False #debug标志，把运行过程中的解画出来
mark = np.zeros((9,661,621)) #地图格点，9（无人机）*661*621（地图网格）
bot_info = np.zeros((9,4))
ap_info = np.zeros((3,2))
mp = [None for i in range(9)]
#读取网格，注意编号与包络网对应
mp[0] = pd.read_csv('D1_3000.csv',index_col = 0)
mp[1] = pd.read_csv('D1_6000.csv',index_col = 0)
mp[2] = pd.read_csv('D1_9000.csv',index_col = 0)
mp[3] = pd.read_csv('D2_3000.csv',index_col = 0)
mp[4] = pd.read_csv('D2_6000.csv',index_col = 0)
mp[5] = pd.read_csv('D2_9000.csv',index_col = 0)
mp[6] = pd.read_csv('D3_3000.csv',index_col = 0)
mp[7] = pd.read_csv('D3_6000.csv',index_col = 0)
mp[8] = pd.read_csv('D3_9000.csv',index_col = 0)
info = pd.read_excel('info.xlsx',sheet_name = 'hx',index_col = 0)

#画出起飞点及落地点
def draw(a, x, y):
    x0 = int(x - 15) if x >= 15 else 0
    y0 = int(y - 15) if y >= 15 else 0
    for i in range(x0,min(x0+30,660)):
        for j in range(y0,min(y0+30,660)):
            mark[a][i][j] = 1

#清除轨迹线
def clear_mat():
    global mark
    mark = np.zeros((9,661,621))
    for i in range(9):
        x0,y0,x1,y1 = bot_info[i][0], bot_info[i][1], bot_info[i][2], bot_info[i][3]
        draw(i, x0, y0)
        draw(i, x1, y1)

#将经纬坐标转为问题中的格点
def calc_mp(lat, lon):
    ilat = (lat - 11)/0.05
    ilon = (lon - 142)/0.05
    return int(ilat),int(ilon)

#计算起飞点以及落地点在格点中的位置
def calc_info():
    for i in range(9):
        bot_info[i][0], bot_info[i][1] = calc_mp(info['fromlat'][i], info['fromlon'][i])
        bot_info[i][2], bot_info[i][3] = calc_mp(info['tolat'][i], info['tolon'][i])

    for i in range(11,14):
        ap_info[i-11][0], ap_info[i-11][1] =  calc_mp(info['fromlat'][i], info['fromlon'][i])
    return bot_info, ap_info

#画出问题描述图
def draw_fig():
    fig = plt.figure(figsize = (20,15))
    plt.subplot(331)
    test = np.stack((1-mp[1],1-mark[0],1-mp[4]),axis = 2)
    plt.title('UAV_A',y = -0.15)
    plt.imshow(test)
    plt.subplot(332)
    test = np.stack((1-mp[1],1-mark[1],1-mp[7]),axis = 2)
    plt.title('UAV_B',y = -0.15)
    plt.imshow(test)
    plt.subplot(333)
    test = np.stack((1-mp[1],1-mark[2],np.ones((661,621))),axis = 2)
    plt.title('UAV_C',y = -0.15)
    plt.imshow(test)
    plt.subplot(334)
    test = np.stack((1-mp[2],1-mark[3],np.ones((661,621))),axis = 2)
    plt.title('UAV_D',y = -0.15)
    plt.imshow(test)
    plt.subplot(335)
    test = np.stack((1-mp[1],1-mark[4],1-mp[7]),axis = 2)
    plt.title('UAV_E',y = -0.15)
    plt.imshow(test)
    plt.subplot(336)
    test = np.stack((1-mp[1],1-mark[5],np.ones((661,621))),axis = 2)
    plt.title('UAV_F',y = -0.15)
    plt.imshow(test)
    plt.subplot(337)
    test = np.stack((1-mp[0],1-mark[6],1-mp[3]),axis = 2)
    plt.title('UAV_G',y = -0.15)
    plt.imshow(test)
    plt.subplot(338)
    test = np.stack((1-mp[0],1-mark[7],1-mp[6]),axis = 2)
    plt.title('UAV_H',y = -0.15)
    plt.imshow(test)
    plt.subplot(339)
    test = np.stack((1-mp[1],1-mark[8],np.ones((661,621))),axis = 2)
    plt.title('UAV_I',y = -0.15)
    plt.imshow(test)
    plt.savefig('UAV.png')
    test = np.stack((1-mp[1],1-mp[8],1-mp[4]),axis = 2)
    plt.title('Coverage',y = -0.15)
    plt.imshow(test)
    plt.savefig('Coverage.png')

#用于产生连续路径
def gen_con_path(old_popu, uav = -1):
    new_popu = list(old_popu)   
    lengh = len(new_popu) 
    i = 0
    global DEBUG
    while i + 1 < lengh:      
        y_now = (new_popu[i]) // (col) 
        x_now = (new_popu[i]) % (col)
        y_next =  (new_popu[i+1]) // (col) 
        x_next =  (new_popu[i+1]) % (col)
        #print(x_now, y_now, x_next, y_next)
        if x_next == 0:
            break
            
        while max(abs(x_next - x_now), abs(y_next - y_now)) != 1:
            x_insert = np.ceil((x_next + x_now) // 2)     
            y_insert = np.ceil((y_next + y_now) // 2)
            if uav != -1 and DEBUG:
                mark[uav][int(y_insert)][int(x_insert)] = 1
                pass
            num_insert = (x_insert) + (y_insert) * col 
            new_popu.insert(i+1,num_insert)
            #print('insert',x_insert,y_insert)
            y_next = num_insert // col
            x_next = num_insert % col

        i = i+1
        lengh = len(new_popu) 

    return  np.asarray(new_popu)

#用于初始化种群
def init_popu(N, uav):
    md = 10
    global DEBUG
    down = bot_info[uav][2] - bot_info[uav][0]
    left = bot_info[uav][1] - bot_info[uav][3]
    sx = int(bot_info[uav][1])
    sy = int(bot_info[uav][0])
    ex = int(bot_info[uav][3])
    ey = int(bot_info[uav][2])
    js = np.zeros((N,10000))
    for n in range(N):
        n = int(n)
        jy = random.sample(range(sy + 1, ey), md)
        jx = random.sample(range(sx, ex, -1), md)
        jy.insert(0, sy)
        jx.insert(0, sx)
        jy.append(ey)
        jx.append(ex)
        for i in range(md + 2):
            js[n][i] = jy[i] * 621 + jx[i]
            if DEBUG:
                mark[uav][jy[i]][jx[i]] = 1
        npopu = gen_con_path(js[n], uav)
        for i in range(3000):
            js[n][i] = npopu[i]
            
    return js

#计算适应度函数
def calcfit(popu, *mpf):
    cl = 100
    mp0 = mpf[0]
    mp1 = mpf[1] if len(mp) > 1 else -1
    global a1,a2,a3
    popu = np.asarray(popu)
    lenth = popu.argmin()+1 if popu[-1] == 0 else len(popu)
    #print(lenth,' ',popu)
    d1 = 0
    d2 = 0
    d5 = 0
    for i in range(lenth):
        y_now = int((popu[i]) // (col))
        x_now = int((popu[i]) % (col))
        y_next = int((popu[i+1]) // (col)) if i + 1 <lenth else y_now
        x_next = int((popu[i+1]) % (col)) if i + 1 <lenth else x_now
        
        #print(mp0, y_now, x_now)
        if mp[mp0].iloc[y_now][x_now] == 1:
            d1 += 1
        elif mp1 != -1 and mp[mp1].iloc[y_now][x_now] == 1:
            d2 += 1
        
        if x_next != 0:
            d5 += np.sqrt((x_next - x_now) * (x_next - x_now) + (y_next - y_now) * (x_next - x_now))
    d3 = lenth - d2 - d1
    
    fit0 = d3 
    fit1 = d5
    fit2 = cl / d1 if d1 !=0 else 0
    return a0 * fit0 + a1 * fit1 + a2 * fit2

#选择
def selection(pop, N, *mpf):
    value = []
    for i in range(len(pop)):
        value.append(calcfit(pop[i], *mpf))
        
    retain_rate = 0.6
    random_rate = 0.3
    dict_my = dict(zip(value, pop))     

    new_popu = []
    sort_dis = sorted(dict_my)   
    retain_lengh = int(len(pop)*retain_rate)      
    temp = sort_dis[:retain_lengh]  
    
    for i in sort_dis[retain_lengh:]:  
        if random.random() < random_rate:
            temp.append(i)
    
    for i in temp:
        new_popu.append(dict_my[i]) 
    return new_popu

#交叉
def cross(parents,pc):
    children = []  
    single_popu_index_list = []
    lenparents = len(parents)  
    parity = lenparents % 2 
    for i in range(0,lenparents-1,2):       
        single_now_popu = list(parents[i])  
        single_next_popu = list(parents[i+1])
        children.append([]) 
        children.append([])
        index_content = list(set(single_now_popu).intersection(set(single_next_popu))) 
        
        num_rep = len(index_content)         
        if random.random() < pc and num_rep>=3:
            content = index_content[random.randint(1,num_rep-2)]   
            now_index = single_now_popu.index(content)  
            next_index = single_next_popu.index(content)
            children[i] = single_now_popu[0:now_index + 1] + single_next_popu[next_index + 1:]
            children[i+1] = single_next_popu[0:next_index + 1] + single_now_popu[now_index + 1:]
        else:
            children[i] = parents[i]
            children[i+1] = parents[i+1]
            
    if parity == 1:     
        children.append([]) 
        children[-1] = parents[-1] 
    return children

#变异
def mutation(children,pm):
    row = len(children)  
    new_popu = []
    for i in range(row):

        single_popu = np.asarray(children[i])
        col = single_popu.argmin()+1 if single_popu[-1] == 0 else len(single_popu)
        single_popu = list(single_popu)
        if random.random()<pm:
            first = random.randint(1,col - 3) 
            second = random.randint(1,col - 3)
            if first != second :    
                #return single_popu, first, second
                if(first<second):
                    single_popu = single_popu[0:first]+single_popu[second+1:]
                else :
                    single_popu = single_popu[0:second] + single_popu[first+1:]
            #print(single_popu)
            temp = gen_con_path(single_popu)
            if temp!= []:
                new_popu.append(temp)
        else:      
            new_popu.append(single_popu)
    return new_popu

#评估
def eva(popu, Iter, flag, uav, *mpf):
    mp0 = mpf[0]
    mp1 = mpf[1] if len(mp) > 1 else -1
    clear_mat()
    
    value = []
    Iter = int(Iter)
    for i in range(len(popu)):
        value.append(calcfit(popu[i], *mpf))
        
    minnum = value[0]
    for i in range(len(value)):
        if value[i] < minnum:
            minnum = value[i]
    popubest = popu[value.index(minnum)]
    print('第',Iter,'次迭代最优适应函数值：',minnum)
        
    if flag:
        for i in popubest:
            y = int(i // col)
            x = int(i % col)
            mark[uav][y][x] = 1
        mat0 = 1 - mp[mp0]
        mat1 = 1 - mp[mp1] if mp != -1 else np.ones((661,621))
        test = np.stack((mat0,1-mark[uav],mat1),axis = 2)
        plt.title('iteration = '+str(Iter),y = -0.15)
        plt.imshow(test)
        plt.savefig(str(Iter) + 'th iteration' + '.png')
        
    return value

#问题求解函数
def run(uav, N, Iter, *mpf):
    clear_mat()
    global iterations
    iterations = 1
    popu = init_popu(N, uav)
    for i in range(Iter):
        lastpopu = popu
        new = selection(popu, N, *mpf)
        child = cross(new,0.8)
        popu = mutation(child,0.8)
        if popu == []:
            popu = lastpopu
            continue
            
        value = eval(popu, iterations, True, 0, 1, 4) 
        iterations += 1

    print('processed')
    if popu == []:  
        print('无路径')
    else:
        value = []
        for i in range(len(popu)):
            value.append(calcfit(popu[i], 1, 4))
        minnum = value[0]
        for i in range(len(value)):
            if value[i] < minnum:
                minnum = value[i]
        popu = popu[value.index(minnum)]

        print('最短路径值为：',minnum)
        print('最短路径为：',popu)
        
if __name__=="__main__":
    print(calc_info()) #计算起飞点与落地点
    draw_fig() #画出问题描述图
    clear_mat() #清除图上的轨迹
    #求解9个无人机的轨迹
    run(0, 5000, 1000, 1, 4)#无人机A，以下类似
    run(1, 5000, 1000, 1, 7)
    run(2, 5000, 1000, 1)
    run(3, 5000, 1000, 2)
    run(4, 5000, 1000, 1, 7)
    run(5, 5000, 1000, 1)
    run(6, 5000, 1000, 0, 3)
    run(7, 5000, 1000, 0, 6)
    run(8, 5000, 1000, 1)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355

从Jupyter Lab直接复制过来的，直接用IDE运行有可能有一些小错误