python自动寻路算法_自动移动寻路算法A*算法

问题描述：

如图所示为环境地图，使用A*算法寻找一条完整路径要求从左上角开始到右下角

1、环境地图建模：将环境用 16×15的矩阵表示，其中0表示障碍物、1表示可移动位置

111111111111111

111111111111111

000000011111111

000000011111111

000000011000000

000000011000000

000000011000000

000000011111111

111111111111111

111111000000000

111111000000000

111111000000000

111111000000000

111111111111111

111111111111111

111111111111111

2、工具类：用于查找的方格类

private static class Grid {

int G,H;

int I,J;

Grid parent = null;//指向上一个节点

public Grid(int i,int j) {

this.I = i;

this.J = j;

}

void update(Grid min_grid){

this.G = min_grid.G + 1;

this.H = (rol_num - I) + (col_num - J) - 2;

this.parent = min_grid;

}

}

3、编写算法：

(1). 将起点加入 open list 。

(2).

while( 终点加入到了 close list 中 OR open list 是空 ){

遍历open list 找到 F值最小的节点，作为为当节点

open list 移出当前节点

close list 加入当前节点

for( 对当前方格的 8 个相邻方格逐一进行检查 ){

if( 如果它是有障碍的方格 OR存在于close list中 ){

忽略这一节点

}

else{

if(方格 在open list中){

检查这条路径 ( 即经由当前方格到达它那里 ) 是否更近。

如果更近，把它的父亲设置为当前方格，并重新计算更新它的G和F值。

}

else{

把该方格加入open list，并且把当前方格设置为它的父亲

}

}

}

}

(3).从终点开始，每个方格沿着父节点移动直至起点，形成路径。

寻路结果：

+11111111111111

1++++++11111111

0000000+1111111

00000001+111111

00000001+000000

00000001+000000

00000001+000000

0000000+1111111

111111+11111111

11111+000000000

11111+000000000

11111+000000000

11111+000000000

111111+11111111

1111111+1111111

11111111+++++++

原理 + 代码详见：(不知道为啥一放代码秒被删)

新浪微博 WealdPoem