【无人机三维路径规划】基于A算法解决三维路径规划问题含危险障碍地形含Matlab源码

1 简介

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

​2 部分代码

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% A* Terrain Profile ALGORITHM Demo% Traditional A* search demo 3D​%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%clearload ('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%%%%%%%%%%%tictimerVal = tic[WayPoints,OPEN_COUNT] = A_star(MAX_X,MAX_Y,MAX_Z,20,20,7,90,70,5,MAP,CLOSED,Display_Data);toc(timerVal)elapsedTime = toc(timerVal)figure(1)axis([1 MAX_X 1 MAX_Y 1 MAX_Z]);plot3(WayPoints(:,1),WayPoints(:,2),WayPoints(:,3),'b','linewidth',2);hold onsurf(Display_Data(1:100,1:100)','linestyle','none');plot3(20,20,7,'*');plot3(90,70,5,'^');set(gca,'xticklabel','');set(gca,'yticklabel','');set(gca,'zticklabel',{'2000','4000','6000','4000','5000','6000','7000','8000','9000','10000'});xlabel('纬度');ylabel('经度');zlabel('高度（m）');grid on%%%%%%%%%%%%%%绘制禁飞区[a,z]=ndgrid((0:.05:1)*2*pi,0:.05:20);x=5*cos(a)+30;y=5*sin(a)+30;surf(x,y,z,x*0,'linestyle','none','Facealpha',0.5)hold on[a,r]=ndgrid((0:.05:1)*2*pi,[0 1]);x=5*cos(a).*r+30;y=5*sin(a).*r+30;surf(x,y,x*0,x*0,'linestyle','none','Facealpha',0.5)surf(x,y,x*0+20,x*0,'linestyle','none','Facealpha',0.5)%%%%%%%%%%%%%%%%绘制异常天气区[a,z]=ndgrid((0:.05:1)*2*pi,0:.05:20);x=7.5*cos(a)+60;y=7.5*sin(a)+70;surf(x,y,z,x*0,'linestyle','none','Facealpha',0.7,'FaceColor','g')hold on[a,r]=ndgrid((0:.05:1)*2*pi,[0 1]);x=7.5*cos(a).*r+60;y=7.5*sin(a).*r+70;surf(x,y,x*0,x*0,'linestyle','none','Facealpha',0.7,'FaceColor','g')surf(x,y,x*0+20,x*0,'linestyle','none','Facealpha',0.7,'FaceColor','g')hold offgrid onview(70,60)%%%%%%%绘制垂直剖面航图figure(2)X_WayPoints = WayPoints(end:-1:1,1);Y_WayPoints = WayPoints(end:-1:1,2);Z_WayPoints = WayPoints(end:-1:1,3);Total_X_WayPoints = [20 X_WayPoints'];Total_Y_WayPoints = [20 Y_WayPoints'];Total_Z_WayPoints = [7 Z_WayPoints'];Terrain_Data = Final_Data(301:400,101:200);num = size(Total_X_WayPoints);for i= 1:num(1,2)    Terrain_Z_WayPoints(i) = Terrain_Data(Total_X_WayPoints(1,i),Total_Y_WayPoints(1,i));endlat_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);endMIN_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 onplot(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 offxlabel('飞行路程（km）');ylabel('飞行高度（m）');xmaxTeam = lat_lonDisReal(1,num+1);xmax = xmaxTeam(1,1);axis([0 xmax 2500 5500]);grid on​

3 仿真结果

4 参考文献

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

博主简介：擅长智能优化算法、神经网络预测、信号处理、元胞自动机、图像处理、路径规划、无人机等多种领域的Matlab仿真，相关matlab代码问题可私信交流。

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