四种RRT算法三维机械臂避障 只做球体障碍物 matlab机械臂路径规划仿真 《基于改进RRT算法的六自由度六自由度机械臂避障路径规划研究》第四章，第五章中三维空间机械臂避障。 基本与文中效果对应。

机械臂在三维空间玩躲猫猫有多难？RRT算法家族表示这题能解。咱们今天不整虚的，直接上Matlab撸代码，用球体障碍物当陪练，看看RRT、RRT-Connect、RRT、Informed RRT四个兄弟谁更会绕路。

先给机械臂整个活——D-H参数建模不能少。拿6自由度机械臂为例，用Matlab的robotics toolbox五分钟搭个架子：

L1 = Link('d', 0.2, 'a', 0, 'alpha', pi/2);
L2 = Link('d', 0, 'a', 0.5, 'alpha', 0);
% ...省略中间4个连杆参数
arm = SerialRobot([L1 L2 L3 L4 L5 L6], 'name', '六轴机械臂');

障碍物生成简单粗暴，直接撒一堆球体。碰撞检测用距离判断就行，但要注意连杆扫过的空间：

function collision = checkCollision(q_new, obstacles)
    [pos, ~] = arm.fkine(q_new); % 正向运动学
    for i = 1:size(obstacles,1)
        if norm(pos(1:3) - obstacles(i,1:3)) < obstacles(i,4)*1.2
            collision = true;
            return;
        end
    end
    collision = false;
end

经典RRT先上阵，效果就像喝醉的扫地机器人。核心代码就三个步骤：撒点、找邻居、长树枝：

while iter < max_iter
    q_rand = rand(1,6)*2*pi - pi; % 6维随机位姿
    [q_near, idx] = findNearest(q_rand, tree);
    q_new = steer(q_near, q_rand, step_size);
    
    if ~checkEdge(q_near, q_new, obstacles)
        tree.addEdge(q_near, q_new);
        if norm(q_new - q_goal) < goal_threshold
            path = extractPath(tree);
            break;
        end
    end
end

这算法在三维空间里容易走成贪吃蛇，路径弯弯绕绕不说，成功率还看脸。这时候RRT-Connect掏出双枪——前后两棵树对着长：

tree_start.init(q_start);
tree_goal.init(q_goal);

while iter < max_iter
    if connect(tree_start, tree_goal)  % 双向奔赴
        path = mergePaths();
        return;
    end
    [tree_start, tree_goal] = swapTrees(tree_start, tree_goal); % 交换生长方向
end

实测发现这货比单棵树快三倍，但生成路径还是像心电图。于是RRT*带着优化光环登场，关键在rewire操作：

function tree = rewire(tree, q_new)
    neighbors = findNearNeighbors(q_new, tree);
    for q_near = neighbors
        new_cost = cost(q_new) + norm(q_new - q_near);
        if new_cost < cost(q_near)
            tree.setParent(q_near, q_new); % 换爹操作
        end
    end
end

这波操作让路径逐渐收敛到最优，代价是每次迭代都要遍历邻居节点。最后出场的Informed RRT*掏出椭圆采样大杀器：

if current_best < inf
    c_min = norm(q_start - q_goal);
    C = rotationToEllipse(c_min, current_best); % 生成椭圆采样区域
    q_rand = sampleEllipse(C); % 椭圆内采样
end

实测这个版本在复杂障碍环境里收敛速度提升明显，特别是当初始路径找到后，采样范围从整个空间缩到椭圆区域，相当于给算法开了导航。

跑完四个算法，拿同一组障碍物测试（图1）。RRT花5秒找到一条扭成麻花的路径；RRT-Connect 2秒找到差不多的麻花；RRT 15秒磨出相对顺滑的路线；Informed RRT前10秒和RRT*一样慢，但15秒后突然开窍，路径长度缩短20%。

最后奉上避坑指南：

1. 碰撞检测别直接用位姿点，要检查连杆轨迹（代码里偷懒了）
2. 6维空间里步长别超过0.2弧度，否则容易穿模
3. Informed RRT*的椭圆旋转矩阵计算容易翻车，建议用四元数转换

Matlab仿真有个隐藏彩蛋——用animatedline函数实时显示搜索过程，看着树枝在障碍之间钻来钻去，比看进度条有意思多了。源码已扔GitHub，调参时记得备好枸杞，三维路径规划这玩意，玄学起来能让你怀疑人生。