常用的路径规划算法总结

路径规划算法根据规划场景不同可以分为全局路径规划算法和局部路径规划算法。
一般来说，全局的算法只适用于静态障碍物的规划，不适用于动态环境；
而局部的算法适用于动态障碍的避障，但容易陷入局部最优。
接下来将分别介绍各种类型的代表性算法

1 全局路径规划算法

1.1基于采样的算法

1.1.1RRT系列算法

1.1.1RRT算法 （快速扩展随机树法）

主要思想：通过构建一颗从起点开始生长的随机树来探索地图，当随机树扩展到目标区域，通过回溯即可得到从起点到目标区域的无碰撞路径。
优点：①适用于高维空间的路径规划问题；②具有概率完备性，即只要存在可行路径，当算法运行时间足够长的时候，一定可以找到这条可行路径。
缺点：①生成的路径 的曲折度高；②具有生成的路径具有不确定性
简要伪代码如下：

Initialize map, starting point, random tree T and target area
	while not find path:
		randPoint = randSample(map) 	//Get random points by random sampling in map space
		nearestPoint = findNearestPoint(randPoint,T) 	//Get the nearest point to randPoint from the nodes of the random tree T
		newPoint = extend(nearestPoint,randPoint)	//Extend newPoint in the direction of nearestPoint-randPoint in the default step size.
		if collisionDetection(newPoint,nearestPoint):	//Check if there is obstacle between nearestPoint-newPoint.
			addPoint(newPoint,T) 	//Add newPoint to the random tree
			if Reach the target area：
				path = getPath(newPoint,T)
				break
		else:
			continue
	return path

1.1.1.2RRT算法*

RRT算法是RRT算法的改进算法，相较于RRT算法，作出了如下改进：
改进点1：重选父节点(choose parent)。在一定范围内为新扩展出的节点newPoint重新搜索到起点的代价更小的父节点。
改进点2：剪枝(rewire)。在一定范围内，尝试将新扩展出的节点newPoint作为范围内节点的父节点，如果修改后路径代价更小，则保留修改。
优点：①适用于高维空间；②具有概率完备性，同RRT；③具有渐进最优性，即：随着算法运行时间变长，路径会逐渐变短，只要运行时间足够长，则一定会找到最优路径。
缺点：①路径曲折度高；②具有生成的路径具有不确定性。

1.1.2PRM算法 （概率路线图法）

主要思想：在地图空间随机采样一批点，然后将这些点作为节点，用搜索算法(例如：A*、D*等)进行搜索，从而获得路径。
优点：①适用于高维空间；②采样点可以重复使用，适用于固定、需要重复多次规划的场景
缺点：①对于障碍密集、狭窄的环境，不适用；②路径曲折、具有随机性

1.2基于采样搜索的算法

1.2.1Dijkstra算法

主要过程：
①初始化：将起始节点的距离标记为0，其他节点的距离标记为无穷大。同时，将起始节点加入open list，用于按照距离的顺序选择下一个要访问的节点。
②迭代更新：从起始节点开始，不断更新与当前节点相邻的节点的距离，即更新open list，确保选择的路径是到达这些节点的最短路径。
③标记已访问： 将已经确定最短路径的节点标记为已访问，加入到closed list中，不再考虑它们。
④输出最短路径： 当所有节点的最短路径都被确定后，即可从closed list中得到从起始节点到每个节点的最短路径。
优点：①简单易懂；②可并行化；③可以得到划分网格后的全局最优解；
缺点：①计算复杂度高；②内存消耗较大；③不适用于机械臂等高维问题的路径规划。

1.2.2A*算法

A*算法是Dijkstra算法的改进版本。除了在考虑节点的代价时，加入了启发项，其他均与Dijkstra算法相同，其评价函数为：

f(n) = g(n)+w * h(n)

其中f(n)代表该节点的代价，g(n)代表该节点到起点的代价，h(n)代表该节点到终点的预估代价，w为启发权重。在评价函数中“wh(n)”是启发项。这使得A算法的搜索相较于Dijkstra更加高效。
同时，当启发权重w>1时，算法搜索会更快，但搜索出的路径不一定是最优的；当w<1时，算法搜索出的路径是全局最优的，但是效率会较低，当w=0时，则会退化成为Dijkstra算法。
优点：
①完备性（Completeness）：A*算法可以保证找到一条可行的路径（如果存在的话），只要存在路径可到达目标点，算法就会找到它；
②最优性（Optimality）：在w<=1的情况下，A*算法可以保证找到最优路径，即经过的节点数量最少或者路径代价最低。
缺点：
①空间复杂度高：A*算法需要维护一个优先队列来存储待探索的节点，并且在某些情况下可能需要存储大量的节点信息。这可能导致算法的空间复杂度较高，特别是在处理大型地图时；
②时间复杂度不稳定：尽管A*算法通常具有较好的时间复杂度，但在某些情况下，特别是在启发函数不合适或目标节点远离起始节点时，算法的时间复杂度可能会显著增加。
③不适用于动态环境：A*算法通常假设环境是静态的，即地图和障碍物不会发生变化。在动态环境下，如果有障碍物移动或者路径发生变化，A*算法可能需要重新规划路径，而这会增加计算成本。
④不适用于高维空间：在高维空间中，A*算法可能会受到维数灾难的影响，导致搜索空间过大，使得算法效率降低。
⑤不适用于连续状态空间：A*算法通常用于离散状态空间，而在连续状态空间中，由于节点数量无限，A*算法无法直接应用。

1.2.3 Theta*算法

Theta*算法是A*算法的改进版本，旨在解决A*算法在存在大量障碍物或者网格地图精细度较高时效率低下的问题。
核心思想在于利用直线可见性检查来减少不必要的节点扩展，从而提高了路径规划的效率和性能。其算法流程相对于传统的A*算法更加高效，特别适用于需要快速路径规划的实时系统和大规模地图的场景。

1.3基于生物启发式的算法

1.3.1 蚁群算法

2 局部路径规划算法

2.1 DWA（动态窗口法）

动态窗口法（Dynamic Window Approach，DWA）是一种用于移动机器人的局部路径规划算法，特别适用于速度和转向同时考虑的动态环境中。它基于机器人的动力学模型和感知信息，通过动态调整速度和转向，以适应环境的变化，从而实现安全、高效的路径规划。

以下是动态窗口法的基本思想和步骤：

1. 搜索可行速度和转向
根据机器人的动力学模型和环境的感知信息，确定机器人在当前时刻可以采取的所有速度和转向组合。
这些速度和转向的范围被称为动态窗口，通常由最小速度、最大速度、最小转向角速度和最大转向角速度等参数限定。
2. 轨迹生成
针对搜索到的每个速度和转向组合，根据机器人的动力学模型，在短时间内生成一系列可能的轨迹。
这些轨迹代表了机器人可能采取的移动路径，包括直线运动、曲线运动等。
3. 轨迹评估
对于生成的每条轨迹，对其进行三个指标的评估，分别是：轨迹与障碍物方向的偏向角，角度越小得分越高；这条轨迹中机器人的速度大小，速度越大得分越高；这条轨迹与障碍物的距离，距离越大，得分越高。基于这三项给出综合的得分
评估可以基于机器人的运动学特性、环境的障碍物信息和路径的优化目标。
4. 选择最优轨迹
根据轨迹的评估结果，选择得分最高的轨迹作为机器人的移动策略。
最优轨迹通常是能够最大程度地接近目标路径，并且避开障碍物的轨迹。
5. 控制指令下发
将最优轨迹对应的速度和转向作为机器人的控制指令，下发给机器人的执行系统。
机器人根据控制指令执行相应的动作，实现路径规划和导航。

优点：

1. 实时性： DWA算法能够在实时环境中快速生成路径，并且能够根据动态环境的变化实时调整机器人的速度和转向，适用于需要快速响应的场景。
2. 适应性： DWA算法可以根据机器人的动态窗口，动态调整速度和转向，在不同的环境和任务中实现灵活适应，适用性较广。
3. 避障能力： DWA算法通过评估生成的轨迹与障碍物的接触情况，能够有效避免碰撞和避开障碍物，提高了机器人的安全性和稳定性。
4. 简单实现： DWA算法的实现相对简单，不需要复杂的地图建模和路径规划算法，只需基于机器人的动力学模型和感知信息，即可进行路径规划和导航。

缺点：

• 局部最优解： DWA算法是一种局部路径规划方法，只考虑短时间内的动态窗口，可能会陷入局部最优解而无法找到全局最优路径。

动态窗口法通过动态调整速度和转向，可以在动态环境中实现快速、灵活的路径规划，并且能够有效应对环境的变化和不确定性。它广泛应用于移动机器人的自主导航、人机交互、避障和轨迹跟踪等领域。

未完待续。。。