Dijkstra，A*，DWA，TEB

Dijkstra

Dijkstra算法是典型的单元最短路径算法，用于计算赋权有向图或无向图的单源最短路径问题

Dijkstra算法的思想是，将带权有向图或无向图中的顶点分为两类，一类是已经确定最短路径的顶点集合S，开始时只有源点s，另一类是未确定最短路径的点集合U，然后从集合U中找出最短路径长度的顶点添加到S中，并更新其他顶点的最短路径。这样源点s到S中的所有顶点的最短路径距离都不大于源点s到U中顶点的最短路径距离，保证了S中顶点都是已经求出了最短的路径长度。

def dijkstra_search(graph, start, end):
	frontier = PriorityQueue()
	frontier.put(start, 0)
	cost_so_far = {}
	cost_so_far[star] = 0
	come_from = {}
	come_from[start] = None

	while not frontier.empty():
		current = frontier.get()
		if current == end:
			break
		for next in graph.neighbors(current):
			tmp = cost_so_far[current] + graph.cost(current, next)
			if next not in cost_so_far or tmp < cost_so_far[next]:
				cost_so_far[next] = tmp
				come_from[next] = current
				priority = tmp
				frontier.put(next, priority)
	
	return cost_so_far, come_from

A*

在静态路网中搜索最短路径的方法

• A*算法是广度优先搜索算法（BFS）的优化。从起点开始，首先遍历起点周围邻近的点，然后再遍历已经遍历过的点邻近的点，逐步的向外扩散，直到找到终点。
• A算法与Dijkstra算法的不同之处在于，A算法是一个“启发式”算法，它已经有了一些我们告诉它的先验知识，如“朝着终点的方向走更可能走到”。它不仅关注已走过的路径，还会对未走过的点或状态进行预测。

def a_star_search(graph, start, end):
	frontier = PriorityQueue()
	frontier.put(start, 0)
	cost_so_far = {}
	cost_so_far[star] = 0
	come_from = {}
	come_from[start] = None

	while not frontier.empty():
		current = frontier.get()
		if current == end:
			break
		for next in graph.neighbors(current):
			tmp = cost_so_far[current] + graph.cost(current, next)
			if next not in cost_so_far or tmp < cost_so_far[next]:
				cost_so_far[next] = tmp
				come_from[next] = current
				priority = tmp + h(next, end)
				frontier.put(next, priority)
			
	return cost_so_far, come_from

DWA

DWA算法思想是通过对不同可能的速度进行采样，然后模拟出不同轨迹，利用到目标点距离、障碍物距离和时间等进行评估，选择得分最好的轨迹，并发布速度指令控制小车前进。
DWA算法模拟的速度采样不是随机选取，而是存在一个范围，速度的大小受到小车最大速度和最小速度限制，加速度的大小受到小车电机性能影响，另外，为了保证安全，障碍物的距离也会影响速度的取值。

差速小车，麦轮车在相邻时刻内，运动距离短，可以将相邻两点之间的运动轨迹，看成直线
其他模型的运动轨迹看成弧线

三个评价函数：

• heading(v, w)：方位角评价函数，小车与目标之间的角度差，角度差越小得分越高
• dist(v, w)：小车与最近的障碍物之间的距离，距离越远得分越高
• velocity(v, w)：轨迹对应的速度大小，速度越大得分越高
  物理意义：使小车朝向目标点，避开障碍物，快速行驶

TEB

TEB算法思想是将连接起点和终点的路径看做类似橡皮筋可以发生形变，然后将外界约束当做外力使路径发生形变。

• 跟随路径+避障：约束主要有两个目标，跟随一致的全局路径规划和避障。两个目标函数均十分相似。跟随路径施加外力将局部路径拉向全局路径，避障约束施力使得局部路径远离障碍物
• 速度/加速度约束：速度和加速度应在一定的范围内
• 运动学约束：若干弧段组成的平滑轨迹，控制量仅有线速度和角速度，阿克曼结构有最小转弯半径，麦轮/全向/差速均为0
• 最快路径约束：目标函数使得机器人获得最快路径，路径上的各位姿点在时间上均匀分开，而非传统空间上的最短路径

TEB可以表述为一个多目标优化问题，大多数目标都是局部的，只与一小部分参数相关，因为他们只依赖于几个连续的机器人状态。在确定了N个控制点的位置后，使用开源框架g2o（General Graph Optimization）通用图优化法对路径进行优化。在小车的情景下，所有的位姿点，时间间隔以及障碍物等均被描述为点，而将约束条件描述为边。如下图所示，距离障碍物的最小距离将障碍物和位姿点相连接，速度约束将相邻的两个位姿点和它们的时间差相连接。总得来说，TEB生成的局部轨迹由一系列带有时间信息的离散位姿组成，g2o算法优化的目标即最终由这些离散的位姿组成的轨迹能达到时间最短、距离最短、远离障碍物等目标，同时限制速度与加速度使轨迹满足机器人动力学。