基于RRT与人工势场混合算法的路径规划程序

程序基于RRT和人工势场混合算法的路径规划

前阵子帮实验室的小破车改路径规划模块，一开始纯用RRT跑出来的路总是像无头苍蝇乱晃，遇到个小障碍物就直接贴上去绕半天，有时候甚至会在障碍物旁边打转半小时都找不着路。后来翻了点资料混了人工势场，效果终于顺眼了点，今天就唠唠这个混合算法的事儿。

先简单掰扯下俩算法各自的尿性：RRT虽然说是不会陷局部最优，但胜在全靠瞎蒙采样，跑出来的路径歪歪扭扭不说，还经常绕远路；人工势场倒是思路挺直观，给目标加吸引力，给障碍物加斥力，看着就能把车往终点拽，但最大的坑就是容易陷局部极小——比如俩障碍物中间刚好卡个点，吸引力和斥力刚好抵消，车就原地不动了，或者目标离障碍物太近，斥力把车推得老远，根本到不了终点。

程序基于RRT和人工势场混合算法的路径规划

所以混合的思路其实也简单：用RRT的随机采样兜底，解决势场的局部最优问题；用势场的合力来引导采样方向，减少RRT的盲目性，让路径更顺一点。我写了个简化版的Python demo，咱边看代码边说。

首先是导入库和初始化参数，没啥好说的，就是定义了地图范围、起点终点和俩圆形障碍物，凑合用来看效果：

import numpy as np import matplotlib.pyplot as plt from matplotlib.patches import Circle 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)]

然后整了个简单的节点类，用来存每个采样点的位置和父节点，方便最后回溯路径：

class RRTNode: def __init__(self, pos): self.pos = pos self.parent = None

接下来是找最近节点的函数，纯纯的欧氏距离比对，没啥花活：

def find_nearest_node(nodes, sample_pos): distances = [np.linalg.norm(node.pos - sample_pos) for node in nodes] return nodes[np.argmin(distances)]

碰撞检测也很直白，就是看采样点有没有进到障碍物的圆里面：

def check_collision(pos, obstacles): for obs in obstacles: ox, oy = obs.center r = obs.radius if np.linalg.norm(pos - np.array([ox, oy])) < r: return True return False

重头戏来了，先整个人工势场的力场计算函数。这里用的是最基础的势场公式，吸引力是朝着目标的，斥力是推开远离障碍物的，要是碰到障碍物直接返回无穷大的力，相当于直接放弃这个点：

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) for obs in obstacles: ox, oy = obs.center r_obs = obs.radius obs_pos = np.array([ox, oy]) dist = np.linalg.norm(pos - obs_pos) # 已经撞到障碍物了，直接返回无效力 if dist <= r_obs + 1e-3: return np.array([np.inf, np.inf]) # 只有在斥力作用范围内才计算斥力 if dist <= r_rep: rep_mag = k_rep * (1/dist - 1/r_rep) * (1/(dist**2)) f_rep += rep_mag * (pos - obs_pos) total_force = f_att + f_rep # 如果合力太小，说明陷入局部最优了，加点随机扰动兜底 if np.linalg.norm(total_force) < 1e-3: total_force = np.random.uniform(-1,1,2) # 返回单位方向向量，方便后面沿着这个方向采样 return total_force / np.linalg.norm(total_force)

然后就是修改后的混合算法扩展函数，和纯RRT比起来，主要改了采样的部分：10%的概率还是随机采样，避免一直沿着一个方向走；剩下90%的概率就沿着势场算出来的合力方向采样，再加点小扰动，防止太死板：

def extend_hybrid(start, goal, obstacles, max_iter=1000, step_size=0.5, sample_bias=0.1): nodes = [RRTNode(start)] for _ in range(max_iter): # 随机采样兜底，防止势场坑住 if np.random.rand() < sample_bias: sample_pos = np.random.uniform(MAP_RANGE[:2], MAP_RANGE[2:]) else: # 沿着势场方向采样，再加点小偏移 current_pos = nodes[-1].pos force_dir = get_force_field(current_pos, goal, obstacles) # 沿着合力方向走5步的距离，再加个正态分布的小扰动 sample_offset = np.random.normal(force_dir * step_size * 5, scale=0.5) sample_pos = current_pos + force_dir * step_size *5 + sample_offset # 把采样点限制在地图范围内，别跑出界了 sample_pos[0] = np.clip(sample_pos[0], MAP_RANGE[0], MAP_RANGE[1]) sample_pos[1] = np.clip(sample_pos[1], MAP_RANGE[2], MAP_RANGE[3]) # 后面的步骤和纯RRT一样，找最近节点、走一步、检查碰撞 nearest = find_nearest_node(nodes, sample_pos) direction = sample_pos - nearest.pos if np.linalg.norm(direction) < 1e-6: continue direction = direction / np.linalg.norm(direction) * step_size new_pos = nearest.pos + direction if not check_collision(new_pos, obstacles): new_node = RRTNode(new_pos) new_node.parent = nearest nodes.append(new_node) # 到终点附近就直接收尾 if np.linalg.norm(new_pos - goal) < step_size: final_node = RRTNode(goal) final_node.parent = new_node nodes.append(final_node) return nodes, final_node return nodes, None

最后就是回溯路径和画图的代码了，回溯就是从终点节点往父节点挨个找，然后反转一下变成从起点到终点的路径：

def get_path(final_node): path = [] current = final_node while current is not None: path.append(current.pos) current = current.parent return path[::-1] def plot_result(nodes, path): fig, ax = plt.subplots(figsize=(8,8)) ax.set_xlim(MAP_RANGE[0], MAP_RANGE[1]) ax.set_ylim(MAP_RANGE[2], MAP_RANGE[3]) # 画障碍物 for obs in OBSTACLES: ax.add_patch(obs) obs.set_facecolor('gray') obs.set_edgecolor('black') # 画起点终点 ax.scatter(START[0], START[1], c='green', s=100, label='起点') ax.scatter(GOAL[0], GOAL[1], c='red', s=100, label='终点') # 画RRT的搜索树 for node in nodes: if node.parent is not None: ax.plot([node.pos[0], node.parent.pos[0]], [node.pos[1], node.parent.pos[1]], c='blue', alpha=0.3) # 画最终路径 path_arr = np.array(path) ax.plot(path_arr[:,0], path_arr[:,1], c='red', linewidth=2, label='规划路径') ax.legend() plt.show()

跑起来的主函数也很简单：

if __name__ == "__main__": nodes, final_node = extend_hybrid(START, GOAL, OBSTACLES, max_iter=500) if final_node is not None: path = get_path(final_node) plot_result(nodes, path) else: print("害，没找到路径，调调参数再试试？")

说回实际效果，我一开始没加那个局部最优的扰动，结果跑到俩障碍物中间的时候直接卡死不动了，后来加了之后就会自动往旁边偏移一下绕过去。还有那个sample_bias参数，一开始设成0的话，跑出来的路径太直了，但遇到复杂一点的障碍物就容易陷进去，调到0.1左右刚好，既有势场引导的顺路径，又有RRT的随机兜底。

当然这个demo还是简化版的，实际用的话还要处理更多东西，比如动态障碍物、路径平滑之类的，但比起纯RRT和纯势场，这个混合算法已经好用太多了。当时改完之后，实验室的小破车终于能顺利绕开中间的障碍物开到终点，再也不会贴在墙上瞎转了，属实有点小成就感。