✅博主简介:热爱科研的Matlab仿真开发者,修心和技术同步精进,Matlab项目合作可私信。
🍎个人主页:海神之光
🏆代码获取方式:
海神之光Matlab王者学习之路—代码获取方式
⛳️座右铭:行百里者,半于九十。
更多Matlab仿真内容点击👇
Matlab图像处理(进阶版)
路径规划(Matlab)
神经网络预测与分类(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 雷达方面
卡尔曼滤波跟踪、航迹关联、航迹融合