【无人机三维路径规划】基于A星算法解决三维栅格地图无人机三维路径规划问题附matlab代码...

✅作者简介：热爱科研的Matlab仿真开发者，修心和技术同步精进，matlab项目合作可私信。

🍎个人主页：Matlab科研工作室

🍊个人信条：格物致知。

⛄ 内容介绍

⛄ 部分代码

%Main

clc

clear

close all

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%Grid and path parameters

%Grid size [y,x,z] and resolution

sizeE=[80 80 80];

d_grid=1;

%Starting point

% x0=5;

% y0=7;

% z0=1;

x0=5;

y0=7;

z0=1;

%Arrival point

% xend=68;

% yend=66;

% zend=6;

xend=60;

yend=71;

zend=10;

%Number of points with low elevation around start and end point area 

n_low=3;

%A* or Theta* cost weights

kg=1;

kh=1.25;

ke=sqrt((xend-x0)^2+(yend-y0)^2+(zend-z0)^2);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%Map definition

%Average flight altitude

h=max(z0,zend);

%Points coordinates in [y,x,z] format

P0=[y0 x0 z0];

Pend=[yend xend zend];

%Generate map，生成地图

[E,E_safe,E3d,E3d_safe]=grid_3D_safe_zone(sizeE,d_grid,h,P0,Pend,n_low);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%Path generation，路径生成

%Store gains in vector,相关参数，用于算法

K=[kg kh ke];

%Measure path computation time，计时

tic  

%Generate path (chose one)，选一个算法求解

 %[path,n_points]=a_star_3D(K,E3d_safe,x0,y0,z0,xend,yend,zend,sizeE);

 [path,n_points]=theta_star_3D(K,E3d_safe,x0,y0,z0,xend,yend,zend,sizeE);

%[path,n_points]=lazy_theta_star_3D(K,E3d_safe,x0,y0,z0,xend,yend,zend,sizeE);

toc

T_path=toc;

%Path waypoints partial distance

%Initialize，初始化

path_distance=zeros(n_points,1);%标记每个拐点，(图中的×)的走过距离

for i=2:n_points %标记每个拐点，(图中的×)的走过距离

path_distance(i)=path_distance(i-1)+sqrt((path(i,2)-path(i-1,2))^2+(path(i,1)-path(i-1,1))^2+(path(i,3)-path(i-1,3))^2);      

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%Plot

%Map grid

x_grid=1:d_grid:sizeE(2);

y_grid=1:d_grid:sizeE(1);

%Path on safe map

figure(1)

surf(x_grid(2:end-1),y_grid(2:end-1),E_safe(2:end-1,2:end-1))

hold on

plot3(x0,y0,z0,'gs')

text(x0,y0,z0,'起点','color','g','FontSize',12)

plot3(xend,yend,zend,'rd')

text(xend,yend,zend,'终点','color','r','FontSize',12)

plot3(path(:,2),path(:,1),path(:,3),'yx')

plot3(path(:,2),path(:,1),path(:,3),'r')

axis tight

axis equal

view(0,90);

colorbar

%Path on standard map

figure(2)

surf(x_grid(2:end-1),y_grid(2:end-1),E(2:end-1,2:end-1))

hold on

plot3(x0,y0,z0,'gs')

text(x0,y0,z0,'起点','color','g','FontSize',12)

plot3(xend,yend,zend,'rd')

text(xend,yend,zend,'终点','color','r','FontSize',12)

plot3(path(:,2),path(:,1),path(:,3),'yx')

plot3(path(:,2),path(:,1),path(:,3),'r')

axis tight

axis equal

view(0,90);

colorbar

%Path height profile，高度和飞行距离的关系

figure(3)

plot(path_distance,path(:,3))

grid on

xlabel('Path distance')

ylabel('Path height')

⛄ 运行结果

⛄ 参考文献

[1] 张彪, 曹其新, 王雯珊. 基于三维栅格地图的移动机器人路径规划[J]. 西安交通大学学报, 2013, 47(10):5.

[2] 周良, 王耀南, 印峰,等. 基于类三维地图的无人机路径规划[J]. 计算机测量与控制, 2011, 19(11):4.

[3] 张华, 史思总, 周一廷,等. 一种动态参数更新的无人机三维路径规划方法[J]. 自动化仪表, 2015, 36(9).

⛳️ 代码获取关注我

❤️部分理论引用网络文献，若有侵权联系博主删除

❤️ 关注我领取海量matlab电子书和数学建模资料