【路径规划】3D 网格地图上规划路径（Matlab实现）

原创已于 2024-05-10 17:12:19 修改 · 577 阅读

8 ·

CC 4.0 BY-SA版权

文章标签：

#matlab #开发语言

于 2024-05-10 17:03:29 首次发布

💥💥💞💞欢迎来到本博客❤️❤️💥💥

🏆博主优势：🌞🌞🌞博客内容尽量做到思维缜密，逻辑清晰，为了方便读者。

⛳️座右铭：行百里者，半于九十。

📋📋📋本文目录如下：🎁🎁🎁

💥1 概述

在3D网格地图上进行路径规划是一项重要的任务，特别适用于机器人导航、虚拟现实环境、无人机航行等领域。在3D网格地图中，每个单元格表示一个可行走的区域，而障碍物则占据了一定的单元格。路径规划的目标是找到一条从起点到终点的最优路径，使得路径穿过的单元格数量最少或路径长度最短，同时避开障碍物。根据选择的路径规划算法，在地图上执行路径搜索。从起点开始，不断向周围探索，直到找到终点或确定无法到达终点为止。搜索过程中，需要考虑单元格的通行性和障碍物。一旦找到一条路径，可以对其进行优化以使其更加平滑或更短。例如，可以使用路径平滑算法来减少路径上的拐点，或者使用最短路径算法来进一步缩短路径长度。在动态环境中，地图可能会发生变化，例如障碍物的移动或增加。因此，路径规划系统需要能够实时更新地图和重新计算路径，以适应变化的环境。可以在3D网格地图上成功进行路径规划，从而实现机器人、无人机等移动设备的有效导航和路径跟踪。

📚2 运行结果

主函数部分代码：

%Main

clc
clear


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Grid and path parameters

%Grid size [y,x,z] and resolution
sizeE=[80 80 20];
d_grid=1;

%Starting point
x0=5;
y0=7;
z0=12;

%Arrival point
xend=68;
yend=66;
zend=6;

%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);