1 算法介绍
1.1 D星算法
1.2 栅格地图
栅格地图有两种表示方法，直角坐标系法和序号法，序号法比直角坐标法节省内存
室内环境栅格法建模步骤
1.栅格粒大小的选取
栅格的大小是个关键因素，栅格选的小，环境分辨率较大，环境信息存储量大，决策速度慢。
栅格选的大，环境分辨率较小，环境信息存储量小，决策速度快，但在密集障碍物环境中发现路径的能力较弱。
2.障碍物栅格确定
当机器人新进入一个环境时，它是不知道室内障碍物信息的，这就需要机器人能够遍历整个环境，检测障碍物的位置，并根据障碍物位置找到对应栅格地图中的序号值，并对相应的栅格值进行修改。自由栅格为不包含障碍物的栅格赋值为0，障碍物栅格为包含障碍物的栅格赋值为1.
3.未知环境的栅格地图的建立
通常把终点设置为一个不能到达的点，比如（-1，-1），同时机器人在寻路过程中遵循 “下右上左” 的原则，即机器人先向下行走，当机器人前方遇到障碍物时，机器人转向右走，遵循这样的规则，机器人最终可以搜索出所有的可行路径，并且机器人最终将返回起始点。
备注：在栅格地图上，有这么一条原则，障碍物的大小永远等于n个栅格的大小，不会出现半个栅格这样的情况。
2 部分代码
%*********************************************初始化开始，给出全局参数************************************************
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; %显示路径
3 仿真结果
4 参考文献
[1]杨奇峰, 曲道奎, 徐方. 基于障碍物运动预测的移动机器人路径规划[J]. 计算机工程与设计, 42(1):7.
部分理论引用网络文献，若有侵权联系博主删除。
