1 簡(jiǎn)介

2 部分代碼

function [Nodes, openList, k_old] = process_state(field, Nodes, openList,goalPos)% 獲得openList中k值最小的節(jié)點(diǎn)X及k_old[k_old,idx] = min(openList(:,2));X = openList(idx,1);% 從openList中移出X，并修改其狀態(tài)為closedopenList(idx,:) = [];Nodes(X).t = 'closed';% X的鄰節(jié)點(diǎn)neighborNodes = getNeighborNode(field, X);%% 修正X的父節(jié)點(diǎn)及相關(guān)信息% 判斷k_old 是否小于 Nodes(X).h,若是表明該節(jié)點(diǎn)已經(jīng)受到障礙的影響% 那么就判斷X的鄰節(jié)點(diǎn)，看是否能夠以某個(gè)鄰節(jié)點(diǎn)作為父節(jié)點(diǎn)，使Nodes(X).h變小if k_old < Nodes(X).h ? ?% 遍歷X的鄰節(jié)點(diǎn)Y ? ?for i = 1:size(neighborNodes,1) ? ? ? ?Y = neighborNodes(i,1); ? ? ? ?cost_X_Y = neighborNodes(i,2); ? ? ? ?% 如果Y點(diǎn)的h值沒(méi)有上升，并且X的h值能通過(guò)Y變得更小,那么就修改X的父節(jié)點(diǎn)為Y，重置其h的值。 ? ? ? ?% 同時(shí)判斷Nodes(Y).h <= k_old，考察Y本身是否收到障礙影響導(dǎo)致Nodes(Y).h > k_old ? ? ? ?% 若Nodes(Y).h > k_old，那么再以Y作為父節(jié)點(diǎn)就沒(méi)有意義了 ? ? ? ?if Nodes(Y).h <= k_old && Nodes(X).h > Nodes(Y).h + cost_X_Y ? ? ? ? ? ?Nodes(X).parent = Y; ? ? ? ? ? ?Nodes(X).h = Nodes(Y).h + cost_X_Y; ? ? ? ? ? ?% 注意，此處更新了Nodes(X).h，只表明 Nodes(X).h進(jìn)一步減小了， ? ? ? ? ? ?% 但與k_old誰(shuí)大誰(shuí)小還尚未可知，需要下一節(jié)繼續(xù)判斷 ? ? ? ?end ? ?endend%% 上一節(jié)修正了X的信息，進(jìn)一步判斷，并修正Y的父節(jié)點(diǎn)及相關(guān)信息if k_old == Nodes(X).h ? ? ?% 若k_old = Nodes(X).h，Lower態(tài)，有下面三種可能： ? ?% 1）處于第一遍遍歷的階段； ? ?% 2）該節(jié)點(diǎn)X并沒(méi)有受到障礙影響； ? ?% 3）受到了障礙物影響，但是在上一節(jié)已經(jīng)更新了X的parent ? ?% 遍歷X的鄰節(jié)點(diǎn)Y ? ?for i = 1:size(neighborNodes,1) ? ? ? ?Y = neighborNodes(i,1); ? ? ? ?cost_X_Y = neighborNodes(i,2); ? ? ? ?if ?isequal(Nodes(Y).t, 'new')... ? ? ? ? ? ? ? ?|| ?Nodes(Y).parent == X && ?Nodes(Y).h ~= Nodes(X).h + cost_X_Y... ? ? ? ? ? ? ? ?|| ?Nodes(Y).parent ~= X && ?Nodes(Y).h > Nodes(X).h + cost_X_Y && Y ~= goalPos ? ? ? ? ? ?% 若情況1，表明鄰節(jié)點(diǎn)Y還未納入openList，那么就以X作為父節(jié)點(diǎn)； ? ? ? ? ? ?% 若情況2，表明雖然Y的父節(jié)點(diǎn)是X，但是 Nodes(Y).h卻與Nodes(X).h + cost_X_Y不相等了， ? ? ? ? ? ?% 表明Y的父節(jié)點(diǎn)Nodes(X).h 有過(guò)更新，可能是由于障礙引起的； ? ? ? ? ? ?% 若情況3，表明Y可以通過(guò)將X作為父節(jié)點(diǎn)，使得Nodes(Y).h更小 ? ? ? ? ? ?% 上述情況，都應(yīng)該將X作為Y的父節(jié)點(diǎn)，并把Y移到openList后，再進(jìn)一步考察 ? ? ? ? ? ?Nodes(Y).parent = X; ? ? ? ? ? ?h_new = Nodes(X).h + cost_X_Y; ? ? ? ? ? ?[Nodes,openList] = insert(Nodes,openList,Y,h_new); ? ? ? ?end ? ?end else ? ?% 若k_old ~= Nodes(X).h,Raise態(tài) ? ?% 說(shuō)明節(jié)點(diǎn)X受到了影響，遍歷其鄰域。 ? ?for i = 1:size(neighborNodes,1) ? ? ? ?Y = neighborNodes(i,1); ? ? ? ?cost_X_Y = neighborNodes(i,2); ? ? ? ?if ?isequal(Nodes(Y).t, 'new')||... ? ? ? ? ? ? ? ?Nodes(Y).parent == X && Nodes(Y).h ~= Nodes(X).h + cost_X_Y ? ? ? ? ? ?% 若情況1，表明鄰節(jié)點(diǎn)Y還未納入openList，那么就以X作為父節(jié)點(diǎn)； ? ? ? ? ? ?% 若情況2，表明雖然Y的父節(jié)點(diǎn)是X，但是 Nodes(Y).h卻與Nodes(X).h + cost_X_Y不相等了， ? ? ? ? ? ?% 上述情況，都應(yīng)該將X作為Y的父節(jié)點(diǎn)，并把Y移到openList后，再進(jìn)一步考察 ? ? ? ? ? ?Nodes(Y).parent = X; ? ? ? ? ? ?h_new = Nodes(X).h + cost_X_Y; ? ? ? ? ? ?[Nodes,openList] = insert(Nodes,openList,Y,h_new); ? ? ? ?else ? ? ? ? ? ?if ?Nodes(Y).parent ~= X && Nodes(Y).h > Nodes(X).h + cost_X_Y ? ? ? ? ? ? ? ?% 若滿足，表明Y可以通過(guò)將X作為父節(jié)點(diǎn)，使得Nodes(Y).h更小 ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?% 但是目前針對(duì)X，有 k_old ~= Nodes(X).h，故要將X追加到openList，待下一次循環(huán)滿足條件就將X作為Y的父節(jié)點(diǎn)。 ? ? ? ? ? ? ? ?[Nodes,openList] = insert(Nodes,openList,X,Nodes(X).h); ? ? ? ? ? ?elseif ?Nodes(Y).parent ~= X && Nodes(X).h > Nodes(Y).h + cost_X_Y... ? ? ? ? ? ? ? ? ? ?&& ?isequal(Nodes(Y).t, 'closed') && Nodes(Y).h > k_old ? ? ? ? ? ? ? % 情況1，表明Y的父節(jié)點(diǎn)不是X，但是讓Y成為X父節(jié)點(diǎn)， Nodes(X).h更?。?/code> ? ? ? ? ? ? ? % 情況2，表明并且Y已經(jīng)被open表移除； ? ? ? ? ? ? ? % 情況3，當(dāng)前從openList取出的最小值k_old居然比h(Y)小，表明已經(jīng)被移除open表的Y受到了障礙影響導(dǎo)致h值升高 ? ? ? ? ? ? ? % 上述情況，都要重新將Y置于openList中，進(jìn)行下一輪考察。 ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?[Nodes,openList] = insert(Nodes,openList,Y,Nodes(Y).h); ? ? ? ? ? ?end ? ? ? ?end ? ?endendend

3 仿真結(jié)果

4 參考文獻(xiàn)

[1]王帥軍. 基于D~*算法的移動(dòng)機(jī)器人路徑規(guī)劃. Diss. 廣西大學(xué).

博主簡(jiǎn)介：擅長(zhǎng)智能優(yōu)化算法、神經(jīng)網(wǎng)絡(luò)預(yù)測(cè)、信號(hào)處理、元胞自動(dòng)機(jī)、圖像處理、路徑規(guī)劃、無(wú)人機(jī)等多種領(lǐng)域的Matlab仿真，相關(guān)matlab代碼問(wèn)題可私信交流。

部分理論引用網(wǎng)絡(luò)文獻(xiàn)，若有侵權(quán)聯(lián)系博主刪除。

標(biāo)簽：