Peter Hart
Nils Nilsson
Bertram Raphael
参考：
C#，人工智能（AI）机器人路径规划（Path Planning）的ARA*（Anytime Replanning A* Algorithm）算法与源程序
https://blog.csdn.net/beijinghorn/article/details/125464754
一、A*算法概述
A*算法最初由斯坦福研究院（Stanford Institute）的 Peter Hart，Nils Nilsson，Bertram Raphael 发表于1968年，属于Dijkstra算法的拓展之一。
论文原文https://www.cs.auckland.ac.nz/courses/compsci709s2c/resources/Mike.d/astarNilsson.pdf
A*算法是常用的路径规划、最短路径搜索、图遍历算法。
借助於启发函数，A*同时拥有较好的性能与准确度，因而备受人工智能、机器人研究者们的欢迎。
除了 A* ，现在发展了 D*，D*Lite。。。诸多改进的算法，后续都会给出 C# 源代码。
A* 的算法不复杂，属于入门级别的。
二、计算结果的动画演示
三、A* 源代码
1、节点信息 NodeInfo
#define ANIMATE
using System;
using System.IO;
using System.Text;
using System.Linq;
using System.Collections.Generic;
using System.Text.RegularExpressions;
namespace Legalsoft.Truffer.Algorithm
{
/// <summary>
/// 节点信息
/// </summary>
public class NodeInfo
{
/// <summary>
/// 前一节点
/// </summary>
public NodeInfo Last { get; set; } = null;
/// <summary>
/// 邻接距离(权值)
/// </summary>
public int ManhattanWeight { get; set; } = 0;
/// <summary>
/// 曼哈顿距离(权值)
/// </summary>
public int RemoteWeight { get; set; } = 0;
/// <summary>
/// 综合距离(权值)
/// </summary>
public int TotalWeight { get; set; } = 0;
/// <summary>
/// 行号
/// </summary>
public int Column { get; set; } = 0;
/// <summary>
/// 列号
/// </summary>
public int Row { get; set; } = 0;
/// <summary>
/// 是否为墙(节点)？
/// </summary>
public bool IsWall { get; set; } = false;
/// <summary>
/// 是否被加入到了close中
/// </summary>
public bool InClose { get; set; } = false;
/// <summary>
/// 开口节点？
/// </summary>
public bool InOpen { get; set; } = false;
/// <summary>
/// 路过？
/// </summary>
public bool Visited { get; set; } = false;
/// <summary>
/// 构造函数
/// </summary>
/// <param name="y">行号</param>
/// <param name="x">列号</param>
/// <param name="iswall"></param>
public NodeInfo(int y, int x, bool iswall)
{
Row = y;
Column = x;
IsWall = iswall;
}
}
}
2、地图信息MapInfo
#define ANIMATE
using System;
using System.IO;
using System.Text;
using System.Linq;
using System.Collections.Generic;
using System.Text.RegularExpressions;
namespace Legalsoft.Truffer.Algorithm
{
/// <summary>
/// 地图信息（墙1与空0）
/// </summary>
public class MapInfo
{
/// <summary>
/// 行数
/// </summary>
public int Row { get; set; } = 0;
/// <summary>
/// 列数
/// </summary>
public int Column { get; set; } = 0;
/// <summary>
/// 所有节点
/// </summary>
public NodeInfo[,] Nodes { get; set; } = null;
/// <summary>
/// 从文本创建地图
/// </summary>
/// <param name="buf"></param>
public void FromBuffer(string buf)
{
buf = buf.Replace("\r", "").Trim();
string[] xlines = buf.Split(new char[] { '\n' }, StringSplitOptions.RemoveEmptyEntries);
Nodes = new NodeInfo[xlines.Length, xlines[0].Trim().Length];
Row = Nodes.GetLength(0);
Column = Nodes.GetLength(1);
int y = 0;
foreach (string xu in xlines)
{
for (int x = 0; x < xu.Trim().Length; x++)
{
NodeInfo node = new NodeInfo(y, x, Int32.Parse(xu.Trim().Substring(x, 1)) > 0);
Nodes[y, x] = node;
}
y++;
}
}
/// <summary>
/// 从文件创建地图
/// </summary>
/// <param name="filename"></param>
/// <exception cref="Exception"></exception>
public void FromFile(string filename)
{
try
{
string buf = File.ReadAllText(filename);
FromBuffer(buf);
}
catch (Exception ex)
{
throw new Exception("");
}
}
/// <summary>
/// this重载
/// </summary>
/// <param name="y"></param>
/// <param name="x"></param>
/// <returns></returns>
public NodeInfo this[int y, int x]
{
set { Nodes[y, x] = value; }
get { return Nodes[y, x]; }
}
/// <summary>
/// 第一个节点
/// </summary>
public NodeInfo First
{
get { return Nodes[0, 0]; }
}
/// <summary>
/// 最后一个节点
/// </summary>
public NodeInfo Last
{
get { return Nodes[Row - 1, Column - 1]; }
}
/// <summary>
/// 清除路径
/// </summary>
public void ClearPath()
{
for (int i = 0; i < Row; i++)
{
for (int j = 0; j < Column; j++)
{
this[i, j].Visited = false;
}
}
}
}
}
3、核心代码 AStar Kernal
#define ANIMATE
using System;
using System.IO;
using System.Text;
using System.Linq;
using System.Collections.Generic;
using System.Text.RegularExpressions;
namespace Legalsoft.Truffer.Algorithm
{
public class AStar
{
/// <summary>
/// 开放列表
/// </summary>
private List<NodeInfo> openList { get; set; } = new List<NodeInfo>();
/// <summary>
/// 当前访问的节点
/// </summary>
private NodeInfo curNode { get; set; } = null;
/// <summary>
/// 起始节点
/// </summary>
private NodeInfo startNode { get; set; } = null;
/// <summary>
/// 终止节点
/// </summary>
private NodeInfo endNode { get; set; } = null;
/// <summary>
/// 地图
/// </summary>
public MapInfo map { get; set; } = new MapInfo();
/// <summary>
/// 构造函数
/// </summary>
public AStar()
{
}
/// <summary>
/// 节点加入列表，并计算相关权值
/// </summary>
/// <param name="node"></param>
private void AddToList(NodeInfo node)
{
if (node.IsWall) return;
if (node.InClose) return;
if (node.InOpen) return;
openList.Add(node);
node.InOpen = true;
node.ManhattanWeight = curNode.ManhattanWeight + WeightOf(node, curNode);
node.RemoteWeight = WeightOf(node, endNode);
node.TotalWeight = node.ManhattanWeight + node.RemoteWeight;
node.Last = curNode;
}
/// <summary>
/// 路径规划A*算法(最短路径A*算法)
/// 默认左上角(0,0)为起点；右下角为终点；
/// </summary>
/// <param name="start">起点</param>
/// <param name="end">终点</param>
public void Tracing(NodeInfo start = null, NodeInfo end = null)
{
List<int[]> steps = new List<int[]>() {
new int[2] { 1, 1 },
new int[2] { 0, 1 },
new int[2] { 1, 0 },
new int[2] { 0,-1 },
new int[2] { -1, 0 },
new int[2] { 1,-1 },
new int[2] { -1, 1 },
new int[2] { -1,-1 },
};
startNode = (start == null) ? map.First : start;
endNode = (end == null) ? map.Last : end;
// 将起点加入到开放列表中
openList.Add(startNode);
while (true)
{
openList.Sort(SortByCost);
curNode = openList[0];
openList.Remove(curNode);
curNode.InOpen = false;
curNode.InClose = true;
// 终点?
if (curNode == endNode)
{
return;
}
MarkPath();
map.ClearPath();
}
}
/// <summary>
/// 节点1到节点2的Weight
/// </summary>
/// <param name="a">节点1</param>
/// <param name="b">节点2</param>
/// <returns></returns>
private int WeightOf(NodeInfo a, NodeInfo b)
{
// 直行步长
double straightDistance = 1.0;
// 斜行步长
double diagonalDistance = 1.414;
int deltaX = Math.Abs(a.Column - b.Column);
int deltaY = Math.Abs(a.Row - b.Row);
if (deltaX == deltaY)
{
return (int)(deltaX * diagonalDistance);
}
else if (deltaX < deltaY)
{
return (int)(deltaX * diagonalDistance + (deltaY - deltaX) * straightDistance);
}
else
{
return (int)(deltaY * diagonalDistance + (deltaX - deltaY) * straightDistance);
}
}
/// <summary>
/// 排序的委托函数
/// </summary>
/// <param name="a">节点1</param>
/// <param name="b">节点2</param>
/// <returns></returns>
private int SortByCost(NodeInfo a, NodeInfo b)
{
if (a.TotalWeight.CompareTo(b.TotalWeight) != 0)
{
return (a.TotalWeight.CompareTo(b.TotalWeight));
}
else if (a.RemoteWeight.CompareTo(b.RemoteWeight) != 0)
{
return (a.RemoteWeight.CompareTo(b.RemoteWeight));
}
else
{
return 1;
}
}
/// <summary>
/// 标记当前获取的路径
/// </summary>
public void MarkPath()
{
NodeInfo cd = curNode;
while (cd != null)
{
if (cd == startNode)
{
break;
}
cd.Visited = true;
cd = cd.Last;
}
}
}
}
--------------------------------------------------------------------------------
POWER BY TRUFFER.CN
