Open eyes, open roof

Give me miles, give me truth

​ —— 《Lone Ranger》Rachel Platten

自动导航机器人（AMR）在仓库、工厂等环境中进行路径规划和导航。在这篇博客中，我们将讨论几种寻路算法及其在 AMR 中的应用场景，包括 A、Bidirectional A、Lifelong Planning A 以及其他 A 变种。

Base on Sampling

基于采样的寻路算法，主要代表是 RRT（快速搜索随机树），根据搜索空间分为 2D RRT 和 3D RRT，常用于工业手臂路径规划，近几年在 UAV 领域比较火热，一般用于杂乱的环境中快速寻路及避开移动障碍物等场景。较少直接用于 AMR 调度系统，这里不再赘述。

Base on Search

基于搜索的寻路算法，从最基础的BFS和DFS，再到引入h(x)的best_first、D、A（与best_first的区别之一在于对h(x)和g(x)关系的处理），再到A*算法的变种，常见的比如：

• Bidirectional A*
• Anytime Repairing A*
• Lifelong Planning A*
• Learning Real-time A*
• Jump Point Search A*
• Dynamic A*
• D* Lite

A*

最常用的启发寻路算法，会根据实际场景对启发函数进行修改以满足需要，比如考虑到AMR的旋转代价，会在h(x)中添加转角代价，用来选取场景中转角最小的路径。

转角代价的实现：

为了实现转角代价，我们可以在计算启发函数h(x)时引入转角代价。具体而言，在计算从当前节点到目标节点的启发式代价时，可以考虑当前节点的转角。假设当前节点为n，下一个节点为n’，则可以定义转角代价为：

angle_cost(n, n') = k * abs(angle(n) - angle(n'))

其中，angle(n)和angle(n')分别表示从起点到节点n和节点n’的路径的末端方向角度，k为一个权重系数，用于调整转角代价在整体启发式代价中的影响力。

总结：

• 适用场景：具有静态环境的路径规划，需要考虑 AMR 的旋转代价等场景。
• 优势：广泛应用于各种场景，可定制启发式函数以满足实际需求。

Bidirectional A*

双向A，在A的基础上，从起始点同时开始对向搜索，往往使用同一套h(x)，直到得到相遇点s_meet，结束搜索，典型的以空间换时间的改进思路。但在AMR寻路环境中，双向A*的适用性有限，理由如下：

• AMR实际寻路环境范围尺度不会特别大，优化搜索时间效果不明显
• 实际的拓扑图的连通关系由人为决定，会存在单双向路径，双向搜索失效
• 由于启发函数的局限性，即使双向搜索也无法解决slam场景中站点设置出现“凹形”时AMR描边的问题

总结：

• 适用场景：搜索空间较大且双向搜索可行的场景。
• 优势：通过同时从起点和终点进行搜索，提高搜索效率。
• 劣势：在 AMR 场景中，可能不适用于单双向路径或尺度较小的环境。

Lifelong Planning A*

与基础的A*算法相比，最小heap的key由原先的一维代价变成二维的：

没有了原来的h(x)，新增了：

• rhs(x)：一个基于节点前任的g值的前瞻值（所有g(n’)+d(n’, n)的最小值，其中n’是n的前任，d(x, y)是连接x和y的边的代价）
• U(x)：一个基于节点优先k1排序，其次k2排序的堆

在搜索过程中不断计算rhs(x)与g(x)的大小关系，判定是否从队列中移除该点，在过拟合的情况下，重新计算局部过一致的点来达到绕障的目的。

在实际使用中，AMR往往会按照最早计算的路径开始运动，而未到达的点的是否占用的情况对当位置的影响有限。通常做法是根据AMR运行速度动态检测前向n个点的通畅情况，如果遇到拥堵，可以根据点的空闲情况再次寻路到目标点，也不会遍历过多节点。单纯的使用LPA*并不会带来明显的提升，但可以作为一种可选方案，视具体场景而定。

总结：

• 适用场景：适用于需要在环境中频繁更新和维护路径的场景。
• 优势：可以在过拟合的情况下，重新计算局部过一致的点来达到绕障的目的。

Anytime Repairing A (ARA)

ARA是一种能够在有限时间内找到次优解并随着时间的推移不断优化解的算法。它在每次迭代中使用一个较低的启发式值放松约束，以便在有限时间内找到次优解。随后，ARA会不断改进找到的路径，直到找到最优解或达到用户设定的时间限制。

在实际应用中，ARA适用于需要在短时间内找到可行解并在有空闲时间时继续优化解的场景。例如，在机器人动态环境中，ARA可以在短时间内找到一个可行路径，然后在机器人执行该路径时继续优化路径。

大致过程如下：

• 初始化优先队列和起始节点
• 在每次迭代中，执行以下操作：
  • 从优先队列中选取最优节点
  • 如果找到了目标节点，则返回当前路径
  • 扩展当前节点的邻居节点，并更新优先队列
• 在有限时间内，继续优化找到的路径

总结：

• 优势：ARA 能够在短时间内找到次优解并随着时间推移不断优化解。这意味着在 AMR 需要快速作出决策时，ARA 是一个很好的选择。
• 劣势：与其他算法相比，ARA* 可能需要更多的计算资源来持续优化解。
• 适配：对于需要在短时间内找到可行解并在有空闲时间时继续优化解的 AMR 场景，ARA* 是一个不错的选择。

Learning Real-time A (LRTA)

LRTA是一种实时搜索算法，可以在搜索过程中不断更新和学习启发式函数。在每次移动到下一个节点时，LRTA会更新当前节点的启发式值。通过学习和调整启发式函数，LRTA*能够在动态环境中更有效地找到路径。

LRTA适用于机器人在不断变化的环境中实时寻路的场景。例如，在机器人需要在实时避开移动障碍物的情况下规划路径时，LRTA能够在每次移动时更新启发式值，以适应环境的变化。

大致过程如下：

• 初始化起始节点
• 当未到达目标节点时，执行以下操作：
  • 选择当前节点的最优邻居节点
  • 更新当前节点的启发式值
  • 移动到选择的邻居节点

总结：

• 优势：LRTA* 能够在搜索过程中不断更新和学习启发式函数，适用于动态环境中的实时寻路。
• 劣势：LRTA* 可能在寻找最优解方面表现不如其他算法。
• 适配：对于 AMR 需要在不断变化的环境中实时寻路的场景，LRTA* 是一个有效的选择。

Jump Point Search A* (JPS)

Jump Point Search (JPS) 是一种用于大规模地图上的A*寻路算法优化。通过跳过部分节点，JPS可以减少搜索空间，从而提高搜索效率。JPS特别适用于大规模网格地图，如游戏地图和地理信息系统（GIS）中的路径规划。

在实际应用中，JPS适用于大规模地图上的高效路径搜索。例如，在游戏地图或GIS中，JPS可以在短时间内找到路径，同时减少计算资源的消耗。

大致过程如下：

• 初始化优先队列和起始节点
• 在每次迭代中，执行以下操作：
  • 从优先队列中选取最优节点
  • 如果找到了目标节点，则返回当前路径
  • 找到当前节点的跳跃点，并将其添加到优先队列中

总结：

• 优势：JPS 通过跳过部分节点来减少搜索空间，特别适用于大规模网格地图。在 AMR 中，这意味着 JPS 可以在较大场景中提供高效的路径搜索。
• 劣势：JPS 主要适用于网格地图，可能不适用于所有 AMR 场景。
• 适配：在 AMR 的大规模地图上进行高效路径搜索时，JPS 是一个合适的选择。

Dynamic A (D)

Dynamic A (D) 是一种用于动态环境中的路径规划算法，可以在环境发生变化时有效地修复和更新路径。与A算法相比，D算法在处理环境变化时不需要重新计算整个路径，而是只修复受到变化影响的部分。这使得D*在动态环境中的搜索效率更高。

在实际应用中，D适用于需要在环境发生变化时快速更新路径的场景。例如，在机器人需要在实时避开移动障碍物的情况下规划路径时，D算法能够快速修复受到影响的路径部分，以适应环境的变化。

大致过程如下：

• 初始化优先队列和起始节点
• 在每次迭代中，执行以下操作：
  • 从优先队列中选取最优节点
  • 如果找到了目标节点，则返回当前路径
  • 扩展当前节点的邻居节点，并更新优先队列
• 当环境发生变化时，修复受影响的路径部分

总结：

• 优势：D 和 D Lite 适用于动态环境中快速更新路径的场景，这意味着在 AMR 需要在实时避开移动障碍物的情况下规划路径时，这些算法可以高效地修复受到影响的路径部分。
• 劣势：这些算法可能不如 A* 或其变体在寻找最优解方面表现出色。
• 适配：对于需要在环境发生变化时快速更新路径的 AMR 场景，D 和 D Lite 是合适的选择。

D* Lite

D Lite 是 Dynamic A (D) 算法的一种改进版本，其优化了D算法的效率和实时性。D Lite 通过反向搜索并且使用优先队列来提高搜索效率。与D相比，D* Lite 更适用于实时动态环境中的路径规划。

在实际应用中，D Lite 适用于需要在环境发生变化时快速更新路径的场景，例如，自动驾驶汽车在实时避开移动障碍物的情况下规划路径时，D Lite 能够高效地修复受到影响的路径部分，以适应环境的变化。

大致过程如下：

• 初始化优先队列和起始节点
• 在每次迭代中，执行以下操作：
  • 从优先队列中选取最优节点
  • 如果找到了目标节点，则返回当前路径
  • 扩展当前节点的邻居节点，并更新优先队列
• 当环境发生变化时，修复受影响的路径部分，使用反向搜索

总结

以上所提到的 A 变种算法具有各自的特点和应用场景。Anytime Repairing A (ARA) 可以在短时间内找到次优解并随着时间推移不断优化解；Learning Real-time A (LRTA) 能够在搜索过程中不断更新和学习启发式函数，适用于动态环境中的实时寻路；Jump Point Search A (JPS) 通过跳过部分节点来减少搜索空间，特别适用于大规模网格地图；Dynamic A (D) 和 D* Lite 则适用于动态环境中快速更新路径的场景。

Automated guided robots (AMR) perform path planning and navigation in environments such as warehouses and factories. In this blog, we will discuss several pathfinding algorithms and their application scenarios in AMR, including A, Bidirectional A, Lifelong Planning A, and other A variants.

Base on Sampling

Sampling-based pathfinding algorithm, the main representative is RRT (Rapid Search Random Tree), which is divided into 2D RRT and 3D RRT according to the search space, commonly used in industrial arm path planning, and has been hot in UAV field in recent years, generally used for fast pathfinding in cluttered environment and avoiding moving obstacles. It is rarely used directly in AMR scheduling system, so it is not repeated here.

Base on Search

Search-based pathfinding algorithms, from the most basic BFS and DFS, to the introduction of h(x) best_first, D, A (one of the differences with best_first is the treatment of the relationship between h(x) and g(x)), to the A* algorithm variants, common ones such as:

• Bidirectional A*
• Anytime Repairing A*
• Lifelong Planning A*
• Learning Real-time A*
• Jump Point Search A*
• Dynamic A*
• D* Lite

A*

The most commonly used heuristic path finding algorithm will modify the heuristic function to meet the needs according to the actual scene, for example, considering the rotation cost of AMR, the corner cost will be added to h(x) to select the path with the smallest corner in the scene.

Implementation of corner cost:

To implement the corner cost, we can introduce the corner cost in the calculation of the heuristic function h(x). Specifically, the corner of the current node can be considered when calculating the heuristic cost from the current node to the target node. Assuming that the current node is n and the next node is n’, the corner cost can be defined as

angle_cost(n, n') = k * abs(angle(n) - angle(n'))

where angle(n) and angle(n') denote the end direction angle of the path from the starting point to node n and node n’, respectively, and k is a weighting factor to adjust the influence of the corner cost in the overall heuristic cost.

To summarize:

• Applicable scenarios: Path planning with static environment, scenarios that require consideration of the rotation cost of AMR, etc.
• Advantage: Widely used in various scenarios, the heuristic function can be customized to meet practical needs.

Bidirectional A*

Bidirectional A, on the basis of A, starts the opposite direction search from the starting point at the same time, often using the same set of h(x), until the encounter point s_meet, ending the search, a typical space-for-time improvement idea. However, the applicability of bidirectional A* is limited in the AMR pathfinding environment for the following reasons:

• AMR actual pathfinding environment range scale will not be particularly large, optimizing the search time effect is not obvious
• The actual topological map of the connectivity relationship is determined by human, there will be a single bi-directional path, bi-directional search failure
• Due to the limitations of the heuristic function, even two-way search can not solve the problem of AMR tracing when the site settings in the slam scenario appears “concave”

Summary:

• Applicable scenarios: the search space is large and the two-way search is feasible scenarios.
• Advantage: By searching from the starting point and the end point at the same time, the search efficiency is improved.
• Disadvantage: In AMR scenarios, it may not be applicable to single and two-way paths or small-scale environments.

Lifelong Planning A*

Compared with the basic A* algorithm, the key of the minimum heap is changed from the original one-dimensional cost to a two-dimensional:

Without the original h(x), a new:

• rhs(x): a look-ahead value based on the g-values of the node predecessors (the minimum of all g(n’) + d(n’, n), where n’ is the predecessor of n and d(x, y) is the cost of the edge connecting x and y)
• U(x): a heap based on node-first k1 sorting, followed by k2 sorting

The size relationship between rhs(x) and g(x) is continuously computed during the search process to determine whether to remove the point from the queue, and in the case of overfitting, to recompute the local over-consistent points to achieve the purpose of bypassing the barrier.

In practice, AMR tends to start movement according to the earliest calculated path, and the occupancy or non-occupancy of the un-arrived points has limited impact on the current position. The usual practice is to dynamically detect the smoothness of the forward n points according to the AMR running speed, and if congestion is encountered, the path can be sought again to the target point according to the point’s availability, and it will not traverse too many nodes. Using LPA* alone does not bring significant improvement, but can be an optional solution depending on the specific scenario.

Summary:

• Applicable scenarios: Suitable for scenarios where paths need to be updated and maintained frequently in the environment.
• Advantage: It can recalculate local over-consistent points to achieve the purpose of bypassing in the case of overfitting.

Anytime Repairing A (ARA)

ARA is an algorithm that can find suboptimal solutions in finite time and keep optimizing them over time. It uses a lower heuristic value in each iteration to relax the constraint in order to find the suboptimal solution in finite time. Subsequently, ARA continuously improves the found path until an optimal solution is found or a user-set time limit is reached.

In practical applications, ARA is suitable for scenarios where a feasible solution needs to be found in a short time and continued to optimize the solution when free time is available. For example, in a robotic dynamic environment, ARA can find a feasible path in a short time and then continue to optimize the path as the robot executes it.

The approximate process is as follows:

• Initialize the priority queue and starting node
• In each iteration, the following operations are performed:
  • Selects the optimal node from the priority queue
  • If the target node is found, the current path is returned
  • Expand the neighbor nodes of the current node and update the priority queue
• Continue optimizing the found path for a limited time

Summarize:

• Advantage: ARA is able to find the suboptimal solution in a short time and keep optimizing the solution over time. This means that ARA is a good choice when AMR needs to make fast decisions.
• Disadvantage: ARA* may require more computational resources to continuously optimize the solution compared to other algorithms.
• Suitability: ARA* is a good choice for AMR scenarios where you need to find a feasible solution in a short time and continue optimizing the solution when you have free time.

Learning Real-time A (LRTA)

LRTA is a real-time search algorithm that continuously updates and learns heuristic functions during the search process. On each move to the next node, LRTA updates the heuristic value of the current node. By learning and adjusting the heuristic function, LRTA* is able to find paths more efficiently in dynamic environments.

LRTA is suitable for scenarios where robots are seeking paths in real time in a constantly changing environment. For example, in situations where a robot needs to plan a path in real time while avoiding moving obstacles, LRTA is able to update the heuristic values at each move to adapt to the changing environment.

The approximate process is as follows:

• Initialize the starting node
• When the target node is not reached, the following operations are performed:
  • Select the optimal neighbor node of the current node
  • Update the heuristic value of the current node
  • Move to the selected neighbor node

Summary:

• Advantage: LRTA* is able to continuously update and learn heuristic functions during the search process, which is suitable for real-time pathfinding in dynamic environments.
• Disadvantage: LRTA* may not perform as well as other algorithms in finding the optimal solution.
• Suitability: LRTA* is an effective choice for scenarios where AMR requires real-time pathfinding in a constantly changing environment.

Jump Point Search A* (JPS)

Jump Point Search (JPS) is an optimization of the A* pathfinding algorithm for large-scale maps. By skipping some nodes, JPS can reduce the search space and thus improve the search efficiency. JPS is particularly suitable for path planning in large-scale grid maps, such as game maps and geographic information systems (GIS).

In practical applications, JPS is suitable for efficient path search on large scale maps. For example, in game maps or GIS, JPS can find paths in a short time while reducing the consumption of computational resources.

The approximate process is as follows:

• Initialize the priority queue and starting node
• In each iteration, the following operations are performed:
  • Select the optimal node from the priority queue
  • If the target node is found, the current path is returned
  • Find the jump point of the current node and add it to the priority queue

Summarize:

• Advantage: JPS reduces the search space by skipping some nodes, which is especially suitable for large-scale grid maps. In AMR, this means that JPS can provide efficient path search in larger scenarios.
• Disadvantage: JPS is mainly suitable for grid maps and may not be applicable to all AMR scenarios.
• Suitability: JPS is a suitable choice when performing efficient path search on large scale maps in AMR.

Dynamic A (D)

Dynamic A (D) is a path planning algorithm used in dynamic environments to efficiently repair and update paths as the environment changes. In contrast to the A algorithm, the D algorithm does not need to recompute the entire path when dealing with changes in the environment, but only repairs the part affected by the change. This makes D* more efficient in searching in dynamic environments.

In practical applications, D is suitable for scenarios where the path needs to be updated quickly when the environment changes. For example, when a robot needs to plan a path while avoiding moving obstacles in real time, the D algorithm can quickly repair the affected part of the path to adapt to changes in the environment.

The approximate process is as follows:

• Initialize the priority queue and starting node
• In each iteration, the following operations are performed:
  • Select the optimal node from the priority queue
  • If the target node is found, return the current path
  • Expand the neighbor nodes of the current node and update the priority queue
• Repair the affected part of the path when the environment changes

Summary:

• Strengths: D and D Lite are suitable for scenarios where paths are updated quickly in dynamic environments, meaning that these algorithms can efficiently repair affected path sections when AMR needs to plan paths in real-time while avoiding moving obstacles.
• Disadvantage: These algorithms may not perform as well as A* or its variants in finding optimal solutions.
• Suitability: For AMR scenarios that require fast path updates as the environment changes, D and D Lite are suitable choices.

D* Lite

D Lite is an improved version of the Dynamic A (D) algorithm, which optimizes the efficiency and real-time performance of the D algorithm. Compared to D, D Lite is more suitable for path planning in real-time dynamic environments.

In practice, D Lite is suitable for scenarios where the path needs to be updated quickly when the environment changes, e.g., when a self-driving car is planning a path while avoiding moving obstacles in real time, D Lite can efficiently repair the affected part of the path to adapt to the changes in the environment.

The approximate process is as follows:

• Initialize the priority queue and starting node
• In each iteration, the following operations are performed:
  • Selects the optimal node from the priority queue
  • If the target node is found, return the current path
  • Expand the neighbor nodes of the current node and update the priority queue
• When the environment changes, repair the affected part of the path, using reverse search

Summary

The above mentioned A variants have their own characteristics and application scenarios. Anytime Repairing A (ARA) can find suboptimal solutions in a short time and optimize them over time; Learning Real-time A (LRTA) can continuously update and learn heuristic functions during the search process, which is suitable for Jump Point Search A (JPS) reduces the search space by skipping some nodes, which is especially suitable for large-scale grid maps; Dynamic A (D) and D* Lite are suitable for fast path update scenarios in dynamic environments.