1. Introduction
The name "D* Algorithm" is derived from Dynamic A Star, which was originally introduced by Anthony Stentz in "Optimal and Efficient Path Planning for Partially-Known Environments". It is a heuristic path search algorithm, suitable for scenes where the surrounding environment is unknown or there are dynamic changes in the surrounding environment.
1 Algorithm introduction
Similar to algorithm A, D-star searches for path nodes in the scene by maintaining a priority queue (OpenList). The difference is that D does not start searching from the starting point, but starting from the target point. At the beginning, the search is started by placing the target point in the Openlist until the node at the current position of the robot is dequeued from the queue (Of course, if there is a dynamic change in the state of a node in the middle, it needs to be found again, so it is a dynamic pathfinding algorithm) .
2.1 Symbols Representation
This section mainly introduces some symbols and their meanings used in the thesis.
In the paper, the path points in the map are represented by State, and each State contains the following information:
Backpointer: Pointer to the previous state. The pointed state is the parent of the current state. The current state is called the descendant of the pointer to the state. The target state has no Backpointer. (After the path search is completed, you can move to the target Goal state step by step through the backpointer through the state where the robot is located, and GoalState will be denoted by G in the future), b(X)=Y means that the parent of X is Y.
Tag: Indicates the current state of the state. There are three states: New, Open, and Closed. New indicates that the State has never been placed in the Openlist, Open indicates that the State is in the OpenList, and Closed indicates that it is no longer in the Openlist.
H(X): Cost function estimate, which represents the cost estimate from the current State to G.
K(X): Key Function, this value is the basis for sorting in the priority queue Openlist. The State with the smallest K value is at the head of the queue. For State X on the OpenList, K(X) means that after X is placed in the Openlist, X The minimum cost H(X) to G can be simply understood as. K(X) divides the State X in the Openlist into two different states. One state is Raise (if K(X)<H(X)), which is used to convey the increase in path cost (for example, between certain two points). The increase in cost will cause the path cost from the related node to the target to increase accordingly); the other state is Lower (if K(X)<H(X)), which is used to convey the decrease in path cost (for example, a certain The reduction of the cost between the two points, or a new node is added to the Openlist, may cause the path cost of the related node to the target to be reduced accordingly).
kmin: Represents the minimum K value of all states on the Openlist.
C(X,Y): Represents the path cost between X and Y.
Openlist is a priority queue sorted according to the K value from small to large.
1.2 Algorithm description
The key to search is the transfer process of state, that is, the process of searching from G to the location of the robot. This transfer process is realized by continuously taking out the State with the smallest K value from the current OpenList, every time a State When removed from the Openlist, it will pass the cost to its neighbor states. These neighbor states will be placed in the Openlist and continue the cycle until the state of the robot is Closed or the Openlist is empty (indicating that it does not exist to G's path).
The main algorithm of the algorithm is two functions, Process-State and Modify-Cost, the former is used to calculate the optimal path to the goal G, the latter is used to change the cost C (X, Y) between the two states and will be affected by The affected state is placed in the Openlist.
The main flow of the algorithm, at the beginning, t (Tag) of all states are set to New, H (G) is set to 0, G is placed in OpenList, and then the Process-State function is continuously executed until the robot is in the state X is dequeued from the openlist, and then can point to the target G by backpointer through the current state of the robot. When a newly detected obstacle is found during the movement, the Modify-Cost function is called immediately to correct the path cost in C (°) and reset the affected state to the openlist. Let Y represent the state where the robot finds obstacles. By continuously calling Process-State until kmin≥H(Y), it means that the path cost change has propagated to Y. At this time, the new path construction is completed.
In the above figure, L1-L3 indicate that X with the lowest K value is removed from the openlist. If X is Lower, then its path cost is the best.
In L8-L13, all neighboring states of X are tested to see if their path cost can be lower. The neighboring state of New is assigned the initial path cost value, and the cost change is propagated to each neighboring state Y that points to X by the backpointer. (Regardless of whether the new cost is larger or smaller than the original cost), that is to say, as long as you point to X, when the path cost of X changes, your path cost must change accordingly. There may be a situation where the path cost of X is too large, Y can reach the target through other states other than X and the path cost is smaller. This is not handled in L8-13, but is placed in the subsequent process- for Y. In the state function, when Y is processed, its backpointer is pointed to the state with the least path cost around. If X's adjacent State state is New, the backpointer of its adjacent state should be pointed to X. All states whose path costs have changed are placed in the Openlist for processing, so that the changes are propagated to adjacent states.
In the above discussion, X is the Lower state, and then X is the Raise state.
If X is Raise, its path cost H may not be optimal. In L4-L7, the path cost of X is optimized by the nodes in the neighbor state that are already at the optimal cost (that is, h(Y)≤kold). If there is a shorter path, point the backpointer of X to its neighbor. In L15-L18, the cost change is propagated to the neighbor state whose state is New. If X can minimize the path cost of a backpointer that does not point to the neighbor state of X, that is, the distance between Y through X and the target G is shorter, but at this time the backpointer of Y does not point to X. In this case, X can be reset Put it in the Openlist to optimize Y. In L23-25, if X can reduce the path cost through a closed neighbor stateY that is not the most ideal, then Y is placed in the Openlist again.
In modify-cost, update C(X,Y) and put X back in the Openlist. When X is propagated through process-state, the cost of Y will be calculated, h(Y)=h(X)+c (X,Y).
2 Algorithm summary
Compared with the A-star algorithm, the main feature of D-star is to search for a path from the target position to the starting position. When the object moves from the starting position to the target position, a new obstacle is found in the path. , For the path nodes in the range from the target location to the new obstacle, the new obstacle will not affect the path to the target. The new obstacle only affects the path from the location of the object to the node in the range between the obstacle. At this time, by adding the nodes around the new obstacle to the Openlist for processing and then propagating to the location of the object, the computational overhead can be minimized. I personally feel that the process of path search is actually similar to Dijkstra's algorithm. In A-star algorithm, f(n)=g(n)+h(n), h(n) is not reflected in D-star. Path search There is no sense of direction that A-star has, that is, the feeling of searching towards the target. This kind of search is more of a divergent search from the target position to the surroundings until the starting position is included in the search range, more like Dijkstra algorithm.
Second, the source code
function varargout = A_GUI(varargin)
% A_GUI MATLAB code for A_GUI.fig
% A_GUI, by itself, creates a new A_GUI or raises the existing
% singleton*.
%
% H = A_GUI returns the handle to a new A_GUI or the handle to
% the existing singleton*.
%
% A_GUI('CALLBACK',hObject,eventData,handles,...) calls the local
% function named CALLBACK in A_GUI.M with the given input arguments.
%
% A_GUI('Property','Value',...) creates a new A_GUI or raises the
% existing singleton*. Starting from the left, property value pairs are
% applied to the GUI before A_GUI_OpeningFcn gets called. An
% unrecognized property name or invalid value makes property application
% stop. All inputs are passed to A_GUI_OpeningFcn via varargin.
%
% *See GUI Options on GUIDE's Tools menu. Choose "GUI allows only one
% instance to run (singleton)".
%
% See also: GUIDE, GUIDATA, GUIHANDLES
% Edit the above text to modify the response to help A_GUI
% Last Modified by GUIDE v2.5 21-Oct-2018 17:10:48
% Begin initialization code - DO NOT EDIT
gui_Singleton = 1;
gui_State = struct('gui_Name', mfilename, ...
'gui_Singleton', gui_Singleton, ...
'gui_OpeningFcn', @A_GUI_OpeningFcn, ...
'gui_OutputFcn', @A_GUI_OutputFcn, ...
'gui_LayoutFcn', [] , ...
'gui_Callback', []);
if nargin && ischar(varargin{
1})
gui_State.gui_Callback = str2func(varargin{
1});
end
if nargout
[varargout{
1:nargout}] = gui_mainfcn(gui_State, varargin{
:});
else
gui_mainfcn(gui_State, varargin{
:});
end
% End initialization code - DO NOT EDIT
% --- Executes just before A_GUI is made visible.
function A_GUI_OpeningFcn(hObject, eventdata, handles, varargin)
% This function has no output args, see OutputFcn.
% hObject handle to figure
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
% varargin command line arguments to A_GUI (see VARARGIN)
% Choose default command line output for A_GUI
handles.output = hObject;
% Update handles structure
guidata(hObject, handles);
% UIWAIT makes A_GUI wait for user response (see UIRESUME)
% uiwait(handles.figure1);
% --- Outputs from this function are returned to the command line.
function varargout = A_GUI_OutputFcn(hObject, eventdata, handles)
% varargout cell array for returning output args (see VARARGOUT);
% hObject handle to figure
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
% Get default command line output from handles structure
varargout{
1} = handles.output;
% --- Executes on button press in pushbutton1.
function pushbutton1_Callback(hObject, eventdata, handles)
% hObject handle to pushbutton1 (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
% set up color map for display 生成彩色地图
global cmap;
global map;
global n_r;
global n_c;
global state;
cmap = [1 1 1; ...% 1 -白色-无障碍
0 0 0; ...% 2 -黑色-有障碍
0 0.8 0; ...% 3 -绿色-已搜索
0 0.4 0; ...% 4 -粉色-正在搜索
0 1 1; ...% 5 -浅蓝色-起始点
1 1 0; ...% 6 -黄色-目标点
0 0 1]; % 7 -蓝色-最终路径
colormap(cmap);
%生成随机地图
map = zeros(n_r,n_c);
randmap = rand(n_r,n_c);
for i = 2:(sub2ind(size(randmap),n_r,n_c)-1)
if (randmap(i) >= 0.75)
map(i) = 2;
end
end
map(1, 1) = 5; % start_coords 起点坐标
map(n_r, n_c) = 6; % dest_coords 终点坐标
image(1.5,1.5,map);
grid on;
axis image;
set(handles.text5,'string','随机地图生成完毕');
% --- Executes on button press in pushbutton2.
function pushbutton2_Callback(hObject, eventdata, handles)
% hObject handle to pushbutton2 (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
%搜索最佳路径
global n_r;
global n_c;
global cmap;
global map;
global state;
nrows = n_r;
ncols = n_c;
start_node = sub2ind(size(map), 1, 1);
%sub2ind()函数将矩阵中的某个元素的线性序号计算出来
%线性索引号例子：2*2矩阵[1 3;中，1是第一个，5是第二个
% 5 7] ，3是第三个，7是第四个
%（matlab是列优先，不是我们通常习惯的行优先）
dest_node = sub2ind(size(map), n_r, n_c);
% Initialize distance array 初始化距离数组
distanceFromStart = Inf(nrows,ncols);
distanceFromStart(start_node) = 0 ;
% For each grid cell this array holds the index of its parent 对于每个网格单元，该数组都保存其父单元的索引
parent = zeros(nrows,ncols);
% Main Loop
while true
% Draw current map
map(start_node) = 5;
map(dest_node) = 6;
image(1.5, 1.5, map);
grid on; %网格
axis image; %显示坐标
drawnow; %刷新屏幕
% Find the node with the minimum distance 找到距离最短的节点
[min_dist, current] = min(distanceFromStart(:));
if ((current == dest_node) || isinf(min_dist)) %TF = isinf(A) 返回一个和A尺寸一样的数组， 如果A中某个元素是inf (无穷)， 则对应TF中元素是1， 否则TF中对应元素是0。
break;
end;
%搜索中心的索引坐标：current,
%搜索中心与起始点的路程：min_dist
% 这两个值后面会用。
map(current) = 3;
distanceFromStart(current) = Inf;
[i, j] = ind2sub(size(distanceFromStart), current); %索引号变为坐标
neighbor = [i-1,j;
i+1,j;
i,j+1;
i,j-1];
outRangetest = (neighbor(:,1)<1) + (neighbor(:,1)>nrows)+(neighbor(:,2)<1) + (neighbor(:,2)>ncols);
locate = find(outRangetest>0); %返回outRangetest中大于0的元素的相对应的线性索引值。
neighbor(locate,:)=[];
neighborIndex = sub2ind(size(map),neighbor(:,1),neighbor(:,2));
for i=1:length(neighborIndex)
if (map(neighborIndex(i))~=2) && (map(neighborIndex(i))~=3 && map(neighborIndex(i))~= 5)
map(neighborIndex(i)) = 4;
if (distanceFromStart(neighborIndex(i))>= min_dist + 1 )
distanceFromStart(neighborIndex(i)) = min_dist+1;
parent(neighborIndex(i)) = current;
% pause(0.02);
end
end
end
end
Three, running results
Four, remarks
Complete code or write on behalf of adding QQ 1564658423