3 移动机器人路径规划（4- A*路径规划算法）_使用astat算法统计所有路径

4.1 Astat路径规划算法原理

A-Star算法是一种静态路网中求解最短路径最有效的直接搜索方法，也是许多其他问题的常用启发式算法。注意其是最有效的直接搜索算法，之后涌现了很多预处理算法（如ALT，CH，HL等等），在线查询效率是A*算法的数千甚至上万倍。

• 公式表示为： f(n)=g(n)+h(n),
• 其中， f(n) 是从初始状态经由状态n到目标状态的最小代价估计，
• g(n) 是在状态空间中从初始状态到状态n的最小代价，
• h(n) 是从状态n到目标状态的路径的最小估计代价。对于路径搜索问题，状态就是图中的节点，代价就是距离）

参考文档
Astat算法-百度百科
Astat中文博客
Astat最火英文博客

4.2 Astat路径规划例子示例

4.3 Astat路径规划算法MATLAB代码

4.3.1 MATLAB代码示例

地图信息和示例是一致的，最后得到的最短路径，和搜寻的地图区域如图所示：

4.3.2 主代码：Astat.m

%% 定义基础数据，包括地图行列的长度，起点位置等
clc;clear;close all
rows = 6;cols = 7;                              % 地图的尺寸
dy_SearchArea =[];                              % 动态绘制搜索区域
searchHead = 2;searchEnd = searchHead;          % 动态绘制搜索区域的速度
startSub = [3,2];                             % 起点行列位置
goalSub = [3,6];                              % 终点行列位置
obsSub = [2,4;3,4;4,4];                       % 障碍物行列位置(n*2)



%% 定义栅格地图，并初始化，此部分以后内容，均不用修改-------------------------------------------------
% 初始化地图数值为1，代表全空白区域
field = ones(rows, cols);

% 起点、终点、障碍物区域的权值
% Tips:对于单个点，Filed(x,y) = 2是可以的，但是多个点需要转成索引数组才能
field(startSub(1),startSub(2)) = 4;
field(goalSub(1),goalSub(2)) = 5;

% 后续路径规划算法，对于点的位置，均用索引值进行表示（输入变量只需要一个就行）
obsR = obsSub(:,1);obsC = obsSub(:,2);
obsIndex = sub2ind([rows,cols],obsR,obsC);
startIndex = sub2ind([rows,cols],startSub(1),startSub(2));
goalIndex = sub2ind([rows,cols],goalSub(1),goalSub(2));
field(obsIndex) = 2;


%% ******建立openList(n*4)和closeList(n*2)并初始化************************************
% 初始时，openList只有起点，closeList为空
% openList(n*4),分别记录记录点的位置信息，G、H、F值
openList = [startIndex,0,0,0];

% closeList(n*2)记录位置信息和距离权值F值，初始化时候为空
closeList = [];



%% *************初始化path，即从起点到地图任意点的路径矩阵(n*2)***************************
for i = 1:rows*cols
    path{i,1} = i;              % 存放地图任意点的索引
    path{i,2} = [];             % 存放起点到该点的路径
end
% 对于起点，其路径是已知的，写入起点路径
path{startIndex,2} = startIndex;



%% 绘制地图--------------------------------------------------------------------------
% 定义函数，列数，以及障碍物坐标
cmap = [1 1 1; ...       % 1-白色-空地
    0 0 0; ...           % 2-黑色-静态障碍
    1 0 0; ...           % 3-红色-动态障碍
    1 1 0;...            % 4-黄色-起始点 
    1 0 1;...            % 5-品红-目标点
    0 1 0; ...           % 6-绿色-到目标点的规划路径   
    0 1 1];              % 7-青色-动态规划的路径
colormap(cmap);
image(1.5,1.5,field);

% 设置栅格属性------------------------------------------------------------------------------
grid on;hold on;
set(gca,'gridline','-','gridcolor','k','linewidth',0.5,'GridAlpha',0.5);
set(gca,'xtick',1:cols+1,'ytick',1:rows+1);
set(gca, 'XAxisLocation','top')
axis image;



%% ******A*算法*************************************************************************
while true
    % 1、从openList开始搜索移动代价最小的节点，min函数返回值为[值，位置]
    [~,idxNode] = min(openList(:,4));
    node = openList(idxNode,1);

    % 2、判断是否搜索到终点
    if node == goalIndex
        break
    end
    
    % ******3、在openList选中最小的F值点作为父节点
    nextNodes = Astat_NextNode(field,closeList,node);

    % *******4、判断父节点周围子节点情况，并将子节点依次添加或者更新到openList中
    for i = 1:length(nextNodes)
        % 需要判断的子节点
        nextNode = nextNodes(i);
        
        % 计算代价函数
        [rowNode,colNode] = ind2sub([rows, cols], node);
        [row_nextNode,col_nextNode] = ind2sub([rows, cols], nextNode);
        [row_goalPos,col_goalPos] = ind2sub([rows, cols],goalIndex);
        g = openList(idxNode,2) + norm( [rowNode,colNode] -[row_nextNode,col_nextNode]);
        h = abs(row_goalPos - row_nextNode) + abs(col_goalPos - col_nextNode);
        f = g + h;
        
        % 判断该子节点是否存在在openList中
        [inOpen,idx_nextNode] = ismember(nextNode, openList(:,1));
        
        % ******如果存在，则需要比较F值，取F值小的更新F和G、H同时更新路径
        if inOpen && f < openList(idx_nextNode,4)
            openList(idx_nextNode,2) = g;
            openList(idx_nextNode,3) = h;
            openList(idx_nextNode,4) = f;
            path{nextNode,2} = [path{node,2}, nextNode];
        end
            
        % *******如果不存在，则添加到openList表中
        if ~inOpen
            openList(end+1,:) = [nextNode,g,h,f];
            path{nextNode,2} = [path{node,2}, nextNode];
        end
    end
    
    % 将父节点从openList中移除，添加到closeList中
    closeList(end+1,: ) = [openList(idxNode,1), openList(idxNode,4)];
    openList(idxNode,:)= [];
    
    % =======绘制动态搜索区域
    dy_SearchArea = [openList(:,1)',closeList(:,1)'];   % 添加节点到动态区域中
    field(dy_SearchArea) = 7;                           % 动态区域的颜色
    field(startIndex) = 4;                              % 起点颜色保持不变
    field(goalIndex) = 5;                               % 终点颜色保持不变
    if mod(searchHead,searchEnd) == 0                   % 控制动图绘制速度
        image(1.5,1.5,field);                           % 联合drawnow函数绘制动态效果
        drawnow;
    end
    searchHead = searchHead + 1;
end

%% 绘制最优路径的折线图*******************************************************************
% 由于绘图函数plot针对的窗口是x,y，它们与row，col是的先后顺序是相反的,即X对应col，y对应row
image(1.5,1.5,field);                                       % 将动画速度未显示完的搜索区域全部显示
field(dy_SearchArea) = 1;                                   % 最后将搜索区域的值改回为空白值
optPath = path{goalIndex,2};                                % 最优路径
[plotr,plotc] = ind2sub([rows,cols],optPath);
plot(plotc+0.5,plotr+0.5,'LineWidth',2.5);



% % function nextNodes = Astat_NextNode(field,closeList,node)
% % % ASTAT_NEXTNODE 对父节点周围的8个节点进行判断
% %     % 判断内容需要排除超过边界之外的、位于障碍区的、位于closeList中的三大类
% % 
% % [rows, cols] = size(field);                             % 获取地图尺寸
% % [r,c] = ind2sub([rows, cols], node);                    % 得到父节点行列值，方便进行上下左右移动
% % movePos = [-1,1;0,1;1,1;-1,0;1,0;-1,-1;0,-1;1,-1];      % 移动方向矩阵
% % nextNodes = [];                                         % 存放子节点线性索引位置的的矩阵(1*n)
% % 
% % % closeList内第一列存放点的索引值，单独拎出第一列来进行判断是否在closeList中
% % closenode = [];                                         % 初始化时候closeList为空，不能执行(:,1)操作，
% %                                                         % 故函数中的closenode作为临时变量初始为空
% % if ~isempty(closeList)                                  % if的目的就是为了保证初始的closeList=[]也有效
% %     closenode = closeList(:,1);
% % end
% % 
% % for i = 1:8
% %     if 0 < r+movePos(i,1) && r+movePos(i,1) <= rows && 0 < c+movePos(i,2) && c+movePos(i,2) <= cols
% %         nextSub = [r + movePos(i,1), c + movePos(i,2)];
% %         nextIndex = sub2ind([rows, cols], nextSub(1), nextSub(2));
% %         if field(nextSub(1), nextSub(2)) ~= 2
% %             if ~ismember(nextIndex, closenode)
% %                 nextNodes(end+1) = nextIndex;
% %             end
% %         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
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

4.3.3 函数代码：Astat_NextNode.m

function nextNodes = Astat_NextNode(field,closeList,node)
% ASTAT_NEXTNODE 对父节点周围的8个节点进行判断
    % 判断内容需要排除超过边界之外的、位于障碍区的、位于closeList中的三大类

[rows, cols] = size(field);                             % 获取地图尺寸
[r,c] = ind2sub([rows, cols], node);                    % 得到父节点行列值，方便进行上下左右移动
movePos = [-1,1;0,1;1,1;-1,0;1,0;-1,-1;0,-1;1,-1];      % 移动方向矩阵
nextNodes = [];                                         % 存放子节点线性索引位置的的矩阵(1*n)

% closeList内第一列存放点的索引值，单独拎出第一列来进行判断是否在closeList中
closenode = [];                                         % 初始化时候closeList为空，不能执行(:,1)操作，
                                                        % 故函数中的closenode作为临时变量初始为空
if ~isempty(closeList)                                  % if的目的就是为了保证初始的closeList=[]也有效
    closenode = closeList(:,1);
end

for i = 1:8
    if 0 < r+movePos(i,1) && r+movePos(i,1) <= rows && 0 < c+movePos(i,2) && c+movePos(i,2) <= cols
        nextSub = [r + movePos(i,1), c + movePos(i,2)];
        nextIndex = sub2ind([rows, cols], nextSub(1), nextSub(2));
        if field(nextSub(1), nextSub(2)) ~= 2
            if ~ismember(nextIndex, closenode)
                nextNodes(end+1) = nextIndex;
            end
        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

4.4 Astat路径规划算法Python代码

下面代码逻辑和MATLAB版本是一致的
本次A*在Python中采用存取行列位置的来依次搜索，和之前Dijkstra和Floyd采用线性索引不同
由于数组索引的一致性，在openList和closeList等均采用索引来修改对应值的方式
注意数组的深拷贝和浅拷贝
可以参考：PythonRobotics：（https://github.com/redglassli/PythonRobotics#a-algorithm）

4.4.1 Python实现示例

4.4.2 辅助函数PathPlanning.py

参考博客3 移动机器人路径规划（1- 栅格地图绘制）中的1.4.2复制命名即可。

4.4.3 Astar路径规划Python代码

import numpy as np
import matplotlib.pyplot as plt
import copy

import PathPlanning

'''
Astat_NextNode(field, closeList_Sub, nodeSub)：输入地图和closeList_Sub集合和父节点行列位置
# 返回子节点的行列位置矩阵nextNodes_Sub
'''
def Astat_NextNode(field, closeList_Sub, nodeSub):
    # 获取地图尺寸
    rows = len(field)
    cols = len(field[0])
    # 移动方向矩阵
    movePos = [[-1, 1], [0, 1], [1, 1], [-1, 0],
               [1, 0], [-1, -1], [0, -1], [1, -1]]
    # 存放子节点行列位置的矩阵
    nextNodes_Sub = []

    for i in range(8):
        r = nodeSub[0] + movePos[i][0]
        c = nodeSub[1] + movePos[i][1]

        # 在地图内
        if -1 < r and r < rows and -1 < c and c < cols:
            # 不为障碍物且不在closeList_Sub内则添加进子节点位置矩阵
            if field[r, c] != 2:
                if [r, c] not in closeList_Sub:
                    nextNodes_Sub.append([r, c])

    return nextNodes_Sub



'''
# 测试地图信息---------------------------------------------------------------
'''

rows = 5
cols = 6
startSub = [2,1]
goalSub = [2,5]
obsSub = [[1,3],[2,3],[3,3]]

# 栅格地图属性
field = np.ones((rows, cols))

field[startSub[0], startSub[1]] = 4
field[goalSub[0], goalSub[1]] = 5

for i in range(len(obsSub)):
    field[obsSub[i][0], obsSub[i][1]] = 2

'''
# 最关键一点：=========利用数组下标长度一致，顺序一致的关系，创建表分别存放值和点=====
# openList有四个存储内容,分别记录记录点的行列位置信息，G、H、F值
# 初始时，openList只有起点，closeList为空
'''
openList_Sub = [startSub]
openList_G = [0]
openList_H = [0]
openList_F = [0]

# closeList记录位置信息和距离权值F值，初始化时候为空
closeList_Sub = []
closeList_F = []

# 初始化path，即从起点到地图任意点的路径矩阵
pathSub = []
path = []

# 初始化所有存放路径的矩阵
for c in range(cols+1):
    for r in range(rows+1):
        pathSub.append([r,c])
        # 添加的元素是数组append([])
        path.append([[-1,-1]])

# 对于起点，其路径是已知的，写入起点路径
idx = pathSub.index(startSub)
path[idx] = [startSub]


'''
# ============================开始执行A*算法迭代========================
'''
while True:
    # 1、从openList开始搜索移动代价最小的节点，min函数返回值为[值，位置]
    idx_nodeSub = openList_F.index(min(openList_F))
    nodeSub = openList_Sub[idx_nodeSub]

    # 2、判断是否搜索到终点
    if nodeSub[0] == goalSub[0] and nodeSub[1] == goalSub[1]:
        break
    
    # ******3、在openList选中最小的F值点作为父节点
    nextNodes_Sub = Astat_NextNode(field, closeList_Sub, [nodeSub[0], nodeSub[1]])

    # *******4、判断父节点周围子节点情况，并将子节点依次添加或者更新到openList中
    for i in range(len(nextNodes_Sub)):

        # 需要判断的子节点
        nextSub = nextNodes_Sub[i]

        # 计算代价函数
        g = openList_G[idx_nodeSub] + np.sqrt((nodeSub[0]-nextSub[0])**2+(nodeSub[1]-nextSub[1])**2)
        h = np.abs(goalSub[0]-nextSub[0]) + np.abs(goalSub[1]-nextSub[1])
        f = g + h

        # 判断该子节点是否存在在openList中
        if nextSub in openList_Sub:
            # ******如果存在，则需要比较F值，取F值小的更新F和G、H同时更新路径
            idx_nextSub = openList_Sub.index(nextSub)
            if f < openList_F[idx_nextSub]:
                openList_G[idx_nextSub] = g
                openList_H[idx_nextSub] = h
                openList_F[idx_nextSub] = f

                idx_NextPath = pathSub.index(nextSub)
                idx_NodePath = pathSub.index(nodeSub)
                path[idx_NextPath] = copy.deepcopy(path[idx_NodePath])
                path[idx_NextPath].append(nextSub)

        # *******如果不存在，则添加到openList表中
        else:
            openList_Sub.append(nextSub)
            openList_G.append(g)
            openList_H.append(h)
            openList_F.append(f)

            idx_NextPath = pathSub.index(nextSub)
            idx_NodePath = pathSub.index(nodeSub)
            path[idx_NextPath] = copy.deepcopy(path[idx_NodePath])
            path[idx_NextPath].append(nextSub)
    
    # 将父节点从openList中移除，添加到closeList中
    closeList_Sub.append(nodeSub)
    closeList_F.append(f)
    openList_Sub.pop(idx_nodeSub)
    openList_G.pop(idx_nodeSub)
    openList_H.pop(idx_nodeSub)
    openList_F.pop(idx_nodeSub)


idx = pathSub.index(goalSub)
optpath = path[idx]

for i in range(len(optpath)):
    field[optpath[i][0],optpath[i][1]] = 6


field[startSub[0], startSub[1]] = 4
field[goalSub[0], goalSub[1]] = 5

PathPlanning.DrawHeatMap(field)
plt.show()
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

4.5 对角线路径过滤

4.5.1 优化对角线的例子

4.5.2 优化方法

主要是在对A*算法父节点周围子节点进行过滤，所以只需要修改nextnode函数即可

4.5.3 MATLAB子节点过滤函数Astat_NextNode.m

将其替换上面的Astat_NextNode.m代码运行即可

function nextNodes = Astat_NextNode(field,closeList,node)
% ASTAT_NEXTNODE 对父节点周围的8个节点进行判断
    % 判断内容需要排除超过边界之外的、位于障碍区的、位于closeList中的三大类

[rows, cols] = size(field);                             % 获取地图尺寸
[r,c] = ind2sub([rows, cols], node);                    % 得到父节点行列值，方便进行上下左右移动
movePos = [-1,1;0,1;1,1;-1,0;1,0;-1,-1;0,-1;1,-1];      % 移动方向矩阵
nextNodes = [];                                         % 存放子节点线性索引位置的的矩阵(1*n)
nextObs = [];

% closeList内第一列存放点的索引值，单独拎出第一列来进行判断是否在closeList中
closenode = [];                                         % 初始化时候closeList为空，不能执行(:,1)操作，
                                                        % 故函数中的closenode作为临时变量初始为空
if ~isempty(closeList)                                  % if的目的就是为了保证初始的closeList=[]也有效
    closenode = closeList(:,1);
end

for i = 1:8
    if 0 < r+movePos(i,1) && r+movePos(i,1) <= rows && 0 < c+movePos(i,2) && c+movePos(i,2) <= cols
        nextSub = [r + movePos(i,1), c + movePos(i,2)];
        nextIndex = sub2ind([rows, cols], nextSub(1), nextSub(2));
        % 如果为障碍物，且为父节点上下左右四个方向的障碍物，则记录到障碍物表中
        if field(nextSub(1), nextSub(2)) == 2
            if movePos(i,1)==0 || movePos(i,2)==0
                nextObs(end+1) = nextIndex;
            end
        % 不为障碍物且不在closeList_Sub内则添加进子节点位置矩阵
        else
            if ~ismember(nextIndex, closenode)
                nextNodes(end+1) = nextIndex;
            end
        end
    end
    
    % 保证父节点上下左右障碍物不为空才能进行for循环
    if ~isempty(nextObs)
        for i = 1:length(nextObs)
            % 保证子节点集合不为空才能进行for循环
            if ~isempty(nextNodes)
                for j=1:length(nextNodes)
                    [ro,co] = ind2sub([rows,cols],nextObs(i));
                    [rn,cn] = ind2sub([rows,cols],nextNodes(j));
                    % 判断距离，如果为1则删除
                    d = norm([ro,co]-[rn,cn]);
                    if d == 1
                        nextNodes(j) = [];      % 删除元素
                        break;
                    end
                end
            end
        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
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55

4.5.3 Python子节点过滤函数Astat_NextNode

类似，将Python的def Astat_NextNode(field, closeList_Sub, nodeSub):替换下面代码即可

'''
Astat_NextNode(field, closeList_Sub, nodeSub)：输入地图和closeList_Sub集合和父节点行列位置
# 返回子节点的行列位置矩阵nextNodes_Sub
'''
def Astat_NextNode(field, closeList_Sub, nodeSub):
    # 获取地图尺寸
    rows = len(field)
    cols = len(field[0])
    # 移动方向矩阵
    movePos = [[-1, 1], [0, 1], [1, 1], [-1, 0],
               [1, 0], [-1, -1], [0, -1], [1, -1]]
    # 存放子节点行列位置的矩阵
    nextNodes_Sub = []
    nextObs_Sub = []

    for i in range(8):
        r = nodeSub[0] + movePos[i][0]
        c = nodeSub[1] + movePos[i][1]

        # 在地图内
        if -1 < r and r < rows and -1 < c and c < cols:
            # 如果为障碍物，且为父节点上下左右四个方向的障碍物，则记录到障碍物表中
            if field[r, c] == 2:
                if movePos[i][0] == 0 or movePos[i][1] == 0:
                    nextObs_Sub.append([r,c])
            # 不为障碍物且不在closeList_Sub内则添加进子节点位置矩阵
            else:
                if [r, c] not in closeList_Sub:
                    nextNodes_Sub.append([r, c])

    # 如果子节点到父节点上下左右障碍物的距离为1，则不添加进子节点列表
    for i in range(len(nextObs_Sub)):
        for j in range(len(nextNodes_Sub)):
            if np.sqrt((nextNodes_Sub[j][0]-nextObs_Sub[i][0])**2 + (nextNodes_Sub[j][1]-nextObs_Sub[i][1])**2) == 1:
                nextNodes_Sub.pop(j)
                break

    return nextNodes_Sub
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