一、人工势场法
# 初始化参数设置
import numpy as np
import matplotlib.pyplot as plt
import copy
from celluloid import Camera # 保存动图时用，pip install celluloid
%matplotlib qt5
## 初始化车的参数
d = 3.5 #道路标准宽度
W = 1.8 # 汽车宽度
L = 4.7 # 车长
P0 = np.array([0, - d / 2, 1, 1]) #车辆起点位置，分别代表x,y,vx,vy
Pg = np.array([99, d / 2, 0, 0]) # 目标位置
# 障碍物位置
Pobs = np.array([
[15, 7 / 4, 0, 0],
[30, - 3 / 2, 0, 0],
[45, 3 / 2, 0, 0],
[60, - 3 / 4, 0, 0],
[80, 3/2, 0, 0]])
P = np.vstack((Pg,Pobs)) # 将目标位置和障碍物位置合放在一起
Eta_att = 5 # 引力的增益系数
Eta_rep_ob = 15 # 斥力的增益系数
Eta_rep_edge = 50 # 道路边界斥力的增益系数
d0 = 20 # 障碍影响的最大距离
num = P.shape[0] #障碍与目标总计个数
len_step = 0.5 # 步长
n=1
Num_iter = 300 # 最大循环迭代次数
# 数据存储变量定义
path = [] # 保存车走过的每个点的坐标
delta = np.zeros((num,2)) # 保存车辆当前位置与障碍物的方向向量，方向指向车辆；以及保存车辆当前位置与目标点的方向向量，方向指向目标点
dists = [] # 保存车辆当前位置与障碍物的距离以及车辆当前位置与目标点的距离
unite_vec = np.zeros((num,2)) # 保存车辆当前位置与障碍物的单位方向向量，方向指向车辆；以及保存车辆当前位置与目标点的单位方向向量，方向指向目标点
F_rep_ob = np.zeros((len(Pobs),2)) # 存储每一个障碍到车辆的斥力,带方向
v=np.linalg.norm(P0[2:4]) # 设车辆速度为常值
## ***************初始化结束，开始主体循环******************人工势场法核心代码
Pi = P0[0:2] # 当前车辆位置
# count=0
for i in range(Num_iter):
if ((Pi[0] - Pg[0]) ** 2 + (Pi[1] - Pg[1]) ** 2) ** 0.5 < 1:
break
dists=[]
path.append(Pi)
# print(count)
# count+=1
#计算车辆当前位置与障碍物的单位方向向量
for j in range(len(Pobs)):
delta[j]=Pi[0:2] - Pobs[j, 0:2]
dists.append(np.linalg.norm(delta[j]))
unite_vec[j]=delta[j]/dists[j]
#计算车辆当前位置与目标的单位方向向量
delta[len(Pobs)]=Pg[0:2] - Pi[0:2]
dists.append(np.linalg.norm(delta[len(Pobs)]))
unite_vec[len(Pobs)] = delta[len(Pobs)]/dists[len(Pobs)]
## 计算引力
F_att = Eta_att*dists[len(Pobs)]*unite_vec[len(Pobs)]
## 计算斥力
# 在原斥力势场函数增加目标调节因子（即车辆至目标距离），以使车辆到达目标点后斥力也为0
for j in range(len(Pobs)):
if dists[j] >= d0:
F_rep_ob[j] = np.array([0, 0])
else:
# 障碍物的斥力1，方向由障碍物指向车辆
F_rep_ob1_abs = Eta_rep_ob * (1 / dists[j] - 1 / d0) * (dists[len(Pobs)])**n / dists[j] ** 2 # 斥力大小
F_rep_ob1 = F_rep_ob1_abs*unite_vec[j] # 斥力向量
# 障碍物的斥力2，方向由车辆指向目标点
F_rep_ob2_abs = n/2 * Eta_rep_ob * (1 / dists[j] - 1 / d0) **2 *(dists[len(Pobs)])**(n-1) # 斥力大小
F_rep_ob2 = F_rep_ob2_abs * unite_vec[len(Pobs)] # 斥力向量
# 改进后的障碍物合斥力计算
F_rep_ob[j] = F_rep_ob1 + F_rep_ob2
# 增加道路边界斥力势场，根据车辆当前位置，选择对应的斥力函数
if Pi[1] > - d + W / 2 and Pi[1] <= - d / 2:
F_rep_edge = [0, Eta_rep_edge * v * np.exp(-d / 2 - Pi[1])] # 下道路边界区域斥力势场，方向指向y轴正向
elif Pi[1] > - d / 2 and Pi[1] <= - W / 2:
F_rep_edge = np.array([0, 1 / 3 * Eta_rep_edge * Pi[1] ** 2])
elif Pi[1] > W / 2 and Pi[1] <= d / 2:
F_rep_edge = np.array([0, - 1 / 3 * Eta_rep_edge * Pi[1] ** 2])
elif Pi[1] > d / 2 and Pi[1] <= d - W / 2:
F_rep_edge = np.array([0, Eta_rep_edge * v * (np.exp(Pi[1] - d / 2))])
## 计算合力和方向
F_rep = np.sum(F_rep_ob, axis=0)+F_rep_edge
F_sum = F_att+F_rep
UnitVec_Fsum = 1 / np.linalg.norm(F_sum) * F_sum
#计算车的下一步位置
Pi = copy.deepcopy(Pi+ len_step * UnitVec_Fsum)
# Pi[0:2] = Pi[0:2] + len_step * UnitVec_Fsum
# print(Pi)
path.append(Pg[0:2]) # 最后把目标点也添加进路径中
path=np.array(path) # 转为numpy
## 画图
fig=plt.figure(1)
# plt.ylim(-4, 4)
plt.axis([-10,100,-15,15])
camera = Camera(fig)
len_line = 100
# 画灰色路面图
GreyZone = np.array([[- 5, - d - 0.5], [- 5, d + 0.5],
[len_line, d + 0.5], [len_line, - d - 0.5]])
for i in range(len(path)):
plt.fill(GreyZone[:, 0], GreyZone[:, 1], 'gray')
plt.fill(np.array([P0[0], P0[0], P0[0] - L, P0[0] - L]), np.array([- d /
2 - W / 2, - d / 2 + W / 2, - d / 2 + W / 2, - d / 2 - W / 2]), 'b')
# 画分界线
plt.plot(np.array([- 5, len_line]), np.array([0, 0]), 'w--')
plt.plot(np.array([- 5, len_line]), np.array([d, d]), 'w')
plt.plot(np.array([- 5, len_line]), np.array([- d, - d]), 'w')
# 设置坐标轴显示范围
# plt.axis('equal')
# plt.gca().set_aspect('equal')
# 绘制路径
plt.plot(Pobs[:,0],Pobs[:,1], 'ro') #障碍物位置
plt.plot(Pg[0],Pg[1], 'gv') # 目标位置
plt.plot(P0[0],P0[1], 'bs') # 起点位置
# plt.cla()
plt.plot(path[0:i,0],path[0:i,1], 'k') # 路径点
plt.pause(0.001)
# camera.snap()
# animation = camera.animate()
# animation.save('trajectory.gif')二、D*算法
# 首次搜索
# coding=utf-8
import matplotlib.pyplot as plt
import numpy as np
import math
map_grid = [[1 for j in range(0, 8)] for i in range(0, 8)] # 定义列表
map_grid = np.array(map_grid) # 将列表转化为数组，因为只有数组才有维度的概念，方便切片
map_grid[3:6, 1] = 0 # 障碍物
map_grid[3:6, 5] = 0
map_grid[0, 3] = 5 # 起点
map_grid[7, 3] = 6 # 终点
def draw_effect(map_grid,second_path):
plt.imshow(map_grid, cmap=plt.cm.hot, interpolation='nearest', vmin=0, vmax=10) # 绘制热力图
plt.colorbar()
plt.xlim(-1, 8) # x轴的范围
plt.ylim(-1, 8)
my_x_ticks = np.arange(0, 8, 1) # x轴标号的范围
my_y_ticks = np.arange(0, 8, 1)
plt.xticks(my_x_ticks)
plt.yticks(my_y_ticks)
second_path = np.array(second_path)
plt.plot(second_path[:, 1:2],second_path[:, 0:1],'-')
# plt.grid(True) # 开启栅格 可以不开启
plt.show() # 可视化
open_list = [[7, 3, 0, 0, None, None]] # 将终点置于open列表中列表中分别有x0,y0坐标,h值,父节点X、Y坐标
close_list = []
# draw_effect(map_grid)
# 将邻域放入open_list中
def open_list_append(x0, y0, x1, y1, h0, h1, map_grid, open_list):
if 0 <= x1 <= 7 and 0 <= y1 <= 7 and map_grid[x1, y1] != 4 and map_grid[x1, y1] != 0: # 左边没有越界并且没有在closelist里面
if map_grid[x1, y1] == 3: # 如果是在open_list中,h要更新
open_list = np.array(open_list)
if (h1 + h0) < open_list[np.where((open_list[:, 0] == x1) & (open_list[:, 1] == y1)), 2]:
h = h1 + h0
k = h1 + h0
open_list[np.where((open_list[:, 0] == x1) & (open_list[:, 1] == y1)), 2] = h
open_list[np.where((open_list[:, 0] == x1) & (open_list[:, 1] == y1)), 3] = k
open_list[np.where((open_list[:, 0] == x1) & (open_list[:, 1] == y1)), 4] = x0
open_list[np.where((open_list[:, 0] == x1) & (open_list[:, 1] == y1)), 4] = y0
open_list = list(open_list.tolist())
else: # 是new节点
h = h1 + h0
k = h1 + h0
# open_list = list(open_list)
open_list.append([x1, y1, h, k, x0, y0])
map_grid[x1, y1] = 3
return open_list
# 首次搜索
def first_search(open_list, close_list, map_grid): # 给出终点坐标，完成首次遍历
# 采用D算法遍历
# 选openlist中h最小的,将openlist按照h排序，取第一个，并删除第一个，将它放到close_list里面
open_list = list(open_list)
open_list.sort(key=lambda x: x[2])
# open_list.pop(0)
insert_list = open_list[0] # 引入中间列表，用来存储每一次被选中的遍历的点
x0 = int(insert_list[0])
y0 = int(insert_list[1])
open_list.pop(0)
close_list.append(list(insert_list))
map_grid[x0, y0] = 4 # 被加入到close_list里面
# 找insert_list的邻域 ----->寻找顺序：从左边开始逆时针
h0 = int(insert_list[2])
x1 = x0
y1 = y0 - 1
h1 = 10
open_list = open_list_append(x0, y0, x1, y1, h0, h1, map_grid, open_list)
x1 = x0 - 1
y1 = y0 - 1
h1 = 14
open_list = open_list_append(x0, y0, x1, y1, h0, h1, map_grid, open_list)
x1 = x0 - 1
y1 = y0
h1 = 10
open_list = open_list_append(x0, y0, x1, y1, h0, h1, map_grid, open_list)
x1 = x0 - 1
y1 = y0 + 1
h1 = 14
open_list = open_list_append(x0, y0, x1, y1, h0, h1, map_grid, open_list)
x1 = x0
y1 = y0 + 1
h1 = 10
open_list = open_list_append(x0, y0, x1, y1, h0, h1, map_grid, open_list)
x1 = x0 + 1
y1 = y0 + 1
h1 = 14
open_list = open_list_append(x0, y0, x1, y1, h0, h1, map_grid, open_list)
x1 = x0 + 1
y1 = y0
h1 = 10
open_list = open_list_append(x0, y0, x1, y1, h0, h1, map_grid, open_list)
x1 = x0 + 1
y1 = y0 - 1
h1 = 14
open_list = open_list_append(x0, y0, x1, y1, h0, h1, map_grid, open_list)
return [open_list, close_list, map_grid]
while map_grid[0, 3] != 4 and open_list != []:
[open_list, close_list, map_grid] = first_search(open_list, close_list, map_grid)
# 首次搜索完成
first_path = []
close_list = np.array(close_list)
xn = 0
yn = 3
while xn != 7 or yn != 3:
list1 = list(close_list[np.where((close_list[:, 0] == xn) & (close_list[:, 1] == yn))][0])
xn = int(list1[4])
yn = int(list1[5])
first_path.append(list1)
first_path.append([7, 3, 0, 0, None, None])
# 第二次搜索
# 通过上面的程序已经找到了一条路径，完成了第一次的搜索，此时每个节点的h和k是相等的。此时开始点在close list里面，最短路径在firstpath中。
# 可以看出，每个节点的父节点都是该节点的八个邻节点中k值最小的哪个。
# 当出现动态变化时，我们可以利用这个图尽快修正我们的路径，而不是重新规划。
# 当我们检测到某点被阻碍了：1、修改这个点的h值，h变为inf，把它放入openlist中。注意此时该节点的k还是小值，是原来哪个h的值，因此它将立即被取出
# 2、把这个修改扩散出去，直到kmin >=h
# 设置一个突然出现的障碍
map_grid[3, 3] = 0
close_list[np.where((close_list[:, 4] == 3) & (close_list[:, 5] == 3)), 2] = math.inf
close_list[np.where((close_list[:, 0] == 3) & (close_list[:, 1] == 3)), 2] = math.inf
insertRow = list(close_list[np.where((close_list[:, 4] == 3) & (close_list[:, 5] == 3))][0])
x = int(insertRow[0])
y = int(insertRow[1])
open_list.append(insertRow) # ->>>>>>open_list是列表格式
map_grid[x, y] = 3
close_list = list(close_list.tolist()) # ----->>>>>close_list是列表格式
close_list.remove(insertRow)
open_list.sort(key=lambda x: x[3]) # 先排序，选择k最小的节点
k_min = open_list[0][3] #
hx = open_list[0][2]
# 接下来把这个点扩散出去
def find_neighbor(x0, y0, x1, y1, k_old, hx, h1, close_list):
close_list = np.array(close_list)
hy = close_list[np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 2][0]
if (hy <= k_old) and (hx > hy + h1):
close_list[np.where((close_list[:, 0] == x0) & (close_list[:, 1] == y0)), 4] = x1
close_list[np.where((close_list[:, 0] == x0) & (close_list[:, 1] == y0)), 5] = y1
close_list[np.where((close_list[:, 0] == x0) & (close_list[:, 1] == y0)), 2] = hy + h1
hx = hy + h1
return [hx, list(close_list.tolist())]
def find_neighbor2(x0, y0, x1, y1, k_old, hx, h1, close_list, open_list, map_grid):
close_list = np.array(close_list)
hy = close_list[np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 2][0]
if (close_list[np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 4] == x0 and close_list[
np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 5] == y0 and (hy != hx + h1)) or (
(hy > hx + h1) and (
(close_list[np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 4] != x0) or (
close_list[np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 5] != y0))):
close_list[np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 4] = x0
close_list[np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 5] = y0
close_list[np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 2] = hx + h1
Y = list(close_list[np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1))][0])
# 把Y放入open_list中
close_list = list(close_list.tolist())
close_list.remove(Y)
open_list.append(Y)
map_grid[x1, y1] = 3
return [open_list, close_list, map_grid]
def find_neighbor3(x0, y0, x1, y1, k_old, hx, h1, close_list, open_list, map_grid):
close_list = np.array(close_list)
hy = close_list[np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 2][0]
if (close_list[np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 4] == x0 and close_list[
np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 5] == y0 and (hy != hx + h1)):
close_list[np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 4] = x0
close_list[np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 5] = y0
close_list[np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 2] = hx + h1
Y = list(close_list[np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1))][0])
# 把Y放入open_list中
close_list = list(close_list.tolist())
close_list.remove(Y)
open_list.append(Y)
map_grid[x1, y1] = 3
else:
if ((close_list[np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 4] != x0 or close_list[
np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 5] != y0) and (hy > hx + h1)):
#print(list(close_list[np.where((close_list[:, 0] == x0) & (close_list[:, 1] == y0))][0]))
if map_grid[x0,y0]!=3:
X = list(close_list[np.where((close_list[:, 0] == x0) & (close_list[:, 1] == y0))][0])
close_list = list(close_list.tolist())
close_list.remove(X)
open_list.append(X)
else:
open_list = np.array(open_list)
X = list(open_list[np.where((open_list[:, 0] == x0) & (open_list[:, 1] == y0))][0])
open_list = list(open_list.tolist())
# # 把Y放入open_list中
map_grid[x0, y0] = 3
else:
if ((close_list[np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 4] != x0 or close_list[
np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 5] != y0) and (hx > hy + h1)) and \
map_grid[x1, y1] == 4 and hy > k_old:
if map_grid[x1, y1] != 3:
Y = list(close_list[np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1))][0])
close_list = list(close_list.tolist())
close_list.remove(Y)
open_list.append(Y)
else:
open_list = np.array(open_list)
Y = list(open_list[np.where((open_list[:, 0] == x1) & (open_list[:, 1] == y1))][0])
open_list = list(open_list.tolist())
# 把Y放入open_list中
map_grid[x1, y1] = 3
return [open_list, close_list, map_grid]
# 扩散程序 process-state:先弹出openlist列表中k最小的节点，并删除这个节点。然后分类处理：
def process_state(open_list, close_list, map_grid):
# 修改这个点的h值
open_list.sort(key=lambda x: x[3]) # 先排序，选择k最小的节点
X = open_list[0] # X表示k最小的节点
x0 = int(X[0])
y0 = int(X[1])
close_list.append(X) # 将它放入closelist
map_grid[x0, y0] = 4
open_list.remove(X)
# 从openlist中删除这个节点
# 分类处理：（该节点处于lower状态，该节点处于lower状态）
k_old = X[3]
hx = X[2]
# print(close_list)
if k_old < hx: # k_old是上升状态
x1 = x0
y1 = y0 - 1
h1 = 10
[hx, close_list] = find_neighbor(x0, y0, x1, y1, k_old, hx, h1, close_list)
x1 = x0 - 1
y1 = y0 - 1
h1 = 14
[hx, close_list] = find_neighbor(x0, y0, x1, y1, k_old, hx, h1, close_list)
x1 = x0 - 1
y1 = y0
h1 = 10
[hx, close_list] = find_neighbor(x0, y0, x1, y1, k_old, hx, h1, close_list)
x1 = x0 - 1
y1 = y0 + 1
h1 = 14
[hx, close_list] = find_neighbor(x0, y0, x1, y1, k_old, hx, h1, close_list)
x1 = x0
y1 = y0 + 1
h1 = 10
[hx, close_list] = find_neighbor(x0, y0, x1, y1, k_old, hx, h1, close_list)
x1 = x0 + 1
y1 = y0 + 1
h1 = 14
[hx, close_list] = find_neighbor(x0, y0, x1, y1, k_old, hx, h1, close_list)
x1 = x0 + 1
y1 = y0
h1 = 10
[hx, close_list] = find_neighbor(x0, y0, x1, y1, k_old, hx, h1, close_list)
x1 = x0 + 1
y1 = y0 - 1
h1 = 14
[hx, close_list] = find_neighbor(x0, y0, x1, y1, k_old, hx, h1, close_list)
# 找它的邻节点，看能不能让它的h降低
#print(hx)
# if k_old == hx: # 该节点x处于lower状态
# x1 = x0
# y1 = y0 - 1
# h1 = 10
# [open_list, close_list, map_grid] = find_neighbor2(x0, y0, x1, y1, k_old, hx, h1, close_list, open_list,
# map_grid)
#
# x1 = x0 - 1
# y1 = y0 - 1
# h1 = 14
# [open_list, close_list, map_grid] = find_neighbor2(x0, y0, x1, y1, k_old, hx, h1, close_list, open_list,
# map_grid)
#
# x1 = x0 - 1
# y1 = y0
# h1 = 10
# [open_list, close_list, map_grid] = find_neighbor2(x0, y0, x1, y1, k_old, hx, h1, close_list, open_list,
# map_grid)
#
# x1 = x0 - 1
# y1 = y0 + 1
# h1 = 14
# [open_list, close_list, map_grid] = find_neighbor2(x0, y0, x1, y1, k_old, hx, h1, close_list, open_list,
# map_grid)
#
# x1 = x0
# y1 = y0 + 1
# h1 = 10
# [open_list, close_list, map_grid] = find_neighbor2(x0, y0, x1, y1, k_old, hx, h1, close_list, open_list,
# map_grid)
#
# x1 = x0 + 1
# y1 = y0 + 1
# h1 = 14
# [open_list, close_list, map_grid] = find_neighbor2(x0, y0, x1, y1, k_old, hx, h1, close_list, open_list,
# map_grid)
#
# x1 = x0 + 1
# y1 = y0
# h1 = 10
# [open_list, close_list, map_grid] = find_neighbor2(x0, y0, x1, y1, k_old, hx, h1, close_list, open_list,
# map_grid)
#
# x1 = x0 + 1
# y1 = y0 - 1
# h1 = 14
# [open_list, close_list, map_grid] = find_neighbor2(x0, y0, x1, y1, k_old, hx, h1, close_list, open_list,
# map_grid)
#
# else:
# x1 = x0
# y1 = y0 - 1
# h1 = 10
# [open_list, close_list, map_grid] = find_neighbor3(x0, y0, x1, y1, k_old, hx, h1, close_list, open_list,
# map_grid)
#
# x1 = x0 - 1
# y1 = y0 - 1
# h1 = 14
# [open_list, close_list, map_grid] = find_neighbor3(x0, y0, x1, y1, k_old, hx, h1, close_list, open_list,
# map_grid)
# #
# x1 = x0 - 1
# y1 = y0
# h1 = 10
# [open_list, close_list, map_grid] = find_neighbor3(x0, y0, x1, y1, k_old, hx, h1, close_list, open_list,
# map_grid)
# #
# x1 = x0 - 1
# y1 = y0 + 1
# h1 = 14
# [open_list, close_list, map_grid] = find_neighbor3(x0, y0, x1, y1, k_old, hx, h1, close_list, open_list,
# map_grid)
# #
# x1 = x0
# y1 = y0 + 1
# h1 = 10
# [open_list, close_list, map_grid] = find_neighbor3(x0, y0, x1, y1, k_old, hx, h1, close_list, open_list,
# map_grid)
# x1 = x0 + 1
# y1 = y0 + 1
# h1 = 14
# [open_list, close_list, map_grid] = find_neighbor3(x0, y0, x1, y1, k_old, hx, h1, close_list, open_list,
# map_grid)
#
# x1 = x0 + 1
# y1 = y0
# h1 = 10
# [open_list, close_list, map_grid] = find_neighbor3(x0, y0, x1, y1, k_old, hx, h1, close_list, open_list,
# map_grid)
# x1 = x0 + 1
# y1 = y0 - 1
# h1 = 14
# [open_list, close_list, map_grid] = find_neighbor3(x0, y0, x1, y1, k_old, hx, h1, close_list, open_list,
# map_grid)
open_list.sort(key=lambda x: x[3]) # 先排序，选择k最小的节点
k_min = open_list[0][3] #
return [open_list, list(close_list), map_grid,k_min,hx]
while k_min<hx:
[open_list, close_list, map_grid,k_min,hx] = process_state(open_list, close_list, map_grid)
#避障
second_path = []
close_list = np.array(close_list)
xn = 0
yn = 3
while xn != 7 or yn != 3:
list1 = list(close_list[np.where((close_list[:, 0] == xn) & (close_list[:, 1] == yn))][0])
xn = int(list1[4])
yn = int(list1[5])
second_path.append(list1)
second_path.append([7, 3, 0, 0, None, None])
draw_effect(map_grid,second_path)
print("Find it")三、遗传算法
import numpy as np
import matplotlib.pyplot as plt
from matplotlib import cm
from mpl_toolkits.mplot3d import Axes3D
DNA_SIZE = 24
POP_SIZE = 200
CROSSOVER_RATE = 0.8
MUTATION_RATE = 0.005
N_GENERATIONS = 50
X_BOUND = [-3, 3]
Y_BOUND = [-3, 3]
def F(x, y):
return 3*(1-x)**2*np.exp(-(x**2)-(y+1)**2)- 10*(x/5 - x**3 - y**5)*np.exp(-x**2-y**2)- 1/3**np.exp(-(x+1)**2 - y**2)
def plot_3d(ax):
X = np.linspace(*X_BOUND, 100)
Y = np.linspace(*Y_BOUND, 100)
X,Y = np.meshgrid(X, Y)
Z = F(X, Y)
ax.plot_surface(X,Y,Z,rstride=1,cstride=1,cmap=cm.coolwarm)
ax.set_zlim(-10,10)
ax.set_xlabel('x')
ax.set_ylabel('y')
ax.set_zlabel('z')
plt.pause(3)
plt.show()
def get_fitness(pop):
x,y = translateDNA(pop)
pred = F(x, y)
return (pred - np.min(pred)) + 1e-3 #减去最小的适应度是为了防止适应度出现负数，通过这一步fitness的范围为[0, np.max(pred)-np.min(pred)],最后在加上一个很小的数防止出现为0的适应度
def translateDNA(pop): #pop表示种群矩阵，一行表示一个二进制编码表示的DNA，矩阵的行数为种群数目
x_pop = pop[:,1::2]#奇数列表示X
y_pop = pop[:,::2] #偶数列表示y
#pop:(POP_SIZE,DNA_SIZE)*(DNA_SIZE,1) --> (POP_SIZE,1)
x = x_pop.dot(2**np.arange(DNA_SIZE)[::-1])/float(2**DNA_SIZE-1)*(X_BOUND[1]-X_BOUND[0])+X_BOUND[0]
y = y_pop.dot(2**np.arange(DNA_SIZE)[::-1])/float(2**DNA_SIZE-1)*(Y_BOUND[1]-Y_BOUND[0])+Y_BOUND[0]
return x,y
def crossover_and_mutation(pop, CROSSOVER_RATE = 0.8):
new_pop = []
for father in pop: #遍历种群中的每一个个体，将该个体作为父亲
child = father #孩子先得到父亲的全部基因（这里我把一串二进制串的那些0，1称为基因）
if np.random.rand() < CROSSOVER_RATE: #产生子代时不是必然发生交叉，而是以一定的概率发生交叉
mother = pop[np.random.randint(POP_SIZE)] #再种群中选择另一个个体，并将该个体作为母亲
cross_points = np.random.randint(low=0, high=DNA_SIZE*2) #随机产生交叉的点
child[cross_points:] = mother[cross_points:] #孩子得到位于交叉点后的母亲的基因
mutation(child) #每个后代有一定的机率发生变异
new_pop.append(child)
return new_pop
def mutation(child, MUTATION_RATE=0.003):
if np.random.rand() < MUTATION_RATE: #以MUTATION_RATE的概率进行变异
mutate_point = np.random.randint(0, DNA_SIZE*2) #随机产生一个实数，代表要变异基因的位置
child[mutate_point] = child[mutate_point]^1 #将变异点的二进制为反转
def select(pop, fitness): # nature selection wrt pop's fitness
idx = np.random.choice(np.arange(POP_SIZE), size=POP_SIZE, replace=True,
p=(fitness)/(fitness.sum()) )
return pop[idx]
def print_info(pop):
fitness = get_fitness(pop)
max_fitness_index = np.argmax(fitness)
print("max_fitness:", fitness[max_fitness_index])
x,y = translateDNA(pop)
print("最优的基因型：", pop[max_fitness_index])
print("(x, y):", (x[max_fitness_index], y[max_fitness_index]))
if __name__ == "__main__":
fig = plt.figure()
ax = Axes3D(fig)
plt.ion()#将画图模式改为交互模式，程序遇到plt.show不会暂停，而是继续执行
plot_3d(ax)
pop = np.random.randint(2, size=(POP_SIZE, DNA_SIZE*2)) #matrix (POP_SIZE, DNA_SIZE)
for _ in range(N_GENERATIONS):#迭代N代
x,y = translateDNA(pop)
if 'sca' in locals():
sca.remove()
sca = ax.scatter(x, y, F(x,y), c='black', marker='o');plt.show();plt.pause(0.1)
pop = np.array(crossover_and_mutation(pop, CROSSOVER_RATE))
#F_values = F(translateDNA(pop)[0], translateDNA(pop)[1])#x, y --> Z matrix
fitness = get_fitness(pop)
pop = select(pop, fitness) #选择生成新的种群
print_info(pop)
plt.ioff()
plot_3d(ax)四、粒子群算法
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
def fit_fun(x): # 适应函数
return sum(100.0 * (x[0][1:] - x[0][:-1] ** 2.0) ** 2.0 + (1 - x[0][:-1]) ** 2.0)
class Particle:
# 初始化
def __init__(self, x_max, max_vel, dim):
self.__pos = np.random.uniform(-x_max, x_max, (1, dim)) # 粒子的位置
self.__vel = np.random.uniform(-max_vel, max_vel, (1, dim)) # 粒子的速度
self.__bestPos = np.zeros((1, dim)) # 粒子最好的位置
self.__fitnessValue = fit_fun(self.__pos) # 适应度函数值
def set_pos(self, value):
self.__pos = value
def get_pos(self):
return self.__pos
def set_best_pos(self, value):
self.__bestPos = value
def get_best_pos(self):
return self.__bestPos
def set_vel(self, value):
self.__vel = value
def get_vel(self):
return self.__vel
def set_fitness_value(self, value):
self.__fitnessValue = value
def get_fitness_value(self):
return self.__fitnessValue
class PSO:
def __init__(self, dim, size, iter_num, x_max, max_vel, tol, best_fitness_value=float('Inf'), C1=2, C2=2, W=1):
self.C1 = C1
self.C2 = C2
self.W = W
self.dim = dim # 粒子的维度
self.size = size # 粒子个数
self.iter_num = iter_num # 迭代次数
self.x_max = x_max
self.max_vel = max_vel # 粒子最大速度
self.tol = tol # 截至条件
self.best_fitness_value = best_fitness_value
self.best_position = np.zeros((1, dim)) # 种群最优位置
self.fitness_val_list = [] # 每次迭代最优适应值
# 对种群进行初始化
self.Particle_list = [Particle(self.x_max, self.max_vel, self.dim) for i in range(self.size)]
def set_bestFitnessValue(self, value):
self.best_fitness_value = value
def get_bestFitnessValue(self):
return self.best_fitness_value
def set_bestPosition(self, value):
self.best_position = value
def get_bestPosition(self):
return self.best_position
# 更新速度
def update_vel(self, part):
vel_value = self.W * part.get_vel() + self.C1 * np.random.rand() * (part.get_best_pos() - part.get_pos()) \
+ self.C2 * np.random.rand() * (self.get_bestPosition() - part.get_pos())
vel_value[vel_value > self.max_vel] = self.max_vel
vel_value[vel_value < -self.max_vel] = -self.max_vel
part.set_vel(vel_value)
# 更新位置
def update_pos(self, part):
pos_value = part.get_pos() + part.get_vel()
part.set_pos(pos_value)
value = fit_fun(part.get_pos())
if value < part.get_fitness_value():
part.set_fitness_value(value)
part.set_best_pos(pos_value)
if value < self.get_bestFitnessValue():
self.set_bestFitnessValue(value)
self.set_bestPosition(pos_value)
def update_ndim(self):
for i in range(self.iter_num):
for part in self.Particle_list:
self.update_vel(part) # 更新速度
self.update_pos(part) # 更新位置
self.fitness_val_list.append(self.get_bestFitnessValue()) # 每次迭代完把当前的最优适应度存到列表
print('第{}次最佳适应值为{}'.format(i, self.get_bestFitnessValue()))
if self.get_bestFitnessValue() < self.tol:
break
return self.fitness_val_list, self.get_bestPosition()
if __name__ == '__main__':
# test 香蕉函数
pso = PSO(4, 5, 10000, 30, 60, 1e-4, C1=2, C2=2, W=1)
fit_var_list, best_pos = pso.update_ndim()
print("最优位置:" + str(best_pos))
print("最优解:" + str(fit_var_list[-1]))
plt.plot(range(len(fit_var_list)), fit_var_list, alpha=0.5)五、蚁群算法
# -*- coding: utf-8 -*-
import random
import copy
import time
import sys
import math
import tkinter #//GUI模块
import threading
from functools import reduce
# 参数
'''
ALPHA:信息启发因子，值越大，则蚂蚁选择之前走过的路径可能性就越大
，值越小，则蚁群搜索范围就会减少，容易陷入局部最优
BETA:Beta值越大，蚁群越就容易选择局部较短路径，这时算法收敛速度会
加快，但是随机性不高，容易得到局部的相对最优
'''
(ALPHA, BETA, RHO, Q) = (1.0,2.0,0.5,100.0)
# 城市数，蚁群
(city_num, ant_num) = (50,50)
distance_x = [
178,272,176,171,650,499,267,703,408,437,491,74,532,
416,626,42,271,359,163,508,229,576,147,560,35,714,
757,517,64,314,675,690,391,628,87,240,705,699,258,
428,614,36,360,482,666,597,209,201,492,294]
distance_y = [
170,395,198,151,242,556,57,401,305,421,267,105,525,
381,244,330,395,169,141,380,153,442,528,329,232,48,
498,265,343,120,165,50,433,63,491,275,348,222,288,
490,213,524,244,114,104,552,70,425,227,331]
#城市距离和信息素
distance_graph = [ [0.0 for col in range(city_num)] for raw in range(city_num)]
pheromone_graph = [ [1.0 for col in range(city_num)] for raw in range(city_num)]
#----------- 蚂蚁 -----------
class Ant(object):
# 初始化
def __init__(self,ID):
self.ID = ID # ID
self.__clean_data() # 随机初始化出生点
# 初始数据
def __clean_data(self):
self.path = [] # 当前蚂蚁的路径
self.total_distance = 0.0 # 当前路径的总距离
self.move_count = 0 # 移动次数
self.current_city = -1 # 当前停留的城市
self.open_table_city = [True for i in range(city_num)] # 探索城市的状态
city_index = random.randint(0,city_num-1) # 随机初始出生点
self.current_city = city_index
self.path.append(city_index)
self.open_table_city[city_index] = False
self.move_count = 1
# 选择下一个城市
def __choice_next_city(self):
next_city = -1
select_citys_prob = [0.0 for i in range(city_num)] #存储去下个城市的概率
total_prob = 0.0
# 获取去下一个城市的概率
for i in range(city_num):
if self.open_table_city[i]:
try :
# 计算概率：与信息素浓度成正比，与距离成反比
select_citys_prob[i] = pow(pheromone_graph[self.current_city][i], ALPHA) * pow((1.0/distance_graph[self.current_city][i]), BETA)
total_prob += select_citys_prob[i]
except ZeroDivisionError as e:
print ('Ant ID: {ID}, current city: {current}, target city: {target}'.format(ID = self.ID, current = self.current_city, target = i))
sys.exit(1)
# 轮盘选择城市
if total_prob > 0.0:
# 产生一个随机概率,0.0-total_prob
temp_prob = random.uniform(0.0, total_prob)
for i in range(city_num):
if self.open_table_city[i]:
# 轮次相减
temp_prob -= select_citys_prob[i]
if temp_prob < 0.0:
next_city = i
break
# 未从概率产生，顺序选择一个未访问城市
# if next_city == -1:
# for i in range(city_num):
# if self.open_table_city[i]:
# next_city = i
# break
if (next_city == -1):
next_city = random.randint(0, city_num - 1)
while ((self.open_table_city[next_city]) == False): # if==False,说明已经遍历过了
next_city = random.randint(0, city_num - 1)
# 返回下一个城市序号
return next_city
# 计算路径总距离
def __cal_total_distance(self):
temp_distance = 0.0
for i in range(1, city_num):
start, end = self.path[i], self.path[i-1]
temp_distance += distance_graph[start][end]
# 回路
end = self.path[0]
temp_distance += distance_graph[start][end]
self.total_distance = temp_distance
# 移动操作
def __move(self, next_city):
self.path.append(next_city)
self.open_table_city[next_city] = False
self.total_distance += distance_graph[self.current_city][next_city]
self.current_city = next_city
self.move_count += 1
# 搜索路径
def search_path(self):
# 初始化数据
self.__clean_data()
# 搜素路径，遍历完所有城市为止
while self.move_count < city_num:
# 移动到下一个城市
next_city = self.__choice_next_city()
self.__move(next_city)
# 计算路径总长度
self.__cal_total_distance()
#----------- TSP问题 -----------
class TSP(object):
def __init__(self, root, width = 800, height = 600, n = city_num):
# 创建画布
self.root = root
self.width = width
self.height = height
# 城市数目初始化为city_num
self.n = n
# tkinter.Canvas
self.canvas = tkinter.Canvas(
root,
width = self.width,
height = self.height,
bg = "#EBEBEB", # 背景白色
xscrollincrement = 1,
yscrollincrement = 1
)
self.canvas.pack(expand = tkinter.YES, fill = tkinter.BOTH)
self.title("TSP蚁群算法(n:初始化 e:开始搜索 s:停止搜索 q:退出程序)")
self.__r = 5
self.__lock = threading.RLock() # 线程锁
self.__bindEvents()
self.new()
# 计算城市之间的距离
for i in range(city_num):
for j in range(city_num):
temp_distance = pow((distance_x[i] - distance_x[j]), 2) + pow((distance_y[i] - distance_y[j]), 2)
temp_distance = pow(temp_distance, 0.5)
distance_graph[i][j] =float(int(temp_distance + 0.5))
# 按键响应程序
def __bindEvents(self):
self.root.bind("q", self.quite) # 退出程序
self.root.bind("n", self.new) # 初始化
self.root.bind("e", self.search_path) # 开始搜索
self.root.bind("s", self.stop) # 停止搜索
# 更改标题
def title(self, s):
self.root.title(s)
# 初始化
def new(self, evt = None):
# 停止线程
self.__lock.acquire()
self.__running = False
self.__lock.release()
self.clear() # 清除信息
self.nodes = [] # 节点坐标
self.nodes2 = [] # 节点对象
# 初始化城市节点
for i in range(len(distance_x)):
# 在画布上随机初始坐标
x = distance_x[i]
y = distance_y[i]
self.nodes.append((x, y))
# 生成节点椭圆，半径为self.__r
node = self.canvas.create_oval(x - self.__r,
y - self.__r, x + self.__r, y + self.__r,
fill = "#ff0000", # 填充红色
outline = "#000000", # 轮廓白色
tags = "node",
)
self.nodes2.append(node)
# 显示坐标
self.canvas.create_text(x,y-10, # 使用create_text方法在坐标（302，77）处绘制文字
text = '('+str(x)+','+str(y)+')', # 所绘制文字的内容
fill = 'black' # 所绘制文字的颜色为灰色
)
# 顺序连接城市
#self.line(range(city_num))
# 初始城市之间的距离和信息素
for i in range(city_num):
for j in range(city_num):
pheromone_graph[i][j] = 1.0
self.ants = [Ant(ID) for ID in range(ant_num)] # 初始蚁群
self.best_ant = Ant(-1) # 初始最优解
self.best_ant.total_distance = 1 << 31 # 初始最大距离
self.iter = 1 # 初始化迭代次数
# 将节点按order顺序连线
def line(self, order):
# 删除原线
self.canvas.delete("line")
def line2(i1, i2):
p1, p2 = self.nodes[i1], self.nodes[i2]
self.canvas.create_line(p1, p2, fill = "#000000", tags = "line")
return i2
# order[-1]为初始值
reduce(line2, order, order[-1])
# 清除画布
def clear(self):
for item in self.canvas.find_all():
self.canvas.delete(item)
# 退出程序
def quite(self, evt):
self.__lock.acquire()
self.__running = False
self.__lock.release()
self.root.destroy()
print (u"\n程序已退出...")
sys.exit()
# 停止搜索
def stop(self, evt):
self.__lock.acquire()
self.__running = False
self.__lock.release()
# 开始搜索
def search_path(self, evt = None):
# 开启线程
self.__lock.acquire()
self.__running = True
self.__lock.release()
while self.__running:
# 遍历每一只蚂蚁
for ant in self.ants:
# 搜索一条路径
ant.search_path()
# 与当前最优蚂蚁比较
if ant.total_distance < self.best_ant.total_distance:
# 更新最优解
self.best_ant = copy.deepcopy(ant)
# 更新信息素
self.__update_pheromone_gragh()
print (u"迭代次数：",self.iter,u"最佳路径总距离：",int(self.best_ant.total_distance))
# 连线
self.line(self.best_ant.path)
# 设置标题
self.title("TSP蚁群算法(n:随机初始 e:开始搜索 s:停止搜索 q:退出程序) 迭代次数: %d" % self.iter)
# 更新画布
self.canvas.update()
self.iter += 1
# 更新信息素
def __update_pheromone_gragh(self):
# 获取每只蚂蚁在其路径上留下的信息素
temp_pheromone = [[0.0 for col in range(city_num)] for raw in range(city_num)]
for ant in self.ants:
for i in range(1,city_num):
start, end = ant.path[i-1], ant.path[i]
# 在路径上的每两个相邻城市间留下信息素，与路径总距离反比
temp_pheromone[start][end] += Q / ant.total_distance
temp_pheromone[end][start] = temp_pheromone[start][end]
# 更新所有城市之间的信息素，旧信息素衰减加上新迭代信息素
for i in range(city_num):
for j in range(city_num):
pheromone_graph[i][j] = pheromone_graph[i][j] * RHO + temp_pheromone[i][j]
# 主循环
def mainloop(self):
self.root.mainloop()
#----------- 程序的入口处 -----------
if __name__ == '__main__':
print (u"""
--------------------------------------------------------
程序：蚁群算法解决TPS问题程序
作者：许彬
日期：2015-12-10
语言：Python 2.7
--------------------------------------------------------
""")
TSP(tkinter.Tk()).mainloop()本文章为转载内容，我们尊重原作者对文章享有的著作权。如有内容错误或侵权问题，欢迎原作者联系我们进行内容更正或删除文章。
