一、场景

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

二、方案

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 heapq

import skimage
from scipy import ndimage
import geopandas as gpd
from shapely.geometry import LineString

# 室内路径规划
class IndoorPathPlanning:
    def __init__(self):
        self.map_image = None
        self.original_map = None
        self.grid_map = None
        self.vector_network = None
        self.key_points = {}
        self.graph = None
        self.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_map

    def 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，可通行区域为0
        inverted_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.shape
        grid_height = height // cell_size
        grid_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:
                        continue
                    ny, nx = y + dy, x + dx
                    if 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']
            })

        # 创建GeoDataFrame
        if lines:
            gdf = gpd.GeoDataFrame(lines, geometry='geometry', crs="EPSG:4326")
            # 保存为shapefile
            gdf.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 = skeleton
        return skeleton

    def 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_points

    def 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:
                        continue

                    neighbor = (current[0] + dy, current[1] + dx)

                    # 检查是否在栅格地图范围内
                    if (0 <= neighbor[0] < self.grid_map.shape[0] and
                            0 <= 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')) + dist

                        if tentative_g_score < g_score.get(neighbor, float('inf')):
                            # 找到了更好的路径
                            came_from[neighbor] = current
                            g_score[neighbor] = tentative_g_score
                            f_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 None

    def 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()

五、下一步计划

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