MATLAB实现3自由度机械臂RRT路径规划

1. 项目概述：机械臂路径规划的核心挑战

机械臂路径规划是机器人学中最经典也最实际的问题之一。想象一下，当你需要让机械臂在充满障碍物的环境中从A点移动到B点时，如何确保它不会撞到任何东西？这就是路径规划要解决的核心问题。对于3自由度机械臂而言，虽然比工业上常见的6自由度机械臂简单，但在存在圆形障碍物的环境中规划无碰撞路径仍然充满挑战。

RRT（快速扩展随机树）算法因其在高维空间中的优异表现，成为解决这类问题的首选方案。与传统的A*或Dijkstra算法不同，RRT不需要预先构建完整的环境地图，而是通过随机采样和树形扩展来探索可行路径。这种特性使其特别适合机械臂这种高维配置空间的路径规划问题。

提示：3自由度机械臂的配置空间已经是3维的，如果考虑每个关节的角度限制，规划空间实际上是一个复杂的多面体。圆形障碍物在任务空间中是简单的圆形，但在配置空间中可能变成极其复杂的形状。

2. RRT算法原理深度解析

2.1 RRT基本工作流程

RRT算法的核心思想可以用"随机撒点，逐步连接"来概括。具体步骤如下：

1. 初始化树结构，将起始配置（初始关节角度）作为树的根节点
2. 在配置空间中随机采样一个点q_rand
3. 在现有树中找到距离q_rand最近的节点q_near
4. 从q_near向q_rand方向扩展一小步，得到新节点q_new
5. 检查q_near到q_new的路径是否与障碍物碰撞
6. 若无碰撞，则将q_new加入树中，否则舍弃这个扩展
7. 重复步骤2-6，直到q_new进入目标区域或达到最大迭代次数

matlab复制% RRT算法伪代码示例
function path = RRT(start, goal, obstacles, max_iter)
    tree = initializeTree(start);
    for i = 1:max_iter
        q_rand = randomSample();
        q_near = findNearestNeighbor(tree, q_rand);
        q_new = extend(q_near, q_rand, step_size);
        if ~collisionCheck(q_near, q_new, obstacles)
            addNode(tree, q_new);
            addEdge(tree, q_near, q_new);
            if reachGoal(q_new, goal)
                path = extractPath(tree, start, q_new);
                return;
            end
        end
    end
    path = []; % 未找到路径
end

2.2 机械臂运动学与碰撞检测

对于3自由度机械臂，我们需要正运动学模型将关节角度转换为末端执行器位置：

matlab复制function [pos, links] = forwardKinematics(theta)
    % theta: [θ1, θ2, θ3] 关节角度向量
    L1 = 1.0; % 第一段臂长
    L2 = 0.8; % 第二段臂长
    L3 = 0.6; % 第三段臂长
    
    % 第一关节到基座坐标系
    T01 = [cos(theta(1)) -sin(theta(1)) 0 0;
           sin(theta(1))  cos(theta(1)) 0 0;
           0              0             1 0;
           0              0             0 1];
    
    % 第二关节到第一关节
    T12 = [cos(theta(2)) -sin(theta(2)) 0 L1;
           sin(theta(2))  cos(theta(2)) 0 0;
           0              0             1 0;
           0              0             0 1];
    
    % 第三关节到第二关节
    T23 = [cos(theta(3)) -sin(theta(3)) 0 L2;
           sin(theta(3))  cos(theta(3)) 0 0;
           0              0             1 0;
           0              0             0 1];
    
    % 末端到第三关节
    T3E = [1 0 0 L3;
           0 1 0 0;
           0 0 1 0;
           0 0 0 1];
    
    T02 = T01 * T12;
    T03 = T02 * T23;
    T0E = T03 * T3E;
    
    % 返回末端位置和各关节位置用于碰撞检测
    pos = T0E(1:3,4)';
    links = [0 0 0;
             T01(1:3,4)';
             T02(1:3,4)';
             T03(1:3,4)';
             T0E(1:3,4)'];
end

碰撞检测需要考虑机械臂连杆与环境中圆形障碍物的交互。简化方法是把机械臂连杆视为线段，检测线段与圆是否相交：

matlab复制function collision = collisionCheck(q1, q2, obstacles)
    [~, links1] = forwardKinematics(q1);
    [~, links2] = forwardKinematics(q2);
    
    % 检查每个连杆段
    for i = 1:size(links1,1)-1
        p1 = links1(i,:);
        p2 = links1(i+1,:);
        p3 = links2(i,:);
        p4 = links2(i+1,:);
        
        % 线性插值中间点
        for t = 0:0.1:1
            pt = (1-t)*p1 + t*p3;
            pt_next = (1-t)*p2 + t*p4;
            
            % 检查与每个障碍物的碰撞
            for j = 1:size(obstacles,1)
                center = obstacles(j,1:2);
                radius = obstacles(j,3);
                if lineCircleCollision(pt, pt_next, center, radius)
                    collision = true;
                    return;
                end
            end
        end
    end
    collision = false;
end

function collides = lineCircleCollision(p1, p2, center, radius)
    % 线段到圆心的距离计算
    d = abs((p2(2)-p1(2))*center(1) - (p2(1)-p1(1))*center(2) + p2(1)*p1(2) - p2(2)*p1(1)) ...
        / norm(p2-p1);
    collides = (d <= radius);
end

3. MATLAB实现详解

3.1 主程序框架设计

完整的RRT路径规划MATLAB实现包含以下几个关键模块：

matlab复制% 主程序框架
clear; clc;

% 1. 参数设置
start = [0, 0, 0]; % 初始关节角度 [θ1,θ2,θ3] (rad)
goal = [pi/2, pi/3, -pi/4]; % 目标关节角度
obstacles = [1.2 0.8 0.5;  % [x,y,radius]
             0.5 -0.7 0.3;
             -0.8 0.3 0.4]; % 障碍物列表

% 2. RRT参数
max_iter = 5000;   % 最大迭代次数
step_size = 0.1;   % 扩展步长(rad)
goal_bias = 0.1;   % 目标偏向概率
goal_radius = 0.2; % 目标区域半径(rad)

% 3. 运行RRT算法
[path, tree] = RRT_Planner(start, goal, obstacles, max_iter, step_size, goal_bias, goal_radius);

% 4. 可视化结果
visualizeRRT(start, goal, obstacles, tree, path);

3.2 RRT核心算法实现

matlab复制function [path, tree] = RRT_Planner(start, goal, obstacles, max_iter, step_size, goal_bias, goal_radius)
    % 初始化树结构
    tree.nodes = start;
    tree.edges = [];
    tree.parent = 0;
    
    % 开始RRT扩展
    for iter = 1:max_iter
        % 随机采样，带有目标偏向
        if rand < goal_bias
            q_rand = goal;
        else
            q_rand = [2*pi*rand()-pi, pi*rand()-pi/2, pi*rand()-pi/2]; % 各关节角度范围
        end
        
        % 寻找最近邻节点
        [q_near, idx] = findNearestNeighbor(tree.nodes, q_rand);
        
        % 向随机点方向扩展
        q_new = extendConfiguration(q_near, q_rand, step_size);
        
        % 碰撞检测
        if ~checkArmCollision(q_near, q_new, obstacles)
            % 添加到树中
            new_node_idx = size(tree.nodes,1)+1;
            tree.nodes = [tree.nodes; q_new];
            tree.edges = [tree.edges; idx new_node_idx];
            tree.parent = [tree.parent; idx];
            
            % 检查是否到达目标区域
            if norm(q_new - goal) < goal_radius
                % 回溯路径
                path = reconstructPath(tree, new_node_idx);
                return;
            end
        end
    end
    
    % 未找到路径
    path = [];
    disp('未能找到路径！');
end

function [q_near, idx] = findNearestNeighbor(nodes, q)
    distances = sum((nodes - q).^2, 2);
    [~, idx] = min(distances);
    q_near = nodes(idx,:);
end

function q_new = extendConfiguration(q_near, q_rand, step_size)
    direction = q_rand - q_near;
    distance = norm(direction);
    if distance <= step_size
        q_new = q_rand;
    else
        q_new = q_near + (direction/distance)*step_size;
    end
end

function path = reconstructPath(tree, node_idx)
    path = [];
    while node_idx ~= 0
        path = [tree.nodes(node_idx,:); path];
        node_idx = tree.parent(node_idx);
    end
end

3.3 可视化实现

可视化是调试和展示结果的关键环节：

matlab复制function visualizeRRT(start, goal, obstacles, tree, path)
    figure; hold on; axis equal; grid on;
    xlabel('X'); ylabel('Y'); zlabel('Z');
    title('3自由度机械臂RRT路径规划');
    
    % 绘制障碍物
    for i = 1:size(obstacles,1)
        [x,y,z] = sphere;
        surf(obstacles(i,1)+obstacles(i,3)*x, ...
             obstacles(i,2)+obstacles(i,3)*y, ...
             0.1*obstacles(i,3)*z, 'FaceAlpha',0.3, 'EdgeColor','none');
    end
    
    % 绘制RRT树
    for i = 1:size(tree.edges,1)
        q1 = tree.nodes(tree.edges(i,1),:);
        q2 = tree.nodes(tree.edges(i,2),:);
        [~, links1] = forwardKinematics(q1);
        [~, links2] = forwardKinematics(q2);
        plot3([links1(:,1); links2(:,1)], ...
              [links1(:,2); links2(:,2)], ...
              [links1(:,3); links2(:,3)], 'b', 'LineWidth',0.5);
    end
    
    % 绘制起始和目标配置
    drawArmConfiguration(start, 'g', 3);
    drawArmConfiguration(goal, 'r', 3);
    
    % 绘制找到的路径
    if ~isempty(path)
        for i = 1:size(path,1)
            drawArmConfiguration(path(i,:), 'm', 2);
            if i > 1
                [~, links_prev] = forwardKinematics(path(i-1,:));
                [~, links_curr] = forwardKinematics(path(i,:));
                plot3([links_prev(:,1); links_curr(:,1)], ...
                      [links_prev(:,2); links_curr(:,2)], ...
                      [links_prev(:,3); links_curr(:,3)], 'm', 'LineWidth',2);
            end
        end
    end
end

function drawArmConfiguration(q, color, linewidth)
    [~, links] = forwardKinematics(q);
    plot3(links(:,1), links(:,2), links(:,3), 'Color',color, 'LineWidth',linewidth);
    plot3(links(:,1), links(:,2), links(:,3), 'o', 'Color',color, 'MarkerSize',6, 'MarkerFaceColor',color);
end

4. 算法优化与性能提升

4.1 RRT*：渐进最优的改进

基本RRT算法找到的路径通常不是最优的。RRT*通过引入"重布线"和"重选父节点"机制，可以渐进地优化路径：

matlab复制% RRT*的核心改进部分
function tree = rewireRRTStar(tree, q_new, new_node_idx, obstacles, radius)
    % 在给定半径内寻找邻近节点
    distances = sum((tree.nodes - q_new).^2, 2);
    near_indices = find(distances < radius & distances > 0);
    
    % 尝试重布线
    for i = 1:length(near_indices)
        near_idx = near_indices(i);
        near_node = tree.nodes(near_idx,:);
        
        % 检查是否可以通过q_new获得更短路径
        new_cost = tree.cost(new_node_idx) + norm(q_new - near_node);
        if new_cost < tree.cost(near_idx)
            % 检查路径是否无碰撞
            if ~checkArmCollision(q_new, near_node, obstacles)
                % 更新父节点和成本
                tree.parent(near_idx) = new_node_idx;
                tree.cost(near_idx) = new_cost;
                
                % 更新边
                edge_idx = find(tree.edges(:,2) == near_idx);
                tree.edges(edge_idx,:) = [tree.parent(near_idx), near_idx];
            end
        end
    end
end

4.2 双向RRT（RRT-Connect）

双向RRT从起点和终点同时生长两棵树，可以显著提高搜索效率：

matlab复制function [path, treeA, treeB] = RRT_Connect(start, goal, obstacles, max_iter, step_size)
    % 初始化两棵树
    treeA.nodes = start; treeA.edges = []; treeA.parent = 0;
    treeB.nodes = goal;  treeB.edges = []; treeB.parent = 0;
    
    for iter = 1:max_iter
        % 交替扩展两棵树
        if mod(iter,2) == 1
            [treeA, treeB, q_new] = extendOneTree(treeA, treeB, obstacles, step_size);
        else
            [treeB, treeA, q_new] = extendOneTree(treeB, treeA, obstacles, step_size);
        end
        
        % 检查连接
        if ~isempty(q_new)
            % 重建路径
            path1 = reconstructPath(treeA, size(treeA.nodes,1));
            path2 = reconstructPath(treeB, size(treeB.nodes,1));
            path = [path1; flipud(path2(1:end-1,:))];
            return;
        end
    end
    
    path = [];
end

function [treeA, treeB, q_new] = extendOneTree(treeA, treeB, obstacles, step_size)
    q_rand = [2*pi*rand()-pi, pi*rand()-pi/2, pi*rand()-pi/2];
    [q_near, idx] = findNearestNeighbor(treeA.nodes, q_rand);
    q_new = extendConfiguration(q_near, q_rand, step_size);
    
    if ~checkArmCollision(q_near, q_new, obstacles)
        % 添加到树A
        new_node_idx = size(treeA.nodes,1)+1;
        treeA.nodes = [treeA.nodes; q_new];
        treeA.edges = [treeA.edges; idx new_node_idx];
        treeA.parent = [treeA.parent; idx];
        
        % 尝试连接树B
        [q_near_b, idx_b] = findNearestNeighbor(treeB.nodes, q_new);
        if norm(q_near_b - q_new) < step_size && ~checkArmCollision(q_near_b, q_new, obstacles)
            % 直接连接两棵树
            treeB.edges = [treeB.edges; idx_b new_node_idx];
            treeB.parent = [treeB.parent; idx_b];
            return;
        end
    else
        q_new = [];
    end
end

4.3 自适应步长策略

固定步长可能导致效率低下。自适应步长可以根据环境复杂度调整：

matlab复制function step_size = adaptiveStepSize(iteration, max_iter, min_step, max_step)
    % 随着迭代次数增加，逐步减小步长
    ratio = iteration / max_iter;
    step_size = max_step - (max_step - min_step) * ratio;
end

5. 实际应用中的关键问题与解决方案

5.1 机械臂关节限制处理

实际机械臂各关节都有转动范围限制，需要在采样和扩展时考虑：

matlab复制function q_new = boundedExtend(q_near, q_rand, step_size, joint_limits)
    % joint_limits: [min1 max1; min2 max2; min3 max3]
    direction = q_rand - q_near;
    distance = norm(direction);
    if distance <= step_size
        q_new = q_rand;
    else
        q_new = q_near + (direction/distance)*step_size;
    end
    
    % 应用关节限制
    for i = 1:3
        q_new(i) = max(joint_limits(i,1), min(joint_limits(i,2), q_new(i)));
    end
end

5.2 狭窄通道问题

狭窄通道是RRT的典型挑战，可以通过以下方法改善：

1. 障碍物膨胀法：暂时扩大障碍物，找到粗略路径后再细化
2. 桥测试采样：在狭窄通道区域增加采样概率
3. 路径优化：对找到的路径进行后处理优化

matlab复制function q_rand = bridgeSampling(tree, obstacles, joint_limits)
    % 1. 随机采样一个点
    q1 = [joint_limits(1,1)+(joint_limits(1,2)-joint_limits(1,1))*rand(), ...
          joint_limits(2,1)+(joint_limits(2,2)-joint_limits(2,1))*rand(), ...
          joint_limits(3,1)+(joint_limits(3,2)-joint_limits(3,1))*rand()];
    
    % 2. 检查是否在障碍物内
    if checkArmCollision(q1, q1, obstacles)
        % 3. 在附近采样第二个点
        q2 = q1 + 0.1*(rand(1,3)-0.5);
        
        % 4. 检查两点是否都在障碍物内，但中间点不在
        if checkArmCollision(q2, q2, obstacles)
            mid_q = 0.5*(q1+q2);
            if ~checkArmCollision(mid_q, mid_q, obstacles)
                q_rand = mid_q;
                return;
            end
        end
    end
    
    % 默认返回普通随机采样
    q_rand = [joint_limits(1,1)+(joint_limits(1,2)-joint_limits(1,1))*rand(), ...
              joint_limits(2,1)+(joint_limits(2,2)-joint_limits(2,1))*rand(), ...
              joint_limits(3,1)+(joint_limits(3,2)-joint_limits(3,1))*rand()];
end

5.3 动态障碍物处理

对于缓慢移动的障碍物，可以采用以下策略：

1. 局部重规划：当检测到障碍物移动时，在受影响区域重新规划
2. 速度障碍法：预测障碍物运动轨迹，提前规避
3. 实时RRT：在每次控制循环中运行简化版RRT

matlab复制function path = dynamicRRT(start, goal, dynamic_obstacles, max_iter, step_size)
    % 初始规划
    path = RRT_Planner(start, goal, dynamic_obstacles(), max_iter, step_size);
    
    % 执行路径并监控环境变化
    for i = 1:size(path,1)-1
        % 检查下一段路径是否仍然安全
        if checkArmCollision(path(i,:), path(i+1,:), dynamic_obstacles())
            % 从当前位置重新规划
            new_path = RRT_Planner(path(i,:), goal, dynamic_obstacles(), max_iter/2, step_size);
            if ~isempty(new_path)
                path = [path(1:i,:); new_path];
            else
                warning('动态障碍物阻挡，无法继续执行路径！');
                break;
            end
        end
        
        % 执行移动到path(i+1,:)
        % ... (实际机械臂控制代码)
    end
end

6. MATLAB实现中的工程技巧

6.1 高效碰撞检测优化

碰撞检测是RRT中最耗时的部分，可以通过以下方法优化：

1. 空间划分法：使用KD-tree或八叉树组织障碍物
2. 层次检测：先粗略检测，再精细检测
3. 并行计算：使用MATLAB的parfor并行化碰撞检测

matlab复制function collision = fastCollisionCheck(q1, q2, obstacles, kdtree)
    % 使用KD-tree快速查找附近障碍物
    [~, links1] = forwardKinematics(q1);
    [~, links2] = forwardKinematics(q2);
    
    % 对每个连杆段进行采样检查
    for i = 1:size(links1,1)-1
        p1 = links1(i,:);
        p2 = links1(i+1,:);
        p3 = links2(i,:);
        p4 = links2(i+1,:);
        
        % 只检查线段附近的障碍物
        mid_point = 0.5*(p1+p3);
        search_radius = norm(p1-p3)/2 + max(obstacles(:,3));
        nearby_obs = rangesearch(kdtree, mid_point(1:2), search_radius);
        
        for j = 1:length(nearby_obs{1})
            obs_idx = nearby_obs{1}(j);
            center = obstacles(obs_idx,1:2);
            radius = obstacles(obs_idx,3);
            
            % 精确碰撞检测
            if lineCircleCollision(p1, p3, center, radius) || ...
               lineCircleCollision(p2, p4, center, radius)
                collision = true;
                return;
            end
        end
    end
    collision = false;
end

6.2 可视化调试技巧

良好的可视化能极大提高调试效率：

1. 实时显示RRT生长过程
2. 高亮显示碰撞区域
3. 显示配置空间到任务空间的映射

matlab复制% 实时动画显示RRT生长
function animateRRT(tree, obstacles, path)
    h = figure;
    hold on; axis equal; grid on;
    
    % 绘制障碍物
    for i = 1:size(obstacles,1)
        rectangle('Position',[obstacles(i,1:2)-obstacles(i,3), 2*obstacles(i,3)*[1 1]], ...
                  'Curvature',[1 1], 'FaceColor',[0.8 0.2 0.2 0.3]);
    end
    
    % 逐帧显示树生长
    for i = 1:size(tree.edges,1)
        q1 = tree.nodes(tree.edges(i,1),:);
        q2 = tree.nodes(tree.edges(i,2),:);
        [~, links1] = forwardKinematics(q1);
        [~, links2] = forwardKinematics(q2);
        
        plot3(links1(:,1), links1(:,2), links1(:,3), 'b', 'LineWidth',0.5);
        plot3(links2(:,1), links2(:,2), links2(:,3), 'b', 'LineWidth',0.5);
        drawnow;
        
        % 减慢动画速度
        if mod(i,10) == 0
            pause(0.01);
        end
    end
    
    % 绘制最终路径
    if ~isempty(path)
        for i = 1:size(path,1)-1
            [~, links1] = forwardKinematics(path(i,:));
            [~, links2] = forwardKinematics(path(i+1,:));
            plot3([links1(:,1); links2(:,1)], ...
                  [links1(:,2); links2(:,2)], ...
                  [links1(:,3); links2(:,3)], 'm', 'LineWidth',2);
        end
    end
end

6.3 性能分析与瓶颈定位

使用MATLAB Profiler识别性能瓶颈：

matlab复制% 性能分析示例
profile on;
[path, tree] = RRT_Planner(start, goal, obstacles, 1000, 0.1);
profile off;
profile viewer;

常见优化点：

1. 向量化碰撞检测计算
2. 预分配数组内存
3. 减少不必要的图形绘制
4. 使用更高效的数据结构（如KD-tree）

7. 扩展应用与进阶方向

7.1 多机械臂协同规划

多个机械臂协同工作时，需要避免相互碰撞：

matlab复制function collision = multiArmCollision(robots_config, obstacles)
    % robots_config: {[θ1,θ2,θ3], [θ1,θ2,θ3], ...}
    % 检查每个机械臂的自碰撞和与障碍物的碰撞
    for i = 1:length(robots_config)
        [~, links] = forwardKinematics(robots_config{i});
        % 检查与障碍物的碰撞
        if checkArmCollision(robots_config{i}, robots_config{i}, obstacles)
            collision = true;
            return;
        end
        % 检查与其他机械臂的碰撞
        for j = i+1:length(robots_config)
            [~, links_j] = forwardKinematics(robots_config{j});
            if checkArmArmCollision(links, links_j)
                collision = true;
                return;
            end
        end
    end
    collision = false;
end

function collision = checkArmArmCollision(links1, links2)
    % 简化方法：检查两机械臂的连杆线段是否相交
    for i = 1:size(links1,1)-1
        for j = 1:size(links2,1)-1
            if lineSegmentIntersection(links1(i,1:2), links1(i+1,1:2), ...
                                      links2(j,1:2), links2(j+1,1:2))
                collision = true;
                return;
            end
        end
    end
    collision = false;
end

7.2 结合机器学习的方法

1. 使用神经网络预测优质采样区域
2. 强化学习优化RRT参数
3. 学习型碰撞检测器加速查询

matlab复制% 示例：使用学习型采样策略
function q_rand = learnedSampling(tree, goal, net)
    % net: 训练好的神经网络，输入当前树和目标，输出采样偏好
    if rand < 0.3  % 30%概率使用学习采样
        input = [tree.nodes(:); goal(:)];
        preference = predict(net, input');
        q_rand = preference + 0.1*randn(size(preference)); % 添加噪声
    else
        q_rand = [2*pi*rand()-pi, pi*rand()-pi/2, pi*rand()-pi/2];
    end
end

7.3 工业应用中的实际考量

1. 路径平滑化处理
2. 速度与加速度约束
3. 奇异点规避
4. 能耗优化

matlab复制function smooth_path = smoothPath(raw_path, obstacles, max_iter)
    smooth_path = raw_path;
    for iter = 1:max_iter
        % 随机选择两个点
        i = randi([1 size(smooth_path,1)-2]);
        j = randi([i+2 size(smooth_path,1)]);
        
        % 尝试直接连接
        if ~checkArmCollision(smooth_path(i,:), smooth_path(j,:), obstacles)
            smooth_path = [smooth_path(1:i,:); smooth_path(j:end,:)];
        end
    end
end