欢迎光临散文网 会员登陆 & 注册

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

2021-08-16 00:13 作者:Matlab工程师  | 我要投稿

一、算法介绍

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; %显示路径




【路径规划】基于D星算法实现栅格地图机器人路径规划的评论 (共 条)

分享到微博请遵守国家法律