Matlab实现Bi-RRT双向快速扩展随机树：机器人无碰撞路径规划实战

做机器人路径规划，绕不开的就是采样类算法。前段时间我做一个移动机器人的避障导航仿真，环境里摆了不少不规则障碍物，要求从起点绕到终点，保证整条路径无碰撞。方案对比下来，最终用的是Bi-RRT（Bidirectional Rapidly-exploring Random Tree，双向快速扩展随机树）算法，在Matlab里一步步实现并验证通过，确实找到了可用的无碰撞路径。这篇就把完整的实现思路、代码细节、实验结果和调试经验全部梳理出来，方便正在做相关工作的朋友直接参考。

先说适合谁看。如果你在做机器人导航、机械臂运动规划、自动驾驶局部避障，或者只是对采样类路径规划算法感兴趣，都可以读一读。我会从算法原理讲到Matlab实现细节，再讲各种坑和排查手段。有基础的朋友可以直接看代码部分，完全新手建议从原理开始，后面所有代码都能照着跑。

1. 算法选型：为什么在实际项目中选了Bi-RRT

路径规划的算法非常多，经典的有Dijkstra、A*，采样类有PRM、RRT系列，再加上各种改进版本。第一次做规划问题的人，很容易一上来就选A*，但放到机器人真正的运动规划场景里，A*的局限性其实非常明显。

1.1 传统图搜索算法在高维空间的局限

A依赖栅格地图或者路图，本质上是把连续空间离散化。离散分辨率高了，内存和搜索时间都会爆炸；分辨率低了，又会漏掉关键通道。而且A规划的路径是沿着栅格中心跳转的，跟机器人真实的运动学约束往往脱节，做后续平滑和轨迹跟踪时要处理不少额外麻烦。

PRM（概率路图）是另一类常用方案，它通过离线随机采样建立路图，查询阶段在路图上搜索路径。PRM适合反复查询的静态环境，但环境一变就得重新构图。我做的是动态避障场景，环境本身会变，所以PRM用起来很别扭。

1.2 单向RRT做起来总觉得差一口气

RRT的核心思想是随机采样驱动树生长。算法从起点出发生成一棵树，每次在状态空间里随机采一个点，找到树上距离采样点最近的节点，向采样点方向扩展一小步。如果扩展出来的边没有碰撞，就把新节点加入树中。重复这个过程，树会像藤蔓一样在空间中蔓延，直到某个节点落入目标区域。

这个思路最大的好处是不用离散化空间，天然适合高维，也能方便地加入运动学约束。但实际跑起来会发现一个尴尬的问题：树的随机性太强，终点引导很弱。如果环境很大、通道又窄，单向RRT可能在地图里探索半天，还是找不到通向目标的路。

1.3 Bi-RRT核心改进：让两棵树双向奔赴

Bi-RRT的改进思路非常直接：我不只从起点长树，也从终点长树。起点树和终点树交替扩展，每一轮扩展完尝试把两棵树上最近的两个节点连起来。一旦两棵树成功连接，完整的无碰撞路径就自然形成了。

这个改动带来的效果是立竿见影的。两棵树同时向中间生长，等效于把整个搜索空间切成两个相对小的子问题，绕过了单棵树需要跨越整个空间的漫长过程。尤其是起点和终点各自在狭长通道两端时，Bi-RRT两个方向同时往中间探，相遇的速度和概率都大得多。

1.4 常见路径规划算法对比

做选型时，我习惯把几种算法放在一张表里对比：

在我做的这类“快速找到一条无碰撞路径”的需求下，Bi-RRT是性价比很高的选择。它不是万能的，但多数工程场景里，它比单向RRT快得多，比A*更适合复杂约束。

2. Bi-RRT算法原理解析与关键细节

原理必须吃透，否则代码写着写着就容易陷入细节里出不来。这一节我把算法拆开，讲清楚每个环节的内部逻辑和实现时的注意事项。

2.1 算法主流程：两棵树的交替生长

Bi-RRT的完整流程可以拆成几个核心环节：初始化、随机采样、最近邻搜索、步长扩展、碰撞检测、两树连接、路径提取。

初始化阶段建立两个树的节点容器，起点作为树1的根节点，终点作为树2的根节点。进入迭代后，主要执行五步操作：

1. 树1随机采样，找到最近节点并向采样点方向扩展出一个新节点。
2. 检查新节点的扩展边是否无碰撞，如果碰撞则本轮扩展失败。
3. 扩展成功后，尝试把树1的新节点与树2中的某个节点做连接判定。
4. 如果连接失败，交换两棵树的角色，下一轮换树2扩展。
5. 两棵树连接到一起后，从连接点分别回溯到起点和终点，拼接出完整路径。

值得强调的是，两棵树不一定要每轮强制交替。更实用的策略是轮流扩展，每次扩展完都立刻尝试连接另一棵树的最近节点，这样能够更快捕捉到两棵树接近的瞬间。

2.2 单棵树的扩展机制：采样、最近邻、步长

单棵树扩展有四个关键组件：随机采样、最近邻搜索、步长扩展、碰撞检测。

随机采样默认使用均匀分布，在状态空间范围内随机取点。加入目标偏置可以提升效率，也就是以一定概率直接采样目标点，让树更有方向感地生长，但又不会完全丢失随机探索能力。目标偏置概率通常设为0.05到0.15之间，设太大会让树过度贪婪，反而容易在障碍物附近卡住。

最近邻搜索的效率是算法性能的关键。朴素实现对每个点遍历所有已生成节点，树节点数量达到数千个时，遍历开销会明显上升。工程中通常会引入KD-Tree来加速，但在Matlab仿真里，如果节点数量不夸张，直接遍历也能接受。

步长扩展控制树的生长速度。步长太大，新节点容易穿越障碍物，尤其在窄通道里频繁扩展失败；步长太小，树生长缓慢，需要迭代更多次才能覆盖大范围。常规经验是把步长设置为地图最小无障碍通道宽度的三分之一左右，或者大约是地图对角线长度的1/50到1/100。

碰撞检测负责确认新节点与最近节点之间的线段不穿过任何障碍物。对结构简单的障碍物可以精确求交，对复杂环境一般用离散采样方式，把线段切成多个点逐一检测。检测分辨率要和步长联动考虑，分辨率太粗会漏掉细长障碍物。

2.3 两棵树的连接策略：贪心连接和逐步逼近

两棵树之间的连接策略，直接影响算法成功率和路径质量。主流方法有两类：逐步逼近式和贪心式。

逐步逼近式是每次两棵树各扩展一小步，慢慢缩短彼此之间的距离。这种方式安全性好，但收敛速度较慢，适合通道较窄、环境复杂的场景。

贪心式是当一棵树扩展出新的节点后，直接计算该节点与另一棵树所有节点的距离。如果最小距离小于设定阈值，并且两点之间的连线无碰撞，就直接用直线把两棵树连起来，搜索结束。

贪心式连接效率高，但要求判断条件严谨。连接阈值过大会导致误连接，线段可能穿越障碍物边缘；阈值过小则两棵树永远差一口气连不上。实际调试时，我通常从步长的1.2倍开始调，再根据失败模式增大或缩小。

2.4 算法的收敛性和复杂度

Bi-RRT是概率完备的，就是说当迭代次数趋于无穷时，找到路径的概率会趋近于1。从工程角度看，我们不需要追求数学上的绝对保证，只要迭代上限设置得足够充分，实际表现稳定可靠。

复杂度方面，单次扩展的主要开销来自最近邻搜索和碰撞检测。节点数为N、障碍物数量为M时，一次朴素最近邻搜索是O(N)，一次碰撞检测是O(M)。整体每次迭代的复杂度大约是O(N+M)，实际规划中碰撞检测往往是瓶颈。

3. Matlab代码实现全解析

下面进入实操环节。我用Matlab实现了一个完整的Bi-RRT路径规划器，包含环境定义、参数设置、核心算法和各功能函数。这一节将逐步拆解代码，并标注每个模块的作用和容易出错的细节。

3.1 环境配置与参数定义

先把环境地图和算法参数都定义清楚。我用的模拟场景是二维平面，范围20米乘20米，起点和终点分别放在左下和右上区域，障碍物用圆形和矩形组合摆放，模拟真实空间中的柱子、台架等物体。

% 环境参数 map.width = 20; map.height = 20; map.limit = [0 map.width, 0 map.height]; % 障碍物定义：每个障碍物按类型和几何数据保存 % circle: 数据为[x, y, r]，矩形: 数据为[x1, x2, y1, y2] obstacles = struct('type', {}, 'data', {}); obstacles(end+1) = struct('type', 'circle', 'data', [10, 10, 1.5]); obstacles(end+1) = struct('type', 'rect', 'data', [3, 5, 14, 17]); obstacles(end+1) = struct('type', 'circle', 'data', [15, 4, 1.2]); obstacles(end+1) = struct('type', 'rect', 'data', [12, 14, 8, 16]); % 算法参数 param.stepSize = 0.4; param.maxIter = 5000; param.connectDist = 0.5; param.goalTol = 0.4; param.sampleBias = 0.1;

参数取值我有几条经验：步长取环境最小自由通道宽度的三分之一左右最稳，如果不知道最小通道宽度，就先设一个中间值跑几次，观察扩展失败率再调整。连接阈值一般设为步长的1到1.5倍，太大会产生穿越障碍物边的假连接，太小则两棵树很难碰头。

3.2 核心数据结构设计

在Matlab中，我用结构体保存一棵树的全部信息。节点坐标放在nodes矩阵中，每个节点的父节点索引用parents数组保存。

% 树节点结构 % tree.nodes: N×2 坐标矩阵 % tree.parents: N×1 父节点索引，根节点为0 tree1 = struct('nodes', startPos, 'parents', 0); tree2 = struct('nodes', goalPos, 'parents', 0);

结构体直观好用，配合Matlab矩阵运算也比较方便。缺点是大量节点遍历时速度一般，但普通二维规划场景里生成几千个节点完全没问题，不需要提前优化。

3.3 主函数框架

主函数负责把各个功能模块串起来。迭代逻辑非常清晰，每次循环扩展树1，然后尝试与树2连接，连接失败则交换角色。

function [path, tree1, tree2] = biRRTMain(obstacles, startPos, goalPos, param) % 初始化两棵树 tree1.nodes = startPos; tree1.parents = 0; tree2.nodes = goalPos; tree2.parents = 0; path = []; % 迭代搜索 for iter = 1:param.maxIter % 扩展树1 [tree1, newNode1, success1] = extendTree(tree1, obstacles, param); if success1 % 尝试与树2连接 [tree1, tree2, connected] = tryConnectTrees(tree1, tree2, obstacles, param); end if connected path = extractPath(tree1, tree2); break; end % 交换两棵树角色 temp = tree1; tree1 = tree2; tree2 = temp; end end

这里有一个关键的设计技巧：每轮迭代末尾交换两棵树。这样下一轮“扩展树1”实际上操作的是原来的树2，两棵树实现了天然交替扩展，不用写两套重复代码。对称写法简洁，也方便后续改成RRT*等变体。

3.4 核心模块函数拆解

3.4.1 随机采样与目标偏置

采样模块可以用一个简单的均匀采样实现，再加一点目标偏置来提高效率。

function point = sampleFree(map, param, goalPos) % 默认均匀随机采样 if rand < param.sampleBias point = goalPos; else point = [map.limit(1) + rand*(map.limit(2)-map.limit(1)), ... map.limit(3) + rand*(map.limit(4)-map.limit(3))]; end end

目标偏置的作用是让树偶尔直接向目标方向生长，避免完全失去方向的随机游荡。但注意不要设太大，0.1左右已经足够。有次我设到0.3，结果树在一个U型障碍物前反复向目标方向冲，反而一直找不到绕行路径。

3.4.2 最近邻搜索与节点扩展

最近邻搜索计算出当前树上所有节点到采样点的距离平方，返回最近节点索引。

function [nearestNode, nearestIdx] = findNearest(tree, point) diff = tree.nodes - point; distSq = sum(diff.^2, 2); [~, nearestIdx] = min(distSq); nearestNode = tree.nodes(nearestIdx, :); end

扩展函数从最近节点出发，沿着指向采样点的方向迈出一个步长。

function newNode = extendNode(nearestNode, point, stepSize) direction = point - nearestNode; dist = norm(direction); if dist < stepSize newNode = point; else direction = direction / dist; newNode = nearestNode + direction * stepSize; end end

当采样点距离最近节点小于步长时，新节点直接取采样点本身。这个逻辑让树在接近目标时能更快贴合目标区域。

3.4.3 碰撞检测函数的实现细节

碰撞检测是整个算法正确性的根基。我用线段离散化采样加多点包含测试的方式实现。

function collision = isCollisionFree(obstacles, p1, p2, resolution) collision = false; segmentLength = norm(p2 - p1); nSamples = max(2, ceil(segmentLength / resolution)); for i = 0:nSamples ratio = i / nSamples; point = p1 + (p2 - p1) * ratio; for k = 1:length(obstacles) if isInsideObs(point, obstacles(k)) collision = true; return; end end end end function inside = isInsideObs(point, obs) if strcmp(obs.type, 'circle') inside = norm(point - obs.data(1:2)) < obs.data(3); elseif strcmp(obs.type, 'rect') inside = point(1) > obs.data(1) && point(1) < obs.data(2) && ... point(2) > obs.data(3) && point(2) < obs.data(4); end end

碰撞检测有一个非常关键的细节：机器人不是质点，如果直接用几何边界判断，规划出的路径虽然不与障碍物中心相交，但机器人本体可能刮蹭。工程上通用做法是把所有障碍物膨胀一个安全半径。膨胀操作本质上就是把圆的半径加大，把矩形的四边外扩，这样规划出来的路径天然留出安全余量。

另一个经验是碰撞检测的采样分辨率要够细。我之前用0.2米的间距检测，结果一个直径0.3米的细柱子被跳过去了，导致线路直接穿过柱子。后来把间距改成0.05到0.1米，问题就消失了。

3.4.4 两棵树连接与合并策略

连接实现是Bi-RRT的题眼。当树1扩展出新节点后，立即寻找树2上离它最近的节点，判断距离与碰撞条件。

function [tree1, tree2, connected] = tryConnectTrees(tree1, tree2, obstacles, param) connected = false; % 树1最后一个节点作为当前连接点 newNode = tree1.nodes(end, :); % 在树2中找最近节点 [nearestOther, idxOther] = findNearest(tree2, newNode); % 距离判断和无碰撞判断 dist = norm(nearestOther - newNode); if dist < param.connectDist && isCollisionFree(obstacles, newNode, nearestOther, 0.05) % 将树2翻转并接入树1 [tree1, tree2] = mergeTrees(tree1, tree2, idxOther); connected = true; end end

合并树的具体操作比较微妙。假设树2的根在终点方向，它的父节点链是从终点回溯到某个中间节点。合并时要把树2的父节点反转，让它变成从连接点指向终点。这个反转操作如果做错，提取的路径会出现局部回环。

3.4.5 路径提取与回溯

路径提取需要仔细处理两棵树的拼接顺序。从树1的连接节点向上回溯可以得到起点到连接点的路径，从树2的连接节点向上回溯可以得到终点到连接点的路径，但后者需要反转。

function path = extractPath(tree1, tree2) % 从树1连接节点回溯到起点 path1 = []; idx = tree1.parents(end); while idx > 0 path1 = [tree1.nodes(idx, :); path1]; idx = tree1.parents(idx); end path1 = [tree1.nodes(1, :); path1]; % 从树2连接节点回溯到终点，再反转顺序 path2 = []; idx = tree2.parents(end); while idx > 0 path2 = [tree2.nodes(idx, :); path2]; idx = tree2.parents(idx); end path2 = [tree2.nodes(1, :); path2]; % 拼接完整路径 path = [path1; flipud(path2)]; end

提取路径时最容易犯的错误是忘记反转树2段。我第一次没有反转，结果路径后半段从终点往连接点方向走，看起来就是一个掉头路径。

3.5 可视化与动画调试

没有可视化，纯看坐标数据很难定位问题。我把障碍物、两棵树和最终路径绘制在同一个坐标系里。

function plotResult(map, obstacles, tree1, tree2, path) figure; hold on; axis equal; axis(map.limit); % 绘制障碍物 for k = 1:length(obstacles) obs = obstacles(k); if strcmp(obs.type, 'circle') theta = linspace(0, 2*pi, 100); fill(obs.data(1) + obs.data(3)*cos(theta), ... obs.data(2) + obs.data(3)*sin(theta), 'k'); elseif strcmp(obs.type, 'rect') xv = [obs.data(1), obs.data(2), obs.data(2), obs.data(1)]; yv = [obs.data(3), obs.data(3), obs.data(4), obs.data(4)]; fill(xv, yv, 'k'); end end % 绘制两棵树：红色起点树，蓝色终点树 plot(tree1.nodes(:,1), tree1.nodes(:,2), 'r.'); plot(tree2.nodes(:,1), tree2.nodes(:,2), 'b.'); % 绘制最终路径 if ~isempty(path) plot(path(:,1), path(:,2), 'g-', 'LineWidth', 2); end end

调试时，我习惯加上逐帧绘制加drawnow，让树的生长过程动画化。通过动画能非常直观地看到树在哪个区域扩展缓慢、在哪个通道反复尝试失败，从而快速定位参数或逻辑问题。这个习惯帮我在调试复杂障碍场景时节省了大量时间。

4. 仿真测试与结果分析

代码写完只是第一步，验证算法能否在不同场景下稳定工作才是重点。我做了几个不同难度的测试。

4.1 空旷环境下的基准测试

第一个场景几乎不放障碍物，只在中间放一个很小的圆形障碍物。这个场景的目的是验证算法基本流程是否走通。在这个环境里，两棵树几乎在几十次迭代内就连接成功了，路径近似直线，但带有极小锯齿。由于RRT的随机性，路径不可能完全平直，这属于正常现象。

这个测试暴露出一个重要特性：Bi-RRT找到的第一条路径通常不是最短路径。它追求的是“快速找到一条可用路径”，而不是全局最优路径。如果业务需求对路径长度敏感，后续必须叠加平滑和优化。

4.2 复杂障碍场景测试

第二个场景设置了更多障碍物，包括三个矩形和两个圆形障碍物，构成类似S形通道的地图。起点和终点之间没有直接直线通路，任何路径都必须绕开障碍物。

实际运行中，两棵树分别从起点和终点向通道内部扩展，最终在S型通道的某个位置相遇。由于通道较窄，树在这里扩展成功率明显下降，会出现多次采样点打在通道壁上的情况。但整体迭代次数仍然远低于单向RRT。我统计了节点数和耗时，Bi-RRT在本次场景中平均扩展节点数量约为单向RRT的一半左右。

4.3 性能指标对比

为了量化效果，我在相同环境下分别跑了单向RRT和Bi-RRT各10次，取平均做了对比：

这些数据显示，Bi-RRT在节点数和迭代次数上都接近减半，搜索时间也大幅缩短。需要说明的是，这是在没有加任何高级加速技巧的初始版代码上统计的，如果再加KD-Tree索引，时间缩短会更明显。

4.4 参数调优的实验心得

调参是工程绕不开的活，以下几点完全来自实测感受：

• 步长太大会导致新节点频繁碰撞障碍物，表现为很多迭代轮次都扩展失败，树几乎不增长。
• 步长太小则树增长缓慢，覆盖同样区域需要更多迭代，总时间上升。
• 目标偏置概率超过0.2，树的全局探索能力会降低，遇到复杂障碍物时很容易反复撞墙。
• 连接阈值设置比步长小时，两棵树几乎没有机会连接，迭代次数跑到上限仍然无路径。
• 路径质量差时，最有效的手段不是修改算法，而是加后处理流程。

这些参数互相耦合，单独调整一个不一定能解决问题。我的做法是先用一组初始参数跑通，再按需小步调整，每次只改一个参数，观察动画对比效果。这样能很清晰地知道每个参数的具体影响力。

5. 常见问题排查与调试技巧实录

这一节完全是实战层面的总结。这些问题我基本都碰到过，有些当时还困扰了我好几天。

5.1 树扩展成功率太低

一种常见现象是：循环跑了很多轮，但树上节点增长非常缓慢，可视化中只见起点附近零星几个点。

排查先从碰撞检测入手，确认步长没有超过最小通道宽度。其次是检查障碍物绘制时是否已经加上了安全膨胀半径，如果膨胀过大，自由空间会被大大压缩。还有一种坑是采样范围写错，采样点落在障碍物内部或者地图边界之外，树的生长自然受阻。

解决方案是动态调小步长，或者在不希望深入探索的区域增加禁止采样的逻辑。另外，可以在代码中临时统计成功扩展率，如果成功率低于30%，基本说明参数和环境严重不匹配。

5.2 两棵树无法相遇

现象是两棵树各自长得很茂盛，但始终碰不到一起，中间留了一小段空隙，迭代次数已经接近上限。

这个问题最常见的根源是连接阈值太小，或者连接尝试执行得不够频繁。我的解决方法是把连接检查放在每次扩展成功之后立刻执行，不要等下一轮再处理。同时，把连接阈值适当放宽，从步长的1倍调成1.5倍，往往就能把最后一段缝隙补上。

更隐蔽的一种情况是，两棵树在空间上非常接近，但中间隔了一个障碍物，距离阈值通过了，碰撞检测却一直失败。这时可以在连接检测时加入渐进式连接逻辑，从一端的节点出发，以小步长逐步向另一端的节点推进，观察推进过程中每个中间点是否无碰撞。这样能有效判断“虽然直线连不上，但经过一个小折线是否可行”。

5.3 路径出现尖角和大量锯齿

RRT系列的通病，就是路径点序列不平滑，因为树是随机生长的。路径中的转折可能非常突兀，不利于机器人执行。

我通常分两步处理。第一步做短路处理，从起点开始，尝试将当前点与更远的后续点直接连接，如果连线无碰撞就删除中间节点。反复进行，直到没有可删除的冗余节点。第二步做曲线拟合，把短路后的路径点用B样条等参数曲线拟合，生成曲率连续的平滑轨迹。拟合后的曲线仍然要做碰撞检测，防止曲线部分穿入障碍物。

5.4 碰撞检测漏检与误检

漏检的典型原因是离散采样间距太粗，细长障碍物刚好被相邻采样点跳过。误检的典型原因则是用质点检测，机器人本体尺寸没有被考虑进去，导致路径离障碍物太近，实际执行时刮蹭。

解决思路是对障碍物做膨胀处理，给每个障碍物增加机器人半径大小的膨胀距离，同时碰撞检测的离散间距要小于膨胀距离的一半。这样既能避免漏检，也保证了路径留有余量。

5.5 性能瓶颈与加速技巧

当树节点数量达到几千甚至上万，最近邻搜索和碰撞检测都会成为性能瓶颈。我最初实现的Matlab版本平均规划时间4到5秒，做了两处优化后降到1.5秒左右。

第一处优化是把最近邻搜索从线性遍历换成KD-Tree或者空间栅格索引。Matlab里可以用自带的索引函数，也可以自己写简单的网格哈希。第二处优化是给障碍物建立包围盒，碰撞检测时先粗筛掉明显不相交的障碍物，再做精细求交。这两处优化在障碍物数量多时收益尤其明显。

如果追求极限速度，可以把核心循环用C写MEX函数编译，这样速度还能提升一个数量级。但对于二维稀疏环境的仿真需求，Matlab纯脚本通常已经足够。

5.6 问题排查速查表

调试时，我习惯对照一张速查表快速定位问题：

调试最依赖的手段还是可视化。每次改动后先跑一版看动画，快速定位问题区域，再针对性地看字段数据。只看打印信息很难还原树的生长过程，效率低得多。

6. 后续扩展与实际应用思考

Bi-RRT在我实际工作中的使用频率很高，尤其是需要“快速得到一条可行路径”做进一步优化时。它最大的优点是找路快、实现简单、扩展性强。这篇文章只是基础版，实际产品里我们通常会做很多增强。

在我这个项目的后续版本里，已经加入了运动学约束扩展。具体做法是扩展新节点时不再单纯沿直线走一步，而是检查候选方向是否满足机器人的最小转弯半径或速度约束，如果不满足就调整方向。这个改动让规划出的路径可以直接送给低层控制器执行，省去了大量后处理。它的核心思路并不复杂，但效果提升明显。

另一个扩展方向是RRT*。它在Bi-RRT的基础上增加了重连和优化步骤，能够在迭代过程中不断优化路径代价，最终逼近最优路径。代价是每次迭代的开销增加。对于时效性要求不高、但路径质量要求高的场景，可以优先考虑RRT*。以后我打算单独写一篇RRT*的实现对比，把两者的收敛速度和路径代价差异直观展示出来。

最后分享一个我做过多版规划器后总结出的习惯：实现任何采样规划算法时，一定把碰撞检测、最近邻搜索、节点扩展这三个模块完全解耦，写成独立函数。这样无论后续替换成RRT*、RRT-Connect还是其他变体，主流程只需要改很少的代码，扩展和维护成本明显降低。这一点在实验阶段省了我大量时间。

如果你正准备在自己的项目里引入Bi-RRT，或者已经在调试但遇到瓶颈，希望这篇内容能帮你把路径规划流程跑通，少踩几个坑。规划算法这条路水很深，但核心逻辑通了之后，剩下的就是针对自己的机器人模型做适配优化，慢慢把效果磨到可用的状态。