一、算法介紹

A* 在靜態(tài)路網(wǎng)中非常有效（very efficient for static worlds），但不適于在動(dòng)態(tài)路網(wǎng)，環(huán)境如權(quán)重等不斷變化的動(dòng)態(tài)環(huán)境下。

D是動(dòng)態(tài)A（D-Star,Dynamic A Star） 卡內(nèi)及梅隆機(jī)器人中心的Stentz在1994和1995年兩篇文章提出，主要用于機(jī)器人探路。是火星探測(cè)器采用的尋路算法。

Optimal and Efficient Path Planning for Partially-Known Environments

The Focussed D* Algorithm for Real-Time Replanning

主要方法（這些完全是Drew在讀了上述資料和編制程序中的個(gè)人理解，不能保證完全正確，僅供參考）：

1.先用Dijstra算法從目標(biāo)節(jié)點(diǎn)G向起始節(jié)點(diǎn)搜索。儲(chǔ)存路網(wǎng)中目標(biāo)點(diǎn)到各個(gè)節(jié)點(diǎn)的最短路和該位置到目標(biāo)點(diǎn)的實(shí)際值h,k（k為所有變化h之中最小的值,當(dāng)前為k=h。每個(gè)節(jié)點(diǎn)包含上一節(jié)點(diǎn)到目標(biāo)點(diǎn)的最短路信息1(2),2(5),5(4)，4（7）。則1到4的最短路為1-2-5-4。 原OPEN和CLOSE中節(jié)點(diǎn)信息保存。

2.機(jī)器人沿最短路開(kāi)始移動(dòng)，在移動(dòng)的下一節(jié)點(diǎn)沒(méi)有變化時(shí)，無(wú)需計(jì)算，利用上一步Dijstra計(jì)算出的最短路信息從出發(fā)點(diǎn)向后追述即可，當(dāng)在Y點(diǎn)探測(cè)到下一節(jié)點(diǎn)X狀態(tài)發(fā)生改變，如堵塞。機(jī)器人首先調(diào)整自己在當(dāng)前位置Y到目標(biāo)點(diǎn)G的實(shí)際值h(Y)，h(Y)=X到Y(jié)的新權(quán)值c(X,Y)+X的原實(shí)際值h(X).X為下一節(jié)點(diǎn)(到目標(biāo)點(diǎn)方向Y->X->G），Y是當(dāng)前點(diǎn)。k值取h值變化前后的最小。

3.用A或其它算法計(jì)算，這里假設(shè)用A算法,遍歷Y的子節(jié)點(diǎn)，點(diǎn)放入CLOSE,調(diào)整Y的子節(jié)點(diǎn)a的h值，h(a)=h(Y)+Y到子節(jié)點(diǎn)a的權(quán)重C(Y,a),比較a點(diǎn)是否存在于OPEN和CLOSE中，方法如下：

while() { 從OPEN表中取k值最小的節(jié)點(diǎn)Y; 遍歷Y的子節(jié)點(diǎn)a,計(jì)算a的h值 h(a)=h(Y)+Y到子節(jié)點(diǎn)a的權(quán)重C(Y,a) { if(a in OPEN) 比較兩個(gè)a的h值 if( a的h值小于OPEN表a的h值 ) { 　　　　　更新OPEN表中a的h值;k值取最小的h值 有未受影響的最短路經(jīng)存在 break; } if(a in CLOSE) 比較兩個(gè)a的h值 //注意是同一個(gè)節(jié)點(diǎn)的兩個(gè)不同路徑的估價(jià)值 if( a的h值小于CLOSE表的h值 ) { 　　　　　更新CLOSE表中a的h值; k值取最小的h值;將a節(jié)點(diǎn)放入OPEN表 有未受影響的最短路經(jīng)存在 break; } if(a not in both) 將a插入OPEN表中;　//還沒(méi)有排序 } 放Y到CLOSE表； OPEN表比較k值大小進(jìn)行排序； } 機(jī)器人利用第一步Dijstra計(jì)算出的最短路信息從a點(diǎn)到目標(biāo)點(diǎn)的最短路經(jīng)進(jìn)行。

D*算法在動(dòng)態(tài)環(huán)境中尋路非常有效，向目標(biāo)點(diǎn)移動(dòng)中，只檢查最短路徑上下一節(jié)點(diǎn)或臨近節(jié)點(diǎn)的變化情況，如機(jī)器人尋路等情況。對(duì)于距離遠(yuǎn)的最短路徑上發(fā)生的變化，則感覺(jué)不太適用。

上圖是Drew在4000個(gè)節(jié)點(diǎn)的隨機(jī)路網(wǎng)上做的分析演示，細(xì)黑線(xiàn)為第一次計(jì)算出的最短路，紅點(diǎn)部分為路徑上發(fā)生變化的堵塞點(diǎn)，當(dāng)機(jī)器人位于982點(diǎn)時(shí)，檢測(cè)到前面發(fā)生路段堵塞，在該點(diǎn)重新根據(jù)新的信息計(jì)算路徑，可以看到圓圈點(diǎn)為重新計(jì)算遍歷過(guò)的點(diǎn)，僅僅計(jì)算了很少得點(diǎn)就找到了最短路，說(shuō)明計(jì)算非常有效，迅速。綠線(xiàn)為計(jì)算出的繞開(kāi)堵塞部分的新的最短路徑。

二、柵格地圖

柵格地圖有兩種表示方法，直角坐標(biāo)系法和序號(hào)法，序號(hào)法比直角坐標(biāo)法節(jié)省內(nèi)存

室內(nèi)環(huán)境柵格法建模步驟

1.柵格粒大小的選取

柵格的大小是個(gè)關(guān)鍵因素，柵格選的小，環(huán)境分辨率較大，環(huán)境信息存儲(chǔ)量大，決策速度慢。

柵格選的大，環(huán)境分辨率較小，環(huán)境信息存儲(chǔ)量小，決策速度快，但在密集障礙物環(huán)境中發(fā)現(xiàn)路徑的能力較弱。

2.障礙物柵格確定

當(dāng)機(jī)器人新進(jìn)入一個(gè)環(huán)境時(shí)，它是不知道室內(nèi)障礙物信息的，這就需要機(jī)器人能夠遍歷整個(gè)環(huán)境，檢測(cè)障礙物的位置，并根據(jù)障礙物位置找到對(duì)應(yīng)柵格地圖中的序號(hào)值，并對(duì)相應(yīng)的柵格值進(jìn)行修改。自由柵格為不包含障礙物的柵格賦值為0，障礙物柵格為包含障礙物的柵格賦值為1.

3.未知環(huán)境的柵格地圖的建立

通常把終點(diǎn)設(shè)置為一個(gè)不能到達(dá)的點(diǎn)，比如（-1，-1），同時(shí)機(jī)器人在尋路過(guò)程中遵循“下右上左”的原則，即機(jī)器人先向下行走，當(dāng)機(jī)器人前方遇到障礙物時(shí)，機(jī)器人轉(zhuǎn)向右走，遵循這樣的規(guī)則，機(jī)器人最終可以搜索出所有的可行路徑，并且機(jī)器人最終將返回起始點(diǎn)。

備注：在柵格地圖上，有這么一條原則，障礙物的大小永遠(yuǎn)等于n個(gè)柵格的大小，不會(huì)出現(xiàn)半個(gè)柵格這樣的情況。

三、代碼?

%D* Lite算法 %By Aword 2019/05/20 %*********************************************初始化開(kāi)始，給出全局參數(shù)************************************************ clc; clear; global n_r; global n_c; global s_start; global s_goal; global U; global km; global g; global rhs; global key; global neighbour; global map; %*******************************可修改參數(shù)************************************* n_r=30;%定義地圖大小-行 n_c=30;%定義地圖大小-列 s_start=[1 1];%起始點(diǎn) s_goal=[30 30];%目標(biāo)點(diǎn) %********************************初始化*************************************** U=[];%優(yōu)先級(jí)列表，用于存儲(chǔ)待擴(kuò)展的非一致節(jié)點(diǎn)(g(s)!=rhs(s)) km=0;%記錄最初起點(diǎn)到當(dāng)前起始點(diǎn)的代價(jià)值 g=[];%g和rhs表示節(jié)點(diǎn)s到目標(biāo)點(diǎn)的最小代價(jià)的估計(jì)值 rhs=[];%rhs是由其前向節(jié)點(diǎn)（起始點(diǎn)到當(dāng)前點(diǎn)）的g值計(jì)算得到 key=[]; path=[];%存儲(chǔ)規(guī)劃路徑 neighbour=[1,0; %八向搜尋 ? ? ? ? ? 0,1; ? ? ? ? ? 0,-1; ? ? ? ? ? -1,0; ? ? ? ? ? 1,1; ? ? ? ? ? 1,-1; ? ? ? ? ? -1,1; ? ? ? ? ? -1,-1]; % neighbour=[1,0; %四向搜尋 % ? ? ? ? ? ?0,1; % ? ? ? ? ? ?0,-1; % ? ? ? ? ? ?-1,0]; s_last=s_start;%當(dāng)前位置點(diǎn)sl(下一時(shí)刻的位置點(diǎn))視為新的起始點(diǎn)反復(fù)計(jì)算目標(biāo)點(diǎn)sg與新的起始點(diǎn)間的最短路徑 g(1:n_r,1:n_c)=Inf;%遍歷地圖節(jié)點(diǎn)集S并初始化,這里注意行列對(duì)應(yīng)的坐標(biāo)是相反的 rhs(1:n_r,1:n_c)=Inf; rhs(s_goal(1),s_goal(2))=0;%目標(biāo)點(diǎn)rhs置0 CalculateKey(s_goal); U=[s_goal,key(s_goal(1),s_goal(2)).key1,key(s_goal(1),s_goal(2)).key2];%講目標(biāo)點(diǎn)及其key值插入到優(yōu)先列表U中 %*******************************生成原始地圖************************************ cmap = [1 1 1; ...% 1 -白色-無(wú)障礙 ? ? ? ?0 0 0; ...% 2 -有障礙 ? ? ? ?0 1 0; ...% 3 -起始點(diǎn) ? ? ? ?0 0 1; ...% 4 -目標(biāo)點(diǎn) ? ? ? ?1 0 0; ...% 5 -最終路徑 ? ? ? ?0.5 0.5 0.5]; % 6 -擴(kuò)展節(jié)點(diǎn) % 黑 ? 0 0 0 % 白 ? 1 1 1 % 紅 ? 1 0 0 % 綠 ? 0 1 0 % 藍(lán) ? 0 0 1 % 黃 ? 1 1 0 % 灰 ? 0.5 0.5 0.5 % 洋紅 ?1 0 1 % 青藍(lán) ?0 1 1 % 天藍(lán) ?0.67 0 1 % 橘黃 ?1 0.5 0 % 深紅 ?0.5 0 0 colormap(cmap); map = ones(n_r,n_c); randmap = rand(n_r,n_c);%返回一個(gè)n_rxn_c的（0～1）隨機(jī)數(shù)矩陣,生成隨機(jī)地圖 for i=1:n_r%遍歷地圖節(jié)點(diǎn)并初始化障礙物信息 ? ?for j=1:n_c ? ? ? ?if (randmap(i,j) >= 0.75)%該數(shù)大小決定障礙物密度 ? ? ? ? ? ?map(i,j) = 2;%置為障礙物 ? ? ? ?end ? ?end end map(s_start(1), s_start(2)) = 3; % 起點(diǎn)坐標(biāo) map(s_goal(1), s_goal(2)) = 4; % 終點(diǎn)坐標(biāo) % set(gca,'xtick',[],'ytick',[])%去掉刻度標(biāo)記 subplot(1, 3, 1) image(1.5,1.5,map); grid on; %網(wǎng)格 axis image; %顯示更新前地圖 hold on; %*******************************生成動(dòng)態(tài)地圖************************************ map_ob = ones(n_r,n_c); randmap = rand(n_r,n_c);%返回一個(gè)n_rxn_c的（0～1）隨機(jī)數(shù)矩陣 ? ?end %*******************************動(dòng)態(tài)繪制地圖************************************ ? ?for i=1:size(path,1) ? ? ? ?map(path(i,1),path(i,2))=5; ? ?end ? ?map(s_start(1), s_start(2)) = 3; % 起點(diǎn)坐標(biāo) ? ?map(s_goal(1), s_goal(2)) = 4; % 終點(diǎn)坐標(biāo) ? ?subplot(1, 3, 3) ? ?image(1.5,1.5,map); ? ?if (n_r==10)&&(n_c==10) ? ? ? ?for i=1:n_r%遍歷地圖節(jié)點(diǎn)并標(biāo)記節(jié)點(diǎn)信息 ? ? ? ? ? ?for j=1:n_c ? ? ? ? ? ? ? ?text(j,i+0.2,num2str(g(i,j)),'FontSize',10,'color','k') ? ? ? ? ? ? ? ?text(j,i+0.5,num2str(rhs(i,j)),'FontSize',10,'color','k') ? ? ? ? ? ? ? ?text(j,i+0.8,num2str(key(i,j).key1),'FontSize',10,'color','k') ? ? ? ? ? ? ? ?text(j+0.5,i+0.8,num2str(key(i,j).key2),'FontSize',10,'color','k') ? ? ? ? ? ?end ? ? ? ?end ? ?end ? ?grid on; %網(wǎng)格 ? ?axis image; %顯示路徑 ? ?pause(0.1); %*******************************動(dòng)態(tài)繪制地圖************************************ end %*************************************************主體循環(huán)結(jié)束****************************************************** ? for i=1:n_r%查看擴(kuò)展節(jié)點(diǎn) ? ?for j=1:n_c ? ? ? ?if map(i,j)==1 ? ? ? ? ? ?if rhs(i,j)~=Inf ? ? ? ? ? ? ? ?map(i,j)=6; ? ? ? ? ? ?elseif key(i,j).key2~=Inf ? ? ? ? ? ? ? ?map(i,j)=6; ? ? ? ? ? ?end ? ? ? ? ? ?end ? ?end end ? ? subplot(1, 3, 3) image(1.5,1.5,map); if (n_r==10)&&(n_c==10) ? ?for i=1:n_r%遍歷地圖節(jié)點(diǎn)并標(biāo)記節(jié)點(diǎn)信息 ? ? ? ?for j=1:n_c ? ? ? ? ? ?text(j,i+0.2,num2str(g(i,j)),'FontSize',10,'color','k') ? ? ? ? ? ?text(j,i+0.5,num2str(rhs(i,j)),'FontSize',10,'color','k') ? ? ? ? ? ?text(j,i+0.8,num2str(key(i,j).key1),'FontSize',10,'color','k') ? ? ? ? ? ?text(j+0.5,i+0.8,num2str(key(i,j).key2),'FontSize',10,'color','k') ? ? ? ?end ? ?end end grid on; %網(wǎng)格 axis image; %顯示路徑