Ich versuche die optimierte Version aus dem ersten Paper umzusetzen (S. 5, Fig. 4). Zum Debugging verwende ich das Beispiel aus Paper 2 (S. 6, Fig. 6 und Fig. 7).
Ich hänge zur Zeit in Zeile 36 und 37 des PseudoCodes:
Zitat:
{36} Scan graph for changed edge costs;
{37} if any edge costs changed
Irgendwie bin ich nicht sicher, wie ich bei meiner Implementierung die sich ändernen Kosten der Kanten feststellen und vorallem dann auch "umsetzen" kann. Hat jemand von euch dabei Hilfe und kann mir weiterhelfen?
Mein aktueller MatLab-Code (Datei: myOwnDStarDemo.m, Aufruf mit: myOwnDStarDemo() ) ist unten zu sehen.
NACH dem Aufruf von computeShortestPath() in Zeile 109 lauten nach dem zweiten Paper die Matrizen knownGrid, gridRHS und gridG wie folgt:
Leider komme ich irgendwie nicht darauf, wie ich die Kantenkosten korrekt aktualisiere. Weiß jemand weiter?
Die Fehler dürften wohl in der Funktion scanChangedEdgeCost() und myDStarSearch() (Zeile 131-163) sein.
% delta = [-1, 0 ; % go up % 0, -1; % go left % 1, 0; % go down % 0, 1]; % go right
delta = [-1, 0 ; % go up 0, -1; % go left 1, 0; % go down 0, 1; % go right -1, 1; % go north-east 1, 1; % go south-east 1, -1; % go south-west -1, -1]; % go north-west
% if any edge costs changed if ~isempty( changeGrid )
kM = kM + dStarHeuristic(sStart,sLast);
sLast = sStart;
for e = 1:size(changeGrid,1)
u = changeGrid(e,1:2);
v = changeGrid(e,3:4);
cOld = changeGrid(e,5);
cUV = changeGrid(e,6); % Update the edge cost: we don't really save all edge costs, so it is just being saved for this one step if cOld > cUV
if ~(u(1) == sGoal(1) && u(2) == sGoal(2))
gridRHS(u(1),u(2)) = min([ gridRHS(u(1),u(2)), cUV + gridG(v(1),v(2))]);
end elseif gridRHS(u(1),u(2)) == cOld + gridG(v(1),v(2)) if ~(u(1) == sGoal(1) && u(2) == sGoal(2)) % loop all successors s' of u
minCost = Inf;
for d = 1:length(delta)
succX = u(1) + delta(d,1); % x-coordinate of successor of current cell/node u
succY = u(2) + delta(d,2); % y-coordinate of successor of current cell/node u if succX >= 1 && succX <= size(knownGrid,1) && succY >= 1 && succY <= size(knownGrid,2)% && knownGrid(succX,succY) == 0
curCost = deltaCost(d) + gridG(succX,succY);
if curCost < minCost
minCost = curCost;
end end end
gridRHS(u(1),u(2)) = minCost;
end end
uPrioQueue = updateVertex(u, uPrioQueue, gridG, gridRHS, sStart, kM);
end [ uPrioQueue, gridRHS, gridG ] = computeShortestPath( uPrioQueue, knownGrid, gridRHS, gridG, sStart, sGoal, delta, deltaCostInv, kM );
end end end end
function[ knownGrid, changeGrid ] = scanChangedEdgeCost( trueGrid, knownGrid, sCurrent, sGoal, delta, deltaCost, deltaCostInv ) % compare the known grid map 'knownGrid' with the real grid map 'trueGrid' % at all successors of the current cell/node 'sCurrent'
%
% if any new obstacles appear (or believed ones disappear!) we save the successors % x-,y-coordinate, the old, and the new costs for travelling from sCurrent to this successor in 'changeGrid', else it will stay emtpy % % Furthermore, we update the known map by this new information.
changeGrid = [];
for d = 1:length(delta)
succX = sCurrent(1) + delta(d,1); % x-coordinate of successor of current node sCurrent
succY = sCurrent(2) + delta(d,2); % y-coordinate of successor of current node sCurrent if succX >= 1 && succX <= size(knownGrid,1) && succY >= 1 && succY <= size(knownGrid,2) if knownGrid(succX,succY) ~= trueGrid(succX,succY) if trueGrid(succX,succY) == 1 % we found a new obstacle, we didn't know it existed before; % so before we thought it was traversable and the old cost was % the regular cost for moving to that successor
cOld = deltaCost(d);
cOldInv = deltaCostInv(d);
cNew = Inf;
else % we found a new cell we thought before it was not-traversable but % blocked by an obstacle (but that obstacle never existed!); % so the old cost for moving this successor was infinite (=not traversable)
cOld = Inf;
cOldInv = Inf;
cNew = deltaCost(d);
end % the current cell [succX,succY] we are on has changed, so % we check all possible predecessors
newEntry = [];
for p = 1:length(delta)
predX = succX - delta(p,1); % determine the predecessor by subtracting the delta-step
predY = succY - delta(p,2); % determine the predecessor by subtracting the delta-step if predX >= 1 && predX <= size(knownGrid,1) && predY >= 1 && predY <= size(knownGrid,2) && knownGrid(predX,predY) == 0 if ~(predX == sGoal(1) && predY == sGoal(2)) ifisinf( cOld )
newEntry = [ newEntry; succX, succY, predX, predY, cOld, deltaCostInv(p); predX, predY, succX, succY, cOld, deltaCost(p)];
else
newEntry = [ newEntry; succX, succY, predX, predY, deltaCostInv(p), cNew; predX, predY, succX, succY, cNew, deltaCostInv(p)];
end end end end
newEntry = [ sCurrent(1), sCurrent(2), succX, succY, cOld, cNew; succX, succY, sCurrent(1), sCurrent(2), cOldInv, cNew; newEntry ];
changeGrid = [ changeGrid; newEntry ]; % append the newly found changed cell-coordinates and old costs
knownGrid(succX,succY) = trueGrid(succX,succY); % end end end end
function[ uPrioQueue, gridRHS, gridG ] = computeShortestPath( uPrioQueue, grid, gridRHS, gridG, sStart, sGoal, delta, deltaCostInv, kM ) while ~isempty(uPrioQueue) && ( keyIsLessEq( getTopKeyPrioQueue(uPrioQueue), calculateKey(sStart,gridRHS,gridG,sStart,kM)) || gridRHS(sStart(1),sStart(2)) > gridG(sStart(1),sStart(2)))
minU = getTopCellPrioQueue(uPrioQueue);
kOld = getTopKeyPrioQueue(uPrioQueue);
kNew = calculateKey(minU, gridRHS, gridG, sStart, kM);
if keyIsLess( kOld, kNew )
uPrioQueue = updatePrioQueue(minU, kNew, uPrioQueue);
elseif gridG(minU(1),minU(2)) > gridRHS(minU(1),minU(2))
gridG(minU(1),minU(2)) = gridRHS(minU(1),minU(2));
uPrioQueue = removePrioQueue(minU, uPrioQueue);
for d = 1:length(delta)
predX = minU(1) - delta(d,1); % determine the predecessor by subtracting the delta-step
predY = minU(2) - delta(d,2); % determine the predecessor by subtracting the delta-step if predX >= 1 && predX <= size(gridRHS,1) && predY >= 1 && predY <= size(gridRHS,2) && grid(predX,predY) == 0 if ~(predX == sGoal(1) && predY == sGoal(2))
gridRHS(predX,predY) = min([ gridRHS(predX,predY), deltaCostInv(d) + gridG(minU(1),minU(2))]);
s = [predX, predY];
uPrioQueue = updateVertex(s, uPrioQueue, gridG, gridRHS, sStart, kM);
end end end else
gOld = gridG(minU(1),minU(2));
gridG(minU(1),minU(2)) = Inf;
newDelta = [delta;0,0]; % we also have to include the cell minU itself, not only it's predecessors, so we add a "zero-move" to all possible movements
newDeltaCostInv = [deltaCostInv, 0];
for d = 1:length(newDelta)
predX = minU(1) - newDelta(d,1); % determine the predecessor by subtracting the delta-step
predY = minU(2) - newDelta(d,2); % determine the predecessor by subtracting the delta-step if predX >= 1 && predX <= size(gridRHS,1) && predY >= 1 && predY <= size(gridRHS,2) && grid(predX,predY) == 0 if gridRHS(predX,predY) == newDeltaCostInv(d) + gOld
if ~(predX == sGoal(1) && predY == sGoal(2))
gridRHS(predX,predY) = min([ gridRHS(predX,predY), deltaCostInv(d) + gridG(minU(1),minU(2))]);
end end
s = [predX, predY];
uPrioQueue = updateVertex(s, uPrioQueue, gridG, gridRHS, sStart, kM);
end end end end end
function[ uPrioQueue, kM, gridRHS, gridG ] = initializeDStar( knownGrid, sStart, sGoal );
uPrioQueue = []; % initialize Priority Queue U as empty % 1st and 2nd row are priority k = [k1,k2] % 3rd and 4th row are x- and y-coordinate of vertex
kM = 0; % Keymodifier k_m is initialized as zero % initialize rhs(s) = g(s) = Inf for all s in S (so for all vertices in the grid) for x = 1:size(knownGrid,1) for y = 1:size(knownGrid,2)
gridRHS(x,y) = Inf; % right-hand-side-value (quote: "rhs-value of a vertex is based on the g-values of its predecessors and thus potentially better informed than them")
gridG(x,y) = Inf; % g-value end end % initialize rhs(sGoal) = 0
gridRHS( sGoal(1), sGoal(2)) = 0;
function[ uPrioQueue ] = insertPrioQueue( newEntry, uPrioQueue )
uPrioQueue = [ uPrioQueue; newEntry ]; % insert into Priority Queue
uPrioQueue = sortrows(uPrioQueue, [-1-2]); % first order by key 1 (1st column) in descending order, then by key 2 (2nd column) also in descending order end
function[ uPrioQueue ] = removePrioQueue( s, uPrioQueue )
rowNum = find(ismember( uPrioQueue(:,3:4), s, 'rows' ), 1);
if ~isempty( rowNum )
uPrioQueue(rowNum,:) = [];
uPrioQueue = sortrows(uPrioQueue, [-1-2]); % first order by key 1 (1st column) in descending order, then by key 2 (2nd column) also in descending order end end
function[ uPrioQueue ] = updatePrioQueue( s, kNew, uPrioQueue )
rowNum = find(ismember( uPrioQueue(:,3:4), s, 'rows' ), 1);
if ~isempty( rowNum )
uPrioQueue(rowNum,1:2) = kNew;
uPrioQueue = sortrows(uPrioQueue, [-1-2]); % first order by key 1 (1st column) in descending order, then by key 2 (2nd column) also in descending order end end
function[ topCell ] = getTopCellPrioQueue( uPrioQueue ) ifisempty(uPrioQueue)
topCell = ones(0,2); % empty 0-by-2-vector else
topCell = uPrioQueue(end,3:4); % return cell coordinates with lowest priority in priority queue end end
function[ topKey ] = getTopKeyPrioQueue( uPrioQueue ) ifisempty(uPrioQueue)
topKey = Inf(1,2); % if priority queue is empty, we return [Inf,Inf] else
topKey = uPrioQueue(end,1:2);
end end
function[ uPrioQueue, minCell ] = popMinPrioQueue( uPrioQueue ) % get cell with smallest priority from priority queue (last element, since we ordered in descending order) % element will be removed from priority queue
uPrioQueue = uPrioQueue(1:end-1,:);
minCell = uPrioQueue(end,:);
end
Du kannst Beiträge in dieses Forum schreiben. Du kannst auf Beiträge in diesem Forum antworten. Du kannst deine Beiträge in diesem Forum nicht bearbeiten. Du kannst deine Beiträge in diesem Forum nicht löschen. Du kannst an Umfragen in diesem Forum nicht mitmachen. Du kannst Dateien in diesem Forum posten Du kannst Dateien in diesem Forum herunterladen
MATLAB, Simulink, Stateflow, Handle Graphics, Real-Time Workshop, SimBiology, SimHydraulics, SimEvents, and xPC TargetBox are registered trademarks and The MathWorks, the L-shaped membrane logo, and Embedded MATLAB are trademarks of The MathWorks, Inc.
RSS