隨機路線圖算法(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))
測試結果
存在的問題
測試在有狹窄通道的環境（迷宮）中很難找到路徑，如圖：
