参考:
C#,人工智能(AI)机器人路径规划(Path Planning)A*(AStar Algorithm)算法、源代码及计算数据可视化https://blog.csdn.net/beijinghorn/article/details/124119531
Maxim Likhachev CMU
Dave Ferguson CMU
Geoff Gordon CMU
Anthony Stentz CMU
sebastian thrun of Stanford
参考论文:
ARA*: Anytime A* with Provable Bounds on Sub-Optimalityhttps://papers.nips.cc/paper/2003/file/ee8fe9093fbbb687bef15a38facc44d2-Paper.pdfAnytime Dynamic A*: An Anytime, Replanning Algorithm
https://www.cs.cmu.edu/~ggordon/likhachev-etal.anytime-dstar.pdf
我们提出了一种基于图的规划和重新规划算法,该算法能够产生有界次优解以随时可用的方式。我们的算法调整了质量基于可用搜索时间的解决方案重复使用以前的搜索工作。收到有关基础图表的更新信息时,该算法以增量方式修复其以前的解决方案。结果是一种结合了效益的方法-适合随时提供ef的增量计划员-有效解决复杂的动态搜索问题。
我们对该算法进行了理论分析,在模拟机器人运动臂上的实验结果,以及动态路径规划中的两个最新应用户外移动机器人。
规划在现实世界中运行的系统包括应对许多simpler领域未面临的挑战。首先,现实世界本质上是一个不确定和动态的地方;规划的准确模型包括很难获得,很快就会过时。其次在现实世界中操作时,考虑的时间是通常非常有限;代理人需要做出决策并采取行动迅速做出这些决定。幸运的是,许多研究人员已经在这些挑战。应对不完善的信息,以及动态环境,高效的重新规划算法根据最新信息纠正以前的解决方案(Stentz 1994;1995;Koenig&Likhachev
2002b;2002a;Ramalingam&Reps 1996;巴托、布拉特克、,&辛格,1995年)。这些算法为生成这些解决方案都是从头开始的。
然而,当规划问题复杂时可能无法在代理人可用的审议时间。Anytime algo rithms(Zilberstein&Russell 1995;Dean&Boddy 1988;周汉森2002;Likhachev、Gordon和Thrun 2003)已经证明自己在这种情况下特别合适设置,因为它们通常会很快提供一个初始的、可能是高度次优的解决方案,然后集中精力改进此解决方案,直到有时间进行规划用完了。
到目前为止,这两个研究领域之间的互动相对较少。重新规划算法专注于找到一个固定次优界和anytime算法集中于静态环境。但至少对我们来说,最令人感兴趣的问题是那些动态(需要重新规划)和复杂(需要任何时间方法)的问题。例如,我们当前的工作重点是动态、相对高维状态下的路径规划空间,例如移动机器人在部分已知室外导航时考虑速度的轨迹规划环境。
在本文中,我们提出了一种基于启发式的随时重新规划算法,该算法弥补了这两者之间的差距研究领域。我们的算法,Anytime Dynamic A*(AD*),在考虑的同时不断改进其解决方案时间允许,并在收到更新的信息时纠正其解决方案。一个简单的机器人应用实例图1显示了八个连接网格中的导航。本文的组织结构如下。我们从讨论当前的增量重新规划算法,尤其是D*和D*Lite(Stentz 1995;Koenig&Likhachev 2002a)。接下来,我们介绍现有的任意时间算法,包括最近的ARA*算法(Likhachev、Gordon和Thrun 2003)。那么我们介绍我们的新算法Anytime Dynamic A*,以及在动态路径中提供一个示例真实应用程序户外移动机器人规划。我们展示了通过实验结果和以讨论和扩展结束。
由于 A*算法对于规划的时间没有任何"反应",国内外学者已经提出多种不同形式的 Anytime 算法,它们都有各自的优缺点∶Ziberstein和 Russell 提出的ARUAA算法是可以可行的算法,然后它不能评价每个次优路径相比最优路径的次优率;Zhou.R提出的 MSAUA*算法能够给出这个次优率,然后它对以前信息的重用率很低,以至于要浪费很多计算;然而,Likhachey.M等人提出的 ARA*是目前对 A*在对时间"反应"的最好改进。ARA*算法是一种启发式增量搜索算法。启发式搜索是指使用启发式函数来控制搜索的扩展范围以求最优路径的搜索方法。因为启发式搜索能够将搜索空间控制在一个比较小的控制范围内,即搜索面积更小,所以具有较快的速度。增量式搜索是指在相似的环境中进行一系列搜索时,通过重用技术来更快地得到最优路径的搜索方法。因为每次增量搜索能够判断节点信息是否改变,并且只去修改已经改变的节点信息,所以增量搜索比每次从零开始搜索更快。当膨胀因子e减小时,ARA*从新排列不均衡列表中的节点的次序,然后只对不均衡列表中的节点进行操作,而不再处理其他占大多数的均衡的节点。
using System;
using System.IO;
using System.Text;
using System.Collections;
using System.Collections.Generic;
namespace Legalsoft.Truffer
{
public abstract class MapInfo
{
public int rows { get; set; }
public int columns { get; set; }
public Points start_pos { get; set; } = new Points();
public Points goal_pos { get; set; } = new Points();
public List obstacle_list { get; set; } = new List();
public MapInfo()
{
}
public void LoadMap(string filename)
{
try
{
string buf = File.ReadAllText(filename);
buf = buf.Replace("\t", " ").Replace("\r", "");
string[] xlines = buf.Split(new char[] { '\n' }, StringSplitOptions.RemoveEmptyEntries);
int begin = 0;
if (xlines[0].Split(new char[] { ' ' }).Length < 2)
{
begin = 2;
}
rows = 0;
for (int i = begin; i < xlines.Length; i++)
{
string[] ca = xlines[i].Trim().Split(new char[] { ' ' }, StringSplitOptions.RemoveEmptyEntries);
if (ca.Length < 1) break;
if (columns == 0)
{
columns = ca.Length;
}
else
{
if (columns != ca.Length)
{
throw new Exception("ERROR columns number at line:" + i);
}
}
for (int j = 0; j < columns; j++)
{
if (ca[j].ToLower() == "s")
{
start_pos.y = i - begin;
start_pos.x = j;
}
else if (ca[j].ToLower() == "g")
{
goal_pos.y = i - begin;
goal_pos.x = j;
}
else if (ca[j].ToLower() == "x")
{
obstacle_list.Add(new Points(i - begin, j));
}
else
{
// _
}
}
rows++;
}
}
catch (Exception ex)
{
throw new Exception("Load map ERROR:" + ex.Message);
}
}
}
}
using System;
using System.Collections.Generic;
namespace Legalsoft.Truffer
{
public class Points
{
public int x { get; set; } = 0;
public int y { get; set; } = 0;
public Points() { }
public Points(Points b)
{
this.x = b.x;
this.y = b.y;
}
public Points(int y, int x)
{
this.x = x;
this.y = y;
}
public static bool operator ==(Points a, Points b)
{
return (a.x == b.x) && (a.y == b.y);
}
public static bool operator !=(Points a, Points b)
{
return (a.x != b.x) || (a.y != b.y);
}
public override bool Equals(object obj)
{
return (Points)obj == this;
}
public override int GetHashCode()
{
return base.GetHashCode();
}
public override string ToString()
{
return base.ToString();
}
}
}
using System;
using System.Collections.Generic;
namespace Legalsoft.Truffer
{
public static class Vector
{
public static T back(List list)
{
return list[list.Count - 1];
}
public static void pop_back(List list)
{
list.RemoveAt(list.Count - 1);
}
public static void push_back(List list, T v)
{
list.Add(v);
}
public static T begin(List list)
{
return list[0];
}
public static int size(List list)
{
return list.Count;
}
public static bool empty(List list)
{
return (list.Count == 0);
}
}
}
using System;
using System.Collections.Generic;
namespace Legalsoft.Truffer
{
public class Cell
{
public Points xoy { get; set; } = new Points();
public double f_value { get; set; }
public double h_value { get; set; }
public double g_value { get; set; }
public double v_value { get; set; }
public Cell(Points xoy, double f_value, double h_value, double g_value, double v_value)
{
this.xoy = new Points(xoy);
this.f_value = f_value;
this.h_value = h_value;
this.g_value = g_value;
this.v_value = v_value;
}
public Cell(Cell b)
{
this.xoy = new Points(b.xoy);
this.f_value = b.f_value;
this.h_value = b.h_value;
this.g_value = b.g_value;
this.v_value = b.v_value;
}
public static bool operator <(Cell a, Cell b)
{
if (Math.Abs(a.f_value - b.f_value) < float.Epsilon)
{
return (a.g_value < b.g_value);
}
else
{
return (a.f_value < b.f_value);
}
}
public static bool operator >(Cell a, Cell b)
{
if (Math.Abs(a.f_value - b.f_value) < float.Epsilon)
{
return (a.g_value > b.g_value);
}
else
{
return (a.f_value > b.f_value);
}
}
}
}
#define __VECTOR__
using System;
using System.Text;
using System.Collections;
using System.Collections.Generic;
namespace Legalsoft.Truffer
{
public class ARAstar : MapInfo
{
private Points current_start { get; set; } = new Points();
private List current_path { get; set; } = new List();
private List current_open_list { get; set; } = new List();
private List current_obstacle_list { get; set; } = new List();
private Hashtable current_save_path_hash { get; set; } = new Hashtable();
private Hashtable current_observed_cell_info_list { get; set; } = new Hashtable();
private double heuristic_factor { get; set; } = 0.0;
private int current_expand_points_count { get; set; } = 0;
private int all_expand_points_count { get; set; } = 0;
private int search_nums_count { get; set; } = 0;
private int move_step_nums { get; set; } = 0;
private List sum_result { get; set; } = new List();
public StringBuilder sb { get; set; } = new StringBuilder();
public ARAstar()
{
}
private double DistanceToGoal(Points current)
{
return (double)(Math.Abs(current.y - goal_pos.y) +
Math.Abs(current.x - goal_pos.x));
}
private void DecreaseHeuristicFactor(Cell goal_arg)
{
Cell p = current_open_list[0];
heuristic_factor = Math.Min(heuristic_factor, (goal_arg.g_value / (p.g_value + p.h_value)));
sb.AppendLine("heuristic_factor = " + heuristic_factor);
}
private bool ArriveGoal()
{
return (current_start == goal_pos);
}
private void StartMove()
{
#if __CPP__
current_start = current_path.back();
#else
#if __VECTOR__
current_start = Vector.back(current_path);
#else
current_start = current_path[current_path.Count - 1];
#endif
#endif
//current_path.pop_back();
//Vector.pop_back(current_path);
current_path.RemoveAt(current_path.Count - 1);
move_step_nums++;
}
private bool NextStepIsInObstacleList()
{
return IsInList(current_path[current_path.Count - 1], current_obstacle_list);
}
private bool IsInList(Points point, List list)
{
return list.Exists(t => t.x == point.x && t.y == point.y);
}
private void ClearCurrentContainers()
{
current_open_list.Clear();
current_save_path_hash.Clear();
current_observed_cell_info_list.Clear();
}
#if __VECTOR__
public Cell OpenListPopMinElem()
{
if (current_open_list.Count == 0)
{
throw new Exception("current_open_list is empty!");
//return new Cell(new Points(0, 0), float.MaxValue, float.MaxValue, float.MaxValue, float.MaxValue);
}
else if (current_open_list.Count == 1)
{
return current_open_list[0];
}
else
{
int idx = 0;
Cell g1 = current_open_list[0];
for (int i = 1; i < current_open_list.Count; i++)
{
if (current_open_list[i] < g1)
{
idx = i;
}
}
int didx = (current_open_list.Count - 1);
if (idx != didx)
{
Cell tmp = current_open_list[idx];
current_open_list[idx] = current_open_list[didx];
current_open_list[didx] = tmp;
}
return current_open_list[didx];
}
}
#endif
private List GetNeighborsPoint(Points current_pos)
{
List neighbors = new List();
if ((current_pos.y - 1) >= 0)
{
neighbors.Add(new Points(current_pos.y - 1, current_pos.x));
}
if ((current_pos.y + 1) < rows)
{
neighbors.Add(new Points(current_pos.y + 1, current_pos.x));
}
if ((current_pos.x - 1) >= 0)
{
neighbors.Add(new Points(current_pos.y, current_pos.x - 1));
}
if ((current_pos.x + 1) < columns)
{
neighbors.Add(new Points(current_pos.y, current_pos.x + 1));
}
return new List(neighbors);
}
private List GetNeighborsInfo(Points current_pos)
{
List neighbors = new List();
List neighbors_pos = GetNeighborsPoint(current_pos);
for (int i = 0; i < neighbors_pos.Count; i++)
{
Points np = neighbors_pos[i];
if (!IsInList(np, current_obstacle_list))
{
if (current_observed_cell_info_list.ContainsKey(np))
{
Cell cx = (Cell)current_observed_cell_info_list[np];
neighbors.Add(new Cell(
np,
cx.f_value,
cx.h_value,
cx.g_value,
cx.v_value)
);
}
else
{
neighbors.Add(new Cell(
np,
float.MaxValue,
0.0,
float.MaxValue,
float.MaxValue)
);
}
}
}
return new List(neighbors);
}
private bool AstarAlgorithm(Cell start, ref Cell goal)
{
List incons_list = new List();
List close_list = new List();
List path_result_list = new List();
int search_successful_flg = 0;
int find_new_path_flg = 0;
ulong loop = 0;
while (goal.g_value > OpenListPopMinElem().f_value && loop < 1000000)
{
loop++;
Cell current_cell_pos = current_open_list[current_open_list.Count - 1];//.Peek();
current_open_list.RemoveAt(current_open_list.Count - 1);
current_cell_pos.v_value = current_cell_pos.g_value;
close_list.Add(current_cell_pos.xoy);
if (!current_observed_cell_info_list.ContainsKey(current_cell_pos.xoy))
{
current_observed_cell_info_list.Add(current_cell_pos.xoy, current_cell_pos);
}
else
{
current_observed_cell_info_list[current_cell_pos.xoy] = current_cell_pos;
}
List neighbors = GetNeighborsInfo(current_cell_pos.xoy);
int neighbor_expand_cnt = 0;
for (int i = 0; i < neighbors.Count; i++)
{
Cell ng = neighbors[i];
if (ng.g_value > (current_cell_pos.g_value + 1.0))
{
neighbor_expand_cnt++;
ng.g_value = current_cell_pos.g_value + 1.0;
ng.h_value = DistanceToGoal(ng.xoy);
ng.f_value = ng.g_value + heuristic_factor * ng.h_value;
if (!current_observed_cell_info_list.ContainsKey(ng.xoy))
{
current_observed_cell_info_list.Add(ng.xoy, ng);
}
else
{
current_observed_cell_info_list[ng.xoy] = ng;
}
if (!current_save_path_hash.ContainsKey(ng.xoy))
{
current_save_path_hash.Add(ng.xoy, current_cell_pos.xoy);
}
else
{
current_save_path_hash[ng.xoy] = current_cell_pos.xoy;
}
if (!IsInList(ng.xoy, close_list))
{
if (ng.xoy == goal.xoy)
{
goal = ng;
find_new_path_flg = 1;
}
current_open_list.Add(ng);
}
else
{
incons_list.Add(ng);
}
}
}
if (neighbor_expand_cnt > 0)
{
current_expand_points_count++;
}
if (current_open_list.Count == 0)
{
search_successful_flg = 1;
break;
}
}
if (search_successful_flg != 0)
{
sb.AppendLine("search fail !!");
return false;
}
else
{
sb.AppendLine("search successfully !!");
if (find_new_path_flg != 0)
{
sb.AppendLine("Have found new path !!");
Points node = goal.xoy;
while (current_save_path_hash.ContainsKey(node))
{
path_result_list.Add(node);
node = (Points)current_save_path_hash[node];
}
current_path.Clear();
current_path.AddRange(path_result_list);
//PrintSearchResult();
}
else
{
sb.AppendLine("Not found new path !!");
}
all_expand_points_count += current_expand_points_count;
current_expand_points_count = 0;
InconsPushOpenlist(incons_list);
return true;
}
}
private void InconsPushOpenlist(List incons_list_arg)
{
current_open_list.AddRange(incons_list_arg);
}
private void UpdateOpenlisByNewFactor()
{
for (int i = 0; i < current_open_list.Count; i++)
{
current_open_list[i].f_value = current_open_list[i].g_value +
heuristic_factor * current_open_list[i].h_value;
}
}
private bool ARAstarGetPath()
{
ClearCurrentContainers();
Cell goal = new Cell(goal_pos, float.MaxValue, 0.0, float.MaxValue, float.MaxValue);
Cell start = new Cell(current_start, 0.0, 0.0, 0.0, float.MaxValue);
start.h_value = DistanceToGoal(start.xoy);
start.f_value = heuristic_factor * start.h_value;
heuristic_factor = 10.0;
current_open_list.Add(start);
if (!AstarAlgorithm(start, ref goal))
{
return false;
}
while (heuristic_factor > 1.0)
{
DecreaseHeuristicFactor(goal);
UpdateOpenlisByNewFactor();
AstarAlgorithm(start, ref goal);
}
search_nums_count++;
return true;
}
private void PrintSearchResult()
{
for (int i = 0; i < rows; i++)
{
for (int j = 0; j < columns; j++)
{
if (current_start.y == i && current_start.x == j)
{
sb.AppendLine("s ");
}
else if (goal_pos.y == i && goal_pos.x == j)
{
sb.AppendLine("g ");
}
else if (IsInList(new Points(i, j), current_obstacle_list))
{
sb.AppendLine("x ");
}
else if (IsInList(new Points(i, j), current_path))
{
sb.AppendLine("o ");
}
else
{
sb.AppendLine("_ ");
}
}
}
sb.AppendLine("shortest path step nums : " + current_path.Count);
sb.AppendLine(" expand point nums : " + current_expand_points_count);
}
private void UpdataMapInfo()
{
if ((current_start.y - 1) >= 0)
{
Points px = new Points(current_start.y - 1, current_start.x);
if (IsInList(px, obstacle_list))
{
current_obstacle_list.Add(px);
}
}
if ((current_start.y + 1) < rows)
{
Points px = new Points(current_start.y + 1, current_start.x);
if (IsInList(px, obstacle_list))
{
current_obstacle_list.Add(px);
}
}
if ((current_start.x - 1) >= 0)
{
Points px = new Points(current_start.y, current_start.x - 1);
if (IsInList(px, obstacle_list))
{
current_obstacle_list.Add(px);
}
}
if ((current_start.x + 1) < columns)
{
Points px = new Points(current_start.y, current_start.x + 1);
if (IsInList(px, obstacle_list))
{
current_obstacle_list.Add(px);
}
}
}
public void SearchOneMap(string filename)
{
base.LoadMap(filename);
while (true)
{
sb.AppendLine("**********");
sb.AppendLine("search num : " + (search_nums_count + 1));
if (!ARAstarGetPath())
{
sb.AppendLine("-——-——-——-——-——-——-——-———-——-——-——-");
sb.AppendLine("|final result : no path to goal !!|");
sb.AppendLine("-——-——-——-——-——-——-——-———-——-——-——-");
PrintCountResult();
break;
}
// 当前起点go along current path
while (current_path.Count > 0)
{
UpdataMapInfo();
if (NextStepIsInObstacleList())
{
break;
}
else
{
StartMove();
}
}
if (ArriveGoal())
{
sb.AppendLine("-——-——-——-——-——-——-——-———-——-——-——-——-");
sb.AppendLine("|final result: get goal successflly!!|");
sb.AppendLine("-——-——-——-——-——-——-——-———-——-——-——-——-");
PrintCountResult();
break;
}
}
sum_result.Add(search_nums_count + " " + all_expand_points_count + " " + move_step_nums);
}
private void PrintCountResult()
{
sb.AppendLine("The nums of search : " + search_nums_count);
sb.AppendLine(" total expanded nums : " + all_expand_points_count);
}
private void PrintSumResult()
{
sb.AppendLine("-——-——-——-——-——-——-——-——-***-——-——-——-——-——-——-——-——-——-");
sb.AppendLine("-—— Sum Result ——-");
sb.AppendLine("-——-——-——-——-——-——-——-——-***-——-——-——-——-——-——-——-——-——-");
sb.AppendLine("| map num | search nums | expand nums | move_step nums |");
for (int i = 0; i < sum_result.Count; i++)
{
sb.AppendLine(sum_result[i] + "\t");
}
}
}
}
| | | | | | | | | |
using System;
using System.IO;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows.Forms;
using Legalsoft.Truffer;
namespace ARA
{
public partial class Form1 : Form
{
public Form1()
{
InitializeComponent();
panel1.Dock = DockStyle.Top;
panel2.Dock = DockStyle.Fill;
button1.Cursor = Cursors.Hand;
this.StartPosition = FormStartPosition.CenterScreen;
this.Text = "ARA* Algorithm -- BEIJING LEGAL SOFTWARE LTD.";
}
private void Form1_Load(object sender, EventArgs e)
{
button1.Text = "GONE";
}
private void button1_Click(object sender, EventArgs e)
{
string filename = Path.Combine(Application.StartupPath, @"1.txt");
ARAstar star = new ARAstar();
star.SearchOneMap(filename);
webBrowser1.DocumentText = star.sb.ToString().Replace("\n", "
\n");
}
}
}
x x x x x x x x x x x x x x x x x x x x x x x x x x x x x
x _ _ _ x _ x _ x _ x _ _ _ _ _ x _ x _ x _ x _ x _ x _ x
x _ x s x _ x _ x _ x _ x _ x _ x _ x _ x _ x _ x _ x _ x
x _ x x x _ x _ _ _ x x x _ x _ x _ x _ x _ x _ x _ x _ x
x _ x _ x _ x _ _ _ x _ x _ x _ x _ x _ _ _ x _ x _ x _ x
_ _ x _ x _ x x x _ x _ x _ x _ x _ x _ x _ x _ x _ x _ x
x _ _ _ x _ x _ x _ x _ x _ x _ x _ x _ x _ x _ x _ x _ x
x _ x _ x _ x _ x _ x _ _ _ x _ x _ x _ x x x _ x _ x _ x
x _ x _ x _ _ _ _ _ x _ x _ x _ x _ x _ x _ x _ x _ x _ x
x _ x _ x _ _ _ x _ x _ x _ x _ x _ _ _ x _ x _ x _ x _ x
x _ x _ x _ x _ x _ x _ _ _ x x x _ x _ x _ _ _ x _ x _ x
x _ x _ x _ x _ x _ _ _ x _ x _ x _ x _ x _ x _ x _ x _ x
x _ x _ x _ x _ x _ x _ x _ x _ x _ x _ x _ x _ x _ x _ x
x _ _ _ x _ x _ x _ x _ x _ x _ x _ x _ x _ x _ x _ x _ x
x _ _ _ x _ _ _ x _ x _ x _ _ _ x _ x _ x _ x _ x _ x _ x
x _ x _ _ _ _ _ x _ x _ x _ x _ x _ x _ _ _ x _ _ _ x _ x
x _ x _ x _ x x x _ _ _ x _ x _ x _ x _ x _ x _ x _ x _ x
x _ x _ x _ x _ _ _ x _ _ _ _ _ _ _ _ _ _ _ x _ x _ x _ x
_ _ x _ x _ _ _ x _ x _ x _ x _ x _ x _ x _ x g x _ x _ x
x x x _ x x x x x x x x x x x x x x x x x _ x x x _ x x x
x _ _ _ _ x x x x x x x x x _ x _ _ _ _ _ _ _ x _ _ x _ x
因赚钱容易,无名利追求,遂倾情奉献,FULL FREE BY TRUFFER。
不仅给你代码,还有论文,甚至介绍作者,只有:
比开源更开源的 TRUFFER!