【机器人栅格地图】基于D星算法实现栅格地图机器人路径规划附matlab代码

1 简介

2 部分代码

function [Nodes, openList, k_old] = process_state(field, Nodes, openList,goalPos)
% 获得openList中k值最小的节点X及k_old
[k_old,idx] = min(openList(:,2));
X = openList(idx,1);
% 从openList中移出X，并修改其状态为closed
openList(idx,:) = [];
Nodes(X).t = 'closed';
% X的邻节点
neighborNodes = getNeighborNode(field, X);
%% 修正X的父节点及相关信息
% 判断k_old 是否小于 Nodes(X).h,若是表明该节点已经受到障碍的影响
% 那么就判断X的邻节点，看是否能够以某个邻节点作为父节点，使Nodes(X).h变小
if k_old < Nodes(X).h
% 遍历X的邻节点Y
for i = 1:size(neighborNodes,1)
Y = neighborNodes(i,1);
cost_X_Y = neighborNodes(i,2);
% 如果Y点的h值没有上升，并且X的h值能通过Y变得更小,那么就修改X的父节点为Y，重置其h的值。
% 同时判断Nodes(Y).h <= k_old，考察Y本身是否收到障碍影响导致Nodes(Y).h > k_old
% 若Nodes(Y).h > k_old，那么再以Y作为父节点就没有意义了
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进一步减小了，
% 但与k_old谁大谁小还尚未可知，需要下一节继续判断
end
end
end
%% 上一节修正了X的信息，进一步判断，并修正Y的父节点及相关信息
if k_old == Nodes(X).h
% 若k_old = Nodes(X).h，Lower态，有下面三种可能：
% 1）处于第一遍遍历的阶段；
% 2）该节点X并没有受到障碍影响；
% 3）受到了障碍物影响，但是在上一节已经更新了X的parent
% 遍历X的邻节点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，表明邻节点Y还未纳入openList，那么就以X作为父节点；
% 若情况2，表明虽然Y的父节点是X，但是 Nodes(Y).h却与Nodes(X).h + cost_X_Y不相等了，
% 表明Y的父节点Nodes(X).h 有过更新，可能是由于障碍引起的；
% 若情况3，表明Y可以通过将X作为父节点，使得Nodes(Y).h更小
% 上述情况，都应该将X作为Y的父节点，并把Y移到openList后，再进一步考察
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态
% 说明节点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，表明邻节点Y还未纳入openList，那么就以X作为父节点；
% 若情况2，表明虽然Y的父节点是X，但是 Nodes(Y).h却与Nodes(X).h + cost_X_Y不相等了，
% 上述情况，都应该将X作为Y的父节点，并把Y移到openList后，再进一步考察
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可以通过将X作为父节点，使得Nodes(Y).h更小
% 但是目前针对X，有 k_old ~= Nodes(X).h，故要将X追加到openList，待下一次循环满足条件就将X作为Y的父节点。
[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的父节点不是X，但是让Y成为X父节点， Nodes(X).h更小；
% 情况2，表明并且Y已经被open表移除；
% 情况3，当前从openList取出的最小值k_old居然比h(Y)小，表明已经被移除open表的Y受到了障碍影响导致h值升高
% 上述情况，都要重新将Y置于openList中，进行下一轮考察。
[Nodes,openList] = insert(Nodes,openList,Y,Nodes(Y).h);
end
end
end
end
end

3 仿真结果

4 参考文献

[1]王帅军. 基于D~*算法的移动机器人路径规划. Diss. 广西大学.

博主简介：擅长智能优化算法、神经网络预测、信号处理、元胞自动机、图像处理、路径规划、无人机等多种领域的Matlab仿真，相关matlab代码问题可私信交流。

部分理论引用网络文献，若有侵权联系博主删除。