添加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