无人机三维路径规划RRT算法实现无人机三维路径规划附matlab代码

clear all
clc
format long
close all
%% 环境生成
start = [120 20 50];
goal  = [50 800 30];
% start = [100 50 80];
% goal  = [300 900 200];
% goal  = [700 800 30];
Task.Boundary = reshape([ min(min(start).*ones(1,3),min(goal).*ones(1,3));max(max(start).*ones(1,3),max(goal).*ones(1,3))],[1,6]) ; 
Task.IniState = start ;
Task.FinState = goal ;
Task.Timeinte = [ 0 , 1000 ] ;
Task.DisScale = [50,50] ; % 环境参考点数
Task.Peaks    = [413,275,689,469,800,0;...
                 248,496,441,717,0  ,200] ; % 列数为山峰个数
             % randi([1,800],[2,5])
% Task.Peaks(3,:) = [200,200,300,150,400,200] ; % 山峰高度 randi([1,200],[1,5])
Task.Peaks(3,:) = [400,300,500,150,300,200] ;
Task.Peaks(4,:) = 150* ones(1,size(Task.Peaks,2)) ; % 山峰陡峭度 X
Task.Peaks(5,:) = 120* ones(1,size(Task.Peaks,2))  ; % 山峰陡峭度 Y


Cycle2.path(1,:) = [ 0 650 80 ] ;
Cycle2.vel   =0.2  ;
Cycle2.t(1)  = 0   ;
Cycle2.R(1)     = 5 ;
enR = 0.1 ;
for ii = 2 : 3000
    Cycle2.t(ii) = Cycle2.t(ii-1)   dt ;
    Cycle2.path(ii,:) = Cycle2.path(ii-1,:)   dt * [ Cycle2.vel, 0 , 0] ;
    Cycle2.R(ii,:) = Cycle2.R(ii-1,:)   dt * enR ;
    if Cycle2.path(ii,1) < -200
        Cycle2.vel = - Cycle2.vel ;
        Cycle2.path(ii,:) = Cycle2.path(ii-1,:) ;
    end
    if Cycle2.path(ii,1) > 100
        Cycle2.vel = - Cycle2.vel ;
        Cycle2.path(ii,:) = Cycle2.path(ii-1,:) ;
    end
end
t0 = 0 ;
% start = [0 0 30];
% start = [120 20 50];
% goal  = [50 800 30];
start = [100 50 80];
goal  = [300 900 200];
hold on
plot3(start(1),start(2),start(3),'sg','MarkerSize',20,'Linewidth',2)
plot3(goal(1),goal(2),goal(3),'sr','MarkerSize',20,'Linewidth',2)
path = RRTplanDyn(start,goal,PeakData,rou,Task,cons,'b','r',dt,Cycle1,Cycle2,t0);
% path = RRTplanDyn(start,goal,PeakData,'b','r',dt,Cycle1,Cycle2,t0);
figure(2)
hold on
grid off
box  on
axis equal
view(-38,58)
colormap('summer')
% colormap('hot')
surf(PeakData.X,PeakData.Y,PeakData.H,'EdgeColor','interp')
plot3(start(1),start(2),start(3),'sg','MarkerSize',20,'Linewidth',2)
plot3(goal(1),goal(2),goal(3),'sr','MarkerSize',20,'Linewidth',2)
% plot3(200,350,80,'vr','MarkerSize',12,'Linewidth',1)
plot3(200,380,80,'vr','MarkerSize',12,'Linewidth',1)
plot3(0,650,80 ,'vr','MarkerSize',12,'Linewidth',1)
for ii = 2 : size(path,1)
    hold on
    plot3(path(ii-1:ii,1), path(ii-1:ii,2), path(ii-1:ii,3),'LineWidth',2,'Color','b');
    tempC = interp1(Cycle1.t,Cycle1.path,path(ii,4)) ;
    enemyR = interp1(Cycle1.t,Cycle1.R,path(ii,4)) ;
    pt = plot3(tempC(1),tempC(2),tempC(3),'vr','LineWidth',1.5,'MarkerSize',7);
    pt2 = circlr3(tempC(1),tempC(2),tempC(3),enemyR,1.5) ;
    tempC = interp1(Cycle2.t,Cycle2.path,path(ii,4)) ;
    enemyR = interp1(Cycle2.t,Cycle2.R,path(ii,4)) ;
    pt3 = plot3(tempC(1),tempC(2),tempC(3),'vr','LineWidth',1.5,'MarkerSize',7);
    pt4 = circlr3(tempC(1),tempC(2),tempC(3),enemyR,1.5) ;
    pause(0.1);
    delete (pt)
    delete (pt2)
    delete (pt3)
    delete (pt4)
end


figure(3)
hold on
grid on
box  on
axis equal
view(-38,58)
colormap('summer')
% colormap('hot')
surf(PeakData.X,PeakData.Y,PeakData.H,'EdgeColor','interp')
plot3(start(1),start(2),start(3),'sg','MarkerSize',20,'Linewidth',2)
plot3(goal(1),goal(2),goal(3),'sr','MarkerSize',20,'Linewidth',2)
plot3(40,400,100,'vr','MarkerSize',12,'Linewidth',1)
plot3(0,650,80 ,'vr','MarkerSize',12,'Linewidth',1)
tplot = linspace(path(1,4),path(end,4),10) ;
pplot  = spline(path(:,4)',path(:,1:3)',tplot) ;
tplot2 = linspace(tplot(1),tplot(end),100) ;
pplot   = spline(tplot,pplot,tplot2) ;
plot3(pplot(1,:),pplot(2,:),pplot(3,:),'LineWidth',2,'Color','b')

% target = [pplot;tplot2]' ;
% save('target.mat','target')