基于Matlab的RRT迷宫路径规划：从原理到实战

做迷宫类路径规划项目很多人的第一反应都是上A*或者把地图栅格化之后用Dijkstra。在规整的网格环境里这些确定性搜索算法确实好用可一旦迷宫变得极其困难——通道比车辆自身宽度大不了多少、U形死角一个接一个、障碍物还是随机生成的——传统方法的效率就会断崖式下跌。后来我把RRTRapidly-exploring Random Tree快速扩展随机树算法用到这套场景里配合Matlab做了完整Demo实测下来无论迷宫怎么随机变化只要参数调对基本都能稳定找到可行路径。这篇文章就把整体思路、Matlab代码、调参经验和踩过的坑一次性讲清楚适合正在做路径规划课设、机器人导航仿真或者想入门采样类规划算法的读者。1. 为什么传统路径规划方法会在随机迷宫里“翻车”1.1 迷宫问题的结构特点先说清楚“极其困难的随机迷宫”到底难在哪。我做的测试环境是一个二维栅格地图尺寸在100×100左右墙的密度很高通道宽度通常只有2到3个栅格。更关键的是迷宫由算法随机生成没有什么“主路”和“支路”之分大量的狭窄走廊、死胡同、回字形陷阱随机堆叠在一起这种结构对路径规划算法极不友好。这类环境的本质问题是可行通道在状态空间中占比极低而且拓扑关系高度非结构化。通道之间互相缠绕没有任何先验规则可循。你想让算法“动脑筋”判断哪个方向更有可能通向终点它根本无从下手因为墙后面是什么完全未知。栅格法在这里也容易踩坑。如果栅格粒度设得太大那些两格宽的通道直接就消失了物理上可行的路径在数学表示里就不存在粒度设小一点地图规模又会以平方级别膨胀搜索耗时迅速失控。我在85×85的迷宫上试过5像素粒度的栅格化光是构建邻接表就耗掉大半内存路径搜索时间更是无法接受。1.2 A*、Dijkstra等确定性方法的局限A*的搜索效率高度依赖启发函数的质量。在开阔地图里欧氏距离或曼哈顿距离能提供很有效的方向指引但在迷宫场景下启发值经常严重“失真”——明明终点就在墙对面实际绕过去却要几千个格子启发函数给出的距离估计和真实代价相差悬殊算法就会大量扩展低价值节点表现和一个盲目广搜没有本质区别。Dijkstra更不用说了在迷宫环境里它基本是在做全图遍历时间复杂度和地图面积直接挂钩。我拿30×30的小迷宫让Dijkstra跑路径倒是能找到耗时已经需要好几秒换成60×60的随机迷宫耗时直接变成分钟级而且这个问题和硬件性能关系不大纯粹是算法复杂度决定的。这引出一个关键结论当环境的拓扑结构极度复杂时任何基于“确定性枚举”的搜索策略都会遇到组合爆炸问题。这也是为什么我会转向采样类算法——它们不试图枚举所有可能性而是通过随机采样的方式在状态空间里“抽奖”用概率换时间。2. RRT快速扩展随机树以随机性对抗复杂性的核心思路2.1 RRT的核心流程拆解RRT的思想可以用一个生活化类比来理解想象你在一片完全黑暗、遍布障碍的森林里找人你手里只有一条能自动伸缩的绳子。你每走一步就往随机方向甩一小段绳子只要绳子没被障碍挡住就沿着这个方向前进一点然后把绳子另一头固定住。重复无数次之后绳子的网络会逐渐覆盖整个森林只要存在一条通路这根绳子大概率最终会碰到目标。算法落到代码层面每轮迭代就做四件事随机采样一个点、在现有树上找到离它最近的节点、从最近节点朝采样点方向延伸一个固定步长、检测这段新路径是否撞障碍。如果没撞新节点就加入树中如果撞了这一轮就放弃重新采样。上面说的是最基础的RRT。实际工程里我会额外引入一个目标偏置策略每次采样时以一定概率比如10%直接把目标点作为采样点其余90%才做全图随机采样。这样做的目的是给树的生长施加一个“方向拉力”避免树完全漫无目的地乱长同时又保留了随机性去探索复杂通道。2.2 为什么RRT天生适合复杂迷宫RRT最迷人的地方在于它的概率完备性只要迭代次数足够多找到一条可行路径的概率趋近于1。这个性质不需要对地图做任何假设管它是规整网格还是随机迷宫。原因很简单——随机采样点在整个自由空间里均匀分布树的扩展方向又不受任何启发函数的误导所以随着采样点数量的增加树对自由空间的覆盖会越来越密最终“必然”触及目标所在的连通分量。实际测试也验证了这一点。在通道宽度只有3个栅格的85×85随机迷宫里RRT通常只需要几千次迭代就能找到路径耗时在2到5秒之间这个表现和A*在高密度迷宫里的表现形成了鲜明对比。另外RRT还有一个隐性优势它对车辆的运动约束天然友好。在这类算法里车辆的运动模型是作为“扩展规则”的一个输入存在的——每次步进的方向不是随便定的而是车辆在当前状态下能够执行的方向。这意味着从仿真到实车时只需要替换掉步进函数算法骨架完全不用动。传统A*规划出来的路径常常是一堆直角折线到了实车上还得再做一次运动约束校验这部分工作RRT在结构上就替你省了。3. Matlab环境下的迷宫生成与参数设定3.1 用递归回溯算法生成随机迷宫测试迷宫我用的是经典的递归回溯算法Recursive Backtracker生成。这个算法的思路很直观把每个格子视为一个房间从起点出发随机选择一个尚未访问的相邻房间打通中间的墙走过去继续这个过程如果当前房间周围没有未访问的邻居就沿来路回溯。全部房间都访问过之后迷宫就生成了。Matlab实现如下function maze generateMaze(rows, cols) maze ones(rows*21, cols*21); visited false(rows, cols); stack [1, 1]; visited(1,1) true; % 起点房间打通 maze(2, 2) 0; while ~isempty(stack) current stack(end,:); r current(1); c current(2); % 随机选择未访问邻居 neighbors []; if r 1 ~visited(r-1, c) neighbors(end1,:) [r-1, c, 1]; end if r rows ~visited(r1, c) neighbors(end1,:) [r1, c, 2]; end if c 1 ~visited(r, c-1) neighbors(end1,:) [r, c-1, 3]; end if c cols ~visited(r, c1) neighbors(end1,:) [r, c1, 4]; end if ~isempty(neighbors) idx randi(size(neighbors,1)); nr neighbors(idx,1); nc neighbors(idx,2); dir neighbors(idx,3); % 打通当前房间与邻居之间的墙 if dir 1 % 上 maze(2*r-1, 2*c) 0; elseif dir 2 % 下 maze(2*r1, 2*c) 0; elseif dir 3 % 左 maze(2*r, 2*c-1) 0; elseif dir 4 % 右 maze(2*r, 2*c1) 0; end maze(2*nr, 2*nc) 0; visited(nr, nc) true; stack(end1,:) [nr, nc]; else stack(end,:) []; % 回溯 end end end生成出来的迷宫是一个(2rows1)×(2cols1)的逻辑地图值为1表示墙0表示可通行的走廊。也就是说如果你想让迷宫主体是85×85实际的地图矩阵就是171×171。测试的时候我建议先从25×25的小迷宫开始跑通之后再逐步挑战更大的尺寸。3.2 地图表示、碰撞检测与车辆模型简化路径规划代码里地图统一用二值矩阵表示车辆被简化为一个质点实际车身宽度通过膨胀障碍物来处理。具体做法是在生成地图之后对所有障碍格执行一次形态学膨胀把墙边界向外扩展若干个栅格等效于车辆的“安全距离”。这样规划出来的路径即使在真实车辆执行时也不会擦墙。% 障碍物膨胀确保车辆安全距离 se strel(square, 7); % 7个栅格的膨胀核相当于3格车身宽2格安全余量 expandedMap imdilate(maze, se);碰撞检测逻辑也依赖膨胀后的地图在判断某个节点或某条路径线段是否可行时只要线段经过的任意栅格在地图上的值为1就判定为碰撞。线段检测用的是经典的Bresenham算法遍历线段经过的所有栅格速度够快实现也简单。提示膨胀核尺寸必须和你设定的小车半径强相关。我在测试中发现如果膨胀尺寸过小生成的路径虽然在地图上是可行的但实际执行时车辆很容易蹭到墙边膨胀过大又会把原本两格宽的通道完全堵死导致算法找不到任何路径。具体尺寸需要针对你的车型反复试验。4. RRT路径规划核心模块的Matlab实现详解4.1 采样、最近邻与步进增长RRT的三板斧就是采样、找最近邻、步进扩展。先说采样模块核心逻辑是目标偏置每次生成随机数如果小于偏置概率就直接返回目标点否则在自由空间内均匀采样。自由空间的范围直接取地图矩阵的尺寸简单高效。function [samplePoint] samplePoint(map, goal, biasProb) if rand() biasProb samplePoint goal; return; end [rows, cols] size(map); while true p [randi(rows), randi(cols)]; if map(p(1), p(2)) 0 samplePoint p; return; end end end最近邻查找的逻辑也很直接遍历树中所有节点计算它们到采样点的欧氏距离取最小值对应的那个节点。在Matlab的向量化加持下这一步很快。但随着树节点的增多遍历开销会线性增长这就是后面要优化的重点。步进扩展是RRT的灵魂。给定最近邻节点和采样点实际的步进方向是从最近邻节点指向采样点的单位向量乘以步长stepSize。注意这里有个关键细节不能直接把采样点作为新节点加入树中而是要走一小步。因为采样点可能离得很远直接一步跨过去中间路径很可能穿越障碍。步长是RRT最重要的参数之一它直接决定了树的“伸展能力”具体怎么调我会在下一节详细讲。function [newNode] extendTree(map, nearestNode, samplePoint, stepSize) dist norm(samplePoint - nearestNode); if dist stepSize newNode samplePoint; % 离得近就直接点对点 else direction (samplePoint - nearestNode) / dist; newNode round(nearestNode direction * stepSize); end % 碰撞检测 if ~isCollisionFree(map, nearestNode, newNode) newNode []; end end4.2 碰撞检测与到达目标的判断逻辑碰撞检测用Bresenham算法遍历两个节点之间的所有栅格只要有一个栅格是墙就返回碰撞。这里有一个重要的工程细节检测半径不能是0。因为车辆模型是有宽度的即使两个节点之间的连线在地图上恰好穿过了空格但在狭窄通道里连线距离墙壁太近也会导致实际车辆过不去。所以我实际用的是“宽线检测”把Bresenham遍历的每个点都检查一个3×3邻域邻域内有墙即判为碰撞。function flag isCollisionFree(map, node1, node2) flag true; [xPoints, yPoints] bresenham(node1(1), node1(2), node2(1), node2(2)); for i 1:length(xPoints) x xPoints(i); y yPoints(i); % 检查3x3邻域 for dx -1:1 for dy -1:1 nx x dx; ny y dy; if nx 1 || ny 1 || nx size(map,1) || ny size(map,2) flag false; return; end if map(nx, ny) 1 flag false; return; end end end end end到达目标的判断条件我做了两重冗余。最常用的方式是当树中新增的节点距离目标点小于一个阈值比如stepSize或者再小一点时就直接把目标点作为终结点加入树然后回溯生成路径。但如果迷宫通道特别诡异新增节点离目标很近却隔着墙这招就会失效。所以我额外加了一条规则新节点加入树后立即检测新节点到目标点的连线是否无碰撞。如果无碰撞哪怕距离远一点也可以直接把目标点连进树里立刻终止搜索。function [pathFound, path] checkGoal(map, tree, newNode, goal, goalThreshold) pathFound false; path []; if norm(newNode - goal) goalThreshold || isCollisionFree(map, newNode, goal) tree(end1) newNode; pathFound true; path [newNode, goal]; end end4.3 完整可运行的RRT主程序把上面的模块拼起来就是一个完整的RRT迷宫路径规划程序。下面是主循环的关键部分完整代码我建议自己整理成一个函数function [path, tree] rrtPlan(map, start, goal, maxIter) % 参数配置 stepSize 8; goalThreshold 5; biasProb 0.15; % 初始化树 tree start; parent -1; pathFound false; for i 1:maxIter sp samplePoint(map, goal, biasProb); [nearestIdx, nearestNode] findNearest(tree, sp); newNode extendTree(map, nearestNode, sp, stepSize); if isempty(newNode) continue; end tree(end1) newNode; parent(end1) nearestIdx; % 检查是否到达目标 if norm(newNode - goal) goalThreshold pathFound true; tree(end1) goal; parent(end1) length(tree) - 1; break; end end % 从目标回溯生成路径 if pathFound path []; idx length(tree); while idx 0 path [tree(idx); path]; idx parent(idx); end else path []; warning(未能在最大迭代次数内找到路径); end end注意这段代码里的一个细节我在每一步扩展后没有做“当前新节点到目标点连线”的二次检测。如果你希望提高成功率可以把4.2节中的checkGoal函数加进来在每次成功加入新节点后额外调用一次。两者配合成功率会有可感知的提升。5. 从能跑到跑好迷宫路径规划调试踩坑记录5.1 步长与迭代次数的博弈步长是最容易出问题的参数。极端情况下步长设得太小树长得极其密集一条长廊从头到尾可能要几千上千个节点迭代次数很快耗尽步长设得太大又会出现“一步跨过窄通道”的尴尬局面——明明通道就挨着但步长太长导致步进终点落在墙的另一边碰撞检测直接否决。我的经验是迷宫通道宽度为2到3个栅格时步长建议取1.5到2倍通道宽度也就是4到6个栅格。如果迷宫更大、通道更宽可以按比例放大。另外最大迭代次数不要舍不得给随机迷宫路径往往绕很远我的测试配置是maxIter10000对于85×85的迷宫实际找到路径通常只要2000到4000次迭代这个余量是给极端随机情况保底的。5.2 狭窄通道下的采样策略问题迷宫场景里最常见的失败模式是树沿着通道走了一段但随机采样点总是落在墙外导致树的扩展方向长期被“卡死”在某一条通道里无法转入其他通道。这是纯粹随机采样在面对狭窄通道时的一个天然弱点——因为通道的宽度在整个地图面积里占的比例太小随机点落到通道内的概率本来就很低。一个有效的补救方案是双向RRT从起点和终点各长一棵树两棵树交替扩展互相把对方当作目标。这样做的好处是两棵树可以同时探索不同区域一旦它们的距离小于某个阈值就认为路径连通了。我在单向RRT需要4000次迭代才能找到的迷宫里双向RRT通常只需要1500次左右。代价是代码复杂度翻倍但对迷宫场景来说完全值得。5.3 目标偏置参数的经验取值区间目标偏置是一把双刃剑。偏置概率设得太小比如小于0.02树基本失去了方向感纯粹靠运气漫游偏置概率设得太大比如大于0.3树的扩展会被强烈吸引向目标方向反而容易在遇到墙体阻碍时反复试探同一条死路陷入局部振荡。调试中发现偏置概率在0.08到0.15之间比较稳。在这个区间内树的扩展既有方向性又保留了全局探索的能力。另外还有一个心法偏置概率可以随迭代次数动态变化——迭代初期用小偏置让树先充分铺开探索迭代中期加大偏置集中力量向目标推进。这算是一个性价比很高的“小魔改”。5.4 随机数种子的真随机陷阱RRT的随机性会带来一个很扎心的现象同一份代码这次运行能找到路径下次运行可能就找不到。很多人在调试时会把失败归咎于算法不行其实只是随机采样的运气问题。解决思路很简单在程序启动时固定随机数种子rng(42)保证调试过程可复现等算法稳定之后再随机化种子去做统计测试。我实际统计过一组参数在某迷宫上的表现固定种子下100次运行成功95次不固定种子则成功率会在88%到97%之间波动。这个数字说明两点第一参数没问题成功率已经足够高第二任何RRT的单次运行结果都不能代表算法性能要做多次实验取平均值才有说服力。6. RRT的进阶优化方向与工程化建议6.1 RRT*的代价函数与渐进最优特性基础RRT找到的路径通常弯弯曲曲完全谈不上“最优”因为它只保证能通不保证路径短。RRT*的改进在于引入了重新布线机制每次新增节点后候选父节点不再局限于“最近邻”而是在附近一定半径内寻找所有潜在父节点选择使该节点代价最小的那个同时对新节点附近的其他节点进行重连看能否通过新节点缩短它们到起点的代价。代价被我定义为路径长度累计步长之和。RRT会随着迭代次数的增加不断优化树中各节点的代价因此路径质量会逐渐提升。代价是计算量显著提高而且**如果迭代次数不够RRT甚至可能比基础RRT更慢**——因为多做了很多无效的重连计算。迷宫场景下我的建议是先用基础RRT快速找出一条路径然后再用RRT去优化它而不是一上来就上RRT。6.2 双向树、动态步长与平滑后处理双向RRT带来的成功率提升前面已经说了这里补充一个动态步长的实现思路在靠近目标时自动将步长缩小一半远离目标时恢复默认步长。这个策略能有效减少到达目标后的过冲问题。路径平滑也值得做。RRT产生的路径是多段折线在机器人上执行会让电机频繁启停。最实用的后处理方法是路径剪枝从起点开始尝试直接连接更远的节点如果这段连线无碰撞就删掉中间的所有节点。反复剪枝几轮路径通常会变得非常利落。如果需要曲率连续还可以用三次样条拟合但要注意拟合后的路径不能偏移到障碍物里必须再做一次碰撞校验。6.3 从仿真到实车的注意事项仿真代码移植到实车时有几个坑要提前规避。第一仿真中车辆是理想点模型实车则要重新计算质心位置和转弯半径建议规划层的安全距离至少再加大20%第二仿真地图是静态的实车环境里可能有动态障碍RRT的随机采样特性可以较快适应局部地图变化但如果障碍物运动太快规划层只能负责生成参考路径避障还得靠局部控制器第三路径重规划频率不要太高否则车辆会频繁急转弯反而增加执行风险。我在某项目里的做法是规划层以0.5到1秒的周期调用RRT进行全局重规划执行层用PID跟踪路径。测试下来即使局部障碍突变车辆也能在1到2秒内修正轨迹整体表现比用A*时平滑不少。6.4 新方向的探索心得最近我在尝试把RRT与深度强化学习结合训练一个小型策略网络学习“哪些区域更容易产生优质采样点”再用这个策略网络替代均匀随机采样。本质上是在保持RRT概率完备性的同时把“探索”变得更聪明。目前效果还在验证中但在有些迷宫上确实能把找到路径的平均迭代次数再降30%到40%。如果你有基础这个方向值得跟一跟。写在最后从最初在迷宫里频繁“翻车”到最终能稳定跑通各类随机迷宫RRT给我的最大感受是它不讲究精巧讲究的是概率上的“磨”。在极其复杂的环境里探索本身往往比聪明更重要。如果你也正在被迷宫路径规划的问题困扰建议先把基础RRT跑通再去折腾优化版本——先让路径出现再让路径变得更好。这八个字大概是我在这个项目里最想分享的一句话。