程序基于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和纯势场，这个混合算法已经好用太多了。当时改完之后，实验室的小破车终于能顺利绕开中间的障碍物开到终点，再也不会贴在墙上瞎转了，属实有点小成就感。