最完整FMT*算法解析：从原理到2D路径规划实现

最完整FMT*算法解析：从原理到2D路径规划实现

【免费下载链接】PathPlanning Common used path planning algorithms with animations. 项目地址: https://gitcode.com/gh_mirrors/pa/PathPlanning

你还在为路径规划算法效率低下而困扰吗？一文掌握Fast Marching Trees核心原理

在移动机器人（Mobile Robot）和自动驾驶（Autonomous Driving）领域，路径规划（Path Planning）是实现自主导航的核心技术之一。采样-based方法如RRT虽然能在高维空间中找到可行路径，但往往存在收敛速度慢、路径质量不稳定等问题。Fast Marching Trees（FMT） 作为一种结合快速行进法（Fast Marching Method）与概率路标图（Probabilistic Roadmap）优势的优化算法，在保持概率完备性的同时，实现了更快的收敛速度和更优的路径质量。

读完本文你将获得：

• FMT*算法的核心原理与数学基础
• 与RRT*/A*等经典算法的全面对比分析
• 完整的2D路径规划实现代码与注释
• 算法参数调优指南与性能评估方法
• 工程应用中的避障策略与优化技巧

算法原理与数学基础

FMT*算法核心思想

FMT*（Fast Marching Trees）算法由J.J. Kuffner于2009年提出，其核心思想是将计算几何中的快速行进法与采样-based路径规划相结合，通过维护"前沿集"（Front Set）逐步扩展已探索区域，实现高效的路径搜索。

关键数学公式

1. 邻域半径计算：

   r_n = r_{search} \times \sqrt{\frac{\log{n}}{n}}
   

   其中$n$为采样点数量，$r_{search}$为基础搜索半径。

2. 代价函数：

   cost(x) = \min_{y \in Y_{near}} \{ cost(y) + distance(y, x) \}
   

   表示节点$x$的最小代价为所有邻域节点$y$的代价与$y$到$x$的距离之和的最小值。

3. 快速行进条件： FMT*算法满足"先到先服务"原则，即节点一旦被添加到闭合集（Closed Set），其代价将不再更新，这保证了算法的高效性。

与经典路径规划算法对比

特性	FMT*	RRT*	A*	Dijkstra
完备性	概率完备	概率完备	完备	完备
最优性	渐进最优	渐进最优	最优	最优
时间复杂度	O(n log n)	O(n log n)	O(E log V)	O(E + V log V)
空间复杂度	O(n)	O(n)	O(V)	O(V)
采样效率	高	中	无采样	无采样
动态环境适应	中	高	低	低
实现复杂度	中高	中	低	低

注：n为采样点数量，V为图节点数，E为图边数

性能对比实验

在相同环境配置下（200×200网格，50个随机障碍物），各算法性能测试结果：

实验结果表明，FMT在保持接近A算法的路径质量的同时，具有比RRT*更高的计算效率，特别适合于高维空间和复杂环境的路径规划任务。

2D路径规划完整实现

核心代码结构

class FMT:
    def __init__(self, x_start, x_goal, search_radius):
        self.x_init = Node(x_start)  # 起始节点
        self.x_goal = Node(x_goal)   # 目标节点
        self.search_radius = search_radius  # 搜索半径
        
        # 环境与工具初始化
        self.env = env.Env()
        self.plotting = plotting.Plotting(x_start, x_goal)
        self.utils = utils.Utils()
        
        # 算法参数初始化
        self.V = set()  # 所有节点集
        self.V_unvisited = set()  # 未访问节点集
        self.V_open = set()  # 开放集(前沿集)
        self.V_closed = set()  # 闭合集
        self.sample_numbers = 1000  # 采样点数量

初始化与采样

def Init(self):
    samples = self.SampleFree()  # 采样自由空间点
    
    # 初始化节点集
    self.x_init.cost = 0.0
    self.V.add(self.x_init)
    self.V.update(samples)
    self.V_unvisited.update(samples)
    self.V_unvisited.add(self.x_goal)
    self.V_open.add(self.x_init)

def SampleFree(self):
    n = self.sample_numbers
    delta = self.utils.delta
    Sample = set()
    
    ind = 0
    while ind < n:
        # 随机采样
        node = Node((random.uniform(self.x_range[0] + delta, self.x_range[1] - delta),
                     random.uniform(self.y_range[0] + delta, self.y_range[1] - delta)))
        # 检查是否在障碍物内
        if self.utils.is_inside_obs(node):
            continue
        else:
            Sample.add(node)
            ind += 1
    
    return Sample

核心规划逻辑

def Planning(self):
    self.Init()
    z = self.x_init  # 起始节点
    n = self.sample_numbers
    # 计算邻域半径
    rn = self.search_radius * math.sqrt((math.log(n) / n))
    Visited = []  # 记录访问顺序
    
    while z is not self.x_goal:  # 未到达目标
        V_open_new = set()  # 新开放节点集
        # 寻找未访问的邻域节点
        X_near = self.Near(self.V_unvisited, z, rn)
        Visited.append(z)
        
        for x in X_near:
            # 寻找开放集中的邻域节点
            Y_near = self.Near(self.V_open, x, rn)
            # 计算所有可能前驱节点的代价
            cost_list = {y: y.cost + self.Cost(y, x) for y in Y_near}
            # 选择最小代价的前驱节点
            y_min = min(cost_list, key=cost_list.get)
            
            # 检查碰撞
            if not self.utils.is_collision(y_min, x):
                x.parent = y_min  # 设置父节点
                V_open_new.add(x)  # 添加到新开放集
                self.V_unvisited.remove(x)  # 从未访问集移除
                x.cost = y_min.cost + self.Cost(y_min, x)  # 更新代价
        
        # 更新开放集
        self.V_open.update(V_open_new)
        self.V_open.remove(z)  # 从开放集移除当前节点
        self.V_closed.add(z)   # 添加到闭合集
        
        if not self.V_open:  # 开放集为空，无路径
            print("open set empty!")
            break
        
        # 选择开放集中代价最小的节点
        cost_open = {y: y.cost for y in self.V_open}
        z = min(cost_open, key=cost_open.get)
    
    # 提取路径
    path_x, path_y = self.ExtractPath()
    # 动画展示
    self.animation(path_x, path_y, Visited[1: len(Visited)])

路径提取与可视化

def ExtractPath(self):
    path_x, path_y = [], []
    node = self.x_goal  # 从目标节点回溯
    
    while node.parent:  # 直到起始节点
        path_x.append(node.x)
        path_y.append(node.y)
        node = node.parent
    
    # 添加起始节点
    path_x.append(self.x_init.x)
    path_y.append(self.x_init.y)
    
    return path_x, path_y  # 返回路径点列表

def animation(self, path_x, path_y, visited):
    self.plot_grid("Fast Marching Trees (FMT*)")
    
    # 绘制所有采样点
    for node in self.V:
        plt.plot(node.x, node.y, marker='.', color='lightgrey', markersize=3)
    
    count = 0
    # 绘制探索过程
    for node in visited:
        count += 1
        plt.plot([node.x, node.parent.x], [node.y, node.parent.y], '-g')
        plt.gcf().canvas.mpl_connect(
            'key_release_event',
            lambda event: [exit(0) if event.key == 'escape' else None])
        if count % 10 == 0:  # 每10步刷新一次
            plt.pause(0.001)
    
    # 绘制最终路径
    plt.plot(path_x, path_y, linewidth=2, color='red')
    plt.pause(0.01)
    plt.show()

参数调优指南

关键参数影响分析

1. 采样点数量（sample_numbers）：

   • 过小：可能导致路径不连通，降低完备性
   • 过大：增加计算负担，降低实时性
   • 推荐值：500-2000（根据环境复杂度调整）

2. 搜索半径（search_radius）：

   • 过小：邻域节点少，可能无法找到最优路径
   • 过大：增加计算量，邻域搜索效率降低
   • 推荐值：环境对角线长度的1/10-1/5

3. 邻域半径系数：

   • 影响邻域节点数量，间接影响算法效率和路径质量
   • 推荐值：1.0（默认），复杂环境可适当增大至1.2-1.5

参数调优流程图

工程应用与避障策略

复杂环境避障实现

FMT*算法通过以下机制实现复杂环境避障：

1. 采样阶段障碍物检查：

def is_inside_obs(self, node):
    # 检查是否在矩形障碍物内
    for (ox, oy, w, h) in self.obs_rectangle:
        if ox < node.x < ox + w and oy < node.y < oy + h:
            return True
    # 检查是否在圆形障碍物内
    for (ox, oy, r) in self.obs_circle:
        if np.hypot(node.x - ox, node.y - oy) < r:
            return True
    # 检查是否在边界外
    for (ox, oy, w, h) in self.obs_boundary:
        if ox < node.x < ox + w and oy < node.y < oy + h:
            return True
    return False

2. 路径段碰撞检查：

def is_collision(self, start, end):
    # 检查线段start-end是否与障碍物碰撞
    if start is None or end is None:
        return True
    
    # 线段离散化检查
    points = self.discrete_points(start, end)
    for p in points:
        if self.is_inside_obs(Node(p)):
            return True
    return False

动态障碍物处理扩展

FMT*算法可通过以下扩展处理动态障碍物：

class DynamicFMT(FMT):
    def __init__(self, x_start, x_goal, search_radius):
        super().__init__(x_start, x_goal, search_radius)
        self.dynamic_obs = []  # 动态障碍物列表
    
    def update_dynamic_obs(self, new_obs):
        """更新动态障碍物位置"""
        self.dynamic_obs = new_obs
        
        # 重新检查受影响的路径段
        for node in self.V_closed:
            if node.parent:
                for obs in new_obs:
                    if self.check_segment_obs_collision(node, node.parent, obs):
                        # 障碍物影响到已有路径，需要重新规划
                        return True
        return False
    
    def check_segment_obs_collision(self, start, end, obs):
        # 检查线段是否与动态障碍物碰撞
        # ...实现细节...
        return False

性能优化与未来改进方向

算法优化技巧

1. 采样策略优化：

   • 采用确定性采样（如Halton序列）替代随机采样，提高采样均匀性
   • 重点区域加密采样，提高关键区域探索效率

2. 并行计算优化：

   • 邻域搜索并行化，利用多核CPU资源
   • 采样与路径搜索并行处理，降低整体延迟

3. 内存优化：

   • 使用KD树或R树存储采样点，提高邻域搜索效率
   • 动态调整采样点数量，根据环境复杂度自适应

未来研究方向

1. 高维空间扩展： 目前FMT*在3D及以上空间的性能下降明显，需研究更高效的高维空间采样与搜索策略。

2. 多机器人协同规划： 扩展FMT*算法以支持多机器人系统的协同路径规划，解决避碰与任务分配问题。

3. 深度学习融合： 利用深度学习预测环境特征，指导采样过程，提高复杂环境下的规划效率。

完整项目实现与使用指南

项目结构

PathPlanning/
├── CurvesGenerator/           # 曲线生成模块
├── Sampling_based_Planning/   # 采样-based规划算法
│   ├── gif/                   # 算法演示GIF
│   ├── rrt_2D/                # 2D RRT系列算法
│   └── rrt_3D/                # 3D RRT系列算法
└── Search_based_Planning/     # 搜索-based规划算法
    ├── Search_2D/             # 2D搜索算法
    ├── Search_3D/             # 3D搜索算法
    └── gif/                   # 算法演示GIF

快速开始

1. 克隆项目：

git clone https://gitcode.com/gh_mirrors/pa/PathPlanning
cd PathPlanning

2. 安装依赖：

pip install numpy matplotlib

3. 运行FMT*算法示例：

cd Sampling_based_Planning/rrt_2D
python fast_marching_trees.py

4. 参数配置： 修改fast_marching_trees.py中的以下参数进行自定义：

# 起点和终点
x_start = (18, 8)
x_goal = (37, 18)
# 搜索半径
search_radius = 40
# 采样点数量
sample_numbers = 1000

总结与展望

FMT算法作为一种高效的采样-based路径规划算法，在保持概率完备性和渐进最优性的同时，通过快速行进策略实现了比RRT更高的规划效率。本文详细介绍了FMT*算法的核心原理、数学基础、实现细节和参数调优方法，并通过与其他经典算法的对比，展示了其在路径规划领域的优势。

随着机器人技术和自动驾驶的发展，FMT*算法在以下方向具有广阔的应用前景：

• 无人机自主导航与避障
• 自动驾驶汽车路径规划
• 工业机器人运动规划
• 多智能体协同控制

未来研究可进一步关注算法在动态未知环境中的鲁棒性和实时性优化，以及与深度学习等领域的融合创新。

如果你觉得本文对你有帮助，请点赞、收藏并关注，下期将带来FMT*与强化学习结合的路径规划算法详解！

【免费下载链接】PathPlanning Common used path planning algorithms with animations. 项目地址: https://gitcode.com/gh_mirrors/pa/PathPlanning