1. 项目背景与核心需求
机械臂路径规划是机器人学中的经典问题,特别是在存在障碍物的环境中。3自由度机械臂虽然结构相对简单,但在狭小空间或复杂障碍物环境下,如何高效找到无碰撞路径仍然具有挑战性。这个项目实现了基于RRT(快速扩展随机树)算法的路径规划器,专门针对带有圆形障碍物的环境进行优化。
注意:圆形障碍物在工业场景中很常见,比如管道、圆柱形设备等。选择圆形作为障碍物模型既简化了碰撞检测计算,又保持了实际应用的代表性。
2. RRT算法原理与实现
2.1 RRT基础原理
RRT是一种基于采样的路径规划算法,通过随机扩展树结构来探索自由空间。其核心优势在于:
- 不需要对环境进行完整建模
- 在高维空间中依然有效
- 概率完备性(随着迭代次数增加,找到解的概率趋近于1)
算法基本流程:
- 初始化树结构,起点作为根节点
- 随机采样一个配置点
- 在树中找到距离采样点最近的节点
- 向采样点方向扩展一步,生成新节点
- 检查新节点与路径是否发生碰撞
- 若无碰撞,将新节点加入树中
- 重复直到到达目标点或达到最大迭代次数
2.2 3自由度机械臂的特殊考量
对于3自由度机械臂,我们需要在关节空间(而非笛卡尔空间)进行规划。这意味着:
- 每个节点代表一组关节角度[q1,q2,q3]
- 距离度量使用关节空间的欧氏距离
- 碰撞检测需要将关节角度转换为实际机械臂位形
matlab复制% 关节角度到末端位置的转换示例
function pos = forwardKinematics(q)
L1 = 1; L2 = 0.8; L3 = 0.6; % 机械臂连杆长度
pos = [
L1*cos(q(1)) + L2*cos(q(1)+q(2)) + L3*cos(q(1)+q(2)+q(3));
L1*sin(q(1)) + L2*sin(q(1)+q(2)) + L3*sin(q(1)+q(2)+q(3))
];
end
3. 障碍物建模与碰撞检测
3.1 圆形障碍物表示
圆形障碍物用中心坐标和半径表示:
matlab复制obstacles = [
% x, y, radius
1.0, 1.5, 0.3;
2.0, 1.0, 0.4;
% 更多障碍物...
];
3.2 机械臂连杆的简化建模
为简化碰撞检测,我们将机械臂连杆建模为线段,检查各连杆与障碍物的相交情况:
matlab复制function collision = checkCollision(q, obstacles)
% 计算各关节位置
p0 = [0;0];
p1 = [L1*cos(q(1)); L1*sin(q(1))];
p2 = p1 + [L2*cos(q(1)+q(2)); L2*sin(q(1)+q(2))];
p3 = p2 + [L3*cos(q(1)+q(2)+q(3)); L3*sin(q(1)+q(2)+q(3))];
% 检查各连杆与障碍物的碰撞
for i = 1:size(obstacles,1)
if lineCircleCollision(p0,p1,obstacles(i,:)) || ...
lineCircleCollision(p1,p2,obstacles(i,:)) || ...
lineCircleCollision(p2,p3,obstacles(i,:))
collision = true;
return;
end
end
collision = false;
end
3.3 线段与圆的碰撞检测
matlab复制function collision = lineCircleCollision(p1, p2, circle)
% circle: [cx, cy, r]
% 实现线段到圆的距离检测
% 具体实现省略...
end
4. MATLAB实现详解
4.1 主算法框架
matlab复制function path = RRTPlanner(start, goal, obstacles, maxIter)
tree.vertices = start;
tree.edges = [];
tree.costs = 0;
for iter = 1:maxIter
% 随机采样(有一定概率直接采样目标点)
if rand < 0.1
q_rand = goal;
else
q_rand = sampleConfiguration();
end
% 找到最近节点
[q_near, idx] = nearestNeighbor(q_rand, tree);
% 向随机点方向扩展
q_new = steer(q_near, q_rand);
% 碰撞检测
if ~checkCollisionPath(q_near, q_new, obstacles)
% 添加到树中
tree.vertices = [tree.vertices; q_new];
tree.edges = [tree.edges; idx size(tree.vertices,1)];
tree.costs = [tree.costs; tree.costs(idx) + norm(q_new-q_near)];
% 检查是否到达目标
if norm(q_new - goal) < goalThreshold
path = extractPath(tree);
return;
end
end
end
path = []; % 未找到路径
end
4.2 关键参数设置
| 参数 | 典型值 | 说明 |
|---|---|---|
| maxIter | 5000 | 最大迭代次数 |
| stepSize | 0.1 | 每次扩展的最大步长 |
| goalBias | 0.1 | 直接采样目标点的概率 |
| goalThreshold | 0.2 | 认为到达目标的距离阈值 |
提示:stepSize需要根据机械臂的工作空间大小调整。太大可能导致频繁碰撞,太小则收敛慢。
5. 路径优化与后处理
5.1 路径平滑
原始RRT生成的路径通常不够平滑,可以应用以下优化:
- 剪枝:移除不必要的中间节点
- 插值:增加中间点使运动更平滑
- 梯度下降:在关节空间进行局部优化
matlab复制function smoothPath = smoothPath(originalPath, obstacles)
smoothPath = originalPath(1,:);
lastValid = 1;
for i = 3:size(originalPath,1)
% 尝试连接更远的节点
if checkCollisionPath(originalPath(lastValid,:), originalPath(i,:), obstacles)
% 不能直接连接,添加前一个节点
smoothPath = [smoothPath; originalPath(i-1,:)];
lastValid = i-1;
end
end
smoothPath = [smoothPath; originalPath(end,:)];
end
5.2 可视化实现
良好的可视化有助于调试和演示:
matlab复制function plotArm(q, color)
% 绘制机械臂当前位置
% 实现省略...
end
function plotObstacles(obstacles)
% 绘制障碍物
% 实现省略...
end
6. 实际应用中的注意事项
-
关节限位处理:
- 采样和扩展时需要考虑各关节的实际运动范围
- 在碰撞检测中加入关节角度限制检查
-
动态障碍物扩展:
- 可以通过膨胀障碍物半径来考虑机械臂安全距离
- 对于移动障碍物,可以使用速度障碍物法进行扩展
-
性能优化技巧:
- 使用KD树加速最近邻搜索
- 并行化碰撞检测
- 自适应步长调整
-
工程实践建议:
- 在实际控制前,应在仿真中验证路径
- 考虑加入速度、加速度约束
- 记录规划失败案例用于算法改进
7. 完整MATLAB代码框架
matlab复制classdef RRTPlanner3DOF
properties
armLengths = [1, 0.8, 0.6]; % 连杆长度
jointLimits = [-pi pi; -pi/2 pi/2; -pi/2 pi/2]; % 关节限位
stepSize = 0.1;
goalBias = 0.1;
maxIter = 5000;
goalThreshold = 0.2;
obstacles = []; % nx3矩阵 [x,y,radius]
end
methods
function path = plan(self, start, goal)
% 主规划函数实现
end
function collision = checkCollision(self, q)
% 碰撞检测实现
end
function plotPath(self, path)
% 可视化实现
end
end
end
8. 算法评估与改进方向
8.1 性能指标
- 成功率:在不同障碍物密度下的规划成功率
- 规划时间:从开始到找到路径的平均时间
- 路径质量:路径长度、平滑度、安全距离
8.2 可能的改进方向
-
RRT变种算法:
- RRT*:渐进最优版本
- Informed-RRT*:在椭圆区域内采样加速收敛
- RRT-Connect:双向扩展树
-
混合规划策略:
- 在狭窄区域使用RRT
- 在开阔区域使用更简单的直线连接
-
机器学习辅助:
- 使用学习模型预测采样方向
- 通过经验数据优化参数
在实际项目中,我发现对于简单的3自由度机械臂,基础RRT算法已经能够满足大部分需求。但当障碍物密度很高时,可以考虑改用RRT*或加入启发式采样策略。一个实用的技巧是在初始化时先尝试直线连接,失败后再启动RRT,这样可以节省大量简单场景下的计算时间。