基于python实现一个二维图片的路径规划问题

一、场景

基于如下的一个楼层平面图，假设有几个预置的点（实际项目中可能是动态的点，比如找车位，找工位），做路径规划，并画在平面图上

二、方案

1.准备平面室内图

可以自己用QGIS/cad等其他方式自己画，或者看是否有现成的

2.将室内图转换为可计算的栅格数据或矢量数据

这里就是借助于OpenCV+python

• 栅格地图：

就是用个黑白像素表示可以行走的区域（白）与障碍物（黑）

适合简单场景

• 矢量路网：

用节点和变构成图结构，适合复杂场景

矢量路网：适合路径规划，GIS分析，存储道路的几何信息

二值图像：适合图像处理，只有黑白两种像素值

3.实现寻路算法

有现成的路径规划库，比如A*算法，networkX算法等

栅格地图+A*算法

矢量地图+NetworkX算法

三、实现步骤

1.扫描地图，处理后得到二值图像

2.栅格地图处理

3.矢量路网生成

4.定义关键路径点

5.使用A*算法进行路径规划

四、代码

import cv2
import networkx
import numpy as np
import matplotlib.pyplot as plt
import heapqimport skimage
from scipy import ndimage
import geopandas as gpd
from shapely.geometry import LineString# 室内路径规划
class IndoorPathPlanning:def __init__(self):self.map_image = Noneself.original_map = Noneself.grid_map = Noneself.vector_network = Noneself.key_points = {}self.graph = Noneself.cell_size = 1  # 默认单元格大小def scan_real_map(self, image_path):"""扫描真实地图并加载为图像参数:image_path: 地图图像的路径返回:处理后的二值图像"""print("步骤1: 扫描真实地图")# 读取图像self.map_image = cv2.imread(image_path)if self.map_image is None:raise ValueError(f"无法读取图像: {image_path}")# 保存原始图像的副本self.original_map = self.map_image.copy()# 转换为灰度图gray_map = cv2.cvtColor(self.map_image, cv2.COLOR_BGR2GRAY)# 应用阈值分割得到二值图像_, binary_map = cv2.threshold(gray_map, 127, 255, cv2.THRESH_BINARY)# 显示结果 这里如果不指定是默认字体，没有就报错，用于画图的字体指定plt.rcParams['font.sans-serif'] = ['KaiTi', 'SimHei', 'FangSong']  # 汉字字体,优先使用楷体，如果找不到楷体，则使用黑体plt.rcParams['font.size'] = 12  # 字体大小plt.rcParams['axes.unicode_minus'] = False  # 正常显示负号plt.figure(figsize=(10, 8))plt.imshow(cv2.cvtColor(self.map_image, cv2.COLOR_BGR2RGB))plt.title('原始地图')plt.savefig('original_map.png')return binary_mapdef process_grid_map(self, binary_map, cell_size=10, obstacle_dilation=5):"""处理二值图像为栅格地图参数:binary_map: 二值图像cell_size: 栅格单元大小obstacle_dilation: 障碍物膨胀像素数返回:处理后的栅格地图 (0表示可通行，1表示障碍物)"""print("步骤2: 栅格地图处理")# 保存单元格大小，用于后续坐标转换self.cell_size = cell_size# 反转二值图，使障碍物为1，可通行区域为0inverted_map = cv2.bitwise_not(binary_map) // 255# 对障碍物进行膨胀处理，确保机器人不会撞到障碍物kernel = np.ones((obstacle_dilation, obstacle_dilation), np.uint8)dilated_map = cv2.dilate(inverted_map, kernel, iterations=1)# 根据单元格大小调整栅格地图的分辨率height, width = dilated_map.shapegrid_height = height // cell_sizegrid_width = width // cell_size# 创建栅格地图self.grid_map = np.zeros((grid_height, grid_width), dtype=np.uint8)# 将原始图像映射到栅格地图 就是0可通行 1不可同通行for i in range(grid_height):for j in range(grid_width):cell = dilated_map[i * cell_size:(i + 1) * cell_size, j * cell_size:(j + 1) * cell_size]# 如果单元格中有任何障碍物，将其标记为不可通行if np.any(cell):self.grid_map[i, j] = 1# 显示栅格地图plt.figure(figsize=(10, 8))plt.imshow(self.grid_map, cmap='binary')plt.title('栅格地图')plt.savefig('grid_map.png')return self.grid_map# 添加一个新方法用于栅格坐标转换为原图坐标def grid_to_original_coords(self, grid_point):"""将栅格地图上的坐标转换为原始图像上的坐标参数:grid_point: 栅格地图上的坐标 (行, 列)返回:原始图像上的坐标 (y, x)"""return (grid_point[0] * self.cell_size + self.cell_size // 2,grid_point[1] * self.cell_size + self.cell_size // 2)def generate_vector_network(self, output_shapefile="vector_network.shp"):"""生成矢量路网参数:output_shapefile: 输出的shapefile文件路径返回:矢量路网图"""print("步骤3: 矢量路网生成")# 使用距离变换找到空闲空间的中心线dist_transform = ndimage.distance_transform_edt(1 - self.grid_map)# 使用骨架化算法提取中心线skeleton = skimage.morphology.skeletonize(1 - self.grid_map)# 将骨架转换为图像skeleton_img = skeleton.astype(np.uint8) * 255# 找到骨架上的所有点points = np.column_stack(np.where(skeleton_img > 0))# 创建一个图结构来表示路网self.graph = networkx.Graph()# 将所有骨架点添加到图中for point in points:self.graph.add_node((point[0], point[1]))# 将相邻的骨架点连接起来for point in points:y, x = point# 检查8个相邻方向for dy in [-1, 0, 1]:for dx in [-1, 0, 1]:if dy == 0 and dx == 0:continueny, nx = y + dy, x + dxif 0 <= ny < skeleton.shape[0] and 0 <= nx < skeleton.shape[1] and skeleton[ny, nx]:# 计算欧几里得距离作为边的权重weight = np.sqrt(dy ** 2 + dx ** 2)self.graph.add_edge((y, x), (ny, nx), weight=weight)# 将路网保存为shapefile格式# 创建线要素列表lines = []for u, v, data in self.graph.edges(data=True):# 创建一个从点u到点v的线要素line = LineString([(u[1], u[0]), (v[1], v[0])])lines.append({'geometry': line,'weight': data['weight']})# 创建GeoDataFrameif lines:gdf = gpd.GeoDataFrame(lines, geometry='geometry', crs="EPSG:4326")# 保存为shapefilegdf.to_file(output_shapefile)# 显示结果 这里如果不指定是默认字体，没有就报错，用于画图的字体指定plt.rcParams['font.sans-serif'] = ['KaiTi', 'SimHei', 'FangSong']  # 汉字字体,优先使用楷体，如果找不到楷体，则使用黑体plt.rcParams['font.size'] = 12  # 字体大小plt.rcParams['axes.unicode_minus'] = False  # 正常显示负号# 可视化路网plt.figure(figsize=(10, 8))plt.imshow(skeleton, cmap='gray')plt.title('矢量路网')plt.savefig('vector_network.png')self.vector_network = skeletonreturn skeletondef define_key_points(self, points_dict):"""定义路径关键点参数:points_dict: 字典，键为点的名称，值为(行, 列)坐标返回:关键点字典"""print("步骤4: 定义路径关键点")self.key_points = points_dict# 可视化关键点plt.figure(figsize=(10, 8))plt.imshow(self.grid_map, cmap='binary')# 在图上标记关键点for name, point in self.key_points.items():plt.plot(point[1], point[0], 'ro', markersize=8)plt.text(point[1] + 2, point[0] + 2, name, color='red', fontsize=12)plt.title('路径关键点')plt.savefig('key_points.png')# 可视化关键点在原始地图上original_img = self.original_map.copy()# 在原图上标记关键点for name, grid_point in self.key_points.items():# 转换栅格坐标到原图坐标orig_point = self.grid_to_original_coords(grid_point)# 绘制点cv2.circle(original_img, (orig_point[1], orig_point[0]), 5, (0, 0, 255), -1)# 添加文本cv2.putText(original_img, name, (orig_point[1] + 10, orig_point[0] + 10),cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 2)# 显示和保存plt.figure(figsize=(10, 8))plt.imshow(cv2.cvtColor(original_img, cv2.COLOR_BGR2RGB))plt.title('原始地图上的路径关键点')plt.savefig('key_points_original.png')return self.key_pointsdef find_path_a_star(self, start_point_name, end_point_name):"""使用A*算法在栅格地图上寻找最短路径参数:start_point_name: 起点名称end_point_name: 终点名称返回:最短路径的坐标列表"""print(f"步骤5: 使用A*算法寻找从{start_point_name}到{end_point_name}的路径")if start_point_name not in self.key_points or end_point_name not in self.key_points:raise ValueError(f"起点或终点不在关键点列表中")start = self.key_points[start_point_name]goal = self.key_points[end_point_name]# 定义启发函数（曼哈顿距离）def heuristic(a, b):return abs(a[0] - b[0]) + abs(a[1] - b[1])# A*算法实现open_set = []heapq.heappush(open_set, (0, start))came_from = {}g_score = {start: 0}f_score = {start: heuristic(start, goal)}open_set_hash = {start}while open_set:current = heapq.heappop(open_set)[1]open_set_hash.remove(current)if current == goal:# 重建路径path = []while current in came_from:path.append(current)current = came_from[current]path.append(start)path.reverse()# 可视化路径plt.figure(figsize=(10, 8))plt.imshow(self.grid_map, cmap='binary')# 绘制路径path_y = [p[0] for p in path]path_x = [p[1] for p in path]plt.plot(path_x, path_y, 'b-', linewidth=2)# 标记起点和终点plt.plot(start[1], start[0], 'go', markersize=10)plt.plot(goal[1], goal[0], 'ro', markersize=10)plt.title(f'从{start_point_name}到{end_point_name}的A*路径规划')plt.savefig('astar_path.png')# 2. 可视化路径在原始地图上original_img = self.original_map.copy()# 将栅格路径转换为原始图像坐标original_path = [self.grid_to_original_coords(p) for p in path]# 在原图上绘制路径for i in range(len(original_path) - 1):pt1 = (original_path[i][1], original_path[i][0])pt2 = (original_path[i + 1][1], original_path[i + 1][0])cv2.line(original_img, pt1, pt2, (0, 255, 0), 2)# 标记起点和终点start_orig = self.grid_to_original_coords(start)goal_orig = self.grid_to_original_coords(goal)cv2.circle(original_img, (start_orig[1], start_orig[0]), 8, (0, 255, 0), -1)cv2.circle(original_img, (goal_orig[1], goal_orig[0]), 8, (0, 0, 255), -1)# 添加起点终点文本cv2.putText(original_img, start_point_name, (start_orig[1] + 10, start_orig[0] + 10),cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)cv2.putText(original_img, end_point_name, (goal_orig[1] + 10, goal_orig[0] + 10),cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2)# 显示和保存plt.figure(figsize=(12, 10))plt.imshow(cv2.cvtColor(original_img, cv2.COLOR_BGR2RGB))plt.title(f'原始地图上从{start_point_name}到{end_point_name}的A*路径')plt.savefig('astar_path_original.png')return path# 检查8个相邻方向的邻居for dy in [-1, 0, 1]:for dx in [-1, 0, 1]:if dy == 0 and dx == 0:continueneighbor = (current[0] + dy, current[1] + dx)# 检查是否在栅格地图范围内if (0 <= neighbor[0] < self.grid_map.shape[0] and0 <= neighbor[1] < self.grid_map.shape[1]):# 检查是否是障碍物if self.grid_map[neighbor[0], neighbor[1]] == 1:continue# 计算距离（对角线距离为1.414，直线距离为1）dist = np.sqrt(dy ** 2 + dx ** 2)tentative_g_score = g_score.get(current, float('inf')) + distif tentative_g_score < g_score.get(neighbor, float('inf')):# 找到了更好的路径came_from[neighbor] = currentg_score[neighbor] = tentative_g_scoref_score[neighbor] = tentative_g_score + heuristic(neighbor, goal)if neighbor not in open_set_hash:heapq.heappush(open_set, (f_score[neighbor], neighbor))open_set_hash.add(neighbor)# 如果没有找到路径print(f"无法找到从{start_point_name}到{end_point_name}的路径")return Nonedef visualize_complete_system(self, path=None):"""可视化整个系统，包括栅格地图、矢量路网和路径参数:path: 可选，要显示的路径"""plt.figure(figsize=(12, 10))# 显示栅格地图plt.imshow(self.grid_map, cmap='binary', alpha=0.5)# 显示矢量路网if self.vector_network is not None:y, x = np.where(self.vector_network > 0)plt.scatter(x, y, color='blue', s=1, alpha=0.5)# 显示关键点for name, point in self.key_points.items():plt.plot(point[1], point[0], 'ro', markersize=8)plt.text(point[1] + 2, point[0] + 2, name, color='red', fontsize=12)# 显示路径if path:path_y = [p[0] for p in path]path_x = [p[1] for p in path]plt.plot(path_x, path_y, 'g-', linewidth=3)plt.title('栅格地图上的完整系统')plt.savefig('complete_system_grid.png')# 2. 在原始地图上可视化if self.original_map is not None:original_img = self.original_map.copy()# 显示关键点for name, grid_point in self.key_points.items():orig_point = self.grid_to_original_coords(grid_point)cv2.circle(original_img, (orig_point[1], orig_point[0]), 5, (0, 0, 255), -1)cv2.putText(original_img, name, (orig_point[1] + 10, orig_point[0] + 10),cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 2)# 显示路径if path:original_path = [self.grid_to_original_coords(p) for p in path]for i in range(len(original_path) - 1):pt1 = (original_path[i][1], original_path[i][0])pt2 = (original_path[i + 1][1], original_path[i + 1][0])cv2.line(original_img, pt1, pt2, (0, 255, 0), 2)# 显示结果 这里如果不指定是默认字体，没有就报错，用于画图的字体指定plt.rcParams['font.sans-serif'] = ['KaiTi', 'SimHei', 'FangSong']  # 汉字字体,优先使用楷体，如果找不到楷体，则使用黑体plt.rcParams['font.size'] = 12  # 字体大小plt.rcParams['axes.unicode_minus'] = False  # 正常显示负号# 显示和保存plt.figure(figsize=(12, 10))plt.imshow(cv2.cvtColor(original_img, cv2.COLOR_BGR2RGB))plt.title('原始地图上的完整系统')plt.savefig('complete_system_original.png')plt.show()# 示例使用
def main():# 创建路径规划系统planner = IndoorPathPlanning()# 1. 扫描真实地图binary_map = planner.scan_real_map('1.jpg')# 2. 栅格地图处理grid_map = planner.process_grid_map(binary_map, cell_size=5)# 3. 矢量路网生成skeleton = planner.generate_vector_network()# 4. 定义路径关键点key_points = {'A': (20, 20),'C': (50, 80),'D': (100, 100),'E': (110, 50),'F': (70, 80),'B': (110, 20)}planner.define_key_points(key_points)# 5. 使用A*算法进行路径规划path = planner.find_path_a_star('A', 'B')# 可视化整个系统planner.visualize_complete_system(path)if __name__ == "__main__":main()

五、下一步计划

下一步计划研究如果不是平面二维图，而是三维楼层文件，该如何处理路径的规划问题