A星寻路算法最简单理解
Posted zhangxiaofan666
tags:
篇首语：本文由小常识网(cha138.com)小编为大家整理，主要介绍了A星寻路算法最简单理解相关的知识，希望对你有一定的参考价值。
对于a星寻路算法最直白的理解：
从a点走到b点，首先把地图画成网格，让障碍物在网格内
如图，从s点要走到e点，把障碍物设成黑色，还要创建2个队列，一个是寻找新的节点队列（开启队列），一个是保存已走过的节点队列（关闭队列）。在寻找新的节点时，要判断该节点距离，公式为总距离=当前节点距离起点步数+当前节点距离终点步数。这里注意的是即使有障碍物在计算当前节点距离起终点步数也要按照没有障碍物来算。
拿该图来说，从起点s走到终点e（假设可以斜着走且斜走距离为1）。图上所有open，close的标识是走完后的，所以先不看，从s点出发，把s点加入开启队列，进入第一次循环，条件是如果开启队列不为空，找到开启队列中总距离最小点（只有一个点s），则找到s点旁边可走的点（相邻s点且不在open队列，不在close队列），可以找到3,5,8,9,10点，将这些点移入open队列，计算每点总距离，将s点移除open队列，s点移入close队列，进入第二次循环，当open队列不为空，找到open队列总距离最小点，可以看到总距离最小点有3个，8,9,10并且都是4（4=1+3这个1指的是该点距离起点距离3点是该点距离终点距离），假设第一个点是8，那么找到8点的旁边的可走点，由于5，9点在开启队列，4点在关闭队列，所以找到6,7点移入开启队列并计算总距离，把8移入关闭队列
此时，开启队列3,10,9,5,6,7 关闭队列4,8
进入第三次循环，开启队列不为空时，找到开启队列中总距离最小的点，这里有两个9,10总距离为4，假设是9，找邻点，所有邻点要么在开启队列要么在关闭队列，所以9移入关闭队列，
进入第四次循环，找到10点，找邻点2,11,14，此时开启队列2,3,5,6,7,11,14 关闭队列4,8,9,10
第五次循环，找到5点，移入关闭，开启队列2,3,6,7,11,14 关闭队列4,5,8,9,10
第六次开启队列2,6,7,11,14 关闭队列3,4,5,8,9,10
第七次开启队列2,6,11,14 关闭队列3,4,5,7,8,9,10
第八次开启队列2,11,14 关闭队列3,4,5,6,7,8,9,10
不说啦，可能有点错误，你们明白意思就行
最后如何找到最佳路径呢，每次在开启队列找到的点选取邻点时候，都要将邻点的父节点选为该点，图中用箭头表示，这样当第n次循环，当从开启队列找到的第一个点为终点时，回溯父节点就找到了最短路径。
贴出来一张c#代码供参考：
using UnityEngine;
using System.Collections;
public class AStar
public static PriorityQueue closedList, openList; //开启关闭队列
private static float NodeCost(Node a, Node b) //已知a，b点，计算两点距离
Vector3 vecCost = a.position - b.position;
return vecCost.magnitude;
public static ArrayList FindPath(Node start, Node goal) //a星寻路算法
openList = new PriorityQueue();
openList.Push(start); //首先将起点压入队列，计算该点距起点距离和该点距终点距离
start.nodeTotalCost = 0.0f;
start.estimatedCost = NodeCost(start, goal);
closedList = new PriorityQueue();
Node node = null;
while (openList.Length != 0) //当开启队列不为空，进入正式循环
node = openList.First(); //找到开启队列中总距离最小的点
if (node.position == goal.position) //如果开启队列中总距离最小的点是终点就退出循环
return CalculatePath(node);
ArrayList neighbours = new ArrayList();
GridManager.instance.GetNeighbours(node, neighbours); //寻找该点的所有邻点用neighbours保存，这个函数是调用另一个脚本
for (int i = 0; i < neighbours.Count; i++)
Node neighbourNode = (Node)neighbours[i];
if(!closedList.Contains(neighbourNode)) //如果该邻点不在关闭队列中就计算该点总距离
float cost = NodeCost(node, neighbourNode);
float totalCost = node.nodeTotalCost + cost;
float neighbourNodeEstCost = NodeCost(neighbourNode, goal);
neighbourNode.nodeTotalCost = totalCost;
neighbourNode.estimatedCost = totalCost + neighbourNodeEstCost;
neighbourNode.parent = node;
if (!openList.Contains(neighbourNode)) //邻点不在开启队列就压入开启队列
openList.Push(neighbourNode);
closedList.Push(node); //将该点从开启队列移除，移入关闭队列
openList.Remove(node);
if (node.position != goal.position) //如果开启队列全部点都遍历了没找到目标点，报错
Debug.LogError("Goal Not Found");
return null;
return CalculatePath(node); //该方法返回最佳路径
private static ArrayList CalculatePath(Node node) //找到从终点的所有父节点也即终点到起点路径，将list倒过来
ArrayList list = new ArrayList();
while (node != null)
list.Add(node);
node = node.parent;
list.Reverse();
return list;
避障算法：
代码中将障碍物的layer层射到第8层为Obstacles层
参考代码
public class AvoidingObstacles : MonoBehaviour
public float speed = 20.0f;
public float mass = 5.0f;
public float force = 50.0f;
public float minimunDistToAvoid = 20.0f;
private float curSpeed;
private Vector3 targetPoint;
// Use this for initialization
void Start ()
targetPoint = Vector3.zero;
void OnGUI()
GUILayout.Label("Click anywhere to move the vehicle.");
// Update is called once per frame
void Update ()
RaycastHit hit;
//找到目标点
var ray = Camera.main.ScreenPointToRay(Input.mousePosition);
if (Input.GetMouseButton(0) && Physics.Raycast(ray, out hit, 100.0f)) //发射100单位长度射线
targetPoint = hit.point;
//找到坦克的目标移动方向，并得到方向向量
Vector3 dir = targetPoint - transform.position;
dir.Normalize();
AvoidObstacles(ref dir); //执行避障算法
if (Vector3.Distance(targetPoint, transform.position) < 3.0f)
return;
curSpeed = speed * Time.deltaTime;
var rot = Quaternion.LookRotation(dir);
//坦克一直朝着目标方向旋转
transform.rotation = Quaternion.Slerp(transform.rotation, rot, 5.0f * Time.deltaTime);
transform.position += transform.forward * curSpeed;
private void AvoidObstacles(ref Vector3 dir) //在避障算法中传入移动方向并修改移动方向
RaycastHit hit;
// 0000 0000 0000 0000 0000 0001 0000 0000
// 1
int layerMask = 1 << 8; //因为把障碍物层设成第8层所以这样
//从坦克点的前方发射一条长度为minimunDistToAvoid=20单位长度的射线，与layer为第8层的物体相交
if (Physics.Raycast(transform.position, transform.forward,
out hit, minimunDistToAvoid, layerMask))
Vector3 hitNormal = hit.normal;
hitNormal.y = 0.0f;
dir = transform.forward + hitNormal * force;
Flocking算法：
用于群体物体追踪一个领头物体，比如gta5警车追人，红警派出一群小兵去哪里，如图绿色的物体一直在追踪红色物体移动，把红色物体的移动代码写好，在hierarchy中把绿色物体拖动到红色物体下面，给绿色物体添加flock代码即可
红色物体代码：比较简单就不注释了：
public class FlockControl : MonoBehaviour
public float speed = 100.0f;
public Vector3 bound;
private Vector3 initialPosition;
private Vector3 nextMovementPoint;
// Use this for initialization
void Start ()
initialPosition = transform.position;
CalculateNextMovementPoint();
private void CalculateNextMovementPoint()
float posX = Random.Range(-bound.x, bound.x);
float posY = Random.Range(-bound.y, bound.y);
float posZ = Random.Range(-bound.z, bound.z);
nextMovementPoint = initialPosition + new Vector3(posX, posY, posZ);
// Update is called once per frame
void Update ()
transform.Translate(Vector3.forward * speed * Time.deltaTime);
transform.rotation = Quaternion.Slerp(transform.rotation, Quaternion.LookRotation(nextMovementPoint - transform.position), 2.0f * Time.deltaTime);
if (Vector3.Distance(nextMovementPoint,transform.position) <= 20.0f)
CalculateNextMovementPoint();
public class Flock : MonoBehaviour
public float minSpeed = 100.0f; //movement speed of the flock
public float turnSpeed = 20.0f; //rotation speed of the flock
public float randomFreq = 20.0f;
public float randomForce = 20.0f; //Force strength in the unit sphere
public float toOriginForce = 20.0f;
public float toOriginRange = 100.0f;
public float gravity = 2.0f; //Gravity of the flock
public float avoidanceRadius = 400.0f; //Minimum distance between flocks
public float avoidanceForce = 20.0f;
public float followVelocity = 4.0f;
public float followRadius = 40.0f; //Minimum Follow distance to the leader
private Transform origin; //Parent transform
private Vector3 velocity; //Velocity of the flock
private Vector3 normalizedVelocity;
private Vector3 randomPush; //Random push value
private Vector3 originPush;
private Transform[] objects; //Flock objects in the group
private Flock[] otherFlocks; //Unity Flocks in the group
private Transform transformComponent; //My transform
void Start ()
randomFreq = 1.0f / randomFreq;
//Assign the parent as origin
origin = transform.parent;
//Flock transform
transformComponent = transform;
//Temporary components
Component[] tempFlocks= null;
//Get all the unity flock components from the parent transform in the group
if (transform.parent)
tempFlocks = transform.parent.GetComponentsInChildren<Flock>();
//Assign and store all the flock objects in this group
objects = new Transform[tempFlocks.Length];
otherFlocks = new Flock[tempFlocks.Length];
for(int i = 0;i<tempFlocks.Length;i++)
objects[i] = tempFlocks[i].transform;
otherFlocks[i] = (Flock)tempFlocks[i];
//Null Parent as the flock leader will be UnityFlockController object
transform.parent = null;
//Calculate random push depends on the random frequency provided
StartCoroutine(UpdateRandom());
IEnumerator UpdateRandom ()
while(true)
randomPush = Random.insideUnitSphere * randomForce;
yield return new WaitForSeconds(randomFreq + Random.Range(-randomFreq / 2.0f, randomFreq / 2.0f));
void Update ()
//Internal variables
float speed= velocity.magnitude;
Vector3 avgVelocity = Vector3.zero;
Vector3 avgPosition = Vector3.zero;
float count = 0;
float f = 0.0f;
float d = 0.0f;
Vector3 myPosition = transformComponent.position;
Vector3 forceV;
Vector3 toAvg;
Vector3 wantedVel;
for(int i = 0;i<objects.Length;i++)
Transform transform= objects[i];
if (transform != transformComponent)
Vector3 otherPosition = transform.position;
// Average position to calculate cohesion
avgPosition += otherPosition;
count++;
//Directional vector from other flock to this flock
forceV = myPosition - otherPosition;
//Magnitude of that directional vector(Length)
d= forceV.magnitude;
//Add push value if the magnitude is less than follow radius to the leader
if (d < followRadius)
//calculate the velocity based on the avoidance distance between flocks
//if the current magnitude is less than the specified avoidance radius
if(d < avoidanceRadius)
f = 1.0f - (d / avoidanceRadius);
if(d > 0)
avgVelocity += (forceV / d) * f * avoidanceForce;
//just keep the current distance with the leader
f = d / followRadius;
Flock tempOtherFlock = otherFlocks[i];
avgVelocity += tempOtherFlock.normalizedVelocity * f * followVelocity;
if(count > 0)
//Calculate the average flock velocity(Alignment)
avgVelocity /= count;
//Calculate Center value of the flock(Cohesion)
toAvg = (avgPosition / count) - myPosition;
else
toAvg = Vector3.zero;
//Directional Vector to the leader
forceV = origin.position - myPosition;
d = forceV.magnitude;
f = d / toOriginRange;
//Calculate the velocity of the flock to the leader
if(d > 0)
originPush = (forceV / d) * f * toOriginForce;
if(speed < minSpeed && speed > 0)
velocity = (velocity / speed) * minSpeed;
wantedVel = velocity;
//Calculate final velocity
wantedVel -= wantedVel * Time.deltaTime;
wantedVel += randomPush * Time.deltaTime;
wantedVel += originPush * Time.deltaTime;
wantedVel += avgVelocity * Time.deltaTime;
wantedVel += toAvg.normalized * gravity * Time.deltaTime;
//Final Velocity to rotate the flock into
velocity = Vector3.RotateTowards(velocity, wantedVel, turnSpeed * Time.deltaTime, 100.00f);
transformComponent.rotation = Quaternion.LookRotation(velocity);
//Move the flock based on the calculated velocity
transformComponent.Translate(velocity * Time.deltaTime, Space.World);
//normalise the velocity
normalizedVelocity = velocity.normalized;
以上是关于A星寻路算法最简单理解的主要内容，如果未能解决你的问题，请参考以下文章