RRT 路径规划 rrt 基于珊格地图的RRT路径规划 ——————————————
刚接触路径规划那会儿，总想着怎么让机器人像电影里那样在迷宫里丝滑走位。直到被现实打脸——传统A*算法遇到复杂环境直接算到冒烟。后来在导师电脑上看到RRT（快速扩展随机树）的演示动画，树枝状结构像活物一样在障碍物间蜿蜒生长，当时就跪了。
咱们今天要玩的场景是栅格地图，这种由0和1组成的矩阵特别适合快速验证算法。先来点直观的：假设我们有个20x20的迷宫，黑色像素代表障碍物，白色是可行走区域。RRT的核心逻辑就像朝随机方向扔飞镖，然后拉着树苗往那个方向长。
先整点基础代码架子：
class Node: def __init__(self, x, y): self.x = x self.y = y self.parent = None class RRT: def __init__(self, grid, step_size=3): self.grid = grid[::-1] # 处理坐标系翻转 self.step_size = step_size self.nodes = [Node(1, 1)] # 起点设在左上角看到没？Node类简单得令人发指，就存坐标和父节点。重点在RRT初始化时把地图上下翻转了，因为栅格地图的(0,0)通常在左上角，而数学坐标系是左下角为原点，这个细节坑过我整晚。
碰撞检测是路径规划的灵魂，但在栅格环境里可以耍个花招：
def is_collision(self, x1, y1, x2, y2): dx = abs(x2 - x1) dy = abs(y2 - y1) if dx + dy > self.step_size * 2: return True return self.grid[int(y1)][int(x1)] == 0这里偷懒用了曼哈顿距离做近似判断，实际上应该做直线插值检测。但实测在低分辨率栅格中完全够用，毕竟RRT本身就有容错性。当年我坚持写Bresenham直线算法，结果调试到凌晨三点，血的教训告诉咱：别在demo代码里追求完美。
核心的扩展过程才是戏肉：
def expand(self): rand_x = random.randint(0, len(self.grid[0])-1) rand_y = random.randint(0, len(self.grid)-1) nearest = min(self.nodes, key=lambda n: (n.x-rand_x)**2 + (n.y-rand_y)**2) theta = math.atan2(rand_y - nearest.y, rand_x - nearest.x) new_x = nearest.x + self.step_size * math.cos(theta) new_y = nearest.y + self.step_size * math.sin(theta) if 0 <= new_x < len(self.grid[0]) and 0 <= new_y < len(self.grid): if not self.is_collision(nearest.x, nearest.y, new_x, new_y): new_node = Node(new_x, new_y) new_node.parent = nearest self.nodes.append(new_node) return new_node return None这段代码里藏着三个精妙之处：1）随机采样时直接取整，让树自然贴合栅格结构；2）用最近邻节点+固定步长代替直线连接，避免复杂碰撞计算；3）通过三角函数确定生长方向，比向量归一化更直观。记得第一次跑通这段时，看着树枝在控制台字符画里七扭八拐地避开障碍，比通关魂系游戏还爽。
当树枝终于碰到目标点时，回溯路径就像解开缠住的耳机线：
def get_path(self, end_node): path = [] current = end_node while current: path.append((current.x, current.y)) current = current.parent return path[::-1]这种链表式的逆向追溯虽然简单粗暴，但在实际项目中要小心循环引用。有次我在C++版本里没处理好智能指针，结果路径回溯变成了无限循环，直接把仿真程序跑崩了。
最后说点实在的：RRT在栅格地图上的表现就像喝醉的扫地机器人——路径曲里拐弯但总能到达。想要更丝滑？试试RRT*的代价重构机制；想要更快？上Bidirectional RRT同时生长两棵树。不过对于刚入门的兄弟，先把基础版本吃透更重要。就像我导师常说的："别急着优化，跑起来再说。"
