A*算法的基础上修改成可放缩路径长度的A*

function path = A_star_search(map,MAX_X,MAX_Y) %% %This part is about map/obstacle/and other settings %pre-process the grid map, add offset size_map = size(map,1); Y_offset = 0; X_offset = 0; %Define the 2D grid map array. %障碍-1 目标0 起始1 Obstacle=-1, Target = 0, Start=1 MAP=2*(ones(MAX_X,MAX_Y)); % disp(MAP) % 设置目标点为0 Initialize MAP with location of the target xval=floor(map(size_map, 1)) + X_offset; yval=floor(map(size_map, 2)) + Y_offset; xTarget=xval; yTarget=yval; MAP(xval,yval)=0; % disp(MAP) %Initialize MAP with location of the obstacle(障碍) for i = 2: size_map-1 xval=floor(map(i, 1)) + X_offset; yval=floor(map(i, 2)) + Y_offset; MAP(xval,yval)=-1; end % disp(MAP) %Initialize MAP with location of the start point xval=floor(map(1, 1)) + X_offset; yval=floor(map(1, 2)) + Y_offset; xStart=xval; yStart=yval; MAP(xval,yval)=1; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%my global min_dis min_dis = distance(xStart,yStart,xTarget,yTarget); disp(min_dis) rate = 1.3;%%放大倍率(1-5) global tar_dis tar_dis= rate*min_dis; disp(tar_dis) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%my %disp(MAP) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %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); CLOSED=[]; % 把所有障碍放进closed list Put all obstacles on the Closed list k=1;% Dummy counter for i=1:MAX_X for j=1:MAX_Y if(MAP(i,j) == -1) CLOSED(k,1)=i; CLOSED(k,2)=j; k=k+1; end end end % disp(CLOSED) CLOSED_COUNT=size(CLOSED,1); %set the starting node as the first node xNode=xval; yNode=yval; OPEN_COUNT=1; goal_distance=distance(xNode,yNode,xTarget,yTarget);% 计算笛卡尔坐标系下的距离 path_cost=0; OPEN(OPEN_COUNT,:)=insert_open(xNode,yNode,xNode,yNode,goal_distance,path_cost,goal_distance); OPEN(OPEN_COUNT,1)=1;% Note: here it is changed CLOSED_COUNT=CLOSED_COUNT+1; CLOSED(CLOSED_COUNT,1)=xNode; CLOSED(CLOSED_COUNT,2)=yNode; NoPath=1; %% %This part is your homework %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % START ALGORITHM %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% while(1) % if the queue(OPEN) is empty, return False; break; if sum(OPEN(:,1)) == 0 %IS ON LIST 1/0 break; end % disp(OPEN) % disp(OPEN(:,1)) 第一列IS ON LIST 1/0 % disp(sum(OPEN(:,1))) 第一列的和 %从优先级队列中删除 g(n) 最小的节点“n”。 % Remove the node "n" with the lowest g(n) from the priority queue. i_min = min_fn(OPEN,length(OPEN(:,1)),xTarget,yTarget); % OPEN: LIST: 1/0 |X val |Y val |Parent X val |Parent Y val |h(n) |g(n)|f(n)| %从open里弹出 xNode_expanded = OPEN(i_min,2); yNode_expanded = OPEN(i_min,3); gNode_expanded = OPEN(i_min,7); OPEN(i_min,1) = 0; % this flag denotes whether it is pushed out, min_fn checks this % 1 means in OPEN, o means out of OPEN %放到closed list % Mark node "n" as expanded CLOSED_COUNT = CLOSED_COUNT + 1; CLOSED(CLOSED_COUNT,1)=xNode_expanded; CLOSED(CLOSED_COUNT,2)=yNode_expanded; % if 到终点 % If the node "n" is the goal state, return TRUE; break; if xNode_expanded == xTarget && yNode_expanded == yTarget NoPath = 0; break; end % 节点n的每个未扩展邻居m % For all unexpanded neighbors "m" of node "n", this has been % done in expand_array exp_array=expand_array(xNode_expanded,yNode_expanded,gNode_expanded,xTarget,yTarget,CLOSED,MAX_X,MAX_Y); % exp_array: m*5*1 matrix |X val |Y val ||h(n) |g(n)|f(n)| if ~isempty(exp_array) for m = 1:1:length(exp_array(:,1)) % For all unexpanded neighbors 搈? of node 搉? xNode = exp_array(m,1); yNode = exp_array(m,2); % If node m is not in OPEN, push node m into OPEN if isempty(node_index(OPEN,xNode,yNode)) OPEN_COUNT=OPEN_COUNT + 1; hn=distance(xNode,yNode,xTarget,yTarget); gn=gNode_expanded + distance(xNode,yNode,xNode_expanded,yNode_expanded); hn = hn*rate; fn=abs(hn + gn - tar_dis); %fn=abs(hn + gn - (gn/(hn + gn)*tar_dis)); %%%!!!!!!!!!!!!!!!!!!!!!!!!!!!!!1 %disp("gn/(hn + gn)*tar_dis") %disp(gn/(hn + gn)*tar_dis) % 插入open OPEN(OPEN_COUNT,:)=insert_open(xNode,yNode,xNode_expanded,yNode_expanded,hn,gn,fn); OPEN(OPEN_COUNT,1)=1; else %If m Node is in OPEN and g(m) > g(n) + Cnm, which means a better path to this node is found %then update parents and g , f value n_inx = node_index(OPEN, xNode,yNode); %%%%%%%%%%%%%%%%%%%555 %%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%% %if OPEN(n_inx,7) > (gNode_expanded + distance(xNode,yNode,xNode_expanded,yNode_expanded)) %此处也需要修改 if OPEN(n_inx,8) > (abs(OPEN(n_inx,6) + (gNode_expanded + distance(xNode,yNode,xNode_expanded,yNode_expanded))- tar_dis)) OPEN(n_inx,4) = xNode_expanded; % Parent X val OPEN(n_inx,5) = yNode_expanded; % Parent Y val OPEN(n_inx,7) = gNode_expanded + distance(xNode,yNode,xNode_expanded,yNode_expanded); % g(m) OPEN(n_inx,8) = abs(OPEN(n_inx,6) + OPEN(n_inx,7)- tar_dis); % f(m) %OPEN(n_inx,8) = abs(OPEN(n_inx,6) + OPEN(n_inx,7)- (gn/(hn + gn)*tar_dis)); % f(m) %%%!!!!!!!!!!!!!!!!!!!!!!!!!!!!!1 end end end end 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 % %How to get the optimal path after A_star search? %please finish it % path = []; if NoPath return; else % find the path from goal to start xval = xTarget; yval = yTarget; n = 1; index = node_index(OPEN,xval,yval); % 终点 disp(OPEN(index,7)) disp(OPEN(index,8)) %disp(OPEN(node_index-1,8)) %%!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!11 path(n,:) = [xval,yval]; while index > 1 assert(OPEN(index,1)==0, 'Something wrong!') xval = OPEN(index,4); yval = OPEN(index,5); index = node_index(OPEN,xval,yval); n = n + 1; path(n,:) = [xval,yval]; end