%%?a*?algorithm
%?seminar?TU?KL
%?Author:?Francisco?J.?Garcia?R.
%?based?on:?
%????R.?Kala?（2014）?Code?for?Robot?Path?Planning?using?A*?algorithm?
%????Indian?Institute?of?Information?Technology?Allahabad?Available?at:?http://rkala.in/codes.html

%%?Load?Map?and?initial?parameters
figure（1）
mapOriginal=im2bw（imread（‘Maps/a_map2.bmp‘））;?%?input?map?read?from?a?bmp?file.

resolutionX=100;
resolutionY=100;

mapResized=imresize（mapOriginal[resolutionX?resolutionY]）;
map=mapResized;?

%?Conection?matrix?-?define?admisible?movement?of?robot
conn=[1?1?1;
??????1?2?1;
??????1?1?1];

display_process=true;?%?display?processing?of?nodes

%?grow?boundary?by?1?unit?pixel?-?take?into?account?size?of?robot
for?i=1:size（mapResized1）
????for?j=1:size（mapResized2）
????????if?mapResized（ij）==0
????????????if?i-1>=1?map（i-1j）=0;?end
????????????if?j-1>=1?map（ij-1）=0;?end
????????????if?i+1<=size（map1）?map（i+1j）=0;?end
????????????if?j+1<=size（map2）?map（ij+1）=0;?end
????????????if?i-1>=1?&&?j-1>=1?map（i-1j-1）=0;?end
????????????if?i-1>=1?&&?j+1<=size（map2）?map（i-1j+1）=0;?end
????????????if?i+1<=size（map1）?&&?j-1>=1?map（i+1j-1）=0;?end
????????????if?i+1<=size（map1）?&&?j+1<=size（map2）?map（i+1j+1）=0;?end
????????end
????end
end

image（（map==0）.*0?+?（map==1）.*255?+?（mapResized-map）.*150）;
colormap（gray（256））

disp（‘select?source?in?the?image‘）;
[xy]?=?ginput（1）;
source=[double（int8（y））?double（int8（x））];????%?source?position?in?Y?X?format

disp（‘select?goal?in?the?image‘）;
[xy]?=?ginput（1）;
goal?=?[double（int8（y））?double（int8（x））];????%?goal?position?in?Y?X?format

if?length（find（conn==2））~=1?error（‘no?robot?specified?in?connection?matrix‘）;?end

%%?Compute?path

%structure?of?a?node?is?taken?as?positionY?positionX?historic?cost?heuristic?cost?total?cost?parent?index?in?closed?list?（-1?for?source）?
Q=[source?0?heuristic（sourcegoal）?0+heuristic（sourcegoal）?-1];?%?the?processing?queue?of?A*?algorihtm?open?list
closed=ones（size（map））;?%?the?closed?list?taken?as?a?hash?map.?1=not?visited?0=visited
closedList=[];?%?the?closed?list?taken?as?a?list
pathFound=false;
tic;
counter=0;

size（Q）

while?size（Q1）>0
?????[A?I]=min（Q[]1）;
?????n=Q（I（5）:）;?%?smallest?cost?element?to?process
?????Q=[Q（1:I（5）-1:）;Q（I（5）+1:end:）];?%?delete?element?under?processing
?????if?n（1）==goal（1）?&&?n（2）==goal（2）?%?goal?test
?????????pathFound=true;break;
?????end
?????[rxryrv]=find（conn==2）;?%?robot?position?at?the?connection?matrix
?????[mxmymv]=find（conn==1）;?%?array?of?possible?moves
?????for?mxi=1:size（mx1）?%iterate?through?all?moves
?????????newPos=[n（1）+mx（mxi）-rx?n（2）+my（mxi）-ry];?%?possible?new?node
?????????if?checkPath（n（1:2）newPosmap）?%if?path?from?n?to?newPos?is?collission-free
??????????????if?closed（newPos（1）newPos（2））~=0?%?not?already?in?closed
??????????????????historicCost=n（3）+historic（n（1:2）newPos）;
??????????????????heuristicCost=heuristic（newPosgoal）;
??????????????????totalCost=historicCost+heuristicCost;
?????????????????

?屬性????????????大小?????日期????時(shí)間???名稱
-----------?---------??----------?-----??----
?????目錄???????????0??2016-11-28?14:46??Path_Planning_Simulator-master\
?????文件?????????378??2016-11-28?14:46??Path_Planning_Simulator-master\.gitattributes
?????文件????????4700??2016-11-28?14:46??Path_Planning_Simulator-master\Bidirectional_RRT.m
?????文件????????1413??2016-11-28?14:46??Path_Planning_Simulator-master\LoadMap.m
?????目錄???????????0??2016-11-28?14:46??Path_Planning_Simulator-master\Maps\
?????文件??????251078??2016-11-28?14:46??Path_Planning_Simulator-master\Maps\a_map1.bmp
?????文件??????251078??2016-11-28?14:46??Path_Planning_Simulator-master\Maps\a_map2.bmp
?????文件??????251078??2016-11-28?14:46??Path_Planning_Simulator-master\Maps\a_map3.bmp
?????文件??????251078??2016-11-28?14:46??Path_Planning_Simulator-master\Maps\a_map4.bmp
?????文件??????251078??2016-11-28?14:46??Path_Planning_Simulator-master\Maps\a_map5.bmp
?????文件??????308346??2016-11-28?14:46??Path_Planning_Simulator-master\Maps\map1.bmp
?????文件????????5439??2016-11-28?14:46??Path_Planning_Simulator-master\Potential_field_function.m
?????文件?????????362??2016-11-28?14:46??Path_Planning_Simulator-master\README.md
?????文件???????36632??2016-11-28?14:46??Path_Planning_Simulator-master\RobotSimulation.fig
?????文件???????19465??2016-11-28?14:46??Path_Planning_Simulator-master\RobotSimulation.m
?????文件????????4685??2016-11-28?14:46??Path_Planning_Simulator-master\a_star.m
?????文件????????3272??2016-11-28?14:46??Path_Planning_Simulator-master\a_start_compute_path.m
?????文件????????3454??2016-11-28?14:46??Path_Planning_Simulator-master\bidirectional_RRT_compute_final_path.m
?????文件????????5889??2016-11-28?14:46??Path_Planning_Simulator-master\callback_interrupt.m
?????文件?????????978??2016-11-28?14:46??Path_Planning_Simulator-master\checkPath.m
?????文件?????????441??2016-11-28?14:46??Path_Planning_Simulator-master\distToEdge.m
?????文件?????????552??2016-11-28?14:46??Path_Planning_Simulator-master\distanceCost.m
?????文件?????????603??2016-11-28?14:46??Path_Planning_Simulator-master\distanceCost_RRT.m
?????文件????????5477??2016-11-28?14:46??Path_Planning_Simulator-master\ex_guide_timergui.fig
?????文件????????7074??2016-11-28?14:46??Path_Planning_Simulator-master\ex_guide_timergui.m
?????文件?????????727??2016-11-28?14:46??Path_Planning_Simulator-master\feasiblePoint.m
?????文件?????????542??2016-11-28?14:46??Path_Planning_Simulator-master\heuristic.m
?????文件?????????535??2016-11-28?14:46??Path_Planning_Simulator-master\historic.m
?????文件?????????521??2016-11-28?14:46??Path_Planning_Simulator-master\minDistToEdges.m
?????文件????????1362??2016-11-28?14:46??Path_Planning_Simulator-master\plotRobot.m
?????文件????????3218??2016-11-28?14:46??Path_Planning_Simulator-master\potential_field.m
............此處省略4個(gè)文件信息