基于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 10 0 01 0 0 0 1 00 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:Karrived=is_goal_arrived(vertices,q_goal,delta_q);if arrivedvertices=[vertices;q_goal];edges = [edges;[size(vertices,1),size(vertices,1)-1]];break;endif rand <= pq_rand = q_goal;%q(1)寬x，q(2)高yelseq_rand = [randi(map_height),randi(map_width)];endif map( q_rand(1,1),q_rand(1,2) ) == 1 %map(1)height,map(2)widthcontinue;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_qnewvertices=[vertices;q_new];r_v = size(vertices,1);edges = [edges;[r_v,q_near_ind]];elsecontinue;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.
總結
以上是生活随笔為你收集整理的基于RRT算法的路径规划实现（matlab）的全部內容，希望文章能夠幫你解決所遇到的問題。
如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。
