使用A*算法实现自动寻路
前些日子我有兄弟给我打电话，问我会不会人工智能，来实现一个机器人在仓库自动寻路的功能。因为没有接触过这样的场景，但是自己又比较对此兴趣，所以就看了一些自动寻路的算法，比如：基于二叉树的深度优先遍历、D Star、A Star算法，其中我感觉A Star算法最好。下面我给大家介绍一下，首次实现的语言是Java，但是Java不太直观，又不想使用Java的图形界面，所以就使用JS+HTML来实现的，首先展示一下效果图。
效果图如下：
1、什么是A Start算法
A*搜索算法是求出在一个二维平面中从起点到终点最低运算代价的算法，它可以算出A点到B点的最短距离，也就是最优路径。常见的应有主要是在游戏中，人物的自动寻路；机器人探路；交通路线导航等。
2、A Star算法的原理和流程
2.1 前提
在讲述A Star算法之前，需要声明下列这些属性：
（1）从起点开始扩散的节点；
（2）最短距离计算公式：F = G + H；
（3）欧几里得距离计算公式：p = ( x 2 − x 1 ) 2 + ( y 2 − y 1 ) 2 \sqrt (x_2 - x_1)^2+(y_2 - y_1)^2 (x2−x1)2+(y2−y1)2（其实就是勾股定理）；
（4）OPENLIST 和 CLOSELIST；
上面的属性和公式不懂没关系，下面我会对他们一一进行详细介绍。非常简单！
2.1.1 从起点开始扩散的节点
我们在HTML页面上使用横和 竖画出来的格子。所谓扩散就是以 起点 为基点向上、下、左、右四个放向进行扩散，这些扩展的节点就是可以走的“路”。如下图所示黄色的方格就是扩散的点：
PS:A Star有四个方向和八个方向的扩散。扩展四个方向的节点就是目前我们所说的；八个方向是还包含了，上左、上右、下左、下右四个方向的节点。我们通篇使用的是四个方向的扩展。
2.1.2 最短距离计算公式：F = G + H
如何在扩散的节点中找到最优也就是最短的一条路呢？就需要用到这个公式：F=G+H。那么这个公式里面的属性都代表什么意识呢？下面我们就说明一下：
（1）G:
表示从起点到扩散的四个节点的距离，换句话说就是从起点到扩散的四个节点需要移动的格子。G的值可以使用欧几里得距离计算公式进行计算。
如下图：
（2）H:
表示从起点开始，到终点需要移动的格子（注意：忽略障碍物，可以从障碍物中穿过去），这个距离需要通过 欧几里得距离计算公式 公式算出来。当然你没必要一定要使用欧几里得距离计算公式，你还可以单纯的将起点到终点的x报坐标差值和y坐标差值进行相加即可。
如下图：
（3）F:
F = G + H，在扩散节点中F的值最小的就是我们需要走的节点。也就是最短的路径。
2.1.3 欧几里得距离计算公式
这个公式是用来计算H和G的。公式：p = ( x 2 − x 1 ) 2 + ( y 2 − y 1 ) 2 \sqrt (x_2 - x_1)^2+(y_2 - y_1)^2 (x2−x1)2+(y2−y1)2 。其实就是终点的x坐标减去起点的x坐标的平方 + 终点的y坐标减去起点的y坐标的平方 开根号，这不就是勾股定理嘛。
2.1.4 OPENLIST 和 CLOSELIST
OPENLIST和CLOSELIST代表两个“容器”（“容器”在代码中就是两个集合，使用List集合或者数组声明都可以）。这个两个“容器”存放的内容和作用如下：
（1）OPENLIST
用于存储扩散的节点。刚开始由起点开始向四个方向扩散的节点就需要放到OPENLIST集合中（如果扩散的节点是障碍物或者是在CLOSELIST中已经存在则不放入）。OPENLIST是主要遍历的集合，计算F值的节点都是来自这个集合。
（2）CLOSELIST
用于存储起点、障碍物节点、走过的点。在扩散节点的时候，需要到CLOSELIST集合中去检查，如果扩散的节点D已经在CLOSELIST集合中了（根据坐标进行判断），或者是D节点是障碍物那么就跳过此节点。走过的点也需要放到CLOSELIST中去。
走过的点如下图所示：
2.2 流程
2.2.1 第一步：扩散
从起点开始向上下左右扩散四个节点。假如起点的坐标为（x:3,y:2），那么四个节点为：上（x:2,y:2）、下（x:4,y:2）、左（x:3,y:1）、右（x:3,y:3）。
2.2.2 第二步：检查节点
遍历CLOSELIST集合，判断扩散的这四个节点是否存在于CLOSELIST或者OPENLIST中，如果不存在放到OPENLIST中，反之跳过该节点。如果该节点是障碍物也需要跳过。
2.2.3 第三步：计算F的值
遍历OPENLIST集合，并计算集合中节点的F的值，找出F值为最小的节点（距离最近）minNode，这个节点就是要走的节点。然后把除了mindNode之外的其它扩散的节点放入到CLOSELIST中。
2.2.4 第四步：改变起点的位置
通过第三步我们找到了F值最小的一个节点minNode，那么就把起点等于minNode。然后继续进行扩散重复上面的四个步骤，直至在扩散的节点中包含终点我们走过的节点就是最短路径。
3.A Star算法代码实现
Java代码我就不放出来了，如果想要的可以评论区留言。下面是用JS写的，本人在其他文章里面也说过我的JS就是二半吊子（但是注释写的多）。写的不好的地方大家指出，我会即时更正！
HTML:
<!DOCTYPE html>
<html lang="en">
<head>
<meta charset="UTF-8">
<title>寻路02</title>
<style>
*{
margin: 0;
padding: 0;
}
#con{
width: 100%;
height: 100%;
}
body{
font-size: 0px;
}
#map{
width: 800px;
height: 800px;
/*border: 1px gray solid;*/
margin-top: 20px;
border-radius: 5px;
/*background-image: url("store/grass.png");*/
}
.square {
width: 40px;
height: 40px;
border: 1px gray solid;
/*background-image: url("store/tree01.png");*/
display: inline-block;
box-sizing:border-box;
color: red;
}
.roadblock{
width: 1px;
height: 1px;
border: 1px black solid;
background-color: black;
}
.p{
color: red;
margin: 0px;
padding: 0px;
display: inline-block;
}
</style>
</head>
<body>
<div id="con">
<button onclick="drawOther();">随机生成障碍物</button>
<button onclick="startFindWay()">开始寻路</button>
<button onclick="stop()">停止</button>
<div id="map">
</div>
</div>
</body>
</html>
JS：
<script>
window.onload=function () {
init();
drawMAP();
}
//地图div
var bg = document.querySelector('#map');
//开放集合
var openList=[];
//闭合集合
var closeList=[];
//起点
var startNode={
};
//终点
var endNode = {
};
//在由当前节点扩散的节点中 F值最小的一个节点
// count是一个计数器，用来判断是否进入了死胡同
var minF = {
topNode:'', F:'' ,G:'',H:'',x:'',y:''};
//当期节点
var currentNode = {
};
//绘制地图
function drawMAP() {
for(var i = 0 ; i < 20; i++){
for(var j = 0; j < 20;j ++ ){
var div = document.createElement('div');
div.className = 'square'
var p = document.createElement('p');
p.className = 'p';
p.innerHTML='('+i+','+j+')';
div.append(p)
div.id = i+'-'+j;
bg.append(div);
}
}
}
//初始化
function init() {
//添加起点和终点
startNode.x=1;
startNode.y=2;
startNode.des='start';
endNode.x =15;
endNode.y = 8;
endNode.des='end';
//添加障碍物
openList.push(startNode);
//将当前节点设置为startNode
currentNode = startNode;
}
//绘制障碍物、起点、终点
function drawOther() {
//绘制起点
var idStart = startNode.x+'-'+startNode.y;
document.getElementById(idStart).style.backgroundColor='red';
//绘制终点
var idEnd = endNode.x +'-'+endNode.y;
document.getElementById(idEnd).style.backgroundColor='blue';
randCreatBlock();
}
//随机生成障碍物
function randCreatBlock() {
for (let i = 0; i < 100; i++) {
var x = Math.floor(Math.random()*(20));
var y = Math.floor(Math.random()*(20));
if ( x == startNode.x && y == startNode.y) {
return ;}
if(x == endNode.x && y == endNode.y){
return ;}
var point = x+'-'+y;
document.getElementById(point).style.backgroundColor = 'black';
var node = {
x:x,y:y};
closeList.push(node);
}
}
//寻路
function findWay() {
//扩散上下左右四个节点
var up ={
topNode:'', F:'',G:'',H:'',x:currentNode.x-1,y:currentNode.y};
var down ={
topNode:'', F:'',G:'',H:'',x:currentNode.x+1,y:currentNode.y};
var left ={
topNode:'', F:'',G:'',H:'',x:currentNode.x,y:currentNode.y-1};
var right ={
topNode:'', F:'',G:'',H:'',x:currentNode.x,y:currentNode.y+1};
//检查这些扩散的节点是否合法,如果合法放到openlist中
checkNode(up);
checkNode(down);
checkNode(left);
checkNode(right);
//移除已扩散完毕的节点移除，并放到closeList中去
removeNode();
//计算F
computersF();
}
function checkNode(node) {
//校验扩散的点是否超过了地图边界
if(node.x<0||node.y<0){
return ;
}
//如果node存在closelist中则忽略
for (let i = 0; i < closeList.length; i++) {
if (closeList[i].x == node.x && closeList[i].y == node.y) {
return;
}
}
for (let i = 0; i <openList.length; i++) {
if(openList[i].x==node.x&&openList[i].y==node.y){
return;
}
}
if(node.topNode == '' ||node.topNode == null){
node.topNode = currentNode;
}
//如果扩散的这些节点 一个也没有存到openList中，那么说明进入了死胡同
openList.push(node);
changeColor(node.x,node.y,'k');
}
//改变颜色
function changeColor(x,y,desc) {
var id = x+'-'+y;
if(desc == 'k'){
document.getElementById(id).style.backgroundColor = 'yellow';
}
if(desc == 'r'){
document.getElementById(id).style.backgroundColor = 'pink';
}
}
//计算FGH
function computersF() {
var x = endNode.x;
var y = endNode.y;
for (let i = 0; i < openList.length; i++) {
//计算H
var hx = parseInt(x) - parseInt(openList[i].x);
if(hx<0){
hx = -(parseInt(x) - parseInt(openList[i].x));
}
var hy = parseInt(y) - parseInt(openList[i].y);
if(hy<0){
hy = -(parseInt(y) - parseInt(openList[i].y));
}
var H = hx+hy;
openList[i].H= H;
//计算G
var G = Math.sqrt(Math.floor(Math.pow(parseInt(currentNode.x) - parseInt(openList[i].x),2))+
Math.floor(Math.pow(parseInt(currentNode.y) - parseInt(openList[i].y),2)));
openList[i].G= G;
//计算F
var F = G + H;
openList[i].F = F;
if(minF.F==''){
minF = openList[i];
}else {
if(minF.F>F){
minF = openList[i];
}
}
}
//201和204行代码把openList赋值给了minF,openList并没有定义count，count为undefined
// 所以需要判断
if(minF.count==undefined){
minF.count = 0;
}
minF.count++;
console.log(this.minF.count);
//将当前节点设置为F最小的节点
currentNode = minF;
if(minF.count!=undefined&&minF.count>1){
//说明进入了死胡同
//1.将此节点放到closeList中
this.removeNode();
//2.在openList中去寻找 仅次于 此节点（进入死胡同）的其它节点
var minFSecond = openList[0];
for (let i = 0; i < openList.length; i++) {
console.log(openList[i])
if(minFSecond.F>=openList[i].F){
minFSecond = openList[i];
}
}
if(minFSecond.count==undefined){
minFSecond.count = 0;
}
minF = minFSecond;
currentNode = minFSecond;
console.log(currentNode);
}
//并将当前节点的颜色变为红色
var id= currentNode.x +'-'+currentNode.y;
document.getElementById(id).style.backgroundColor='red';
}
//移除节点
function removeNode() {
var index = openList.indexOf(currentNode);
openList.splice(index,1);
closeList.push(currentNode);
//并将当前节点的颜色改变
changeColor(currentNode.x,currentNode.y,'r');
}
var myStart;
// 启动
function startFindWay(){
myStart = setInterval(function startFindWay() {
findWay();
if(minF.x===endNode.x&&minF.y===endNode.y){
clearInterval(myStart);
return;
}
},100);
}
//停止
function stop(){
clearInterval(myStart);
}
</script>
4. 结语
A Star扩散四个方向的算法就这些。如果有时间可以把扩散八个方向的总结一下。写这篇文章我想向大家提供的是A Star算法的过程与实现的思路，但是如果想要真正运用还需要考虑很多东西，要贴合自己的场景。上面的代码还有问题。希望大家指出来，我会即时更正。还有什么不懂的可以在评论区讨论。
路漫漫其修远兮，吾将上下而求索