java实现路径规划（A*算法）_java a*算法

一、适用的场景

在一张二维坐标系地图中存在有障碍物的情况下，须要绕开障碍，获取从起点坐标到终点坐标的最优路径。

二、算法思路

1.使用回溯法得到路径
采用“节点与节点的父节点的关系”的关系从最终节点回溯到起点，得到路径。
2.路径代价的估算：F=G+H
A*算法的代价计算使用了被称作启发式的代价函数。
G： G表示的是从起点到当前节点的实际路径代价（就是已经走过了，边走边将实际路径代价计算好）；
H： H表示当前节点到达最终节点的估计代价（就是还没走过，在不知道是否存在障碍的情况下的估计值）；
F： F表示当前节点所在路径从起点到最终点预估总路径代价；
**G的计算方式：**计算方式有多种，这里我们采用的，假设每个节点代表一个正方形，横竖移动距离：斜向移动距离 = 1：1.4，我们取整数10和14，也就是说当前节点G值=父节点G+（10或14）；
**H的计算方式：**估计代价的计算方式也存在多种，这里采用“曼哈顿”法，H=|当前节点X值 - 最终节点X值| + |当前节点Y值 - 最终节点Y值| 。（| | 代表是取绝对值）；
F的计算方式： F = G + H
3、辅助表：Open、Close列表
在A星算法中，需要使用两个辅助表来记录节点。
一个用于记录可被访问的节点，称为Open表；
一个用于记录已经访问过的节点，称为Close表；
这来给你个表决定了算法的结束：条件是最终点在Close表中（找到路径），或者Open表为空（未找到路径）；
4、移动节点、相邻节点的处理
现在就开始移动当前节点，规划路径。
每次从Open表中取出F值（从起点到最终点预估总路径代价）最小的节点出来(这里使用有限队列来处理比较合适)，作为当前节点；然后将当前节点的所有相邻节点按照邻节点规则加入到Open表中；最后将当前节点放入到Close表中，这里就是每次循环的执行内容了。
邻节点规则：

1. 当前邻节点不在地图中，不加入Open表；
2. 当前邻节点是障碍物，不加入Open表；
3. 当前邻节点在Close表中，不加入Open表；
4. 当前邻节点不在Open中，加入Open表，设该邻节点的父节点为当前节点；
5. 当前邻节点在Open表中，我们需要做个比较，如果邻节点的G值（从起点到当前节点的实际路径代价）大于当前节点的G值+当前节点到这个节点的实际路径代价，那么修改该邻节点的父节点为当前的节点（因为在Open表中的节点除了起点，都会有父节点），修改G值 = 当前节点的G值+当前节点到这个节点的实际路径代价；
   蓝色框框表示在Close表（已访问过）中，绿色框框表示在Open表（未访问过）中

最后回溯得到路径

三、java代码实现

1.输入
（1）代表地图二值二维数组（0表示可通行，1表示障碍物）

int[][] maps = {
                { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },
                { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },
                { 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 0 },
                { 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 1, 0, 0, 0, 0 },
                { 0, 0, 0, 1, 0, 0, 0, 0, 1, 1, 0, 0, 0, 0, 0 },
                { 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0 },
                { 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0 }
                };

1
2
3
4
5
6
7
8
9
10

（2）按照二维数组的特点，坐标的原点在在左上角，所以Y是高，X是宽，Y向下递增，X向右递增。我们将X和Y封装未一个类，方便传参，重写equals方法比较坐标是不是同一个

public class Coord
{
    public int x;
    public int y;

    public Coord(int x, int y)
    {
        this.x = x;
        this.y = y;
    }

    @Override
    public boolean equals(Object obj)
    {
        if (obj == null) return false;
        if (obj instanceof Coord)
        {
            Coord c = (Coord) obj;
            return x == c.x && y == c.y;
        }
        return false;
    }
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24

（3）封装路径节点类，字段要包含：坐标、G值、H值、父节点，实现Comparable接口方便有限队列排序。

public class Node implements Comparable<Node>
{

    public Coord coord; // 坐标
    public Node parent; // 父结点
    public int G; // G：是个准确的值，是起点到当前结点的代价
    public int H; // H：是个估值，当前结点到目的结点的估计代价

    public Node(int x, int y)
    {
        this.coord = new Coord(x, y);
    }

    public Node(Coord coord, Node parent, int g, int h)
    {
        this.coord = coord;
        this.parent = parent;
        G = g;
        H = h;
    }

    @Override
    public int compareTo(Node o)
    {
        if (o == null) return -1;
        if (G + H > o.G + o.H)
            return 1;
        else if (G + H < o.G + o.H) return -1;
        return 0;
    }
}

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

（4）最后一个数据结构是A*算法输入的所有数据，封装在一起，便于传参数。

public class MapInfo
{
    public int[][] maps; // 二维数组的地图
    public int width; // 地图的宽
    public int hight; // 地图的高
    public Node start; // 起始结点
    public Node end; // 最终结点

    public MapInfo(int[][] maps, int width, int hight, Node start, Node end)
    {
        this.maps = maps;
        this.width = width;
        this.hight = hight;
        this.start = start;
        this.end = end;
    }
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18

2、处理
（1）在算法里面需要定义几个常量来确定：二维数组中那个值表示障碍、二维数组中绘制路径的代表值、计算G值需要横纵及斜向的移动代价。

    public final static int BAR = 1; // 障碍值
    public final static int PATH = 2; // 路径
    public final static int DIRECT_VALUE = 10; // 横竖移动代价
    public final static int OBLIQUE_VALUE = 14; // 斜移动代价

1
2
3
4
5

（2）定义两个辅助表：Open和Close表。Open表的使用是需要取最小值，这里我们使用Java工具包中的有限队列PriorityQueue,Close只用来保存节点，没有特殊用途，就用ArrayList。

    Queue<Node> openList = new PriorityQueue<Node>(); // 优先队列(升序)
    List<Node> closeList = new ArrayList<Node>();

1
2
3

（3）定义几个buer判断方法：最终点的判断、节点是否能加入Open表的判断、节点是否在Close表中的判断。

    /**
     * 判断结点是否是最终结点
     */
    private boolean isEndNode(Coord end,Coord coord)
    {
        return coord != null && end.equals(coord);
    }

    /**
     * 判断结点能否放入Open列表
     */
    private boolean canAddNodeToOpen(MapInfo mapInfo,int x, int y)
    {
        // 是否在地图中
        if (x < 0 || x >= mapInfo.width || y < 0 || y >= mapInfo.hight) return false;
        // 判断是否是不可通过的结点
        if (mapInfo.maps[y][x] == BAR) return false;
        // 判断结点是否存在close表
        if (isCoordInClose(x, y)) return false;

        return true;
    }

    /**
     * 判断坐标是否在close表中
     */
    private boolean isCoordInClose(Coord coord)
    {
        return coord!=null&&isCoordInClose(coord.x, coord.y);
    }

    /**
     * 判断坐标是否在close表中
     */
    private boolean isCoordInClose(int x, int y)
    {
        if (closeList.isEmpty()) return false;
        for (Node node : closeList)
        {
            if (node.coord.x == x && node.coord.y == y)
            {
                return true;
            }
        }
        return false;
    }

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

（4）计算H值，“曼哈顿”法，坐标分别取绝对差值相加

private int calcH(Coord end,Coord coord)
{
    return Math.abs(end.x - coord.x) + Math.abs(end.y - coord.y);
}

1
2
3
4
5

（5）从Open列表中查找节点

private Node findNodeInOpen(Coord coord)
{
    if (coord == null || openList.isEmpty()) return null;
    for (Node node : openList)
    {
        if (node.coord.equals(coord))
        {
            return node;
        }
    }
    return null;
}

1
2
3
4
5
6
7
8
9
10
11
12
13

（6）添加邻节点到Open表

/**
 * 添加所有邻结点到open表
 */
private void addNeighborNodeInOpen(MapInfo mapInfo,Node current)
{
    int x = current.coord.x;
    int y = current.coord.y;
    // 左
    addNeighborNodeInOpen(mapInfo,current, x - 1, y, DIRECT_VALUE);
    // 上
    addNeighborNodeInOpen(mapInfo,current, x, y - 1, DIRECT_VALUE);
    // 右
    addNeighborNodeInOpen(mapInfo,current, x + 1, y, DIRECT_VALUE);
    // 下
    addNeighborNodeInOpen(mapInfo,current, x, y + 1, DIRECT_VALUE);
    // 左上
    addNeighborNodeInOpen(mapInfo,current, x - 1, y - 1, OBLIQUE_VALUE);
    // 右上
    addNeighborNodeInOpen(mapInfo,current, x + 1, y - 1, OBLIQUE_VALUE);
    // 右下
    addNeighborNodeInOpen(mapInfo,current, x + 1, y + 1, OBLIQUE_VALUE);
    // 左下
    addNeighborNodeInOpen(mapInfo,current, x - 1, y + 1, OBLIQUE_VALUE);
}

/**
 * 添加一个邻结点到open表
 */
private void addNeighborNodeInOpen(MapInfo mapInfo,Node current, int x, int y, int value)
{
    if (canAddNodeToOpen(mapInfo,x, y))
    {
        Node end=mapInfo.end;
        Coord coord = new Coord(x, y);
        int G = current.G + value; // 计算邻结点的G值
        Node child = findNodeInOpen(coord);
        if (child == null)
        {
            int H=calcH(end.coord,coord); // 计算H值
            if(isEndNode(end.coord,coord))
            {
                child=end;
                child.parent=current;
                child.G=G;
                child.H=H;
            }
            else
            {
                child = new Node(coord, current, G, H);
            }
            openList.add(child);
        }
        else if (child.G > G)
        {
            child.G = G;
            child.parent = current;
            // 重新调整堆
            openList.add(child);
        }
    }
}

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

（7）回溯法绘制路径

private void drawPath(int[][] maps, Node end)
{
    if(end==null||maps==null) return;
    System.out.println("总代价：" + end.G);
    while (end != null)
    {
        Coord c = end.coord;
        maps[c.y][c.x] = PATH;
        end = end.parent;
    }
}

1
2
3
4
5
6
7
8
9
10
11
12

（8）开始算法，循环移动节点寻找路径，设定循环结束条件，Open表为空或者最终节点在Close表

public void start(MapInfo mapInfo)
{
    if(mapInfo==null) return;
    // clean
    openList.clear();
    closeList.clear();
    // 开始搜索
    openList.add(mapInfo.start);
    moveNodes(mapInfo);
}

/**
 * 移动当前结点
 */
private void moveNodes(MapInfo mapInfo)
{
    while (!openList.isEmpty())
    {
        Node current = openList.poll();
        closeList.add(current);
        addNeighborNodeInOpen(mapInfo,current);
        if (isCoordInClose(mapInfo.end.coord)) // bug修正
        {
            drawPath(mapInfo.maps, mapInfo.end);
            break;
        }
    }
}

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