使⽤AStar算法实现⾃动寻路详解
@
⽬录
前些⽇⼦我有兄弟给我打电话，问我会不会⼈⼯智能，来实现⼀个机器⼈在仓库⾃动寻路的功能。因为没有接触过这样的场景，但是⾃⼰⼜⽐较对此兴趣，所以就看了⼀些⾃动寻路的算法，⽐如：基于⼆叉树的深度优先遍历、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 = $\sqrt (x_2 - x_1)^2+(y_2 - y_1)^2$（其实就是勾股定理）；
（4）OPENLIST 和 CLOSELIST；
上⾯的属性和公式不懂没关系，下⾯我会对他们⼀⼀进⾏详细介绍。⾮常简单！
2.1.1 从起点开始扩散的节点
我们在HTML页⾯上使⽤横和竖画出来的格⼦。所谓扩散就是以起点为基点向上、下、左、右四个放向进⾏扩散，这些扩展的节点就是可以⾛的“路”。如下图所⽰黄⾊的⽅格就是扩散的点：
PS:A Star有四个⽅向和⼋个⽅向的扩散。扩展四个⽅向的节点就是⽬前我们所说的；⼋个⽅向是还包含了，上左、上右、下左、下右四个⽅向的节点。我们通篇使⽤的是四个⽅向的扩展。
2.1.2 最短距离计算公式：F = G + H
如何在扩散的节点中到最优也就是最短的⼀条路呢？就需要⽤到这个公式：F=G+H。那么这个公式⾥⾯的属性都代表什么意识呢？下⾯我们就说明⼀下：
（1）G:
表⽰从起点到扩散的四个节点的距离，换句话说就是从起点到扩散的四个节点需要移动的格⼦。G的值可以使⽤欧⼏⾥得距离计算公式进⾏计算。
如下图：
表⽰从起点开始，到终点需要移动的格⼦（注意：忽略障碍物，可以从障碍物中穿过去），这个距离需要通过欧⼏⾥得距离计算公式公式算出来。当然你没必要⼀定要使⽤欧⼏⾥得距离计算公式，你还可以单纯的将起点到终点的x报坐标差值和y坐标差值进⾏相加即可。
如下图：
（3）F:
F =
G + H，在扩散节点中F的值最⼩的就是我们需要⾛的节点。也就是最短的路径。
2.1.3 欧⼏⾥得距离计算公式
这个公式是⽤来计算H和G的。公式：p = $\sqrt (x_2 - x_1)^2+(y_2 - y_1)^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%;
}
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>
init();
drawMAP();
}
//地图div
nodeselectorvar 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 = ateElement('div');
div.className = 'square'
var p = ateElement('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;
//绘制终点
var idEnd = endNode.x +'-'+endNode.y;
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;
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;
}
}
pNode == '' ||pNode == null){
}
//如果扩散的这些节点⼀个也没有存到openList中，那么说明进⼊了死胡同        openList.push(node);
changeColor(node.x,node.y,'k');
}
/
/改变颜⾊
function changeColor(x,y,desc) {
var id = x+'-'+y;
if(desc == 'k'){
}
if(desc == 'r'){
}
}
//计算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
// 所以需要判断
unt==undefined){
}
console.log(unt);
//将当前节点设置为F最⼩的节点
currentNode = minF;
unt!=undefined&&unt>1){
//说明进⼊了死胡同
//1.将此节点放到closeList中
//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];
}
}
unt==undefined){
}
minF = minFSecond;
currentNode = minFSecond;
console.log(currentNode);
}
//并将当前节点的颜⾊变为红⾊
var id= currentNode.x +'-'+currentNode.y;
}
//移除节点
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);

版权声明：本站内容均来自互联网，仅供演示用，请勿用于商业和其他非法用途。如果侵犯了您的权益请与我们联系QQ:729038198，我们将在24小时内删除。