function?path?=?FocusdDstar（map?StartPos?GoalPos）
%map——二維數組圖?0:?passable?1:?obstacle
%?startPos:?Original?Position
%?endPos:???Target?Postion


%?The?routine?is?programmed?according?to?article:
%?Anthony?Stentz.?“The?D*?Algorithm?for?Real-Time?Planning?of?Optimal
%?Traverse“?CMU-RI-TR-94-37?1994


%路徑規劃

????clear?global?Var;
????clear?global?ori_map;
????persistent?hLine;
????if?ishandle（hLine）?delete（hLine）;?end
????global?ori_map;
????global?Var;
????[mn]?=?size（map）;
????X.x=[];X.y=[];
????Var.B（mn）?=?X;?%?B（X）??:?b（X）?backpointers?
????Var.r（mn）?=X;
????Var.Tag?=?zeros（mn）;???%?Tag（X）?=?0（NEW）1（OPEN）?or?2（CLOSED）
????Var.H?=?-1?*?Inf*ones（mn）;????????%?H（X）??:?estimate?of?the?sum?of?the?arc?costs?from?X?to?G
????Var.K?=?-1?*?Inf*ones（mn）;????????%?K（X）??:?state?key?function?value?=?min（H（X）Pre（X））?By?which?the?states?on?the?OPEN?list?are?sorted
????Var.f?=?-1?*?Inf*ones（mn）;????????%?f（X）??:?state?estimate?cost?from?S?through?X?to?G
????Var.fB?=?-1?*?Inf*ones（mn）;???????%?fb（X）?:?state?bias?estimate?cost?of?X
????Var.OPEN?=?[];
????
???
????G.x?=?GoalPos（1）;?G.y?=?GoalPos（2）;
????Var.H（G.y?G.x）?=?0;
????
????S.x?=?StartPos（1）;?S.y?=?StartPos（2）;
????path?=?S;
????Var.R?=?S;
????Var.Rcur?=?S;
????Var.Dcur?=?0;
????INSERT（G?Var.H（G.y?G.x））;
????val?=?0;?
????while?~isempty（val）?&?val（1）????????val?=?PROCESS_STATE2（map）;
????end

????if?Var.Tag（S.y?S.x）?~=?2
????????warndlg（‘There?is?no?path?existed!‘）;
????????path?=?[];
????????return;
????end
????
????cur?=?Var.B（S.y?S.x）;
????while?cur.x~=G.x?|?cur.y~=G.y
????????path?=?[path?cur];
????????cur?=?Var.B（cur.y?cur.x）;
????end
????path?=?[path?cur];
????????
????hold?on;
????hLine?=?plot（[path（:）.x]?[path（:）.y]?‘b.:‘）;

????ori_map?=?map;
????choice?=?questdlg（‘請選擇‘‘狀態變化‘?‘SetObstacle‘?‘CancelObstacle‘‘NoChange‘‘NoChange‘）;
????while?~isequal（choice?‘NoChange‘）
????????switch?choice
???????????case?‘SetObstacle‘
???????????????v?=?0;
???????????????c?=?‘k‘;
???????????case?‘CancelObstacle‘
???????????????v?=?255;
???????????????c?=?‘g‘
????????end
????????Obs?=?min（size（map）?round（ginput（1）））;
????????Obs?=?max（[11]?Obs）;
????????x?=?Obs（1）;y=Obs（2）;
????????Obj.x?=?x;?Obj.y?=?y;
????????map（y?x）=v;
????????X?=?[x-0.5?x+0.5?x+0.5?x-0.5];
????????Y?=?[y-0.5?y-0.5?y+0.5?y+0.5];
????????fill（XYc‘EdgeColor‘c）;drawnow;
????????choice?=?questdlg（‘請選擇‘‘狀態變化‘?‘SetObstacle‘?‘CancelObstacle‘‘NoChange‘‘NoChange‘）;
????end

????Var.R?=?S;
????path?=?Var.R;
????while?Var.R.x~=G.x?|?Var.R.y~=G.y
???????hR?=?plot（Var.R.x?Var.R.y?‘rs‘）;drawnow;?pause（0.2）;
???????InconsistentX?=?CheckBySensor（Var.R?5?map）;
???????if?~isempty（InconsistentX）
????????????if?~isequal（Var.Rcur?Var.R）
????????????????Var.Dcur?=?Var.Dcur?+?Get_gVal（Var.Rcur?Var.R）;
????????????????Var.Rcur?=?Var.R;
????????????end
????????????for?i=1?:?length（InconsistentX）
???

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----

?????文件??????17359??2007-12-15?01:48??FocusdDstar.m

-----------?---------??----------?-----??----

????????????????17577????????????????????2