A_star三维路径规划基于matlab A_star算法和B次样条危险模型无人机三维航迹规划含Matlab源码 2162期

Posted 2022-10-30 海神之光

一、A_star算法简介

1 A Star算法及其应用现状
进行搜索任务时提取的有助于简化搜索过程的信息被称为启发信息.启发信息经过文字提炼和公式化后转变为启发函数.启发函数可以表示自起始顶点至目标顶点间的估算距离, 也可以表示自起始顶点至目标顶点间的估算时间等.描述不同的情境、解决不同的问题所采用的启发函数各不相同.我们默认将启发函数命名为H (n) .以启发函数为策略支持的搜索方式我们称之为启发型搜索算法.在救援机器人的路径规划中, A Star算法能结合搜索任务中的环境情况, 缩小搜索范围, 提高搜索效率, 使搜索过程更具方向性、智能性, 所以A Star算法能较好地应用于机器人路径规划相关领域.

2 A Star算法流程
承接2.1节, A Star算法的启发函数是用来估算起始点到目标点的距离, 从而缩小搜索范围, 提高搜索效率.A Star算法的数学公式为:F (n) =G (n) +H (n) , 其中F (n) 是从起始点经由节点n到目标点的估计函数, G (n) 表示从起点移动到方格n的实际移动代价, H (n) 表示从方格n移动到目标点的估算移动代价.

如图2所示, 将要搜寻的区域划分成了正方形的格子, 每个格子的状态分为可通过(walkable) 和不可通过 (unwalkable) .取每个可通过方块的代价值为1, 且可以沿对角移动 (估值不考虑对角移动) .其搜索路径流程如下:

图2 A Star算法路径规划
Step1:定义名为open和closed的两个列表;open列表用于存放所有被考虑来寻找路径的方块, closed列表用于存放不会再考虑的方块;
Step2:A为起点, B为目标点, 从起点A开始, 并将起点A放入open列表中, closed列表初始化为空;
Step3:查看与A相邻的方格n (n称为A的子点, A称为n的父点) , 可通过的方格加入到open列表中, 计算它们的F, G和H值.将A从open移除加入到closed列表中;
Step4:判断open列表是否为空, 如果是, 表示搜索失败, 如果不是, 执行下一步骤;
Step5:将n从open列表移除加入到closed列表中, 判断n是否为目标顶点B, 如果是, 表示搜索成功, 算法运行结束;
Step6:如果不是, 则扩展搜索n的子顶点:
a.如果子顶点是不可通过或在close列表中, 忽略它.
b.子顶点如果不在open列表中, 则加入open列表, 并且把当前方格设置为它的父亲, 记录该方格的F, G和H值.
Step7:跳转到步骤Step4;
Step8:循环结束, 保存路径.从终点开始, 每个方格沿着父节点移动直至起点, 即是最优路径.A Star算法流程图如图3所示.

图3 A Star算法流程

二、部分源代码

clc;
clear;
tic
load(‘P3.mat’);
load(‘PP3.mat’);

global w1 w2 w3 xstart ystart xTarget yTarget
global Max_x Max_y theta Lmin M fmin threat_coef

xstart=5;
ystart=30;
xstart=5xstart-4; %起点比例转换
ystart=5ystart-4;

xTarget=90;
yTarget=90;
xTarget=5xTarget-4; %目标点比例转换
yTarget=5yTarget-4;

Max_x=100; %地图大小
Max_y=100;
theta=pi/3; %最大偏航角；

w1=0.001; %距离起始点的路程的权重
w2=300;
w3=0.1;
Lmin=1.7; %最小步长
Lmin=5*Lmin-4; %转换步长
M=7; %最大扩展节点数；
fmin=0.001; %总概率密度的最低阈值；

C=surf(P’);
set(C, ‘LineStyle’,‘none’); %去掉方格线
hold on

MAP=2*(ones(5Max_x-4,5Max_y-4)); %初始化地图矩阵
MAP(xTarget,yTarget)=0; %初始化地图矩阵中目标的标识，赋值为0
MAP(xstart,ystart)=1; %初始化地图矩阵中起点的标识，赋值为1

Close=[]; %Close列表结构：（X，Y）
Close_count=size(Close,1);
Open=[]; %Open列表格式：（1/0，X，Y，父节点X，父节点Y，h(n)，g(n)，f(n)
Open_count=1;
x_present=xstart;
y_present=ystart;
path_cost=0; %路径代价初始化
goal_distance=distance(x_present,y_present,xTarget,yTarget); %计算当前节点（即出发点）距离目标点的距离
Open(Open_count,:)=insert_open(x_present,y_present,x_present,y_present,path_cost,goal_distance,goal_distance); %生成新的一行，记录当前节点、h（n），g（n），f(n)等信息
Open(Open_count,1)=0;
Close_count=Close_count+1;
Close(Close_count,1)=x_present;
Close(Close_count,2)=y_present; %将出发点放入Close列表
B=[x_present,y_present];

% **************************** 主程序 ******************************************

while ((x_present-xTarget)^{2+(y_present-yTarget)}2)>(Lmin)^2 %主循环终止条件, ||代表或， &&代表且
%((x_present-xTarget)^{2+(y_present-yTarget)}2)>(Lmin)^2
B=recall(Open,xstart,ystart,x_present,y_present); %搜索之前经过的点， (x_present_{=xTarget)||(y_present}=yTarget)
exp_node=expand_node(P,x_present,y_present,Close,B); %搜索并保存扩展点
exp_count=size(exp_node);

for i=1:exp_count                                                  %将扩展点筛选并保存到Open列表中
    flag=0;
    for j=1:Open_count
        if(exp_node(i,1) == Open(j,2) && exp_node(i,2) == Open(j,3) )   
            Open(j,8)=min(Open(j,8),exp_node(i,5));
            if Open(j,8)== exp_node(i,5)
                Open(j,4)=x_present;
                Open(j,5)=y_present;
                Open(j,6)=exp_node(i,3);
                Open(j,7)=exp_node(i,4);
            end
            flag=1;
        end
    end
    if flag == 0
        Open_count = Open_count+1;
        Open(Open_count,:)=insert_open(exp_node(i,1),exp_node(i,2),x_present,y_present,exp_node(i,3),exp_node(i,4),exp_node(i,5));
    end
end

index_min_node = min_fn(Open,Open_count,xTarget,yTarget);   %寻找Open列表中f(n)最小的节点
x_present=Open(index_min_node,2);
y_present=Open(index_min_node,3);
path_cost=Open(index_min_node,6);                          %更新到达父节点的代价
Close_count=Close_count+1;
Close(Close_count,1)=x_present;
Close(Close_count,2)=y_present;
Open(index_min_node,1)=0;

% B=[B;[x_present,y_present]]
end
%******************************** 回溯生成航迹 **********************************

m=size(Close,1);
Optimal_path=recall(Open,xstart,ystart,Close(m,1),Close(m,2));

mm=size(Optimal_path,1);

Threat_value=0;
for i=1:mm
Threat_value=Threat_value+P(Optimal_path(i,1),Optimal_path(i,2)); %计算最终航迹的总威胁概率
end

i=2:mm;
Trajectory_length=sum(distance(Optimal_path(i,1),Optimal_path(i,2),Optimal_path(i-1,1),Optimal_path(i-1,2))); %计算最终航迹长度

%**************** 利用样条函数对航迹进行平滑 ****************************

n=size(Optimal_path,1);
t=1:n;
ts=1:0.01:n;
xs=spline(t,Optimal_path(:,1),ts);
ys=spline(t,Optimal_path(:,2),ts);
plot(xs,ys,‘r-’)
plot(xTarget,yTarget,‘rp’)

%**************************************************************************

caxis([0,0.12]) %色彩范围控制
colorbar(‘EastOutside’); %彩色条位置控制
xlabel(‘X轴’,‘Color’,‘black’);
ylabel(‘Y轴’,‘Color’,‘black’);
zlabel(‘Z轴’,‘Color’,‘black’);
view(0,90) %视角控制
grid off
toc
%********************** 参数输出 *****************************************
Trajectory_length
Threat_value

三、运行结果

四、matlab版本及参考文献

1 matlab版本
2014a

2 参考文献
[1]钱程,许映秋,谈英姿.A Star算法在RoboCup救援仿真中路径规划的应用[J].指挥与控制学报. 2017,3(03)

3 备注
简介此部分摘自互联网，仅供参考，若侵权，联系删除