PAPER DEEP DIVE
动态环境中的四旋翼安全区间运动规划
本文提出安全区间运动规划框架，以时空可见图搜索、UTVD 拓扑分类和时空走廊 B 样条优化，使四旋翼在动态障碍中取得超过 95% 的成功率。
一句话总结：本文把“某条边在什么时段可通过”的安全区间放进动态连接可见图，再用时空拓扑分类筛选候选路径，并在真正的时空走廊内优化 B 样条，使四旋翼在动态障碍密集时仍能生成平滑、动力学可行的轨迹。
研究背景与问题
四旋翼在静态空间中规划轨迹的方法已经相对成熟，但低空配送、室内巡检和人机共处场景中的障碍物会运动。若仍把动态世界冻结成静态地图，系统只能依赖高频重规划来追赶环境变化；地图更新越快，并不自动意味着轨迹在下一时刻仍然安全。
一种直接办法是把时间加入状态空间，把三维路径搜索变成四维时空搜索。这个方向概念清楚，但搜索空间会随飞行时间轴迅速膨胀。障碍物只占用小部分时空区域，算法却可能在大量不可能发生冲突的位置和时间组合上重复展开。
安全区间路径规划通过合并连续安全的时间段来压缩搜索。传统工作多在栅格图上预计算每个格子的安全区间，在二维或较小环境中有效，扩展到三维后会遇到计算量和存储量问题。与此同时，若前端忽略速度、加速度边界，后端优化可能接到一条几何上可行、动态上却来不及执行的初始路径。
可见图与概率路线图适合表达多个几何绕行方案，也能减少规则栅格带来的冗余。问题在于，动态环境中的两条轨迹即使几何路径相同，只要穿越同一区域的时间不同，安全性质就可能完全不同。仅比较静态形状的同伦类别，会漏掉时间维度带来的新选择。
本文的出发点因此不是再增加一个局部避障代价，而是重新组织前端搜索：每条边必须拥有安全区间，相邻边的安全区间必须重叠，候选路径还要在不同的时空拓扑类中保持差异。后端只在由此形成的时空走廊内调整轨迹，让“先找可行绕行方案”和“后求平滑动力学轨迹”有明确分工。
预备概念：安全区间与统一时间可见变形
论文考虑完全观测的有限时间窗口 $T=[t_s,t_e]$，并假设动态障碍物的轨迹已知且有界。三维空间在任一时刻被划分为自由空间与障碍空间：
$$\mathbb{R}^{3}=\mathcal{X}_{\mathrm{free}}^{t}\cup\mathcal{X}_{\mathrm{obs}}^{t},\qquad \forall t\in T.$$
对图中的边 $e$，冲突时间集合 $\mathrm{CI}(e)$ 包含边与动态障碍相交的全部时刻。去掉冲突集合后，只有持续时间长于最小通行时间 $t_{\min}$ 的区间才被保留为安全区间：
$$(t_j,t_{j+1})\in T\setminus\mathrm{CI}(e),\qquad t_{j+1}-t_j>t_{\min}.$$
这里的时间长度约束不是细节。如果一条边只有零点几秒空档，而四旋翼按照速度与加速度约束至少需要更久才能通过，那么这个空档不能算真正可用。论文采用梯形速度曲线估算边的最短通行时间，从而在搜索阶段就保留低阶动力学可行性。
一条路径可以通行，还要求相邻两条边的安全区间有交集。这个交集描述“到达连接顶点时仍处于下一段的安全时段”。路径级的时间区间序列构成时间走廊；它不是固定不变的时间表，而是允许后续在区间内重新分配时间的一条可行通道。
在静态环境中，统一可见变形通过一条连续可见线段判断两条路径是否属于同一类。动态环境里，路径起点和终点可能落在不同时间，不能直接套用。本文提出统一时间可见变形（UTVD），先对两条轨迹的参数做线性映射 $s'=\alpha s+\theta$，再要求相应采样点之间的线段在对应时间范围内无碰撞：
$$s'=\alpha s+\theta,\qquad \sigma_1(0)=\sigma_2(\theta),\qquad \sigma_1(1)=\sigma_2(\alpha+\theta).$$
这一定义同时覆盖空间形状和时间平移。图 2 用同一几何区域中的多条候选路线说明：红色与蓝色路线可按 UTVD 归为同类，绿色路线因撞上不同障碍物、发生在不同时间段而属于另一类。算法因此不会只保留形状不同的路线，也会区分“同一形状、不同通过时刻”的路线。
方法一：带边级安全区间的动态连接可见图
前端把静态地图表示为三维占据图，把动态障碍物表示为带多项式轨迹的有界椭球。对于图中的每条候选边，构造包住该边的长方体，再用四旋翼与障碍物的闵可夫斯基和进行膨胀。障碍轨迹与长方体的交点被转换成冲突时间段，最后得到每条边的安全区间。
这种边级检查比顶点级检查更保守，也更适合长边。只在顶点处检测碰撞会留下一种漏洞：两个时刻的端点都在自由空间，连接它们的边却可能在中途穿过障碍。本文明确把安全区间定义在边上，从而把路径安全与图中的连接关系绑定。
图由 Guard 与 Connector 两类顶点组成。起点和终点先作为 Guard；靠近两个 Guard 的采样点可以成为 Connector，并连接两条具备时间重叠的边。若一个新采样无法与两个 Guard 建立有效连接，则它本身成为 Guard。采样区域、连接半径和时间预算共同决定图能否在有限时间内形成足够多的方案。
论文用优先探索连接器稀疏区域的启发式采样减少无效点。构建过程中会检查新路径与邻居路径的 UTVD 等价性：若同类且新路径更短，就替换旧顶点；若属于新类别，则保留为新 Connector。这个在线筛选与只采样几何空间的概率路线图不同，它把时变安全和拓扑差异一起放进图的结构。
图建成后，深度优先搜索找出多个不同拓扑类的路径。对每条路径，时间参数化采用“尽早到达各顶点”的规则，形成初始时间分配。图 3(b) 的仿真显示，绿色图结构可以同时保留多条黄色候选路线，而不是只返回第一条无碰撞路径。
公开实现把四维状态直接存入图节点，节点包含位置、时间和可用时间区间；边查询会按端点、时间范围与参数缓存结果。下面的缓存逻辑对应论文“边级安全区间生成”，也解释了多查询时为什么不必对相同线段反复求解：
// src/kr_dyn_planner/plan_core/include/path_searching/dyn_topo_prm.hpp:572
inline bool DynTopoPRM::findSafeIntervalsForLineCached(
const Eigen::Vector3d& pt1, const Eigen::Vector3d& pt2,
const Eigen::Vector2d& time_range,
std::vector<Eigen::Vector2d>& intervals) const
{
const LineQueryKey key = makeLineQueryKey(pt1, pt2, time_range);
const auto cached = line_interval_cache_.find(key);
if (cached != line_interval_cache_.end()) {
intervals = cached->second.intervals;
return cached->second.valid;
}
LineQueryValue value;
value.valid = CollisionChecker_->findSafeIntervalForLine(
pt1, pt2, time_range, value.intervals,
speed_limit_, acc_limit_);
line_interval_cache_.emplace(key, value);
intervals = value.intervals;
return value.valid;
}缓存键不仅包含两个三维端点，还量化时间区间。这个选择很关键：同一条物理线段在不同搜索起点时间下具有不同的障碍占用模式，只按空间端点缓存会返回错误的安全区间。把时间纳入键，保留了动态问题的语义。
方法二：把初始路径膨胀成时空走廊
前端路径由若干几何边组成，如果直接把轨迹限制在边的细线上，后端几乎没有避让余量，也无法利用障碍物之间的空闲空间。本文沿每条边选择连续种子点，在三维方向逐轴膨胀长方体；每扩张一次都检查该长方体在候选时间区间内是否与动态障碍冲突。
时间区间也在同时膨胀。空间长方体只有在相应时段内无碰撞才算有效，所以最终得到的是成对的 $(\text{空间盒},\text{时间区间})$，而不是静态走廊事后附加一个时间标签。多个盒依次覆盖整条路径，相邻盒在空间上必须连通，在时间上必须重叠，避免优化器找到一条在三维上接得上、在时间上却断开的轨迹。
这一步还有一个工程价值：后端可以围绕初始路径获得足够自由度。平滑项希望减少急动，动态障碍项希望远离椭球，时间走廊项希望控制点留在盒内。若只使用细线路径，第一项与安全项会过度对抗；时空走廊提供了可行域，让优化在“安全但粗糙”与“平滑但冒险”之间寻找折中。
论文对时空走廊采用轴对齐包围盒，因为盒子容易进行碰撞查询和距离代价计算。这个表示会带来一些保守性，尤其是在斜向狭窄通道中；但它保证约束可检查、梯度可定义，也适合在有限时间预算内完成在线优化。
方法三：在走廊内优化 B 样条
四旋翼是微分平坦系统，轨迹及其高阶导数可以由少数平坦输出表示。本文采用均匀 B 样条，以控制点与节点间距描述连续轨迹。B 样条的凸包性质让控制点上的约束能够影响曲线本身，适合表达走廊、速度与加速度边界。
总体目标由控制代价、动态障碍碰撞代价、时空走廊代价和动力学可行代价组成：
$$\min_{\mathbf Q,\mathbf t}\sum_{d\in D}\lambda_d J_d(\mathbf Q,\mathbf t),\qquad D=\{c,od,ct,f\}.$$
其中 $\mathbf Q$ 是控制点，$\mathbf t$ 是节点向量，$\lambda_d$ 控制各项权重。问题同时优化轨迹形状与时间分配；实际实现采用固定节点下的控制点优化，再迭代调整时间，最终从多个拓扑候选轨迹里选择控制代价最小的一条。
由相邻控制点差分可以得到速度、加速度与急动控制点：
$$\mathbf V_i=\frac{\mathbf Q_{i+1}-\mathbf Q_i}{t_s},\quad \mathbf A_i=\frac{\mathbf V_{i+1}-\mathbf V_i}{t_s},\quad \mathbf J_i=\frac{\mathbf A_{i+1}-\mathbf A_i}{t_s}.$$
控制代价惩罚内部急动控制点的平方二范数，用来减少轨迹的剧烈变化和能量消耗：
$$J_c=\sum_{i=p_b-3}^{N_c-p_b}\|\mathbf J_i\|_2^2.$$
动态可行项对速度与加速度相对给定边界 $v_m$、$a_m$ 的偏差施加惩罚。论文中的目标保持低阶边界，随后用高阶优化修正 B 样条：
$$J_f=\sum_{i=p_b-1}^{N_c-p_b}\|\mathbf V_i-v_m\|_2^2+\sum_{i=p_b-2}^{N_c-p_b}\|\mathbf A_i-a_m\|_2^2.$$
走廊内的静态几何约束由控制点到长方体上下界的距离表达。若控制点落在盒内，距离为零；越界则产生线性惩罚：
$$J_{ct}=\sum_{i=p_b}^{N_c-p_b}\left(\|b_{l,j}-\mathbf Q_i\|_1+\|\mathbf Q_i-b_{u,j}\|_1\right).$$
动态障碍代价不是简单的点中心距离，而是在障碍椭球定义的度量中计算。设 $p_i$ 是轨迹采样点，$o_j$ 是障碍中心，$E_j$ 描述椭球尺度，则归一化距离为：
$$d(p_i,o_j)=\left\|E_j^{-1}(p_i-o_j)\right\|_2.$$
当该距离小于阈值 $d_{th}$ 时，对所有采样点和动态障碍求和：
$$J_{od}=\sum_{i=0}^{m}\sum_{j=0}^{k}\left(\max\left(0,d(p_i,o_j)-d_{th}\right)\right)^2.$$
这组代价必须一起看。走廊项限制整体可行域，动态障碍项处理走廊内部仍可能过近的局部风险，平滑项与可行性项保证最终曲线能被控制器执行。若只有走廊项，优化可能贴边；若只有障碍项，局部排斥会把轨迹推离时空逻辑，两者结合才形成本文的两阶段结构。
flowchart TD
A[Static map + moving obstacle trajectories] --> B[Inflate each graph edge]
B --> C[Compute edge collision intervals]
C --> D[Keep intervals longer than t_min]
D --> E[Build Guard-Connector visibility graph]
E --> F[Check UTVD between candidate paths]
F --> G[Search distinct spatial-temporal paths]
G --> H[Inflate cuboids along each path]
H --> I[Check connected space-time corridors]
I --> J[Optimize B-spline control points]
J --> K[Smoothness + feasibility + obstacle + corridor costs]
K --> L[Select minimum-control-cost trajectory]
源码中的主流程与论文结构一致：先调用拓扑路径搜索，再取得每个候选路径对应的走廊与时间区间，过滤掉空间或时间不连通的候选，最后传给后端优化。下面这段代码是论文框架最直接的实现映射：
// src/kr_dyn_planner/plan_core/src/planner_manager.cpp:335
bspline_optimizer_rebound_->dyn_topo_prm_->findTopoPaths(
local_start_pt, local_end_pt, world_start_time, duration,
start_pts, start_ts, topo_graph, topo_paths);
if (topo_paths.empty()) {
ROS_ERROR("No path found.");
return false;
}
std::vector<std::vector<Eigen::Vector2d>> topo_sfc_intervals;
std::vector<std::vector<Eigen::Matrix<double, 3, 2>>> topo_sfc_cuboids;
bspline_optimizer_rebound_->dyn_topo_prm_->getSFC(
topo_sfc_cuboids, topo_sfc_intervals);
for (size_t i = 0; i < candidate_count; ++i) {
if (!validateConnectedSFC(topo_sfc_cuboids[i],
topo_sfc_intervals[i],
spatial_overlap_min,
time_overlap_min, &reason)) {
continue;
}
}后端实现把平滑、可行性、动态障碍距离与走廊归属四项代价及其梯度合并到同一个目标函数。图 5 的仿真把优化后的粉红色轨迹与黄色时空盒叠加显示，直接展示了轨迹如何利用走廊重新分配通过时刻。
实验设计与前端对比
仿真把地图复杂度分为稀疏、中等、稠密三级。占据密度分别为 $[0,0.01]$、$[0.05,0.1]$、$[0.15,0.2]$，动态障碍数量分别为 $[0,20]$、$[20,40]$、$[40,60]$。每个环境执行 100 次试验，并且每三次试验重新生成一次地图，以降低数次试验共享同一随机地图造成的偏置。
起终点在无碰撞位置随机生成。只要四旋翼找到从起点到目标区域且不与静态、动态障碍冲突的轨迹，就记一次成功。前端对照包括 T_PRM、VIS_PRM、SIPP 和 SIPP_IP；为了比较动态可行性，部分概率路线图版本加入梯形速度曲线检查，并以带 dyn 标记的版本呈现。
| 环境 | 占据密度 | 动态障碍数 | 试验次数 | 地图更新方式 |
|---|---|---|---|---|
| 稀疏 | [0, 0.01] | [0, 20] | 100 | 每 3 次试验重建 |
| 中等 | [0.05, 0.1] | [20, 40] | 100 | 每 3 次试验重建 |
| 稠密 | [0.15, 0.2] | [40, 60] | 100 | 每 3 次试验重建 |
图 6 汇总前端方法在成功率、计算时间、路径长度和飞行时间上的表现。本文方法在三个密度级别均取得更高成功率，飞行时间也保持在较低水平。作者对结果的解释是：基于栅格安全区间的方法需要较重的预处理，稠密地图下扩展代价上升；T_PRM 计算快，但为了用顶点碰撞近似整条边安全，只能限制边长度，路径因此更绕；VIS_PRM 在高动态障碍密度下更容易失去成功率。
加入动力学检查后的结果也值得区分。T_PRM(dyn) 的成功率下降、飞行时间增加，说明“限制最大边长”并不能充分保证时间参数化可行；VIS_PRM 给顶点随机时间戳，往往天然留下足够长的通行时段，因此加入检查后的变化不显著。SIPP_IP 的计算时间最长，并常在达到最大扩展顶点数后停止。
轨迹质量与实机验证
后端比较对象 TPRMO 使用时间概率路线图前端，并在优化中计入到动态障碍的欧氏距离，但不使用时空走廊。作者在三个环境各做 100 次单次优化试验，重点观察成功率和平均急动代价，结果见表 I。
| 环境 | 方法 | 成功率 | 轨迹长度 | 飞行时间 | 平均控制代价 |
|---|---|---|---|---|---|
| 稀疏 | 本文 | 100% | 7.66 m | 6.04 s | 17.71 m²/s⁵ |
| 稀疏 | TPRMO | 99% | 7.89 m | 5.55 s | 72.67 m²/s⁵ |
| 中等 | 本文 | 98% | 7.93 m | 6.05 s | 26.20 m²/s⁵ |
| 中等 | TPRMO | 97% | 8.11 m | 5.72 s | 73.27 m²/s⁵ |
| 稠密 | 本文 | 97% | 7.44 m | 5.69 s | 30.53 m²/s⁵ |
| 稠密 | TPRMO | 97% | 7.52 m | 5.36 s | 70.31 m²/s⁵ |
最重要的差异不是成功率只高出一个或两个百分点，而是控制代价显著下降。本文在稀疏、中等、稠密环境中的平均控制代价分别为 17.71、26.20、30.53，TPRMO 分别为 72.67、73.27、70.31。直接计算可见降幅约为 75.6%、64.2% 和 56.6%。代价越低，轨迹的急动变化越少，对姿态跟踪和机体执行更友好。
本文轨迹平均比 TPRMO 短 1.1% 至 2.9%，但飞行时间长 5.8% 至 8.8%。这不是矛盾，而是控制策略的取舍：算法愿意在时空走廊内多花一点时间换取更平滑的加减速和更安全的间隔。稠密环境中两者成功率同为 97%，说明本文的主要优势在该工况下体现为轨迹质量，而不是把成功率继续抬高。
| 环境 | 控制代价降幅 | 轨迹长度变化 | 飞行时间变化 | 成功率变化 |
|---|---|---|---|---|
| 稀疏 | 约 75.6% | 缩短约 2.9% | 增加约 8.8% | 99% → 100% |
| 中等 | 约 64.2% | 缩短约 2.2% | 增加约 5.8% | 97% → 98% |
| 稠密 | 约 56.6% | 缩短约 1.1% | 增加约 6.2% | 97% → 97% |
实机实验使用两台 Scarab 地面机器人作为运动障碍，每台安装 $0.91\,\mathrm{m}\times0.16\,\mathrm{m}$ 的圆柱体，并跟踪矩形轨迹。环境中另设三个 $1.2\,\mathrm{m}\times0.3\,\mathrm{m}$ 的静态圆柱。四旋翼采用定制 Dragonfly 230，搭载 VOXL 飞控、前向飞行时间相机和下视跟踪相机，Vicon 提供公共参考系与里程计。
测试中，四旋翼从右下角起飞，在地面机器人持续运动时飞向黑色箱体上方的目标。图 7 的右上角显示规划器构造的时空走廊与最小控制代价轨迹，后续分帧给出实际飞行过程。实验规模不大，但它验证了仿真中的核心链条能在真实四旋翼上闭合，而不是只停留在离线优化。
图 2 的俯视记录进一步说明了规划触发时刻的几何关系：红色区域对应运动中的地面机器人，黑色区域对应静态障碍，绿色箭头是里程计方向。规划器不只是对最近障碍作反射式排斥，而是用未来一段时间的占用关系生成绕行顺序。
理论边界与工程含义
论文在“环境可以被图完整表示”的假设下讨论完备性与最优性。SIPP 在栅格图和给定动作集合上具有完备与分辨率最优性质；本文把边级安全区间、低阶动力学和概率路线图结合后，给出的是有限时间内的概率完备与概率最优，而不是无条件的绝对保证。
这个理论定位意味着两件事。其一，增加采样时间、采样数和候选路径数，找到可行拓扑类的概率会提高；如果采样预算过小，图本身就可能没有表达出真正可行的通道。其二，最优性建立在候选图能够覆盖相应路径的前提上，优化器只能在已有走廊里改进轨迹。
前端使用梯形速度曲线估算最小通行时间，对四旋翼而言是一个保守的低阶近似。它能让安全区间与速度、加速度上限发生联系，却没有在搜索阶段完整求解高阶动力学。后续 B 样条优化负责补齐高阶平滑与可行性，因此最终轨迹仍可能因时间节点过紧、走廊过窄或权重配置而优化失败。
动态障碍被建模为轨迹已知的椭球，这是相对友好的设定。真实感知会带来检测延迟、中心位置误差、尺度误差和轨迹预测误差。安全区间对这些误差非常敏感：一个边界误判就可能把冲突时段切成错误的空档。作者在结论中明确把“接入机载感知”和“处理更复杂运动模式”列为未来工作。
实机定位依赖 Vicon，运动障碍沿预先设定的矩形轨迹行驶，与完全未知、反应式移动的人群仍有差距。论文的公开发布增强了可复核性，但也应区分当前代码与后续版本：本次核对的仓库提交为 401262c，实现基于 Ego-Planner 代码结构，并使用参数化动态地图模块。复现实验时应记录提交版本、参数文件、地图范围与障碍轨迹，否则成功率数字不能直接横比。
局限分析
第一项局限来自作者自述：方法尚未接入完整的机载感知系统，实验依赖预先获得或外部系统提供的障碍轨迹；面对复杂、不可预测的运动模式，鲁棒性仍待验证。对本文而言，这不是单纯把一个视觉模型接到规划器前面就能解决，还需要把状态估计不确定性传播到安全区间与碰撞代价中。
第二项局限来自图的表达能力。概率路线图的采样覆盖、最大采样数、连接半径和提前停止条件都会改变可找到的拓扑类。论文的理论结论以“环境可被图完整表示”为前提，实际系统只能接近这个前提。若真通道需要极窄或极长的边，有限采样可能错过它。
第三项局限是时间与安全的耦合仍依赖已知未来。障碍轨迹的多项式表示简化了几何碰撞计算，却不代表预测可靠。对四旋翼这类高速、欠驱动系统，预测误差可能直接消耗预留的安全间隔。未来若把预测分布纳入优化，单一确定轨迹的安全区间就需要变成鲁棒或机会约束。
第四项局限是实验覆盖。仿真每个密度级执行 100 次，具有统计意义；实机只报告一组包含两个运动障碍和三个静态障碍的典型实验，没有给出大量实机成功率、重复次数或不同光照与通信条件。它足以证明系统可运行，却不足以量化真实场景的可靠性。
结论与展望
本文的贡献可以概括为把三个原本容易分离的层次真正接在一起：边级安全区间提供时间可行的图连接，UTVD 区分动态环境中的时空拓扑类，时空走廊把每个初始绕行方案转成后端可优化的安全区域。三者共同避免了“前端找到几何路、后端来不及飞”和“局部代价能避障、整体落入同一绕行类”两类问题。
从机器人系统角度看，最值得借鉴的是表示方式。把安全区间附着在图上，而不是只给整张地图打时间戳；把时间区间和空间盒成对膨胀，而不是先做静态走廊再补时间；把多个拓扑候选交给后端择优，而不是让一次非线性优化承担全部探索。这些设计对地面机器人、机械臂和高空作业平台也有迁移价值。
下一步最自然的方向是把确定轨迹假设替换为带不确定性的预测，把机载感知延迟纳入规划时域，并在大规模实机实验中统计不同运动障碍密度下的成功率。若这些环节能闭合，安全区间方法就不只是一个离线轨迹生成器，而会成为动态环境中可审计、可解释的实时规划组件。
时空走廊的价值，不是把轨迹塞进盒子，而是把“何时通过哪里”变成可搜索、可优化、可验证的同一个问题。
资料核对：论文 arXiv v2 全文、IEEE ICRA 2025 出版信息、作者提供的 SIMP 仓库与视频。实验数字均取自论文表 I 和正文描述，派生百分比为本文基于原表计算。
