利用Astar算法实现飞行轨迹的三维规划附Matlab代码

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

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

🍊个人信条：格物致知。

更多Matlab仿真内容点击👇

智能优化算法  神经网络预测 雷达通信  无线传感器

信号处理 图像处理 路径规划 元胞自动机 无人机  电力系统

⛄ 内容介绍

飞行器的测量精度，航迹路径的合理规划，飞行器工作时的稳定性、安全性等这些变化对飞行器的综合控制系统要求越来越高。无人机航路规划是为了保证无人机完成特定的飞行任务，并且能够在完成任务的过程中躲避各种障碍、威胁区域而设计出最优航迹路线的问题。常见的航迹规划算法如图1所示。

图1 常见路径规划算法 

文中主要对无人机巡航阶段的航迹规划进行研究，假设无人机在飞行中维持高度与速度不变，那么航迹规划成为一个二维平面的规划问题。在航迹规划算法中,A*算法计算简单，容易实现。在改进A*算法基础上，提出一种新的、易于理解的改进A*算法的无人机航迹规划方法。传统A*算法将规划区域栅格化，节点扩展只限于栅格线的交叉点，在栅格线的交叉点与交叉点之间往往存在一定角度的两个运动方向。将存在角度的两段路径无限放大、细化，然后分别用两段上的相应路径规划点作为切点，找到相对应的组成内切圆的圆心，然后作弧，并求出相对应的两切点之间的弧所对应的圆心角，根据下式计算出弧线的长度

式中：R———内切圆的半径；

α———切点之间弧线对应的圆心角。

**1 A*算法概述**

A*算法是在Dijstar算法的基础上引入的启发式函数，通过定义的代价函数来评估代价大小，从而确定最优路径。A*算法的代价函数

式中：f(x,y）———初始状态X0(x0,y0）到达目标状态X1(x1,y1）的代价估计；

g(x,y）———状态空间中从初始状态X0(x0,y0）到状态N(x1,y1）的实际代价；

h(x,y）———从状态N(x1,y1）到目标状态X1(x1,y1）最佳路径的估计代价。

要找到最短路径的实质是找到f(x,y）的最小值，其中在式（2）中寻找最短路径的关键在于求估计代价h (x,y）值。设系数λ表示状态N(x1,y1）到X1(x1,y1）最优距离，如果λ<h(x,y），搜索范围小，不能保证得到最优解；λ>h(x,y），搜索范围大，费时，但能找到最优解；λ=h(x,y），搜索到最短路径。其中h(x,y）一般用欧几里德距离（式（3））或者绝对值距离（式（4））计算。

A*算法是以起始点为中心，周围8个栅格的中心为下一步预选，并不断地计算预选位置的f(x,y）值，其中f(x,y）值最小的作为当前位置，依次逐层比较，直到当前位置的临近点出现目标点为止，其最小单元如图2所示。

图2 最小单元

A*算法的流程如下：

1）创建开始节点START，目标节点TARGET、OPEN列表、CLOSE列表、CLOSE列表初始为空；

2）将START加入到OPEN列表；

3）检查OPEN列表中的节点，若列表为空，则无可行路径；若不为空，选择使f(x,y）值最小的节点k;

4）将节点k从OPEN中去除，并将其添加到CLOSE中，判断节点k是否目标节点TARGET，若是，则说明找到路径；若不是，则继续扩展节点k，生成k节点的子节点集，设q为k的子节点集，对所有节点q计算相应的f(x,y）值，并选择f(x,y）值最小的节点，将该节点放入CLOSE列表中；

5）跳到3），直到算法获得可行路径或无解退出。

⛄ 部分代码

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

% A* Terrain Profile ALGORITHM Demo

% Traditional A* search demo 3D

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

clear

load ('MapData.mat');

WayPoints = [];

WayPointsAll = [];

OPEN_COUNT = 0;

OPEN_COUNT_ALL = 0;

%%%%%%Terrain Data Fill%%%%%%%

Cut_Data = Final_Data(301:400,101:200);

MIN_Final_Data = min(min(Cut_Data));

%%%%%%%ALGORITHM START%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%Compute time%%%%%%%%%%%

tic

end

lat_lonD = [];

lat_lonDisReal = [];

lat_lonDisReal(1) = 0;

plat = (37.3565 - (25/54)*Total_X_WayPoints./100)';

plon = (101.7130 + (25/54)*Total_Y_WayPoints./100)';

pi=3.1415926;

num = size(plat)-1;

for i = 1:num(1,1)

    lat_lonD(i)=distance(plat(i),plon(i),plat(i+1),plon(i+1));

    lat_lonD(i)=lat_lonD(i)*6371*2*pi/360;

    lat_lonDisReal(i+1) = lat_lonDisReal(i) + lat_lonD(i);

end

MIN_Final_Data = min(min(Final_Data(301:400,101:200)));

Total_Z_WayPoints = Total_Z_WayPoints.*100 + MIN_Final_Data;

h1 = plot(lat_lonDisReal,Total_Z_WayPoints,'b');

hold on

plot(lat_lonDisReal,Terrain_Z_WayPoints,'c');

h2 = plot(lat_lonDisReal,Terrain_Z_WayPoints + 1000,'r');

X_fill = lat_lonDisReal;

Y_fill = Terrain_Z_WayPoints;

Y_size = size(Y_fill);

Y_fill_low = zeros(Y_size(1,1),Y_size(1,2));

X_fillfor = [fliplr(X_fill),X_fill];

Y_fillfor = [fliplr(Y_fill_low),Y_fill];

h3 = fill(X_fillfor,Y_fillfor,'c','FaceAlpha',1,'EdgeAlpha',0.3,'EdgeColor','k');

hleg = legend([h1,h2,h3],'规划航迹垂直剖面投影','低空飞行上边界','地形垂直剖面');

set(hleg,'Location','NorthWest','Fontsize',8);

hold off

xlabel('飞行路程（km）');

ylabel('飞行高度（m）');

xmaxTeam = lat_lonDisReal(1,num+1);

xmax = xmaxTeam(1,1);

axis([0 xmax 2500 5500]);

grid

⛄ 运行结果

⛄ 参考文献

​[1]赵德群, 段建英, 陈鹏宇,等. 基于A*算法的三维地图最优路径规划[J]. 计算机系统应用, 2017, 26(7):7.

⛄ Matlab代码关注

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

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