通过行驶定位数据和路径宽度生成栅格地图用于导航

生成连通图或导航图的步骤通常包括以下几个步骤：

1. 数据准备：收集包含行走路线和预设宽度的定位数据。定位数据通常以GPS坐标（经度和纬度）形式存在。

2. 数据预处理：对定位数据进行预处理，包括坐标转换、数据清洗、去噪等。

3. 生成路线多边形：根据行走路线和预设宽度，生成每个路段的多边形。可以使用缓冲区（buffer）方法将路线扩展到预设宽度。

4. 合并多边形：将所有路段的多边形合并，生成一个连通的区域图。

5. 生成导航图：根据合并的多边形生成导航图，标注出可通行的路径和障碍区域。

安装所需的库：

pip install shapely geopandas matplotlib pandas geopandas pyproj numpy pillow

示例代码：

import pandas as pd
import geopandas as gpd
from shapely.geometry import LineString, Point
from shapely.ops import unary_union
from pyproj import Proj
import matplotlib.pyplot as plt
import numpy as np
from PIL import Image
import yaml

# 将经纬度转换为UTM坐标
def latlon_to_utm(lat, lon):
    zone = int((lon + 180) / 6) + 1
    is_northern = lat >= 0  # 判断是否是北半球 True北 False南
    proj_string = f"+proj=utm +zone={zone} +{'north' if is_northern else 'south'} +datum=WGS84"
    proj_utm = Proj(proj_string)
    easting, northing = proj_utm(lon, lat)
    return easting, northing

# 去除重复点
def remove_duplicates(points):
    return [point for i, point in enumerate(points) if i == 0 or point != points[i - 1]]

# 移动平均滤波
def moving_average(points, window_size=3):
    smoothed_points = []
    for i in range(len(points)):
        start = int(max(0, i - window_size // 2))
        end = int(min(len(points), i + window_size // 2 + 1))
        avg_point = np.mean(points[start:end], axis=0)
        smoothed_points.append(avg_point)
    return smoothed_points

# 卡尔曼滤波
def kalman_filter(points):
    n = len(points)
    x_est = np.zeros((n, 2))
    p_est = np.zeros((n, 2, 2))
    x_pred = np.zeros((n, 2))
    p_pred = np.zeros((n, 2, 2))
    q = np.eye(2)  # 过程噪声协方差
    r = np.eye(2)  # 测量噪声协方差
    k = np.zeros((n, 2, 2))  # 卡尔曼增益

    x_est[0] = points[0]
    p_est[0] = np.eye(2)

    for t in range(1, n):
        x_pred[t] = x_est[t - 1]
        p_pred[t] = p_est[t - 1] + q

        k[t] = p_pred[t] @ np.linalg.inv(p_pred[t] + r)
        x_est[t] = x_pred[t] + k[t] @ (points[t] - x_pred[t])
        p_est[t] = (np.eye(2) - k[t]) @ p_pred[t]

    return x_est

# 去除间隔距离小于设定值的点
def remove_close_points(points, min_distance):
    filtered_points = [points[0]]
    for point in points[1:]:
        if np.linalg.norm(np.array(point) - np.array(filtered_points[-1])) >= min_distance:
            filtered_points.append(point)
    return filtered_points

# 创建函数来处理单个文件
def process_file(file_path, min_distance):
    df = pd.read_csv(file_path)
    if 'Latitude' not in df.columns or 'Longitude' not in df.columns:
        raise ValueError("The selected file does not contain Latitude and Longitude columns.")
    
    points = list(zip(df['Latitude'], df['Longitude']))
    points = remove_duplicates(points)
    points = np.array([latlon_to_utm(lat, lon) for lat, lon in points])
    points = remove_close_points(points, min_distance)
    points = moving_average(points)
    points = kalman_filter(points)

    return LineString(points)

# 获取LineString的坐标列表
def get_coordinates(line):
    return list(line.coords)

# 读取多个CSV文件并处理它们
file_paths = ['path1.csv', 'path2.csv']  # 替换为你的CSV文件路径列表
lines = []
min_distance = 1.0  # 设定的最小距离阈值，单位为米
for file_path in file_paths:
    try:
        line = process_file(file_path, min_distance)
        lines.append(line)
    except ValueError as e:
        print(e)

# 简化所有路径
tolerance = 1.0  # 设置简化容差
lines = [line.simplify(tolerance) for line in lines]

# 设置缓冲区半径（单位：米）
buffer_radius = 10  # 例如10米

# 生成所有路径的缓冲区
buffered_lines = [line.buffer(buffer_radius) for line in lines]

# 合并所有缓冲区生成连通地图
buffered_union = unary_union(buffered_lines)

# 设置栅格地图的分辨率（单位：米）
resolution = 0.1  # 每个栅格的大小

# 计算栅格地图的尺寸
minx, miny, maxx, maxy = buffered_union.bounds
width = int((maxx - minx) / resolution)
height = int((maxy - miny) / resolution)

# 创建栅格地图
occupancy_grid = np.full((height, width), 255, dtype=np.uint8)  # 初始化为255（占用空间）

# 将多边形填充到栅格地图中
for i in range(width):
    for j in range(height):
        x = minx + i * resolution
        y = miny + j * resolution
        point = Point(x, y)
        if buffered_union.contains(point):
            occupancy_grid[height - j - 1, i] = 0  # 使用 0 表示自由空间

# 保存为PGM文件
pgm_filename = 'occupancy_grid.pgm'
image = Image.fromarray(occupancy_grid)
image.save(pgm_filename)

# 生成YAML文件
yaml_filename = 'occupancy_grid.yaml'
yaml_data = {
    'image': pgm_filename,
    'resolution': resolution,
    'origin': [minx, miny, 0.0],
    'negate': 0,
    'occupied_thresh': 0.65,
    'free_thresh': 0.196
}

with open(yaml_filename, 'w') as yaml_file:
    yaml.dump(yaml_data, yaml_file)

# 可视化路径和缓冲区
fig, ax = plt.subplots()
gdf_lines = gpd.GeoDataFrame(geometry=lines)
gdf_buffered = gpd.GeoDataFrame(geometry=[buffered_union])

# 绘制路径，颜色设为紫色
gdf_lines.plot(ax=ax, color='purple', linewidth=0.1)  # 线宽为0.1点

# 绘制缓冲区
gdf_buffered.plot(ax=ax, color='green', alpha=0.5, edgecolor='black')

# 设置标题和标签
ax.set_title('Paths and Unified Buffer Visualization')
ax.set_xlabel('UTM Easting')
ax.set_ylabel('UTM Northing')

# 显示栅格地图
plt.figure()
plt.imshow(occupancy_grid, cmap='gray', origin='lower')
plt.title('Occupancy Grid Map')
plt.xlabel('X')
plt.ylabel('Y')
plt.show()