论文阅读——SIPP: Safe Interval Path Planning for Dynamic Environments
这篇论文提出了一种名为安全间隔路径规划（SIPP）的方法来解决动态环境中路径规划的问题。以下是总结：
核心思想：
- 安全间隔：引入了连续的安全间隔概念来表示机器人在特定配置下可以停留的时间段。每个状态由当前配置和其对应的最小安全间隔组成。
- 减少状态空间：通过将时间与位置-姿态组合在一起作为单一的状态表示（即（配置、安全间隔）对），显著减少了状态空间的数量。
优势：- 最优性保证：通过优先队列A*搜索确保了路径的最优性。
- 效率提升：相比于传统的时间步长模型（如HCA*），SIPP降低了搜索空间大小并加快了规划速度。
- 复杂环境适应性：适用于包含大量动态障碍物的情况。
实验结果：- 在模拟环境中与HCA*相比，SIPP表现出更快的平均计划时间。
- 在真实机器人PR2上的测试显示其高效性，在狭窄或开放环境下均表现良好。
挑战与未来方向：- 需要解决增量扩展和重新规划问题。
- 可能需要结合其他优化技术（如膨胀启发式或增量A*）以进一步提高效率。
- 探索更复杂的环境和更具挑战性的动态障碍物轨迹预测。
结论：
SIPP通过创新的安全区间概念有效解决了动态环境中的路径规划问题，并展示了其在理论和实际应用中的有效性。尽管存在一些限制和挑战，但该方法为未来的研究提供了重要的方向。
这篇论文不仅提出了一个高效的算法框架，还展示了其实验结果的支持，并指出了未来研究的方向，具有重要的理论意义和应用价值。
SIPP：动态环境的安全区间路径规划
目录
- 动态环境下的安全区间路径规划（SIPP）*
一、概述*
*
二、相关研究回顾
*
三、算法设计与实现
** 1.A 安全距离规划策略* ** 1.B 理论探讨* ** *四、案例分析与实验结果
* 5.实验结果
* * A. 规划器实施
* B. 模拟
* C. 对PR2的测试
* 4.结论摘要
1. 介绍
无论是自动驾驶车辆还是家用服务机器人（如扫地机器人），这些机器人都需完成各种任务。为了实现从一处安全到达另一处的目的地，在动态障碍物环境中（即人、宠物和汽车等移动物体存在的情况下），机器人的导航系统必须具备高度的安全性。为了有效应对环境变化，在未来时间段内预测这些动态障碍物的位置变化是必要的前提条件。这些机器人的设计还必须具备预先规划好避开潜在威胁的能力，并能根据实时信息快速调整导航策略。而要实现这一目标，则需要设计出高效的路径规划方案，并能在复杂多变的环境下保持稳定运行能力。
改写说明
2. 相关工作
大多数动态障碍物处理方法都将被建模为静态障碍物，在其投影轨迹起始点附近会产生较高的计算成本和较短的时间窗口 [5]、[9]。正如文中所述，这些方案虽能奏效但可能带来高度次优或不完整的解决方案。另一种常见的策略是在规划路径时仅考虑动态障碍物（仍将其视为静止物体）并采用局部避障策略 [2]。这种方法可能导致局部最优解而非全局最优解的收敛结果。另一种选择是使用本地规划器结合速度障碍技术它通过最小化所需控制量与速度约束之间的差异来确定潜在碰撞风险 [13]。然而该方法仍可能陷入局部最优因为其策略过于贪心以减少计算开销
一些研究工作在时空搜索空间中进行全局规划 [10] 。Silver提出的 HCA* 算法专为多机器人环境设计但在论文中他指出该算法也可应用于动态环境中的路径规划问题在动态环境中HCA能够保证与我们相同的最优性并做出相同的假设条件在此实验中我们对比了与 HCA 算法的结果展示了显著的加速优势我们的算法之所以更快是因为其搜索空间规模显著小于 HCA* 的规模 HCA*及其相关算法会将每个（配置 时间步长）的状态作为一个独立节点由于时间步长数量通常较多导致搜索空间庞大而我们的算法则通过将连续的安全时间段划分为安全间隔并采用（配置 安全间隔）的形式缩减了状态空间规模从而显著降低了计算复杂度
与我们采用的方法相似的是文献 [12] 它认识到只有在安全时间段内最早到达的时间点才需要一个状态节点主要区别在于我们在评估从一个时间段到另一个时间段边缘的方式上采用了不同的策略 在文献 [12] 中 “扩展操作”被定义为探针沿单个时间步沿边移动后必须将其放回队列以便后续处理这会导致每次边操作都需要多次入队（而 A* 算法中的队列操作具有较高的代价）因此该文献的方法将每条 cost-n 边转化为 n 条 cost-1 边从而导致渐进运行时间呈线性增长 但这种方法仍然保持了与我们不同的核心特性
3. 算法
我们将安全区间被定义为配置中的一系列连续时间段，在这些时间段内没有任何碰撞事件，并且这些时间段前后各有一个时间单位与动态障碍物发生碰撞。值得注意的是，在某些情况下（如动态障碍物不再通过该配置），最后一个安全间隔可能会无限延伸。碰撞区间与安全区间相对应。碰撞区间被定义为配置中的一系列连续时间段，在这些时间段内每个时间单位都与动态障碍物发生碰撞，并且在这些时间段前后各有一个时间单位处于安全状态。每个空间配置（如图2所示）都对应着一条有序的时间线（图3），它只是一个交替出现的安全和碰撞区间列表。显然，在这种情况下不可能出现两个连续的安全区间（如图2所示），因为这将违反安全区间的定义（如图3所示），因为按照定义的安全区间应以相邻的安全区间的边界条件作为分隔符（如图1所示）。同样的逻辑也适用于碰撞区间。为了高效地表示搜索空间的时间维度并成为我们算法的关键思想之一，我们采用基于安全区间的表示方法：每个由（配置、间隔）对所表示的状态取代了传统方法中的多个单独状态——每个状态分别对应于一个时间单位的安全区间的状态描述。（如图4所示）这种表示方式使得状态空间明显减小了，并且使我们的搜索算法能够在更短的时间内找到解决方案。（如图5所示）
动态障碍物被用来表示我们所提出的算法基于外部系统对环境中的动态障碍物进行跟踪，并生成它们未来的运动轨迹，并以我们定义的一般形式进行编码。该算法生成一个动态障碍物列表，在这个列表中每个障碍物都具有半径和预估的运动轨迹。每个点都是包含状态变量的对象，在这些变量中记录了障碍物的配置、时间和与之相关的不确定性度量。所有轨迹按照从最早到最晚的时间顺序排列，在读取过程中能够清晰地观察到这些障碍物在未来一段时间内的运动预测情况。此外，在这种表示方案下还能处理多个可能的未来运动路径问题。然而，在本文中描述的方法虽然适用于多种假设情况下的多个假设轨迹分析，
但为了简化起见，
这里仅考虑每个障碍物只有一个预估路径的情况。
符号和假设
A. 规划安全间隔
图形构建 当规划器被初始化时，我们使用预测的动态障碍物轨迹为每个空间配置创建一个时间线。 这是通过沿每个动态障碍物的轨迹迭代每个点并更新该点碰撞距离内的所有配置的时间线来完成的。 这个碰撞距离是障碍物半径和机器人半径之和。 （半径也可以根据与该点相关的不确定性适当地膨胀。）
图搜索 初始化之后，我们运行 A* 搜索，如图 4 所示，除了如何获取状态的后继者（图 5）以及如何更新状态的时间变量（第 11 行，图 4）。
在图 5 中，函数 M(s) 返回可以从状态 s 执行的运动。 这些运动表明它们如何改变状态的空间变量。 这些动作也有执行它们所需的时间，我们将使用它来帮助确定我们可以在生成的配置中达到哪些安全间隔。 startTime(i) 返回安全区间 i 的开始时间和 endTime(i)，即安全区间 i 的结束时间。
当状态 s 扩展时，我们为其生成后继者，如图 5 所示。对于我们的机器人可以从 s 执行的每个动作，我们计算结果配置和执行时间（图 5 的第 3-5 行） . 然后对于这个新配置中的每个时间间隔，我们生成一个具有最早可能到达时间且沿运动没有任何碰撞的后继。 在生成后继者时，我们说 s 使用“等待和移动”动作。 这意味着对于新配置中的每个安全间隔，我们等待尽可能少的时间，以便在使用移动到新配置时，我们尽可能早地到达新的安全间隔。 到达时间与状态一起存储，但状态不会由它的时间值来标识，而仅由它的其他维度来标识。
在 A* 期间，只要找到到 s0 的更短路径，就会用较小的路径替换成本。 类似地，当这种情况发生时，它对应于更早到达 s0（因为成本等于时间），因此我们也将存储在状态 s0 中的时间值替换为这个新的更短时间（第 10-11 行，图 4）。 我们保证，当 s0 扩展时，我们已经找到了我们可以到达 s0 的最早时间。 这使我们能够安全地为 s0 的后继者生成和设置时间。
图 6 和图 7 显示了 SIPP 算法下的扩展示例。 在图 6 中，我们有两个灰色动态障碍物以每个时间步长一个单元格的速度沿箭头指示的方向移动。 我们将检查三个标记的配置（A、B、C）。 机器人位于配置 B 并且可以在每个时间步以 4 连接方式移动一个单元。 在图 7 中，我们显示了机器人当前状态 SB0 扩展期间的图表。 配置 B (SB1) 中的其他状态未显示，因为您无法在同一配置内的安全区间之间转换，因为机器人必须等待分离碰撞区间（使其无关紧要）。 在配置 B 中，机器人可以在 4 连接的网格上执行两个有效运动（上至配置 A，下至配置 C）。
A 中有两个可能的继任者。机器人可以立即移动到 A 并在安全间隔 0 内到达，一个时间步后 (SA0)。 A中的另一个后继者处于安全间隔1（SA1），但是这个间隔从时间7开始，我们只能在SB0安全地等待到时间4（之后我们会被向上移动的动态障碍物击中），进行这个过渡 无效的。 这种情况在图 5 中的第 9-10 行处理。因此，对于向上运动，我们将仅将 SA0 添加到我们的后继集。 SA0 的时间 (t) 为 1，因为移动一个单元需要一个时间步长，而 SA1 的 t 为 ∞，因为我们还没有找到任何有效路径到该状态。
对于配置 C 的向下运动，我们有三个可能的继任者。机器人可以立即向下移动到 C 并在安全间隔 0 (SC0) 一个时间步后到达（在两个动态障碍物到达 C 之前）。另一个后继者是 SC1，它仅在时间步 3 期间可用（在第一个动态障碍物通过之后，但在第二个动态障碍物到达之前）。要在时间 3 到达，机器人必须等待 2 个时间步才能移动到 C（这需要一个时间步）。这是我们“等待和移动”行动的一个例子。此转换的成本为 3，因为它需要很长时间才能执行。最后可能的后继者是 SC2，它是向上移动的动态障碍物通过后机器人（C）下方的状态。安全间隔对齐，这样机器人可以等到时间 4，然后移动到 C，在安全间隔 2 内到达 5。但是，在这种情况下，图 5 中第 11-13 行的碰撞检查将返回没有有效的转换。这种碰撞检查在机器人移动的两个状态之间进行插值，在这种情况下，将检测到机器人和动态障碍物通过彼此穿过来交换位置。因此，SC2 不是有效的继任者。
所以从SB0展开，后继者的集合是(SA0, SC0, SC1)。
B. 理论分析
在这里, 我们构建了我们算法在完整性和最优性方面的证明。
定理3：当某个特定类型的动态阻碍出现时，在这种情况下会发生n次类似的事件。由此可知，在这种情况下该系统能够承受最多n+1次连续的安全运行阶段。因为每次故障之后必然会出现一个恢复期，在这种情况下系统已经经历了n次恢复期的时间段。假设初始状态也是无问题的状态，则在第一次故障发生之前的一个恢复期内同样处于正常工作状态。这意味着系统总共经历了n+1次完整的恢复周期（包括初始的安全期）。因此，在所有可能的情况下（即各种不同的系统设计），系统的最大容错能力不会超过这一数值。这也就解释了为何通常情况下系统的容错能力非常有限：因为多数复杂系统中不会出现大量的同一类型障碍物同时影响同一个功能模块的情况
4. 例子
我们采用一个简明示例来说明该算法的基本流程。图9展示了初始环境与最优计划中的前三个操作步骤。在图9(a)中可以看出，在标记为R的起始单元格处有一个机器人，在目标位置G处设置了一个目标点。随着系统运行，在右侧出现了向左移动的动态障碍物的同时，在左侧则出现了向上方移动的动态障碍物。值得注意的是这些动态障碍物均以每个时间段移动一格的速度运行
图 9(b) 显示了最优计划第一次行动后的环境。 机器人右侧的单元格 c 只有一个安全区间。 立即向右移动，等待一个时间步然后向右移动都是安全的移动。 我们的算法每个（配置、间隔）对只存储一个状态，即最早可达时间的状态。 因此，立即向右移动是向右的单元格考虑的唯一动作。
图 9© 显示了最优计划第二次行动后的环境。 在这里，规划器的下一个动作将是向左移动到该单元格的第三个安全区间（在第二个动态障碍物通过之后）。 这意味着此操作将在原地等待一个时间步，然后向左移动。
图 9(d) 显示了最优计划第三次行动后的环境。 计划剩余3个动作全部向下，无需等待。
5.实验结果
在模拟环境与实际机器人系统中展开实验，在此基础上探讨其应用价值；同时结合动态环境下的路径规划需求进行深入研究。
A. 规划器实施
我们的测试域是一个四维空间（x,y,θ,time），其中前三维度（x,y,θ）用于生成满足最小转弯半径约束的平滑路径。这些动作组是一组短运动学上可行的运动序列[5]，用于如图10所示的格型规划器进行状态后继者搜索。针对我们的A*启发式算法，在执行16次二维Dijkstra搜索时，默认将机器人视为半径等于实际内接圆的圆形机器人。然而由于我们的格型规划器也将方向维度离散为16个方向，在每个单元格中所使用的启发式值会低估了目标的成本。这是因为2D搜索假定了机器人可以在任何位置自由旋转，并且其碰撞模型是基于其最窄直径进行设计的（如图10所示）。相比之下，在这种情况下计算效率更高是因为其搜索空间维度较低。不过它相较于常见的欧几里德距离启发式却能提供更多的信息量，因为这种启发式考虑到了静态障碍物信息。
B. 模拟
实验设计：我们采用了SIPP与HCA进行对比分析。Silver提出的HCA方法[10]在四维搜索空间（x,y,θ,time）中实现了路径规划。该算法基于从目标向后的低维搜索策略，在规划路径时排除了时间维度以及动态障碍的影响。在我们的实验中这是一个二维搜索空间（仅考虑x,y坐标）。在此信息引导下算法能够在包含时间维度并综合动态障碍因素的状态空间内规划出完整的路径方案。为了评估该算法的性能我们进行了两组随机实验：一组模拟室内环境、另一组模拟室外环境。每组实验均生成50个不同场景：室内场景大小为500x500单元格且θ方向划分为16个离散值；时间分辨率设定为每秒0.1秒；最佳路径持续时间为12.7秒（总计127个时间步）。机器人行动范围定义为单个单元格大小且每个场景都有独立设定起点与终点位置。在室外测试中我们创建了200个动态障碍物实例：每个障碍物占据若干单元格空间且其尺寸大小不一（采用随机分配）；障碍物初始配置完全随机化以模拟复杂自然环境（如树木、岩石等）。通过使用二维A*算法能够有效规划出避让较大阻碍物体的小型路径而轻松穿越狭窄区域这一现象可直观观察到：在室内环境中狭窄通道被全宽大尺寸物体阻塞而较窄通道则能容纳较小尺寸物体顺利通行（图11(a)）。相比之下室外开放地形允许大量圆形障碍物自由穿行形成了较为畅通的道路网络（图11(b)）。
结果：表 I 展示了室内实验的具体结果。计算所得的时间等于平均计划时间。安全间隔计划器成功解决了所有50次试验的问题方案；然而，在HCA算法中仅解决28次问题方案是因为其受限于5分钟的时间限制。因此，在表I中只有两个规划者都找到解决方案的28个测试案例被纳入结果分析之中。对于表II的数据，则显示安全间隔计划器在所有的50次户外试验中都能找到解决方案；而HCA算法则在这一条件下成功解决了其中47次的问题方案。我们的实验结果显示SIPP算法在两种环境测试中均优于HCA算法；但室内场景由于通道狭窄等因素导致机器人移动速度明显更快。值得注意的是，在这种情况下HCA算法也难以在限定时间内解决室内案例的问题方案主要原因是由于狭窄的走廊空间限制了机器人的活动范围；这导致机器人往往需要等待障碍物从通道口移除才能继续前进。
在户外环境中
由于开放区域广阔最佳路径通常不会涉及长时间等待因此机器人能够轻松绕过动态障碍物。
整个实验过程中我们发现每200个动态障碍物的安全间隔平均仅需0.4秒预先计算完成（当动态障碍物数量趋于合理水平时这一时间几乎可忽略不计如真实机器人测试所证实）
C. 对PR2的测试
为了验证SIPP的实际效果，在ROS平台实现了规划器的设计与部署，并将其应用于Willow Garage的PR2机器人进行功能测试。PR2作为一个移动操作机器人，配备了两个各有7个自由度的执行机构、一套机械平衡系统以及一个可360°旋转的全向底座，并搭载了一系列先进传感器：其中包括两套激光雷达系统：一个是固定于基座上的基架式扫描仪；另一个是倾斜式扫描仪；此外搭配多台摄像头设备以及集成了一套惯性测量模块以确保系统的稳定运行。
我们开发了一个全局规划器节点模块，并将其成功地集成到 ROS 导航框架中。该框架旨在为机器人提供精确的姿势信息以及环境空间地图数据，并预期能够生成完整的路径规划方案。为了实现这一功能需求，在原有的系统架构基础上增加了唯一的额外输入端口——用于接收预测性的人群动态轨迹数据。为此我们设计并实现了以下关键功能：首先通过激光雷达传感器对环境进行实时扫描采集；其次对激光雷达扫描得到的数据点进行基础聚类分析；随后与上一时刻的聚类结果进行对比分析；如果发现某个集群呈现出明显的移动轨迹特征则将其识别为动态障碍物并进入持续监测阶段；在每一迭代周期内我们采用恒定速度模型结合集群匹配方法持续更新其位置信息；为了提高预测精度系统还实现了历史轨迹数据积累功能：即对于每一个被识别出的动态障碍物集群系统会保存过去 2 秒至 4 秒内的历史轨迹数据作为建模依据；通过利用线性回归算法对这些历史数据进行深入分析从而能够准确地计算出动态障碍物移动速度向量；最后基于上述计算结果采用基本外推法方法对未来 20 秒内可能发生的障碍物位置变化情况进行精确预测
在我们的实验研究中发现：PR2机器人在一个狭窄走廊内需要经过一个人才能到达目标位置。观察者注意到该人正朝机器人移动，在此情况下规划者负责让出通道（图12），待其安全通过后机器人进入大厅继续行动。第二个案例中出现了一位缓慢前行的人（图13），起初走廊空间有限导致PR2必须等待直至通道变宽后才能通过他并最终超越他以最快的速度抵达目标点。第三个案例涉及两人从相反方向向PR2移动（图14），机器人的任务便是在这两人中间穿梭以便完成目标动作。在完成这三个实验后我们的统计数据显示平均计划时间为0.49秒并且预先计算所需的的安全间隔仅需极少时间（不到0.01秒）。所有真实机器人运行过程中的拍照均来自我们附带的视频资料
4.结论
在本文中