✅作者简介：热爱科研的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).
