3D场景下RRT、RRT*算法、实现绕过两个圆柱障碍物到达目标点，输出路径图和路径点与障碍物最小距离变化图，可用于移动机器人。 也可用于机械臂做参考 RRT.m标准随机树 RRTstar.m改进随机数 distanceCost.m节点距离计算函数 cylinder3.m画圆柱函数 文件列表： RRT.m RRTstar.m cylinder3.m distanceCost.m

路径规划里绕障碍物是个挺烦人的事儿，尤其机械臂和移动机器人这种三维空间作业的。今天咱们直接上代码拆解RRT和RRT*这对兄弟算法的实战操作，手把手看它们怎么在双圆柱障碍物中间杀出血路。

先看环境搭建。用cylinder3.m画障碍物特别方便，这函数底层用mesh网格生成圆柱坐标系。比如设置第一个障碍物参数：

cylinder3([2 3 0], 1.5, 4, 'r', 1);

这里[2 3 0]是圆柱底面中心坐标，1.5是半径，4是高度。红色圆柱体的alpha通道设为0.3实现半透明，碰撞检测时能清晰看到穿帮情况。

RRT.m的核心在节点扩展循环。每次随机采样时有个20%的概率瞄准目标点：

if rand() < 0.2
    sample = goal;
else
    sample = rand(1,3)*diag(space) + space(:,1)';
end

这种偏向性采样能显著加速收敛。找到最近节点后，用distanceCost.m计算两点间欧氏距离，同时作为启发函数评估路径代价。

碰撞检测是三维场景的灵魂。在扩展新节点时，需要沿着移动方向做线性插值检测：

for t = 0:stepSize/dist:1
    point = newNode.position * t + nearestNode.position * (1-t);
    if collisionDetect(point, obstacles)
        break;
    end
end

这里的stepSize控制路径分辨率，t从0到1逐步检查线段上的每个点是否与障碍物相交。圆柱体的碰撞判断用点到中心轴的距离公式：

RRT*.m的优化在于rewire操作。当新节点加入后，会遍历附近节点看能否通过新节点获得更短路径：

nearIndices = findNodesWithinRadius(tree, newNode, searchRadius);

for i = 1:length(nearIndices)

nearNode = tree.nodes(nearIndices(i));

newCost = newNode.cost + distanceCost(newNode, nearNode);

3D场景下RRT、RRT*算法、实现绕过两个圆柱障碍物到达目标点，输出路径图和路径点与障碍物最小距离变化图，可用于移动机器人。 也可用于机械臂做参考 RRT.m标准随机树 RRTstar.m改进随机数 distanceCost.m节点距离计算函数 cylinder3.m画圆柱函数 文件列表： RRT.m RRTstar.m cylinder3.m distanceCost.m

if newCost < nearNode.cost && ~collisionAlongPath(newNode, nearNode)

tree = changeParent(tree, nearIndices(i), newNode);

end

end

这段代码实现动态调整树结构，搜索半径随节点数增加而缩小，平衡计算效率与优化效果。

路径点安全距离监控用逐点计算到两个圆柱的水平投影距离：

d1 = sqrt((path(:,1)-2).^2 + (path(:,2)-3).^2) - 1.5;

d2 = sqrt((path(:,1)-8).^2 + (path(:,2)-6).^2) - 1.2;

min_dist = min([d1, d2], [], 2);

取两个距离中的最小值生成变化曲线，能直观看到路径是否擦着障碍物边缘走。

实际跑出来的路径图中，RRT的路径像喝醉的蛇，弯弯绕绕还贴着障碍物走。RRT*则明显平滑许多，在第二个圆柱外围画了个优雅的弧线。最小距离曲线显示RRT*路径离障碍物最近处保持0.3米以上，而RRT有段距离直接掉到0.1米，这对实际机器人来说容易触发安全警报。

文件里的distanceCost.m虽然只有两行，但三维距离计算千万别手滑写成二维：

function d = distanceCost(a,b)

d = sqrt((a(1)-b(1))^2 + (a(2)-b(2))^2 + (a(3)-b(3))^2);

end

曾经有兄弟在机械臂项目里漏算Z轴，结果路径直接穿模而过，调了三天才发现是距离函数挖的坑。

最后说个实战经验：在机械臂应用时，记得把关节空间映射到三维工作空间，同时约束采样区域。比如圆柱障碍物的高度如果超过机械臂活动范围，可以在Z轴方向缩小采样空间提升效率。