双向rrt树路径规划MATLAB实现 双向rrt算法的三维路径规划 加入路径平滑处理 代码有详细注释

双向rrt树路径规划MATLAB实现 双向rrt算法的三维路径规划 加入路径平滑处理 代码有详细注释，模块化编程

在机器人路径规划领域，双向RRT算法就像是个左右开弓的探路者。今天咱们用MATLAB来实现这个三维版本的路径规划，顺便加点"丝滑"特效——路径平滑处理直接安排上。

老规矩先搭个三维舞台。咱们用20x20x20的立方体空间，随机撒点球状障碍物（代码里我管它们叫蘑菇云）：

function map = create3DMap() map.gridSize = [20,20,20]; map.obstacles = []; for i =1:randi([5,8]) % 随机生成5-8个障碍 center = 18*rand(1,3)+1; radius = 1.5 + rand()*1.2; map.obstacles = [map.obstacles; center radius]; end end

这里每个障碍物用四元组[x,y,z,r]表示，最后那个r是半径。随机生成让每次运行都有新花样，调试的时候记得关掉随机种子。

接下来是重头戏——双向RRT的核心实现。咱们把两棵树分别叫A树和B树，轮流生长：

function [path] = bidirectionalRRT(start, goal, map) treeA.nodes(1) = struct('pos',start, 'parent',0); % 起点树 treeB.nodes(1) = struct('pos',goal, 'parent',0); % 终点树 for iter = 1:5000 % 最多迭代5000次 % 交替扩展方向 [newNode, nearNode] = extendTree(treeA, map); if ~isempty(newNode) treeA.nodes(end+1) = newNode; % 尝试连接另一棵树 [connected, meetNode] = tryConnect(treeB, newNode.pos); if connected path = reconstructPath(treeA, treeB, meetNode); return; end end % 交换两棵树角色 [treeA, treeB] = deal(treeB, treeA); end error('Path not found'); end

注意第14行的灵魂操作——交换两棵树身份，这相当于让两棵树轮流当"进攻方"，比单向RRT效率高出一大截。

说到碰撞检测，这是路径规划里的安检员。咱们用线段离散采样法：

function collision = checkCollision(p1, p2, map) numSteps = ceil(norm(p2-p1)/0.5); % 每0.5单位采样一次 t = linspace(0,1,numSteps); points = p1'*(1-t) + p2'*t; % 线性插值 for i =1:size(points,2) pt = points(:,i)'; for o=1:size(map.obstacles,1) obs = map.obstacles(o,:); if norm(pt - obs(1:3)) < obs(4) collision = true; return; end end end collision = false; end

这种离散检测虽然有点暴力，但在三维空间里比数学解析法省事多了。不过要注意步长别太大，否则可能漏检。

双向rrt树路径规划MATLAB实现 双向rrt算法的三维路径规划 加入路径平滑处理 代码有详细注释，模块化编程

最后给路径做个马杀鸡——三次样条平滑：

function smoothPath = pathSmoothing(rawPath, map) % 提取关键点 keyPoints = rawPath(1:ceil(end/5):end, :); % 三次样条插值 curve = cscvn(keyPoints'); smoothPath = fnval(curve, linspace(0,curve.breaks(end),100))'; % 碰撞复查 for i=2:size(smoothPath,1) if checkCollision(smoothPath(i-1,:), smoothPath(i,:), map) smoothPath = []; % 平滑失败返回原路径 return; end end end

这里先用1/5步长抽取关键点，再用MATLAB自带的cscvn做插值。注意第11行的安全复查，确保平滑后的路径不会穿墙。

跑起来的效果就像无人机在障碍间优雅地画曲线，而不是机械式的折线运动。不过要提醒的是，实际工程中得根据机器人运动学约束调整平滑参数。

整个项目我坚持模块化设计，每个功能块都可以单独调试。比如把checkCollision函数单独拿出来，就能可视化障碍物检测过程。这种写法虽然看起来代码量多了点，但出bug时真能救命——你绝对不想在200行的巨型函数里找那个该死的逻辑错误吧？

下次如果想让路径更丝滑，可以试试B样条或者贝塞尔曲线。对了，代码里我埋了个彩蛋：按Ctrl+R切换2D/3D显示模式，方便调试时观察路径走向。