无人驾驶全局路径规划之A星算法

根据查阅网上关于A星算法的原理，结合自己的理解，使用MATLAB实现A星算法生成路径。原理部分我不再赘述，网上介绍的文章还是很多的。
下面主要展示自己编的A星算法，有疑问欢迎交流。

注：我这里虽然用了栅格地图，但生成新节点时，是按“上下左右”四个方向按一定生长步长生成新节点，这和常见的按栅格生成新栅格的方法有些不同，请注意区分。

clear;
close all;
%注意障碍物的权值按行列生成，而路径曲线是以左下角为原点生成，注意两者的区别
load parkingLotCostVal.mat % costVal
%生成带权值的网格地图
costmap = vehicleCostmap(costVal,'CellSize',1);%每个网格边长1米
%设置障碍物的膨胀范围
ccConfig =inflationCollisionChecker('CenterPlacements',[0.2 0.5 0.8],'InflationRadius',0);
costmap.CollisionChecker = ccConfig;
%画出整个地图
plot(costmap);hold on;
set(gca,"XTick",0:1:150);
set(gca,"YTick",0:1:100);
grid on;%hold on;
legend("off");
%检测有障碍物的区域
occMat = checkOccupied(costmap);

x_start=[6,10];%起点坐标
goal=[90,43];%终点坐标
r=1;%生成步长
likai_length=0.01;%两点距离小于0.01，则认为两点为同一点
goal_radius=1;%搜索停止阈值

%画出起点
plot(x_start(1),x_start(2),'k>','MarkerFaceColor','g','MarkerSize',9);hold on;
%画出终点
plot(goal(1),goal(2),'ko','MarkerFaceColor','r','MarkerSize',9);hold on;

%创建各个中间量记录表
closed=[];%记录探索过的点
closed_parent_index=[];%记录探索过的点的父节点在cLosed表中的位置
open=[];%记录即将探索的点
open_parent_index=[];%记录即将探索的点的父节点在cLosed表中的位置
F_value=[];%记录F值
open_G_value=[];%记录open表中每个点的G值

%初始化各个表
closed=[closed;x_sta