Table of contents
Algorithm Introduction
A* is very effective in static road networks (very efficient for static worlds), but it is not suitable for dynamic road networks and dynamic environments where the environment such as weights are constantly changing.
D is Dynamic A (D-Star, Dynamic A Star) Carney and Stentz of the Mellon Robotics Center proposed two articles in 1994 and 1995, which are mainly used for robot pathfinding. It is the pathfinding algorithm adopted by the Mars rover.
Optimal and Efficient Path Planning for Partially-Known Environments
The Focussed D* Algorithm for Real-Time Replanning
Main methods (these are entirely personal understandings of Drew after reading the above materials and programming, and cannot guarantee complete accuracy, and are for reference only):
1. Use the Dijstra algorithm to search from the target node G to the starting node. Store the shortest path from the target point to each node in the road network and the actual value h,k from the position to the target point (k is the smallest value among all changes h, currently k=h. Each node contains the previous node to the target point The shortest path information of the target point is 1(2), 2(5), 5(4), 4(7). The shortest path from 1 to 4 is 1-2-5-4. The node information in the original OPEN and CLOSE is saved .
2. The robot starts to move along the shortest path. When there is no change in the next node to move, no calculation is required. Use the shortest path information calculated by Dijstra in the previous step to trace back from the starting point. When the next node X is detected at point Y A state change occurs, such as a blockage. The robot first adjusts its actual value h(Y) from the current position Y to the target point G, h(Y)=the new weight c(X,Y)+X’s original actual value h(X).X For the next node (to the target point direction Y->X->G), Y is the current point. The value of k is the minimum before and after the change of the value of h.
3. Use A or other algorithms to calculate. Here, it is assumed to use A algorithm to traverse the child nodes of Y, put the point into CLOSE, and adjust the h value of Y’s child node a, h(a)=h(Y)+Y to the child nodes The weight C(Y,a) of a, compare whether point a exists in OPEN and CLOSE, the method is as follows:
while() { Take the node Y with the smallest k value from the OPEN table; traverse the child node a of Y, and calculate the h value of a h(a)=h(Y)+Y to the weight of child node a C(Y,a ) { if(a in OPEN) Compare the h values of two a if (the h value of a is less than the h value of a in the OPEN table) { update the h value of a in the OPEN table; the h value with the smallest k value is unaffected The shortest path exists break; } if(a in CLOSE) Compare the h values of two a's//Note that the estimated value of two different paths of the same node if(a's h value is less than the h value of the CLOSE table) { Update the h value of a in the CLOSE table; the k value takes the smallest h value; put a node into the OPEN table and there is an unaffected shortest path break; } if(a not in both) insert a into the OPEN table; //Not yet sorted} Put Y to the CLOSE table; OPEN table compares the k value for sorting; } The robot uses the shortest path information calculated by Dijstra in the first step to proceed from the shortest path from point a to the target point.
The D* algorithm is very effective in pathfinding in a dynamic environment. When moving to the target point, it only checks the changes of the next node or adjacent nodes on the shortest path, such as robot pathfinding. For changes that occur on the shortest path with a long distance, it does not feel suitable.
The picture above is an analysis demonstration made by Drew on a random road network with 4000 nodes. The thin black line is the shortest path calculated for the first time, and the red dots are the changed congestion points on the path. When the robot is at 982 points, the detection When road congestion occurs in the front, the path is recalculated based on the new information at this point. You can see that the circled points are the points that have been recalculated and traversed. Only a few points are calculated and the shortest path is found, which shows that the calculation is very effective and fast. The green line is the calculated new shortest path around the congested section.
grid map
There are two representation methods for raster maps, the Cartesian coordinate system method and the serial number method. The serial number method saves memory compared to the Cartesian coordinate method.
Indoor environment grid method modeling steps
1. Selection of grid grain size
The size of the grid is a key factor. The smaller the grid, the higher the environmental resolution, the larger the storage of environmental information, and the slower the decision-making speed.
The grid selection is large, the environmental resolution is small, the environmental information storage is small, and the decision-making speed is fast, but the ability to find the path in the dense obstacle environment is weak.
2. Obstacle grid determination
When the robot newly enters an environment, it does not know the information of indoor obstacles, which requires the robot to traverse the entire environment, detect the position of obstacles, and find the serial number value in the corresponding grid map according to the obstacle position, and The corresponding raster values are modified. A free grid assigns a value of 0 to a grid that does not contain obstacles, and an obstacle grid assigns a value of 1 to a grid that contains obstacles.
3. Establishment of the grid map of the unknown environment
Usually, the end point is set as an unreachable point, such as (-1, -1), and the robot follows the principle of "down, right, up, left" during the pathfinding process, that is, the robot walks down first, when the robot encounters an obstacle in front of it When , the robot turns to the right, following such a rule, the robot can eventually search out all feasible paths, and the robot will eventually return to the starting point.
Remarks: On the grid map, there is such a principle that the size of the obstacle is always equal to the size of n grids, and there will be no such situation as half a grid.
the code
%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; %显示路径
-----------------------------------
running result
