✅作者简介：热爱科研的Matlab仿真开发者，修心和技术同步精进，matlab项目合作可私信。
🍎个人主页：Matlab科研工作室
🍊个人信条：格物致知。
更多Matlab仿真内容点击👇
⛄ 内容介绍
经典的路径规划算法大都需要在全局已知空间中对环境进行建模,包括人工势场法,遗传算法,启发式算法,仿生学算法等.由于需要预先构建环境,因此这些方法并不适合解决在高维度空间中的路径规划问题.基于快速扩展随机树(RRT)的路径规划方式其优势在于可以避免对全局环境的构建,通过对状态空间进行随机采样,检测碰撞点,能够有效地解决在平面及三维状态空间下的复杂路径规划问题.通过与人工势场法和A*算法进行比对,确定了RRT算法在复杂环境中解决无人机路径规划问题的优势,在对相关参数进行优化后该方法是概率完备且存在最优解的,同时在固定翼智能集群飞行编队控制及协同项目中应用.
⛄ 部分代码
clearvars
close all
rand('state',1);
x_max = 500;
y_max = 500;
%obstacle = [500,150,200,200];
obstacle1 = [0,200,100,100];
obstacle2 = [200,200,200,100];
obstacle3 = [500,0,100,500];
obstacle4 = [0,500,500,100];
ob_vec=[obstacle1;obstacle2;obstacle3;obstacle4];
EPS = 10;
rho=80;
step=10; %which is EPS in old edition
BETA=EPS/rho; %my addition
%numNodes = 3000;
numNodes=2000;
q_start.coord = [10 10];
q_start.dir=pi/3; %My addition
q_start.cost = 0;
q_start.parent = 0;
q_goal.coord = [490 490];
q_goal.cost = 0;
q_goal.dir=0;
nodes(1) = q_start;
figure(1)
set(gcf,'unit','normalized','position',[0.4 0.4 0.5 0.5])
axis equal
axis([0 x_max 0 y_max])
%axis([100 110 100 110])
rectangle('Position',obstacle1,'FaceColor',[0 .5 .5])
hold on
rectangle('Position',obstacle2,'FaceColor',[0 .5 .5])
plot(q_goal.coord(1),q_goal.coord(2),'ro','LineWidth',2)
for i = 1:1:numNodes
i
% Break if goal node is already reached
% IsReached=0;
% % for j = 1:1:length(nodes)
% % if dist(nodes(j).coord, q_goal.coord)<5
% % IsReached=1;
% % break
% % end
% % end
% if IsReached
% break;
% end
if rand(1)<0.3
% disp('sampling: goal 1')
q_rand=q_goal;
else
% disp('sampling: goal 0')
q_rand.coord = [floor(rand(1)*x_max) floor(rand(1)*y_max)];
q_rand.dir=(rand(1)-0.5)*2*pi;
end
% Pick the closest node from existing list to branch out from
ndist = [];
for j = 1:1:length(nodes)
n = nodes(j);
%new func
tmp = distDubinThree_obs(n, q_rand, rho, ob_vec);
ndist = [ndist tmp];
end
[val, idx] = min(ndist);
q_near = nodes(idx);
q_new.coord = steer_DubinThree(q_near, q_rand, rho, step);
q_new.dir=steer_DubinThree2(q_near, q_rand, rho, step);
%/* RG-RRT part: the q_goal is closer to q_near or q_new?
% while(dist(q_rand.coord,q_near.coord)<dist(q_rand.coord,q_new.coord)) || ~noCollision(q_rand.coord, q_near.coord, obstacle1) || ~noCollision(q_new.coord, q_near.coord, obstacle1) || ~noCollision(q_rand.coord, q_near.coord, obstacle2) || ~noCollision(q_new.coord, q_near.coord, obstacle2)
%/* not RG-RRT
while ~noCollision_obs(q_rand.coord, q_near.coord, ob_vec) || ~noCollision_obs(q_new.coord, q_near.coord, ob_vec)
% plot(q_rand.coord(1), q_rand.coord(2), 'o', 'Color', [1 0 0.7410])
q_rand.coord = [floor(rand(1)*x_max) floor(rand(1)*y_max)];
q_rand.dir=(rand(1)-0.5)*2*pi;
% Pick the closest node from existing list to branch out from
ndist = [];
for j = 1:1:length(nodes)
n = nodes(j);
%jan 28 tmp = dist_Dubin(n, q_rand, BETA, EPS);
tmp = distDubinThree_obs(n, q_rand, rho, ob_vec);
ndist = [ndist tmp];
end
[val, idx] = min(ndist);
q_near = nodes(idx);
q_new.coord = steer_DubinThree(q_near, q_rand, rho, step);
q_new.dir=steer_DubinThree2(q_near, q_rand, rho, step);
end
% plot(q_rand.coord(1), q_rand.coord(2), 'x', 'Color', [0 0.4470 0.7410])
%*/
% rewiring process
if noCollision_obs(q_rand.coord, q_near.coord, ob_vec) && noCollision_obs(q_new.coord, q_near.coord, ob_vec)
% line([q_near.coord(1), q_new.coord(1)], [q_near.coord(2), q_new.coord(2)], 'Color', 'k', 'LineWidth', 2);
% drawnow
hold on
q_new.cost = distDubinThree(q_near, q_new, rho) + q_near.cost;
% Within a radius of r, find all existing nodes
q_nearest = [];
r = 100;
neighbor_count = 1;
% Initialize cost to currently known value
q_min = q_near;
C_min = q_new.cost;
for j=1:length(nodes)
if noCollision_obs(nodes(j).coord, q_new.coord,ob_vec) && distDubinThree_obs(nodes(j),q_new, rho, ob_vec)<r
q_nearest(neighbor_count).coord=nodes(j).coord;
q_nearest(neighbor_count).dir=nodes(j).dir;
q_nearest(neighbor_count).cost=nodes(j).cost;
if nodes(j).cost+distDubinThree_obs(nodes(j),q_new, rho, ob_vec)<C_min-2
q_min=nodes(j);
C_min=nodes(j).cost+distDubinThree_obs(nodes(j),q_new, rho, ob_vec);
end
neighbor_count=neighbor_count+1;
end
end
% Update parent to least cost-from node % near q_near best one
for j = 1:1:length(nodes)
if nodes(j).coord == q_min.coord
q_new.parent = j;
end
end
% q_new.parent=idx; % old q_near
q_new.cost=C_min; % add by Zhu Wang
% line([q_min.coord(1),q_new.coord(1)], [q_min.coord(2),q_new.coord(2)],'Color','g');
% Append to nodes
nodes = [nodes q_new];
for j=1:length(nodes)-1
if noCollision_obs(nodes(j).coord, q_new.coord,ob_vec) && distDubinThree_obs(q_new,nodes(j), rho, ob_vec)<r
if nodes(j).cost>C_min+distDubinThree_obs(q_new, nodes(j),rho,ob_vec)
nodes(j).parent=length(nodes);
nodes(j).cost=C_min+distDubinThree_obs(q_new, nodes(j),rho,ob_vec);
% line([nodes(j).coord(1),q_new.coord(1)], [nodes(j).coord(2),q_new.coord(2)],'Color','y');
end
end
end
end
if mod(i,100)==1
text=['i=',int2str(i-1)];
title(text)
end
% if (mod(i,10)==1)
% F((i-1)/10+1)=getframe(gcf);
% end
end
D = [];
for j = 1:1:length(nodes)
tmpdist = distDubinThree_obs(nodes(j), q_goal, rho,ob_vec);
D = [D tmpdist];
end
%% Search backwards from goal to start to find the optimal least cost path
path=[q_goal.coord q_goal.dir];
[val, idx] = min(D);
q_final = nodes(idx);
q_goal.parent = idx;
q_end = q_goal;
totaldist=q_final.cost+distDubinThree_obs(q_final,q_goal,rho,ob_vec);
q_goal.cost=totaldist;
nodes = [nodes q_goal];
while q_end.parent ~= 0
start = q_end.parent;
% % disp(start)
% plot(q_end.coord(1), q_end.coord(2),'bv')
% line([q_end.coord(1), nodes(start).coord(1)], [q_end.coord(2), nodes(start).coord(2)], 'Color', 'r', 'LineWidth', 2);
% drawnow;
% hold on;
q_end = nodes(start);
% F=[F getframe(gcf)];
path=[q_end.coord q_end.dir; path];
end
% plot(path(:,1),path(:,2),'k--');
⛄ 运行结果
⛄ 参考文献
[1] 王永皎,郑春峰.基于RRT算法的自由飞行空间机器人避碰运动规划[J].农机化研究, 2006(3):3.DOI:10.3969/j.issn.1003-188X.2006.03.017.
[2] 刘强,阳小燕,朱克佳.基于RRT算法的桥检无人机航线规划仿真系统设计[J].计算机科学与应用, 2022(008):012.
[3] 李华忠,梁永生,但唐仁,等.基于RRT的机器人避碰运动规划算法研究[J].深圳信息职业技术学院学报, 2012, 10(3):8.DOI:10.3969/j.issn.1672-6332.2012.03.005.
[4] 马蓉.基于改进RRT算法的无人机航路规划与跟踪方法研究[J].导航定位与授时, 2020, 7(1):6.DOI:CNKI:SUN:DWSS.0.2020-01-004.
