路径搜索算法——RRT算法

RRT算法概述

RRT算法是一种基于采样的路径规划算法，其基本思想是通过产生随机点的方式通过一个步长向目标点搜索前进，有效躲避障碍物，避免路径陷入局部最小值，收敛速度快。

MATLAB代码链接如下

链接：https://pan.baidu.com/s/1BApjX7dqaOvttlo2QFMjCQ 
提取码：mt2k

RRT算法代码解析

1、main主函数

%% 初始化
count = 1;               %迭代次数
count_max = 3000;% 最大迭代次数
% 地图范围
minX = 0; maxX = 10;%地图中x轴的范围
minY = 0; maxY = 10;%地图中轴的范围
% 记录所有节点，及其父节点
points = zeros(count_max, 2);
parent = zeros(count_max, 1);
% 步长
growStep = 0.1;
% 起始点、目标点
start = [1.0,1.0]; target = [9.0, 9.0];
points(1, :) = start; %节点中第一个节点为起始点
currentIndex = 1;
% 障碍物 x y r
%x，y为障碍物的坐标点，r为障碍物的半径
obs = [3, 4, 0.5;
       6, 7, 1;
       8, 2, 0.5;
       5, 2, 1;
       ];

axis([0, 10, 0, 10]);
hold on;
for i = 1: size(obs, 1)
    rectangle('Position', [obs(i,1)-obs(i,3), obs(i,2)-obs(i,3), obs(i,3) * 2, obs(i,3) * 2], 'Curvature', [1 1]);
end

%% 主循环
while count < count_max
    count = count + 1;
    % 生成随机点
    if mod(count, 5) == 0
        randPoint = target;
    else
        randPoint = NewRandPoint(minX, maxX, minY, maxY);
    end
    plot(randPoint(1), randPoint(2), '.', markersize=3, color='blue');

    % 寻找最近节点为新节点的父节点
    currentParent= FindNearestPoint(points, currentIndex, randPoint);
    % 从父节点向随机点生长
    newPoint = Grow(points(currentParent, :), randPoint, growStep);
    % 检测是否发生碰撞
    if (~Collisionless(obs, newPoint, points(currentParent, :))) 
        continue;
    end
    % 将新节点添加
    currentIndex = currentIndex + 1;
    points(currentIndex, :) = newPoint;
    parent(currentIndex) = currentParent;

    plot([newPoint(1), points(currentParent, 1)], [newPoint(2), points(currentParent, 2)], '-', color='black');
    plot(start(1), start(2), '.', markersize=30, color='red');
    plot(target(1), target(2), '.', markersize=30, color='green');

    pause(0)
    if norm(target - newPoint) < growStep
        points(currentIndex + 1, :) = target;
        parent(currentIndex + 1) = currentIndex;
        break;
    end
end
hold off;

2、NewRandPoint函数

function randPoint = NewRandPoint(minX, maxX, minY, maxY)
%RANDPOINT 根据范围随机产生新节点
    randPoint = [minX + (maxX - minX) * rand, minY + (maxY - minY) * rand];    
end

3、FindNearestPoint函数

function index = FindNearestPoint(points, size, point)
%FINDNEARESTPOINT 寻找距离最近的点
% points 待查找数组, size 待查找数组有用大小，point 目标点
    minDistance = norm(points(1, :) - point);
    index = 1;
    for i = 1 : size
        if norm(points(i, :) - point) < minDistance
            minDistance = norm(points(i, :) - point);
            index = i;
        end
    end
end

4、Grow函数

function newPoint = Grow(src, tar, step)
%GROW 根据源节点、目标节点和步长生成新节点
    newPoint = src +  (tar - src) / norm(tar - src) * step;
end

5、

function reval = Collisionless(obs, p1, p2)
%COLLISIONLESS 判断是否碰撞返回布尔值
%   输入障碍物中心点和半径、待检测点
    for i = 1: size(obs, 1)
        center = obs(i, 1:2);
        radio = obs(i, 3);
        % 点在圆内
        if norm(center - p1) < radio || norm(center - p2) < radio
            reval = false;
            return;
        end
        % 直线 Ax + By + C = 0;  (y1 - y2) x + (x2 - x1) y + x1y2 - y1x2 = 0;
        if p1(1) == p2(1)
            A = 1;
            B = 0;
            C = -p1(1);
        elseif p1(2) == p2(2)
            A = 0;
            B = 1;
            C = -p1(2);
        else
            A = p1(2) - p2(2);
            B = p2(1) - p1(1);
            C = p1(1) * p2(2) - p1(2) * p2(1);
        end
        dist1 = (A * center(1) + B * center(2) + C) ^ 2;
        dist2 = (A * A + B * B) * radio * radio;
        if dist1 > dist2
            % 圆心到直线距离大于半径，不发生碰撞
            continue;
        end
        angle1 = (center(1) - p1(1)) * (p2(1) - p1(1)) + (center(2) - p1(2)) * (p2(2) - p1(2));
        angle2 = (center(1) - p2(1)) * (p1(1) - p2(1)) + (center(2) - p2(2)) * (p1(2) - p2(2));
        if angle1 > 0 && angle2 > 0
            % 余弦都为正，锐角，相交
            reval = false;
            return;
        end
    end
    reval = true;
end

运行结果图