A星算法格栅地图路径考虑障碍物的动态切点光滑算法-matlab
代码
⼀、动态切点调整算法
动态切点调整算法去除凹凸点，该⽅法能够得到既有曲率连续性，同时满⾜⼏何特性的路径。
⼆、加⼊节点缩短后的代码效果
三、部分代码
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 2020-10-17 传统A*算法%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear ;
clc;
close all
figure
%%%只能设置正⽅形矩阵，⾏和列相等，否则旋转时会出现错误
MAX0 = [ 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 1
1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 1
1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 1
1 0 0 0 1 0 0 0 0 1 1 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 1
1 0 0 0 1 0 0 0 1 1 1 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 1
1 0 0 0 1 0 0 0 1 1 1 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 1
1 0 0 0 1 0 0 0 0 1 1 0 0 0 0 0 0 1 1 1 1 0 0 0 0 0 0 0 0 1
1 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 1 1 1
1 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1
1 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1
1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1
1 0 0 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1
1 0 0 0 0 0 0 1 0 0 0 0 0 0 1 1 1 1 1 1 1 0 0 0 0 0 0 0 0 1
1 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 1
1 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 1
1 0 0 0 0 0 0 1 1 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1
1 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 1
1 0 0 0 1 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 1
1 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 1 0 0 0 0 0 1
1 0 0 0 0 0 0 1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 0 1
1 0 0 0 0 0 0 1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1 1 1 0 0 0 1
1 0 0 0 0 0 0 0 0 0 0 1 1 1 0 0 0 0 1 0 0 0 0 1 1 0 0 0 0 1
1 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 1 0 0 0 0 0 1
1 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 1
1 0 1 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 1
1 0 1 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 1
1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 ] ;
%%% 通道设置为 0 ；障碍点设置为 1 ；起始点设置为 2 ；⽬标点设置为 -1 。
MAX=rot90(MAX0,3); %%%设置0,1摆放的图像与存⼊的数组不⼀样，需要先逆时针旋转90*3=270度给数组，最后输出来的图像就是⾃⼰编排的图像MAX_X=size(MAX,2); %%% 获取列数，即x轴长度
MAX_Y=size(MAX,1); %%% 获取⾏数，即y轴长度
MAX_VAL=10; %%% 返回由数字组成的字符表达式的数字值，就是函数⽤于将数值字符串转换为数值
%This array stores the coordinates of the map and the
%Objects in each coordinate
%%% 这个数组存储地图的坐标和每个坐标中的对象。
% // MAP=2*(ones(MAX_X+1,MAX_Y+1)); %%%%% ⽣成MAX_X⾏,MAX_Y列，且全部元素为2
% // MAP=2*(ones(MAX_X+1,MAX_Y+1)); %%%%% ⽣成MAX_X⾏,MAX_Y列，且全部元素为2
%%%// 改进2 ⾃⼰设置地图
% Obtain Obstacle, Target and Robot Position
% Initialize the MAP with input values
% Obstacle=-1,Target = 0,Robot=1,Space=2
x_val = 1;
y_val = 1;
axis([1 MAX_X+1, 1 MAX_Y+1]) %%% 设置x，y轴上下限
set(gca,'xtick',1:1:MAX_X+1,'ytick',1:1:MAX_Y+1,'GridLineStyle','-',...
'xGrid','on','yGrid','on')
grid on; %%% 在画图的时候添加⽹格线
hold on; %%% 当前轴及图像保持⽽不被刷新，准备接受此后将绘制的图形，多图共存
n=0;%Number of Obstacles %%% 障碍的数量
k=1; %%%% 将所有障碍物放在关闭列表中；障碍点的值为1；并且显⽰障碍点
CLOSED=[];
for j=1:MAX_X
for i=1:MAX_Y
if (MAX(i,j)==1)
%%plot(i+.5,j+.5,'ks','MarkerFaceColor','b'); 原来是红点圆表⽰
fill([i,i+1,i+1,i],[j,j,j+1,j+1],'k'); %%%改成⽤⿊⽅块来表⽰障碍物
CLOSED(k,1)=i; %%% 将障碍点保存到CLOSE数组中
CLOSED(k,2)=j;
k=k+1;
end
end
end
%%% 选择⽬标位置
pause(1); %%% 程序暂停1秒
h=msgbox('Please Select the Target using the Left Mouse button'); %%% 显⽰提⽰语原句是：Please Select the Target using the Left Mouse button uiwait(h,5); %%% 程序暂停
if ishandle(h) == 1 %%% ishandle(H) 将返回⼀个元素为 1 的数组；否则，将返回 0。
delete(h);
end
xlabel('Please Select the Target using the Left Mouse button','Color','black'); %%% 显⽰图x坐标下⾯的提⽰语原句是：Please Select the Target using the Left M but=0;
while (but ~= 1) %Repeat until the Left button is not clicked %%% 重复，直到没有单击“向左”按钮
[xval,yval,but]=ginput(1); %%% ginput提供了⼀个⼗字光标使我们能更精确的选择我们所需要的位置，并返回坐标值。
end
xval=floor(xval); %%% floor（）取不⼤于传⼊值的最⼤整数，向下取整
yval=floor(yval);
xTarget=xval;%X Coordinate of the Target %%% ⽬标的坐标
yTarget=yval;%Y Coordinate of the Target
MAP(xval,yval) = -1 ; %%% ⽬标坐标点位置的值设为-1
plot(xval+.5,yval+.5,'go'); %%% ⽬标点颜⾊b 蓝⾊ g 绿⾊ k ⿊⾊ w⽩⾊ r 红⾊ y
黄⾊ m紫红⾊ c蓝绿⾊
% text(xval+1,yval+.5,'Target') %%% text(x,y,'string')在⼆维图形中指定的位置(x,y)上显⽰字符串string
%%% 选择起始位置
h=msgbox(' Please Select the Vehicle initial position using the Left Mouse button'); %%%原⽂ Please Select the Vehicle initial position using the Left M uiwait(h,5);
if ishandle(h) == 1
delete(h);
end
xlabel(' Please Select the Vehicle initial position using the Left Mouse button ','Color','black'); %%% 原⽂ Please Select the Vehicle initial position
but=0;
while (but ~= 1) %Repeat until the Left button is not clicked %%%重复，直到没有单击“向左”按钮
[xval,yval,but]=ginput(1);
xval=floor(xval);
yval=floor(yval);
end
xStart=xval;%Starting Position
yStart=yval;%Starting Position
yStart=yval;%Starting Position
MAP(xval,yval)=2; %%% 起始点位置的值设置为1；⽬标点为0，障碍点为-1，其余空⽩点为2
plot(xval+.5,yval+.5,'b^');
% xlabel('起始点位置标记为△，⽬标点位置标记为 o ','Color','black');
%End of obstacle-Target pickup
tic %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %LISTS USED FOR ALGORITHM %%%⽤于算法的列表%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %OPEN LIST STRUCTURE %%%开放列表结构
%--------------------------------------------------------------------------
%IS ON LIST 1/0 |X val |Y val |Parent X val |Parent Y val |h(n) |g(n)|f(n)|
%--------------------------------------------------------------------------
OPEN=[];
%CLOSED LIST STRUCTURE %%% 封闭的列表结构
%--------------
%X val | Y val |
%--------------
% CLOSED=zeros(MAX_VAL,2); %%% ⽣成MAX_VAL⾏，2列的0矩阵
CLOSED_COUNT=size(CLOSED,1); %%% CLOSED的⾏数，即障碍点的个数
Nobs=CLOSED_COUNT;
%set the starting node as the first node %%%将起始节点设置为第⼀个节点
xNode=xval; %%% =xStart
yNode=yval; %%% =yStart
OPEN_COUNT=1; %%% OPEN_COUNT 开启列表的⾏数标志
path_cost=0;
goal_distance=distance(xNode,yNode,xTarget,yTarget); %%% 调⽤distance（）函数，求两坐标点之间的笛卡尔距离
OPEN(OPEN_COUNT,:)=insert_open(xNode,yNode,xNode,yNode,path_cost,goal_distance,goal_distance); %%% 插⼊到开放列表
%%% OPEN（第⼀⾏的元素）=（1，xNode,yNode,xNode,yNode,path_cost,goal_distance,goal_distanc）；
OPEN(OPEN_COUNT,1)=0; %%% OPEN(1,1)=0
CLOSED_COUNT=CLOSED_COUNT+1; %%% CLOSED 存储完障碍点后，下⼀个单元
CLOSED(CLOSED_COUNT,1)=xNode; %%% 下⼀个存储起始点的坐标
CLOSED(CLOSED_COUNT,2)=yNode;
NoPath=1; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % START ALGORITHM 开始算法%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% while((xNode ~= xTarget || yNode ~= yTarget) && NoPath == 1) %%% 判断当前点是否等于⽬标点
% plot(xNode+.5,yNode+.5,'go');
fill([xNode,xNode+1,xNode+1,xNode],[yNode,yNode,yNode+1,yNode+1],[0.85 0.85 0.85]);
% fill([xNode,xNode+1,xNode+1,xNode],[yNode,yNode,yNode+1,yNode+1],[0.85 0.85 0.85]);
% fill([xNode,xNode+1,xNode+1,xNode],[yNode,yNode,yNode+1,yNode+1],[1 1 0]);
drawnow;
% xnode=xNode,ynode=yNode %%%****输出当前节点，⽤来学习了解A*算法的运算过程**** ///不需要知道过程可注释掉///
exp_array=expand_array(xNode,yNode,path_cost,xTarget,yTarget,CLOSED,MAX_X,MAX_Y); %%% 不在关闭列表的⼦节点，（x,y,gn,hn,fn），列数是个数
exp_count=size(exp_array,1); %%% 可选择的⼦节点个数
%UPDATE LIST OPEN WITH THE SUCCESSOR NODES
%OPEN LIST FORMAT
%--------------------------------------------------------------------------
%IS ON LIST 1/0 |X val |Y val |Parent X val |Parent Y val |h(n) |g(n)|f(n)|
%--------------------------------------------------------------------------
%EXPANDED ARRAY FORMAT 扩展阵列格式
go2map地图北京%--------------------------------
%|X val |Y val ||h(n) |g(n)|f(n)|
%--------------------------------
for i=1:exp_count %%% 把exp_array内的元素添加到开启列表⾥⾯
flag=0; %%% 将exp_array内的点的标志位设为0
for j=1:OPEN_COUNT %%% OPEN_COUNT 从1开始，⾃加
if(exp_array(i,1) == OPEN(j,2) && exp_array(i,2) == OPEN(j,3) ) %%%判断可选⼦节点是否与OPEN[]中的点相同
OPEN(j,8)=min(OPEN(j,8),exp_array(i,5)); %%%如果相同，⽐较两个fn的值的⼤⼩，并将fn⼩的坐标点赋值给OPEN(j,8)
if OPEN(j,8)== exp_array(i,5) %%% 表⽰，上⼀步⽐较中 exp_array(i,5)⼩，则把exp_array(i,：）中的值赋给OPEN
%UPDATE PARENTS,gn,hn
OPEN(j,4)=xNode;
OPEN(j,5)=yNode;
OPEN(j,6)=exp_array(i,3);
OPEN(j,6)=exp_array(i,3);
OPEN(j,7)=exp_array(i,4);
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),xNode,yNode,exp_array(i,3),exp_array(i,4),exp_array(i,5)); 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 出fn最⼩的节点
index_min_node = min_fn(OPEN,OPEN_COUNT,xTarget,yTarget); %%%选出fn最⼩那⼀⾏，将⾏数赋给 index_min_node
if (index_min_node ~= -1)
%Set xNode and yNode to the node with minimum fn 将xNode和yNode设置为最⼩fn的节点
xNode=OPEN(index_min_node,2);
yNode=OPEN(index_min_node,3);
path_cost=OPEN(index_min_node,6);% Update the cost of reaching the parent node 更新到达⽗节点的成本 gn
%Move the Node to list CLOSED 将节点移动到列表CLOSED
CLOSED_COUNT=CLOSED_COUNT+1;
CLOSED(CLOSED_COUNT,1)=xNode;
CLOSED(CLOSED_COUNT,2)=yNode;
OPEN(index_min_node,1)=0;
% CLOSED %%%****输出CLOSE[]，⽤来学习了解A*算法的运算过程**** ///不需要知道过程可注释掉///
% OPEN %%%****输出OPEN[]，⽤来学习了解A*算法的运算过程**** ///不需要知道过程可注释掉///
else
%No path exists to the Target!!
NoPath=0;%Exits the loop!
end;%End of index_min_node check
end;%End of While Loop
%Once algorithm has run The optimal path is generated by starting of at the
%last node(if it is the target node) and then identifying its parent node
%until it reaches the start node.This is the optimal path
i=size(CLOSED,1); %%%CLOSE⾥⾯的长度
Optimal_path=[]; %%%路径数组
xval=CLOSED(i,1); %%%把CLOSE最后⼀组数提出来，最后⼀组数为⽬标点
yval=CLOSED(i,2);
i=1;
Optimal_path(i,1)=xval; %%%把⽬标点的坐标赋给路径数组的第⼀组
Optimal_path(i,2)=yval;
i=i+1;
if ( (xval == xTarget) && (yval == yTarget)) %%%检测CLOSE最后⼀组是否为⽬标点
inode=0;
%Traverse OPEN and determine the parent nodes 遍历OPEN并确定⽗节点
parent_x=OPEN(node_index(OPEN,xval,yval),4); %node_index returns the index of the node node_index返回节点的索引
parent_y=OPEN(node_index(OPEN,xval,yval),5);%%% 将当前点的⽗节点提出来
while( parent_x ~= xStart || parent_y ~= yStart) %%% 判断⽗节点是否为起始点
Optimal_path(i,1) = parent_x; %%% 不是则将⽗节点送给路径数组
Optimal_path(i,2) = parent_y;
%Get the grandparents:-)
inode=node_index(OPEN,parent_x,parent_y);
parent_x=OPEN(inode,4);%node_index returns the index of the node
parent_y=OPEN(inode,5);
i=i+1;
end;
toc
j = size(Optimal_path,1) + 1;
版权声明：本站内容均来自互联网，仅供演示用，请勿用于商业和其他非法用途。如果侵犯了您的权益请与我们联系QQ:729038198，我们将在24小时内删除。
发表评论