文献：SANDO: Safe Autonomous Trajectory Planning for Dynamic Unknown Environments
期刊：IEEE Transactions on Robotics（T-RO / TRO，2026） ；
作者：Kota Kondo, Jesús Tordesillas, Juan Rached, Lili Sun, Yixuan Jia, Jonathan P. How
研究机构：MIT Aerospace Controls Laboratory；Comillas Pontifical University / TRAIL
关键词：Dynamic Unknown Environments（动态未知环境）、STSFC（时空安全飞行走廊）、Heat Map A（热力图 A ）、MIQP（混合整数二次规划）、Continuous-Time Safety（连续时间安全）、Variable Elimination（变量消元）**
作者主页（期刊状态）：https://kotakondo.github.io/
1. 文献核心结论
SANDO 讨论的不是机械臂全身控制，也不是端到端强化学习，而是无人机如何在没有先验地图、动态障碍运动未知、环境中存在狭窄通道的条件下，持续生成可实时求解且满足明确安全假设的轨迹。它解决的核心矛盾是：软约束动态避障方法速度快，但不能严格保证整条轨迹不碰撞；传统硬约束方法较安全，却可能因最坏情况障碍膨胀过大而失去可行性，或因求解时间太长而无法实时重规划。
论文提出三项彼此配合的主要技术：Heat Map-based A（软风险全局引导） *、Spatiotemporal Safe Flight Corridor（STSFC，按轨迹时间段构造的硬安全走廊） 、Variable-Eliminated MIQP（消元降维后的硬约束轨迹优化）。动态障碍的运动预测主要服务于软风险路径引导，而连续时间安全所需的约束，依赖已知速度上界、尺寸上界与定位误差界构建的最坏情况可达集。
可以将技术主线概括为：
*点云输入与时序占据地图 → 动态目标跟踪 / AEKF → 静态和动态 Heat Map → 三维体素 A 全局路径 → 按时间层构造 STSFC → Bézier 轨迹与 MIQP 硬约束优化 → PX4 轨迹跟踪 → 持续重规划。**
最重要的是区分两种不同的保证：SANDO 给出的是在满足明确假设时，已经规划出的有限时间轨迹的连续时间安全性，而非保证所有未来重规划问题都有解，也非无条件的真机绝对安全加粗样式**。在有真实障碍状态的动态基准中三档难度均达到 100% 成功率；但感知闭环的 Hard 场景只有约 40%，说明实际性能高度依赖早期动态目标发现与估计。
2. 研究背景与核心科学问题
SANDO 研究的是一个比普通静态避障更困难的问题：无人机需要在没有先验地图的三维环境中高速飞行，同时环境中还存在位置、速度和未来运动均未知的动态障碍物。
在静态未知环境中，规划器通常可以假设"已经观测为空闲的区域之后仍然为空闲"；但在动态环境中，这一假设不成立。一个刚刚生成的无碰轨迹，可能因为行人、移动机器人或其他动态障碍物进入轨迹区域而在很短时间后变得不安全。因此规划器不仅要快速重规划，还要同时回答两个问题：
- 空间上走哪里？------路径不能过分贴近静态/动态障碍物，否则后续可行走廊会变窄甚至消失；
- 时间上什么时候经过？------同一片空间在不同时间的安全性不同，动态障碍物的可达区域随时间增长。
现有方法大致存在三类局限：未知静态环境规划器速度快，但不能正确处理动态障碍；动态环境中的软约束方法可以很快地绕障，但碰撞约束只是代价函数，不能给出严格安全保证；硬约束动态规划方法虽然更安全，但不少方法只在离散 MPC 节点或采样点上检查碰撞，而且计算量大、在狭窄动态环境中容易过于保守。
SANDO 的核心目标可概括为：
U n k n o w n D y n a m i c E n v i r o n m e n t s ⏟ S e t t i n g ⟹ C o n t i n u o u s - T i m e S a f e t y ⏟ G o a l \underbrace{\mathrm{Unknown\;Dynamic\;Environments}}{\mathrm{Setting}}\;\Longrightarrow\;\underbrace{\mathrm{Continuous\!\!\text{-}\!Time\;Safety}}{\mathrm{Goal}} Setting UnknownDynamicEnvironments⟹Goal Continuous-TimeSafety
这也是论文真正解决的关键矛盾：如何在"严格安全约束"和"足够快、不过度保守"之间取得平衡。
表 1 SANDO 与典型相关规划器的定位（根据原文 Table I 整理）
| 方法 | 动态环境 | 狭窄环境 | 安全约束形式 | 主要局限 |
|---|---|---|---|---|
| EGO-Planner | 否 | 是 | 软约束 | 假设环境静态 |
| FASTER | 否 | 是 | 连续时间硬约束 | 只考虑静态障碍 |
| PANTHER | 是 | 主要为开放环境 | 连续时间约束 | 障碍物数量增加时计算开销较大 |
| FAPP | 是 | 是 | 软约束 | 无形式化连续时间安全保证 |
| OA-MPC / I-MPC | 是 | 是 | 离散 MPC 节点硬约束 | 节点之间仍可能发生碰撞 |
| SANDO | 是 | 是 | 连续时间硬约束 | 依赖速度界、估计误差界等假设 |
图 1（SANDO 原文 Fig. 1）软约束方法、传统硬约束方法与 SANDO 的概念对比：SANDO 不使用整个预测时域的统一最坏情况膨胀，而是按照轨迹经过该区域的时间层进行动态障碍物可达集膨胀，从而在保持安全性的同时降低保守性。 来源：Kondo et al., SANDO, arXiv:2604.07599v3, Fig. 1。 查看原图
图 1 已经给出了论文最核心的思想。若把动态障碍物在整个规划时域内可能到达的所有位置一次性全部膨胀，虽然安全，但可行空间会被大量吞噬；如果只把障碍物作为软代价，又可能得到速度快但没有严格安全保证的轨迹。SANDO 的做法是引入 Spatiotemporal Safe Flight Corridor（STSFC，时空安全飞行走廊） ：第 n n n 个轨迹段只需要避开第 n n n 个时间层内障碍物能够达到的区域，而不是避开整个时域的最坏情况区域。
3. SANDO 的总体技术路线
SANDO 不是单独的局部规划器，而是一套完整的 感知---全局引导---时空走廊---硬约束轨迹优化---执行重规划 系统。其主要模块包括：
- Dynamic Obstacle Tracker（动态障碍跟踪器）：从点云中检测动态体、聚类、关联，并用 AEKF 估计位置、速度和加速度；
- Heat Map Generator（热力图生成器）：在静态障碍和动态障碍周围构造连续风险代价；
- Heat Map-based A Global Planner（基于热力图的 A 全局规划器）**：在体素地图上搜索尽量远离高风险区域的全局参考路径；
- STSFC Generator（时空安全飞行走廊生成器）：对每一个时间层建立一组凸多面体安全区域；
- Hard-constrained MIQP Trajectory Optimizer（硬约束混合整数二次规划轨迹优化器）：把每段 Bézier 轨迹分配到对应时间层的安全多面体，并严格限制速度、加速度和 jerk。
图 2（SANDO 原文 Fig. 2）SANDO 系统总体框架。点云同时进入地图管理与动态障碍跟踪模块；动态障碍预测用于热力图和 STSFC 构造；A 提供全局路径，STSFC 提供时变凸安全区域，最终由硬约束 MIQP 生成安全轨迹。 来源：Kondo et al., SANDO, arXiv:2604.07599v3, Fig. 2。 查看原图*
从规划层次上看，可以把 SANDO 写成：
Point Cloud → Dynamic Detection/Tracking → Voxel Map + Heat Map → A ∗ Global Path → STSFC → MIQP Trajectory → PX4 Tracking . \text{Point Cloud} \rightarrow \text{Dynamic Detection/Tracking} \rightarrow \text{Voxel Map + Heat Map} \rightarrow A^*\text{ Global Path} \rightarrow \text{STSFC} \rightarrow \text{MIQP Trajectory} \rightarrow \text{PX4 Tracking}. Point Cloud→Dynamic Detection/Tracking→Voxel Map + Heat Map→A∗ Global Path→STSFC→MIQP Trajectory→PX4 Tracking.
其中一个很重要的设计是：**全局路径层用软风险代价，局部轨迹层用硬安全约束。*前者负责"尽量不要走到以后难优化的地方"，后者负责"真正执行的轨迹必须安全"。这比单纯把所有障碍直接硬膨胀到 A 占据栅格中更合理，因为过度硬阻塞可能让下一次重规划连起点都落在不可行区域内。
4. STSFC：时空安全飞行走廊
4.1 从普通 SFC 到时空 SFC
普通 Safe Flight Corridor（SFC）只表示空间中的凸自由区域。对静态障碍而言，只要轨迹始终位于这些凸多面体内，就可以保证不碰撞；但动态环境中，同一个空间区域在 t 0 t_0 t0 时刻可能安全，在 t 0 + 1 s t_0+1\,\mathrm{s} t0+1s 时可能已经被动态障碍占据，因此必须给安全走廊增加时间维度。
SANDO 将 STSFC 写成二维数组：
C n p , \mathcal Cnp, Cnp,
其中：
- n ∈ { 0 , ... , N − 1 } n\in\{0,\ldots,N-1\} n∈{0,...,N−1} 表示时间层 ，与第 n n n 段轨迹一一对应；
- p ∈ { 0 , ... , P − 1 } p\in\{0,\ldots,P-1\} p∈{0,...,P−1} 表示该时间层中的空间多面体编号。
每个凸多面体表示为：
C n p = { x ∈ R 3 ∣ F n p x ≤ g n p } . \mathcal Cnp=\left\{\mathbf x\in\mathbb R^3\mid \mathbf F_{np}\mathbf x\leq \mathbf g_{np}\right\}. Cnp={x∈R3∣Fnpx≤gnp}.
第 n n n 个时间层覆盖：
t 0 + n d t , t 0 + ( n + 1 ) d t . t_0+n\\,dt,\\;t_0+(n+1)dt. t0+ndt,t0+(n+1)dt.
因此，规划器不再问"这段空间是不是安全"，而是问：机器人在第 n n n 个时间段经过这片空间时，它是否安全？
4.2 已跟踪动态障碍物的可达集膨胀
对第 k k k 个动态障碍物，在当前规划时刻 t 0 t_0 t0 得到其估计 AABB 为 O ^ k ( t 0 ) \hat{\mathcal O}_k(t_0) O^k(t0)。SANDO 对第 n n n 个时间层构造：
O ^ k n = O ^ k ( t 0 ) ⊕ R n ⊕ C ∞ ( ϵ ) ⊕ D . \hat{\mathcal O}_k^n =\hat{\mathcal O}k(t_0) \oplus \mathcal R_n \oplus C\infty(\epsilon) \oplus \mathcal D. O^kn=O^k(t0)⊕Rn⊕C∞(ϵ)⊕D.
这里：
- R n \mathcal R_n Rn：动态障碍在第 n n n 个时间层结束前的最坏情况可达集；
- C ∞ ( ϵ ) C_\infty(\epsilon) C∞(ϵ)：位置估计误差缓冲；
- D \mathcal D D：无人机自身 AABB 尺寸；
- ⊕ \oplus ⊕：Minkowski Sum（闵可夫斯基和）。
论文给出了两种速度界形式。
欧氏范数速度界：
∥ c ˙ k ( t ) ∥ 2 ≤ v max o b s , \|\dot{\mathbf c}_k(t)\|2\leq v{\max}^{\mathrm{obs}}, ∥c˙k(t)∥2≤vmaxobs,
对应：
R n = B 2 ( v max o b s ( n + 1 ) d t ) . \mathcal R_n=B_2\left(v_{\max}^{\mathrm{obs}}(n+1)dt\right). Rn=B2(vmaxobs(n+1)dt).
逐轴速度界：
∣ c ˙ k i ( t ) ∣ ≤ v max , i o b s , i ∈ { x , y , z } , |\dot c_k^i(t)|\leq v_{\max,i}^{\mathrm{obs}},\qquad i\in\{x,y,z\}, ∣c˙ki(t)∣≤vmax,iobs,i∈{x,y,z},
对应轴对齐盒：
R n = { δ : ∣ δ i ∣ ≤ v max , i o b s ( n + 1 ) d t } . \mathcal R_n=\left\{\boldsymbol\delta:\;|\delta_i|\leq v_{\max,i}^{\mathrm{obs}}(n+1)dt\right\}. Rn={δ:∣δi∣≤vmax,iobs(n+1)dt}.
SANDO 对已跟踪障碍实际采用逐轴形式，因为障碍物本身已经用 AABB 表示，逐轴膨胀既紧凑又计算方便。
关键点在于 R n \mathcal R_n Rn 随 n n n 增大。早期时间层的障碍膨胀小，后期时间层的膨胀大，因此不会像"整个时域统一使用最大膨胀"那样把大量早期自由空间浪费掉。
4.3 未知区域也必须考虑"可能突然出现的动态障碍"
只对已经跟踪到的动态障碍做可达集膨胀还不够。无人机前方存在视野盲区或从未观测过的体素，动态障碍完全可能从未知区域中突然出现。
SANDO 将未知空间记为 U ( t 0 ) \mathcal U(t_0) U(t0)，其边界体素为 B U ( t 0 ) \mathcal B_{\mathcal U}(t_0) BU(t0)，并构造：
U ^ n = U ( t 0 ) ∪ ( B U ( t 0 ) ⊕ R n ϵ ⊕ B 2 ( r d r o n e ) ) . \hat{\mathcal U}^n =\mathcal U(t_0) \cup \left( \mathcal B_{\mathcal U}(t_0) \oplus \mathcal R_n^{\epsilon} \oplus B_2(r_{\mathrm{drone}}) \right). U^n=U(t0)∪(BU(t0)⊕Rnϵ⊕B2(rdrone)).
这里 R n ϵ \mathcal R_n^{\epsilon} Rnϵ 把动态障碍速度界与位置误差界统一考虑， B 2 ( r d r o n e ) B_2(r_{\mathrm{drone}}) B2(rdrone) 用于加入无人机自身尺寸。
图 3（SANDO 原文 Fig. 3）未知空间膨胀与安全子目标选择。左侧目标位于已观测区域，可直接在膨胀后的未知边界外生成 STSFC；右侧目标进入未知区时，规划器沿全局路径向后回退，得到位于最坏情况安全边界之外的新子目标 G 2 ′ G_2' G2′。 来源：Kondo et al., SANDO, arXiv:2604.07599v3, Fig. 3。 查看原图
4.4 Safe Subgoal：为什么要"沿路径往回退"
如果 A* 的全局目标落入未知区域，STSFC 无法在未知区域内部构造可证明安全的多面体。SANDO 因此先沿全局路径向前找到首次进入未知区域的位置，再以 0.1 m 0.1\,\mathrm m 0.1m 步长沿路径反向搜索，直到找到位于最坏情况膨胀区域之外的候选点，将其作为新的安全子目标 G ′ G' G′。
其最坏情况回退半径为：
r i n f l a t e = v max , e n c o b s T max . r_{\mathrm{inflate}}=v_{\max,\mathrm{enc}}^{\mathrm{obs}}T_{\max}. rinflate=vmax,encobsTmax.
这一处理使局部优化永远只在当前能够建立安全走廊的区域内结束，而不会强迫 MIQP 向未知空间中"硬钻"。
5. Heat Map-based A*：软风险全局引导
5.1 为什么全局层不能直接硬阻塞动态可达集
如果 A* 直接把动态障碍的整个最坏情况可达集标记为 occupied，当前规划可能安全，但下一次重规划时动态障碍已经移动，阻塞区域随之平移，无人机当前位置 A ′ A' A′ 反而可能落在新的禁止区域内，使下一次搜索直接无解。
SANDO 因此把职责拆开：
- 当前静态障碍和动态障碍当前位置：仍然作为硬占据；
- 动态障碍周围和预测轨迹附近：使用连续 Heat（风险代价）；
- 真正的连续时间碰撞保证：交给后端 STSFC + MIQP。
图 4（SANDO 原文 Fig. 4）硬阻塞最坏情况可达集与 SANDO"Heat Map + STSFC + MIQP"的对比。硬阻塞方式在下一次重规划时可能使新起点 A ′ A' A′ 落入阻塞区而失败；SANDO 用软代价引导全局路径远离风险区，并在后端用时空走廊实施硬安全约束。 来源：Kondo et al., SANDO, arXiv:2604.07599v3, Fig. 4。 查看原图
5.2 静态障碍热力图
对静态障碍边界体素 b b b，以其中心 c b \mathbf c_b cb 为热源：
H b s ( q ) = { α s ( 1 − ∥ q − c b ∥ R s ) p s , ∥ q − c b ∥ ≤ R s , 0 , otherwise . H_b^s(\mathbf q)= \begin{cases} \alpha^s\left(1-\dfrac{\|\mathbf q-\mathbf c_b\|}{R^s}\right)^{p^s}, & \|\mathbf q-\mathbf c_b\|\leq R^s,\\ 0, & \text{otherwise}. \end{cases} Hbs(q)=⎩ ⎨ ⎧αs(1−Rs∥q−cb∥)ps,0,∥q−cb∥≤Rs,otherwise.
总静态热度取最大值并截断：
H s ( q ) = min ( max b H b s ( q ) , H max ) . H^s(\mathbf q)=\min\left(\max_b H_b^s(\mathbf q),H_{\max}\right). Hs(q)=min(bmaxHbs(q),Hmax).
使用 max 而不是累加的原因是避免在障碍密集区因为多个热源叠加而产生过大的人工代价。
5.3 动态障碍热力图：当前位置 + 预测轨迹管道
动态障碍热力图由两部分组成。
当前位置附近的 Base Heat：
R 0 , k = max i h k i + r b a s e , R_{0,k}=\max_i h_k^i+r_{\mathrm{base}}, R0,k=imaxhki+rbase,
R k d = R 0 , k + ( max i v max , i o b s ) T h , R_k^d=R_{0,k}+\left(\max_i v_{\max,i}^{\mathrm{obs}}\right)T_h, Rkd=R0,k+(imaxvmax,iobs)Th,
H k b a s e ( q ) = { α 0 d ( 1 − ∥ q − c ^ k ∥ R k d ) p d , ∥ q − c ^ k ∥ ≤ R k d , 0 , otherwise . H_k^{\mathrm{base}}(\mathbf q)= \begin{cases} \alpha_0^d\left(1-\dfrac{\|\mathbf q-\hat{\mathbf c}_k\|}{R_k^d}\right)^{p^d}, & \|\mathbf q-\hat{\mathbf c}_k\|\leq R_k^d,\\ 0, & \text{otherwise}. \end{cases} Hkbase(q)=⎩ ⎨ ⎧α0d(1−Rkd∥q−c^k∥)pd,0,∥q−c^k∥≤Rkd,otherwise.
对预测轨迹离散采样 c ^ k , j \hat{\mathbf c}_{k,j} c^k,j，构造 Tube Penalty：
R k , j = R 0 , k + γ k t j , R_{k,j}=R_{0,k}+\gamma_k t_j, Rk,j=R0,k+γktj,
w ( t j ) = exp ( − t j τ d ) , w(t_j)=\exp\left(-\frac{t_j}{\tau^d}\right), w(tj)=exp(−τdtj),
H k t u b e ( q ) = α 1 d max j w ( t j ) ( max ( 0 , 1 − ∥ q − c \^ k , j ∥ R k , j ) ) q d . H_k^{\mathrm{tube}}(\mathbf q) =\alpha_1^d\max_j\left w(t_j) \\left( \\max\\left(0,1-\\frac{\\\|\\mathbf q-\\hat{\\mathbf c}_{k,j}\\\|}{R_{k,j}}\\right) \\right)\^{q\^d} \\right. Hktube(q)=α1djmaxw(tj)(max(0,1−Rk,j∥q−c\^k,j∥))qd.
因此：
H k ( q ) = H k b a s e ( q ) + H k t u b e ( q ) , H_k(\mathbf q)=H_k^{\mathrm{base}}(\mathbf q)+H_k^{\mathrm{tube}}(\mathbf q), Hk(q)=Hkbase(q)+Hktube(q),
H d ( q ) = max k H k ( q ) . H^d(\mathbf q)=\max_k H_k(\mathbf q). Hd(q)=kmaxHk(q).
最终融合静态与动态热度：
H ( q ) = min ( max ( H s ( q ) , H d ( q ) ) , H max ) . H(\mathbf q)=\min\left(\max\left(H^s(\mathbf q),H^d(\mathbf q)\right),H_{\max}\right). H(q)=min(max(Hs(q),Hd(q)),Hmax).
图 5（SANDO 原文 Fig. 5）实机中的 Heat Map 可视化。静态墙面热度随空间距离衰减；动态障碍静止时以当前位置 Base Heat 为主，运动时预测轨迹形成随空间和时间衰减的 Tube Penalty，从而把 A 路径提前推离潜在冲突区域。 来源：Kondo et al., SANDO, arXiv:2604.07599v3, Fig. 5。 查看原图*
5.4 A* 搜索代价
SANDO 在 26 邻接三维体素图上运行 A*。边代价为：
c ( q i , q i + 1 ) = d ( q i , q i + 1 ) + w h e a t H ( q i + 1 ) , c(\mathbf q_i,\mathbf q_{i+1}) =d(\mathbf q_i,\mathbf q_{i+1}) +w_{\mathrm{heat}}H(\mathbf q_{i+1}), c(qi,qi+1)=d(qi,qi+1)+wheatH(qi+1),
启发函数使用欧氏距离：
h ( q ) = ∥ q − q g ∥ . h(\mathbf q)=\|\mathbf q-\mathbf q_g\|. h(q)=∥q−qg∥.
因此 A* 不只是最短路搜索，而是在"路径长度"和"未来可优化性"之间做折中。需要强调：**Heat Map 本身不提供安全证明。**即使热力图参数选得不理想，形式化安全仍由 STSFC 硬约束决定；热力图主要影响路径质量、走廊大小和 MIQP 的可行率。
A* 输出后还进行三步后处理：共线点删除、基于代价的拐角 shortcut、按最大点间距 Δ v t x \Delta_{\mathrm{vtx}} Δvtx 重采样。随后只取前 P + 1 P+1 P+1 个 waypoint，形成 P P P 个局部路径段供 STSFC 使用，因此单次局部规划长度最多约为：
L ≤ P Δ v t x . L\leq P\Delta_{\mathrm{vtx}}. L≤PΔvtx.
6. 硬约束 MIQP 轨迹优化
6.1 三重积分器与三次 Bézier 轨迹
无人机平移状态定义为：
x T = p T , v T , a T , \mathbf x^T=\\mathbf p\^T,\\mathbf v\^T,\\mathbf a\^T, xT=pT,vT,aT,
控制输入为 jerk。每一段保持常 jerk，因此位置轨迹是三次多项式：
p n ( τ ) = a n τ 3 + b n τ 2 + c n τ + d n , τ ∈ 0 , d t . \mathbf p_n(\tau) =\mathbf a_n\tau^3+ \mathbf b_n\tau^2+ \mathbf c_n\tau+ \mathbf d_n, \qquad \tau\in0,dt. pn(τ)=anτ3+bnτ2+cnτ+dn,τ∈0,dt.
相邻轨迹段必须满足位置、速度、加速度连续：
x n + 1 ( 0 ) = x n ( d t ) , n = 0 , ... , N − 2. \mathbf x_{n+1}(0)=\mathbf x_n(dt), \qquad n=0,\ldots,N-2. xn+1(0)=xn(dt),n=0,...,N−2.
对应的四个三次 Bézier 控制点为：
p n 0 = d n , \mathbf p_{n0}=\mathbf d_n, pn0=dn,
p n 1 = c n d t + 3 d n 3 , \mathbf p_{n1}=\frac{\mathbf c_n dt+3\mathbf d_n}{3}, pn1=3cndt+3dn,
p n 2 = b n d t 2 + 2 c n d t + 3 d n 3 , \mathbf p_{n2}=\frac{\mathbf b_n dt^2+2\mathbf c_n dt+3\mathbf d_n}{3}, pn2=3bndt2+2cndt+3dn,
p n 3 = a n d t 3 + b n d t 2 + c n d t + d n . \mathbf p_{n3}=\mathbf a_n dt^3+\mathbf b_n dt^2+\mathbf c_n dt+\mathbf d_n. pn3=andt3+bndt2+cndt+dn.
6.2 为什么 Bézier 控制点能提供连续时间安全保证
Bézier 曲线有一个关键性质：**整条曲线位于其控制点凸包内部。**因此只需要把一段轨迹的全部控制点约束到同一个凸多面体，就能保证这整个连续时间轨迹段都位于该多面体内部，而不需要只在离散采样点上检查碰撞。
定义二进制变量：
z n p ∈ { 0 , 1 } , z_{np}\in\{0,1\}, znp∈{0,1},
当 z n p = 1 z_{np}=1 znp=1 时，第 n n n 段轨迹被分配到时间层 n n n 中的第 p p p 个 STSFC 多面体，并要求：
z n p = 1 ⇒ F n p p n j ≤ g n p , j = 0 , 1 , 2 , 3. z_{np}=1 \Rightarrow \mathbf F_{np}\mathbf p_{nj}\leq \mathbf g_{np}, \qquad j=0,1,2,3. znp=1⇒Fnppnj≤gnp,j=0,1,2,3.
每段至少属于一个多面体：
∑ p = 0 P − 1 z n p ≥ 1. \sum_{p=0}^{P-1}z_{np}\geq 1. p=0∑P−1znp≥1.
这就是 SANDO 与许多"在 MPC 节点上避障"的方法在安全意义上的核心区别：SANDO 约束的是整段连续曲线，而不是若干离散点。
6.3 动力学约束同样是连续时间约束
对速度、加速度对应的 Bézier 控制点以及每段 jerk 施加：
∥ v n j ∥ ∞ ≤ v max , \|\mathbf v_{nj}\|\infty\leq v{\max}, ∥vnj∥∞≤vmax,
∥ a n j ∥ ∞ ≤ a max , \|\mathbf a_{nj}\|\infty\leq a{\max}, ∥anj∥∞≤amax,
∥ j n ∥ ∞ ≤ j max . \|\mathbf j_n\|\infty\leq j{\max}. ∥jn∥∞≤jmax.
利用导数曲线的 Bézier 凸包性质，可保证整段连续轨迹上的速度、加速度与 jerk 都不超过限制。
目标函数是最小化平方 jerk：
J = ∑ n = 0 N − 1 ∥ j n ∥ 2 . J=\sum_{n=0}^{N-1}\|\mathbf j_n\|^2. J=n=0∑N−1∥jn∥2.
因此完整问题本质上是：
min trajectory coefficients , z n p ∑ n ∥ j n ∥ 2 s.t. Boundary conditions , Position/velocity/acceleration continuity , STSFC control-point constraints , Velocity/acceleration/jerk bounds , z n p ∈ { 0 , 1 } . \begin{aligned} \min_{\text{trajectory coefficients},\;z_{np}}\quad &\sum_n\|\mathbf j_n\|^2\\ \text{s.t.}\quad &\text{Boundary conditions},\\ &\text{Position/velocity/acceleration continuity},\\ &\text{STSFC control-point constraints},\\ &\text{Velocity/acceleration/jerk bounds},\\ &z_{np}\in\{0,1\}. \end{aligned} trajectory coefficients,znpmins.t.n∑∥jn∥2Boundary conditions,Position/velocity/acceleration continuity,STSFC control-point constraints,Velocity/acceleration/jerk bounds,znp∈{0,1}.
求解器采用 Gurobi ，仿真中单线程 MIQP 时间上限为 0.5 s 0.5\,\mathrm{s} 0.5s，硬件中为 1 s 1\,\mathrm{s} 1s，相对 MIP gap 使用 10 − 4 10^{-4} 10−4。
6.4 Variable Elimination：为什么 MIQP 能跑到实时
如果直接优化，每个轨迹段每个轴有四个多项式系数，因此每轴有 4 N 4N 4N 个连续变量，三维共：
12 N 12N 12N
个连续决策变量。
但是位置、速度、加速度连续约束以及起终状态约束一共提供每轴：
3 ( N − 1 ) + 6 = 3 N + 3 3(N-1)+6=3N+3 3(N−1)+6=3N+3
个线性等式。因此论文在离线阶段对这些等式进行符号消元，把大部分系数写成少数自由变量的仿射函数，最终每轴只剩：
4 N − ( 3 N + 3 ) = N − 3 4N-(3N+3)=N-3 4N−(3N+3)=N−3
个连续自由变量。
例如：
- N = 4 N=4 N=4：每轴只剩 1 个自由变量；
- N = 5 N=5 N=5：每轴只剩 2 个；
- N = 6 N=6 N=6：每轴只剩 3 个。
这意味着 N = 4 N=4 N=4 时三维连续变量从原本 48 48 48 个压缩到仅 3 3 3 个，再加上 N P NP NP 个二进制走廊分配变量。论文对 N = 4 , 5 , 6 N=4,5,6 N=4,5,6 预先生成符号消元表达式；当 N ≥ 7 N\geq7 N≥7 时符号表达式增长过快，因此目前没有继续扩展。
6.5 并行 Time Allocation
每段轨迹持续时间写成：
d t = f d t 0 , f ≥ 1 , dt=f\,dt_0,\qquad f\geq1, dt=fdt0,f≥1,
其中 d t 0 dt_0 dt0 是在速度、加速度和 jerk 限制下得到的最小可行时间。
这里存在一个很有意思的耦合： d t dt dt 太短可能动力学不可行；但 d t dt dt 太长又会让动态障碍有更多时间移动，使 R n \mathcal R_n Rn 变大、STSFC 变窄，同样可能导致无解。
SANDO 因此维护多个候选 f i f_i fi，并行启动多个 MIQP 线程：
d t i = f i d t 0 . dt_i=f_i dt_0. dti=fidt0.
每个线程因为 d t i dt_i dti 不同，都需要构造自己的 STSFC。一旦某个线程找到可行解，就立即终止其他线程并采用该解。若本轮成功，下轮将窗口中心移动到成功的 f f f 附近；若全部失败，则整体向更大的 f f f 平移一个 Δ f \Delta f Δf。
这一设计把"寻找合适轨迹时间尺度"从串行搜索变成并行竞争，是 SANDO 实时性的另一个重要来源。
7. 动态障碍检测、跟踪与预测
SANDO 的规划安全不仅依赖优化器，也依赖前端能否及时发现动态障碍。
7.1 Temporal Occupancy Grid
系统借鉴 Dynablox 的思想维护时序占据体素：长期保持 occupied 的体素倾向于被归为静态；从 free 突然变为 occupied，且附近静态邻居较少的体素被视作动态候选。动态标签还会保留一段时间，以应对短暂遮挡。
7.2 聚类与数据关联
动态体素经过 Euclidean Clustering（欧氏聚类），每个 cluster 计算：
- centroid（质心）；
- AABB half-extents（轴对齐包围盒半尺寸）。
AABB 尺寸使用指数平滑。轨迹关联采用距离阈值下的 nearest-neighbor matching（最近邻匹配）。论文明确指出，当多个障碍交叉或距离很近时，最近邻关联可能出错；这一模块可以被 Hungarian Algorithm 等更强的数据关联方法替换，而不改变后端规划结构。
7.3 AEKF：Adaptive Extended Kalman Filter
每个障碍的状态为九维：
x k = c k T , v k T , a k T T . \mathbf x_k= \\mathbf c_k\^T,\\mathbf v_k\^T,\\mathbf a_k\^T^T. xk=ckT,vkT,akTT.
滤波内部使用 constant-acceleration（恒加速度）过程模型，而测量只有 cluster centroid。为适应未知且随时间变化的噪声，论文在线更新测量噪声协方差和过程噪声协方差：
R i = α R i − 1 + ( 1 − α ) r i r i T , \mathbf R_i =\alpha\mathbf R_{i-1} +(1-\alpha)\mathbf r_i\mathbf r_i^T, Ri=αRi−1+(1−α)ririT,
Q i = α Q i − 1 + ( 1 − α ) K i d i d i T K i T , \mathbf Q_i =\alpha\mathbf Q_{i-1} +(1-\alpha)\mathbf K_i\mathbf d_i\mathbf d_i^T\mathbf K_i^T, Qi=αQi−1+(1−α)KididiTKiT,
其中 α = 0.9 \alpha=0.9 α=0.9。
7.4 为什么"估计用恒加速度，预测却用恒速度"
这是论文中一个容易忽略但很合理的工程选择。
AEKF 内部保留 acceleration state，是因为人或移动机器人可能突然起步、刹车、转弯；但做未来轨迹外推时却使用：
c ( t ) = c 0 + v 0 t . \mathbf c(t)=\mathbf c_0+\mathbf v_0t. c(t)=c0+v0t.
原因是加速度本身是位置的二阶导估计，噪声通常较大。如果恒加速度预测中的加速度误差为 δ a \delta a δa，预测位置误差会以：
1 2 δ a t 2 \frac12\delta a\,t^2 21δat2
二次增长；而恒速度预测中的速度误差只以：
δ v t \delta v\,t δvt
线性增长。SANDO 不依赖预测轨迹本身提供安全保证------真正的安全性由最坏情况 reachable set 兜底------因此宁可让预测模型保持简单稳定，用于 Heat Map 引导。
8. 形式化安全分析
8.1 安全保证依赖哪些假设
表 2 SANDO 安全定理的关键假设
| 假设 | 含义 | 若违反会怎样 |
|---|---|---|
| 障碍速度有已知上界 | 真正位移必须落在 R n \mathcal R_n Rn 内 | 障碍可能跑出膨胀区域进入走廊 |
| 位置估计误差有界 | ∣ c k − c ^ k ∣ ∞ ≤ ϵ |\mathbf c_k-\hat{\mathbf c}k|\infty\leq\epsilon ∣ck−c^k∣∞≤ϵ | 真障碍可能不再被膨胀 AABB 包含 |
| AABB 尺寸不低估 | h ^ k i ≥ h k i \hat h_k^i\geq h_k^i h^ki≥hki | 长条形物体端视时可能超出估计包围盒 |
| 轨迹完美跟踪 | 实际位置等于规划位置 | 实际无人机可能离开安全多面体 |
| 未跟踪动态障碍位于未知空间 | 未检测目标仍被未知区膨胀覆盖 | "已经看见但尚未被判为动态"的目标存在安全空档 |
硬件实验中作者用 ϵ = 0.3 m \epsilon=0.3\,\mathrm m ϵ=0.3m 同时吸收障碍位置估计误差和无人机轨迹跟踪误差；仿真中位置误差可忽略，因此 ϵ = 0 \epsilon=0 ϵ=0。
8.2 连续时间安全为什么成立
论文的 Theorem 1 可以分成三步理解。
第一步，若第 n n n 段轨迹的四个 Bézier 控制点都在被选中的 C n p n ∗ \mathcal Cnp_n\^\* Cnpn∗ 内，则由凸包性质：
p n ( t ) ∈ C n p n ∗ , ∀ t ∈ t 0 + n d t , t 0 + ( n + 1 ) d t . \mathbf p_n(t)\in\mathcal Cnp_n\^\*, \qquad \forall t\int_0+n\\,dt,t_0+(n+1)dt. pn(t)∈Cnpn∗,∀t∈t0+ndt,t0+(n+1)dt.
第二步，由障碍速度界、估计误差界和尺寸上界，真实动态障碍加上无人机体积被包含于膨胀区域：
O k ( t ) ⊕ D ⊆ O ^ k n . \mathcal O_k(t)\oplus\mathcal D \subseteq \hat{\mathcal O}_k^n. Ok(t)⊕D⊆O^kn.
第三步，走廊在构造时满足：
C n p ∩ O ^ k n = ∅ , \mathcal Cnp\cap\hat{\mathcal O}_k^n=\emptyset, Cnp∩O^kn=∅,
并且启用未知空间膨胀时：
C n p ∩ U ^ n = ∅ . \mathcal Cnp\cap\hat{\mathcal U}^n=\emptyset. Cnp∩U^n=∅.
因此：
A ( p ( t ) ) ∩ O k ( t ) = ∅ , ∀ t ∈ t 0 , t 0 + N d t . \mathcal A(\mathbf p(t))\cap\mathcal O_k(t)=\emptyset, \qquad \forall t\int_0,t_0+Ndt. A(p(t))∩Ok(t)=∅,∀t∈t0,t0+Ndt.
也就是说，安全保证覆盖的是整架无人机的物理体积 ，而不是只保证无人机质心不碰障碍；同时保证的是整个连续时间区间，而不是离散采样时刻。
8.3 必须明确：SANDO 不保证 Recursive Feasibility
这是理解论文时非常重要的一点。Theorem 1 保证的是：当前已经求出的轨迹在当前 planning horizon 内安全。它并不证明下一次重规划一定还能找到新解。
SANDO 使用移动子目标的 receding horizon，不像某些 shrinking-horizon MPC 始终朝固定终止时间收缩，因此上一周期可行并不能推出下一周期仍然可行。如果一直重规划失败，无人机会继续执行此前已经提交的安全轨迹；轨迹末端设置为零速度、零加速度的 stop state。但动态障碍可能在当前 horizon 结束后再进入这个 stop state，所以：
P e r − H o r i z o n S a f e t y ≠ I n f i n i t e − H o r i z o n S a f e t y . \boxed{\mathrm{Per\!\!-\!Horizon\;Safety}\;\neq\;\mathrm{Infinite\!\!-\!Horizon\;Safety}}. Per−HorizonSafety=Infinite−HorizonSafety.
论文把"在狭窄、多动态障碍场景下同时获得无限时域安全和实时性"明确留作开放问题。
9. 仿真实验设计
全部仿真运行在 Alienware Aurora R8、16-core Intel Core i9、64 GB RAM 上，SANDO 使用 Ubuntu 22.04 LTS + ROS 2 Humble；其他 ROS1 方法通过 Docker 在同一机器上进行比较。MIQP 使用 Gurobi。
表 3 SANDO 仿真中的主要固定参数（根据原文 Table II 整理）
| 模块 | 参数 | 数值 |
|---|---|---|
| Static Heat | α s \alpha^s αs | 5.0 |
| Static Heat | p s p^s ps | 2 |
| Static Heat | R s R^s Rs | 3 r d r o n e 3r_{\mathrm{drone}} 3rdrone |
| Heat cap | H max H_{\max} Hmax | 50 |
| Dynamic Heat | α 0 d \alpha_0^d α0d | 1.0 |
| Dynamic Heat | α 1 d \alpha_1^d α1d | 2.0 |
| Dynamic Heat | p d , q d p^d,q^d pd,qd | 2 |
| Simulation tube growth | γ k \gamma_k γk | 0 |
| A* path | Δ v t x \Delta_{\mathrm{vtx}} Δvtx | 1.0 m |
| Time allocation | Δ f \Delta f Δf | 0.1 |
| Time allocation | κ \kappa κ | 0.4 |
| AEKF | α \alpha α | 0.9 |
| MIQP solver | --- | Gurobi |
9.1 标准化静态基准：验证 MIQP 本身
标准化环境共设置 61 个目标点， y y y 方向从 − 3 -3 −3 到 3 m 3\,\mathrm m 3m，步长 0.1 m 0.1\,\mathrm m 0.1m；统一动力学约束为：
v max = 1 m / s , a max = 2 m / s 2 , j max = 3 m / s 3 . v_{\max}=1\,\mathrm{m/s},\qquad a_{\max}=2\,\mathrm{m/s^2},\qquad j_{\max}=3\,\mathrm{m/s^3}. vmax=1m/s,amax=2m/s2,jmax=3m/s3.
该实验主要比较 SANDO、FASTER 和 SUPER，目的不是测试动态障碍，而是验证 SANDO 经过 Variable Elimination 后能否保留硬约束轨迹质量，同时降低 MIQP 求解时间。
图 6（SANDO 原文 Fig. 6）标准化静态基准环境与多种方法的轨迹。局部放大可以看到，SUPER 的软约束倾向于将轨迹拉向凸多面体中心，可能产生更长的绕行路径；SANDO 与硬约束方法保持相近轨迹质量。 来源：Kondo et al., SANDO, arXiv:2604.07599v3, Fig. 6。 查看原图
SANDO 的平均单次优化时间随 N N N 增加分别为：
- N = 4 N=4 N=4： 1.3 m s 1.3\,\mathrm{ms} 1.3ms；
- N = 5 N=5 N=5： 6.8 m s 6.8\,\mathrm{ms} 6.8ms；
- N = 6 N=6 N=6： 17.2 m s 17.2\,\mathrm{ms} 17.2ms。
而相同控制点约束版本的 FASTER 在 N = 5 N=5 N=5 时为 18.0 m s 18.0\,\mathrm{ms} 18.0ms，说明变量消元确实显著降低了硬约束 MIQP 的计算负担。
9.2 静态 Forest Benchmark：验证高密度狭窄环境能力
论文在 100 m × 40 m 100\,\mathrm m\times40\,\mathrm m 100m×40m 的 Gazebo 森林中设置 Easy、Medium、Hard 三种障碍密度，与 EGO-Swarm2、SUPER 和 FASTER 对比。
图 7（SANDO 原文 Fig. 7）静态 Forest Benchmark 的 Easy、Medium、Hard 三种障碍密度。 来源：Kondo et al., SANDO, arXiv:2604.07599v3, Fig. 7。 查看原图
图 8（SANDO 原文 Fig. 8）Hard 静态环境中的 RViz 可视化。绿色线框为安全走廊，轨迹颜色表示速度，橙色体素为占据区域，橙色折线为原始全局路径，红线为用于走廊生成的平滑参考路径。 来源：Kondo et al., SANDO, arXiv:2604.07599v3, Fig. 8。 查看原图
SANDO 在三个难度下均达到 100% 成功率。FASTER 原始版本在 Medium、Hard 分别下降到 90% 和 70%；EGO-Swarm2 在 Hard 只有约 50--60%。SANDO 同时保持速度、加速度与 jerk 的 0% 约束违反率。
这里也体现了硬约束与软约束的差异：SUPER 和 EGO-Swarm2 的软优化速度快，但会出现 jerk 或速度约束违反；FASTER 若只在部分控制点上约束，也可能出现连续轨迹上的速度越界。SANDO 将动力学限制同样施加到 Bézier 导数控制点上，因此整个连续时间轨迹满足约束。
9.3 动态 Benchmark：最核心的实验
动态环境仍是 100 m × 40 m 100\,\mathrm m\times40\,\mathrm m 100m×40m，其中包含静态杆状障碍和沿 trefoil-knot（三叶结）轨迹运动的 0.8 m 0.8\,\mathrm m 0.8m 立方体动态障碍。三种难度分别约为：
- Easy：50 个障碍，其中约 33 个动态；
- Medium：100 个障碍，其中约 65 个动态；
- Hard：200 个障碍，其中约 130 个动态。
动态障碍约占 65%。实际逐轴峰值速度约为 ( 0.83 , 0.94 , 1.5 ) m / s (0.83,0.94,1.5)\,\mathrm{m/s} (0.83,0.94,1.5)m/s，SANDO 使用更保守的速度界：
( v max , x o b s , v max , y o b s , v max , z o b s ) = ( 1.0 , 1.0 , 1.5 ) m / s . (v_{\max,x}^{\mathrm{obs}},v_{\max,y}^{\mathrm{obs}},v_{\max,z}^{\mathrm{obs}}) =(1.0,1.0,1.5)\,\mathrm{m/s}. (vmax,xobs,vmax,yobs,vmax,zobs)=(1.0,1.0,1.5)m/s.
无人机动力学上限为：
v max = 5 m / s , a max = 20 m / s 2 , j max = 100 m / s 3 . v_{\max}=5\,\mathrm{m/s},\quad a_{\max}=20\,\mathrm{m/s^2},\quad j_{\max}=100\,\mathrm{m/s^3}. vmax=5m/s,amax=20m/s2,jmax=100m/s3.
图 9（SANDO 原文 Fig. 9）动态 Gazebo Benchmark 的 Easy、Medium、Hard 三种环境。障碍数量随难度快速增加，Hard 中约有 130 个动态障碍。 来源：Kondo et al., SANDO, arXiv:2604.07599v3, Fig. 9。 查看原图
表 4 动态环境 Ground-Truth Benchmark 的关键结果（原文 Table VI 提炼）
| 方法 | Easy 成功率 | Medium 成功率 | Hard 成功率 | 备注 |
|---|---|---|---|---|
| EGO-Swarm2 | 100% | 100% | 80% | 软约束，存在速度违反 |
| FAPP | 80% | 80% | 50% | 软碰撞代价，Hard 明显下降 |
| I-MPC（各环境最佳设置） | 90% | 70% | 40% | 硬约束但在离散 MPC 点实施 |
| SANDO | 100% | 100% | 100% | 连续时间硬约束，0% 动力学约束违反 |
SANDO 在 Easy / Medium / Hard 的单次优化时间分别约为 3.0 / 3.3 / 4.3 m s 3.0/3.3/4.3\,\mathrm{ms} 3.0/3.3/4.3ms。论文强调，SANDO 在该对比中只使用动态障碍当前位置与预先规定的速度上界形成最坏情况可达集，而一些 baseline 获得了更多 ground-truth 速度甚至未来预测信息，因此 100% 成功率不是依赖"更强先验"得到的。
需要注意实验比较口径：SANDO、FASTER、SUPER、FAPP、EGO-Swarm2 的约束违反率是在各自 nominal kinematic trajectory 上计算，而 I-MPC 的公开实现包含更接近真实情况的跟踪器，因此 I-MPC 的约束违反率还会受到跟踪误差影响。论文对此进行了单独说明。
9.4 不提供 Ground Truth：完整感知---规划链路
更现实的实验中，SANDO 只接收模拟深度相机点云，通过时序占据图、聚类和 AEKF 自己发现障碍。此时感知误差和动态障碍"发现得太晚"成为主要瓶颈。
表 5 Perception-only 动态环境中 N = 5 , P = 2 N=5,P=2 N=5,P=2 的代表结果（原文 Table VII）
| 难度 | 成功率 | 单次优化时间 | 总重规划时间 | STSFC 时间 | 动力学约束违反 |
|---|---|---|---|---|---|
| Easy | 80% | 3.4 ms | 23.4 ms | 11.3 ms | 0% |
| Medium | 60% | 3.9 ms | 31.3 ms | 15.1 ms | 0% |
| Hard | 40% | 4.7 ms | 51.4 ms | 24.2 ms | 0% |
这组数据非常重要，因为它说明：**后端优化的安全约束没有失效，但整体系统成功率会受到前端感知延迟影响。**Hard 环境中失败的首要原因不是 MIQP 产生不安全轨迹，而是动态障碍直到距离很近时才被首次识别，留给规划器的反应时间不足。
论文进一步把 P P P 的影响拆成两部分：更大的 P P P 一方面扩大局部 horizon，另一方面会增加 N × P N\times P N×P 个二进制分配变量。在固定 horizon 的受控实验中， P = 2 → 3 → 4 P=2\to3\to4 P=2→3→4 时单次优化时间从 5.5 → 8.8 → 23.2 m s 5.5\to8.8\to23.2\,\mathrm{ms} 5.5→8.8→23.2ms，成功率从 40% 下降到 10%，说明 MIQP 组合复杂度本身就是实时动态避障的重要限制。
10. 消融实验：两个关键设计是否真的必要
10.1 Variable Elimination 消融
表 6 Variable Elimination 消融（原文 Table IX）
| N N N | VE | 成功率 | 单次优化时间 | 轨迹长度 | 最大约束违反率 |
|---|---|---|---|---|---|
| 4 | Yes | 93.4% | 1.3 ms | 6.8 m | 0% |
| 4 | No | 93.4% | 9.6 ms | 6.8 m | 0% |
| 5 | Yes | 100% | 6.8 ms | 6.8 m | 0% |
| 5 | No | 100% | 12.8 ms | 6.8 m | 0% |
| 6 | Yes | 100% | 17.2 ms | 6.8 m | 0% |
| 6 | No | 100% | 24.7 ms | 6.8 m | 0% |
N = 4 N=4 N=4 时速度提升约为：
9.6 1.3 ≈ 7.4 × . \frac{9.6}{1.3}\approx7.4\times. 1.39.6≈7.4×.
而轨迹表现完全不变，因为变量消元并没有近似原问题，只是解析地消除了由线性等式唯一确定的冗余变量。
10.2 STSFC 与 Full-horizon Worst-case Inflation 消融
表 7 STSFC 与全时域最坏情况膨胀对比（原文 Table X）
| 环境 | SFC 模式 | 成功率 | 单次优化时间 | 飞行时间 | 路径长度 | Jerk 指标 |
|---|---|---|---|---|---|---|
| Easy | Full-horizon Worst-Case | 90% | 2.9 ms | 25.4 s | 108.1 m | 720.1 |
| Easy | STSFC | 100% | 3.0 ms | 24.4 s | 107.6 m | 493.7 |
| Medium | Full-horizon Worst-Case | 100% | 3.4 ms | 29.4 s | 113.4 m | 1212.3 |
| Medium | STSFC | 100% | 3.3 ms | 24.8 s | 111.5 m | 675.3 |
| Hard | Full-horizon Worst-Case | 30% | 5.1 ms | 34.0 s | 118.8 m | 2043.9 |
| Hard | STSFC | 100% | 4.3 ms | 26.3 s | 117.8 m | 1054.4 |
Hard 环境最能说明问题：统一用整个 horizon 的最大可达集膨胀时，成功率只有 30%；按时间层逐步膨胀后达到 100%。这证明 STSFC 的价值并不是简单"让安全边界更大或更小"，而是利用时间信息把安全裕量放在真正需要它的时间位置上。
11. 真机实验
11.1 硬件平台与控制链路
真机使用 Holybro PX4 Development Kit X500 四旋翼，配备：
- Livox Mid-360 LiDAR：感知与定位输入；
- DLIO：机载 LiDAR-Inertial Odometry；
- Intel NUC 13，16 线程 Core i7、64 GB RAM：运行 SANDO；
- Pixhawk + PX4：底层飞控。
PX4 采用级联 PID：body-rate 内环约 1 k H z 1\,\mathrm{kHz} 1kHz，姿态环 250 H z 250\,\mathrm{Hz} 250Hz，位置/速度外环 50 H z 50\,\mathrm{Hz} 50Hz。SANDO 输出 Bézier 轨迹上的位置、速度、加速度和 yaw 参考，其中速度、加速度、roll、pitch 等作为前馈项进入底层控制。
图 10（SANDO 原文 Fig. 10）真机四旋翼平台：Holybro X500 + Pixhawk 飞控 + 螺旋桨保护架，上方安装 Livox Mid-360 LiDAR。 来源：Kondo et al., SANDO, arXiv:2604.07599v3, Fig. 10。 查看原图
11.2 静态环境真机
室内静态场景尺寸约 20 m × 8 m 20\,\mathrm m\times8\,\mathrm m 20m×8m，无人机执行从 x = 0 x=0 x=0 到 x = 20 m x=20\,\mathrm m x=20m 再返回的任务，共进行 6 次飞行，速度上限从 1 1 1 到 6 m / s 6\,\mathrm{m/s} 6m/s。
图 11（SANDO 原文 Fig. 11）静态真机 Experiment 3。无人机在密集静态障碍中完成往返飞行；轨迹颜色表示速度，点云颜色表示高度。 来源：Kondo et al., SANDO, arXiv:2604.07599v3, Fig. 11。 查看原图
六次静态飞行全部安全完成，平均总重规划时间约为 32.6 ∼ 35.8 m s 32.6\sim35.8\,\mathrm{ms} 32.6∼35.8ms。即使机载 CPU 明显弱于仿真台式机，系统仍保持实时运行。
11.3 单动态障碍真机
Experiments 7--10 在 8 m × 8 m 8\,\mathrm m\times8\,\mathrm m 8m×8m 环境中进行。动态障碍约以 0.5 m / s 0.5\,\mathrm{m/s} 0.5m/s 运动，轨迹分别为直线、圆形、8 字形和随机行人运动。无人机需要完成五次往返。
图 12（SANDO 原文 Fig. 12）Experiment 10：无人机在 8 m × 8 m 8\,\mathrm m\times8\,\mathrm m 8m×8m 空间中避让随机行走的人体动态障碍；图中展示了不同时间的相对运动过程。 来源：Kondo et al., SANDO, arXiv:2604.07599v3, Fig. 12。 查看原图
表 8 单动态障碍真机结果（原文 Table XII）
| 实验 | 障碍轨迹 | 总重规划时间 | STSFC | MIQP | Tracking RMS | 最小中心距离 | 障碍估计误差 |
|---|---|---|---|---|---|---|---|
| 7 | Line | 18.6 ms | 4.9 ms | 4.6 ms | 0.32 m | 2.01 m | 0.30 m |
| 8 | Circle | 18.4 ms | 4.8 ms | 4.6 ms | 0.30 m | 1.51 m | 0.29 m |
| 9 | Figure Eight | 19.1 ms | 4.9 ms | 4.9 ms | 0.27 m | 1.59 m | 0.30 m |
| 10 | Person | 18.9 ms | 5.0 ms | 4.7 ms | 0.42 m | 1.33 m | 0.43 m |
11.4 五个动态障碍 + 静态障碍
Experiments 11--13 在 8 m × 20 m 8\,\mathrm m\times20\,\mathrm m 8m×20m 区域中设置 5 个动态障碍；Experiments 14--18 在此基础上进一步加入静态障碍。后几次实验把无人机最大速度从 2 m / s 2\,\mathrm{m/s} 2m/s 提升到 3 3 3 和 4 m / s 4\,\mathrm{m/s} 4m/s。
图 13（SANDO 原文 Fig. 13）Experiment 16：无人机以 v max = 2 m / s v_{\max}=2\,\mathrm{m/s} vmax=2m/s 穿越 8 m × 20 m 8\,\mathrm m\times20\,\mathrm m 8m×20m 的静态/动态混合环境，包含 5 个动态障碍。图中方框表示检测到的动态障碍，线段表示其未来预测轨迹。 来源：Kondo et al., SANDO, arXiv:2604.07599v3, Fig. 13。 查看原图
Experiments 11--18 的平均总重规划时间约为 19.7 ∼ 23.5 m s 19.7\sim23.5\,\mathrm{ms} 19.7∼23.5ms。最长连续重规划失败时间为 700 m s 700\,\mathrm{ms} 700ms，但此前已提交的轨迹始终没有被耗尽，因此无人机没有进入"无新解后停在末端"的状态。全部 12 次动态障碍真机实验均无碰撞完成。
11.5 真机结果同时暴露了理论与实际之间的差距
这篇论文值得肯定的一点是作者没有把形式化安全证明直接等同于"真机绝对安全"。理论假设要求估计误差受到 ϵ \epsilon ϵ 约束且轨迹完美跟踪，而真机中：
- ϵ \epsilon ϵ 设置为 0.3 m 0.3\,\mathrm m 0.3m；
- 单障碍实验的平均位置估计误差约为 0.29 ∼ 0.43 m 0.29\sim0.43\,\mathrm m 0.29∼0.43m；
- 轨迹 Tracking RMS 约为 0.21 ∼ 0.57 m 0.21\sim0.57\,\mathrm m 0.21∼0.57m。
因此论文明确指出：**Theorem 1 的严格条件在真机上并未完全满足。**实际安全来自保守膨胀、 0.3 m 0.3\,\mathrm m 0.3m buffer 和持续高频重规划共同提供的工程裕量，而不能把 12 次无碰撞直接解释为数学定理已经在真实世界中被严格满足。
12. 论文的主要创新点与学术贡献
12.1 STSFC：把 Safe Flight Corridor 从空间域扩展到时空域
传统 SFC 是一组静态凸多面体。SANDO 的核心扩展是让每一个时间层拥有自己的安全多面体：
C p ⟶ C n p . \mathcal Cp\quad\longrightarrow\quad\mathcal Cnp. Cp⟶Cnp.
对应的障碍膨胀也从全时域统一最坏情况变成：
R f u l l ⟶ R n . \mathcal R_{\mathrm{full}} \quad\longrightarrow\quad \mathcal R_n. Rfull⟶Rn.
这一变化直接解决了动态障碍下"安全但过度保守"的问题，是全文最核心的算法贡献。
12.2 软全局引导 + 硬局部安全的职责分离
SANDO 没有追求"所有层都用硬约束"。全局 A* 使用 Heat Map soft cost，目的只是产生容易构造大 STSFC 的参考路径；真正安全约束放在后端 MIQP 中。
可以概括为：
S o f t G l o b a l R i s k G u i d a n c e → H a r d L o c a l S a f e t y E n f o r c e m e n t . \boxed{\mathrm{Soft\;Global\;Risk\;Guidance}} \rightarrow \boxed{\mathrm{Hard\;Local\;Safety\;Enforcement}}. SoftGlobalRiskGuidance→HardLocalSafetyEnforcement.
这种混合设计比"全软"更安全，也比"全硬"更不容易因为可达集膨胀导致搜索无解。
12.3 Bézier Convex Hull + MIQP 实现连续时间硬安全
论文不是在若干 trajectory samples 上检查碰撞，而是把每段 Bézier 曲线的所有控制点放进凸安全多面体。借助凸包性质，整段曲线天然位于该多面体内，因此将离散优化变量转化为连续时间几何保证。
这一点也是 SANDO 与不少 hard-constrained MPC 方法最本质的区别：后者可能在 knot points 上安全，但两个 knot 之间并不自动安全。
12.4 Variable Elimination 把硬约束 MIQP 推到实时范围
硬约束不是论文的唯一卖点；真正困难的是如何让硬约束在动态环境中持续高频重算。SANDO 将 4 N 4N 4N 个每轴多项式变量解析压缩到 N − 3 N-3 N−3 个，且完全消除线性等式约束。消融实验表明最高获得 7.4 × 7.4\times 7.4× 加速，而最优轨迹不变。
这使论文的贡献不仅是"安全表示更合理"，还包括优化问题结构本身的降维。
12.5 将"未观测空间"也纳入动态安全分析
许多动态规划方法只处理已经检测到的目标。SANDO 对 unknown-space boundary 同样进行时间层可达集膨胀，并设计 safe subgoal walk-back，使未跟踪障碍只要满足"当前完全位于未知区域"这一假设，也可纳入理论安全范围。
12.6 明确给出安全定理的适用边界
论文不仅给出 Theorem 1，还专门讨论：速度上界被突破、估计误差超过 ϵ \epsilon ϵ、AABB 尺寸低估、动态障碍尚未被跟踪、真实控制存在 tracking error 以及 recursive feasibility 不成立时会发生什么。这使论文的"安全"概念有清楚边界，而不是把实验无碰撞泛化成无条件保证。
13. 关键算法之间的逻辑关系
SANDO 容易被误解为"一个复杂版 A*"或"一个 MIQP 局部规划器"。实际上各模块承担的是不同职责：
表 9 SANDO 各模块的职责分工
| 模块 | 输入 | 输出 | 主要作用 | 是否直接承担安全证明 |
|---|---|---|---|---|
| Temporal Occupancy + Tracker | 点云 | 动态目标状态/AABB | 发现并估计动态目标 | 间接，提供假设量 |
| AEKF + CV Prediction | 目标观测 | 位置/速度/预测轨迹 | 平滑估计与风险引导 | 预测本身不提供安全保证 |
| Heat Map | 静态地图 + 动态预测 | 风险代价 | 把全局路径推离风险区 | 否 |
| A* | 体素图 + Heat | 全局参考路径 | 提供局部走廊参考 | 否 |
| STSFC | 路径 + 障碍可达集 | 时间分层凸多面体 | 建立安全自由空间 | 是 |
| MIQP | STSFC + 动力学约束 | Bézier 轨迹 | 连续时间轨迹优化 | 是 |
| Variable Elimination | MIQP 等式结构 | 降维 MIQP | 提升实时性 | 不改变安全性 |
| PX4 | 轨迹参考 | 电机控制 | 真机轨迹跟踪 | 理论中假设完美，实际有误差 |
最简洁的理解是：
H e a t M a p + A ∗ \boxed{\mathrm{Heat\;Map}\;+\;A^*} HeatMap+A∗
S T S F C + M I Q P \boxed{\mathrm{STSFC}\;+\;\mathrm{MIQP}} STSFC+MIQP
V E + P a r a l l e l T i m e A l l o c a t i o n \boxed{\mathrm{VE}\;+\;\mathrm{Parallel\;Time\;Allocation}} VE+ParallelTimeAllocation
14. 论文局限与仍未解决的问题
14.1 不保证下一次重规划一定可行
SANDO 的连续时间安全是 per-horizon guarantee，不是 recursive feasibility。若动态环境在短时间内显著恶化，下一周期仍可能找不到新轨迹。
14.2 最坏情况速度界仍然会带来保守性
STSFC 已经比 full-horizon worst-case inflation 小得多，但每一层仍使用 worst-case reachable set。障碍速度界设得越大， R n \mathcal R_n Rn 越大，走廊越窄；在高密度环境中依然可能无解。
14.3 感知延迟是完整系统中的主要薄弱环节
Perception-only Hard 环境只有约 40% 的最佳成功率，主要原因是动态障碍被发现得过晚。特别是"障碍已经出现在地图里，但还没有被 tracker 归类为 dynamic"的阶段，既不满足 unknown-space 假设，也还没有动态 reachable-set inflation，是理论覆盖最薄弱的时间窗口。
14.4 AABB 尺寸估计可能低估真实障碍
LiDAR 常常只看到障碍物的一个表面。如果细长障碍端面朝向传感器，真实深度可能远大于观测到的 AABB 厚度，直接破坏尺寸上界假设。
14.5 变量消元当前只覆盖 N = 4 , 5 , 6 N=4,5,6 N=4,5,6
理论上 N N N 越大可以提供更多分段自由度，但符号消元表达式随 N N N 快速膨胀。当前开源实现预计算 N ∈ { 4 , 5 , 6 } N\in\{4,5,6\} N∈{4,5,6}，当 N ≥ 7 N\geq7 N≥7 时可扩展性有限。
14.6 MIQP 的组合复杂度仍由 N × P N\times P N×P 控制
连续变量已经被大幅减少，但每段与每个多面体之间仍需要 binary assignment。论文的受控实验表明，即使固定局部 horizon，仅增加 P P P 也会显著提高计算时间并降低动态环境成功率。
14.7 真机并未严格满足形式化定理的全部假设
这是最需要避免误读的一点。真机实验表明 SANDO 工程上表现安全，但位置估计误差与轨迹跟踪误差已经接近甚至超过设置的 ϵ = 0.3 m \epsilon=0.3\,\mathrm m ϵ=0.3m。因此论文自己也明确写出，形式化保证在硬件上并非严格成立。
15. 与 FASTER、SUPER、FAPP、MPC 类方法的本质区别
15.1 与 FASTER
FASTER 的核心思想也是 SFC + MIQP 硬约束，因此 SANDO 在优化结构上与它有明显继承关系。但 FASTER 假设环境静态，而 SANDO 把走廊改造成 time-layered STSFC，再通过动态障碍的 per-layer reachable set 使其适用于动态环境。
此外，SANDO 的 Variable Elimination 与并行 Time Allocation 进一步解决硬约束 MIQP 的实时性问题。
15.2 与 SUPER
SUPER 更偏向软约束连续优化，计算速度快，但碰撞和动力学限制通过 penalty 处理，不存在同等级别的硬连续安全保证。SANDO 接受 MIQP 带来的组合复杂度，以换取严格约束，同时通过 VE 尽量把计算量压回实时范围。
15.3 与 FAPP 等动态软约束规划
FAPP 等方法利用障碍预测并通过软代价避障，实用性强，但"代价很大"不等于"绝对不能违反"。SANDO 则把预测主要用于 Heat Map 引导，安全底线由 worst-case reachable set + hard STSFC 决定。因此：
Prediction improves efficiency/path quality , \text{Prediction improves efficiency/path quality}, Prediction improves efficiency/path quality,
而：
Reachable-set hard constraints provide the safety envelope . \text{Reachable-set hard constraints provide the safety envelope}. Reachable-set hard constraints provide the safety envelope.
15.4 与普通 MPC 动态避障
许多 MPC 方法只在离散预测节点 t k t_k tk 上满足：
d ( x k , O k ) ≥ d min . d(\mathbf x_k,\mathcal O_k)\geq d_{\min}. d(xk,Ok)≥dmin.
这并不能自动推出：
d ( x ( t ) , O ( t ) ) ≥ d min , t ∈ ( t k , t k + 1 ) . d(\mathbf x(t),\mathcal O(t))\geq d_{\min},\qquad t\in(t_k,t_{k+1}). d(x(t),O(t))≥dmin,t∈(tk,tk+1).
SANDO 依靠 Bézier convex hull 把整段轨迹约束进无碰凸多面体，因此其保证天然是连续时间的。
16. 对轨迹规划研究的启示
SANDO 最有价值的并不只是某一个公式，而是其系统设计方式。
第一，**安全与效率可以由不同层承担。**全局层不必承担所有硬安全逻辑，只要把路径引向容易生成安全走廊的区域；局部层再实施严格约束。
第二，**动态规划必须把时间显式引入几何可行域。**动态障碍不是"静态障碍加大一点"这么简单。Full-horizon inflation 会丢掉大量本来在早期时间层可利用的自由空间，STSFC 的意义就是把"什么时候经过"纳入走廊定义。
第三，**实时优化不仅取决于求解器，也取决于变量参数化。**Variable Elimination 没有更换 Gurobi，也没有把硬约束改成软约束，而是从问题结构出发删除被连续性/边界条件确定的冗余变量；这类思路对 MPC、MINCO、B-spline / Bézier 轨迹优化同样具有借鉴价值。
第四，**形式化安全必须与感知假设一起阅读。**后端证明再严格，如果前端没有及时把障碍纳入模型，或者实际估计误差超过 buffer，定理就不再覆盖当前状态。SANDO 的 perception-only 和硬件实验很好地展示了这一点。
17. 论文最值得记住的六个结论
- SANDO 的核心不是单纯使用 MIQP，而是 STSFC：动态障碍只按其在对应时间层的最坏可达集进行膨胀。
- Heat Map A* 是"可行性引导器"，不是安全证明器。软风险代价把全局路径推离危险区域，真正的安全由 STSFC + hard MIQP 实现。
- Bézier 控制点全部位于安全多面体内，就可以把离散约束提升为连续时间整段轨迹安全。这比只在 MPC knot point 上约束更强。
- Variable Elimination 是实现实时硬约束优化的关键工程贡献。 N = 4 N=4 N=4 时每轴只剩一个连续自由变量，消融最高加速约 7.4 × 7.4\times 7.4×。
- 在具有 ground-truth 障碍信息的动态基准中，SANDO 在 Easy/Medium/Hard 均达到 100% 成功率且没有动力学约束违反；但 perception-only Hard 环境下降到约 40%，说明感知延迟是完整系统的核心瓶颈。
- 论文的安全保证是当前 planning horizon 内的 continuous-time safety，不是递归可行性或无限时域绝对安全；真机误差还使理论假设不能被严格满足。
18. 开源实现
开源仓库提供 ROS 2 实现，当前安装脚本面向 Ubuntu 22.04 + ROS 2 Humble + Gurobi，并包含 Livox 相关驱动、DecompROS2 等依赖。仓库：
https://github.com/mit-acl/sando
从论文结构看，如果要复现算法，建议按以下依赖顺序理解代码：
Map/Tracker → Heat Map A* → STSFC Generator → Variable-Eliminated MIQP → Trajectory/Controller Interface . \text{Map/Tracker} \rightarrow \text{Heat Map A*} \rightarrow \text{STSFC Generator} \rightarrow \text{Variable-Eliminated MIQP} \rightarrow \text{Trajectory/Controller Interface}. Map/Tracker→Heat Map A*→STSFC Generator→Variable-Eliminated MIQP→Trajectory/Controller Interface.
其中最值得首先核对的参数是 N N N、 P P P、 Δ v t x \Delta_{\mathrm{vtx}} Δvtx、 v max o b s v_{\max}^{\mathrm{obs}} vmaxobs、 ϵ \epsilon ϵ、 w h e a t w_{\mathrm{heat}} wheat 和 time-allocation factor window，因为它们直接决定"轨迹 horizon---MIQP 复杂度---障碍膨胀量---重规划频率"之间的耦合关系。
19. 参考链接
- SANDO arXiv v3：https://arxiv.org/abs/2604.07599
- SANDO arXiv HTML v3：https://arxiv.org/html/2604.07599v3
- SANDO Code：https://github.com/mit-acl/sando
- SANDO Video：https://youtu.be/_T10DJiLQXg
- Kota Kondo Homepage（T-RO conditional acceptance status）：https://kotakondo.github.io/
- MIT Aerospace Controls Laboratory：https://acl.mit.edu/
20. 总结
SANDO 的研究价值在于，它没有简单地在已有动态规划器上增加一个"障碍预测模块"，而是重新设计了动态环境下安全约束的几何表示。通过 Heat Map-based A * 提前把全局路径推离高风险区域，通过 STSFC 把障碍最坏情况可达集按时间层嵌入凸走廊，再利用 Bézier Convex Hull + Hard-constrained MIQP 获得当前规划时域内的连续时间安全保证；同时通过 Variable Elimination + Parallel Time Allocation 把原本计算量较大的硬约束 MIQP 压到可在线重规划的时间尺度。
从实验结果看，论文最有说服力的两组证据分别是：一方面，STSFC 在 Hard 动态环境中将 full-horizon worst-case inflation 的 30% 成功率提升到 100%；另一方面，Variable Elimination 在不改变轨迹质量的情况下最高实现约 7.4 × 7.4\times 7.4× 优化加速。这两点分别回答了"如何不过度保守"和"如何让硬约束足够快"两个核心问题。
同时，这篇工作并没有解决动态未知环境中的全部安全问题。感知延迟、障碍尺寸低估、跟踪误差、速度界选择以及 recursive feasibility 仍然是决定系统能否真正长期安全运行的关键因素。也正因为论文对这些假设与失败模式给出了明确说明，SANDO 更适合作为一套具有清晰安全边界的实时动态轨迹规划框架来理解，而不是被简单概括为"已经实现无条件安全的动态避障"。