RRT算法实现，结合目标偏向采样和引导域偏向采样

function rrt_path = rrt_planning(xStart, yStart, xTarget, yTarget, MAX_X, MAX_Y, resolution, obstacles, guidance_area)
    % RRT算法实现，结合目标偏向采样和引导域偏向采样
    % xStart, yStart: 起点坐标
    % xTarget, yTarget: 目标点坐标
    % MAX_X, MAX_Y: 地图的最大X和Y坐标
    % resolution: 栅格的分辨率
    % obstacles: 障碍物矩阵，true表示障碍物
    % guidance_area: 引导域矩阵，true表示引导域
    % 初始化参数
    MAX_X = 50;         % 设置地图的最大x坐标
    MAX_Y = 50;         % 设置地图的最大y坐标

    num_iterations = 1000; % 最大迭代次数
    step_size = 10 * resolution; % 树扩展步长
    goal_bias = 0.05; % 目标偏向概率
    guidance_bias = 0.5; % 引导域偏向概率
    % 初始化RRT树
    tree = struct('parent', {}, 'position', {});
    start_idx = 1;
    tree(start_idx) = struct('parent', NaN, 'position', [xStart, yStart]);
    % 主循环
    for i = 1:num_iterations
        % 偏向采样
        if rand() < goal_bias
            sample_point = [xTarget, yTarget];
        elseif rand() < guidance_bias
            % 在引导域内随机采样
            valid_guidance_indices = find(guidance_area);
            if ~isempty(valid_guidance_indices)
                idx = valid_guidance_indices(randi(length(valid_guidance_indices)));
                [gy, gx] = ind2sub(size(guidance_area), idx);
                sample_point = [(gx - 0.5) * resolution, (gy - 0.5) * resolution];
            else
                sample_point = rand(1, 2) * [MAX_X, MAX_Y];
            end
        else
            % 随机采样
            sample_point = rand(1, 2) .* [MAX_X, MAX_Y];

        end
        % 找到树中最近的点
        [nearest_idx, nearest_point] = find_nearest_point(tree, sample_point);
        % 向采样点扩展
        new_point = steer(nearest_point, sample_point, step_size);
        % 碰撞检测
        if ~is_collision(nearest_point, new_point, obstacles, resolution)
            % 添加新点至树
            new_idx = length(tree) + 1;
            tree(new_idx) = struct('parent', nearest_idx, 'position', new_point);
            % 检查是否到达目标
            if norm(new_point - [xTarget, yTarget]) < step_size
                goal_idx = new_idx;
                break;
            end
        end
    end
    % 提取路径
    if exist('goal_idx', 'var')
        rrt_path = extract_path(tree, goal_idx);
    else
        rrt_path = [];
        warning('RRT planning failed to reach the goal.');
    end
end
% 以下是辅助函数的实现

function [nearest_idx, nearest_point] = find_nearest_point(tree, sample_point)
    % 找到树中最近的点
    distances = arrayfun(@(i) norm(tree(i).position - sample_point), 1:length(tree));
    [~, nearest_idx] = min(distances); % 保留min_dist
    nearest_point = tree(nearest_idx).position;
end

function new_point = steer(nearest_point, sample_point, step_size)
    % 向采样点扩展
    direction = (sample_point - nearest_point) / norm(sample_point - nearest_point);
    new_point = nearest_point + direction * step_size;
end


function is_coll = is_collision(point1, point2, obstacles, resolution, MAX_X, MAX_Y)
    % 碰撞检测
    % 使用bresenham_line函数
    t1=floor(point1 / resolution);t2= floor(point2 / resolution);
    xy = bresenham_line(t1, t2, MAX_X, MAX_Y);
    idx = sub2ind(size(obstacles), xy(:, 2), xy(:, 1));
    is_coll = any(obstacles(idx));
end

% 确保在函数定义中包含所有需要的参数
function xy = bresenham_line(p1, p2, MAX_X, MAX_Y)
    % Bresenham's line algorithm with boundary check
    % ... (实现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;

    xy = [x1, y1];
    while x1 ~= x2 || y1 ~= y2
        e2 = 2 * err;
        if e2 > -dy
            err = err - dy;
            x1 = x1 + sx;
        end
        if e2 < dx
            err = err + dx;
            y1 = y1 + sy;
        end
        % Boundary check
        x1 = min(max(x1, 1), MAX_X);
        y1 = min(max(y1, 1), MAX_Y);
        xy = [xy; x1, y1];
    end
end


function path = extract_path(tree, goal_idx)
    % 提取路径
    path = tree(goal_idx).position;
    while ~isnan(tree(goal_idx).parent)
        goal_idx = tree(goal_idx).parent;
        path = [tree(goal_idx).position; path]; % 将父节点的位置添加到路径中
    end
end