随机路线图算法(Probabilistic Roadmap, PRM)-Python实现
import math
from PIL import Image
import numpy as np
import networkx as nx
import copy
STAT_OBSTACLE='#'
STAT_NORMAL='.'
class RoadMap():
""" 读进一张图片，二值化成为有障碍物的二维网格化地图，并提供相关操作
"""
def __init__(self,img_file):
"""图片变二维数组"""
test_map = []
img = Image.open(img_file)
# img = img.resize((100,100)) ### resize图片尺寸
img_gray = img.convert('L') # 地图灰度化
img_arr = np.array(img_gray)
img_binary = np.where(img_arr<127,0,255)
for x in range(img_binary.shape[0]):
temp_row = []
for y in range(img_binary.shape[1]):
status = STAT_OBSTACLE if img_binary[x,y]==0 else STAT_NORMAL
temp_row.append(status)
test_map.append(temp_row)
self.map = test_map
self.cols = len(self.map[0])
self.rows = len(self.map)
def is_valid_xy(self, x,y):
if x < 0 or x >= self.rows or y < 0 or y >= self.cols:
return False
return True
def not_obstacle(self,x,y):
return self.map[x][y] != STAT_OBSTACLE
def EuclidenDistance(self, xy1, xy2):
"""两个像素点之间的欧几里得距离"""
dis = 0
for (x1, x2) in zip(xy1, xy2):
dis += (x1 - x2)**2
return dis**0.5
def ManhattanDistance(self,xy1,xy2):
"""两个像素点之间的曼哈顿距离"""
dis = 0
for x1,x2 in zip(xy1,xy2):
dis+=abs(x1-x2)
return dis
def check_path(self, xy1, xy2):
"""碰撞检测 两点之间的连线是否经过障碍物"""
steps = max(abs(xy1[0]-xy2[0]), abs(xy1[1]-xy2[1])) # 取横向、纵向较大值，确保经过的每个像素都被检测到
xs = np.linspace(xy1[0],xy2[0],steps+1)
ys = np.linspace(xy1[1],xy2[1],steps+1)
for i in range(1, steps): # 第一个节点和最后一个节点是 xy1，xy2，无需检查
if not self.not_obstacle(math.ceil(xs[i]), math.ceil(ys[i])):
return False
return True
def plot(self,path):
out = []
for x in range(self.rows):
temp = []
for y in range(self.cols):
if self.map[x][y]==STAT_OBSTACLE:
temp.append(0)
elif self.map[x][y]==STAT_NORMAL:
temp.append(255)
elif self.map[x][y]=='*':
temp.append(127)
else:
temp.append(255)
out.append(temp)
for x,y in path:
out[x][y] = 127
out = np.array(out)
img = Image.fromarray(out)
img.show()
def path_length(path):
"""计算路径长度"""
l = 0
for i in range(len(path)-1):
x1,y1 = path[i]
x2,y2 = path[i+1]
if x1 == x2 or y1 == y2:
l+=1.0
else:
l+=1.4
return l
class PRM(RoadMap):
def __init__(self, img_file, **param):
""" 随机路线图算法(Probabilistic Roadmap, PRM)
**param: 关键字参数，用以配置规划参数
start: 起点坐标 (x,y)
end: 终点左边 (x,y)
num_sample: 采样点个数，默认100 int
distance_neighbor: 邻域距离，默认100 float
"""
RoadMap.__init__(self,img_file)
self.num_sample = param['num_sample'] if 'num_sample' in param else 100
self.distance_neighbor = param['distance_neighbor'] if 'distance_neighbor' in param else 100
self.G = nx.Graph() # 无向图，保存构型空间的完整连接属性
def learn(self):
"""PRM算法的学习阶段
学习阶段只需要运行一次
"""
# 随机采样节点
while len(self.G.node)<self.num_sample:
XY = (np.random.randint(0, self.rows),np.random.randint(0, self.cols)) # 随机取点
if self.is_valid_xy(XY[0],XY[1]) and self.not_obstacle(XY[0],XY[1]): # 不是障碍物点
self.G.add_node(XY)
# 邻域范围内进行碰撞检测，加边
for node1 in self.G.nodes:
for node2 in self.G.nodes:
if node1==node2:
continue
dis = self.EuclidenDistance(node1,node2)
if dis<self.distance_neighbor and self.check_path(node1,node2):
self.G.add_edge(node1,node2,weight=dis) # 边的权重为 欧几里得距离
def find_path(self,startXY=None,endXY=None):
""" 使用学习得到的无障碍连通图进行寻路
(为方便测试，默认起点为左上，终点为右下)
"""
# 寻路时再将起点和终点添加进图中，以便一次学习多次使用
temp_G = copy.deepcopy(self.G)
startXY = tuple(startXY) if startXY else (0,0)
endXY = tuple(endXY) if endXY else (self.rows-1, self.cols-1)
temp_G.add_node(startXY)
temp_G.add_node(endXY)
for node1 in [startXY, endXY]: # 将起点和目的地连接到图中
for node2 in temp_G.nodes:
dis = self.EuclidenDistance(node1,node2)
if dis<self.distance_neighbor and self.check_path(node1,node2):
temp_G.add_edge(node1,node2,weight=dis) # 边的权重为 欧几里得距离
# 直接调用networkx中求最短路径的方法
path = nx.shortest_path(temp_G, source=startXY, target=endXY)
return self.construct_path(path)
def construct_path(self, path):
"""find_path寻路得到的是连通图的节点，扩展为经过的所有像素点"""
out = []
for i in range(len(path)-1):
xy1,xy2=path[i],path[i+1]
steps = max(abs(xy1[0]-xy2[0]), abs(xy1[1]-xy2[1])) # 取横向、纵向较大值，确保经过的每个像素都被检测到
xs = np.linspace(xy1[0],xy2[0],steps+1)
ys = np.linspace(xy1[1],xy2[1],steps+1)
for j in range(0, steps+1):
out.append((math.ceil(xs[j]), math.ceil(ys[j])))
return out
#======= test case ==============
prm = PRM('map_4.bmp',num_sample=200,distance_neighbor=200)
prm.learn()
path = prm.find_path()
prm.plot(path)
print('Path length:',path_length(path))
测试结果
存在的问题
测试在有狭窄通道的环境（迷宫）中很难找到路径，如图：
