RRT与人工势场混合算法在路径规划中的实践

1. 基于RRT与人工势场混合算法的路径规划实践

实验室的小破车终于能正常绕开障碍物了！之前纯用RRT算法时，那路径规划简直惨不忍睹——车子要么像醉汉一样东倒西歪，要么就死贴着障碍物打转，有时候甚至能在墙角转上半小时。后来把人工势场算法混进去，效果立竿见影。今天就来详细聊聊这个混合算法的实现细节。

1.1 算法选型的痛点分析

RRT（快速随机探索树）算法最大的特点就是随机采样，理论上不会陷入局部最优解。但实际用起来就会发现，这种完全随机的探索方式会导致：

• 生成的路径像蚯蚓爬行一样歪歪扭扭
• 遇到障碍物时容易贴着边缘"蹭"过去
• 路径长度远非最优，经常绕远路

而人工势场算法（APF）则完全相反：

• 目标点产生吸引力，障碍物产生斥力
• 路径自然平滑，理论上最短
• 但致命缺陷是会陷入局部极小点（如两个障碍物中间）

1.2 混合算法的核心思路

既然两个算法优缺点互补，那把它们结合起来就顺理成章了：

1. RRT兜底：用随机采样保证一定能找到解（如果存在）
2. 势场引导：用势场力引导采样方向，减少随机性
3. 扰动机制：当检测到局部最优时，加入随机扰动

这种混合方式既保留了RRT的全局收敛性，又获得了势场的路径平滑优势。实测下来，规划时间比纯RRT缩短了约40%，路径长度减少了15-20%。

2. 算法实现细节拆解

2.1 基础数据结构设计

首先定义节点类，用于存储RRT搜索树的结构：

python复制class RRTNode:
    def __init__(self, pos):
        self.pos = pos  # 节点位置[x,y]
        self.parent = None  # 父节点指针

地图参数初始化时需要注意：

• 障碍物半径要略大于实际物理尺寸（留安全余量）
• 地图边界要预留缓冲空间，防止采样点出界

python复制# 地图范围[x_min, x_max, y_min, y_max]
MAP_RANGE = np.array([0, 10, 0, 10])  

# 起点和终点坐标
START = np.array([1, 1])
GOAL = np.array([9, 9])

# 障碍物用圆形表示(中心坐标,半径)
OBSTACLES = [
    Circle((5,5), 1),  # 中间障碍物
    Circle((3,7), 0.8) # 左上障碍物
]

2.2 势场力计算的关键实现

势场计算函数是整个算法的核心之一，需要特别注意几个关键点：

python复制def get_force_field(pos, goal, obstacles, k_att=1.0, k_rep=5.0, r_rep=2.0):
    # 吸引力计算（线性势场）
    f_att = -k_att * (pos - goal)
    
    # 斥力计算（分段函数）
    f_rep = np.zeros(2)
    fo