Using A* Algorithm to Realize Automatic Pathfinding
A few days ago, a brother of mine called me and asked me if I would use artificial intelligence to realize the function of a robot automatically finding its way in the warehouse. Because I have never been in contact with such a scene, but I am more interested in it, so I read some automatic pathfinding algorithms, such as: depth-first traversal based on binary tree, D Star, A Star algorithm, among which I think A Star algorithm is the most good. Let me introduce to you that the first implementation language is Java, but Java is not very intuitive, and I don’t want to use Java’s graphical interface, so I use JS+HTML to realize it. First, let’s show the renderings.
The renderings are as follows:
1. What is A Start Algorithm
The A* search algorithm is an algorithm that finds the lowest calculation cost from the starting point to the end point in a two-dimensional plane. It can calculate the shortest distance from point A to point B , which is the optimal path. The common application is mainly in the game, the automatic pathfinding of the characters; the pathfinding of the robot; the navigation of the traffic route, etc.
2. The principle and process of A Star algorithm
2.1 Prerequisites
Before describing the A Star algorithm, the following attributes need to be declared:
(1) The node that spreads from the starting point;
(2) The shortest distance calculation formula: F = G + H;
(3) The Euclidean distance calculation formula: 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 (actually the Pythagorean theorem);
(4)OPENLIST and CLOSELIST;
It doesn't matter if you don't understand the above attributes and formulas, I will introduce them in detail below. very simple!
2.1.1 Nodes spreading from the starting point
We use horizontal and vertical grids on HTML pages. The so-called diffusion is based on the starting pointsuperior、Down、left、rightDiffusion is carried out in four directions, and these expanded nodes are the "roads" that can be taken. As shown belowyellowThe squares of are the points of diffusion:
PS: A Star hasfour directionsandeight directionsdiffusion. Expanding the nodes in four directions is what we are talking about at present; the eight directions also include nodes in the four directions of up-left, up-right, down-left, and down-right. We use four directions of expansion throughout.
2.1.2 Shortest distance calculation formula: F = G + H
How to find the optimal among the diffuse nodes that isat the earliestWhat about a road? You need to use this formula:F=G+H. So what kind of consciousness do the attributes in this formula represent? Let's explain it below:
(1) G: Indicates the distance
from the starting point to the four spreading nodes, in other words, the grid that needs to be moved from the starting point to the four spreading nodes. The value of G can be usedEuclidean distance calculation formulaCalculation.
As shown below:
(2) H:
Indicates the grid that needs to be moved from the starting point to the end point (note: ignore obstacles, you can pass through obstacles), this distance needs to be calculated by the Euclidean distance calculation formula formula. Of course, you don't have to use the Euclidean distance calculation formula. You can simply add the difference between the x coordinates and the y coordinates from the start point to the end point .
As shown in the figure below:
(3) F:
F = G + H, the node with the smallest value of F among the diffusion nodes is the node we need to go. That is the shortest path.
2.1.3 Euclidean distance calculation formula
This formula is used to calculateHandGof. formula: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. In fact, it is the x coordinate of the end point minus the square of the x coordinate of the starting point + the y coordinate of the end point minus the square root of the y coordinate of the starting point. Isn’t this the Pythagorean theorem?
2.1.4 OPENLIST and CLOSELIST
OPENLIST and CLOSELIST represent two "containers" ("containers" are two collections in the code, and List collection or array declaration can be used). The contents and functions of these two "containers" are as follows:
(1) OPENLIST
is used to store diffused nodes. The nodes that spread from the starting point to the four directions at the beginning need to be placed in the OPENLIST set (if the diffused nodes areobstacleor inAlready exists in CLOSELISTis not inserted). OPENLIST is the collection that is mainly traversed, and the nodes that calculate the F value are all from this collection.
(2) CLOSELIST
is used to store the starting point , obstacle nodes , and passed points . When spreading nodes, you need to check in the CLOSELIST set. If the diffused node D is already in the CLOSELIST set (judged according to the coordinates), or if the node D is an obstacle, then skip this node . The passed points also need to be put in the CLOSELIST.
The points passed are shown in the figure below:
2.2 Process
2.2.1 Step 1: Diffusion
From the starting point up, down, left, and rightdiffusionfour nodes. If the coordinates of the starting point are (x:3, y:2), then the four nodes are: up (x:2, y:2), down (x:4, y:2), left (x:3, y :1), Right(x:3,y:3).
2.2.2 Step 2: Check Nodes
Traverse the CLOSELIST collection, to determine whether the four diffused nodes exist in CLOSELIST or OPENLIST, if not, put them in OPENLIST, otherwise skip the node. If the node isobstacleAlso needs to be skipped.
2.2.3 The third step: calculate the value of F
Traverse the OPENLIST collection, and calculate the F value of the nodes in the set, find out the node with the smallest F value (the closest distance) minNode, this node is the node to go. Then put other diffuse nodes except mindNode into CLOSELIST.
2.2.4 Step 4: Change the position of the starting point
Through the third step, we find a node minNode with the smallest F value, then set the starting point equal to minNode. Then continue to diffuse and repeat the above four steps until theDiffuse nodes contain endpointsThe nodes we have traveled are the shortest paths.
3. A Star algorithm code implementation
I will not release the Java code, if you want, you can leave a message in the comment area. The following is usedJSWritten, I also said mine in other articlesJSIt's just half-baked (but there are too many comments). Please point out the bad writing, and I will correct it immediately!
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. Conclusion
These are the algorithms for the four directions of A Star diffusion. If you have time, you can summarize the eight directions of diffusion. In writing this article, I want to provide you with the process and implementation ideas of the A Star algorithm, but if you want to really use it, you need to consider a lot of things, and you have to fit your own scene. There is still a problem with the code above. I hope you can point it out and I will correct it immediately. Anything else you don't understand can be discussed in the comment area.
The road is long and long, I will search up and down