路径规划基于D星算法实现栅格地图机器人路径规划
Posted Matlab走起
tags:
篇首语:本文由小常识网(cha138.com)小编为大家整理,主要介绍了路径规划基于D星算法实现栅格地图机器人路径规划相关的知识,希望对你有一定的参考价值。
一、算法介绍
A* 在静态路网中非常有效(very efficient for static worlds),但不适于在动态路网,环境如权重等不断变化的动态环境下。
D是动态A(D-Star,Dynamic A Star) 卡内及梅隆机器人中心的Stentz在1994和1995年两篇文章提出,主要用于机器人探路。是火星探测器采用的寻路算法。
Optimal and Efficient Path Planning for Partially-Known Environments
The Focussed D* Algorithm for Real-Time Replanning
主要方法(这些完全是Drew在读了上述资料和编制程序中的个人理解,不能保证完全正确,仅供参考):
1.先用Dijstra算法从目标节点G向起始节点搜索。储存路网中目标点到各个节点的最短路和该位置到目标点的实际值h,k(k为所有变化h之中最小的值,当前为k=h。每个节点包含上一节点到目标点的最短路信息1(2),2(5),5(4),4(7)。则1到4的最短路为1-2-5-4。 原OPEN和CLOSE中节点信息保存。
2.机器人沿最短路开始移动,在移动的下一节点没有变化时,无需计算,利用上一步Dijstra计算出的最短路信息从出发点向后追述即可,当在Y点探测到下一节点X状态发生改变,如堵塞。机器人首先调整自己在当前位置Y到目标点G的实际值h(Y),h(Y)=X到Y的新权值c(X,Y)+X的原实际值h(X).X为下一节点(到目标点方向Y->X->G),Y是当前点。k值取h值变化前后的最小。
3.用A或其它算法计算,这里假设用A算法,遍历Y的子节点,点放入CLOSE,调整Y的子节点a的h值,h(a)=h(Y)+Y到子节点a的权重C(Y,a),比较a点是否存在于OPEN和CLOSE中,方法如下:
while() { 从OPEN表中取k值最小的节点Y; 遍历Y的子节点a,计算a的h值 h(a)=h(Y)+Y到子节点a的权重C(Y,a) { if(a in OPEN) 比较两个a的h值 if( a的h值小于OPEN表a的h值 ) { 更新OPEN表中a的h值;k值取最小的h值 有未受影响的最短路经存在 break; } if(a in CLOSE) 比较两个a的h值 //注意是同一个节点的两个不同路径的估价值 if( a的h值小于CLOSE表的h值 ) { 更新CLOSE表中a的h值; k值取最小的h值;将a节点放入OPEN表 有未受影响的最短路经存在 break; } if(a not in both) 将a插入OPEN表中; //还没有排序 } 放Y到CLOSE表; OPEN表比较k值大小进行排序; } 机器人利用第一步Dijstra计算出的最短路信息从a点到目标点的最短路经进行。
D*算法在动态环境中寻路非常有效,向目标点移动中,只检查最短路径上下一节点或临近节点的变化情况,如机器人寻路等情况。对于距离远的最短路径上发生的变化,则感觉不太适用。
上图是Drew在4000个节点的随机路网上做的分析演示,细黑线为第一次计算出的最短路,红点部分为路径上发生变化的堵塞点,当机器人位于982点时,检测到前面发生路段堵塞,在该点重新根据新的信息计算路径,可以看到圆圈点为重新计算遍历过的点,仅仅计算了很少得点就找到了最短路,说明计算非常有效,迅速。绿线为计算出的绕开堵塞部分的新的最短路径。
二、栅格地图
栅格地图有两种表示方法,直角坐标系法和序号法,序号法比直角坐标法节省内存
室内环境栅格法建模步骤
1.栅格粒大小的选取
栅格的大小是个关键因素,栅格选的小,环境分辨率较大,环境信息存储量大,决策速度慢。
栅格选的大,环境分辨率较小,环境信息存储量小,决策速度快,但在密集障碍物环境中发现路径的能力较弱。
2.障碍物栅格确定
当机器人新进入一个环境时,它是不知道室内障碍物信息的,这就需要机器人能够遍历整个环境,检测障碍物的位置,并根据障碍物位置找到对应栅格地图中的序号值,并对相应的栅格值进行修改。自由栅格为不包含障碍物的栅格赋值为0,障碍物栅格为包含障碍物的栅格赋值为1.
3.未知环境的栅格地图的建立
通常把终点设置为一个不能到达的点,比如(-1,-1),同时机器人在寻路过程中遵循“下右上左”的原则,即机器人先向下行走,当机器人前方遇到障碍物时,机器人转向右走,遵循这样的规则,机器人最终可以搜索出所有的可行路径,并且机器人最终将返回起始点。
备注:在栅格地图上,有这么一条原则,障碍物的大小永远等于n个栅格的大小,不会出现半个栅格这样的情况。
三、代码
%D* Lite算法
%By Aword 2019/05/20
%*********************************************初始化开始,给出全局参数************************************************
clc;
clear;
global n_r;
global n_c;
global s_start;
global s_goal;
global U;
global km;
global g;
global rhs;
global key;
global neighbour;
global map;
%*******************************可修改参数*************************************
n_r=30;%定义地图大小-行
n_c=30;%定义地图大小-列
s_start=[1 1];%起始点
s_goal=[30 30];%目标点
%********************************初始化***************************************
U=[];%优先级列表,用于存储待扩展的非一致节点(g(s)!=rhs(s))
km=0;%记录最初起点到当前起始点的代价值
g=[];%g和rhs表示节点s到目标点的最小代价的估计值
rhs=[];%rhs是由其前向节点(起始点到当前点)的g值计算得到
key=[];
path=[];%存储规划路径
neighbour=[1,0; %八向搜寻
0,1;
0,-1;
-1,0;
1,1;
1,-1;
-1,1;
-1,-1];
% neighbour=[1,0; %四向搜寻
% 0,1;
% 0,-1;
% -1,0];
s_last=s_start;%当前位置点sl(下一时刻的位置点)视为新的起始点反复计算目标点sg与新的起始点间的最短路径
g(1:n_r,1:n_c)=Inf;%遍历地图节点集S并初始化,这里注意行列对应的坐标是相反的
rhs(1:n_r,1:n_c)=Inf;
rhs(s_goal(1),s_goal(2))=0;%目标点rhs置0
CalculateKey(s_goal);
U=[s_goal,key(s_goal(1),s_goal(2)).key1,key(s_goal(1),s_goal(2)).key2];%讲目标点及其key值插入到优先列表U中
%*******************************生成原始地图************************************
cmap = [1 1 1; ...% 1 -白色-无障碍
0 0 0; ...% 2 -有障碍
0 1 0; ...% 3 -起始点
0 0 1; ...% 4 -目标点
1 0 0; ...% 5 -最终路径
0.5 0.5 0.5]; % 6 -扩展节点
% 黑 0 0 0
% 白 1 1 1
% 红 1 0 0
% 绿 0 1 0
% 蓝 0 0 1
% 黄 1 1 0
% 灰 0.5 0.5 0.5
% 洋红 1 0 1
% 青蓝 0 1 1
% 天蓝 0.67 0 1
% 橘黄 1 0.5 0
% 深红 0.5 0 0
colormap(cmap);
map = ones(n_r,n_c);
randmap = rand(n_r,n_c);%返回一个n_rxn_c的(0~1)随机数矩阵,生成随机地图
for i=1:n_r%遍历地图节点并初始化障碍物信息
for j=1:n_c
if (randmap(i,j) >= 0.75)%该数大小决定障碍物密度
map(i,j) = 2;%置为障碍物
end
end
end
map(s_start(1), s_start(2)) = 3; % 起点坐标
map(s_goal(1), s_goal(2)) = 4; % 终点坐标
% set(gca,'xtick',[],'ytick',[])%去掉刻度标记
subplot(1, 3, 1)
image(1.5,1.5,map);
grid on; %网格
axis image; %显示更新前地图
hold on;
%*******************************生成动态地图************************************
map_ob = ones(n_r,n_c);
randmap = rand(n_r,n_c);%返回一个n_rxn_c的(0~1)随机数矩阵
end
%*******************************动态绘制地图************************************
for i=1:size(path,1)
map(path(i,1),path(i,2))=5;
end
map(s_start(1), s_start(2)) = 3; % 起点坐标
map(s_goal(1), s_goal(2)) = 4; % 终点坐标
subplot(1, 3, 3)
image(1.5,1.5,map);
if (n_r==10)&&(n_c==10)
for i=1:n_r%遍历地图节点并标记节点信息
for j=1:n_c
text(j,i+0.2,num2str(g(i,j)),'FontSize',10,'color','k')
text(j,i+0.5,num2str(rhs(i,j)),'FontSize',10,'color','k')
text(j,i+0.8,num2str(key(i,j).key1),'FontSize',10,'color','k')
text(j+0.5,i+0.8,num2str(key(i,j).key2),'FontSize',10,'color','k')
end
end
end
grid on; %网格
axis image; %显示路径
pause(0.1);
%*******************************动态绘制地图************************************
end
%*************************************************主体循环结束******************************************************
for i=1:n_r%查看扩展节点
for j=1:n_c
if map(i,j)==1
if rhs(i,j)~=Inf
map(i,j)=6;
elseif key(i,j).key2~=Inf
map(i,j)=6;
end
end
end
end
subplot(1, 3, 3)
image(1.5,1.5,map);
if (n_r==10)&&(n_c==10)
for i=1:n_r%遍历地图节点并标记节点信息
for j=1:n_c
text(j,i+0.2,num2str(g(i,j)),'FontSize',10,'color','k')
text(j,i+0.5,num2str(rhs(i,j)),'FontSize',10,'color','k')
text(j,i+0.8,num2str(key(i,j).key1),'FontSize',10,'color','k')
text(j+0.5,i+0.8,num2str(key(i,j).key2),'FontSize',10,'color','k')
end
end
end
grid on; %网格
axis image; %显示路径
完整代码添加QQ1575304183
以上是关于路径规划基于D星算法实现栅格地图机器人路径规划的主要内容,如果未能解决你的问题,请参考以下文章