超快速的路径优化IKD-SWOpt：SHIFT Planner 中增量 KD 树滑动窗口优化算法详解

IKD-SWOpt：SHIFT Planner 中增量 KD 树滑动窗口优化算法详解

今天本博主王婆卖瓜自卖自夸😄，介绍自己paper中的算法，本算法已经持续开源中(部分关键内容)Github，之前很多读者朋友一直说要详细讲讲路径优化算法，我这篇paper中的一个叫IKD-SWOpt的模块创新性的使用IKDtree改进的Astar给出比较好的初始路径，并通过滑动窗口检测需要优化的轨迹段进行无精度损失的轨迹优化算法，其内存开销及优化速度都打到了SOTA水平，在内存及计算资源极其有限的环境下也可以运行。github代码地址如下：SHIFTPlanner-Robotics SHIFT-Planner 可以点个小星星🌟支持下，今天就讲解下这部分轨迹优化内容。
SHIFT Planner 系统架构

图1：SHIFT Planner 系统架构

本算法使机器人能够高效且安全地在复杂环境中导航。Incremental IKD-tree Sliding Window Optimization (IKD-SWOpt) 算法，这是我们 SHIFT Planner 框架的核心部分。我们将从理论基础和实际实现两个方面，全面解析 IKD-SWOpt 在动态障碍物避让和路径优化中的作用。

在路径优化过程中，IKD-SWOpt（增量 KD 树滑动窗口优化）算法通过结合障碍物避免、路径平滑以及局部搜索来优化路径。

1. 路径优化目标

IKD-SWOpt 的核心目标是优化路径，使其更加平滑、短且避开障碍物。假设有一个由一系列路径点组成的路径，我们的优化目标可以分为以下几个部分：

路径长度最小化： 我们希望路径尽量短，以减少机器人行驶的距离。
避障性最大化： 路径应尽量避开障碍物，以确保机器人安全。
平滑性最大化： 路径应尽量平滑，避免过多的急转弯，减少机器人运动的能量消耗。

在数学上，路径优化可以通过一个包含多个目标的优化函数表示。该函数通常是一个带有约束条件的加权和。

2. 路径优化的数学公式

目标函数：

$\mathcal{L}(P) = \lambda_1 \cdot \mathcal{L}_{\text{path}}(P) + \lambda_2 \cdot \mathcal{L}_{\text{obstacle}}(P) + \lambda_3 \cdot \mathcal{L}_{\text{smoothing}}(P)$
表示路径长度：
$\mathcal{L}_{\text{path}}(P) = \sum_{i=1}^{n-1} \| p_{i+1} - p_i \|$

是路径中相邻点之间的欧几里得距离。
表示路径与障碍物的距离。对于路径中的每一个点，计算该点到最近障碍物的距离。假设障碍物集合为 $O$ ，那么：

$\mathcal{L}_{\text{obstacle}}(P) = \sum_{i=1}^n \frac{1}{\text{dist}(p_i, O)}$

其中， $p_i, O)$ 是点 $p_i$ 到障碍物集合 $O$ 中最近障碍物的距离。

$L$ 是路径平滑项，用来减少路径中的急转弯。通常使用路径上相邻点之间的角度变化量来表示：

$\mathcal{L}_{\text{smoothing}}(P) = \sum_{i=2}^{n-1} \| (p_{i+1} - p_i) - (p_i - p_{i-1}) \|$

这项控制了路径在转弯处的平滑程度，确保路径不会因为局部的细节变化而出现过于急剧的变化。

加权系数：

$\lambda_1, \lambda_2, \lambda_3$ 是加权系数，分别对应路径长度、避障和路径平滑性。通过调整这些系数，可以在优化过程中根据具体需求，决定不同目标之间的权重。

3. 优化过程

IKD-SWOpt 使用增量 KD 树来动态更新障碍物地图，并根据当前的路径局部调整进行优化。具体步骤如下：

初始化： 通过 A* 或其他路径规划算法生成初步路径。这个路径可能有不平滑的转折，或者过于接近障碍物。
增量优化： 将路径分成若干个子路径（可以是滑动窗口中的一部分路径）。对于每个子路径，使用局部优化算法（如梯度下降、牛顿法等）来优化该路径段，目标是最小化上述目标函数。
- 局部搜索: 通过增量 KD 树实现局部搜索，快速计算路径与障碍物的距离以及路径平滑性。每次优化时，计算增量更新，并根据距离函数调整路径。
- 更新路径： 在每一步优化后，更新路径并继续搜索，直到找到全局最优路径。
停止条件： 当优化结果满足设定的精度或计算次数达到上限时，停止优化。

4. IKD-SWOpt 中的增量更新

增量更新是 IKD-SWOpt 中的一大特点，主要体现在两个方面：

障碍物更新： 随着机器人在环境中移动，障碍物可能会发生变化。增量 KD 树可以动态更新障碍物的空间分布，从而实时调整路径优化。
路径调整： 在每次增量优化时，只有路径的局部段需要被优化，而不需要重新计算整个路径。这种方法极大地提高了计算效率，适合在实时系统中应用。

5. 实验验证与可视化（先看效果）

在这里插入图片描述
图2：IKD-SWOpt路径可视化距离场环境优化图

在这里插入图片描述
图3：IKD-SWOpt路径优化算法效果图

为什么叫IKD-SWOpt

自主机器人在复杂和动态的环境中操作时，路径规划至关重要，路径优化使用ESDF地图实际很花费内存，并且有精度损失（删格化），通过IKDtree动态构建的距离场可以精确的优化出极窄通道的可通行路径。
我的论文 SHIFT Planner 中是一个全新的CCP覆盖路径算法，其中创新性的提出了通过IKDtree来实现初始路径搜索（改进的Astar）还通过IKDtree构建的避障优化项使用LBFGS-B算法来轻量级优化路径，今天就讲讲Incremental IKD-tree Sliding Window Optimization (IKD-SWOpt) 算法。IKD-SWOpt 负责实时路径优化，特别是在机器人遇到意外障碍物的情况下。通过利用增量 KD 树结构，IKD-SWOpt 高效地更新障碍物地图，并在滑动窗口内优化路径，确保路径的安全性和计算效率。

环境设置与障碍物生成

在深入探讨优化算法之前，理解机器人操作的环境至关重要。环境被表示为一个二维网格，随机生成的障碍物模拟了现实世界中机器人需要避开的物体。

代码详解

import numpy as np
import heapq
import random
import matplotlib.pyplot as plt
from matplotlib import animation
from scipy.spatial import KDTree
from scipy.optimize import minimize
from scipy.interpolate import splprep, splev

# 设置随机种子以保证结果可复现（可选）
random.seed(42)
np.random.seed(42)

# 定义网格大小
grid_size = (100, 100)
grid = np.zeros(grid_size)

def generate_obstacle(grid, min_width, max_width, min_height, max_height):
    """生成随机矩形障碍物。"""
    width = random.randint(min_width, max_width)
    height = random.randint(min_height, max_height)
    x = random.randint(0, grid.shape[0] - width - 1)
    y = random.randint(0, grid.shape[1] - height - 1)
    grid[x:x+width, y:y+height] = 1  # 标记障碍物

# 生成多个随机障碍物
for _ in range(50):
    generate_obstacle(grid, min_width=5, max_width=15, min_height=5, max_height=15)

通过这种方式，创建了一个复杂的环境，包含多个障碍物，模拟了机器人在现实世界中需要导航的复杂场景。

势场计算

势场（Potential Field）表示网格中每个点到最近障碍物的距离。势场在引导机器人远离障碍物并朝目标前进中起着关键作用。

代码详解

def calculate_potential_field(grid):
    """计算势场，表示每个点到最近障碍物的距离。"""
    grid_size = grid.shape
    distance_field = np.full(grid_size, np.inf)
    obstacles = np.argwhere(grid == 1)
    free_space = np.argwhere(grid == 0)
    if len(obstacles) > 0:
        kdtree = KDTree(obstacles)
        distances, _ = kdtree.query(free_space)
        for idx, coord in enumerate(free_space):
            distance_field[tuple(coord)] = distances[idx]
    else:
        distance_field[free_space[:,0], free_space[:,1]] = np.inf
    return distance_field

distance_field = calculate_potential_field(grid)

解释：

初始化： 初始化一个与网格大小相同的距离场，所有值设为无穷大，表示初始时每个点到障碍物的距离未知。
识别障碍物与自由空间： 使用 np.argwhere 函数分别找出障碍物和自由空间的坐标。
KD 树构建与查询： 如果存在障碍物，构建 KD 树以加速最近邻查询。对自由空间中的每个点，查询其到最近障碍物的距离，并更新距离场。这里显示的距离场主要用来显示，实际算法中不需要将其转化成可视化的距离场地图，直接通过树访问即可。
返回距离场： 最终得到每个自由空间点到最近障碍物的距离场。

通过这种方式，势场为后续的路径规划提供了关键信息，帮助机器人避开障碍物并高效地接近目标。

结合IKDtree势场的 A* 搜索算法

A* 搜索算法是一种广泛使用的路径规划算法，能够在保证最优性的同时具备较高的效率。在本框架中，A* 算法结合了势场信息，以增强其在复杂环境中的导航能力。
IKDtree可视化距离场
图4：IKDtree距离场地图
Astar改进算法
图5：IKDtree改进的Astar算法结果图

代码详解

def heuristic(a, b):
    """欧几里得距离启发函数。"""
    return np.linalg.norm(np.array(a) - np.array(b))

def a_star_search_with_potential_field(start, goal, grid, distance_field, influence_strength=5.0):
    """结合势场的 A* 搜索算法。"""
    neighbors = [(-1, 0), (1, 0), (0, -1), (0, 1),
                (-1, -1), (-1, 1), (1, -1), (1, 1)]  # 八邻域
    close_set = set()
    came_from = {}
    gscore = {start: 0}
    fscore = {start: heuristic(start, goal)}
    oheap = []
    heapq.heappush(oheap, (fscore[start], start))

    while oheap:
        current = heapq.heappop(oheap)[1]

        if current == goal:
            # 重建路径
            data = []
            while current in came_from:
                data.append(current)
                current = came_from[current]
            data.append(start)
            data.reverse()
            return data

        close_set.add(current)
        for i, j in neighbors:
            neighbor = current[0] + i, current[1] + j

            if 0 <= neighbor[0] < grid.shape[0]:
                if 0 <= neighbor[1] < grid.shape[1]:
                    if grid[neighbor[0]][neighbor[1]] == 1:
                        continue
                else:
                    continue
            else:
                continue

            tentative_gscore = gscore[current] + heuristic(current, neighbor)
            if neighbor in close_set and tentative_gscore >= gscore.get(neighbor, 0):
                continue

            if tentative_gscore < gscore.get(neighbor, float('inf')) or neighbor not in [i[1] for i in oheap]:
                came_from[neighbor] = current
                gscore[neighbor] = tentative_gscore
                potential = influence_strength / (distance_field[neighbor[0]][neighbor[1]] + 1e-5)
                fscore[neighbor] = tentative_gscore + heuristic(neighbor, goal) + potential
                heapq.heappush(oheap, (fscore[neighbor], neighbor))

    return False

解释：

启发函数： 使用欧几里得距离作为启发函数，衡量当前节点到目标节点的直线距离。
邻域定义： 采用八邻域，允许机器人在水平、垂直和对角方向移动。
A 算法流程：*
- 开放列表与关闭列表： 使用堆（优先队列）作为开放列表，存储待处理节点；使用集合作为关闭列表，存储已处理节点。
- 节点展开： 从开放列表中取出具有最低 f 值的节点作为当前节点。
- 路径重建： 当当前节点为目标节点时，重建并返回路径。
- 邻居处理： 遍历当前节点的所有邻居节点，计算临时 g 值（从起点到当前节点的实际代价加上从当前节点到邻居节点的代价）。
- f 值计算： f 值结合了 g 值、启发式 h 值和势场的潜在影响。势场通过 influence_strength 参数调节，作用是增加靠近障碍物的节点的 f 值，促使算法选择远离障碍物的路径。
- 节点更新与入堆： 若邻居节点的 g 值被优化，则更新其 came_from、gscore 和 fscore，并将其加入开放列表。

通过将势场信息融入 A* 算法的 f 值计算，算法在路径规划时能够更有效地避开障碍物，选择更加安全且高效的路径。

路径优化：IKD-SWOpt

在路径规划的基础上，进一步优化路径以提高其平滑性和安全性是必要的。IKD-SWOpt 算法通过增量 KD 树和滑动窗口优化技术，实时调整路径，确保路径在动态环境中的适应性和效率。

识别优化段

首先，需要识别路径中靠近障碍物的段落，这些段落需要进行优化以避免碰撞并提升路径质量。

代码详解

def min_distance_to_obstacle(point, grid):
    """计算点到最近障碍物的距离。"""
    obstacles = np.argwhere(grid == 1)
    if len(obstacles) == 0:
        return np.inf
    kdtree = KDTree(obstacles)
    dist, _ = kdtree.query(point)
    return dist

def identify_segments_close_to_obstacles(path, grid, threshold):
    """识别并合并靠近障碍物的连续路径段。"""
    indices_to_optimize = []
    for i, point in enumerate(path):
        dist = min_distance_to_obstacle(point, grid)
        if dist < threshold:
            indices_to_optimize.append(i)

    # 将相邻的索引合并为连续段
    segments = []
    if not indices_to_optimize:
        return segments

    start_idx = indices_to_optimize[0]
    end_idx = indices_to_optimize[0]
    for idx in indices_to_optimize[1:]:
        if idx == end_idx + 1:
            end_idx = idx
        else:
            # 为了平滑，扩展优化段前后各 5 个点
            segment_start = max(start_idx - 5, 0)
            segment_end = min(end_idx + 5, len(path) - 1)
            segments.append((segment_start, path[segment_start:segment_end + 1]))
            start_idx = idx
            end_idx = idx
    # 添加最后一个段
    segment_start = max(start_idx - 5, 0)
    segment_end = min(end_idx + 5, len(path) - 1)
    segments.append((segment_start, path[segment_start:segment_end + 1]))
    return segments

解释：

最小距离计算： min_distance_to_obstacle 函数计算路径中每个点到最近障碍物的距离。若距离小于设定阈值，则认为该点需要优化。
识别优化段： identify_segments_close_to_obstacles 函数遍历路径，收集需要优化的点的索引。然后，将相邻的需要优化的点合并为连续的路径段，前后各扩展 5 个点以确保优化段的平滑性。

通过这种方法，确保了优化算法只对靠近障碍物的关键路径段进行处理，减少了计算负担，同时提升了路径的安全性。

IKD-SWOpt 算法

IKD-SWOpt 算法利用增量 KD 树和滑动窗口技术，对识别出的非合规路径段进行实时优化。

代码详解

def compute_total_cost(variables, num_points, start, goal, kdtree, obs_weight=1.0, smooth_weight=0.1, length_weight=0.1):
    """计算优化的总成本函数。"""
    path_points = [np.array(start)]
    for i in range(num_points - 2):
        x = variables[2 * i]
        y = variables[2 * i + 1]
        path_points.append(np.array([x, y]))
    path_points.append(np.array(goal))

    total_cost = 0.0

    # 平滑性成本
    for i in range(1, num_points - 1):
        xi_prev = path_points[i - 1]
        xi = path_points[i]
        xi_next = path_points[i + 1]
        smoothness = np.linalg.norm(xi_prev - 2 * xi + xi_next) ** 2
        total_cost += smooth_weight * smoothness

    # 障碍物避让成本
    for i in range(1, num_points - 1):
        xi = path_points[i]
        dist, _ = kdtree.query(xi)
        obs_cost = obs_weight / (dist + 1e-5)
        total_cost += obs_cost

    # 路径长度成本
    for i in range(num_points - 1):
        xi = path_points[i]
        xi_next = path_points[i + 1]
        length = np.linalg.norm(xi_next - xi)
        total_cost += length_weight * length

    return total_cost

def optimize_path_segment_with_scipy(path_segment, grid, obs_weight=1.0, smooth_weight=0.1, length_weight=0.1, max_iterations=100):
    """使用 SciPy 对路径段进行优化。"""
    num_points = len(path_segment)
    if num_points < 3:
        return path_segment, []  # 无需优化

    # 构建 KD 树
    obstacles = np.argwhere(grid == 1)
    kdtree = KDTree(obstacles)

    initial_variables = []
    for i in range(1, num_points - 1):
        initial_variables.extend(path_segment[i])

    bounds = []
    for _ in range(num_points - 2):
        bounds.append((0, grid.shape[0] - 1))  # x 范围
        bounds.append((0, grid.shape[1] - 1))  # y 范围

    # 记录优化历史以便动画展示
    path_history = []

    def callback(variables):
        path_points = [path_segment[0]]
        for i in range(num_points - 2):
            x = variables[2 * i]
            y = variables[2 * i + 1]
            path_points.append((x, y))
        path_points.append(path_segment[-1])
        path_history.append(path_points.copy())

    result = minimize(
        compute_total_cost,
        initial_variables,
        args=(num_points, path_segment[0], path_segment[-1], kdtree, obs_weight, smooth_weight, length_weight),
        method='L-BFGS-B',
        bounds=bounds,
        options={'maxiter': max_iterations},
        callback=callback
    )

    optimized_segment = [path_segment[0]]
    optimized_variables = result.x
    for i in range(num_points - 2):
        x = optimized_variables[2 * i]
        y = optimized_variables[2 * i + 1]
        optimized_segment.append((x, y))
    optimized_segment.append(path_segment[-1])

    return optimized_segment, path_history

解释：

成本函数计算： compute_total_cost 函数计算路径段的总成本，包括平滑性成本、障碍物避让成本和路径长度成本。
- 平滑性成本： 通过计算相邻三点的二阶差分，衡量路径的平滑程度。
- 障碍物避让成本： 利用 KD 树快速查询每个点到最近障碍物的距离，并计算避让成本。
- 路径长度成本： 计算路径的总长度，鼓励路径尽可能短。
路径段优化： optimize_path_segment_with_scipy 函数使用 SciPy 的 minimize 函数对路径段进行优化。
- 初始变量： 提取需要优化的路径段中间点的坐标作为优化变量。
- 边界设置： 设置每个优化变量的边界，确保优化后的路径点仍在网格范围内。
- 回调函数： callback 函数记录每次迭代后的路径点，用于后续的动画展示。
- 优化过程： 使用 L-BFGS-B 算法进行优化，逐步调整路径点以最小化总成本。

通过这种方法，IKD-SWOpt 能够高效地优化靠近障碍物的路径段，确保路径的安全性和平滑性，同时保持计算效率。

B-样条路径平滑

优化后的路径虽然避开了障碍物，但可能仍存在一定的折线性。为了使路径更加平滑和适合实际机器人运动，使用 B-样条曲线对路径进行平滑处理。

代码详解

def smooth_path_with_spline(path, smoothing_factor=0):
    """使用 B-样条平滑路径。"""
    path = np.array(path)
    x = path[:, 0]
    y = path[:, 1]

    # 去除重复点
    _, idx = np.unique(path, axis=0, return_index=True)
    path = path[np.sort(idx)]
    x = path[:, 0]
    y = path[:, 1]

    # 需要至少 4 个点来拟合 B-样条
    if len(x) < 4:
        return path

    # 生成 B-样条参数和控制点
    tck, u = splprep([x, y], s=smoothing_factor)

    # 生成更高采样率的平滑曲线
    unew = np.linspace(0, 1, 500)
    smooth_x, smooth_y = splev(unew, tck)

    return list(zip(smooth_x, smooth_y))

解释：

路径转换： 将路径转换为 NumPy 数组，并提取 x 和 y 坐标。
去重： 去除路径中重复的点，防止 B-样条拟合时出现问题。
B-样条拟合： 使用 SciPy 的 splprep 函数生成 B-样条参数和控制点。smoothing_factor 参数控制平滑程度，值越大，曲线越平滑。
曲线生成： 使用 splev 函数在参数空间内生成更高采样率的平滑曲线点。
返回平滑路径： 返回平滑后的路径点列表。

通过 B-样条曲线的拟合，路径变得更加平滑，适合机器人实际运动，提高了运动的连续性和舒适性。

集成与最终路径规划

优化和平滑后的路径需要与原始路径进行整合，形成最终的导航路径。

代码详解

def integrate_path_segments(path, segments_with_indices):
    """将优化后的路径段重新整合回整体路径。"""
    path = list(path)  # 确保路径可变
    # 按起始索引逆序排序，以处理重叠段
    segments_with_indices.sort(key=lambda x: x[0], reverse=True)
    for start_idx, optimized_segment, _ in segments_with_indices:
        end_idx = start_idx + len(optimized_segment)
        path[start_idx:end_idx] = optimized_segment
    return path

解释：

路径转换： 确保路径是可变的列表类型。
段排序： 按照起始索引进行逆序排序，以处理可能的路径段重叠。
路径替换： 将优化后的路径段替换回原始路径中相应的位置，形成整体优化后的路径。

通过这种方式，优化后的路径段被有效地整合回整体路径，确保路径的连贯性和优化效果。

可视化

为了更直观地展示路径优化的过程，使用 Matplotlib 的动画功能生成优化过程的动画。可以查看 https://github.com/fanzexuan/SHIFTPlanner-Robotics ，有更解释代码及paper。

代码详解

def animate_optimization(path_history, grid, start, goal, segment_idx):
    """创建路径优化过程的动画。"""
    fig, ax = plt.subplots(figsize=(8, 8))
    # 绘制障碍物
    obstacles = np.argwhere(grid == 1)
    if len(obstacles) > 0:
        ax.scatter(obstacles[:, 0], obstacles[:, 1], c='lightgray', s=10)

    # 绘制起点和终点
    ax.scatter(start[0], start[1], marker='o', color='green', s=100, label='Start')
    ax.scatter(goal[0], goal[1], marker='*', color='red', s=100, label='Goal')

    line, = ax.plot([], [], 'b-', linewidth=2, label='Optimizing Segment')

    def init():
        line.set_data([], [])
        return line,

    def animate(i):
        path = path_history[i]
        x, y = zip(*path)
        line.set_data(x, y)
        ax.set_title(f'段 {segment_idx} 优化步骤: {i+1}/{len(path_history)}')
        return line,

    ani = animation.FuncAnimation(fig, animate, frames=len(path_history), init_func=init,
                                  blit=True, interval=200, repeat=False)
    plt.legend()
    plt.show()

解释：

图形初始化： 创建一个新的图形和轴，设置图形大小。
绘制障碍物、起点和终点： 使用散点图绘制障碍物，并用不同的标记和颜色标记起点和终点。
动画元素初始化： 初始化一条用于展示优化路径的线条。
初始化函数： init 函数清空线条数据，准备动画开始。
动画帧函数： animate 函数在每一帧中更新路径点，并在图表标题中显示当前优化步骤。
生成动画： 使用 FuncAnimation 生成动画，逐帧展示优化过程。
显示图例与图形： 显示图例并展示生成的动画。

通过动画，可以直观地观察到路径优化的逐步过程，理解 IKD-SWOpt 如何逐步调整路径以避开障碍物并提升路径质量。

实验验证

为验证 IKD-SWOpt 算法的有效性，进行了多组实验，展示了该算法在不同环境中的表现。

实验设置

真实环境： 使用真实机器人在实际场景中进行测试，验证路径规划和优化的效果。
仿真环境： 在仿真环境中生成不同复杂度的网格，测试算法的鲁棒性和效率。
性能指标： 主要考察路径的平滑性、避障能力、优化时间和计算资源消耗。

结果分析

通过对比优化前后的路径，可以明显看到 IKD-SWOpt 对路径的改善效果。优化后的路径更加平滑，避障能力更强，计算效率也得到了显著提升。

代码详解

def main():
    # 随机选择起点和终点
    free_space = np.argwhere(grid == 0)
    start = tuple(free_space[np.random.choice(len(free_space))])
    goal = tuple(free_space[np.random.choice(len(free_space))])

    # 运行 A* 搜索
    path = a_star_search_with_potential_field(start, goal, grid, distance_field, influence_strength=5.0)

    if not path:
        print("未找到路径")
        return

    # 识别需要优化的路径段
    distance_threshold = 3  # 可根据需要调整
    segments_to_optimize = identify_segments_close_to_obstacles(path, grid, distance_threshold)

    # 优化每个路径段并收集优化后的段落
    segments_with_indices = []
    for idx, (start_idx, segment) in enumerate(segments_to_optimize):
        optimized_segment, path_history = optimize_path_segment_with_scipy(
            segment, grid, obs_weight=0.5, smooth_weight=1.5, length_weight=1.0, max_iterations=20)
        segments_with_indices.append((start_idx, optimized_segment, path_history))
        # 为每个优化段显示动画
        animate_optimization(path_history, grid, start, goal, idx+1)

    # 将优化后的段落整合回整体路径
    optimized_path = integrate_path_segments(path, segments_with_indices)

    # 使用 B-样条平滑整体路径
    smoothed_path = smooth_path_with_spline(optimized_path, smoothing_factor=1.0)

    # 绘制最终结果
    fig, ax = plt.subplots(figsize=(10, 10))
    obstacles = np.argwhere(grid == 1)
    if len(obstacles) > 0:
        ax.scatter(obstacles[:, 0], obstacles[:, 1], c='lightgray', s=10)

    # 绘制起点和终点
    ax.scatter(start[0], start[1], marker='o', color='green', s=100, label='Start')
    ax.scatter(goal[0], goal[1], marker='*', color='red', s=100, label='Goal')

    # 绘制初始路径
    x_initial, y_initial = zip(*path)
    ax.plot(x_initial, y_initial, 'r--', linewidth=1, label='初始路径')

    # 绘制优化后的路径段，使用不同颜色区分
    colors = ['blue', 'cyan', 'magenta', 'yellow', 'orange']
    for idx, (start_idx, optimized_segment, _) in enumerate(segments_with_indices):
        x_opt, y_opt = zip(*optimized_segment)
        color = colors[idx % len(colors)]
        ax.plot(x_opt, y_opt, color=color, linewidth=2, label=f'优化段 {idx+1}')

    # 绘制平滑后的路径
    x_smooth, y_smooth = zip(*smoothed_path)
    ax.plot(x_smooth, y_smooth, 'g-', linewidth=2, label='平滑路径')

    # 可选：绘制切线圆圈以示安全距离
    for point in path:
        dist = min_distance_to_obstacle(point, grid)
        if dist < distance_threshold:
            circle = plt.Circle((point[0], point[1]), dist, color='orange', fill=False, linestyle='--', linewidth=1)
            ax.add_patch(circle)

    ax.set_xlim(0, grid.shape[0])
    ax.set_ylim(0, grid.shape[1])
    ax.set_aspect('equal')
    ax.legend()
    ax.set_title('带有合并段的路径优化')
    plt.show()

if __name__ == "__main__":
    main()