【栅格地图路径规划】基于matlab D星和D星_Lite算法机器人栅格地图路径规划【含Matlab源码 2530期】

💥💥💥💥💥💥💞💞💞💞💞💞💞💞欢迎来到海神之光博客之家💞💞💞💞💞💞💞💞💥💥💥💥💥💥

✅博主简介：热爱科研的Matlab仿真开发者，修心和技术同步精进；
🍎个人主页：海神之光
🏆代码获取方式：
海神之光Matlab王者学习之路—代码获取方式

⛳️座右铭：行百里者，半于九十。
更多Matlab路径规划仿真内容点击👇
①Matlab路径规划（进阶版）
②付费专栏Matlab路径规划（初级版）

⛳️关注优快云海神之光，更多资源等你来！！

⛄一、简介

“D*算法”的名称源自 Dynamic A Star,最初由Anthony Stentz于“Optimal and Efficient Path Planning for Partially-Known Environments”中介绍。它是一种启发式的路径搜索算法，适合面对周围环境未知或者周围环境存在动态变化的场景。

1 算法介绍
同A算法类似，D-star通过一个维护一个优先队列（OpenList）来对场景中的路径节点进行搜索，所不同的是，D不是由起始点开始搜索，而是以目标点为起始，通过将目标点置于Openlist中来开始搜索，直到机器人当前位置节点由队列中出队为止（当然如果中间某节点状态有动态改变，需要重新寻路，所以才是一个动态寻路算法）。

2 .1 符号表示
本部分主要介绍一下论文中用到的一些符号及其含义。
论文中将地图中的路径点用State表示，每一个State包含如下信息：

Backpointer: 指向前一个state的指针，指向的state为当前状态的父辈，当前state称为指针指向state的后代，目标state无Backpointer。（路径搜索完毕后，通过机器人所在的state，通过backpointer即可一步步地移动到目标Goal state，GoalState以后用 G表示），b（X）=Y表示X的父辈为Y。

Tag：表示当前state的状态，有 New、Open、Closed三种状态，New表示该State从未被置于Openlist中，Open表示该State正位于OpenList中，Closed表示已不再位于Openlist中。

H（X）：代价函数估计，表示当前State到G的开销估计。

K（X）：Key Function，该值是优先队列Openlist中的排序依据，K值最小的State位于队列头 ，对于处于OpenList上的State X，K（X）表示从X被置于Openlist后，X到G的最小代价H（X），可以简单理解为。K（X）将位于Openlist的State X划分为两种不同的状态，一种状态为Raise（如果K（X）<H（X）），用来传递路径开销的增加（例如某两点之间开销的增加，会导致与之相关的节点到目标的路径开销随之增加）；另一种状态为 Lower（如果K（X）<H（X）），用来传递路径开销的减少（例如某两点之间开销的减少，或者某一新的节点被加入到Openlist中，可能导致与之相关的节点到目标的路径开销随之减少）。
kmin：表示所有位于Openlist上的state的最小K值。
C（X,Y） ：表示X与Y之间的路径开销。
Openlist 是依据K值由小到大进行排序的优先队列。

1.2 算法描述
搜索的关键是state的传递过程，即由G向机器人所在位置进行搜索的过程，这种传递过程是通过不断地从当前OpenList中取出K值最小的State来实现的，每当一个State由Openlist中移出时，它会将开销传递给它的邻居state，这些邻居state会被置于Openlist中，持续进行该循环，直到机器人所在State的状态为 Closed ，或者Openlist为空（表示不存在到G的路径）。

算法最主要的是两个函数， Process-State 和 Modify-Cost ，前者用于计算到目标G的最优路径，后者用于改变两个state之间的开销C（X,Y）并将受影响的state置于Openlist中。

算法的主要流程，在初始时，所有state的t（Tag）被设置为 New ，H（G）被设置为0，G被放置于OpenList，然后Process-State函数被不断执行，直到机器人所处state X由openlist中出队，然后可以通过机器人的当前state按backpointer指向目标G。当移动过程中发现新探测到的障碍时，Modify-Cost函数立刻被调用，来更正C（°）中的路径开销并将受影响的state重新置于openlist中。令Y表示robot发现障碍时所在的state，通过不断调用Process-State直到kmin≥H（Y），这时表示路径开销的更改已经传播到了Y，此时，新的路径构建完成。

上图中L1-L3表示拥有最低K值的X由openlist中移出，如果X为Lower，那么它的路径代价为最优的。

在L8-L13，X的所有邻接state都被检测是否其路径代价可以更低，状态为New的邻接state被赋予初始路径开销值，并且开销的变动被传播给每一个backpointer指向X的邻接state Y（不管这个新的开销比原开销大或者小），也就是说只要你指向了X，那么X的路径开销变动时，你的路径代价必须随之改变。这里可能存在由于X路径开销变动过大，Y可以通过非X的其他state到达目标且路径开销更小的情况，这点在L8-13中并没有处理，而是放在后续针对Y的process-state函数中，在对Y进行处理时，会将其backpointer指向周围路径开销最小的state。如果X的邻接State状态为New时，应将其邻接state的backpointer指向X。所有路径开销有所变动的state都被置于Openlist中进行处理，从而将变动传播给邻接的state。
上述讨论的时X为Lower状态，接下来讨论X为Raise状态。

如果X为Raise，它的路径开销H可能不是最优的，在L4-L7中，通过其邻居state中已经处于最优开销（即h（Y）≤kold）的节点来优化X的路径开销，如果存在更短的路径，则将X的backpointer指向其neighbor。在L15-L18中，开销变动传播到状态为New的邻居state。如果X可以使一个backpointer并不指向X的邻居state的路径开销最小，即Y通过X到目标G的距离更短，但是此时Y的backpointer并不指向X，针对这种情况，可以将X重新置于Openlist中进而优化Y。在L23-25中，如果X可以通过一个状态为closed的并不是最理想的邻居stateY来减小路径开销，那么将Y重新置于Openlist中。

在modify-cost中，更新C（X,Y）并将X重新置于Openlist中，当X通过process-state进行传播时，会对Y进行开销计算，h（Y）=h(X)+c(X,Y)。

2 算法总结
相比A-star算法，D-star的主要特点就是由目标位置开始向起始位置进行路径搜索，当物体由起始位置向目标位置运行过程中，发现路径中存在新的障碍时，对于目标位置到新障碍之间的范围内的路径节点，新的障碍是不会影响到其到目标的路径的。新障碍只会影响的是物体所在位置到障碍之间范围的节点的路径。在这时通过将新的障碍周围的节点加入到Openlist中进行处理然后向物体所在位置进行传播，能最小程度的减少计算开销。 路径搜索的过程我个人感觉其实和Dijkstra算法比较像，A-star算法中f(n)=g(n)+h(n)，h(n)在D-star中并没有体现，路径的搜索并没有A-star所具有的方向感，即朝着目标搜索的感觉，这种搜索更多的是一种由目标位置向四周发散搜索，直到把起始位置纳入搜索范围为止，更像是Dijkstra算法。

⛄二、部分源代码

function [Cost, p1, p2, Time] = Dstar( field1, field2, start, goal )

rows = size(field1,1);
cols = size(field1,2);

startPos = rows * start(1) - start(2) + 1;
goalPos = rows * goal(1) - goal(2) + 1;

%% 第一阶段：搜索路径

t1 = tic;

Nodes = struct;
for i = 1:rows*cols
Nodes(i).node = i;
Nodes(i).t = ‘new’;
Nodes(i).k = inf;
Nodes(i).h = inf;
Nodes(i).parent = nan;
end

Nodes(goalPos).t = ‘open’;
Nodes(goalPos).k = 0;
Nodes(goalPos).h = 0;
Nodes(goalPos).parent = nan;

openList = [goalPos,Nodes(goalPos).k];

while true
[Nodes,openList,k_old] = process_state(field1,Nodes,openList,goalPos);
if isequal(Nodes(startPos).t,‘closed’)
break;
end
end

node = startPos;
path1 = node;
while true
path1(end+1) = Nodes(node).parent;
node = Nodes(node).parent;
if node == goalPos
break;
end
end

Time(1) = toc(t1);
Cost(1) = cost_path(field1,path1);

[p1(:,1),p1(:,2)] = ind2sub([rows,cols],path1);

%% 第二阶段：遭遇障碍物，重新搜索路径

node = startPos;
path2 = node;

t2 = tic;

while node ~= goalPos

parentNode = Nodes(node).parent;

if field2(parentNode) == 1 || field2(parentNode) == 2

     Nodes(parentNode).h = inf;

    if isequal(Nodes(node).t,'closed')
        h_new = Nodes(parentNode).h + cost_X_Y(field2,Nodes(node).node,Nodes(parentNode).node);
        [Nodes,openList] = insert(Nodes,openList,node,h_new);
    end
end
    while true
        [Nodes,openList,k_min] = process_state(field2,Nodes,openList,goalPos);
        if k_min >= Nodes(node).h
            break;
        end

    end
node = Nodes(node).parent;
path2(end+1) = node;

end

Time(2) = toc(t2);
Cost(2) = cost_path(field2,path2);

[p2(:,1), p2(:,2)] = ind2sub([rows, cols], path2);

end

⛄三、运行结果

⛄四、matlab版本及参考文献

1 matlab版本
2014a

2 参考文献
[1]卞永明,季鹏成,周怡和,杨濛.基于改进型DWA的移动机器人避障路径规划[J].中国工程机械学报. 2021,19(01)

3 备注
简介此部分摘自互联网，仅供参考，若侵权，联系删除

🍅 仿真咨询
1 各类智能优化算法改进及应用
生产调度、经济调度、装配线调度、充电优化、车间调度、发车优化、水库调度、三维装箱、物流选址、货位优化、公交排班优化、充电桩布局优化、车间布局优化、集装箱船配载优化、水泵组合优化、解医疗资源分配优化、设施布局优化、可视域基站和无人机选址优化

2 机器学习和深度学习方面
卷积神经网络（CNN）、LSTM、支持向量机（SVM）、最小二乘支持向量机（LSSVM）、极限学习机（ELM）、核极限学习机（KELM）、BP、RBF、宽度学习、DBN、RF、RBF、DELM、XGBOOST、TCN实现风电预测、光伏预测、电池寿命预测、辐射源识别、交通流预测、负荷预测、股价预测、PM2.5浓度预测、电池健康状态预测、水体光学参数反演、NLOS信号识别、地铁停车精准预测、变压器故障诊断

3 图像处理方面
图像识别、图像分割、图像检测、图像隐藏、图像配准、图像拼接、图像融合、图像增强、图像压缩感知

4 路径规划方面
旅行商问题（TSP）、车辆路径问题（VRP、MVRP、CVRP、VRPTW等）、无人机三维路径规划、无人机协同、无人机编队、机器人路径规划、栅格地图路径规划、多式联运运输问题、车辆协同无人机路径规划、天线线性阵列分布优化、车间布局优化

5 无人机应用方面
无人机路径规划、无人机控制、无人机编队、无人机协同、无人机任务分配

6 无线传感器定位及布局方面
传感器部署优化、通信协议优化、路由优化、目标定位优化、Dv-Hop定位优化、Leach协议优化、WSN覆盖优化、组播优化、RSSI定位优化

7 信号处理方面
信号识别、信号加密、信号去噪、信号增强、雷达信号处理、信号水印嵌入提取、肌电信号、脑电信号、信号配时优化

8 电力系统方面
微电网优化、无功优化、配电网重构、储能配置

9 元胞自动机方面
交通流 人群疏散 病毒扩散 晶体生长

10 雷达方面
卡尔曼滤波跟踪、航迹关联、航迹融合