随机路线图算法(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))

测试结果

存在的问题

测试在有狭窄通道的环境（迷宫）中很难找到路径，如图：