面向微型无人机的安全保障高速导航

Safety-assured high-speed navigation for MAVs

Yunfan Rent, Fangcheng Zhut, Guozheng Lu, Yixi Cai, Longji Yin, Fanze Kong, Jiarong Lin, Nan Chen, Fu Zhang*

Department of Mechanical Engineering, University of Hong Kong, Pokfulam, Hong Kong, China.

原文： https://www.science.org/doi/10.1126/scirobotics.ado6187

摘要

具备高速自主导航能力的微型无人机（MAV）在未知环境中，有望提升搜救和灾害救援等应用的效率，这些场景中及时且安全的导航至关重要。然而，实现自主、安全且高速的MAV导航面临系统性挑战，需要减轻飞行器的重量和体积以实现高速机动，增强传感能力以远距离探测障碍物，以及采用先进的规划与控制算法，在保证避障的同时最大化飞行速度。本文提出了一种安全保障的高速空中机器人（SUPER），这是一款拥有280毫米轴距且推重比大于5.0的紧凑型微型无人机，能够在复杂环境中灵活飞行。SUPER配备轻量级三维激光雷达（LIDAR）传感器，实现精准远距障碍物探测。

为保证高速飞行的安全性，我们引入了一种高效的规划框架，直接利用激光点云进行轨迹规划。在每个重规划周期内，生成两条轨迹：一条在已知的自由空间中以确保安全，另一条则覆盖已知和未知空间以最大化速度。与基线方法相比，该框架在飞行速度更快的同时，将失败率降低了35.9倍，并将规划时间缩减至一半。在真实环境测试中，SUPER实现了超过20米每秒的自主飞行，成功避开细小障碍物并穿越狭窄空间。SUPER标志着自主微型无人机系统的里程碑，架起了从实验室研究到现实应用的桥梁。

引言

鸟类长期以来以其卓越的飞行能力吸引着人类，能够以极低的失败率在复杂环境中高速穿行。类似地，微型无人机（MAV）作为人类制造的最灵巧机器之一（1），具备实现鸟类般高速灵巧飞行的潜力。MAV快速且安全地到达目的地，是其成功应用于实际场景的关键因素。快速意味着MAV能够及时抵达指定位置，从而实现对时间敏感任务（如搜救（2）或灾害救援（3,4））的快速响应。安全则意味着MAV能够在途中探测并避开障碍物，避免碰撞失败。本研究探索如何赋予MAV鸟类般的能力，仅依赖机载传感与计算单元，实现未知环境中的安全高速飞行（见影片1）。

影片1. 提出SUPER系统概览。SUPER展示了其在未知且复杂环境中高速安全导航的能力；成功避开细小障碍如电线；并能在多种场景下稳健运行，包括目标跟踪和自主探索。

在未知环境中实现安全保障的高速飞行是一项复杂任务，需要系统设计的整体考虑。首先，为了在高速飞行中执行规避先前未知障碍的激进机动，MAV必须具备高机动性，体现为紧凑的体积和高推重比（TWR）。其次，MAV需要具备远距离探测能力，以提供足够的反应时间来避开高速运动中的障碍物。同时，这种探测能力应通过轻量、紧凑的传感器实现，以免削弱MAV的机动性。最后，MAV必须在轨迹规划中谨慎平衡飞行速度与安全。飞行速度与安全是相互冲突的两大因素，且需在MAV有限的机载计算能力下高效实现平衡。

已有多项工作聚焦于自主无人机竞速应用中的挑战（5-9），并在飞行速度与机动性方面取得显著成果。Song 等（5）采用强化学习（RL）技术，在人工竞速赛道上实现了超过 \(30 \mathrm{~m} / \mathrm{s}\) 的飞行速度，使用外部计算进行控制，并通过动作捕捉系统提供状态反馈。Foehn 等（6）提出基于进展优化的策略，离线计算时间最优轨迹，并用机载计算机进行轨迹跟踪；Romero 等提出基于采样的在线规划方法（7）和模型预测轮廓控制（8）进行轨迹跟踪，实现了超过 \(18 \mathrm{~m} / \mathrm{s}\) 的高速竞速飞行。Kaufmann 等（9）采用类似的RL方法，结合机载传感和计算，超越了冠军级人类飞行员的竞速表现。尽管实现了显著的飞行速度和机动性，这些方法依赖外部传感（如动作捕捉系统）（5-8）、实际飞行中的外部计算（5）或离线训练（5, 6, 9）。此外，这些方法假设环境固定且已知（如竞速赛道）（5-9），控制策略为实现最佳性能而全面训练，其在未知环境中的适用性尚不明确。

文献中已有大量研究探索仅依赖机载传感与计算实现未知环境下自主飞行。第一类方法优先考虑飞行速度（10-17）。Escobar-Alvarez 等采用扩张率（ER）方法，在相对开阔区域实现了6至 \(19 \mathrm{~m} / \mathrm{s}\) 的高速飞行（17），使用视觉传感器和单线飞行时间（TOF）激光雷达（LIDAR）。Loquercio 等（16）通过模仿学习，采用端到端方法，使无人机在复杂条件下自主飞行，最高速度超过 \(10 \mathrm{~m} / \mathrm{s}\)。

另一方法为Ren 等（13）提出的Bubble规划器，利用递归视界走廊进行轨迹优化，在复杂环境中实现最高 \(13.7 \mathrm{~m} / \mathrm{s}\) 的速度。这些工作固有问题是专注于飞行速度，忽视安全保障。为最大化飞行速度，现有方法（10-15, 17）均假设因遮挡或传感器视场受限造成的未知区域在轨迹规划时无障碍。这种乐观轨迹存在撞击未知障碍的风险，尤其是在复杂环境频繁遮挡下，高速飞行需要足够空间减速。此外，（16）中基于学习的方法未将安全考虑纳入控制策略，导致当飞行速度超过 \(10 \mathrm{~m} / \mathrm{s}\) 时，整体成功率较低（约 50% 到 80%）。成功率低及缺乏安全保证限制了这些方法（10-16）在现实中的部署。

另一类方法优先考虑飞行安全。第一类注重以安全感知方式提升飞行安全（18-20）。Quan 等（18）和 Wang 等（19）通过在接近未知空间时主动减速，实现安全感知；Zhou 等（20）则最大化轨迹上对未知空间的可视性。尽管这些方法提升了飞行安全，但牺牲了飞行速度。安全感知方法（18-20）中的实地实验显示，其设计较为保守，未能充分利用MAV的机动性，飞行速度不超过 \(5 \mathrm{~m} / \mathrm{s}\)。此外，这些方法依赖多种启发式策略实现安全感知，缺乏严格的安全保证。另一类方法采用安全保障策略，将轨迹完全限制在已知自由空间内（21, 22）。尽管能提供安全保障，安全保障方法（21,22）在复杂环境中飞行速度更保守。Tordesillas 等（23）提出的 Faster 方法部分克服了这种保守性，通过在已知自由空间轨迹的同时，规划一条覆盖已知和未知自由空间的额外轨迹。这种双轨迹规划显著提升了飞行速度，达到超过 \(8 \mathrm{~m} / \mathrm{s}\)，是以往方法（21,22）数倍速度，且不丧失安全保障。但 Faster 采用计算代价昂贵的占用格栅地图（OGM）识别已知自由空间，以及混合整数二次规划（MIQP）进行轨迹优化，导致计算延迟高，严重限制了飞行速度和成功率。

除了规划策略，机载传感能力在实现高速飞行中同样关键。现有自主MAV导航工作（14-16, 18-21, 23）多采用视觉传感器进行定位和障碍感知。视觉传感存在诸多限制，如探测范围有限（通常3至5米）、动态范围低（24）及易受强烈运动模糊影响。这些限制严重制约了飞行速度和安全水平。此外，视觉系统性能受光照条件影响，实地应用中照明不足或强光照变化大，进一步限制其实用性。

本文提出了一种安全保障高速空中机器人（SUPER），完成未知环境中安全高速飞行任务（图1及影片1）。SUPER的主要传感方式为三维激光雷达，具备70米探测距离和厘米级精度，且体积小、重量轻（25）。所有传感、计算及必需组件均机载（详见补充方法中的系统构成），SUPER实现推重比超过5.0 [相比之下F35战机满载推重比为0.87（26）]，轴距紧凑为280毫米 [相比DJI Mavic 3的380毫米（27）]，使其能在复杂环境中灵活飞行。为实现高速飞行并保证安全，我们采纳了（28）中提出的双轨迹规划策略，每个重规划周期规划两条轨迹：一条在已知自由空间内以确保安全，另一条覆盖已知及未知自由空间以提升飞行速度。通过重新设计规划与建图模块，我们使双轨迹策略的计算效率提升一个数量级。对于建图，我们提出直接基于激光点云区分已知自由空间的方法，根本上使得双轨迹策略能采用更高效的点云地图。轨迹规划方面，采用可微分轨迹参数化方法，具体为最小控制努力（MINCO）方法（29），提升轨迹优化效率并优化两轨迹间切换时机。计算效率的提升和切换时机的最优确定，显著提升了可达飞行速度与成功率，已通过基准测试验证。最后，点云地图有效保留了细小物体的测量，且实现厘米级精度环境表达，使无人机能够避开细小障碍物并穿越狭窄空间。SUPER对小型物体、复杂环境及光照变化的鲁棒性，拓展了其在自然及人工环境中的运行范围，实现昼夜全天候作业。因此，SUPER标志着高速自主导航从实验室向现实应用的关键里程碑。

结果

SUPER概述

我们通过大量仿真和真实环境实验，展示了SUPER的安全高速飞行能力。在超过1000次的模拟飞行中，SUPER的规划模块相比最先进的基线方法，将失败率降低了35.9至95.8倍，同时实现了显著更高的平均飞行速度。SUPER在真实场景中的性能也通过大量实地测试得到了验证。在所有实验中，SUPER需在完全机载感知、规划和控制的条件下，到达目标位置。连接目标与MAV当前位置的直线通常不安全且会碰撞，且环境在飞行前完全未知，要求SUPER在线检测并避开环境中的障碍物。在这些真实环境测试中，SUPER在未知、无结构环境中达到了 \(20 \mathrm{~m} / \mathrm{s}\) 的飞行速度，并在八次不同试验中保持了100%成功率（见图1B，i）。此外，SUPER在飞行安全方面优于先进商用MAV，能更好地避开细小障碍物，且适应高度杂乱和低光照环境。尤其是SUPER能够探测并避开野外的细小物体，如电力线和树枝（图1B，ii），并穿越极其杂乱的环境（图1B，iii），甚至在夜间飞行（图1B，iv）。SUPER还成功应用于目标跟踪、自主探索和航点导航任务（见图2），展示了其在真实场景中的鲁棒性和多功能性。部分飞行视频见影片1。

图1. 所提自主空中系统概览。(A) 雷达图展示SUPER与其他先进自主空中机器人对比，包括掌上型自主MAV（ZJU Swarm）（35）、开源灵巧四旋翼平台（Agilicious）（33）以及代表性商用无人机（27）。Agilicious数据取自文献（16）中其机载自主飞行的最佳表现。商用无人机的尺寸、推重比及APAS允许的最大飞行速度来自厂商官网（27）。机载计算能力和安全策略未公开，故未展示。(B) SUPER在真实飞行中的演示。(i) 野外高速飞行；(ii) 避开细电线；(iii) 穿越杂乱环境；(iv) 夜间飞行。

图2. SUPER的附加应用。目标跟踪示例，包括(A) 在密林中跟踪奔跑的人，(B) 黑暗室内场景，(C) 大型场景，(D) 在丘陵村庄道路上行驶的汽车。(A)(i) 和 (D)(i) 显示基于飞行后在线激光雷达测量合成的点云地图，目标轨迹和MAV轨迹分别以蓝色和红色标示。(A)(ii) 和 (D)(ii) 显示实验中的第三人称或第一人称视角快照。(E) (i) 在50×60平方米区域内的自主探索实验，SUPER执行轨迹以白线示意。(ii) 和 (iii) 分别为场景快照及对应的重建结果。(F) 航点导航示例。(i) MAV从起点 \(\mathbf{p}_{s}\) 出发，依次访问11个航点 \(\mathbf{p}_{1} \sim \mathbf{p}_{11}\)，轨迹基于时间编码颜色。(ii) 时序图像合成，展示环境变化和MAV飞行轨迹。黑色伪影由区域内随机移动的两人引起，MAV轨迹以蓝色突出显示。(iii 至 v) 展示MAV在被移动对象经过位置的飞行轨迹，SUPER能够利用移动物体先前穿过的空间。

未知环境中的安全高速导航

我们在未知森林环境中测试了SUPER的飞行速度（见图3和影片S1）。环境面积约为280×90平方米，树木粗细不一，植被多样（图3A，i）。我们在不同时间段共进行八次实验，涵盖从明亮白天到全黑夜晚的多种光照条件（图3A，ii至v）。八次实验中设置不同的最大速度限制，范围为5至 \(20 \mathrm{~m} / \mathrm{s}\)。所有实验中，SUPER从位置 \(\mathbf{p}_{s}\) 出发，依次到达四个航点 \(\mathbf{p}_{1} \sim \mathbf{p}_{4}\)，其中 \(\mathbf{p}_{4}\) 与 \(\mathbf{p}_{s}\) 重合（图3B，i）。这四个航点引导MAV穿越不同障碍密度区域（图3B，ii至iv）。

SUPER在全部八次实验中均成功，成功率为100%。图3C展示了八次实验中的速度分布及最大速度限制。实验6至8中SUPER达到最大飞行速度 \(20 \mathrm{~m} / \mathrm{s}\)。此外，实验6至8分别在白天、黄昏和夜间进行，均限速为 \(20 \mathrm{~m} / \mathrm{s}\)，SUPER在三种光照条件下表现稳定，显示出系统对光照变化的鲁棒性。除高速飞行外，SUPER能适应不同速度，所有实验均满足速度约束。图3D展示了八次实验中的位置跟踪误差分布。即使在 \(20 \mathrm{~m} / \mathrm{s}\) 速度下，SUPER平均跟踪误差仅为0.13米，体现高跟踪精度和飞行稳定性。

图3. 未知环境中的安全高速导航。(A) 实验环境鸟瞰图。(ii 至 v) 八次实验中的四种典型光照条件，(ii) 为测试1的拍摄图，(iii 至 v) 分别为测试6至8的拍摄图。(B) (i) 八次测试中执行的轨迹以不同颜色曲线表示。所有测试中，MAV依次飞越航点 \(\mathbf{p}_{5}, \mathbf{p}_{1}, \mathbf{p}_{2}, \mathbf{p}_{3}\) 并停于 \(\mathbf{p}_{4}\)。轨迹叠加了飞行中采集点测量线下合成的点云地图。(ii 至 iv) 飞行路径上环境的三处详细视图。(C 和 D) 小提琴图展示八次测试中飞行速度和位置跟踪误差分布。小提琴图形状表示数据密度，白点为均值，黑色误差线为标准差。数据通过均匀采样每次测试2000个飞行数据点获得。

杂乱环境中的导航

为验证SUPER在复杂环境中的导航能力，我们指令其在密林环境中跟踪慢跑者（图4A）。被跟踪者依次穿越两片林区：第一片树木相对稀疏（图4C），第二片树木密度较大，被跟踪者需降低身体通过（图4D）。我们通过两组独立实验，将SUPER与先进商用产品（27）进行导航和跟踪性能比较。两组实验中，被跟踪者大致沿同一路径和速度行进。考虑到商用无人机体积较SUPER大（半径0.31米对比0.21米），我们通过在SUPER四臂加装四根杆件将其尺寸增加至0.32米，以保证公平比较（图4B）。目标检测与跟踪方面，商用无人机使用视觉识别技术，SUPER则无此功能。为解决该问题，目标穿戴高反射背心，基于激光雷达点云的反射率测量，易于检测。两系统在整个任务中均成功检测并跟踪目标，确保导航能力的公平对比。

两款MAV的跟踪轨迹见图4A。初期目标处于开阔区域时，商用无人机与SUPER表现相当。目标进入第一片林区后，商用无人机未能继续跟踪，在林区前停下（图4A、C，位置A）。可能原因是商用无人机视觉导航的地图分辨率和精度有限（通常为数十厘米），需更保守的无碰撞轨迹以保障安全。随后，目标离开第一片林区，朝第二片林区跑去。商用无人机通过绕行路径恢复跟踪（图4A，位置A至B间的橙线）。但当目标进入密度更高的第二片林区时，商用无人机因上述原因完全丧失跟踪，退出自动跟踪模式（图4D，i）。相比之下，SUPER基于厘米级精度的激光雷达点云直接规划轨迹，能在杂乱环境中穿越狭窄通道。因此，SUPER在整个任务中表现出平滑且持续的目标跟踪，成功穿越两片林区，无商用无人机遭遇的停滞（图4C，ii及D，ii）。本实验中两款无人机的跟踪过程见影片S2。

图4. 杂乱环境中的导航。(A) 目标、商用无人机和SUPER在目标跟踪实验中的轨迹。目标从 \(\mathbf{p}_{\mathrm{s}}^{\text {target }}\) 出发，依次穿越两片林区，终点为 \(\mathbf{p}_{\mathrm{g}}^{\text {target }}\)。商用无人机在目标进入第一片林区前紧随其后，但在第一片林区未能找到无碰撞路径而停于位置A。目标离开第一片林区后，商用无人机通过绕行路径恢复跟踪。但当目标进入密度更高的第二片林区时，商用无人机再次失去跟踪，停于位置B。相比之下，SUPER在整个过程中持续跟踪目标，未出现停滞。点云地图基于SUPER飞行中在线采集的点测量离线合成。(B) 扩大尺寸的SUPER与商用无人机（27）示意图。(C) 目标进入第一片林区时的跟踪情况。商用无人机因空间狭窄未能跟踪，而SUPER能紧密跟踪目标。(D) 目标进入第二片林区时的跟踪情况。商用无人机再次失去跟踪，而SUPER能穿越目标需降低身体通过的狭窄空间。

避免细小物体

细小物体如电力线和树枝在野外任务中频繁出现且难以探测，对MAV构成挑战。为验证SUPER避开小物体的能力，我们开展一系列定量实验，并与商用无人机（27）性能进行对比。图5显示，我们选择了四根不同直径的细线：30、20、11和2.5毫米（图5A）。这些细线挂于两棵约3米间距的树间。为测试避开细线能力，MAV从细线一侧4米处起飞，目标位置设在对侧4米远。由于商用无人机不支持航点指令，我们使用操纵杆指令其从起点飞向目标。为测试商用无人机是否能探测并避开飞行路径上的细线，我们在飞行时开启其高级飞行员辅助系统（APAS）。飞行速度约为2至3 m/s，该速度也作为SUPER的速度约束，以确保公平比较。

实验结果如图5所示。在30毫米细线上，SUPER和商用无人机均成功避开细线并到达目标位置（图5B，i 和 5C，i）。然而，在20、11和2.5毫米直径的更细细线实验中，商用无人机未能探测并避开细线，导致碰撞细线并使其摆动（图5B，ii 至 iv）。商用无人机未能探测细线，可能归因于其基于视觉的导航系统在准确计算视差和生成深度图像方面的挑战。相比之下，SUPER在所有四次实验中均成功到达目标位置，甚至避开了最细的2.5毫米细线（图5C，i 至 iv）。这种卓越的障碍物规避能力得益于激光雷达传感器和规划模块：激光雷达在细小物体上提供点测量（图5D），规划模块中的点云地图有效保留了这些点以用于轨迹规划。SUPER面对小物体时的鲁棒性进一步增强了其现实任务的安全性。该实验的视频演示见影片S3。

图5. 细小物体规避演示。(A) (i 至 iv) 实验中使用的四根不同直径细线。(B) (i 至 iv) DJI Mavic 3飞行的延时照片。其成功避开直径为30毫米的细线，但未能避开更细细线。碰撞导致细线摆动，延时照片中出现伪影，尽管每次实验仅有一根细线。(C) (i 至 iv) SUPER飞行的延时照片，成功避开所有四种细线。(D) SUPER面对2.5毫米细线时的点云视图。细线在当前扫描测量（白点）中可见，累积点云（彩色点）中更为明显。

安全性、成功率和效率评估

我们通过一系列仿真实验评估了SUPER规划模块的性能。测试环境由一组尺寸固定为110×20米的随机生成类森林三维场景组成（图6A）。场景涵盖六种不同障碍密度，按可通行性（traversability）指标变化范围为3.1至6.5（30）。可通行性为障碍密度相对于机器人尺寸（本模拟中最大半径 \(r=0.2 \mathrm{~m}\)）的归一化度量，数值越高表示避障任务越简单。每个场景中，障碍物的位置和倾斜角随机生成。为保证多样性，每种密度使用不同随机种子生成10张地图，共计60张地图用于评估。我们将SUPER的性能与三种先进基线规划器对比：Bubble规划器，一种乐观规划策略（13）；Raptor，一种安全感知方法（20）；Faster，一种安全保障方法（23）。四个规划器的实现细节汇总见表S1。每个规划器在每张地图上运行18次，最大速度从1至 \(18 \mathrm{~m} / \mathrm{s}\) 不等。最大加速度固定为 \(20 \mathrm{~m} / \mathrm{s}^{2}\)。每次实验中，规划器需使MAV从起点飞向100米外目标，实验在三种条件之一时结束。“成功”指MAV无碰撞且满足所有运动学约束（速度和加速度）；“碰撞”指飞行中与障碍物发生碰撞；“未完成”指在达到目标前任一重规划周期内，规划器未能在30秒内返回轨迹。

我们在图6B中通过安全率评估了所有基准方法的安全性，安全率定义为成功和未完成案例占所有实验的比例。在1080次不同飞行速度和障碍密度的实验中，SUPER无碰撞且轨迹始终可行，实现了完美的安全率。Faster（23）作为一种安全保障方法，原则上因每次重规划周期中规划备份轨迹，类似于SUPER。但我们发现其备份轨迹未受限于包含未知与占用信息的地图区域，导致地图未及时更新时发生碰撞。Faster碰撞率达12.78%，整体安全率为87.22%。Bubble（13）安全率为68.80%；其余31.20%的碰撞由其将未知空间视为自由的乐观规划策略引起。Raptor（20）碰撞率为38.33%，主要因高速度飞行时规划轨迹频繁违反动力学约束，模拟MAV难以跟踪，导致与障碍碰撞，安全率仅61.67%。综上，SUPER在所有实验中均实现安全飞行，表现优于其他基准方法，凸显其卓越的安全保障特性。

除安全外，MAV是否完成任务（如到达目标）及完成任务的速度同样重要。我们在图6D分析了成功率（成功次数占比）和各方法的飞行速度。SUPER在几乎所有障碍密度下均达成最高平均飞行速度，成功率接近完美，达99.63%（图6B）。相比之下，Faster（23）因建图范围有限，平均速度较低。此外，Faster将轨迹优化表述为MIQP问题，既难以找到需复杂时间优化的高速轨迹，又计算耗时大（图6E）。高计算时间限制了探索和备份轨迹的规划视野。因两轨迹在每次重规划均以零速终止，短规划视野导致低速轨迹。MIQP频繁超时也使MAV更常执行备份轨迹（图6C），进一步降低整体飞行速度。乐观规划策略Bubble（13）和安全感知规划方法Raptor（20）在某些障碍密度下飞行速度偶尔超过SUPER，但以成功率降低为代价。总体来看，SUPER将任务失败率相比其他方法降低了35.9至95.8倍，同时保持更高或相当的飞行速度。

我们分析了所有基准方法的时间消耗（图6E）。各方法的计算时间分为建图和轨迹规划，轨迹规划进一步细分为前端和后端。由于直接基于点云规划，SUPER和Bubble的建图时间开销最小（1至5毫秒），点云来自激光雷达原始测量，处理简单（如刚性变换）。Faster使用占用格栅地图（OGM），需枚举激光雷达扫描中所有点穿过的格子，导致建图时间最长达47毫秒。Raptor使用欧几里得带符号距离场（ESDF）地图，由OGM进一步计算每格距离信息（31），建图时间更长达69毫秒。轨迹规划方面，SUPER、Bubble和Raptor均采用多项式或B样条参数化轨迹，可对轨迹参数求导，计算效率高。 相比之下，Faster的MIQP轨迹优化问题不可微且难解，计算时间最长达41毫秒。结果表明，Bubble和SUPER的总计算时间最短（Bubble为8至44毫秒，SUPER为10至47毫秒），SUPER略高于Bubble因需规划额外备份轨迹。Raptor耗时67至83毫秒，Faster耗时67至91毫秒，均高于SUPER。

图6. 安全性、成功率及效率评估。(A) 不同可通行性环境的基准测试场景。(B) 1080次实验中基准方法的飞行结果。(C) SUPER和Faster在不同障碍密度下切换至备份轨迹的比例分布，基于180次测试计算。白点为均值，误差线为标准差。(D) 基准方法在不同障碍密度和飞行速度下的成功率。空白列表示对应速度和密度组合未达成实验。(E) 基准方法的计算时间消耗，方块为均值，误差线为总计算时间的标准差。均值和标准差均基于180次测试计算。

应用

我们展示了SUPER在三个代表性应用中的使用：目标跟踪、未知空间的自主探索以及动态环境中的航点导航（影片S4）。

在目标跟踪中，我们反复将目标后方位置设为SUPER的局部规划目标，指令其跟踪目标。我们进行了大量实验，评估SUPER跟踪人和汽车的性能，如图2（A至D）所示。所有实验中，目标均佩戴高反射材料，使其在基于反射率测量的激光雷达点云中可靠检测。目标采用简单的匀速模型进行跟踪。结果表明，SUPER成功跟踪目标并在所有实验中避开障碍，无论目标速度（1至 \(8 \mathrm{~m} / \mathrm{s}\)）、室内外环境、昼夜光照条件及不同场景尺度均表现优异。

在自主探索应用中，MAV被指令探索未知空间。为完成任务，全球规划器Bubble Explorer（32）在线生成一组航点，旨在以最小成本（如总飞行距离）观察最大未知空间。随后，SUPER作为局部规划器，安全地到达这些航点。实验中，SUPER无碰撞完成全部航点探索，120秒内覆盖3000平方米环境。图2（E，i至iii）展示了探索后的场景快照及建图结果。

最后，我们测试了SUPER在动态环境中的飞行能力（图2F）。基于点云的规划方法常见问题是错误地将移动物体经过的空间视为占用，因点云包含移动物体点。SUPER实现了时效性递减点云地图（材料与方法），避免了该问题。实验中，MAV从 \(\mathbf{p}_{s}\) 出发，依次飞越一系列航点（\(\mathbf{p}_{1}\) 至 \(\mathbf{p}_{11}\)；见图2F，i），同时两人在同一区域随机移动（图2F，ii）。SUPER成功利用移动物体留下的空闲空间，避免与移动或静止障碍物碰撞，顺利完成所有航点。结果凸显了SUPER在复杂未知环境中的卓越导航能力。

讨论

自主空中机器人

SUPER设计用于依赖机载传感与计算，在未知环境中实现高速导航。灵活性与机载计算能力是两大关键因素。SUPER尺寸为280毫米，推重比5.0，体现高机动性。机载计算单元为搭载12核、4.7 GHz的Intel NUC 12，具备充足计算能力，支持高效在线优化探索轨迹（实现高速飞行）和备份轨迹（保障安全）。高机动性与机载计算能力使SUPER能在未知杂乱环境中实现超过 \(20 \mathrm{~m} / \mathrm{s}\) 的高速飞行且无碰撞。SUPER的安全保障与高速飞行能力使其在现有自主空中机器人中独树一帜（图1A）。Agilicious（33）推重比超5.0，尺寸与SUPER相近，但其基于模仿学习的规划（16）飞行速度较低（\(10 \mathrm{~m} / \mathrm{s}\)），无安全保障，高速飞行成功率低（\(10 \mathrm{~m} / \mathrm{s}\) 时约60%）。且机载计算单元为Nvidia TX2（34），4核1.9 GHz CPU，计算能力有限，限制了其应用于计算需求更高任务（如自主探索）。另一方面，ZJU Swarm（35）聚焦小型轻量自主MAV设计，用于蜂群应用，轴距仅110毫米，重量不足300克，但推重比仅2.4，且因重量限制机载计算资源有限，自主飞行最大速度仅 \(3 \mathrm{~m} / \mathrm{s}\)（35）。作为代表性商用MAV（27），其轴距380毫米大于SUPER，推重比仅2.3，限制了其在杂乱环境中的高速飞行能力。该商用机配备APAS，能探测并避开飞行路径潜在障碍，APAS支持最大飞行速度为 \(15 \mathrm{~m} / \mathrm{s}\)（27），仍低于SUPER自主飞行速度。

基于激光雷达的MAV导航

SUPER采用三维激光雷达实现高速导航。相比广泛应用于学术研究（14-16, 18-21, 23）和商用MAV（27, 36）的摄像头，激光雷达具备提升MAV飞行速度、安全性和效率的关键优势。首先，激光雷达提供直接、精确（厘米级）且远距（数十至数百米）的深度测量。远距测量使MAV能感知并避开前方远处障碍，实现高速飞行，精确测量支持MAV穿越杂乱环境中的狭小空间。视觉方法基于视差和三角测量估计深度（37），计算复杂且精度随距离平方递减。现有先进视觉传感器，如Intel RealSense（38）有效测距仅3至5米。其次，激光雷达以极高频率测量三维点，频率高达数十万至百万赫兹。高频测量可估计极快的MAV运动（39），而传统RGB或RGB-D摄像头因长曝光和低动态范围易受运动模糊影响。第三，激光雷达采用飞行时间（TOF）测距，能探测直径低于1厘米的细小物体。视觉（包括RGB、RGB-D及事件摄像头）在探测如电力线等细小物体时效果不佳，因视差计算挑战大。最后，激光雷达基于主动测量机制，即使在低光环境（如夜间）也表现可靠，超越视觉传感器能力。

基于三维激光雷达的MAV导航非首次尝试。如多篇工作（10, 11, 40-42）使用VLP-16激光雷达于多旋翼无人机，另有工作（29, 43-45）使用Ouster OS1-128（46）。这些工作面临的关键问题在于激光雷达传感器体积庞大、重量重且成本高昂：前两因素导致无人机体积大（轴距330至1133毫米）且推重比低（通常约2.0（11,29,40,41,43-45,47,48）），限制了机动性。高成本阻碍激光雷达在无人机导航中的广泛应用，更不用说对系统风险更大的高速灵巧飞行。现有激光雷达无人机飞行速度仅为2至 \(5 \mathrm{~m} / \mathrm{s}\)（40,41,44）。Zhang 等（10, 11, 42）实现了 \(10 \mathrm{~m} / \mathrm{s}\) 的激光雷达无人机飞行，但其无人机体积大（轴距1133毫米）、重量超13公斤，且仅在静态稀疏林区（障碍物间距超10米）示范，飞行非常平稳。SUPER得益于激光雷达技术的快速进展，实现了固态激光雷达的商业化与量产，成本和重量大幅降低。SUPER采用Livox MID360激光雷达（25），重量265克，尺寸65×65×60毫米（长×宽×高），功耗约6.3瓦。尽管激光雷达仍较典型深度摄像头重大且功耗高（如RealSense D435i（49）重72克，尺寸90×25×25毫米，功耗1.6瓦），凭借紧凑机体设计，SUPER轴距仅280毫米，比最先进视觉MAV如DJI Mavic 3（380毫米轴距）更小。且SUPER推重比超过5.0，远超现有激光雷达无人机约2.0的典型推重比。SUPER紧凑体积与高推重比带来了其他作品难以比拟的机动性。

尽管激光雷达在测距范围、分辨率和精度方面优于深度摄像头，利用激光雷达测量实现安全高速飞行仍具挑战，需设计真实反映激光雷达测量环境的地图。主流自主系统（12,14,15,20,23,40）采用占用格栅地图（OGM）（50,51）或欧几里得带符号距离场（ESDF）地图（31）。但这些地图因射线投射或距离场更新计算量大，且受限于射线投射空间分辨率，无法有效建图细小物体（见图S5）。另一选择是点云地图（10,11,13,41,42,52），维护高效，且因避免射线投射能有效保留细小物体点测量。但点云地图难以区分已知自由空间和未知空间，导致现有基于点云的规划方法（10,11,13,41,42,52）常将未知区视为自由，无法保证避开遮挡障碍，且在复杂环境成功率低。我们提出了直接从点云地图区分已知自由空间的方法（补充方法中定理1），使SUPER规划备份轨迹确保飞行安全。因此，SUPER较先前激光雷达导航方法（13,42）实现更高飞行速度和成功率（详见补充方法基准比较）。高精度点云地图还使SUPER能在野外避开细小障碍并安全穿越狭窄空间。最后，点云地图因移动物体或激光雷达误测产生过时点，带来挑战。我们采用时空滑动机制解决该问题，使SUPER能利用移动物体穿越过的区域。

Fig. S5. OGM 上细线检测的视觉示例 (A) 无人机面对细线（绿色显示）的第三人称视角。 (B) 检测过程的侧视图。灰色框代表占据栅格地图（OGM）中包含细线的栅格。蓝线表示穿过该栅格但未击中细线的 LiDAR 光束，红线表示击中细线的光束。由于大多数光束未检测到细线，该栅格被错误地分类为自由空间。

安全保障的高速轨迹规划

SUPER通过每次计算两条轨迹实现高速且安全的规划：一条在已知自由空间和未知空间的探索轨迹，另一条仅在已知自由空间的备份轨迹。这一双轨迹策略最初由Faster（23, 28）提出。SUPER采纳该策略，但在感知、建图和规划方面采用了完全不同的设计，优化了飞行速度、成功率和计算效率。最终，SUPER仅凭机载感知与计算设备，在未知野外实现了超过 \(20 \mathrm{~m} / \mathrm{s}\) 的飞行速度。

在感知方面，Faster（23）使用深度摄像头（38）进行障碍检测，测距范围仅3至5米，限制了系统飞行速度。此外，Faster采用外部动作捕捉系统进行状态估计，限制其在常无测量设备的真实环境中应用。相比之下，SUPER使用轻量级激光雷达（25），测距范围达70米，支持远距离障碍探测与规避。长距离探测是实现本工作高速飞行的基础。 SUPER还使用自主开发的激光雷达惯性测程系统FASTLIO2（39）进行状态估计，完全依赖机载设备，适应无先验设备如无全球导航卫星系统（GNSS）环境。

双轨迹策略（Faster与SUPER共同采用）关键在于区分环境中的已知自由空间。Faster使用占用格栅地图（OGM）维护信息，但OGM更新计算量大，因射线投射操作（50）耗时。为保证实时性，Faster仅更新5米范围OGM，平均计算时长47毫秒。有限建图距离限制规划视野，导致轨迹短且飞行速度低。SUPER直接基于激光雷达点云区分已知自由空间（材料与方法），摒弃耗时OGM，且保留最长70米测距点云，更新仅需1至5毫秒。

规划方面，Faster将探索与备份轨迹优化均表述为混合整数二次规划（MIQP）问题。MIQP不可微、求解困难，计算时间长，偶有约束违例，产生局部最优且速度受限轨迹。高计算时间导致频繁超时，使MAV常切换至低速备份轨迹，且规划视野受限，进一步限制速度。SUPER采用多段多项式参数化轨迹（材料与方法），参考（29），可用梯度法高效优化时空参数。SUPER轨迹优化耗时为Faster的一半，且满足所有约束。另一区别在于切换时间确定方式。Faster基于野蛮假设（备份轨迹沿探索轨迹减速）在备份轨迹优化前启发式确定切换时间。SUPER则将切换时间纳入备份轨迹优化同步确定。SUPER优化的切换时间通常远大于Faster启发式值，减少备份轨迹飞行时间及切换频率（图S6）。

限制与未来方向

硬件方面，更小、更轻、测距更远、点云更密集的激光雷达传感器出现，为自主空中系统带来新机遇。尽管SUPER采用的激光雷达已较以往体积和重量大幅减小，仍较视觉传感器大且重，限制了MAV平台的进一步微型化，制约其在更狭小空间（如宽度不足0.5米的窄通道）中的运行。采用更轻激光雷达可提升无人机推重比，进一步增强机动性。此外，激光雷达测距范围和点云密度仍有提升空间，长距离和更密集点云可提供更详细环境信息，使SUPER能在更高速下避开更细物体。无人机空气动力学设计亦可优化。目前SUPER仅在软件层面考虑空气阻力，通过识别阻力系数并在控制模块补偿。但若从空气动力学设计降低空气阻力，无人机飞行速度和稳定性或将进一步提升。

算法层面，虽SUPER已展示处理动态障碍能力，当前实现尚未主动检测移动物体或预测其运动，限制了其在高度动态环境中的飞行能力。通过主动识别移动物体（53）及预测运动，规划模块可生成更平滑安全轨迹，提升飞行安全并降低能耗。此外，进一步并行化SUPER软件实现，利用硬件加速器如GPU（图形处理单元）可提升系统效率，减少延迟，进而提升飞行速度。

未来，SUPER具备广泛应用潜力。其关键优势在于重视飞行安全、高速能力和较低计算需求，有望推动多种自主无人机应用，包括自主探索、物流运输、基础设施检测及搜救等任务，这些领域安全及时抵达目的地尤为关键。另一显著特性是SUPER的鲁棒性、多功能性和可扩展性，能在多样无结构环境和各种光照条件下运行，实现全天候连续作业。从硬件和软件视角看，SUPER有效应对复杂真实环境导航挑战，为自主机器人从实验室走向现实应用提供宝贵经验。

材料与方法

灵活的基于激光雷达的MAV平台

SUPER的设计目标是在尽可能减小尺寸的同时实现高推重比（TWR）。经过全面调研，最终确定了如图7A所示的SUPER设计。该平台起飞重量为1.5公斤，轴距仅280毫米。电机选用T-Motor F90型号（54），单个电机最大推力23.6牛顿，总推力可达94.4牛顿，理论理想推重比约6.3。由于气流干扰和阻塞等因素，实际推重比略高于5.0，支持高度灵活的机动。

计算部分配备运行PX4飞控软件的飞控单元（FCU）（55）和搭载12核4.7 GHz i7-1270P CPU的Intel NUC机载电脑。

感知方面，搭载轻量级三维激光雷达（25），探测距离超70米，点频率高达20万赫兹。

飞控上的惯性测量单元（IMU）提供200 Hz频率的加速度和角速度测量。

激光雷达点云和IMU数据输入导航软件，包括基于激光雷达的状态估计与障碍建图模块、用于生成安全高速轨迹的规划模块及轨迹跟踪控制器，均运行于机载电脑（见图7）。

轨迹跟踪控制器输出期望角速度（俯仰、横滚和偏航率）和推力加速度，由飞控上的低级控制器执行。

图7. SUPER系统概览。(A) SUPER硬件配置。(B) 感知模块由状态估计和建图两部分组成。(C) 规划模块基于点云地图实现安全保障规划。规划轨迹包括探索轨迹 \(\mathcal{T}_{\mathrm{e}}\)（涵盖已知自由空间与未知空间）和备份轨迹 \(\mathcal{T}_{\mathrm{b}}\)（起始于探索轨迹上某状态，完全位于已知自由空间内）。探索轨迹部分和备份轨迹整体组成已提交轨迹 \(\mathcal{T}_{\mathrm{c}}\)，由MAV执行。(D) 控制模块为精准高效的优化模型预测控制（OMMPC）。

基于激光雷达的感知

感知模块（图7B）由状态估计和建图两部分组成。

状态估计基于我们自主研发的激光雷达惯性测程系统FAST-LIO2（39），实现低延迟的九自由度状态估计（位置、速度、姿态），频率200 Hz，延迟不足1毫秒。该系统同时输出以世界坐标系注册的点云，扫描频率50 Hz，每次约4000点，累计每秒约20万个点，延迟低于10毫秒。

建图模块提出基于点云的时空滑动点云地图，用以表示环境中的占用空间。为防止点云随时间累积变得过密，采用均匀网格数据结构进行降采样。该结构将空间划分为分辨率为0.05至0.3米的均匀网格，仅保留每个占用网格单元的中心点。为避免无必要的大规模地图，我们采用零拷贝滑动策略，仅维护MAV周围50至100米的局部地图，参考先前工作（51）。点云地图面临的常见问题是激光雷达点同时包含静态环境和移动物体历史位置，直接视为占用，误将移动物体路径空间归为占用。为解决该问题，采用时序滑动策略，剔除时间窗口 \(t_{\mathrm{d}}\) 外的点。具体为每网格单元跟踪最后被激光雷达击中的时间 \(t_{\text {hit }}\)，当前时间为 \(t_{\mathrm{c}}\) 时，计算 \(\Delta t = t_{\mathrm{c}} - t_{\text{hit}}\)。若 \(\Delta t < t_{\mathrm{d}}\)，该网格视为占用，否则视为自由。

点云地图构建过程为：新激光雷达扫描到来时，定位被新点击中的网格单元，更新其 \(t_{\text{hit}}\) 为新点时间戳。为降低地图更新延迟，对过时网格采用延迟更新策略，仅在轨迹规划模块查询时剔除。地图更新计算复杂度仅依赖扫描中的点数，与感知距离无关，故能高效利用激光雷达长距离特性完成远距更新。相比之下，占用格栅地图（OGM）（50）或基于ESDF的地图（31）需更新测距范围内所有网格，计算量大且建图范围短，通常约5至10米。

安全保障的高速轨迹规划

框架概述

为实现安全高速轨迹规划，采用双轨迹规划策略，在每次重规划周期规划两条轨迹（图7C）。第一条为探索轨迹 \(\mathcal{T}_{\mathrm{e}}: \mathbf{p}_{\mathrm{c}} \rightarrow \mathbf{p}_{\mathrm{g}}\)，由MAV当前位姿 \(\mathbf{p}_{c}\) 扩展至目标位置 \(\mathbf{p}_{g}\)。为实现高速飞行，探索轨迹将未知空间视为自由空间，规划范围涵盖已知自由空间及未知空间。第二条为备份轨迹 \(\mathcal{T}_{\mathrm{b}}: \mathbf{p}_{\mathrm{s}} \rightarrow \mathbf{p}_{\mathrm{l}}\)，起始于探索轨迹上时刻 \(t_{s}\) 的位置 \(\mathbf{p}_{s}\)，终止于零速度位置 \(\mathbf{p}_{l}\)。为保障安全，备份轨迹及探索轨迹从 \(\mathbf{p}_{\mathrm{c}}\) 到 \(\mathbf{p}_{\mathrm{s}}\) 的段落均须位于已知自由空间。若探索轨迹全处于已知自由空间（如MAV接近目标时），则无需规划备份轨迹。最终已提交轨迹 \(\mathcal{T}_{\mathrm{c}}: \mathbf{p}_{\mathrm{c}} \rightarrow \mathbf{p}_{\mathrm{s}} \rightarrow \mathbf{p}_{\mathrm{l}}\) 由探索轨迹（\(\mathbf{p}_{\mathrm{c}}\) 到 \(\mathbf{p}_{\mathrm{s}}\)）和备份轨迹（\(\mathbf{p}_{\mathrm{s}}\) 到 \(\mathbf{p}_{\mathrm{l}}\)）组成，并传递至控制模块执行。

上述两轨迹以固定10 Hz频率重规划。 每次重规划时，依据外部指定目标位置 \(\mathbf{G}\)（如航点导航任务中的下一个航点，探索任务中的下一个视点）确定探索轨迹目标位置 \(\mathbf{p}_{\mathrm{g}}\)。具体为，若当前位置 \(\mathbf{p}_{\mathrm{c}}\) 与目标 \(\mathbf{G}\) 距离大于规划视野 \(H\)，则将 \(\mathbf{G}\) 投影至以 \(\mathbf{p}_{\mathrm{c}}\) 为中心、半径为 \(H\) 的球面得到 \(\mathbf{p}_{\mathrm{g}}\)；否则 \(\mathbf{p}_{\mathrm{g}} = \mathbf{G}\)。当探索轨迹和备份轨迹均成功生成，或探索轨迹完全处于已知自由空间时，视为重规划成功，更新已提交轨迹至控制模块；否则视为失败，MAV执行上一次已提交轨迹。由于上一次已提交轨迹位于已知自由空间，执行它可确保避免碰撞，保障飞行安全，即便规划失败。

探索轨迹和备份轨迹均通过走廊约束轨迹优化规划。该方法先生成飞行走廊，再在走廊内优化平滑轨迹（图8A）。飞行走廊由一系列凸形状（如立方体（56）、球体（13,41）、多面体（40,23,12））表示从起点至目标的无碰撞空间。在双轨迹框架中，需生成两个多面体走廊：探索走廊涵盖已知自由空间和未知空间，备份走廊仅涵盖已知自由空间。随后在各自走廊内优化探索和备份两条平滑轨迹。后续章节将介绍如何基于激光雷达点云直接生成探索和备份走廊，以及如何在走廊内优化平滑轨迹。

场景空间中的飞行走廊生成

我们将探索走廊和备份走廊均表示为一组重叠多面体，直接从激光雷达点云输入中提取，以提高计算效率。对于探索走廊，目标是找到连接起点 \(\mathbf{p}_{s}\) 与目标位置 \(\mathbf{p}_{g}\) 的一组重叠多面体。为此，首先使用 \(A^{*}\) 路径搜索算法（57）找到连接 \(\mathbf{p}_{\mathrm{c}}\) 和 \(\mathbf{p}_{\mathrm{g}}\) 的无碰撞路径 \(\mathcal{G}\)（图8B，i），路径由一系列步长等于地图分辨率（如0.1米）的离散位置组成。接着沿路径 \(\mathcal{G}\) 反复寻找线段：从上条线段末端出发，寻找最远可见点。具体为，从 \(\mathbf{p}_{\mathrm{c}}\) （\(\mathcal{G}\)的首点）开始，找到最远可见点 \(\mathbf{s}_{1}\)，形成线段（\(\mathbf{p}_{\mathrm{c}}, \mathbf{s}_{1}\)）；再从 \(\mathbf{s}_{1}\) 出发，找到最远可见点 \(\mathbf{s}_{2}\)，形成线段（\(\mathbf{s}_{1}, \mathbf{s}_{2}\)）；依此类推，直至目标位置 \(\mathbf{p}_{\mathrm{g}}\) 可见（图8B，ii）。最后，利用每条线段作为凸分解的种子，提取包含该线段且不含任何输入障碍点的多面体（图8B，iii）。

图8. 双轨迹规划策略。(A) 基于走廊的轨迹生成方法示意，先生成一系列重叠多面体 \(\mathrm{P}_{i}\) 表示的走廊，再在走廊内优化平滑轨迹。轨迹从 \(\mathbf{p}_{0}\) 起始，至 \(\mathbf{p}_{g^{\prime}}\) 终止，通过中间位置 \(\mathbf{q}_{i}\) 连接各段轨迹，采用多段多项式参数化，\(T_{i}\) 为各段时间长度。\(\mathbf{a}_{i}\) 为相邻多面体重叠区中心。(B) 探索轨迹生成过程包含(i)基于A*的路径搜索，(ii)线段种子生成，(iii)凸分解，随后轨迹优化。(C) 备份走廊生成过程包含(i)搜索探索轨迹上的种子，(ii)凸分解，(iii)基于激光雷达视场裁剪走廊（可选）。(D) 探索轨迹与备份轨迹间切换时间 \(t_{\mathrm{s}}\) 及对应切换位置 \(\mathbf{p}_{\mathrm{s}}\) 的影响。切换时间 \(t_{\mathrm{s}}\) 优化约束为 \(t_{\mathrm{c}} < t_{\mathrm{s}} < t_{\mathrm{o}}\)。

凸分解需在机器人场景空间中进行以考虑无人机体积。现有方法如快速迭代区域膨胀（FIRI）（58）、带线搜索的区域膨胀方法（40）及其前身带半正定规划的迭代区域膨胀方法（59）通过点膨胀或平面收缩处理机器人体积。我们提出场景空间迭代区域膨胀 （CIRI） 新方法。CIRI继承了FIRI（58）中的迭代优化过程及基于二阶锥规划的最大内接椭圆体积算法，但区别于前述方法（59,40,58），CIRI直接对输入点云操作，在无人机场景空间内提取多面体，无需先验地图膨胀（3,12,60）或多面体收缩（40,61）。此外，CIRI采用平面调整策略，确保场景空间生成的多面体包含线段种子。相较于现有点膨胀和平面收缩策略（62,58），CIRI获得的多面体体积更大，计算时间更短，且保证场景空间中完全包含线段种子（见图S8）。CIRI方法细节见补充材料。

备份走廊生成

基于CIRI，我们可高效地直接从激光雷达点云生成探索走廊。但备份走廊代表已知自由空间，点云缺乏已知与未知区分信息，生成困难。为克服此限制，我们提出统一探索与备份走廊生成的CIRI框架方法，完全依赖点云数据，摒弃基于射线投射的占用地图。该方法利用激光雷达传感器提供的密集高精度深度测量，基于定理1（见补充方法），即当输入点云构成足够密集的深度图，且输入种子包含当前位姿 \(\mathbf{p}_{c}\) 时，CIRI提取的多面体 \(S\) 将位于已知自由空间。原因在于多面体 \(S\) 为凸体，满足定理1凸性条件；且不包含输入点云（或深度图 \(\mathcal{D}\)）中的点，满足地图排除点的要求；包含种子，保证当前位姿 \(\mathbf{p}_{c}\) 处于走廊内。因而该多面体可作为备份走廊。

基于定理1，详细过程见图8C。种子选取为从当前位姿 \(\mathbf{p}_{\mathrm{c}}\) 出发、至探索轨迹 \(\mathcal{T}_{\mathrm{e}}\) 上 \(\mathbf{p}_{\mathrm{c}}\) 可视的最远点 \(\mathbf{s}\) 的线段（图8C，i）。目的在于利用CIRI算法提取的包含种子（\(\mathbf{p}_{\mathrm{c}}, \mathbf{s}\)）的备份走廊与探索轨迹对齐，进而包含大量探索轨迹。考虑激光雷达非无限密集，为形成足够密集深度图，将近期激光雷达扫描（如1至2秒）累积输入CIRI及线段种子，获得凸多面体 \(S\)（图8C，ii）。根据定理1，多面体 \(S\) 内区域保证为已知自由空间。若激光雷达视场有限，可使用多面体 \(S\) 与视场凸子集的交集（图8C，iii），该交集形成另一多面体，位于已知自由空间内。此外，探索走廊首个多面体亦须与MAV视场相交，确保探索轨迹初段处于已知自由空间，提升备份轨迹优化的可行性。视场凸子集的选择方法及其影响分析见补充材料。

轨迹优化

给定前节生成的探索走廊和备份走廊，接下来考虑如何在其内优化平滑轨迹。探索轨迹生成时，优化高速轨迹具有挑战性，不仅需对轨迹形状进行空间优化，还需对时间分配进行时序优化。

目标是求解一条平滑轨迹 \(\mathbf{p}(t):[0, t_{M}] \rightarrow \mathbb{R}^{3}\)，使飞行时间 \(t_{M}\) 及由 \(s\) 阶导数 \(\|p^{(s)}\|^{2}\)（本文中 \(s=4\)）表示的控制努力最小，同时满足所有动力学和避障约束。动力学约束包含最大速度与加速度限制，避障通过轨迹位置受飞行走廊约束实现。考虑到走廊结构，轨迹采用多段多项式参数化，参考（29）。具体而言，给定由 \(M\) 个连续重叠多面体 \(\mathcal{P}_{i}\) 组成的探索走廊 \(\mathcal{C}_{\mathrm{e}}\)，轨迹 \(\mathbf{p}(t)\) 和飞行时长 \(t_{M}\) 分为 \(M\) 段，第 \(i\) 段轨迹 \(\mathbf{p}_{i}(t)\) 持续时间为 \(T_{i}\)，且包含于多面体 \(\mathcal{P}_{i}\)（图8A）。相邻两段轨迹 \(\mathbf{p}_{i}(t)\) 和 \(\mathbf{p}_{i+1}(t)\) 在位置 \(\mathbf{q}_{i} \in \mathbb{R}^3\) 连续，并在 \(s-1\) 阶导数上连续。设初始状态 \(\mathbf{s}_{0} = \mathbf{p}^{(0:s-1)}(0)\) 为当前MAV状态，终止状态 \(\mathbf{s}_{\mathrm{f}} = \mathbf{p}^{(0:s-1)}(t_{M})\) 为路径终点且各阶导数为零，第 \(i\) 段多项式轨迹由中间点 \(\mathbf{Q} = (\mathbf{q}_1, \ldots, \mathbf{q}_{M-1})\) 和时间分配 \(\mathbf{T} = (T_1, \ldots, T_M)\) 参数化，形式如下：

\[ \mathbf{p}_i(t) = \mathbf{C}_i(\mathbf{Q}, \mathbf{T}, \mathbf{s}_0, \mathbf{s}_f) \beta(t), \quad t \in [0, T_i], \quad i=1,2,\ldots,M \tag{1} \]

其中，\(\beta(t) = [1, t, \ldots, t^{2s-1}]^\mathrm{T}\) 为基函数，\(\mathbf{C}_i\) 是多项式系数矩阵，通过解线性系统从 \(\mathbf{Q}\) 和 \(\mathbf{T}\) 得到（29）。此外，\(\mathbf{C}_i\)（及轨迹 \(\mathbf{p}_i(t)\)）对 \(\mathbf{Q}\) 和 \(\mathbf{T}\) 可微，偏导数详见（29）。

基于式(1)的参数化，轨迹优化目标函数和约束表达为：

\[ \begin{gather*} \min_{\mathbf{Q}, \mathbf{T}} \int_0^{t_M} \| \mathbf{p}^{(s)}(t) \|_2^2 dt + \rho_T t_M + \rho_u \mathcal{V} \tag{2} \\ \text{s.t.} \quad \mathbf{p}(t) = \mathbf{C}(\mathbf{Q}, \mathbf{T}, \mathbf{s}_0, \mathbf{s}_f) \beta(t) \tag{3} \\ t_M = \sum_{i=1}^M T_i \tag{4} \\ \|\dot{\mathbf{p}}\|_2^2 \leq v_{\max}^2, \quad \|\ddot{\mathbf{p}}\|_2^2 \leq a_{\max}^2 \tag{5} \\ \mathbf{p}_i(t) \in \mathcal{P}_i, \quad \forall i \in [1,M], \quad t \in [0, T_i] \tag{6} \end{gather*} \]

其中，\(\rho_T > 0\) 为飞行时间惩罚权重，旨在实现高速飞行；\(\rho_u > 0\) 为软约束惩罚权重。约束(5)保证动力学限制，约束(6)保证轨迹位置受飞行走廊约束。目标函数中的项

\[ \mathcal{V} = \sum_{i=1}^M \mathcal{L}_\mu \left( \| \mathbf{q}_i - \mathbf{a}_i \|_2^2 \right) \tag{7} \]

其中，\(\mathbf{a}_i\) 为多面体 \(\mathcal{P}_i\) 和 \(\mathcal{P}_{i+1}\) 重叠区域中心位置。该项用于避免轨迹过于靠近飞行走廊边界（常为障碍物位置），保持距离有利于机载传感器视野，进而生成更大已知自由空间供备份轨迹规划。\(\mathcal{L}_\mu\) 是二阶连续屏障函数（见图S9），取较大 \(\mu\)（如0.5）使惩罚较软，保证中间点 \(\mathbf{q}_i\) 被吸引至 \(\mathbf{a}_i\)，同时保留一定灵活性，避免过度约束优化问题。为求解式(2)的优化问题，我们采用（29）中的方法，将约束问题转为无约束形式。由于轨迹（及目标函数和约束）对优化变量 \(\mathbf{Q}\) 和 \(\mathbf{T}\) 可微，无约束问题可高效采用基于梯度的L-BFGS求解器（63）求解。求解得到的探索轨迹记为 \(\mathcal{T}_{\mathrm{e}}(t)\)。

备份轨迹需从探索轨迹 \(\mathcal{T}_{\mathrm{e}}(t)\) 上某时刻 \(t_{\mathrm{s}}\) 的状态 \(\mathbf{s}_0 = \mathcal{T}_{\mathrm{e}}(t_{\mathrm{s}})\) 出发，终止于已知自由空间中位置 \(\mathbf{p}_l\)，终止速度与加速度均为零（\(\mathbf{s}_f = (\mathbf{p}_l, \mathbf{0}, \ldots)\)）。为最大化平均飞行速度，应尽量减少备份轨迹执行比例，因为其通常为减速轨迹。故切换时间 \(t_{\mathrm{s}}\) 应尽可能晚，使MAV在高速探索轨迹上飞行更长时间。若重规划失败或超时，晚切换时间可给规划模块更多机会重新规划新轨迹，降低执行备份轨迹的概率。但切换时间 \(t_{\mathrm{s}}\) 不可过大，过大将使备份轨迹起点 \(\mathbf{p}_s\) 过于接近备份走廊边界，可能导致备份轨迹优化无解（见图8D）。

为确定最佳切换时间 \(t_{\mathrm{s}}\)，我们将其与备份轨迹 \(\mathcal{T}_{\mathrm{b}}(t)\) 一同优化。采用与探索轨迹优化（公式2）相同的形式，但将走廊约束替换为备份走廊 \(\mathcal{C}_{\mathrm{b}}\)，确保备份轨迹全处于已知自由空间。目标函数中的惩罚项 \(\mathcal{V}\) 替换为 \(\mathcal{V} = -t_{\mathrm{s}}\)，以最大化切换时间 \(t_{\mathrm{s}}\)。除目标和约束修改外，新增两个优化变量：切换时间 \(t_{\mathrm{s}}\) 和终点位置 \(\mathbf{p}_{1}\)。终点 \(\mathbf{p}_{1}\) 替代公式2中的目标位置 \(\mathbf{p}_{g}\)，需在优化中确定，因为MAV可在已知自由空间任意位置停留。且因备份轨迹位于备份走廊，起点 \(\mathbf{p}_{s}\) 也应位于备份走廊，因此加约束 \(t_{\mathrm{c}} < t_{\mathrm{s}} < t_{\mathrm{o}}\)，其中 \(t_{\mathrm{c}}\) 为当前时间，\(t_{\mathrm{o}}\) 为探索轨迹离开备份走廊时间（见图8D）。为消除不等式约束，采用变量 \(\eta \in \mathbb{R}\) 参数化切换时间：

\[ t_{\mathrm{s}} = (t_{\mathrm{o}} - t_{\mathrm{c}}) \frac{1}{1 + e^{-\eta}} + t_{\mathrm{c}} \tag{8} \]

\(\eta\) 无任何约束（见图S10）。优化变量完整为 \((\mathbf{Q}, \mathbf{T}, \mathbf{p}_{1}, t_{\mathrm{s}})\)。结合轨迹 \(\mathbf{p}_{i}(t)\) 对 \(\mathbf{Q}\)、\(\mathbf{T}\)、初始状态 \(\mathbf{s}_{0}\) 和含 \(\mathbf{p}_{1}\) 的终止状态 \(\mathbf{s}_{\mathrm{f}}\) 可微（已在（29）证明），且 \(\mathbf{s}_{0} = \mathcal{T}_{\mathrm{e}}(t_{\mathrm{s}})\) 对 \(t_{\mathrm{s}}\)（进而 \(\eta\)）可微，整个目标与约束可微，可用基于梯度的L-BFGS求解器（63）求解。

Fig. S10. 从 \(\eta\) 到 \(t_{s}\) 的映射绘图。图示一个类似 Sigmoid 的函数，将 \(\eta \in \mathbb{R}\) 映射到区间 \(\left(t_{c}, t_{o}\right)\)。

流形上的模型预测控制器

规划模块生成的位姿轨迹 \(\mathbf{p}(t)\) 传递至控制模块跟踪（图7D）。首先基于MAV受阻力影响的微分平坦性（64），将位置轨迹转换为状态轨迹 \(\mathbf{s}(t)\)。为高精度跟踪该状态轨迹，实现了先前工作的流形上模型预测控制（OMMPC）（65）。 该方法通过将四旋翼系统状态映射至轨迹 \(\mathbf{s}(t)\) 上各点的局部坐标线性化系统，实现参数最小化且避免运动学奇异，允许MAV执行大姿态角激进机动，如急转弯倾斜。线性化系统为线性时变系统，由传统MPC控制。为提升控制精度，进一步考虑了转子阻力补偿。OMMPC转子阻力补偿的详细推导及补偿效果定量分析见补充方法及图S13。

统计分析

采用三种统计分析方法：误差棒、提琴图和箱线图。误差棒表示均值周围的标准差（SD），反映数据变异性。标准差计算公式为：

\[ \sigma = \sqrt{\frac{1}{n-1} \sum_{i=1}^n (x_i - \mu)^2} \tag{9} \]

其中 \(n\) 为数据点数，\(x_i\) 为第 \(i\) 个数据点，\(\mu\) 为均值。

提琴图（图3、6及图S12使用）展示多个测试的性能指标分布（如飞行速度、轨迹跟踪误差及备份轨迹执行比例）。结合误差棒和核密度估计，显示数据分布、密度和范围。核密度估计 \(\widehat{f}(x)\) 计算公式为：

\[ \widehat{f}(x) = \frac{1}{n h} \sum_{i=1}^n K\left(\frac{x - x_i}{h}\right) \tag{10} \]

其中 \(n\) 为数据点数，\(x_i\) 为数据点，\(h\) 为带宽参数，\(K\) 为核函数（本分析使用高斯核）。提琴图中宽度表示不同取值处数据密度，中心点为均值，误差棒为标准差。

箱线图（图S8使用）总结数据分布，显示中位数和四分位距（IQR），须须延伸至1.5倍IQR，超出范围的数据视为异常值，不显示。

补充材料

补充方法

SUPER系统构成详解

本节详细介绍SUPER系统在重量、功耗和CPU使用率方面的构成（见图S1）。如图S1A所示，SUPER的硬件总重为\(1,512 \mathrm{~g}\)，包括机身框架（510 g）、电池（432 g）、机载计算机（266 g）、激光雷达传感器（255 g）及其他航空电子设备（52 g）。

Fig. S1. SUPER 系统构成 (A) SUPER 的重量、电力和 CPU 使用率分解。 (B) SUPER 的硬件组件。

在公园内典型飞行任务中测量了SUPER的功耗（见图S2A），开启完整机载感知、规划与控制，平均速度为\(2.84 \mathrm{~m}/\mathrm{s}\)。飞行任务中，SUPER被指令连续遍历航点路径（见图S2B）：

\(\mathbf{p}_{1} \rightarrow \mathbf{p}_{2} \rightarrow \mathbf{p}_{3} \rightarrow \mathbf{p}_{4} \rightarrow \mathbf{p}_{1} \rightarrow \mathbf{p}_{5} \rightarrow \mathbf{p}_{3} \rightarrow \mathbf{p}_{4} \rightarrow \mathbf{p}_{1} \rightarrow \mathbf{p}_{6} \rightarrow \mathbf{p}_{3} \rightarrow \mathbf{p}_{4}\)。

搭载3300 mAh电池，无人机可在电量耗尽前自主导航完成给定航点序列。总飞行时间约为11分24秒，飞行总距离为\(1,942.56 \mathrm{~m}\)（见图S2）。总平均功耗约432 W，其中驱动器消耗比例最高，达\(87.9\% (379.7 \mathrm{~W})\)。机载计算机功耗居第二，约43.3 W。激光雷达和其他航空电子设备功耗分别为6.3 W和3.2 W。

Fig. S2. 耐久测试的场景和轨迹 (A) 测试场景。 (B) 耐久测试期间无人机的点云视图和飞行轨迹。

飞行任务中机载计算机CPU使用率亦被测量。平均CPU使用率约\(13.9\%\)，峰值为\(19.7\%\)。状态估计模块为主要计算负载，平均占用\(11.1\%\)。状态估计、规划和控制三大软件组件作为独立进程并行运行，通过机器人操作系统（ROS）的发布-订阅机制通信（66）。为进一步提升计算效率，各模块内进程采用多线程并行化。状态估计模块中，扫描配准时的最近邻搜索并行化为16线程。规划模块中的时空滑动地图更新及轨迹规划任务（包含前端路径搜索和后端轨迹优化）由两个独立线程处理。控制模块中的模型预测控制器（MPC）运行于单线程。

基于激光雷达导航方法的基准比较

为对比SUPER规划模块与最先进激光雷达导航系统Falco (42)、Bubble planner (13)和IPC (67)的性能，本节给出仿真中的详细基准对比。仿真测试设置与正文“安全性、成功率及效率评估”中的主结果一致。测试了两个Falco版本。第一版本为无云台Falco，对应官方开源实现，不包含其真实实验中使用的云台结构。在该设置下，无人机加减速时激光雷达视场（FOV）及相应运动基元与机体坐标系对齐（见图S3A）。第二版本为带云台Falco，通过修改官方实现加入云台结构，使激光雷达水平朝向独立于无人机姿态，如图S3B所示。该云台配置也用于Falco的实飞实验，助力高速飞行时稳定运行。

Fig. S3. 带和不带云台结构的 Falco 示意图 (A) Falco 开源实现的原始版本缺少云台结构。在这种配置下，当多旋翼加速时，运动原语被限制指向向下，禁止无人机向前飞行。 (B) 带云台结构的 Falco 修改版本。LiDAR 传感器保持水平姿态，与无人机的姿态无关。

结果见图S4，其中Bubble结果直接取自主结果。带云台Falco整体成功率为\(57.04\%\)，最大平均速度超\(10 \mathrm{~m}/\mathrm{s}\)，与原文表现一致（42）。无云台Falco，即默认开源版本，整体成功率仅\(40.28\%\)，最大平均速度低于\(6 \mathrm{~m}/\mathrm{s}\)，即使在最稀疏环境下亦如此。IPC成功率高达\(75\%\)，优于两版本Falco及Bubble planner。尽管IPC因其乐观规划策略在低可通行环境下成功率较低，随可通行性改善成功率提升。但最大速度方面，IPC落后于Bubble和SUPER。结果表明，SUPER在成功率上优于Falco、Bubble和IPC，同时飞行速度显著更高。计算时间对比见图S4C。Falco依赖离线轨迹库及无地图规划框架，计算时间低于Bubble和SUPER。IPC每次重规划周期仅生成一至两个多面体，前端计算负载低，且其基于MPC的规划控制后端耗时不足1 ms，整体计算开销显著较低。

Fig. S4. 成功率和效率评估 (A) 在不同障碍物密度（以可通行性衡量）和飞行速度下的基准方法成功率。空白列表示未达到相应速度和密度的组合。 (B) 基准方法在1080次实验中的飞行结果。 (C) 基准方法的时间消耗，方块表示平均值，误差条表示总计算时间的标准差。每个均值和标准差均基于180次测试计算。

不同基线方法实现细节

本文“安全性、成功率及效率评估”部分简要对不同基线方法的实现细节进行了比较。表S1展示了方法在四个方面的比较。首先，基于安全策略将方法分为三类：乐观型、安全感知型及安全保障型。Bubble (13)为乐观型，视未知空间为自由空间。Raptor (20)为安全感知型，主动规划轨迹以增强未知空间可见性，但无安全保证。Faster (23)和本文提出的SUPER均为安全保障型，具备理论安全保证。其次，建图方法存在差异。Raptor采用基于欧氏有符号距离场（ESDF）的地图（31），Faster采用占据栅格地图（OGM）。相比之下，Bubble和SUPER采用点云建图，Bubble额外使用K维树生成走廊。整体上，Bubble和SUPER的点云建图计算时间远低于Faster和Raptor。四种基线均采用前后端式轨迹生成，具体方法不同。Bubble、Faster和SUPER采用基于飞行走廊的前端，Raptor采用拓扑路径搜索。后端方面，Faster采用混合整数二次规划（MIQP），Raptor使用路径引导轨迹优化。但Faster的MIQP及Raptor的路径引导优化仅优化轨迹空间属性，高速飞行时轨迹可行性受限，需引入时间优化。相比之下，Bubble和SUPER基于MINCO (29)实现时空轨迹优化，在高速飞行中表现更优，正文中有验证。

表 S1. 不同方法的实现细节

	类型	映射	前端	后端
Bubble (13)	乐观型	点云 + Kd-Tree	球形走廊	时空轨迹优化（STTP）
Raptor (20)	安全感知型	ESDF	拓扑路径搜索	路径引导轨迹优化（PGO）
Faster (23)	安全保障型	占据栅格地图（OGM）	多面体形走廊	混合整数二次规划（MIQP）
SUPER（本工作）	安全保障型	点云	多面体形走廊	时空轨迹优化（STTP）

Faster与SUPER规划模块的评估

我们在仿真中评估了Faster (23)和SUPER的规划模块，测试环境为一个具有挑战性的室内拐角，障碍物隐藏于拐角背后，如图S6A-i所示。此外，我们对SUPER进行了消融实验，具体为关闭切换时间优化（OT）的SUPER变体。每次测试中，无人机起始位置为\(\mathbf{p}_{\mathbf{s}}=[0,0,1.2]^{\mathrm{T}}\)，目标位置为\(\mathbf{p}_{\mathbf{g}}=[12.5,7.5,1.2]^{\mathrm{T}}\)。三种方法均安全到达目标。Faster与关闭OT的SUPER在无人机探测到拐角后的障碍物时均切换至备份轨迹两次。但由于Faster采用计算量大的基于MIQP的轨迹优化，未能及时规划新轨迹并执行备份轨迹直至停下，导致备份轨迹执行时间远超关闭OT的SUPER。Faster总耗时为6.2秒，超出关闭OT的SUPER的4.9秒。引入备份轨迹切换时间优化的完整SUPER版本避免了切换备份轨迹和不必要的减速，达到最短到达时间4.8秒，体现了所提规划模块的高速能力。

我们还对Faster与SUPER的备份轨迹生成进行了定量控制分析。图S6C所示，两规划器在相同探索轨迹和备份走廊下，生成备份轨迹及切换时间\(t_s\)分别以绿色曲线和橙色圆点展示。Faster采用启发式方法确定切换时间，将\(t_s\)设为无人机当前状态后0.45秒。SUPER则同步优化切换时间与备份轨迹，在确保备份轨迹位于备份走廊内的前提下，最大化\(t_s\)，最终获得\(t_s=0.84\mathrm{~s}\)。相比Faster，SUPER延迟切换时间86\%，为规划模块争取更多重新规划探索和备份轨迹的时间，减少了备份轨迹执行比例（见图S6B），提升了整体飞行速度和平滑性。

为验证规划模块集成完整系统后的性能，我们在真实环境中复现上述仿真实验（见图S6D-F）。无人机起始于\(\mathbf{p}_{\mathbf{s}}=[0,0,1.2]^{\mathrm{T}}\)，目标为\(\mathbf{p}_{\mathbf{g}}=[12.5,-7.5,1.2]^{\mathrm{T}}\)，最高速度限制\(\mathbf{v}_{\max}=8\mathrm{~m/s}\)，仿真设置一致。图S6E显示，\(t_1=2.1\mathrm{~s}\)时障碍物被墙体遮挡，生成的探索轨迹\(\mathcal{T}_{e}^1\)与障碍物相撞，但备份轨迹\(\mathcal{T}_{b}^1\)限制于备份走廊，保证最坏情况下无人机可安全停车避免碰撞。\(t_2=2.2\mathrm{~s}\)时无人机首次感知障碍物（图S6D, ii），重新规划探索轨迹\(\mathcal{T}_{e}^2\)避开障碍，未执行初始备份轨迹\(\mathcal{T}_{b}^1\)，而生成新的备份轨迹。速度曲线见图S6F，显示无人机未执行任何备份轨迹，高速安全抵达目标，彰显SUPER的高速与安全特性。

Fig. S6. Faster 和 SUPER 规划模块评估 (A) (i) 模拟环境的俯视图，红色圆柱隐藏在角落后方。 (ii-iv) 三种方法执行轨迹比较：Faster（23）、提出的 SUPER 以及未进行切换时间优化的 SUPER（称为 SUPER 无 OT）。 (B) 模拟中备份轨迹的速度曲线和执行情况。 (C) 在相同探索轨迹和备份走廊条件下，Faster 和 SUPER 的切换时间 \(t_{\mathrm{s}}\)。 (D) (i) SUPER 的真实实验，隐藏障碍用红色虚线框标示。 (ii) 时间点 \(t_{2}\) 机载摄像头的第一人称视角。 (E) 时间点 \(t_{1}\) 和 \(t_{2}\) 的规划轨迹。\(\mathcal{T}_{e}^{1}\) 和 \(\mathcal{T}_{b}^{1}\) 分别为 \(t_{1}\) 的探索轨迹和备份轨迹，\(\mathcal{T}_{e}^{2}\) 是 \(t_{2}\) 的探索轨迹。\(t_{2}\) 时的备份轨迹未显示。 (F) 实验中的速度曲线。

场景空间中的凸分解

凸分解是一种围绕给定种子（如点或线段）识别凸形状的技术，旨在有效地将空间中的凸区域与障碍物分离。在机器人导航任务中，在场景空间（C空间）执行凸分解并在分解空间规划轨迹（12,13,23,40）是实现高效避障的常用方法。目前存在两种主流的C空间自由多面体生成方法。

第一种称为点膨胀法（12,23,60），通过膨胀点云并对膨胀后的点云执行凸分解（见图S7A, i）。该方法导致点云数量指数增长，降低计算效率，且膨胀后的障碍点云常不能准确表示场景空间，导致生成多面体小且尖锐（见图S7A, i），限制轨迹优化问题的可行解空间。

第二种为平面收缩法（40,61），直接由原始点云生成凸形状，随后以机器人半径收缩得到C空间自由多面体（见图S7A, ii）。但该方法先最大化多面体后收缩，可能导致结果多面体变小且尖锐，且收缩后多面体可能不包含种子，限制了需保证种子位于多面体内场景的适用性。

Fig. S7. 提出的飞行走廊生成算法（CIRI）示意图 (A) 场景空间中三种不同安全飞行走廊生成方法的比较。 (B) 说明椭球坐标系 \(\mathcal{E}\) 和世界坐标系 \(\mathcal{W}\) 之间的变换示例。 (C) C 空间中凸分解过程的视觉展示。 (D) 平面调整策略示例，确保给定的种子点位于生成的多面体内。

针对上述问题，我们提出了场景空间迭代区域膨胀（CIRI）算法。不同于以往将障碍表示为点的方法（40,58,59,68,69），我们将障碍点建模为半径等于机器人半径的球体。此方法直接在C空间生成自由多面体，具备多项优势。首先，算法直接作用于原始点云，无需膨胀，减少参与点数，显著提升计算效率。其次，准确的场景空间障碍建模使得我们方法能生成比基于收缩或膨胀方法更大的多面体。此外，我们提出平面调整方法，确保生成多面体包含种子，有利于生成备份走廊。

所提CIRI基于FIRI框架（58）的迭代流程（见算法1）改进，提升计算效率并生成更大多面体。算法以线段种子\(\mathrm{S}:\{\mathbf{s}_a, \mathbf{s}_b\}\)及障碍点云\(\mathcal{O}\)为输入，初始将椭球设为单位球（第2行），迭代直至椭球体积收敛。迭代过程包含两步：第一，均匀膨胀椭球至与障碍点云相切，生成一组超平面，将凸区域与障碍分离且保证包含种子（第7行）；第二，基于二阶锥规划（SOCP）优化，求解由上述超平面定义的无障碍半空间多面体内最大体积椭球（第8行）。通过迭代精炼，超平面与椭球逐步调整，扩展内接椭球体积，得到更大多面体与更宽阔的无障碍空间区域。

算法 1： 场景空间迭代区域膨胀（CIRI）

符号说明：单位矩阵：\(\mathbf{I} \in \mathbb{R}^{3 \times 3}\)；以 \(\mathbf{d}\) 为中心，形状矩阵为 \(\mathbf{C}\) 的椭球 \(\mathbf{E}\)；收敛阈值：\(\varepsilon\)

输入：障碍物点云：\(\mathcal{O}\)；机器人半径：\(r\)；种子点：S

输出：多面体：\(\mathcal{P}\)

基于椭球的凸分解

本步骤旨在确定由一组分割超平面组成的自由多面体，这些超平面确保给定种子区域无障碍物。每个超平面用\(\mathcal{H}(A,b)\)表示，其中\(A \mathbf{x} \leq b, \mathbf{x} \in \mathbb{R}^3\)定义一个半空间。多面体\(\mathcal{P}\)由\(K\)个半空间的交集定义：\(\mathcal{P}:\{\mathcal{H}_1, \ldots, \mathcal{H}_k\}\)。椭球使用单位球的像，即Löwner-John椭球（70）表示，定义如下：

\[ \mathbf{E}(\mathbf{C}, \mathbf{d}) = \{\mathbf{C} \tilde{\mathbf{x}} + \mathbf{d} \mid \|\tilde{\mathbf{x}}\| \leq 1\} \tag{S1} \]

其中\(\mathbf{C} \in \mathbb{R}^{3 \times 3}\)为对角形状矩阵，定义为\(\mathbf{C}=\operatorname{diag}(r_x, r_y, r_z)\)，\(\mathbf{d} \in \mathbb{R}^3\)为椭球中心。其均匀扩展定义为：

\[ \mathbf{E}_\alpha(\mathbf{C}, \mathbf{d}) = \{\mathbf{C} \tilde{\mathbf{x}} + \mathbf{d} \mid \|\tilde{\mathbf{x}}\| \leq \alpha \}, \quad \alpha \geq 1 \tag{S2} \]

其中\(\alpha\)为扩展因子。球体的定义类似，\(\mathbf{C} = \operatorname{diag}(r, r, r)\)，\(\mathbf{d}\)为球心。

所提方法输入包含障碍点云\(\mathcal{O} \in \mathbb{R}^{3 \times N}\)（\(N\)点数）、世界坐标系下椭球\(\mathbf{E}^\mathcal{W}\)、种子\(\mathbf{S}\)及机器人半径\(r\)。流程见算法2，示意于图S7C。

算法 2： 基于椭球的凸分解

参数：活动障碍物点云 \(\mathcal{O}_{a}\)；

输入：障碍物点云：\(\mathcal{O}\)；机器人半径：\(r\)；种子点：\(\mathbf{S}\)；椭球：\(\mathbf{E}^{\mathcal{W}}\)

输出：多面体：\(\mathcal{P}\)

初始时，将激活障碍点云\(\mathcal{O}_a\)设为全障碍点云\(\mathcal{O}\)（第2行）。随后循环生成分割超平面，直到\(\mathcal{O}_a\)为空。每轮迭代选择离椭球中心\(\mathbf{E}\)最近的障碍球体\(\mathbf{E}_s^\mathcal{W}\)（第4行），尝试寻找椭球与该障碍球体间的分割超平面。寻找超平面的关键是定位与障碍物相交且与均匀扩展椭球\(\mathbf{E}_\alpha^\mathcal{W}\)相切的平面。基于文献（59）思路，将问题转到椭球坐标系\(\mathcal{E}\)，转化为单一最短距离规划问题，避免对不同扩展因子\(\alpha\)的搜索。根据式S1，椭球\(\mathbf{E}^\mathcal{W}\)为椭球系\(\mathcal{E}\)中单位球的像。通过构建逆映射，将问题转换至椭球系\(\mathcal{E}\)（见图S7B）。交点\(\mathbf{p}_t^\mathcal{W}\)对应椭球系中最靠近原点的点\(\mathbf{p}_t^\mathcal{E}\)，问题转为最小距离优化：

\[ \mathbf{p}_t^\mathcal{E} = \arg \min_{\mathbf{x}} \|\mathbf{x}\|_2, \quad \text{s.t.} \quad \mathbf{x} \in \mathbf{E}_s^\mathcal{E} \tag{S3} \]

该问题可高效通过求解六次多项式根（71）获得。我们用C++实现，Intel i7 CPU下典型求解时间不足0.5微秒。得切点\(\mathbf{p}_t^\mathcal{E}\)后，计算半空间\(\mathcal{H}^\mathcal{E}(A^\mathcal{E}, b^\mathcal{E}) = (\mathbf{p}_t^\mathcal{E} / \|\mathbf{p}_t^\mathcal{E}\|, -\|\mathbf{p}_t^\mathcal{E}\|)\)，及扩展因子\(\alpha = \|\mathbf{p}_t^\mathcal{E}\|\)。再将其反变换回世界系得到：

\[ \mathcal{H}^\mathcal{W}(\mathbf{C} A^\mathcal{E}, b^\mathcal{E} + [\mathbf{C} A^\mathcal{E}] \cdot \mathbf{d}) \tag{S4} \]

其中\(\mathbf{C}, \mathbf{d}\)分别为椭球\(\mathbf{E}\)的形状矩阵和中心。

上述过程得到椭球与障碍球体的分割超平面。但可能出现种子不在生成半空间内的情况。为此，我们提出平面调整方法，确保生成多面体包含种子（第6行）。如图S7D所示，先检查\(\mathbf{s}_a\)和\(\mathbf{s}_b\)是否均在半空间\(\mathcal{H}\)内。若种子点\(\mathbf{s}_a\)在\(\mathcal{H}\)外，则将\(\mathcal{H}\)调整为过\(\mathbf{s}_a\)且切于障碍球体、指向另一种子点\(\mathbf{s}_b\)的新超平面\(\mathcal{H}_{adj}\)。该调整保证\(\mathcal{H}_{adj}\)包含\(\mathbf{s}_a\)和\(\mathbf{s}_b\)。随后用\(\mathcal{H}_{adj}\)替代本轮生成的\(\mathcal{H}\)。调整后（如必要），将\(\mathcal{H}\)外的障碍点从\(\mathcal{O}_a\)中剔除，并将\(\mathcal{H}\)加入多面体\(\mathcal{P}\)。该过程循环直至所有障碍点被排除。

Fig. S7. 提出的飞行走廊生成算法（CIRI）示意图 (A) 场景空间中三种不同安全飞行走廊生成方法的比较。 (B) 说明椭球坐标系 \(\mathcal{E}\) 和世界坐标系 \(\mathcal{W}\) 之间的变换示例。 (C) C 空间中凸分解过程的视觉展示。 (D) 平面调整策略示例，确保给定的种子点位于生成的多面体内。

最大内切椭球

给定多面体（一组超平面），此步骤通过确定该多面体内的最大体积内切椭球（MVIE）来更新椭球。假设多面体由\(K\)个超平面组成\(\mathcal{P}=\{\mathcal{H}_1, \ldots, \mathcal{H}_K\}\)，椭球定义如式S1，优化问题可表述为：

\[ \begin{align*} \max _{\mathbf{C}, \mathbf{d}} & \operatorname{det}(\mathbf{C}) \\ \text { s.t. } & \sup \left(A_{i} \mathbf{C} \tilde{\mathbf{x}}\right)+A_{i} \mathbf{d} \leq b_{i}, \forall i=1, \ldots, K, \tag{S5}\\ & \mathbf{C} \succeq 0, \end{align*} \]

其中\(\mathbf{C}\)和\(\mathbf{d}\)分别为椭球的形状矩阵和中心。\(A_i\)和\(b_i\)为超平面\(\mathcal{H}(A,b): A \mathbf{x} \leq b\)的参数。式S5是计算几何中经典的凸优化问题。本文采用文献（72）提出的SOCP转化方法，并通过锥增强拉格朗日方法（73）高效求解。

凸分解方法比较

我们在三种不同障碍形态环境——柱子（Pillars）、森林（Forest）和Perlin（74）（见图S8）——中进行凸分解方法对比分析。每种障碍类型随机生成300个不同障碍密度的场景。障碍密度以障碍填充率（OFR）衡量，即障碍体积总和与空间总体积比值。共900个场景涵盖OFR从0.0005到0.6。每个场景采样分辨率为0.1 m的点云地图，随机生成线段种子，保证种子距最近障碍至少0.2 m（等于机器人半径），确保场景空间内存在可行多面体。点云与种子输入不同凸分解方法。

比较三种场景空间凸分解方法（见图S7A）：第一为平面收缩法，先在原始点云上生成多面体，再按机器人半径收缩各超平面；第二为点膨胀法，通过在每输入点中心构建边长等于机器人半径的立方体插入点云，实现点云膨胀后执行凸分解，保证生成多面体在场景空间有效；第三为CIRI（场景空间迭代区域膨胀），将输入点视为半径等于机器人半径的球体，在此基础上执行凸分解。前两种方法分别基于两种先进凸分解技术FIRI (58)和RILS (40)实现，命名为FIRI-Shrinkage、FIRI-Inflation、RILS-Shrinkage和RILS-Inflation。

评估指标包括种子包含率（多面体包含线段种子比例）、计算时间及多面体体积（见图S8B-D）。种子包含率方面（图S8B），CIRI及点膨胀方法（FIRI-Inflation或RILS-Inflation）均达100\%。而平面收缩法（FIRI-Shrinkage或RILS-Shrinkage）种子包含率较低，仅保证收缩前种子在多面体内。多面体体积方面（图S8C），CIRI因准确建模场景空间，生成的多面体平均体积最大。计算时间方面（图S8D），CIRI及两种收缩基线（FIRI-Shrinkage和RILS-Shrinkage）计算时间低于点膨胀方法。

Fig. S8. 场景空间中凸分解的比较 (A) 障碍物填充率（OFR）从0.005到0.6的基准环境示例。 (B) 不同方法生成多面体的种子包含率随OFR变化。 (C) 不同方法生成的多面体体积的箱线图，基于100次测试。中线表示中位数。 (D) 各方法计算时间的箱线图，基于100次测试。中线表示中位数。

点膨胀法由于点云数量增多，显著增加计算时间。与收缩法相比，CIRI计算时间略高，因需求解球与椭球间切平面，而其他方法仅需点-椭球切平面，计算更简单。

由于收缩法（包括FIRI-Shrinkage和RILS-Shrinkage）不保证种子包含，不适合SUPER。种子包含保障方法中，CIRI生成更大体积多面体且计算时间低于点膨胀法。综上，CIRI在计算时间较少情况下有效提升走廊质量，进一步优化SUPER整体性能。

备份安全飞行走廊生成定理

定理1. 设\(\mathcal{D}\)为在位置\(\mathbf{p}_c \in \mathbb{R}^3\)采集的全方向且无限密集深度图像。若集合\(\mathcal{S} \subseteq \mathbb{R}^3\)满足：

(a) \(\mathcal{S}\)为凸集；

(b) \(\mathcal{S}\)不含\(\mathcal{D}\)中的点；

(c) \(\mathbf{p}_c \in \mathcal{S}\)，

则\(\mathcal{S}\)必位于已知自由空间内。

证明: 假设\(\mathcal{S}\)不完全包含于已知自由空间，则存在点\(\mathbf{p}\)处于占据或未知状态。若\(\mathbf{p}\)占据空间，则必存在于\(\mathcal{D}\)，与(b)矛盾。若\(\mathbf{p}\)处于未知空间，则其深度大于相同方位方向上任一点\(\mathbf{d} \in \mathcal{D}\)。\(\mathbf{d}\)位于连接\(\mathbf{p}\)与\(\mathbf{p}_c\)的直线上，凸性条件保证\(\mathbf{d} \in \mathcal{S}\)，再次与(b)矛盾。故\(\mathcal{S}\)必完全包含于已知自由空间。

有限激光雷达视场下的轨迹规划

考虑激光雷达视场（FOV）有限性，备份走廊需与FOV相交以确保交集空间完全处于已知自由区域。常见激光雷达具360度水平FOV与有限垂直FOV（见图S11A）。设垂直FOV角度为\(\theta < \pi\)（图S11B），则总FOV为非凸形状。为保证交集备份走廊凸性，需选取FOV的凸子集与由CIRI生成的多面体相交。360度水平FOV最大凸子集为由切于FOV上下界的两个超平面定义的半空间交集（见图S11A、B）。FOV存在无数此类凸子集，由一自由度航向\(\mathbf{h}\)唯一确定（见图S11C）。为使备份走廊包含最大探索轨迹，令\(\mathbf{h}\)与探索轨迹切向对齐（图S11D）。

Fig. S11. 有限视场备份走廊生成 (A) 非凸视场的最大凸子集由两个由切割视场上边界 \(\mathcal{H}_{u}\) 和下边界 \(\mathcal{H}_{l}\) 切平面的半空间交集形成。 (B) 切平面 \(\mathcal{H}_{u}\) 和 \(\mathcal{H}_{l}\) 的侧视图及垂直视场角 \(\theta\)。 (C) 俯视图显示凸子集和两个切平面，由航向方向 \(\mathbf{h}\) 唯一确定。 (D) 航向方向与探索轨迹对齐，确保生成的备份走廊包含探索轨迹的初始部分。

为研究垂直FOV角度\(\theta\)对规划性能的影响，我们在仿真环境中进行系列实验，垂直FOV角度从30度到180度（全方位FOV）变化。测试环境与正文基准对比类似。选取两种不同可通行性环境：一为可通行性3.1的稠密环境，另一为6.5的较稀疏环境。每种环境测试10张地图，每张地图10次试验，无速度限制。每次记录平均速度、备份轨迹执行比例及最终结果（成功或失败）。结果见图S12。

成功率方面，无论低高可通行性环境，SUPER均达100\%成功率（图S12A,i及图S12B,i），体现安全保障属性。在稠密环境中，平均速度随垂直FOV增大而提升（图S12A,ii），因更大FOV生成更大备份走廊，减少备份轨迹执行比例（图S12A,iii），从而提高平均速度。稀疏环境中平均速度趋势相似，但FOV大小影响较小（图S12B,ii），因障碍较少，较小FOV亦能规划安全高速轨迹，且备份轨迹执行比例受FOV影响较小（图S12B,iii）。

Fig. S12. 不同垂直视场下的基准比较 (A) 在可通行性得分为3.1的模拟地图上的基准结果。 (B) 在可通行性得分为6.5的模拟地图上的基准结果。对于 (A) 和 (B)，分布图 (ii-iii) 展示平均飞行速度和执行备份轨迹的比例。误差条表示标准差，黑点表示均值。每个分布基于180次测试数据。

具有转子阻力补偿的流形模型预测控制（On-manifold MPC）

在我们之前的工作（65）中，提出了一种适用于机器人系统的流形模型预测控制（OMMPC）框架，并演示了室内四旋翼特技飞行。然而，之前工作忽略了空气动力学效应，这在无人机执行高速户外飞行时会影响轨迹跟踪精度。为克服此限制，我们扩展了之前的四旋翼模型，引入转子阻力——导致跟踪误差的主要气动效应（64）。由此导出新的OMMPC算法，并在本文所有飞行实验中实现，验证了其高精度跟踪效果。通过对比实验进一步证明了该改进的有效性。

四旋翼动力学

四旋翼坐标系定义为\(X\)轴前进方向，\(Y\)轴右侧，\(Z\)轴向下。包含转子阻力的动力学模型（64）表达为：

\[ \begin{align*} \dot{\mathbf{p}} & =\mathbf{v} \\ \dot{\mathbf{v}} & =\mathbf{g}-a_{T} \mathbf{R e}_{3}-\frac{1}{m} \mathbf{R D R}^{\mathrm{T}} \mathbf{v} \tag{S6}\\ \dot{\mathbf{R}} & =\mathbf{R}\left\lfloor\omega^{\mathcal{B}}\right\rfloor \end{align*} \]

其中\(m\)为无人机质量，\(\mathbf{g}\)为重力加速度向量，大小固定为\(9.81\, \mathrm{m/s}^2\)，\(a_T\)为标量推力加速度。位置与速度分别为惯性系中的\(\mathbf{p} \in \mathbb{R}^3\)和\(\mathbf{v} \in \mathbb{R}^3\)。机体姿态表示为\(\mathbf{R} \in SO(3)\)，\(\boldsymbol{\omega}\)为机体系角速度。操作符\(\lfloor \cdot \rfloor\)将向量转为反对称矩阵。\(\mathbf{D}\)为转子阻力系数矩阵，参数化为\(\mathbf{D} = \operatorname{diag}(d_h, d_h, d_v)\)。

如文献（64）所述，含转子阻力的四旋翼动力系统满足微分平坦性。平坦输出通常选为\(\sigma = [\mathbf{p}, \psi]^\top\)，其中\(\psi\)为偏航角。该特性允许规划平坦输出轨迹\(\sigma(t)\)，替代规划四旋翼完整状态轨迹\(\mathbf{s}(t)\)。

系统线性化

如前述（65），OMMPC将四旋翼系统嵌入复合流形，定义状态与输入为：

\[ \begin{align*} & \mathcal{M}=\mathbb{R}^{3} \times \mathbb{R}^{3} \times S O(3), \quad \operatorname{dim}(\mathcal{M})=9 \\ & \mathbf{x}=\left(\begin{array}{lll} \mathbf{p}^{\mathcal{I}} & \mathbf{v}^{\mathcal{I}} & \mathbf{R} \end{array}\right) \in \mathcal{M}, \quad \mathbf{u}=\left[\begin{array}{ll} a_{T} & \boldsymbol{\omega}^{\mathcal{B}} \end{array}\right] \in \mathbb{R}^{4} \tag{S7} \end{align*} \]

测量状态\(\mathbf{x}\)与参考状态\(\mathbf{x}_d\)均位于流形\(\mathcal{M}\)上，误差映射至参考点周围的局部同胚空间（欧氏空间的开集）。状态误差定义为：

\[ \begin{align*} & \delta \mathbf{x} \triangleq \mathbf{x}_{d} \boxminus \mathbf{x}=\left[\begin{array}{ll} \delta \mathbf{p}^{\mathrm{T}} & \delta \mathbf{v}^{\mathrm{T}} \quad \delta \mathbf{R}^{\mathrm{T}} \end{array}\right]^{\mathrm{T}} \in \mathbb{R}^{9} \\ & \delta \mathbf{p} \triangleq \mathbf{p}_{d} \boxminus \mathbf{p}=\mathbf{p}_{d}-\mathbf{p} \in \mathbb{R}^{3} \tag{S8}\\ & \delta \mathbf{v} \triangleq \mathbf{v}_{d} \boxminus \mathbf{v}=\mathbf{v}_{d}-\mathbf{v} \in \mathbb{R}^{3} \\ & \delta \mathbf{R} \triangleq \mathbf{R}_{d} \boxminus \mathbf{R}=\log \left(\mathbf{R}^{\mathrm{T}} \mathbf{R}_{d}\right) \in \mathbb{R}^{3}, \end{align*} \]

其中\(\boxminus\)为映射\(\boxminus: \mathcal{M} \times \mathcal{M} \to \mathbb{R}^n\)的封装算子（详见65），输入误差定义为：

\[ \begin{align*} \delta \mathbf{u} & \triangleq \mathbf{u}_{d}-\mathbf{u}=\left[\begin{array}{ll} \delta a_{T} & \delta \boldsymbol{\omega}^{\mathrm{T}} \end{array}\right]^{\mathrm{T}} \in \mathbb{R}^{4} \\ \delta a_{T} & \triangleq a_{T_{d}}-a_{T} \in \mathbb{R} \tag{S9}\\ \delta \boldsymbol{\omega} & \triangleq \boldsymbol{\omega}_{d}-\boldsymbol{\omega} \in \mathbb{R}^{3} \end{align*} \]

原始四旋翼系统等价于误差系统，状态映射至参考轨迹每点的局部坐标。依据（65）推导，线性化误差系统为：

\[ \begin{equation*} \delta \mathbf{x}_{k+1}=\mathbf{F}_{\mathbf{x}_{k}} \delta \mathbf{x}_{k}+\mathbf{F}_{\mathbf{u}_{k}} \delta \mathbf{u}_{k}, \tag{S10} \end{equation*} \]

其中

\[ \mathbf{F}_{\mathbf{x}_k}=\left[\begin{array}{ccc}\mathbf{I}_3 & \mathbf{I}_3 \Delta t & \mathbf{0} \\ \mathbf{0} & \mathbf{I}_3-\frac{\Delta t}{m} \mathbf{R}_k^d \mathbf{D} \mathbf{R}_k^{d^{\mathrm{T}}} & \frac{\Delta t}{m} \mathbf{R}_k^d\left(2 \mathbf{I}_3-\mathbf{D}\right)\left\lfloor a_T m \mathbf{e}_3+\mathbf{D} \mathbf{R}_k^{d^{\mathrm{T}}} \mathbf{v}-\mathbf{R}_k^{d^{\mathrm{T}}} \mathbf{v}\right\rfloor \\ \mathbf{0} & \mathbf{0} & \operatorname{Exp}\left(-\boldsymbol{\omega}_k^d \Delta t\right)\end{array}\right], \tag{S11}\\ \mathbf{F}_{\mathbf{u}_k}=\Delta t\left[\begin{array}{cc}\mathbf{0} & \mathbf{0} \\ -\mathbf{R}_k^d \mathbf{e}_3 & \mathbf{0} \\ \mathbf{0} & \mathbf{A}\left(\boldsymbol{\omega}_k^d \Delta t\right)^{\mathrm{T}}\end{array}\right] \]

其中

\[ \begin{equation*} \mathbf{A}(\boldsymbol{\theta})=\mathbf{I}_{3}+\left(\frac{1-\cos \|\boldsymbol{\theta}\|}{\|\boldsymbol{\theta}\|}\right) \frac{\lfloor\boldsymbol{\theta}\rfloor}{\|\boldsymbol{\theta}\|}+\left(1-\frac{\sin \|\boldsymbol{\theta}\|}{\|\boldsymbol{\theta}\|}\right) \frac{\lfloor\boldsymbol{\theta}\rfloor^{2}}{\|\boldsymbol{\theta}\|^{2}} . \tag{S12} \end{equation*} \]

因此，OMMPC根据当前误差和参考轨迹给出高效的二次规划（QP）解：

\[ \begin{array}{ll} \min _{\delta \mathbf{u}_{k}} & \sum_{k=0}^{N-1}\left(\left\|\delta \mathbf{x}_{k}\right\|_{\mathbf{Q}}^{2}+\left\|\delta \mathbf{u}_{k}\right\|_{\mathbf{R}}^{2}\right)+\left\|\delta \mathbf{x}_{N}\right\|_{\mathbf{P}}^{2}, \tag{S13}\\ \text { s.t. } & \delta \mathbf{x}_{k+1}=\mathbf{F}_{\mathbf{x}_{k}} \delta \mathbf{x}_{k}+\mathbf{F}_{\mathbf{u}_{k}} \delta \mathbf{u}_{k}, \\ & \delta \mathbf{u}_{k} \in \delta \mathbb{U}_{k}, \quad k=0, \cdots, N-1, \end{array} \]

其中\(N\)为预测时域，\(\mathbf{Q}, \mathbf{R}, \mathbf{P}\)为阶段状态、阶段输入和终端状态的正定对角惩罚矩阵。输入误差约束集为\(\mathbb{U}_k = \{\delta \mathbf{u} \in \mathbb{R}^4 \mid \mathbf{u}_{min} - \mathbf{u}_k^d \leq \delta \mathbf{u} \leq \mathbf{u}_{max} - \mathbf{u}_k^d \}\)。

实验结果

为定量评估所提含转子阻力模型的OMMPC效果，我们使用带有Gazebo仿真器（75）的PX4 SITL软件，对比分析了传统四旋翼模型（65）与改进模型的性能。该仿真器能提供真实的动力学与空气阻力模拟。我们在平坦空间生成了最大速度为\(15 \mathrm{~m/s}\)的离线多项式轨迹\(\sigma(t)\)。轨迹为沿\(x\)轴从起点\(\mathbf{p}_0 = [0,0,0]^\top\)至终点\(\mathbf{p}_g = [120,0,0]^\top\)的直线。初始时，基于常规模型的微分平坦性生成期望状态轨迹，并用文献（65）中提出的四旋翼OMMPC进行跟踪。图S13A显示，无人机存在显著姿态跟踪误差，且位置误差明显受速度方向影响，最大达0.4 m。相比之下，采用式(S6)中含转子阻力的四旋翼模型，OMMPC有效预测并补偿阻力，显著改善了姿态跟踪性能，位置跟踪误差减小至0.05 m。

补充图表

Fig. S1. SUPER 系统构成 (A) SUPER 的重量、电力和 CPU 使用率分解。 (B) SUPER 的硬件组件。

Fig. S2. 耐久测试的场景和轨迹 (A) 测试场景。 (B) 耐久测试期间无人机的点云视图和飞行轨迹。

Fig. S3. 带和不带云台结构的 Falco 示意图 (A) Falco 开源实现的原始版本缺少云台结构。在这种配置下，当多旋翼加速时，运动原语被限制指向向下，禁止无人机向前飞行。 (B) 带云台结构的 Falco 修改版本。LiDAR 传感器保持水平姿态，与无人机的姿态无关。

Fig. S4. 成功率和效率评估 (A) 在不同障碍物密度（以可通行性衡量）和飞行速度下的基准方法成功率。空白列表示未达到相应速度和密度的组合。 (B) 基准方法在1080次实验中的飞行结果。 (C) 基准方法的时间消耗，方块表示平均值，误差条表示总计算时间的标准差。每个均值和标准差均基于180次测试计算。

Fig. S5. OGM 上细线检测的视觉示例 (A) 无人机面对细线（绿色显示）的第三人称视角。 (B) 检测过程的侧视图。灰色框代表占据栅格地图（OGM）中包含细线的栅格。蓝线表示穿过该栅格但未击中细线的 LiDAR 光束，红线表示击中细线的光束。由于大多数光束未检测到细线，该栅格被错误地分类为自由空间。

Fig. S6. Faster 和 SUPER 规划模块评估 (A) (i) 模拟环境的俯视图，红色圆柱隐藏在角落后方。 (ii-iv) 三种方法执行轨迹比较：Faster（23）、提出的 SUPER 以及未进行切换时间优化的 SUPER（称为 SUPER 无 OT）。 (B) 模拟中备份轨迹的速度曲线和执行情况。 (C) 在相同探索轨迹和备份走廊条件下，Faster 和 SUPER 的切换时间 \(t_{\mathrm{s}}\)。 (D) (i) SUPER 的真实实验，隐藏障碍用红色虚线框标示。 (ii) 时间点 \(t_{2}\) 机载摄像头的第一人称视角。 (E) 时间点 \(t_{1}\) 和 \(t_{2}\) 的规划轨迹。\(\mathcal{T}_{e}^{1}\) 和 \(\mathcal{T}_{b}^{1}\) 分别为 \(t_{1}\) 的探索轨迹和备份轨迹，\(\mathcal{T}_{e}^{2}\) 是 \(t_{2}\) 的探索轨迹。\(t_{2}\) 时的备份轨迹未显示。 (F) 实验中的速度曲线。

Fig. S7. 提出的飞行走廊生成算法（CIRI）示意图 (A) 场景空间中三种不同安全飞行走廊生成方法的比较。 (B) 说明椭球坐标系 \(\mathcal{E}\) 和世界坐标系 \(\mathcal{W}\) 之间的变换示例。 (C) C 空间中凸分解过程的视觉展示。 (D) 平面调整策略示例，确保给定的种子点位于生成的多面体内。

Fig. S8. 场景空间中凸分解的比较 (A) 障碍物填充率（OFR）从0.005到0.6的基准环境示例。 (B) 不同方法生成多面体的种子包含率随OFR变化。 (C) 不同方法生成的多面体体积的箱线图，基于100次测试。中线表示中位数。 (D) 各方法计算时间的箱线图，基于100次测试。中线表示中位数。

Fig. S9. 障碍函数 \(\mathcal{L}_{\mu}\) 的绘图。图示了函数 \(\mathcal{L}_{\mu}\)。随着参数 \(\mu\) 从 \(\mu=0.5\) 降至 \(\mu=0.01\)，障碍函数逐渐变得更陡峭，形状更尖锐。

Fig. S10. 从 \(\eta\) 到 \(t_{s}\) 的映射绘图。图示一个类似 Sigmoid 的函数，将 \(\eta \in \mathbb{R}\) 映射到区间 \(\left(t_{c}, t_{o}\right)\)。

Fig. S11. 有限视场备份走廊生成 (A) 非凸视场的最大凸子集由两个由切割视场上边界 \(\mathcal{H}_{u}\) 和下边界 \(\mathcal{H}_{l}\) 切平面的半空间交集形成。 (B) 切平面 \(\mathcal{H}_{u}\) 和 \(\mathcal{H}_{l}\) 的侧视图及垂直视场角 \(\theta\)。 (C) 俯视图显示凸子集和两个切平面，由航向方向 \(\mathbf{h}\) 唯一确定。 (D) 航向方向与探索轨迹对齐，确保生成的备份走廊包含探索轨迹的初始部分。

Fig. S12. 不同垂直视场下的基准比较 (A) 在可通行性得分为3.1的模拟地图上的基准结果。 (B) 在可通行性得分为6.5的模拟地图上的基准结果。对于 (A) 和 (B)，分布图 (ii-iii) 展示平均飞行速度和执行备份轨迹的比例。误差条表示标准差，黑点表示均值。每个分布基于180次测试数据。

Fig. S13. 不同无人机模型下 MPC 跟踪性能比较。速度、俯仰角和各轴位置跟踪误差的绘图。

Table S1. 不同方法的实现细节

	类型	建图	前端	后端
Bubble (13)	乐观型	点云 + Kd-Tree	球形走廊	时空轨迹优化（STTP）
Raptor (20)	安全感知型	ESDF	拓扑路径搜索	路径引导轨迹优化（PGO）
Faster (23)	安全保障型	占据栅格地图（OGM）	多面体形走廊	混合整数二次规划（MIQP）
SUPER（本工作）	安全保障型	点云	多面体形走廊	时空轨迹优化（STTP）

本文其他补充材料包括：

视频1

视频 1. 提出 SUPER 系统的概览。SUPER 展示了其在高速下安全穿越未知且杂乱环境的能力，能够避开如电力线等细小障碍，并在多种场景中表现出鲁棒性，包括目标跟踪和自主探索。

视频S1至S4

视频 S1. 未知环境中的安全高速导航。

视频 S2. 杂乱环境中的导航。

视频 S3. SUPER 避开细小障碍能力演示。

视频 S4. SUPER 在目标跟踪、自主探索和航路点导航任务中的应用。

参考文献及注释

1. J. Verbeke, J. D. Schutter, Experimental maneuverability and agility quantification for rotary unmanned aerial vehicle. Int. J. Micro Air Veh. 10, 3-11 (2018).
2. B. Mishra, D. Garg, P. Narang, V. Mishra, Drone-surveillance for search and rescue in natural disaster. Comput. Commun. 156, 1-10 (2020).
3. S. M. S. M. Daud, M. Y. P. M. Yusof, C. C. Heo, L. S. Khoo, M. K. C. Singh, M. S. Mahmood, H. Nawawi, Applications of drone in disaster management: A scoping review. Sci. Justice 62, 30-42 (2022).
4. B. Rabta, C. Wankmüller, G. Reiner, A drone fleet model for last-mile distribution in disaster relief operations. Int. J. Disaster Risk Reduct. 28, 107-112 (2018).
5. Y. Song, A. Romero, M. Müller, V. Koltun, D. Scaramuzza, Reaching the limit in autonomous racing: Optimal control versus reinforcement learning. Sci. Robot. 8, eadg1462 (2023).
6. P. Foehn, A. Romero, D. Scaramuzza, Time-optimal planning for quadrotor waypoint flight. Sci. Robot. 6, eabh1221 (2021).
7. A. Romero, R. Penicka, D. Scaramuzza, Time-optimal online replanning for agile quadrotor flight. IEEE Robot. Autom. Lett. 7, 7730-7737 (2022).
8. A. Romero, S. Sun, P. Foehn, D. Scaramuzza, Model predictive contouring control for timeoptimal quadrotor flight. IEEE Trans. Robot. 38, 3340-3356 (2022).
9. E. Kaufmann, L. Bauersfeld, A. Loquercio, M. Müller, V. Koltun, D. Scaramuzza, Championlevel drone racing using deep reinforcement learning. Nature 620, 982-987 (2023).
10. J. Zhang, R. G. Chadha, V. Velivela, S. Singh, P-cal: Pre-computed alternative lanes for aggressive aerial collision avoidance, paper presented at the 12th International Conference on Field and Service Robotics (FSR), Tokyo, Japan, 31 August 2019.
11. J. Zhang, R. G. Chadha, V. Velivela, S. Singh, P-cap: Pre-computed alternative paths to enable aggressive aerial maneuvers in cluttered environments, in 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) (IEEE, 2018), pp. 8456-8463.
12. Y. Ren, S. Liang, F. Zhu, G. Lu, F. Zhang, Online whole-body motion planning for quadrotor using multi-resolution search, in 2023 IEEE International Conference on Robotics and Automation (ICRA) (IEEE, 2023), pp. 1594-1600.
13. Y. Ren, F. Zhu, W. Liu, Z. Wang, Y. Lin, F. Gao, F. Zhang, Bubble planner: Planning highspeed smooth quadrotor trajectories using receding corridors, in 2022 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) (IEEE, 2022), pp. 63326339.
14. X. Zhou, Z. Wang, H. Ye, C. Xu, F. Gao, Ego-planner: An esdf-free gradient-based local planner for quadrotors. IEEE Robot. Autom. Lett. 6, 478-485 (2020).
15. H. Ye, X. Zhou, Z. Wang, C. Xu, J. Chu, F. Gao, Tgk-planner: An efficient topology guided kinodynamic planner for autonomous quadrotors. IEEE Robot. Autom. Lett. 6, 494-501 (2020).
16. A. Loquercio, E. Kaufmann, R. Ranftl, M. Müller, V. Koltun, D. Scaramuzza, Learning highspeed flight in the wild. Sci. Robot. 6, eabg5810 (2021).
17. H. D. Escobar-Alvarez, N. Johnson, T. Hebble, K. Klingebiel, S. A. Quintero, J. Regenstein, N. A. Browning, R-advance: Rapid adaptive prediction for vision-based autonomous navigation, control, and evasion. J. Field Robot. 35, 91-100 (2018).
18. L. Quan, Z. Zhang, X. Zhong, C. Xu, F. Gao, Eva-planner: Environmental adaptive quadrotor planning, in 2021 IEEE International Conference on Robotics and Automation (ICRA) (IEEE, 2021), pp. 398-404.
19. L. Wang, Y. Guo, Speed adaptive robot trajectory generation based on derivative property of bspline curve. IEEE Robot. Autom. Lett. 8, 1905-1911 (2023).
20. B. Zhou, J. Pan, F. Gao, S. Shen, Raptor: Robust and perception-aware trajectory replanning for quadrotor fast flight. IEEE Trans. Robot. 37, 1992-2009 (2021).
21. H. Oleynikova, Z. Taylor, R. Siegwart, J. Nieto, Safe local exploration for replanning in cluttered unknown environments for microaerial vehicles. IEEE Robot. Autom. Lett. 3, 14741481 (2018).
22. S. Liu, M. Watterson, S. Tang, V. Kumar, High speed navigation for quadrotors with limited onboard sensing, in 2016 IEEE International Conference on Robotics and Automation (ICRA) (IEEE, 2016), pp. 1484-1491.
23. J. Tordesillas, B. T. Lopez, M. Everett, J. P. How, Faster: Fast and safe trajectory planner for navigation in unknown environments. IEEE Transact. Robot. 38, 922-938 (2021).
24. E. Mueggler, H. Rebecq, G. Gallego, T. Delbruck, D. Scaramuzza, The event-camera dataset and simulator: Event-based data for pose estimation, visual odometry, and slam. Int. J. Robot. Res. 36, 142-149 (2017).
25. Livox Technology Company Limited, Livox Mid-360 User Manual (2023); https://livoxtech.com/mid-360.
26. Wikipedia, Lockheed Martin F-35 Lightning II stealth multirole combat aircraft (2024); https://en.wikipedia.org/wiki/Lockheed_Martin_F-35_Lightning_II.
27. DJI, DJI Mavic 3 (2024); https://dji.com/mavic-3.
28. J. Tordesillas, B. T. Lopez, J. P. How, Faster: Fast and safe trajectory planner for flights in unknown environments, in 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) (IEEE, 2019), pp. 1934-1940.
29. Z. Wang, X. Zhou, C. Xu, F. Gao, Geometrically constrained trajectory optimization for multicopters. IEEE Trans. Robot. 38, 3259-3278 (2022).
30. C. Nous, R. Meertens, C. De Wagter, G. De Croon, Performance evaluation in obstacle avoidance, in 2016 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) (IEEE, 2016), pp. 3614-3619.
31. L. Han, F. Gao, B. Zhou, S. Shen, Fiesta: Fast incremental Euclidean distance fields for online motion planning of aerial robots, in 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) (IEEE, 2019), pp. 4423-4430.
32. B. Tang, Y. Ren, F. Zhu, R. He, S. Liang, F. Kong, F. Zhang, Bubble explorer: Fast uav exploration in large-scale and cluttered 3D-environments using occlusion-free spheres. arXiv:2304.00852 [cs.RO] (2023).
33. P. Foehn, E. Kaufmann, A. Romero, R. Penicka, S. Sun, L. Bauersfeld, T. Laengle, G. Cioffi, Y. Song, A. Loquercio, D. Scaramuzza, Agilicious: Open-source and open-hardware agile quadrotor for vision-based flight. Sci. Robot. 7, eabl6259 (2022).
34. NVIDIA Corporation, Nvidia Jetson TX2 embedded AI computing device (2024); https://developer.nvidia.com/embedded/jetson-tx2.
35. X. Zhou, X. Wen, Z. Wang, Y. Gao, H. Li, Q. Wang, T. Yang, H. Lu, Y. Cao, C. Xu, F. Gao, Swarm of micro flying robots in the wild. Sci. Robot. 7, eabm5954 (2022).
36. Skydio 2 Plus Enterprise, An intelligent flying robot powered by AI (2024); https://skydio.com/skydio-2-plus-enterprise.
37. A. Harmat, M. Trentini, I. Sharf, Multi-camera tracking and mapping for unmanned aerial vehicles in unstructured environments. J. Intell. Robot. Syst. 78, 291-317 (2015).
38. Intel, Intel RealSense Depth Camera D435i (2024); https://intelrealsense.com/depth-camerad435i.
39. W. Xu, Y. Cai, D. He, J. Lin, F. Zhang, Fast-lio2: Fast direct lidar-inertial odometry. IEEE Trans. Robot. 38, 2053-2073 (2022).
40. S. Liu, M. Watterson, K. Mohta, K. Sun, S. Bhattacharya, C. J. Taylor, V. Kumar, Planning dynamically feasible trajectories for quadrotors using safe flight corridors in 3-d complex environments. IEEE Robot. Autom. Lett. 2, 1688-1695 (2017).
41. F. Gao, W. Wu, W. Gao, S. Shen, Flying on point clouds: Online trajectory generation and autonomous navigation for quadrotors in cluttered environments, J. Field Robot. 36, 710-733 (2019).
42. J. Zhang, C. Hu, R. G. Chadha, S. Singh, Falco: Fast likelihood-based collision avoidance with extension to human-guided navigation. J. Field Robot. 37, 1300-1313 (2020).
43. D. Cheng, F. C. Ojeda, A. Prabhu, X. Liu, A. Zhu, P. C. Green, R. Ehsani, P. Chaudhari, V. Kumar, Treescope: An agricultural robotics dataset for lidar-based mapping of trees in forests and orchards, in 2024 IEEE International Conference on Robotics and Automation (ICRA) (IEEE, 2023), pp. 14860-14866.
44. P. De Petris, H. Nguyen, M. Dharmadhikari, M. Kulkarni, N. Khedekar, F. Mascarich, K. Alexis, Rmf-owl: A collision-tolerant flying robot for autonomous subterranean exploration, in 2022 International Conference on Unmanned Aircraft Systems (ICUAS) (IEEE, 2022), pp. 536543.
45. Y. Cai, F. Kong, Y. Ren, F. Zhu, J. Lin, F. Zhang, Occupancy grid mapping without raycasting for high-resolution LiDAR sensors. IEEE Trans. Robot. 40, 172-192 (2023).
46. Ouster, Ouster OS1-128 sensor (2024); https://ouster.com/products/scanning-lidar/os1-sensor.
47. DJI, DJI Matrice 300 RTK (2024); https://enterprise.dji.com/zh-tw/matrice-300.
48. LiDAR USA, LiDAR USA surveyor 32 (2024); https://lidarusa.com/.
49. Intel, Product Family D400 Series Datasheet (2024); https://intelrealsense.com/download/21345/?tmstv=1697035582.
50. A. Hornung, K. M. Wurm, M. Bennewitz, C. Stachniss, W. Burgard, Octomap: An efficient probabilistic 3d mapping framework based on octrees. Autonom. Robot. 34, 189-206 (2013).
51. Y. Ren, Y. Cai, F. Zhu, S. Liang, F. Zhang, Rog-map: An efficient robocentric occupancy grid map for large-scene and high-resolution lidar-based motion planning. arXiv:2302.14819 [cs.RO] (2023).
52. F. Kong, W. Xu, Y. Cai, F. Zhang, Avoiding dynamic small obstacles with onboard sensing and computation on aerial robots. IEEE Robot. Autom. Lett. 6, 7869-7876 (2021).
53. H. Wu, Y. Li, W. Xu, F. Kong, F. Zhang, Moving event detection from LiDAR point streams. Nat. Commun. 15, 345 (2024).
54. T-MOTOR, T-MOTOR F90 series racing drone motor specifications (2024); https://store.tmotor.com/goods-1064-F90.html.
55. Dronecode Foundation, PX4: Open source autopilot for drone developers (2024); https://px4.io.
56. J. Chen, K. Su, S. Shen, Real-time safe trajectory generation for quadrotor flight in cluttered environments, in 2015 IEEE International Conference on Robotics and Biomimetics (ROBIO) (IEEE, 2015), pp. 1678-1685.
57. P. Hart, N. Nilsson, B. Raphael, A formal basis for the heuristic determination of minimum cost paths. IEEE Trans. Syst. Sci. Cybern. 4, 100-107 (1968).
58. Q. Wang, Z. Wang, C. Xu, F. Gao, Fast iterative region inflation for computing large 2-d/3-d convex regions of obstacle-free space. arXiv:2403.02977 [cs.RO] (2024).
59. R. Deits, R. Tedrake, Computing large convex regions of obstacle-free space through semidefinite programming, in Algorithmic Foundations of Robotics XI: Selected Contributions of the Eleventh International Workshop on the Algorithmic Foundations of Robotics (Springer, 2015), pp. 109-124.
60. L. Yin, F. Zhu, Y. Ren, F. Kong, F. Zhang, Decentralized swarm trajectory generation for lidarbased aerial tracking in cluttered environments, in 2023 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) (IEEE, 2023), pp. 9285-9292.
61. J. Ji, N. Pan, C. Xu, F. Gao, Elastic tracker: A spatio-temporal trajectory planner for flexible aerial tracking, in 2022 International Conference on Robotics and Automation (ICRA) (IEEE, 2022), pp. 47-53.
62. S. Liu, N. Atanasov, K. Mohta, V. Kumar, Search-based motion planning for quadrotors using linear quadratic minimum time control, in 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) (IEEE, 2017), pp. 2872-2879.
63. ZJU FAST Lab, LBFGS-Lite: A header-only L-BFGS unconstrained optimizer (Github, 2024); https://github.com/ZJU-FAST-Lab/LBFGS-Lite.
64. M. Faessler, A. Franchi, D. Scaramuzza, Differential flatness of quadrotor dynamics subject to rotor drag for accurate tracking of high-speed trajectories. IEEE Robot. Autom. Lett. 3, 620-626 (2017).
65. G. Lu, W. Xu, F. Zhang, On-manifold model predictive control for trajectory tracking on robotic systems. IEEE Trans. Ind. Electron. 70, 9192-9202 (2022).
66. A. Koubaa, Ed., Robot Operating System (ROS): The Complete Reference (Volume 1), vol. 625 of Studies in Computational Intelligence (Springer, 2017).
67. W. Liu, Y. Ren, F. Zhang, Integrated planning and control for quadrotor navigation in presence of suddenly appearing objects and disturbances. IEEE Robot. Autom. Lett. 9, 899-906 (2023).
68. C. Toumieh, A. Lambert, Voxel-grid based convex decomposition of 3D space for safe corridor generation. J. Intell. Robot. Syst. 105, 87 (2022).
69. X. Zhong, Y. Wu, D. Wang, Q. Wang, C. Xu, F. Gao, Generating large convex polytopes directly on point clouds. arXiv:2010.08744 [cs.RO] (2020).
70. C. D. Toth, J. O'Rourke, J. E. Goodman, Handbook of Discrete and Computational Geometry (CRC Press, 2017).
71. D. Eberly, Distance from a point to an ellipse, an ellipsoid, or a hyperellipsoid (Geometric Tools, 2013); https://geometrictools.com/Documentation/DistancePointEllipseEllipsoid.pdf.
72. Z. Wang, "A geometrical approach to multicopter motion planning," thesis, Zhejiang University, Hangzhou, China (2022).
73. Y. Cui, D. Sun, K.-C. Toh, On the r-superlinear convergence of the KKT residuals generated by the augmented lagrangian method for convex composite conic programming. Math. Program. 178, 381-415 (2019).
74. W. Wu, mockamap, https://github.com/HKUST-Aerial-Robotics/mockamap.
75. Dronecode Foundation, PX4 software in the loop simulation with Gazebo (2024); https://docs.px4.io/v1.12/en/simulation/gazebo.html.

Submitted 13 February 2024 Accepted 23 December 2024 Published 29 January 2025 10.1126/scirobotics.ado6187