A*算法及其改进_matlab改进a*算法

附MATLAB代码：
1.主程序

%%基于A*算法的全局路径规划
clc
clear
%%
%建立地图和设置目标点
%设置地图的长和宽
map_XYMAX=20;
map=zeros(map_XYMAX,map_XYMAX);
%绘制地图
for i=1:map_XYMAX+1
    line([-0.5,map_XYMAX-0.5],[i-1.5,i-1.5]);
    line([i-1.5,i-1.5],[-0.5,map_XYMAX-0.5]);
    hold on
end
%设置起点和终点
map_start=[0,0];
map_goal=[19,19];
%绘制起点和终点
plot(map_start(1),map_start(2),'or');
hold on
plot(map_goal(1),map_goal(2),'ob');
hold on
​
%%
%随机生成障碍物
obstacle=[];
obstacle_number=150;
ob=floor(rand([obstacle_number,2])*map_XYMAX);
removeInd=[];
for io=1:length(ob(:,1))
    if(isequal(ob(io,:),map_start) || isequal(ob(io,:),map_goal))
        removeInd=[removeInd;io];
    end
end
ob(removeInd,:)=[];
obstacle=[obstacle;ob];
for z=1:map_XYMAX+2
    obstacle=[obstacle;-1 z-2];
    obstacle=[obstacle;z-2 -1];
    obstacle=[obstacle;20 z-2];
    obstacle=[obstacle;z-2 20];
end
%将障碍物填充颜色
for i = 1:length(obstacle(:,1))
    x = obstacle(i,1);
    y = obstacle(i,2);
    X = [x-0.5,x+0.5,x+0.5,x-0.5];
    Y = [y-0.5,y-0.5,y+0.5,y+0.5];
    fill(X,Y,'k');
    hold on;
end
​
%%
%路径规划
%创建存储路径
path=[];        %此列表是从map_start向map_goal方向搜寻的路径列表
path_1=[];      %此列表是从map_goal向map_start方向搜寻的路径列表
%创建open列表
open=[];        %此列表是从map_start向map_goal方向搜寻open列表
open_1=[];      %此列表是从map_goal向map_start方向搜寻open列表
%创建close列表
close=[];       %此列表是从map_start向map_goal方向搜寻close列表
close_1=[];     %此列表是从map_goal向map_start方向搜寻close列表
%用于判断while循环是否结束
findFlag=false;
%计算起始点和目标点之间的距离
dis=10*abs(map_start(1)-map_goal(1))+10*abs(map_start(2)-map_goal(2));
%将起始点添加到open列表中去
open=[map_start(1),map_start(2),0+dis,0,0,0];
%将目标点添加到open_1列表中去
open_1=[map_goal(1),map_goal(2),0+dis,0,0,0];
%进入循环
while ~findFlag
    %%从map_start向map_goal搜寻路径
    %判断open列表是否为空
    if isempty(open(:,1))
        disp('No path!');
        return;
    end
    %判断目标点是否在open列表中
    for i=1:length(open(:,1))
        if isequal(map_goal(1:2),open(i,1:2))
            disp('Find Goal!');
            close = [open(i,:);close];
            findFlag=true;
            break;
        end
    end
    %将F值进行排序
    [Y,I]=sort(open(:,3));
    open=open(I,:);
    %将此时open列表中的第一行移除，添加到close列表中
    close=[open(1,:);close];
    current=open(1,:);
    open(1,:)=[];
    %计算更新后的8个节点的F值
    next=[-1,1,14;0,1,10;1,1,14;-1,0,10;...
        1,0,10;-1,-1,14;0,-1,10;1,-1,14];
    for j=1:length(next(:,1))
        %下一个节点参数
        m=[current(1,1)+next(j,1),current(1,2)+next(j,2),0,0,0,0];
        %从当前节点到下一个节点的G值
        m(4)=current(4)+next(j,3);
        %从当前节点到下一个节点的H值
        H=10*abs(m(1)-map_goal(1))+10*abs(m(2)-map_goal(2));
        %从当前节点到下一个节点的F值
        m(3)=m(4)+H;
        %判断当前节点是否为障碍物,如果是障碍物则忽略
        if isObstacle(m,obstacle)
            continue;
        end
        %判断下一节点所在列表
        [flag,targetInd]=FindList(m,open,close);
        %case_1:在close列表中
        if flag==1
            continue;
            %case_2:不在列表中
        elseif flag==2
            m(5:6)=[current(1,1),current(1,2)];%将当前节点作为其父节点
            open=[open;m];
            %case_3:在open列表中
        elseif m(3)<open(targetInd,3)
            %将当前节点作为其父节点
            m(5:6)=[current(1,1),current(1,2)];
            open(targetInd,:)=m;
        end
    end
    pause(0.01);
    %将open列表和close列表填充颜色
    for i=1:length(close(:,1))
        x=close(i,1);
        y=close(i,2);
        X=[x-0.5,x+0.5,x+0.5,x-0.5];
        Y=[y-0.5,y-0.5,y+0.5,y+0.5];
        fill(X,Y,'r');
    end
​
    %%从map_goal向map_start方向搜寻路径
    %判断open——1列表是否为空
    if isempty(open_1(:,1))
        disp('No path!');
        return;
    end
    %判断起始点是否在open_1列表中
    for i=1:length(open_1(:,1))
        if isequal(map_start(1:2),open_1(i,1:2))
            disp('Find Goal!');
            close_1=[open_1(i,:);close_1];
            findFlag=true;
            break;
        end
    end
    %将F值进行排序
    [Y,I]=sort(open_1(:,3));
    open_1=open_1(I,:);
    %将此时open_1列表中的第一行移除，添加到close_1列表中
    close_1=[open_1(1,:);close_1];
    current_1=open_1(1,:);
    open_1(1,:)=[];
    %计算更新后的8个节点的F值
    next=[-1,1,14;0,1,10;1,1,14;-1,0,10;...
        1,0,10;-1,-1,14;0,-1,10;1,-1,14];
    for j=1:length(next(:,1))
        %下一个节点参数
        n=[current_1(1,1)+next(j,1),current_1(1,2)+next(j,2),0,0,0,0];
        %从当前节点到下一个节点的G值
        n(4)=current_1(4)+next(j,3);
        %从当前节点到下一个节点的H1值
        H1=10*abs(n(1)-map_start(1))+10*abs(n(2)-map_start(2));
        %从当前节点到下一个节点的F值
        n(3)=n(4)+H1;
        %判断当前节点是否为障碍物,如果是障碍物则忽略
        if isObstacle(n,obstacle)
            continue;
        end
        %判断下一节点所在列表
        [flag,targetInd]=FindList(n,open_1,close_1);
        %case_1:在close_1列表中
        if flag==1
            continue;
            %case_2:不在列表中
        elseif flag==2
            n(5:6)=[current_1(1,1),current_1(1,2)];%将当前节点作为其父节点
            open_1=[open_1;n];
            %case_3:在open_1列表中
        elseif n(3)<open_1(targetInd,3)
            %将当前节点作为其父节点
            n(5:6)=[current_1(1,1),current_1(1,2)];
            open_1(targetInd,:)=n;
        end
    end
    pause(0.01);
    %将open_1列表和close_1列表填充颜色
    for i=1:length(close_1(:,1))
        x=close_1(i,1);
        y=close_1(i,2);
        X=[x-0.5,x+0.5,x+0.5,x-0.5];
        Y=[y-0.5,y-0.5,y+0.5,y+0.5];
        fill(X,Y,'y');
    end
end
​
%%
% 绘制path路线
ind=1;
while true
    path=[path; close(ind,1:2)];
    if isequal(close(ind,1:2),map_start)
        break;
    end
    for i3=1:length(close(:,1))
        if isequal(close(i3,1:2),close(ind,5:6))
            ind=i3;
            break;
        end
    end
end
if length(path)>=1
    plot(path(:,1),path(:,2),'-c','LineWidth',5);
end
disp("The line of cyan is from start to goal.")
% 绘制path_1路线
ind=1;
while true
    path_1=[path_1; close_1(ind,1:2)];
    if isequal(close_1(ind,1:2),map_goal)
        break;
    end
    for i3=1:length(close_1(:,1))
        if isequal(close_1(i3,1:2),close_1(ind,5:6))
            ind=i3;
            break;
        end
    end
end
if length(path_1)>=1
    plot(path_1(:,1),path_1(:,2),'-g','LineWidth',5);
end
disp("The line of green is from goal to start.")
%%
%判断两条路径用时的长短
Flag=false;
while ~Flag
    for k1=1:length(close(:,1))
        if isequal(map_goal(1:2),close(k1,1:2))
            disp("The line of cyan used less time.")
            Flag=true;
            break
        else
            continue
        end
    end
    for k2=1:length(close_1(:,1))
        if isequal(map_start(1:2),close_1(k2,1:2))
            disp("The line of green used less time.")
            Flag=true;
            break
        else
            continue
        end
    end
end
axis([-2 map_XYMAX+1 -2 map_XYMAX+1])
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263

2、FindList.m

function [flag,targetInd]=FindList(m,open,close)
​
%如果open为空，则一定不在open列表中
if  isempty(open)
    flag = 2;
    targetInd = [];
%open不为空时，需要检查是否在open列表中    
else
    for io = 1:length(open(:,1))
        %在Open列表中
        if isequal(  m(1:2) , open(io,1:2)  )  
            flag = 3;
            targetInd = io;
            return;
        %不在Open列表中
        else  
            flag = 2;
            targetInd = [];
        end
    end
end
​
%在Close列表中
for ic = 1:length(close(:,1))
    if isequal(  m(1:2) , close(ic,1:2)  )  %
        flag = 1;
        targetInd = ic;
        return;%在Closelist中直接return
    end
end
​
end
​
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33

3、isObstacle.m

function flag = isObstacle( m,obstacle )
​
%判断节点m是否为障碍点，如果是就返为true，不是就返回false
for io=1:length(obstacle(:,1))
    if isequal(obstacle(io,:),m(1:2))
        flag=true;
        return;
    end
end
flag=false;
​
end
1
2
3
4
5
6
7
8
9
10
11
12

关注公众号回复【A*算法】可获取源代码