RTT/RTT.m

@@ -1,144 +0,0 @@
-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

homework_map.m

@@ -13,7 +13,7 @@ map = [0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0;   % 1
        1 0 0 0 0 0 0 0 0 0 0 1 1 0 0 0 0 0 0 0;   %10
        0 0 0 0 0 0 0 0 0 0 1 1 0 0 0 0 0 0 0 0;   %11
        0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 1 1 0 0;   %12
-       0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 0;   %13
+       0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0;   %13
        0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0;   %14
        0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0;   %15
        0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0;   %16
@@ -30,12 +30,10 @@ goal = [19, 19];
 path_Astar = Astar(map, start, goal);
 % 调用Dijkstra算法
 path_Dijkstra = Dijkstras(map, start, goal);
-%调用RTT算法
-path_RTT = RTT(map, start, goal);
 
-visualize_path(map, start, goal, path_Astar, path_Dijkstra, path_RTT);
+visualize_path(map, start, goal, path_Astar, path_Dijkstra);
 
-function visualize_path(map, start, goal, path_Astar, path_Dijkstra, path_RTT)
+function visualize_path(map, start, goal, path_Astar, path_Dijkstra)
     % 可视化地图，0为空地，1为障碍
     imagesc(map);
     colormap(flipud(gray));          % 灰度图（0白 1黑）
@@ -44,29 +42,25 @@ function visualize_path(map, start, goal, path_Astar, path_Dijkstra, path_RTT)
 
     % 蓝色起点
     plot(start(2), start(1), 'bo', 'MarkerSize', 10, 'LineWidth', 2);
+
     % 红色终点
     plot(goal(2), goal(1), 'ro', 'MarkerSize', 10, 'LineWidth', 2);
 
-    % 绘制 A* 路径
     if ~isempty(path_Astar)
+    % 如果起点不等于路径首元素，插入
         if ~isequal(path_Astar(1,:), start)
             path_Astar = [start; path_Astar];
         end
+    % 如果终点不等于路径末元素，插入
         if ~isequal(path_Astar(end,:), goal)
             path_Astar = [path_Astar; goal];
         end
-        plot(path_Astar(:,2), path_Astar(:,1), 'r-', 'LineWidth', 2);
-        plot(path_Astar(:,2), path_Astar(:,1), 'ro', 'MarkerSize', 4, 'MarkerFaceColor', 'r');
-    end
 
-    % 绘制 Dijkstra 路径
+    % 显示路径线
-    if ~isempty(path_Dijkstra)
+        plot(path_Astar(:,2), path_Astar(:,1), 'r-', 'LineWidth', 2); % 绿色路径线
-        plot(path_Dijkstra(:,2), path_Dijkstra(:,1), 'g-', 'LineWidth', 2);
+        plot(path_Astar(:,2), path_Astar(:,1), 'ro', 'MarkerSize', 4, 'MarkerFaceColor', 'r'); % 每个点画圈
-        plot(path_Dijkstra(:,2), path_Dijkstra(:,1), 'go', 'MarkerSize', 4, 'MarkerFaceColor', 'g');
+        plot(path_Dijkstra(:,2), path_Dijkstra(:,1), 'g-', 'LineWidth', 2); % 绿色路径线
-    end
+        plot(path_Dijkstra(:,2), path_Dijkstra(:,1), 'go', 'MarkerSize', 4, 'MarkerFaceColor', 'g'); % 每个点画圈
-    % 绘制 RTT 路径
-    if ~isempty(path_RTT)
-        plot(path_RTT(:,2), path_RTT(:,1), 'b-', 'LineWidth', 2);
     end
 
     % 网格线