添加RTT算法

RTT/RTT.m

@@ -0,0 +1,144 @@
+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 0 1 0;   %13
+       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;   %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,10 +30,12 @@ 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);
+visualize_path(map, start, goal, path_Astar, path_Dijkstra, path_RTT);
 
-function visualize_path(map, start, goal, path_Astar, path_Dijkstra)
+function visualize_path(map, start, goal, path_Astar, path_Dijkstra, path_RTT)
     % 可视化地图，0为空地，1为障碍
     imagesc(map);
     colormap(flipud(gray));          % 灰度图（0白 1黑）
@@ -42,25 +44,29 @@ function visualize_path(map, start, goal, path_Astar, path_Dijkstra)
 
     % 蓝色起点
     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 路径
-        plot(path_Astar(:,2), path_Astar(:,1), 'r-', 'LineWidth', 2); % 绿色路径线
+    if ~isempty(path_Dijkstra)
-        plot(path_Astar(:,2), path_Astar(:,1), 'ro', 'MarkerSize', 4, 'MarkerFaceColor', 'r'); % 每个点画圈
+        plot(path_Dijkstra(:,2), path_Dijkstra(:,1), 'g-', 'LineWidth', 2);
-        plot(path_Dijkstra(:,2), path_Dijkstra(:,1), 'g-', 'LineWidth', 2); % 绿色路径线
+        plot(path_Dijkstra(:,2), path_Dijkstra(:,1), 'go', 'MarkerSize', 4, 'MarkerFaceColor', 'g');
-        plot(path_Dijkstra(:,2), path_Dijkstra(:,1), 'go', 'MarkerSize', 4, 'MarkerFaceColor', 'g'); % 每个点画圈
+    end
+    % 绘制 RTT 路径
+    if ~isempty(path_RTT)
+        plot(path_RTT(:,2), path_RTT(:,1), 'b-', 'LineWidth', 2);
     end
 
     % 网格线