【路径规划】基于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; %显示路径