【三维路径规划】基于matlabA_star算法无人机三维路径规划【含

【三维路径规划】基于matlabA_star算法⽆⼈机三维路径规划【含Matlab源码

1387期】

⼀、A_star算法简介

0 引⾔

随着现代技术的发展，飞⾏器种类不断变多，应⽤也⽇趋专⼀化、完善化，如专门⽤作植保的⼤疆PS-X625⽆⼈机，⽤作街景拍摄与监控巡察的宝鸡⾏翼航空科技的X8⽆⼈机，以及⽤作⽔下救援的⽩鲨MIX⽔下⽆⼈机等，决定飞⾏器性能主要是内部的飞控系统和外部的路径规划问题。就路径问题⽽⾔，在具体实施任务时仅靠操作员⼿中的遥控器控制⽆⼈飞⾏器执⾏相应的⼯作，可能会对操作员⼼理以及技术提出极⾼的要求，为了避免个⼈操作失误，进⽽造成飞⾏器损坏的危险，⼀种解决问题的⽅法就是对飞⾏器进⾏航迹规划。

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

timerVal = 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 on

surf(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,[01]);

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,[01]);

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 off

grid on

view(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));

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

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

%LISTS USED FOR ALGORITHM %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%

function [WayPoints,OPEN_COUNT]=A_star(MAX_X,MAX_Y,MAX_Z,xval,yval,zval,xTarget,yTarget,zTarget,MAP,CLOSED,Display_Data,MIN_Final_Data )

%%%%%%%SET Optimal_path NODE%%%%%%%%%%

反光道钉WayPoints =[];

%%%%%%%SET STARTING NODE%%%%%%%%%%

xStart = xval;

yStart = yval;

zStart = zval; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%

%LISTS USED FOR ALGORITHM %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%

%OPEN LIST STRUCTURE

%--------------------------------------------------------------------------

%--------------------------------------------------------------------------

OPEN=[];

%CLOSED LIST STRUCTURE

%--------------

%X val | Y val | Z val |

%--------------

% CLOSED=zeros(MAX_VAL,2);

CLOSED_COUNT=size(CLOSED,1);

%set the starting node as the first node

xNode=xval;

yNode=yval;

zNode=zval;

xFNode = xval;

yFNode = yval;

zFNode = zval;

OPEN_COUNT=1;

path_cost=0;

goal_distance=distanced(xNode,yNode,zNode,xTarget,yTarget,zTarget);

%%%%%%%%%%%%%%%%%%%%%%%%%%insert_open(当前x，当前y,⽗节点x,⽗节点y，路径h(n)，启发g(n),f(n))%%%%%%%%%%%%%%%%%%%%% OPEN(OPEN_COUNT,:)=insert_open(xNode,yNode,zNode,xNode,yNode,zNode,path_cost,goal_distance,goal_distance);

OPEN(OPEN_COUNT,1)=0;

CLOSED_COUNT=CLOSED_COUNT+1;

CLOSED(CLOSED_COUNT,1)=xNode;

CLOSED(CLOSED_COUNT,2)=yNode;

CLOSED(CLOSED_COUNT,3)=zNode;

NoPath=1;%%%%%是否有路径判断符1有，0⽆%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

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

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

while((xNode ~= xTarget || yNode ~= yTarget || zNode ~= zTarget)&& NoPath ==1)

%%%%%%%%%%%expand_array(⽗节点x,⽗节点y，当前节点x，当前节点y,h(n),⽬标x,⽬标y,CLOSED，表X，表y，地形上界y)%%%%%%%%% %%%%%%%%%%%返回值exp_array：扩展x，扩展y，扩展z,h(n),g(n),f(n)

exp_array=expand_array(xFNode,yFNode,zFNode,xNode,yNode,zNode,path_cost,xTarget,yTarget,zTarget,CLOSED,MAX_X,MAX_Y,MAX_Z,Display_Da ta);

exp_count=size(exp_array,1);

冰鲜台%UPDATE LIST OPEN WITH THE SUCCESSOR NODES

%OPEN LIST FORMAT

%--------------------------------------------------------------------------

%--------------------------------------------------------------------------

%EXPANDED ARRAY FORMAT

%--------------------------------

%|X val |Y val ||h(n)|g(n)|f(n)|

%--------------------------------

for i=1:exp_count

flag=0;%%%%%%%判断exp_array中节点在不在OPEN列表中，0不在，1在

for j=1:OPEN_COUNT

if(exp_array(i,1)==OPEN(j,2)&&exp_array(i,2)==OPEN(j,3)&&exp_array(i,3)==OPEN(j,4))

OPEN(j,10)=min(OPEN(j,10),exp_array(i,6));%#ok<*SAGROW>

if OPEN(j,10)==exp_array(i,6)

%UPDATE PARENTS,gn,hn

OPEN(j,5)=xNode;

OPEN(j,6)=yNode;

OPEN(j,7)=zNode;

OPEN(j,8)=exp_array(i,4);

OPEN(j,9)=exp_array(i,5);

活塞式抽水机end;%End of minimum fn check

flag=1;

end;%End of node check

%if flag ==1

%break;

end;%End of j for

if flag ==0

OPEN_COUNT = OPEN_COUNT+1;

OPEN(OPEN_COUNT,:)=insert_open(exp_array(i,1),exp_array(i,2),exp_array(i,3),xNode,yNode,zNode,exp_array(i,4),exp_array(i,5),exp_array(i,6)); end;%End of insert new element into the OPEN list

end;%End of i for

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

%END OF WHILE LOOP %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%Find out the node with the smallest fn

index_min_node =min_fn(OPEN,OPEN_COUNT,xTarget,yTarget,zTarget);

if(index_min_node ~=-1)

%Set xNode and yNode to the node with minimum fn

xNode=OPEN(index_min_node,2);

yNode=OPEN(index_min_node,3);

zNode=OPEN(index_min_node,4);

xFNode=OPEN(index_min_node,5);

yFNode=OPEN(index_min_node,6);

zFNode=OPEN(index_min_node,7);

path_cost=OPEN(index_min_node,8);%Update the cost of reaching the parent node

%Move the Node to list CLOSED

CLOSED_COUNT=CLOSED_COUNT+1;

CLOSED(CLOSED_COUNT,1)=xNode;

CLOSED(CLOSED_COUNT,2)=yNode;

CLOSED(CLOSED_COUNT,3)=zNode;

麻石脱硫除尘器

OPEN(index_min_node,1)=0;

else

%No path exists to the Target!!

NoPath=0;%Exits the loop!

end;%End of index_min_node check

end;%End of While Loop

三、运⾏结果

>醚链

本文发布于:2024-09-22 14:17:32，感谢您对本站的认可！

本文链接：https://www.17tex.com/tex/3/311441.html

上一篇：AGV路径规划分析

下一篇：【三维路径规划】基于matlabRRT_Star算法三维路径规划【含Matlab源码...

标签：路径规划算法节点航迹

留言与评论（共有 0 条评论）