144 lines
3.7 KiB
Matlab
144 lines
3.7 KiB
Matlab
function path = rtt(map, start, goal)
|
||
maxIterations = 5000;
|
||
stepSize = 5;
|
||
goalThreshold = 5; % 这里用行列坐标距离阈值
|
||
|
||
mapSize = size(map);
|
||
|
||
% 初始化树，存储节点和父节点索引
|
||
tree.nodes = start;
|
||
tree.parents = 0;
|
||
|
||
for i = 1:maxIterations
|
||
% 采样随机点
|
||
randPoint = sampler(mapSize, goal);
|
||
|
||
% 找到最近树节点
|
||
[nearestIdx, nearestPoint] = find_nearest(tree.nodes, randPoint);
|
||
|
||
% 局部规划器扩展新节点
|
||
newPoint = local_planner(map, nearestPoint, randPoint, stepSize);
|
||
|
||
% 如果无法扩展则跳过
|
||
if isempty(newPoint)
|
||
continue;
|
||
end
|
||
|
||
% 加入树
|
||
tree.nodes = [tree.nodes; newPoint];
|
||
tree.parents = [tree.parents; nearestIdx];
|
||
|
||
% 判断是否到达目标
|
||
if norm(newPoint - goal) < goalThreshold
|
||
tree.nodes = [tree.nodes; goal];
|
||
tree.parents = [tree.parents; size(tree.nodes, 1) - 1];
|
||
path = make_path(tree);
|
||
return;
|
||
end
|
||
end
|
||
|
||
% 没有找到路径，返回空数组
|
||
path = [];
|
||
end
|
||
|
||
function point = sampler(mapSize, goal)
|
||
% 10%概率采样目标点，90%概率采样随机点
|
||
if rand() < 0.1
|
||
point = goal;
|
||
else
|
||
point = round([rand()*mapSize(1), rand()*mapSize(2)]);
|
||
% 保证点不越界
|
||
point(1) = max(min(point(1), mapSize(1)), 1);
|
||
point(2) = max(min(point(2), mapSize(2)), 1);
|
||
end
|
||
end
|
||
|
||
function [idx, nearest] = find_nearest(nodes, point)
|
||
% 计算所有节点到point的距离，返回最近节点的索引和坐标
|
||
dists = vecnorm(nodes - point, 2, 2);
|
||
[~, idx] = min(dists);
|
||
nearest = nodes(idx, :);
|
||
end
|
||
|
||
function newPoint = local_planner(map, nearestPoint, randPoint, stepSize)
|
||
% 沿着方向扩展，检查碰撞和越界
|
||
direction = randPoint - nearestPoint;
|
||
if norm(direction) == 0
|
||
newPoint = [];
|
||
return;
|
||
end
|
||
direction = direction / norm(direction);
|
||
newPoint = round(nearestPoint + direction * stepSize);
|
||
|
||
% 越界检测
|
||
if newPoint(1) < 1 || newPoint(2) < 1 || ...
|
||
newPoint(1) > size(map, 1) || newPoint(2) > size(map, 2)
|
||
newPoint = [];
|
||
return;
|
||
end
|
||
|
||
% 碰撞检测(用简单连线检测)
|
||
if isCollision(map, nearestPoint, newPoint)
|
||
newPoint = [];
|
||
return;
|
||
end
|
||
end
|
||
|
||
function collision = isCollision(map, p1, p2)
|
||
% 使用Bresenham算法检查两点间是否碰撞
|
||
linePts = bresenham(p1, p2);
|
||
collision = false;
|
||
for i = 1:size(linePts,1)
|
||
pt = linePts(i,:);
|
||
if map(pt(1), pt(2)) == 1
|
||
collision = true;
|
||
return;
|
||
end
|
||
end
|
||
end
|
||
|
||
function pts = bresenham(p1, p2)
|
||
% Bresenham直线算法实现
|
||
x1 = p1(1); y1 = p1(2);
|
||
x2 = p2(1); y2 = p2(2);
|
||
dx = abs(x2 - x1); dy = abs(y2 - y1);
|
||
sx = sign(x2 - x1); sy = sign(y2 - y1);
|
||
err = dx - dy;
|
||
|
||
pts = [];
|
||
while true
|
||
pts = [pts; x1, y1];
|
||
if x1 == x2 && y1 == y2
|
||
break;
|
||
end
|
||
e2 = 2*err;
|
||
if e2 > -dy
|
||
err = err - dy;
|
||
x1 = x1 + sx;
|
||
end
|
||
if e2 < dx
|
||
err = err + dx;
|
||
y1 = y1 + sy;
|
||
end
|
||
end
|
||
end
|
||
|
||
function path = make_path(tree)
|
||
% 通过父节点回溯路径
|
||
path = tree.nodes(end, :);
|
||
idx = size(tree.nodes, 1);
|
||
while tree.parents(idx) ~= 0
|
||
idx = tree.parents(idx);
|
||
path = [tree.nodes(idx, :); path];
|
||
end
|
||
|
||
% 打印路径
|
||
if ~isempty(path)
|
||
disp('路径为：');
|
||
for i = 1:size(path, 1)
|
||
fprintf('(%d, %d)\n', path(i, 2), path(i, 1));
|
||
end
|
||
else
|
||
disp('未找到路径。');
|
||
end
|
||
end |