王少冬1. 路径规划与RRT算法概述
路径规划是机器人导航中的核心问题，就像在陌生城市里寻找最近的厕所——既要快速到达，又要避开死胡同。传统算法如A*在低维空间表现良好，但当机器人自由度增加时（比如机械臂有6个关节），"构型空间"的维度会爆炸式增长，这就是RRT（快速扩展随机树）算法大显身手的地方。
RRT算法由Steven M. LaValle在1998年提出，其核心思想是模拟树木在空间中的生长过程：
- 从起点开始像树根一样向四周探索
- 通过随机采样避免在高维空间中穷举所有可能
- 逐步构建覆盖可行空间的树结构
与确定性算法不同，RRT具有概率完备性——只要存在可行路径，当迭代次数足够大时必定能找到。这种特性使其特别适合解决高维空间的复杂路径规划问题。
需要模型API调用？ 免费领10W Token，多模型网关一键接入 Claude、DeepSeek 等主流模型。
2. RRT算法实现详解
2.1 算法框架搭建
我们先搭建MATLAB实现的基础框架。首先定义节点类，这是构成搜索树的基本单元：
matlab复制classdef Node
properties
pos % 节点坐标[x,y]
parent % 父节点索引
cost % 从起点到该节点的路径成本(RRT*使用)
end
end
初始化阶段需要设置：
- 起点和终点坐标
- 障碍物信息
- 树生长步长(step size)
- 最大迭代次数
matlab复制% 初始化参数
start = [0, 0]; % 起点坐标
goal = [10, 10]; % 终点坐标
stepSize = 0.5; % 单次生长步长
maxIter = 5000; % 最大迭代次数
% 初始化树结构
tree = Node();
tree.pos = start;
tree.parent = 0;
tree.cost = 0;
nodes = [tree]; % 节点集合
2.2 核心算法流程
RRT的主循环包含四个关键步骤：
- 随机采样：在构型空间中生成随机点
matlab复制function sample = randomSample(goal, sampleBias)
if rand < sampleBias
sample = goal; % 有偏向性地采样目标点
else
sample = rand(1,2)*20-10; % 在[-10,10]范围内随机采样
end
end
- 寻找最近邻节点：
matlab复制function nearestNode = findNearest(nodes, randPoint)
positions = reshape([nodes.pos], 2, [])'; % 提取所有节点坐标
[~, idx] = min(vecnorm(positions - randPoint, 2, 2));
nearestNode = nodes(idx);
end
- 扩展新节点：
matlab复制function newNode = extend(fromNode, toPoint, step)
direction = toPoint - fromNode.pos;
distance = norm(direction);
if distance > step
direction = direction/distance * step; % 步长限制
end
newNode = Node();
newNode.pos = fromNode.pos + direction;
newNode.parent = fromNode;
end
- 碰撞检测：
matlab复制function collision = checkCollision(start, finish, obstacles)
for i = 1:size(obstacles,1)
% 圆形障碍物检测
obstacle = obstacles(i,:);
a_to_center = start - obstacle(1:2);
b_to_center = finish - obstacle(1:2);
% 端点检测
if norm(a_to_center) < obstacle(3) || norm(b_to_center) < obstacle(3)
collision = true;
return;
end
% 线段到圆心距离检测
ab = finish - start;
t = dot(-a_to_center, ab)/norm(ab)^2;
nearest = start + max(0, min(1, t))*ab;
if norm(nearest - obstacle(1:2)) < obstacle(3)
collision = true;
return;
end
end
collision = false;
end
2.3 可视化实现
良好的可视化能直观展示算法运行过程：
matlab复制% 初始化图形窗口
figure;
hold on;
axis([-10 20 -10 20]);
rectangle('Position',[2,2,6,6],'Curvature',[1,1],'FaceColor',[0.5 0.5 0.5]);
% 主循环
for iter = 1:maxIter
randPoint = randomSample(goal, 0.05);
nearest = findNearest(nodes, randPoint);
newNode = extend(nearest, randPoint, stepSize);
if ~checkCollision(nearest.pos, newNode.pos, [5,5,3])
% 绘制新分支
plot([nearest.pos(1), newNode.pos(1)],...
[nearest.pos(2), newNode.pos(2)], 'g');
nodes = [nodes, newNode];
% 到达目标判断
if norm(newNode.pos - goal) < stepSize
disp('路径找到！');
break;
end
end
end
3. RRT*算法优化
RRT*在RRT基础上增加了路径优化过程，主要改进在于：
3.1 近邻节点重连
每次添加新节点后，检查附近节点是否能通过该节点获得更优路径：
matlab复制function nearIndices = findNearNodes(nodes, newNode, radius)
positions = reshape([nodes.pos], 2, [])';
distances = vecnorm(positions - newNode.pos, 2, 2);
nearIndices = find(distances < radius);
end
% 在添加新节点后执行优化
nearIndices = findNearNodes(nodes, newNode, 2.0);
minNode = nearest;
minCost = nearest.cost + norm(newNode.pos - nearest.pos);
for i = nearIndices
nearNode = nodes(i);
if ~checkCollision(nearNode.pos, newNode.pos, [5,5,3]) &&...
(nearNode.cost + norm(newNode.pos - nearNode.pos)) < minCost
minNode = nearNode;
minCost = nearNode.cost + norm(newNode.pos - nearNode.pos);
end
end
newNode.parent = minNode;
newNode.cost = minCost;
3.2 路径回溯
找到目标后，从终点回溯到起点获取完整路径：
matlab复制path = [];
currentNode = nodes(end);
while any(currentNode.pos ~= start)
path = [currentNode.pos; path];
currentNode = nodes([nodes.pos] == currentNode.parent);
end
path = [start; path];
% 绘制最终路径
plot(path(:,1), path(:,2), 'r-', 'LineWidth', 2);
4. 实战经验与调优技巧
4.1 参数选择经验
-
步长(stepSize)：
- 太小：收敛慢，树生长效率低
- 太大：容易"穿墙"，错过狭窄通道
- 经验值：环境最小通道宽度的1/3~1/2
-
目标偏向系数(sampleBias)：
- 典型值：0.05~0.1
- 过高：失去随机探索性
- 过低：可能长时间无法找到目标
-
近邻半径(rewireRadius)：
- 决定优化范围
- 通常取步长的3~5倍
4.2 常见问题排查
-
路径穿过障碍物：
- 检查碰撞检测函数是否准确
- 确认障碍物边界条件处理正确
- 减小步长测试
-
算法运行时间过长：
- 增加目标偏向系数
- 检查最近邻搜索效率
- 考虑使用KD树加速搜索
-
路径不够平滑：
- 增加RRT*的迭代次数
- 添加后处理平滑算法
- 调整近邻半径参数
4.3 性能优化技巧
- 加速最近邻搜索：
matlab复制% 使用KD树加速搜索(需要Statistics and Machine Learning Toolbox)
kdtree = KDTreeSearcher(positions);
idx = knnsearch(kdtree, randPoint);
nearestNode = nodes(idx);
- 并行化采样：
matlab复制% 使用parfor并行处理多个采样点
parfor i = 1:batchSize
randPoints(i,:) = randomSample(goal, sampleBias);
end
- 自适应步长调整：
matlab复制% 根据环境复杂度动态调整步长
if mod(iter,100) == 0
successRate = sum(~[nodes.collision])/iter;
stepSize = stepSize * (0.9 + 0.2*successRate);
end
5. 进阶应用与扩展
5.1 动态环境处理
对于移动障碍物，需要定期更新环境信息：
matlab复制% 在每次迭代中检查障碍物位置变化
if mod(iter,10) == 0
obstacles = updateObstacles();
end
5.2 多RRT协同搜索
结合双向RRT(Bi-RRT)提高效率：
matlab复制% 初始化两棵树
tree_start = initializeTree(start);
tree_goal = initializeTree(goal);
% 交替生长两棵树
if mod(iter,2) == 0
% 生长起点树
randPoint = randomSample(goal, sampleBias);
nearest = findNearest(tree_start, randPoint);
% ...其余步骤类似
else
% 生长目标树
randPoint = randomSample(start, sampleBias);
nearest = findNearest(tree_goal, randPoint);
% ...其余步骤类似
end
% 连接检查
if canConnect(tree_start(end), tree_goal(end))
% 找到路径
path = extractPath(tree_start, tree_goal);
end
5.3 复杂约束处理
对于带动力学约束的系统（如无人机），需要修改扩展方式：
matlab复制function newNode = kinodynamicExtend(fromNode, toPoint, dt)
% 基于当前状态和控制输入计算新状态
u = calculateControl(fromNode, toPoint);
newNode.pos = fromNode.pos + fromNode.vel*dt + 0.5*u*dt^2;
newNode.vel = fromNode.vel + u*dt;
newNode.parent = fromNode;
end
在实际项目中，RRT算法通常需要根据具体应用场景进行调整和优化。比如在自动驾驶中，可能需要考虑车辆动力学；在机械臂路径规划中，需要处理关节角度限制等问题。理解算法核心思想后，这些扩展都是水到渠成的事情。