基于RRT算法的路径规划实现（matlab）

基于RRT算法的路径规划实现（matlab）

快速随机扩展树算法(Rapidly-exploring Random Tree, RRT)

RRT算法基于采样的方式在配置空间中搜索，它的搜索过程类似于一棵树不断生长、向四周扩散的过程，它以起点作为搜索树T根节点。

伪代码

详细步骤

1.起点为X_init，目标地点为X_goal;
2.在空间中随机采样，得到随机点X_rand;
3.然后计算随机点X_rand与随机树Ｔ中所有节点的距离，找出离随机点X_rand最近的节点作为邻近点X_near;
4.随机树Ｔ从邻近点X_near向随机点X_rand方向移动给定步长，扩展生成一 个新的节点X_new;
5.如果在邻近点X_near向新节点X_new方向扩展的过程中不与障碍物发生碰撞，则将新节点X_new加入到随机树Ｔ中，若发生碰撞，重新采样随机点X_rand。
6.最后判断新节点X_new是否在目标点X_goal的区域内，若不在目标点区域内，重复上过程；若在目标点区域内，将新节点X_new作为目标点的父节点，并将目标点X_goal加入到随机树Ｔ中.

新节点生长过程

部分代码

function My_RRT
clc
clear
close all
%%  color map
load maze.mat map
[map_height,map_width]=size(map); %行是高y，列是宽x
q_start = [206, 198]; %q s t a r t ( 1 ) : x宽 , q s t a r t ( 2 ) : y高
q_goal = [416, 612];
colormap=[1 1 1
          0 0 0
          1 0 0 
          0 1 0
          0 0 1];
imshow(uint8(map),colormap)
hold on
%% rrt tree %行是y坐标，列是x坐标
%initial
vertices=q_start;
edges = [];
K=10000;
delta_q=50;
p=0.3;
q_rand=[];
q_near=[];
q_new=[];
%main loop
plot(q_start(2),q_start(1),'*b')
plot(q_goal(2),q_goal(1),'*y')
for k = 1:K
    arrived=is_goal_arrived(vertices,q_goal,delta_q);
    if arrived
        vertices=[vertices;q_goal];
        edges = [edges;[size(vertices,1),size(vertices,1)-1]];
        break;
    end
    if rand <= p
        q_rand = q_goal;%q(1)宽x，q(2)高y
    else
        q_rand = [randi(map_height),randi(map_width)];
    end
    if map( q_rand(1,1),q_rand(1,2) ) == 1 %map(1)height,map(2)width
        continue;
    end
    [q_new,q_near,q_near_ind,vector_dir] = get_qnew_qnear(delta_q,q_rand,vertices);
    add_qnew = is_add_in_veritces(map,q_new,q_near,vector_dir,10);
    if add_qnew
        vertices=[vertices;q_new];
        r_v = size(vertices,1);
        edges = [edges;[r_v,q_near_ind]];
    else
        continue;
    end
%     plot(q_near(1,1),q_near(2,1),'*b');
   plot([q_near(1,2),q_new(1,2)],[q_near(1,1),q_new(1,1)],'-b')
   drawnow
end
path =find_path_node(edges);
%plot base path
plot(vertices(path,2),vertices(path,1),'-r')
%smooth
path_smooth = smooth(path,vertices,map);
%plot smooth path
plot(vertices(path_smooth,2),vertices(path_smooth,1),'-g');
end

RRT算法实现结果

下载链接

https://download.csdn.net/download/iii66yy/80675832

参考文献
[1] 阮晓钢,周静,张晶晶,朱晓庆.基于子目标搜索的机器人目标导向RRT路径规划算法[J].控制与决策,2020,35(10):2543-2548.DOI:10.13195/j.kzyjc.2019.0043.