移动机器人路径规划
路径搜索算法(DFS,BFS,Dijkstra,A*)-演示
部分源码:
// BFS(广度优先搜索算法) 算法用队列 先进先出 逐层扩展,直到终点 只有BFS算法能保证路径的最优性
//起始点 start 终止点kraj 蓝色
public void BFS(Vektor2D start, Vektor2D kraj, Graphics g)
{ //找到起止点最近的多边形顶点
var novo = UbaciPocetakKraj(start, kraj, g, Color.Blue);
start = novo.Item1;//更新起止点
kraj = novo.Item2;
// printGrane();
Dictionary<int, Tuple<int, Vektor2D>> prethodni = new Dictionary<int, Tuple<int, Vektor2D>>();//prethodni 已遍历字典
Queue<Vektor2D> queue = new Queue<Vektor2D>();//队列
queue.Enqueue(start);//起点压入队列
while (queue.Count > 0)
{
var vertex = queue.Dequeue();//队列中取出一个顶点
foreach (Vektor2D neighbor in NeighborList[vertex.V])//遍历该顶点的所有邻居顶点
{
if (prethodni.ContainsKey(neighbor.V))//如果邻居顶点已经存在于 prethodni字典 则跳过
continue;
//构建邻居顶点 字典
prethodni[neighbor.V] = new Tuple<int, Vektor2D>(vertex.V, vertex);//将该顶点索引和该顶点坐标值 存入已遍历字典 prethodni
queue.Enqueue(neighbor);//将其他未遍历的顶点压入队列
}
}
var path = new List<Tuple<int, Vektor2D>> { };//路径
int trenutni = kraj.V;//更新后的终点索引
Vektor2D trenutniCvor = kraj;//新的终点
while (!trenutni.Equals(start.V))//更新后的终点和起点不重合
{
path.Add(new Tuple<int, Vektor2D>(trenutni, trenutniCvor));//新的终点节点索引及坐标添加到路径中
trenutni = prethodni[trenutni].Item1;// 新的终点节点索引
trenutniCvor = prethodni[trenutni].Item2;//取出终点坐标
};
path.Add(new Tuple<int, Vektor2D>(start.V, start));//将起点添加到路径
path.Reverse();//反转路径点排序
path.ToList().Take(10).ToList().ForEach(p => Debug.Write(p.Item1 + " "));
for (int i = 0; i < path.Count - 1; i++)
{
// 绘制路径
Segment s = new Segment(Node[path[i].Item1], Node[path[i+1].Item1]);//
s.DrawSegment(g, Color.Blue);
}
}
//DFS算法用栈 先进后出 按分支扩展 直到终点 红色
public void DFS(Vektor2D start, Vektor2D kraj, Graphics g)
{
var novo = UbaciPocetakKraj(start, kraj, g, Color.Red);//找到分别距离起点和终点最近的点
start = novo.Item1;//更新起点
kraj = novo.Item2;//更新终点
List<bool> posjeceni = new List<bool>(new bool[velicina]);//存储节点是否已遍历的列表
Stack<Vektor2D> stack = new Stack<Vektor2D>();//堆
stack.Push(start);//新的起点压入堆
Dictionary<int, Tuple<int, Vektor2D>> prethodni = new Dictionary<int, Tuple<int, Vektor2D>>();//已遍历节点字典
while (stack.Count > 0)
{
var vertex = stack.Pop();//取节点
if (posjeceni[vertex.V])//如果该节点已处理,跳过
continue;
posjeceni[vertex.V] = true;//设置已处理标志true
foreach (Vektor2D neighbor in NeighborList[vertex.V])//遍历节点的邻居节点
{
if (prethodni.ContainsKey(neighbor.V))//如果邻居节点已遍历跳过
continue;
prethodni[neighbor.V] = new Tuple<int, Vektor2D>(vertex.V, vertex);//将该点添加到已遍历字典
stack.Push(neighbor);//将邻居节点压入堆
}
}
var path = new List<Tuple<int, Vektor2D>> { };//路径
int trenutni = kraj.V;//终点索引
Vektor2D trenutniCvor = kraj;//终点坐标
//寻找一条路径,很绕远
while (!trenutni.Equals(start.V))//新的终点与新的起点不重合
{
path.Add(new Tuple<int, Vektor2D>(trenutni, trenutniCvor));//新的终点添加到路径
trenutni = prethodni[trenutni].Item1;//字典中新终点的节点索引
trenutniCvor = prethodni[trenutni].Item2;//字典中新终点的坐标
};
path.Add(new Tuple<int, Vektor2D>(start.V, start));//新的起点加入路径
path.Reverse();
path.ToList().ForEach(p => Debug.Write(p.Item1 + " "));
for (int i = 0; i < path.Count - 1; i++)
{
// 画
Segment s = new Segment(Node[path[i].Item1], Node[path[i + 1].Item1]);
s.DrawSegment(g, Color.Red);
}
}
//Dijkstra's algorithm 绿色
public void DJIKSTRA(Vektor2D start, Vektor2D kraj, Graphics g) {
var novo = UbaciPocetakKraj(start, kraj, g, Color.Green);//找到分别里起止点最近的点
start = novo.Item1;//更新新的起点
kraj = novo.Item2;//更新新的终点
int startV = start.V;//新的起点节点索引
int krajV = kraj.V;//新的终点节点索引
int[] dist = new int[velicina];//距离数组:节点数量
int[] prethodni = new int[velicina];//已遍历数组
for (int i = 0; i < velicina; i++)//遍历节点
{
dist[i] = int.MaxValue;//初始距离 max
prethodni[i] = -1;//初始已遍历标志-1
}
dist[startV] = 0;//起点节点的距离设置为0
List<int> Q = new List<int>();//
Q.Add(startV);//起点索引加入列表Q
while (Q.Count > 0)
{
int u = Q[0];//取出节点索引
Q.RemoveAt(0);
foreach (Vektor2D v in NeighborList[u])//遍历节点的邻居
{//找到距离u最近的节点v
if (dist[v.V] > dist[u] + Vektor2D.getUdaljenost(Node[u], Node[v.V]))
{//
dist[v.V] = dist[u] + (int)Vektor2D.getUdaljenost(Node[u], Node[v.V]);//找到节点v到起点的最短距离
prethodni[v.V] = u;//更新节点v的最近邻点索引
if (!Q.Contains(v.V))//Q中不包含v
Q.Add(v.V);//将节点v加入列表Q
}
}
}
//打印
List<int> path = new List<int>();//路径
int trenutni = krajV;//第一个终点节点
while (!trenutni.Equals(startV))
{
path.Add(trenutni);//将新的终点节点添加到路径
trenutni = prethodni[trenutni];//更新终点为trenutni的最近邻节点
}
path.Add(startV);//将起点加入路径
path.Reverse();//路径反转
// path.ForEach(p => Debug.Write(p + " "));
for (int i = 0; i < path.Count - 1; i++)
{
// 绘制路径
Segment s = new Segment(Node[path[i]], Node[path[i + 1]]);
s.DrawSegment(g, Color.Green);
}
}
//A STAR
public void A_STAR(Vektor2D start, Vektor2D kraj, Graphics gr)
{
var novo = UbaciPocetakKraj(start, kraj, gr, Color.Black);//找到起点和终点最近的节点
start = novo.Item1;//更新起点
kraj = novo.Item2;//更新终点
List<int> otvorenaLista = new List<int>();//开放列表
List<int> zatvorenaLista = new List<int>();//关闭列表
int[] prethodni = new int[velicina];//存放节点的相邻节点索引
otvorenaLista.Add(start.V);//起点加入开放列表
int[] f = new int[velicina];
int[] g = new int[velicina];
int[] h = new int[velicina];
for (int i = 0; i < velicina; i++)//遍历所有节点 初始化每个节点的 f g h
{
f[i] = int.MaxValue;
g[i] = int.MaxValue;
h[i] = int.MaxValue;
prethodni[i] = -1;
}
g[start.V] = 0;//节点权重
h[start.V] = (int)Vektor2D.getUdaljenost(start, kraj);//起止节点距离
f[start.V] = g[start.V] + h[start.V];//优先级队列的规则设置为节点的权重和与距离目标点的距离两者的和
while (otvorenaLista.Count > 0)//直到开放列表中无节点
{
int min = int.MaxValue;
int minIndex = -1;
for (int i = 0; i < otvorenaLista.Count; i++)//遍历开放列表
{
if (f[otvorenaLista[i]] < min)
{
min = f[otvorenaLista[i]];//找到开放列表中优先级f最小值
minIndex = i;//优先级f最小值索引
}
}
int trenutni = otvorenaLista[minIndex];//取优先级f最小值的索引对应的 节点索引
otvorenaLista.RemoveAt(minIndex);//该节点从开放列表取出
zatvorenaLista.Add(trenutni);//放入关闭列表,不再考虑
if (trenutni == kraj.V)//如果该节点已经是终点,打印路径
{
// Print
List<int> path = new List<int>();
int trenutniCvor = kraj.V;
while (!trenutniCvor.Equals(start.V))
{
path.Add(trenutniCvor);
trenutniCvor = prethodni[trenutniCvor];
}
path.Add(start.V);
path.Reverse();
//path.ForEach(p => Debug.Write(p + " "));
for (int i = 0; i < path.Count - 1; i++)
{
// Crtanje
Segment s = new Segment(Node[path[i]], Node[path[i + 1]]);
s.DrawSegment(gr, Color.Black);
}
return;
}
foreach (Vektor2D susjed in NeighborList[trenutni])//遍历最后加入关闭列表的节点邻居,找到距离其最近的邻居节点
{
if (zatvorenaLista.Contains(susjed.V))//如果关闭列表已存在其邻居,跳过
continue;
//
int tempG = g[trenutni] + (int)Vektor2D.getUdaljenost(Node[trenutni], Node[susjed.V]);//计算到起点的距离
if (!otvorenaLista.Contains(susjed.V))//开放列表不存在该邻居
otvorenaLista.Add(susjed.V);//该邻居索引加入开放列表
else if (tempG >= g[susjed.V])//如果该邻居节点到起点距离超过上一邻居点到起点距离,跳过
continue;
prethodni[susjed.V] = trenutni;//更新邻居节点的相邻节点索引
g[susjed.V] = tempG;//更新邻居节点到起点最短距离
h[susjed.V] = (int)Vektor2D.getUdaljenost(Node[susjed.V], kraj);//计算邻居到终点距离
f[susjed.V] = g[susjed.V] + h[susjed.V];//更新邻居节点的优先级值
}
}
}
}
#endregion
参考:
https://blog.csdn.net/RoboChengzi/article/details/104096663
https://blog.csdn.net/qq_43133135/article/details/125770582
https://blog.csdn.net/qq_42688495/article/details/113108292
The End