D12 Mini-Drone 综合实战——从零搭建自主探索无人机¶

性质：工程实践教学 | 难度跨度：⭐⭐ ~ ⭐⭐⭐⭐ | 预计精读：25-35 小时（含动手）

一句话定位：D1-D11 是"拆解别人的系统"，D12 是"从零搭建自己的系统"。本章把前 11 章的离散知识点——微分平坦、几何控制、MINCO 轨迹、环境表示、感知探索——焊接成一条**可以真正起飞的端到端自主探索栈**，并把工程上真正会咬人的东西（TF 树对齐、时间同步、坐标系翻转、Offboard 心跳、规划超时、控制震荡、FSM 死锁、实机起飞前的 30 项 checklist）一次讲透。

本章定位：从"读系统"到"搭系统"¶

前 11 章我们一直在做同一件事的不同侧面：理解一个自主飞行系统的某一层。D1 讲控制层为什么用微分平坦，D3-D5 讲轨迹层怎么把航点变成多项式/B 样条/MINCO，D6 讲地图层怎么把点云变成可查询的占据/ESDF，D7 讲决策层怎么选下一个探索目标。每一层我们都精读了顶会论文和开源实现，但它们始终是**别人已经搭好的、被拆开给你看的系统**。

D12 要做的事情根本不同：你要自己把这些层焊接起来，让一架（仿真或真实的）无人机在一个未知的房间里自己飞、自己建图、自己探索、自己回家。 这是一个最小化但完整的系统集成项目，也是整个无人机方向的毕业验收。

这件事的难点**不在任何单一算法**——每一层的算法你都学过了。难点在于**层与层之间的接缝**：

FAST-LIO2 输出的里程计在哪个坐标系？ROG-Map 期望的点云在哪个坐标系？两者差一个 TF，地图就会"飘"或者"镜像"。
规划器要 5 Hz 重规划，但 SLAM 偶尔卡到 80 ms，地图更新偶尔卡到 60 ms——加起来这一帧规划就超时了，无人机此刻在以 3 m/s 前飞，参考轨迹已经过期。怎么办？
PX4 用 NED（北-东-地）坐标系和 FRD（前-右-下）机体系，ROS/MAVROS 用 ENU（东-北-天）和 FLU（前-左-上）。你的几何控制器算出来的推力和角速度，发给 PX4 之前必须做一次坐标系翻转，否则无人机会朝着完全相反的方向猛拉。
Offboard 模式要求 OffboardControlMode 消息流稳定在 2 Hz 以上，否则 PX4 在 0.5 秒后自动退出 Offboard 触发 failsafe。你的控制线程哪怕卡 0.6 秒，无人机就"夺权"了。

本质洞察：自主飞行系统的可靠性，不取决于最强的那一层，而取决于最弱的那个接缝。一个用了 SOTA 规划器但 TF 配错的系统，远不如一个用最朴素 Yamauchi 前沿但每个接缝都对齐的系统。本章 60% 的篇幅在讲接缝，因为那才是从"论文能跑"到"系统能飞"之间真正的鸿沟。

本章是**工程实践教学**：代码是主要载体，但每一段代码、每一个配置项、每一个坐标系约定，都会回答"为什么是这个值""改了会怎样""不这样会出什么错"。我们不重新推导微分平坦（那是 D1），但我们会给出把 D1 的平坦映射真正接到 PX4 MAVLink 上的完整胶水代码。

环境配置指南¶

工程实践文档的第一要务是**让读者能把环境搭起来**。本节给出系统要求、版本兼容表和分场景的安装步骤。无人机全栈对版本极其敏感——ROS2 发行版、PX4 固件、MAVROS、Gazebo 版本之间存在复杂的兼容矩阵，一个版本错配就会卡在编译或运行的某个角落几个小时。

系统要求¶

项目	仿真（路线 α/β）	仿真+实机（路线 γ）	说明
操作系统	Ubuntu 22.04 LTS	Ubuntu 22.04 LTS	22.04 对应 ROS2 Humble，是当前最稳的组合
CPU	4 核以上	8 核以上（机载 Jetson Orin NX/AGX）	SLAM+规划+控制并行跑，核心数直接决定能否实时
内存	8 GB	16 GB	Gazebo + RViz + 多节点，8 GB 是下限
GPU	集显可跑 Gazebo	机载 Jetson（CUDA）	路线 α/β 仿真不强依赖独显；3DGS/nvblox 需 CUDA
磁盘	30 GB	50 GB	PX4 源码编译 + ROS2 + rosbag 录包

本质洞察：选 Ubuntu 22.04 + ROS2 Humble 不是因为它最新（更新的是 24.04 + Jazzy），而是因为它的**生态成熟度最高**——FAST-LIO2、MAVROS、PX4、ego-planner 的 ROS2 移植绝大多数首先在 Humble 上验证。工程实践里"最新"几乎总是错误的默认选择；"被最多人跑通过"才是。这是贯穿本章的版本选择哲学。

版本兼容表¶

下表是本章经过验证的版本组合。强烈建议精确锁定这些版本——无人机全栈的版本地狱（dependency hell）是初学者最容易卡死的地方。

组件	推荐版本	最低版本	最高测试版本	备注
Ubuntu	22.04 LTS	20.04	22.04	20.04 对应 ROS2 Foxy（已 EOL，不推荐）
ROS2	Humble	Foxy	Iron	Humble 是 LTS，支持到 2027
PX4-Autopilot	v1.14.x	v1.13	v1.15	v1.14 起 uXRCE-DDS 取代 microRTPS
Micro-XRCE-DDS-Agent	v2.4.x	v2.0	v2.4.x	与 PX4 内置 client 版本须匹配
px4_msgs	release/1.14	release/1.13	release/1.15	必须与 PX4 固件版本同分支，否则消息定义不匹配
MAVROS	2.x (ros-humble-mavros)	2.0	2.8	路线用 MAVROS 走 MAVLink；与 uXRCE-DDS 二选一
Gazebo	Garden / Harmonic	Classic 11	Harmonic	PX4 v1.14+ 默认 Gazebo Garden（gz-sim）
Eigen	3.4.0	3.3.7	3.4.0	header-only，与 SLAM 主线一致
FAST_LIO (ROS2)	ROS2 分支	-	-	hku-mars/FAST_LIO 的 ROS2 分支或 Ericsii/FAST_LIO_ROS2
ROG-Map	master	-	-	hku-mars/ROG-Map，header-only 风格
Livox-SDK2	1.2.x	1.0	1.2.x	仅实机 Livox 雷达需要
PCL	1.12 (随 Ubuntu)	1.10	1.12	点云处理

⚠️ 最容易踩的版本陷阱：px4_msgs 的版本**必须**与 PX4 固件版本严格对应。PX4 的 uORB 消息定义（如 VehicleLocalPosition、TrajectorySetpoint）在不同固件版本间会增删字段。如果你用 PX4 v1.14 固件但编译了 v1.13 的 px4_msgs，uXRCE-DDS 桥接会**静默失败**——话题存在但内容全是 0，且不报错。这是本章故障排查手册的高频场景。

两条通信路线的选择：uXRCE-DDS vs MAVROS¶

PX4 与机载/地面 ROS2 之间有**两条**主流通信桥，它们不是竞争关系而是分工关系，初学者常常混淆。理解这个选择是搭栈的第一个架构决策。

维度	uXRCE-DDS（px4_ros_com 路线）	MAVROS（mavlink 路线）
协议	DDS（PX4 uORB 直通 ROS2 DDS 域）	MAVLink（消息中继）
延迟	更低（无消息翻译层）	略高（MAVLink ↔ ROS 消息映射）
暴露内容	PX4 内部 uORB 话题（细粒度）	MAVLink 标准消息（粗粒度、跨飞控通用）
坐标系	PX4 原生 NED/FRD，需自己翻转到 ENU	MAVROS 已做 NED↔ENU 翻转
成熟度	PX4 v1.14+ 官方主推方向	老牌、文档多、ArduPilot 也支持
适合	高频自定义控制（角速度/推力直发）	快速上手、标准 Offboard 速度/位置控制
本章用法	路线 β/γ 的高频控制接口	路线 α 的快速验证接口

本质洞察：uXRCE-DDS 不是 MAVROS 的升级版，而是**抽象层级更低**的接口。MAVROS 给你的是"标准化、跨飞控、已翻转坐标系"的便利；uXRCE-DDS 给你的是"PX4 原生、零翻译延迟、但要自己处理 NED/FRD"的控制权。选哪个取决于你需要的是便利还是控制权——这正是 R6 里"抽象层的边界"在通信层的体现。本章路线 α 用 MAVROS 求快速跑通，路线 β/γ 用 uXRCE-DDS 求高频低延迟。

安装步骤¶

下面给出**路线 β 标准栈**（PX4 SITL + uXRCE-DDS + Gazebo）的完整安装命令。路线 α 的 MAVROS 安装在 Quick Start 中给出最短路径。

步骤 1：安装 ROS2 Humble¶

# 设置 locale（ROS2 要求 UTF-8）
sudo apt update && sudo apt install -y locales
sudo locale-gen en_US en_US.UTF-8
sudo update-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8

# 添加 ROS2 apt 源
sudo apt install -y software-properties-common curl
sudo add-apt-repository universe
sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key \
  -o /usr/share/keyrings/ros-archive-keyring.gpg
echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] \
  http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" \
  | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null

# 安装 ros-humble-desktop（含 RViz2、demo 节点）
sudo apt update
sudo apt install -y ros-humble-desktop ros-dev-tools

# 每次新开终端都要 source；建议写进 ~/.bashrc
source /opt/ros/humble/setup.bash

每条命令的作用：locale-gen 确保系统编码是 UTF-8（ROS2 的消息序列化依赖它，编码错会导致中文路径或某些消息乱码）；apt 源那一段是把 ROS2 官方仓库的 GPG 公钥和源地址写入系统；ros-humble-desktop 是完整桌面版（含可视化工具），如果只在机载无头机器上跑可以换成 ros-humble-ros-base 省空间。

步骤 2：安装 PX4-Autopilot 与工具链¶

# 克隆 PX4 源码（递归拉取子模块，--recursive 不可省）
git clone https://github.com/PX4/PX4-Autopilot.git --recursive -b v1.14.0
cd PX4-Autopilot

# 安装 PX4 编译依赖（这个脚本会装 gcc-arm、gazebo、python 依赖等）
bash ./Tools/setup/ubuntu.sh

# 编译 SITL + Gazebo（首次编译约 10-20 分钟）
make px4_sitl gz_x500   # x500 是默认四旋翼机型

--recursive 标志至关重要：PX4 有大量 git 子模块（包括 uXRCE-DDS client、各种驱动），漏掉它编译会在中途报缺文件。-b v1.14.0 锁定固件版本，对应版本兼容表。gz_x500 是要编译的仿真目标——x500 是 Holybro 的标准四旋翼，PX4 自带其 SDF 模型。

步骤 3：安装 Micro-XRCE-DDS-Agent（DDS 桥接代理）¶

git clone -b v2.4.2 https://github.com/eProsima/Micro-XRCE-DDS-Agent.git
cd Micro-XRCE-DDS-Agent
mkdir build && cd build
cmake ..
make
sudo make install
sudo ldconfig /usr/local/lib/   # 刷新动态库缓存

Agent（代理）跑在机载/地面计算机上，是 PX4 内部 uXRCE-DDS client 在 ROS2 这一侧的对端。架构是：PX4 固件里的 client 把选定的 uORB 话题打包，通过串口或 UDP 发给 Agent，Agent 再把它们发布到 ROS2 的 DDS 数据空间。没有 Agent，PX4 的话题在 ROS2 里完全看不见。

步骤 4：创建 ROS2 工作空间并拉取 px4_msgs¶

mkdir -p ~/drone_ws/src && cd ~/drone_ws/src

# px4_msgs 必须与固件版本同分支！
git clone https://github.com/PX4/px4_msgs.git -b release/1.14

# （路线 α 备选）安装 MAVROS
sudo apt install -y ros-humble-mavros ros-humble-mavros-extras
ros2 run mavros install_geographiclib_datasets.sh   # MAVROS 需要地理库数据集

cd ~/drone_ws
colcon build --symlink-install
source install/setup.bash

--symlink-install 让 Python 脚本和配置文件以软链接方式安装，改了源码不用重新 colcon build——开发期强烈建议开启。install_geographiclib_datasets.sh 下载 MAVROS 做地理坐标转换需要的 EGM96 大地水准面数据集，漏掉它 MAVROS 启动会报 geographiclib 缺数据。

Quick Start（10 分钟跑通最小示例）¶

工程实践文档的灵魂是"读者能在十分钟内看到东西动起来"。下面给出**最短路径**：让一架仿真无人机在 Gazebo 里起飞、悬停、降落。这条路径用 PX4 SITL + uXRCE-DDS，不涉及 SLAM/规划，只验证通信链路通了。

打开**四个终端**（建议用 tmux 或 terminator 分屏）：

# ─── 终端 1：启动 PX4 SITL + Gazebo ───
cd ~/PX4-Autopilot
make px4_sitl gz_x500
# 看到 "INFO [commander] Ready for takeoff!" 表示飞控就绪
# Gazebo 窗口会弹出，显示一架 x500 停在地面

# ─── 终端 2：启动 uXRCE-DDS Agent ───
MicroXRCEAgent udp4 -p 8888
# PX4 SITL 默认通过 UDP 8888 连 Agent
# 看到 "client_key" 的连接日志表示桥接建立

# ─── 终端 3：检查 ROS2 话题是否出现 ───
source ~/drone_ws/install/setup.bash
ros2 topic list | grep fmu
# 应看到 /fmu/out/vehicle_local_position、/fmu/in/trajectory_setpoint 等
ros2 topic echo /fmu/out/vehicle_local_position --once
# 应打印出位置数据（z 约为 0，无人机在地面）

# ─── 终端 4：发送起飞指令（用 PX4 自带的 commander） ───
# 在终端 1 的 PX4 shell（pxh> 提示符）里输入：
# pxh> commander takeoff
# 无人机应起飞到默认高度（约 2.5 m）悬停
# pxh> commander land
# 无人机降落

成功标志：终端 3 能 echo 出 /fmu/out/vehicle_local_position 的真实数据（z 坐标随起飞变化），且 Gazebo 里无人机起飞悬停。如果话题列表里看不到 /fmu/*，说明 Agent 没连上 PX4——这是第一个要排查的接缝（见故障排查手册场景一）。

⚠️ 配置陷阱：忘了启动 Agent。新手最常见的卡点是只开了 PX4 SITL（终端 1）就去 ros2 topic list，发现没有任何 /fmu 话题，以为是 ROS2 装坏了。实际上 PX4 的 uORB 话题**必须经过 Agent 才能进入 ROS2 DDS 空间**——没有终端 2 的 Agent，PX4 和 ROS2 是两个隔离的世界。记住这个数据流：PX4 uORB → uXRCE-DDS client（固件内）→ UDP 8888 → Agent（终端 2）→ ROS2 DDS。

跑通这个最小示例后，你已经验证了**整条通信链路**。接下来全章要做的，就是在这条链路上逐层加挂 SLAM、地图、探索、轨迹、控制、FSM。

前置自测¶

开始前先回答下面 5 个问题。这是整个无人机方向的毕业验收章，答不出 2 题以上说明前面的某一层还没吃透，建议先回对应章节——D12 的每一层集成都直接复用前面的核心结论，欠了账会在搭对应模块时卡住。

微分平坦逆映射把什么变成什么？ 给定平坦输出 \(\sigma(t) = (x, y, z, \psi)^\top\) 及其各阶导数，几何控制器需要从中代数地恢复出哪几个量交给底层（集体推力 + 体轴角速度）？为什么这个恢复**不需要积分**？（答不出 → 回 D1 微分平坦与几何控制，§D1.1、§D1.4）
MINCO 轨迹表示相对于直接优化多项式系数，核心优势是什么？ 它的决策变量是什么（不是系数，而是什么）？为什么这让大规模轨迹优化既快又数值稳定？（答不出 → 回 D5 MINCO 表示与安全走廊，§D5.2）
ESDF 和占据栅格（Occupancy Grid）分别回答什么问题？ 规划器做碰撞检测时用哪个、做梯度优化（推离障碍物）时用哪个？ROG-Map 和 voxblox 各自的输出是哪一种？（答不出 → 回 D6 无人机环境表示，§D6.1、§D6.2）
前沿（Frontier）的定义是什么？ 为什么"已知自由空间与未知空间的边界"是自主探索的天然驱动信号？最朴素的 Yamauchi 前沿探索的决策逻辑是什么（一句话）？（答不出 → 回 D7 感知引导规划与自主探索，§D7.1）
ROS2 的 TF 树（Transform Tree）解决什么问题？ 如果一个节点发布的点云在 lidar 坐标系，而规划器需要 map 坐标系下的点云，缺了 map→odom→base_link→lidar 这条变换链会发生什么？时间戳在 TF 查询中起什么作用？（答不出 → 回 v8 Ch31 ROS2 高级，或本章 §D12.2 会详细展开）

参考答案要点（先自己答，再对照）：

微分平坦逆映射把**平坦输出曲线及其导数**代数地变成**全部状态和控制输入**。几何控制器从位置的二阶导（加速度）恢复期望推力方向和集体推力大小，从三阶导（jerk）恢复体轴角速度，从 yaw 及其导数恢复偏航通道。不需要积分是因为微分平坦的本质就是：状态和控制都是平坦输出**及其有限阶导数的纯代数函数**——这正是 D1 的核心结论，也是为什么轨迹规划只需在 4 维平坦空间里设计曲线。
MINCO 的决策变量是**每段轨迹的中间航点位置和段时间**，而不是多项式系数。给定航点和时间，最优的多项式系数有**闭式解**（通过一个稀疏带状线性系统）。这让优化变量数从 \(O(段数 \times 阶数)\) 降到 \(O(段数)\)，且带状矩阵求解 \(O(M)\) 线性复杂度，避免了直接优化系数时 Vandermonde 矩阵的数值病态。这是 D5 的核心，本章轨迹层直接用它。
占据栅格回答"这个体素是占据/空闲/未知"——用于**碰撞检测**（查询路径上的体素是否占据）和**前沿提取**（找已知空闲与未知的边界）。ESDF 回答"这个点到最近障碍物表面的距离是多少"——其**梯度** \(\nabla\text{ESDF}\) 提供推离障碍物的方向，用于**轨迹梯度优化**。ROG-Map 主要输出占据栅格（+ 计数器膨胀），voxblox 输出 TSDF 进而算出 ESDF。
前沿是**已知自由空间与未知空间的边界体素**。它是探索的天然信号，因为"站在已知与未知的边界上往未知方向看"恰好是获取新信息最多的地方——飞到前沿就能把未知变已知。Yamauchi 前沿探索的逻辑：提取所有前沿 → 选最近的一个 → 飞过去 → 地图更新、前沿移动 → 重复，直到没有前沿（全部探索完）。
TF 树维护**各坐标系之间随时间变化的刚体变换**，让任意节点能把数据从一个坐标系变换到另一个。缺了变换链，TF 查询会抛 LookupException，规划器拿不到 map 系下的点云，要么崩溃要么用错坐标的数据（地图会"飘"或镜像）。时间戳关键在于：TF 是**带时间戳的**，查询 t 时刻的变换会做插值——传感器数据的时间戳和 TF 的时间戳必须对齐（时间同步），否则查到的是错误时刻的位姿，高速飞行时这个错位会放大成米级误差。

本章目标¶

学完本章后，你应该能够：

**独立搭建**一条端到端自主探索栈：仿真环境 → LiDAR-惯性里程计（FAST-LIO2）→ 环境表示（ROG-Map/voxblox）→ 前沿探索 → 轨迹优化（MINCO/B 样条）→ SE(3) 几何控制 → FSM 调度，并让无人机在未知室内环境中自主完成 >80% 体积覆盖
诊断并修复系统接缝问题：TF 树缺失/错配、NED↔ENU 坐标系翻转、时间不同步、Offboard 心跳丢失、消息频率不匹配——这些是集成中真正会咬人的东西
设计鲁棒的 FSM：用有限状态机调度起飞-探索-返航的全流程，正确处理规划超时、电池低电、通信中断三类异常，避免状态死锁
做实机部署的安全工程：执行起飞前 checklist、配置 failsafe、用 rosbag 复现问题、从仿真逐步过渡到实机（拴绳测试 → 限高 → 放开）
定位并优化性能瓶颈：用 profiler 和 timeline 测量 SLAM 延迟、地图更新、规划时间、控制频率，画出甘特图，把规划频率从 1 Hz 调到 10 Hz
产出可复现的技术文档：系统架构图、ROS2 计算图、TF 树、参数表、性能指标表——让别人能照着把你的系统跑起来

本章知识导航¶

本章的知识结构不是一棵"概念树"（前面章节是），而是一张**数据流图**——因为系统集成的核心就是让数据正确地在层与层之间流动。下图是本章的主干，每个方框是一层，方框之间的箭头（接缝）才是本章的重点。

                    本章 = 把这 7 层焊接成一个能飞的系统
                    每层你都学过，难点在层与层之间的「接缝」

┌─────────────────────────────────────────────────────────────┐
│ §D12.1 仿真层  Gazebo + PX4 SITL                            │
│   产出：IMU + LiDAR 点云 + 深度图 + 真值位姿               │
└───────────────────┬─────────────────────────────────────────┘
                接缝①：传感器话题 + 时间戳 + 坐标系
                    ▼
┌─────────────────────────────────────────────────────────────┐
│ §D12.2 感知层  FAST-LIO2（LiDAR-惯性里程计）               │
│   产出：Odometry（100 Hz）+ 去畸变点云                     │
│   ★接缝重点：TF 树、时间同步                                │
└───────────────────┬─────────────────────────────────────────┘
                接缝②：里程计 + 点云 → map 坐标系
                    ▼
┌─────────────────────────────────────────────────────────────┐
│ §D12.3 地图层  ROG-Map / voxblox                           │
│   产出：占据栅格 + ESDF + 前沿体素                          │
└───────────────────┬─────────────────────────────────────────┘
                接缝③：占据/ESDF/前沿 → 决策
                    ▼
┌─────────────────────────────────────────────────────────────┐
│ §D12.4 探索决策层  前沿提取 + 视点选择                     │
│   产出：下一个探索目标（位置 + yaw）                       │
└───────────────────┬─────────────────────────────────────────┘
                接缝④：目标点 → 轨迹
                    ▼
┌─────────────────────────────────────────────────────────────┐
│ §D12.5 轨迹层  MINCO / B 样条优化                          │
│   产出：分段轨迹（位置+速度+加速度+yaw）                    │
└───────────────────┬─────────────────────────────────────────┘
                接缝⑤：轨迹采样 → 控制参考
                    ▼
┌─────────────────────────────────────────────────────────────┐
│ §D12.6 控制层  SE(3) 几何控制 + 微分平坦逆映射             │
│   产出：集体推力 + 体轴角速度                               │
│   ★接缝重点：NED↔ENU 翻转、Offboard 心跳                   │
└───────────────────┬─────────────────────────────────────────┘
                接缝⑥：推力/角速度 → MAVLink/uORB
                    ▼
┌─────────────────────────────────────────────────────────────┐
│ §D12.7 FSM 调度层  INIT→TAKEOFF→EXPLORING→RETURN_HOME       │
│   异常：规划超时→悬停；电池低→返航；通信断→降落            │
└─────────────────────────────────────────────────────────────┘
         §D12.8 性能调优  │  §D12.9 实机部署 checklist

小节	主题	难度	一句话
§D12.1	仿真环境搭建	⭐⭐	Gazebo+PX4 SITL，配 LiDAR/深度相机传感器
§D12.2	感知层集成与 TF/时间同步	⭐⭐⭐	FAST-LIO2 接入，攻克 TF 树和时间戳两大接缝
§D12.3	地图层集成	⭐⭐	ROG-Map/voxblox，输出占据/ESDF/前沿
§D12.4	探索决策层	⭐⭐⭐	前沿提取 + 视点选择 + yaw 规划
§D12.5	轨迹层集成	⭐⭐⭐	MINCO/B 样条，安全走廊，重规划触发
§D12.6	控制层与坐标系翻转	⭐⭐⭐⭐	几何控制接 PX4，NED↔ENU，Offboard 心跳
§D12.7	FSM 调度与异常处理	⭐⭐⭐	状态机设计，三类异常，死锁规避
§D12.8	性能调优与 timeline	⭐⭐⭐	profiler、甘特图、把规划频率拉到 10 Hz
§D12.9	实机部署 checklist	⭐⭐⭐⭐	failsafe、拴绳测试、仿真到实机的渐进迁移

三条阅读线（对应骨架的三条难度递增路线）：

路线 α（入门，1 周）：§D12.1→§D12.2（用真值位姿替代 SLAM）→§D12.3（OctoMap）→§D12.4（Yamauchi）→§D12.5（min-snap）→§D12.6（MAVROS）→§D12.7。求**跑通**，不求性能。
路线 β（标准，1.5 周）：全部小节，SLAM 用 FAST-LIO2，地图用 voxblox，轨迹用 B 样条，控制用 uXRCE-DDS DFBC。求 >90% 覆盖、>5 Hz 规划。
路线 γ（进阶，2 周）：全部小节 + §D12.8 深度调优，地图用 ROG-Map，探索用 FUEL 简化版，轨迹用 GCOPTER/MINCO，控制用 NMPC。求林地场景 >10 m/s、规划 <5 ms。

前置知识桥接¶

本章是综合验收，几乎复用前面每一章。这里只重述本章**直接用到的核心结论**，每条都说明"当前怎么复用"。

回顾 D1（微分平坦与几何控制）：四旋翼是微分平坦系统——平坦输出 \(\sigma = (x,y,z,\psi)^\top\) 及其前四阶导数可以**纯代数地**恢复全部状态和控制。Lee 的 SE(3) 几何控制器在旋转群 \(SO(3)\) 上直接定义姿态误差，避免欧拉角奇异。本章 §D12.6 直接把这个逆映射写成胶水代码：从轨迹层给的 \((p, v, a, j, \psi, \dot\psi)\) 算出集体推力和体轴角速度，发给 PX4。你在 D1 推导的 \(f = m\|\ddot{p} + g e_3\|\) 和角速度公式这里逐行用上。

回顾 D5（MINCO 与安全走廊）：MINCO 用**航点 + 段时间**作决策变量，多项式系数由闭式的稀疏带状系统求出，实现时空联合优化的 \(O(M)\) 复杂度。安全走廊（Safe Flight Corridor, SFC）把无穷维的避障约束退化为有限维的多面体约束。本章 §D12.5 用 GCOPTER（MINCO 的开源实现）接收探索层给的目标点，在 ROG-Map 提供的安全走廊里优化出轨迹。你在 D5 学的"为什么 MINCO 既快又稳"这里是轨迹层能 10 Hz 重规划的根本原因。

回顾 D6（环境表示）：占据栅格用于碰撞检测和前沿提取，ESDF 的梯度用于轨迹优化的避障项。ROG-Map 是机器人中心（robocentric）的滑窗占据图，在实际飞行测试中平均只用 29.8% 的帧时间就能以 50 Hz 更新地图（Ren et al., IROS 2024）。本章 §D12.3 把它接在 FAST-LIO2 后面，输出供探索层和轨迹层共用的地图。

回顾 D7（感知引导规划与自主探索）：前沿是已知/未知的边界，是探索的天然信号；yaw 是四旋翼"免费"的感知自由度（不影响位置跟踪却能改变视野朝向）。本章 §D12.4 实现最朴素的 Yamauchi 前沿（路线 α）到 FUEL 简化版（路线 γ），输出带 yaw 的目标视点。你在 D7 学的"探索 = 选视点 + 规 yaw"这里是决策层的全部内容。

回顾 v8 Ch31（ROS2 高级）：节点、话题、服务、动作、生命周期、TF 树、参数、launch。本章每一层都是一个或一组 ROS2 节点，层间通过话题通信，坐标变换通过 TF 树，整个系统用一个 launch 文件拉起。如果 Ch31 学的 TF 和 launch 不熟，§D12.2 和系统集成会很吃力。

前向预告：本章搭的是**经典模块化栈**（感知→地图→规划→控制分离）。这套架构清晰、可调试、每层可独立验证，是工程落地的主流。但它有固有局限：层间延迟累积、模块边界处信息损失。D9（RL 敏捷飞行）和导读里讲的"学习+优化混合架构"指向另一条路——用端到端学习压缩感知到控制的链路。现在只需要知道：模块化栈是你必须先掌握的基本功，它是理解和调试任何更高级架构的地基。你不可能在没搭过模块化栈的情况下，理解为什么端到端学习要那样设计。

如果跳过本章会怎样¶

跳过 D12，你会停留在"每一层都懂、但从没让它们一起飞过"的状态。这个状态的危险在于：你以为你会了，直到你真的去搭。

场景一："每个节点单独跑都对，连起来无人机就乱飞。" 你的 FAST-LIO2 里程计在 RViz 里看着完美，ROG-Map 单独喂真值位姿也建图正确，几何控制器在单元测试里输出合理。但当你用一个 launch 把它们全拉起来，无人机一起飞就朝墙猛冲。根本原因可能是：FAST-LIO2 输出的里程计在 ENU，但你的控制器假设它是 NED，于是南北方向反了；或者 TF 树里 map→odom 没人发布，规划器拿到的障碍物坐标全错。没有 D12 的"接缝工程"训练，你会在这类问题上耗掉几天，且不知道从哪下手——因为每一层"看起来"都是对的。

场景二："仿真飞得好好的，实机一上电就炸。" 你在 Gazebo 里把整套栈调到完美，覆盖率 95%。满怀信心接到真机上，刚切 Offboard 无人机就翻了。可能是：你的控制线程偶尔卡 0.6 秒，PX4 在 0.5 秒没收到 OffboardControlMode 就退出 Offboard 触发 failsafe；或者实机 IMU 噪声比仿真大得多，FAST-LIO2 在起飞震动下发散。没有 D12 的实机 checklist 和 failsafe 配置，你的第一次实飞极可能以炸机收场——而炸机的代价是真金白银和几周的修复。

这两个场景是无人机工程师**真实的成人礼**。D12 的目的就是让你在仿真里、在拴着绳的安全测试里，提前把这些坑踩完。

预计学习时间¶

模式	时长	适合
精读+动手	25-35 小时	第一次搭完整栈：跟着每节搭对应模块，亲手调通每个接缝，跑完路线 α 或 β 的完整验收。建议分 2 周完成。
速读	4-6 小时	已搭过类似系统、想看本章的接缝处理和 checklist：读每节的"接缝重点"、坐标系翻转、Offboard 心跳、FSM 异常处理、实机 checklist。
速查	1 小时	搭栈时卡在某个接缝：直接跳到故障排查手册定位症状，或查对应接缝小节的代码模板。

建议时间分配（对应骨架）：

内容	时间	优先级
§D12.1 环境搭建与传感器配置	3h	必做
§D12.2 SLAM 集成 + TF/时间同步	5h	必做（接缝重灾区）
§D12.3 地图集成	3h	必做
§D12.4 探索决策	3h	必做
§D12.5 轨迹集成	3h	必做
§D12.6 控制 + 坐标系翻转	5h	必做（接缝重灾区）
§D12.7 FSM 与异常处理	4h	必做
§D12.8 性能调优	4h	路线 γ 必做
§D12.9 实机部署	4h	有真机者必做

§D12.1 仿真环境搭建——传感器、坐标系与数据流 ⭐⭐¶

模块功能与在系统中的位置¶

仿真层是整个栈的"虚拟世界"，它替代真实物理环境，向上提供三样东西：传感器数据（IMU、LiDAR 点云、深度图）、真值位姿（ground truth，用于评估 SLAM 误差或在路线 α 中直接替代 SLAM）、执行器接口（接收控制指令驱动无人机）。在实机部署时，这一层会被真实的传感器和飞行器替换，而上面所有层不需要改动——这正是模块化栈的价值。

本质洞察：仿真层的设计目标**不是"逼真"，而是"接口一致"。你的 SLAM/规划/控制看到的应该是和真机**完全相同的话题、相同的坐标系、相同的消息类型。如果仿真里 LiDAR 话题叫 /lidar/points 而真机叫 /livox/points，或者仿真给 ENU 而真机给 NED，那么"仿真调通了"对实机毫无意义。接口一致性比视觉逼真度重要一个数量级——这是为什么本章用 Gazebo（接口标准）而非追求 photorealistic 的 Flightmare 作为主力仿真器。

为什么用 Gazebo + PX4 SITL（动机 → 反面 → 历史 → 选型）¶

动机：我们需要一个能同时仿真**飞行动力学**（无人机怎么飞）和**传感器**（看到什么）的环境，且这个环境产出的接口要和真机一致。

反面：如果只用一个纯运动学仿真（直接给无人机设位置，不走动力学），你的控制器永远不会被真正测试——因为没有"控制指令→推力→加速度→运动"这条物理链路。控制器在这种仿真里"完美"，到真机上才发现根本没调过。

历史：无人机仿真有几代演进。Gazebo Classic + RotorS（ETH ASL）是经典组合，但 Gazebo Classic 已停止维护。PX4 早期用 jMAVSim（轻量但传感器简陋），现在主推 Gazebo Garden/Harmonic（新一代 gz-sim）。追求视觉逼真的有 Flightmare（UZH，Unity 渲染）、Isaac Sim（NVIDIA，GPU+光追）、以及 3DGS 系的 FiGS。本章选 Gazebo Garden + PX4 SITL，因为它把"动力学真实"和"接口标准"平衡得最好，且是 PX4 官方默认。

选型对比（这是 R6E 系统性分类——按"动力学保真 / 视觉保真 / 接口标准化 / 上手成本"四维度）：

仿真器	动力学保真	视觉保真	接口标准化	上手成本	本章用法
Gazebo Garden + PX4	高（PX4 完整动力学）	中	高（ROS2 原生）	中	主力（路线 α/β/γ）
jMAVSim	中	低	中	低	不用（传感器太简）
Flightmare	中（可接 BEM）	高（Unity）	中	高	路线 γ 选配（要 photorealistic 视觉时）
Isaac Sim	高	极高（光追）	中（需 ROS bridge）	高	RL 训练用（见 D9）

配置详解：给无人机装上 LiDAR 和深度相机¶

PX4 的 x500 默认机型不带 LiDAR/深度相机，我们需要在 SDF 模型里加传感器。下面是给 x500 加一个 3D LiDAR 的核心 SDF 片段（放在 PX4-Autopilot/Tools/simulation/gz/models/x500_lidar/model.sdf）：

<!-- 在 x500 的 base_link 下挂载一个 16 线 3D LiDAR -->
<link name="lidar_link">
  <pose>0 0 0.1 0 0 0</pose>   <!-- 相对 base_link：正上方 0.1m -->
  <sensor name="lidar" type="gpu_lidar">
    <topic>/lidar/points</topic>      <!-- 点云话题名，要和上层一致 -->
    <update_rate>10</update_rate>      <!-- 10 Hz，与真实 Livox 一致 -->
    <lidar>
      <scan>
        <horizontal>
          <samples>1800</samples>      <!-- 水平 1800 点 → 0.2° 角分辨率 -->
          <resolution>1</resolution>
          <min_angle>-3.14159</min_angle>   <!-- 水平 360° -->
          <max_angle>3.14159</max_angle>
        </horizontal>
        <vertical>
          <samples>16</samples>        <!-- 16 线 -->
          <min_angle>-0.26</min_angle> <!-- 垂直 ±15° -->
          <max_angle>0.26</max_angle>
        </vertical>
      </scan>
      <range>
        <min>0.3</min>                 <!-- 最近 0.3m（盲区） -->
        <max>50.0</max>                <!-- 最远 50m -->
      </range>
    </lidar>
    <always_on>1</always_on>
    <visualize>true</visualize>        <!-- 在 Gazebo 里画出激光线 -->
  </sensor>
</link>

每个配置项的教学深度（这是工程实践教学的核心——不能只贴配置）：

配置项	含义	推荐值与依据	改了会怎样
`update_rate`	雷达扫描频率	10 Hz——匹配真实 Livox/Velodyne 的常见扫描率	调高到 20 Hz：点云更密但仿真负载翻倍；调低到 5 Hz：FAST-LIO2 在快速运动时去畸变变差
`samples`（水平）	水平采样点数	1800 → 0.2° 分辨率，接近中端机械雷达	调到 360：地图变粗，薄障碍物可能漏检；调到 3600：精度提升但点云处理变慢
`samples`（垂直）	线数	16 线——入门级 3D LiDAR	改 32/64 线：垂直覆盖更全（适合探索），但点云量成倍增长
`range.min`	盲区	0.3m——多数雷达近距盲区	设 0：会把无人机自身机臂/桨当障碍物，地图里出现"自杀式"占据
`range.max`	最远探测	50m——室内足够，室外可加大	设太大（>100m）室内场景：远处墙体反复扫描，地图边界外扩浪费内存

⚠️ 配置陷阱：range.min 设为 0 导致地图被自身污染。错误做法：图省事把 LiDAR 最近距离设为 0 或很小（如 0.05m）。现象：地图里无人机周围一圈总是"占据"，规划器认为自己被障碍物包围，要么无法生成轨迹要么疯狂震荡。根本原因：LiDAR 在近距离会扫到无人机自身的机臂、桨叶、起落架，这些点被当成环境障碍物写进地图。正确做法：range.min 设为略大于机身半径的值（x500 约 0.25m，设 0.3m 留余量）；更严格的做法是在 FAST-LIO2 入口做一个半径滤波（blind 参数），剔除距传感器 < blind 的点。检查方法：起飞前看 RViz 里无人机周围是否有一圈虚假占据。

加深度相机的 SDF 类似，关键差异是 type 用 depth_camera，话题给 /depth_camera/points（PointCloud2）或 /depth_camera/depth_image（Image）。深度相机视场窄（典型 87°×58°，如 RealSense D435）但分辨率高，适合 voxblox 的 TSDF；LiDAR 视场广、距离远，适合 ROG-Map。选哪个传感器决定了你地图层选 voxblox 还是 ROG-Map——这是 §D12.3 的伏笔。

代码走读：用 launch 文件拉起仿真 + 传感器桥接¶

Gazebo Garden 的传感器话题在 gz 的传输层，要通过 ros_gz_bridge 桥接到 ROS2。下面是一个最小 launch 文件（sim_bringup.launch.py）：

# sim_bringup.launch.py —— 拉起 PX4 SITL + Gazebo + 传感器桥接
from launch import LaunchDescription
from launch.actions import ExecuteProcess
from launch_ros.actions import Node

def generate_launch_description():
    return LaunchDescription([
        # 1) 启动 PX4 SITL + Gazebo（带 LiDAR 的 x500）
        ExecuteProcess(
            cmd=['make', 'px4_sitl', 'gz_x500_lidar'],
            cwd='/home/user/PX4-Autopilot',   # 改成你的 PX4 路径
            output='screen'),

        # 2) 启动 uXRCE-DDS Agent（PX4 话题 → ROS2）
        ExecuteProcess(
            cmd=['MicroXRCEAgent', 'udp4', '-p', '8888'],
            output='screen'),

        # 3) 桥接 Gazebo LiDAR 点云 → ROS2 PointCloud2
        Node(
            package='ros_gz_bridge', executable='parameter_bridge',
            arguments=[
                # 格式：话题@ROS类型@GZ类型
                '/lidar/points@sensor_msgs/msg/PointCloud2@gz.msgs.PointCloudPacked',
                # 桥接时钟，保证 ROS 用仿真时间
                '/clock@rosgraph_msgs/msg/Clock@gz.msgs.Clock',
            ],
            output='screen'),
    ])

逐段解释：

进程 1 直接调 PX4 的 make 目标启动 SITL，gz_x500_lidar 是我们加了 LiDAR 的机型（需先在 PX4 里注册该 airframe）。cwd 必须指向你的 PX4 源码目录。
进程 2 是 Agent，把 PX4 的 uORB 话题（位姿、IMU、控制接口）引入 ROS2。
节点 3 是关键的传感器桥——Gazebo 的点云在 gz.msgs.PointCloudPacked 类型，ROS2 用 sensor_msgs/PointCloud2，parameter_bridge 做双向翻译。特别注意桥接了 /clock：这让 ROS2 节点用**仿真时间**而非墙钟时间，是后面时间同步的基础（见 §D12.2）。

⚠️ 配置陷阱：忘记桥接 /clock 导致时间戳混乱。错误做法：只桥接传感器话题，不桥接 /clock，且没设 use_sim_time:=true。现象：FAST-LIO2 报"点云时间戳和 IMU 时间戳差距过大"，或 TF 查询频繁失败，RViz 里数据闪烁。根本原因：ROS2 节点默认用系统墙钟（wall clock）打时间戳，但 Gazebo 内部用仿真时钟。仿真可能比实时快或慢，两套时钟不一致导致所有时间戳错乱。正确做法：桥接 /clock 话题，并给**每个**ROS2 节点设 use_sim_time:=true 参数（在 launch 里统一设）。检查方法：ros2 param get /your_node use_sim_time 应返回 True。

运行验证¶

ros2 launch your_pkg sim_bringup.launch.py
# 新终端验证传感器：
ros2 topic hz /lidar/points        # 应约 10 Hz
ros2 topic echo /fmu/out/vehicle_local_position --once   # 应有位姿
# 在 RViz2 里添加 PointCloud2 显示，frame 设为 lidar，应看到点云

成功标志：/lidar/points 以约 10 Hz 发布，RViz 里能看到环境点云随无人机移动而更新。至此仿真层就绪，可以往上接感知层了。

⚠️ 常见陷阱¶

🔧 配置陷阱：PX4 airframe 没注册，gz_x500_lidar 找不到。错误做法：直接改了 SDF 模型文件就 make px4_sitl gz_x500_lidar。现象：报 Airframe not found 或 Gazebo 启动后无人机模型缺失。根本原因：PX4 通过 airframe 配置文件（ROMFS/px4fmu_common/init.d-posix/airframes/）注册可用机型，光改 SDF 不够，还要加一个 airframe 启动脚本并在 CMakeLists 里登记。正确做法：复制现有的 4001_gz_x500 airframe 脚本改名为 4XXX_gz_x500_lidar，在 airframes/CMakeLists.txt 里加一行，重新 make。或更简单：在现有 x500 的 SDF 里直接 include LiDAR sensor，沿用 gz_x500 目标。

⚠️ 编程陷阱：点云 frame_id 与 TF 树不一致。错误做法：桥接后不检查点云的 header.frame_id，假设它是 lidar。现象：FAST-LIO2 或 ROG-Map 报找不到 lidar 到 base_link 的变换，或点云在 RViz 里位置完全错误。根本原因：Gazebo 桥接出来的点云 frame_id 可能是 x500/base_link/lidar 这样的长名字（带模型命名空间），而你的 TF 树里写的是 lidar。正确做法：ros2 topic echo /lidar/points --field header.frame_id --once 查看实际 frame_id，要么在 TF 树里用这个全名，要么写一个小节点重映射 frame_id 为简短名。

💡 概念误区：以为仿真传感器没噪声就更好。新手想法："仿真里 LiDAR 是完美的，没噪声，这样调 SLAM 更容易。" 实际上：完美无噪声的仿真会让你的 SLAM/控制参数**过拟合到理想世界**。真机 IMU 有偏置漂移、LiDAR 有测距噪声、相机有运动模糊，你在无噪声仿真里调出的参数（如 SLAM 的协方差、控制的增益）到真机上往往不合适。正确理解：仿真应该**主动加噪声**。Gazebo 的 sensor 支持 <noise> 标签（高斯噪声）。路线 β/γ 应给 IMU 和 LiDAR 加接近真机的噪声，让参数在"有噪声"的前提下调，这样才能平滑迁移到实机。这是 sim-to-real 的基本功（详见 D9）。

练习¶

[配置练习] 给 x500 同时加 LiDAR 和深度相机两个传感器，在 RViz 里同时显示两者点云。对比：在一个有薄障碍物（如细栏杆）的场景里，哪个传感器能稳定看到栏杆？解释为什么（提示：角分辨率 vs 视场）。
[配置练习] 给 LiDAR 加 <noise> 标签（stddev 设 0.02m）。录一段 rosbag，对比加噪声前后 FAST-LIO2 的里程计抖动。验收标志：能在 RViz 里观察到加噪声后点云的"厚度"和里程计的微小抖动。
[管线搭建] 把 /clock 桥接去掉，观察会发生什么（哪些节点报错、报什么错）。然后加回来并给所有节点设 use_sim_time:=true。这道题的目的是让你**亲手制造并修复时间同步问题**，为 §D12.2 做准备。

§D12.2 感知层集成——FAST-LIO2、TF 树与时间同步 ⭐⭐⭐¶

这是本章第一个"接缝重灾区"。 算法（FAST-LIO2）你在 SLAM 主线已经精读过，这里不重复其迭代卡尔曼滤波推导。本节的全部重点是**把它正确地接进系统**——TF 树怎么搭、时间戳怎么对齐、坐标系怎么统一。这三件事是初学者搭栈时耗时最多、最容易出错的地方。

模块功能与在系统中的位置¶

感知层吃**点云 + IMU**，吐**里程计（位姿）+ 去畸变点云**。它是整个栈的"定位与建图前端"——上面所有层（地图、探索、规划、控制）都依赖它提供的"我在哪、世界长什么样"。在路线 α 里，你可以用仿真的真值位姿直接替代这一层（跳过 SLAM 的复杂性，先把上层跑通）；路线 β/γ 必须用真正的 SLAM。

我们选 FAST-LIO2（hku-mars），理由在 D6/SLAM 主线已充分讨论：紧耦合迭代 EKF、ikd-Tree 增量建图、100 Hz 里程计、对快速运动鲁棒。它的 ROS2 移植（hku-mars/FAST_LIO 的 ROS2 分支或 Ericsii/FAST_LIO_ROS2）成熟可用。

本质洞察：在自主飞行栈里，SLAM 不是"建一张好看的地图"，而是"给控制器一个能跟的位姿"。地图的视觉质量是次要的，**里程计的频率（≥50 Hz）、低延迟（<20 ms）、不发散**才是命门。一个建图精美但里程计只有 10 Hz 的 SLAM，会让控制器拿着"过期 100 ms 的位姿"去控一架以 5 m/s 飞行的无人机——位置已经偏了 0.5 m。这是为什么无人机首选 FAST-LIO2（100 Hz）而非建图更精细但慢的方案。

接缝重点一：TF 树怎么搭¶

TF 树（Transform Tree）是 ROS2 里维护坐标系之间变换的机制。自主飞行栈的标准 TF 树是一条链：

map ──(SLAM 全局位姿)──> odom ──(里程计)──> base_link ──(固定外参)──> lidar
                                                            └──> camera
                                                            └──> imu

每一段变换的语义和"谁来发布"（这是初学者最容易搞混的）：

变换	语义	谁发布	更新频率	性质
`map → odom`	修正里程计累积漂移的全局校正	SLAM 的回环/全局优化	低（1-10 Hz）或不动	动态
`odom → base_link`	里程计给出的机体位姿（带漂移）	SLAM 里程计（FAST-LIO2）	高（100 Hz）	动态
`base_link → lidar`	雷达相对机体的安装外参	static_transform_publisher	一次性	静态
`base_link → imu`	IMU 相对机体的安装外参	static_transform_publisher	一次性	静态

本质洞察：map→odom→base_link 这个**两段式**设计不是冗余，而是把"漂移的连续位姿"和"离散的全局校正"分开。odom→base_link 必须**连续平滑**（控制器需要它，跳变会让控制抖动）；map→odom 可以**离散跳变**（回环时一下子校正几十厘米）。如果只用一段 map→base_link，回环校正的跳变会直接传到控制器，无人机会"抽搐"。这是 ROS2 导航栈 REP-105 规范的核心设计，FAST-LIO2 也遵循它。

FAST-LIO2 默认发布 odom→base_link（或 camera_init→body，取决于配置，需要重映射成标准名）。map→odom 在纯里程计（无回环）时可以用一个恒等变换的静态发布器顶上（路线 α/β），有回环模块时由回环节点发布（路线 γ）。

静态外参发布（base_link → lidar）用 launch 里的 static_transform_publisher：

# 在 launch 里发布 LiDAR 的安装外参（相对 base_link 正上方 0.1m，无旋转）
Node(
    package='tf2_ros', executable='static_transform_publisher',
    # 参数顺序：x y z  qx qy qz qw  parent_frame child_frame
    arguments=['0', '0', '0.1', '0', '0', '0', '1', 'base_link', 'lidar'],
),

这个外参**必须和仿真 SDF 里 LiDAR 的 <pose> 一致**（我们在 §D12.1 设的是 0 0 0.1）。实机上这个外参要通过标定得到（LiDAR-IMU 外参标定，如 LI-Init 或手动测量），外参错几厘米/几度，建图就会有系统性偏差。

接缝重点二：时间同步¶

时间同步是无人机栈最隐蔽、最致命的接缝。它的核心问题是：多个传感器的数据要在"同一时刻"对齐，但它们各有各的时钟。

三类时间不同步的来源（R6E 系统分类）：

仿真时间 vs 墙钟（§D12.1 已讲）：节点用墙钟打时间戳，但数据是仿真时钟的——use_sim_time:=true 解决。
传感器之间的时钟偏移：LiDAR 和 IMU 是不同硬件，时钟不同步。FAST-LIO2 内部假设 IMU 时间戳和点云时间戳在同一基准——实机上需要硬件时间同步（PTP/GPS PPS）或软件估计时间偏移。
飞控 vs 机载计算机时钟：PX4 的时间和 ROS2 的时间不一致。MAVROS 用 MAVLink 的 TIMESYNC 消息估计两者偏移；uXRCE-DDS 路线需要单独处理。

⚠️ 编程陷阱：用墙钟时间戳查询 TF 导致外推失败。错误做法：在节点里用 this->now()（墙钟）给点云打时间戳，或 TF 查询时用 tf2::TimePointZero 之外的当前时刻但数据是过去的。现象：报 Lookup would require extrapolation into the future（外推到未来），或里程计和点云对不齐导致建图重影。根本原因：TF 查询是按时间戳插值的。如果你用"现在"的时间戳查一个传感器"刚才"采集的数据对应的变换，而 TF 缓冲里还没有"现在"的变换，就会要求外推到未来——TF 默认不外推，于是失败。正确做法：永远用**数据自带的时间戳**（msg->header.stamp）去查 TF，不要用 this->now()。如果确实需要最新变换且能容忍小误差，用 tf2::TimePointZero 查"最新可用"的变换。所有节点统一 use_sim_time。

时间同步配置（MAVROS 路线，在 mavros 的 yaml 里）：

# mavros 配置：开启与飞控的时间同步
time:
  timesync_rate: 10.0      # TIMESYNC 消息频率（Hz），10 Hz 足够
  timesync_avg_alpha: 0.6  # 时钟偏移估计的指数平滑系数

timesync_rate 是 MAVROS 向飞控发 TIMESYNC 的频率，飞控回复后 MAVROS 估计两个时钟的偏移并补偿。设 0 会**关闭**时间同步（危险）。avg_alpha 是偏移估计的平滑——太小（如 0.1）对时钟抖动反应慢，太大（如 0.95）容易被单次异常带偏，0.6 是常用值。

配置详解：FAST-LIO2 的关键参数¶

FAST-LIO2 的 config（如 config/velodyne.yaml）里几个对自主飞行最关键的参数：

common:
    lid_topic: "/lidar/points"     # 必须和仿真桥接出的话题一致
    imu_topic: "/fmu/out/imu"      # IMU 话题（仿真从 PX4 来或单独桥接）
    time_sync_en: false            # LiDAR 和 IMU 已硬件同步时设 false
    time_offset_lidar_to_imu: 0.0  # LiDAR 相对 IMU 的时间偏移（秒）

preprocess:
    lidar_type: 2                  # 1=Livox, 2=Velodyne, 3=Ouster
    scan_line: 16                  # 线数，要和 SDF 里的 vertical samples 一致
    blind: 0.5                     # 盲区半径（米），剔除近距自身点

mapping:
    acc_cov: 0.1                   # 加速度计噪声协方差
    gyr_cov: 0.1                   # 陀螺仪噪声协方差
    b_acc_cov: 0.0001              # 加速度计偏置随机游走
    b_gyr_cov: 0.0001              # 陀螺仪偏置随机游走
    fov_degree: 360                # 雷达视场角
    det_range: 50.0                # 最大探测距离，和 SDF range.max 一致
    extrinsic_T: [0.0, 0.0, 0.1]   # LiDAR→IMU 平移外参（要和 TF 一致）
    extrinsic_R: [1,0,0, 0,1,0, 0,0,1]  # LiDAR→IMU 旋转外参

参数的教学深度：

参数	含义	依据与影响
`blind`	盲区滤波半径	剔除距雷达 <0.5m 的点（自身机臂/桨）。和 §D12.1 的 `range.min` 配合，双保险。设 0 → 自身点污染地图
`scan_line`	雷达线数	必须等于 SDF 的 vertical samples（16）。不匹配 → FAST-LIO2 点云解析错位，里程计立刻发散
`acc_cov`/`gyr_cov`	IMU 噪声协方差	决定滤波器对 IMU 的信任度。仿真无噪声可设小（更信 IMU）；真机要按 IMU datasheet 的 Allan 方差设，设太小会过信噪声 IMU 导致发散
`extrinsic_T/R`	LiDAR→IMU 外参	必须和 TF 树、SDF 三者一致。外参错 → 建图有系统性扭曲。实机靠标定
`det_range`	最大探测距离	和 SDF `range.max` 一致。设大于真实量程 → 远处噪声点进地图

⚠️ 配置陷阱：scan_line 与雷达实际线数不匹配。错误做法：用 16 线雷达但 config 里写 scan_line: 32（或反之）。现象：FAST-LIO2 启动后里程计立刻乱跳、发散，RViz 里轨迹乱飞。根本原因：FAST-LIO2 按 scan_line 把无序点云重组成按线排列的结构做特征提取。线数错了，点被分到错误的线，特征提取全乱，IEKF 的观测全错。正确做法：scan_line 严格等于雷达物理线数（仿真里等于 SDF vertical samples）。检查方法：ros2 topic echo /lidar/points 看点云的 ring 字段范围，或核对雷达型号 datasheet。

代码走读：感知层 launch 与里程计重映射¶

# perception.launch.py —— 拉起 FAST-LIO2 并统一 TF/话题命名
from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    use_sim_time = {'use_sim_time': True}   # 关键：全局仿真时间
    return LaunchDescription([
        # FAST-LIO2 里程计节点
        Node(
            package='fast_lio', executable='fastlio_mapping',
            parameters=[
                '/path/to/config/velodyne.yaml',
                use_sim_time],
            # 重映射：把 FAST-LIO2 默认输出名映射到系统标准名
            remappings=[
                ('/Odometry', '/odom'),         # 里程计话题统一为 /odom
                ('/cloud_registered', '/cloud_in_map'),  # 去畸变点云
            ],
            output='screen'),

        # LiDAR 静态外参（base_link → lidar）
        Node(
            package='tf2_ros', executable='static_transform_publisher',
            arguments=['0','0','0.1','0','0','0','1','base_link','lidar'],
            parameters=[use_sim_time]),

        # 纯里程计模式下，map→odom 用恒等变换顶上（路线 α/β）
        Node(
            package='tf2_ros', executable='static_transform_publisher',
            arguments=['0','0','0','0','0','0','1','map','odom'],
            parameters=[use_sim_time]),
    ])

关键设计点：

每个节点都设 use_sim_time: True——这是仿真里时间同步的总开关，漏一个节点都会出问题。
remappings 统一命名：FAST-LIO2 默认输出 /Odometry 和 /cloud_registered，我们重映射成全系统约定的 /odom 和 /cloud_in_map。命名约定的统一是系统集成的隐形基础设施——如果每个节点用自己的话题名，连接关系会变成一团乱麻。
map→odom 恒等顶替：纯里程计没有回环校正，但 TF 树需要这一段才完整。用一个恒等（identity）静态变换占位，等路线 γ 加了回环再换成动态的。

运行验证¶

ros2 launch your_pkg perception.launch.py
# 验证 TF 树完整：
ros2 run tf2_tools view_frames    # 生成 frames.pdf，应看到 map→odom→base_link→lidar 完整链
# 验证里程计：
ros2 topic hz /odom               # 应约 100 Hz（FAST-LIO2 高频里程计）
# 在 RViz 里：Fixed Frame 设 map，加 Odometry 显示 /odom，加 PointCloud2 显示 /cloud_in_map
# 手动让无人机飞一圈（commander takeoff + 手动给点），看里程计轨迹和点云地图是否合理

成功标志：view_frames 生成的 TF 树是完整的一条链（无断裂、无警告），/odom 以约 100 Hz 发布，RViz 里点云地图随飞行平滑增长且不重影、不漂移。如果 TF 树有断裂（某段缺失），就是接缝没接好——这是本节最常见的问题。

⚠️ 常见陷阱¶

🔧 配置陷阱：use_sim_time 只设了一部分节点。错误做法：给 FAST-LIO2 设了 use_sim_time:=true，但忘了给 static_transform_publisher 或下游的地图节点设。现象：TF 查询间歇性失败，建图忽好忽坏，RViz 数据闪烁。根本原因：设了 sim_time 的节点用仿真时钟打时间戳，没设的用墙钟。两套时钟混在同一个 TF 树里，查询时时间戳对不上。正确做法：**所有**节点统一设。最佳实践是在 launch 顶部定义一个 use_sim_time 变量传给每个节点，或用 SetParameter 全局设置。检查：ros2 param get /每个节点 use_sim_time 全部为 True。

⚠️ 编程陷阱：FAST-LIO2 的输出 frame 没对齐到标准 TF 命名。错误做法：直接用 FAST-LIO2 默认的 camera_init/body 作为坐标系名，不重映射。现象：下游地图/规划节点找不到 map/base_link，TF 查询抛 LookupException。根本原因：FAST-LIO2 历史上用 camera_init（全局）和 body（机体）命名，与 ROS 导航生态的 map/odom/base_link 不一致。正确做法：通过参数把 FAST-LIO2 的 frame 名改成标准名（新版有 map_frame/body_frame 参数），或加一个 TF 重发布节点把 camera_init→body 重映射为 odom→base_link。

💡 概念误区：以为里程计漂移可以靠提高 SLAM 精度根除。新手想法："里程计会漂，那我换个更准的 SLAM 不就不漂了？" 实际上：任何**纯里程计（无回环、无绝对参考）都会随时间累积漂移，这是数学必然——误差在积分中累加。FAST-LIO2 再准也只是漂得慢，不是不漂。正确理解：漂移的根治靠**全局校正——回环检测（把走过的地方认出来，闭合误差）或绝对定位（GPS/UWB/动捕）。这正是 map→odom 这段变换的意义：它承载全局校正。室内无 GPS 时，路线 γ 应加回环模块（如 FAST-LIO2 + Scan Context）或在已知锚点做重定位。

练习¶

[集成测试] 故意把 base_link→lidar 的静态外参 z 值从 0.1 改成 0.5（错 0.4m），手动飞一圈，观察 RViz 里地图发生什么变化。然后改回正确值。这道题让你**直观感受外参错误如何系统性地扭曲地图**。
[管线搭建] 路线 α 模式：不启动 FAST-LIO2，改用仿真真值位姿（PX4 的 /fmu/out/vehicle_local_position）发布 odom→base_link 变换。写一个小节点订阅真值位姿、发布 TF。验收：TF 树完整，下游地图能正常构建。这是路线 α "用真值替代 SLAM" 的具体实现。
[性能调优·跨章综合] 综合 D6（环境表示）和本节：测量 FAST-LIO2 的端到端延迟（从点云时间戳到 /odom 发布时刻）。用 ros2 topic delay /odom 或在代码里打时间差。如果延迟 >20 ms，分析瓶颈在哪（点云太密？ikd-Tree 重建太频繁？），并通过降采样（filter_size_surf）把延迟压到 <15 ms。说明为什么这个延迟对 5 m/s 飞行的位置精度至关重要（提示：\(v \times t_{delay}\)）。

§D12.3 地图层集成——ROG-Map / voxblox ⭐⭐¶

过渡：§D12.2 给了我们位姿和去畸变点云，但点云本身**不能直接拿来规划**——规划器需要的是"哪里能飞、哪里是障碍、离障碍多远"。地图层就是把点云"熟化"成可查询的空间表示。算法你在 D6 学过，本节讲怎么接进系统、怎么选、怎么配。

模块功能与在系统中的位置¶

地图层吃**里程计 + 点云**，吐两样供上层共用的东西：占据栅格（给探索层提前沿、给规划层做碰撞检测）和 ESDF（给轨迹层做梯度避障）。它是感知和规划之间的"翻译层"——把"看到了什么"翻译成"哪里能飞"。

回顾 D6 的核心结论（本章直接复用）：占据栅格用概率模型维护每个体素是占据/空闲/未知；ESDF 给出每点到最近障碍的带符号距离，其梯度指向远离障碍的方向。这两种表示回答不同问题，缺一不可——碰撞检测要占据栅格（离散查询），梯度优化要 ESDF（连续梯度）。

选型：ROG-Map vs voxblox vs OctoMap（动机 → 选型）¶

动机：选哪个地图库，取决于你的传感器（LiDAR 还是深度相机）、场景尺度（房间还是林地）、和是否需要 ESDF。这不是"哪个最好"，而是"哪个匹配你的配置"。

维度	OctoMap	voxblox	ROG-Map
核心结构	八叉树占据	TSDF → ESDF	滑窗均匀栅格占据 + 计数器膨胀
输出	占据栅格	TSDF + ESDF + 占据	占据 + 膨胀（可外接 ESDF）
最适传感器	通用	深度相机（窄 FOV、稠密）	LiDAR（广 FOV、远距）
场景尺度	中小	中	大场景（滑窗，内存恒定）
速度	中（八叉树查询有开销）	中	快（50 Hz 更新仅占 29.8% 帧时间）
ESDF	无（需外接）	原生	需外接
路线	α（最简基线）	β（深度相机）	γ（LiDAR 大场景）

本质洞察：ROG-Map 的"机器人中心滑窗"（robocentric sliding window）设计揭示了无人机地图的一个根本权衡——你不需要记住整个世界，只需要记住身边的世界。无人机飞行时只关心周围几十米内的障碍，把地图做成跟随机器人移动的固定大小窗口，内存占用恒定、更新只碰局部，于是能在大场景里以 50 Hz 实时更新。这和 SLAM 要建全局地图的目标不同——规划用的地图和定位用的地图，设计目标可以完全不一样。这是 R6B 的"不是 X 而是 Y"：地图层要的不是"完整的全局地图"，而是"身边的、能快速查询的局部地图"。

本节以 ROG-Map（路线 γ，LiDAR） 为主线讲集成，voxblox（路线 β）和 OctoMap（路线 α）的接法在配置差异处标注。

配置详解：ROG-Map 的关键参数¶

ROG-Map 是 header-only 风格的库，作为一个 ROS2 节点运行。关键配置（rog_map.yaml）：

rog_map:
    resolution: 0.15            # 体素分辨率（米），0.15m 是室内常用
    inflation_resolution: 0.15  # 膨胀栅格分辨率
    inflation_steps: 2          # 膨胀层数：障碍物向外扩 2 格（安全裕度）
    map_size_x: 40.0            # 滑窗 X 尺寸（米）
    map_size_y: 40.0            # 滑窗 Y 尺寸
    map_size_z: 5.0             # 滑窗 Z 尺寸（室内高度有限）
    point_filt_num: 2           # 点云降采样：每 2 个点取 1 个
    max_ray_length: 30.0        # 光线投射最大长度（米）
    odom_topic: "/odom"         # 来自 FAST-LIO2
    cloud_topic: "/cloud_in_map" # 去畸变点云
    frame_id: "map"             # 地图坐标系

每个参数的教学深度：

参数	含义	推荐值与依据	改了会怎样
`resolution`	体素边长	0.15m——室内障碍最小尺度的折中。门、墙、桌子在这个分辨率下可分辨	调到 0.05m：精度高但内存×27、更新慢；调到 0.3m：快但薄障碍（栏杆）漏检
`inflation_steps`	障碍膨胀层数	2 层（约 0.3m）——给无人机半径留安全裕度	设 0：规划器把无人机当质点，会贴墙飞甚至撞；设太大：通道被膨胀堵死，无法穿越窄门
`map_size_*`	滑窗尺寸	40×40×5m——覆盖局部规划范围	太小：规划器看不到稍远的障碍；太大：内存和更新开销上升，丢失滑窗的意义
`point_filt_num`	点云降采样	2——平衡精度和速度	设 1（不降采样）：精度高但更新慢；设 5：快但点稀疏，地图有洞
`max_ray_length`	光线投射长度	30m——和雷达有效量程匹配	设太长：远处噪声点触发错误的空闲清除；太短：远处障碍不更新

⚠️ 配置陷阱：inflation_steps 设为 0，规划把无人机当质点。错误做法：为了让无人机能穿过窄通道，把膨胀关掉（inflation_steps: 0）。现象：规划器生成的轨迹紧贴墙面甚至穿墙，实飞时无人机蹭墙或撞击。根本原因：膨胀的物理意义是"把无人机的体积转移到障碍物上"——膨胀后障碍物变大，规划器就能把无人机当质点处理。关掉膨胀，规划器以为无人机是个点，会规划出贴着障碍物表面的轨迹，但真实无人机有 0.25m 半径，必然碰撞。正确做法：膨胀层数设为 ceil(无人机半径 / resolution) 加 1 层余量。x500 半径约 0.25m，resolution 0.15m → 至少 2 层。如果窄门穿不过去，应该换更小的无人机或更窄分辨率，而不是关膨胀。

接缝重点：地图坐标系必须和里程计/TF 一致¶

地图层的接缝问题集中在**坐标系一致性**。ROG-Map 订阅 /odom（FAST-LIO2 在 map 系下的位姿）和 /cloud_in_map（去畸变点云），把点云按里程计位姿累积进地图。三个东西必须在同一坐标系基准：

/odom 的 header.frame_id 应该是 map（或 odom，取决于你的 TF 约定）。
/cloud_in_map 的点应该已经变换到 map 系（FAST-LIO2 的 cloud_registered 就是配准到全局的）。
ROG-Map 的 frame_id 参数要和上面一致。

如果点云是在 lidar 系而里程计在 map 系，ROG-Map 要么报 TF 错误，要么（更糟）默默地把 lidar 系的点当 map 系的点累积——地图会变成一团缠绕的乱麻。

⚠️ 编程陷阱：把传感器系点云当世界系点云累积。错误做法：直接把 FAST-LIO2 的原始 /cloud_in_body（机体系）喂给地图层，而不是 /cloud_registered（世界系）。现象：地图层累积出的点云全部叠在原点附近一团，完全不成形。根本原因：机体系点云的坐标是"相对无人机当前位置"的，不随无人机移动而变到全局位置。直接累积等于把所有时刻的点都按"无人机在原点"叠加。正确做法：用已配准到世界系的点云（FAST-LIO2 的 cloud_registered），或者让地图层自己用 TF 把机体系点云变换到世界系再累积。检查方法：RViz Fixed Frame 设 map，点云应随飞行铺开成环境形状，而非聚在原点。

代码走读：地图层 launch 与可视化¶

# mapping.launch.py —— ROG-Map 地图层
from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    use_sim_time = {'use_sim_time': True}
    return LaunchDescription([
        Node(
            package='rog_map', executable='rog_map_node',
            parameters=['/path/to/config/rog_map.yaml', use_sim_time],
            remappings=[
                ('~/odom', '/odom'),
                ('~/cloud', '/cloud_in_map'),
            ],
            output='screen'),
    ])

ROG-Map 节点会发布占据栅格的可视化话题（如 /rog_map/occ_inflate，膨胀后占据体素的 MarkerArray）。在 RViz 里加这个话题就能看到地图。

voxblox 路线（β）的差异：voxblox 节点订阅深度相机的点云，发布 voxblox_node/tsdf_pointcloud（TSDF）和 voxblox_node/esdf_pointcloud（ESDF 切片）。关键额外参数是 tsdf_voxel_size（体素大小）和 esdf_max_distance_m（ESDF 计算的最大距离，太大慢、太小梯度信息不足）。voxblox 原生输出 ESDF，是它相对 ROG-Map 在轨迹优化上的优势。

运行验证¶

ros2 launch your_pkg mapping.launch.py
# 手动飞一圈，在 RViz 里：
# - 加 MarkerArray 显示 /rog_map/occ_inflate，应看到障碍物被膨胀的占据体素
# - 验证地图随飞行正确增长，墙体/障碍位置和真实环境吻合
ros2 topic hz /rog_map/occ_inflate   # 地图更新频率

成功标志：手动飞一圈后，RViz 里的占据地图正确反映了环境（墙在墙的位置、障碍在障碍的位置），且地图随飞行平滑更新无错位。这是上层探索和规划能工作的前提。

⚠️ 常见陷阱¶

🔧 配置陷阱：resolution 和 inflation_resolution 不一致导致膨胀异常。错误做法：把 resolution 设 0.1，inflation_resolution 忘了改还是 0.15。现象：膨胀层在边界处出现锯齿或空洞，规划器在某些区域误判可通行性。根本原因：两套分辨率的栅格不对齐，膨胀计算在不同分辨率的栅格间映射时产生错位。正确做法：除非有明确理由（如为省内存让膨胀图更粗），否则让两者相等。改一个就同步改另一个——这是配置项耦合的典型例子。

💡 概念误区：以为地图越精细越好。新手想法："分辨率调到 0.02m，地图超精细，规划肯定更安全。" 实际上：分辨率减半，内存和计算量增加约 8 倍（3D）。0.02m 分辨率的 40×40×5m 滑窗会吃掉几个 GB 内存，更新跟不上 50 Hz，地图层成为瓶颈，规划反而拿到过期地图。正确理解：分辨率应匹配**任务需要分辨的最小障碍尺度**和**无人机的避障精度**。室内避桌椅、穿门，0.1-0.15m 足够。盲目提高分辨率是用计算资源换不需要的精度，是新手最常见的过度工程。

⚠️ 编程陷阱：地图层和规划层用了不同的地图实例。错误做法：探索节点自己建一个 OctoMap，规划节点又建一个 ROG-Map，两者独立。现象：探索选的目标点在规划器的地图里可能是障碍物，规划失败；或两个地图状态不一致导致行为矛盾。根本原因：同一个物理环境被两套地图各自维护，更新时机和内容不同步，产生不一致。正确做法：全系统共享一个地图实例。要么探索和规划都订阅同一个地图节点的输出，要么把地图做成一个库（如 ROG-Map 是 header-only）让探索和规划共享同一个对象。这是系统集成的重要原则——单一数据源（single source of truth）。

练习¶

[配置练习] 把 resolution 从 0.15 依次改为 0.3、0.1、0.05，每次手动飞同样一圈，记录：地图内存占用（ros2 topic bw）、更新频率、薄障碍（放一根细栏杆）的检出情况。画一张"分辨率 vs 内存 vs 检出率"的权衡表，找到你场景的最优分辨率。
[集成测试] 路线 β 切换：把 LiDAR 换成深度相机，地图层从 ROG-Map 换成 voxblox。对比两者建图效果：在一个开阔房间和一个狭窄走廊里，各自的优劣是什么？（提示：深度相机 FOV 窄、距离近，LiDAR 反之）
[管线搭建] 给地图层加一个 ESDF 输出（ROG-Map 外接一个 ESDF 模块，或直接用 voxblox 的 ESDF）。在 RViz 里可视化 ESDF 的水平切片（用颜色表示距离）。这一步为 §D12.5 轨迹层的梯度避障做准备——验收标志是能看到障碍物周围的距离场梯度。

§D12.4 探索决策层——前沿提取与视点选择 ⭐⭐⭐¶

过渡：§D12.3 给了我们占据地图和前沿信息，但地图不会自己告诉无人机"接下来去哪"。探索决策层就是回答这个问题的"大脑"：从地图里找出未探索的方向，选一个最值得去的目标，交给轨迹层。算法谱系你在 D7 学过（Yamauchi → FUEL → FALCON），本节讲怎么把最朴素的版本实现出来并接进系统。

模块功能与在系统中的位置¶

探索决策层吃**占据地图（含前沿），吐**下一个探索目标（位置 + yaw + 可能的中间路点）。它是系统从"被动建图"变成"主动探索"的关键——没有它，无人机只会跟着你手动给的点飞；有了它，无人机自己决定去哪、自己把未知变已知。

回顾 D7（本章复用）：前沿是已知自由空间与未知空间的边界，是探索的天然信号。yaw 是四旋翼"免费"的感知自由度——改变 yaw 不影响位置跟踪（微分平坦里 yaw 是独立的平坦输出），却能改变相机/雷达的视野朝向，让无人机"边飞边转头看"。

从 Yamauchi 到 FUEL：本节实现哪个（动机 → 选型）¶

动机：探索决策的核心是两个子问题——"哪里是前沿"（提取）和"去哪个前沿"（选择）。不同算法在这两步上复杂度不同。

方法	前沿提取	视点选择	复杂度	探索效率	路线
Yamauchi（1997）	占据栅格扫描找边界体素	选最近的前沿（贪婪）	低	中（会走回头路）	α
nbvplanner	RRT 采样视点	信息增益最大的视点	中	中高	β
FUEL（2021）	前沿聚类 + 视点生成	ATSP 全局 + 局部精修	高	高（全局规划少走回头路）	γ

本质洞察：从 Yamauchi 到 FUEL 的进化，本质是**从"贪婪"到"全局"。Yamauchi 每次只看"最近的前沿"，像一个近视的人走一步看一步，结果是走很多回头路。FUEL 把所有前沿聚成簇，求解一个"访问所有簇的最短路径"（ATSP，非对称旅行商问题），像一个有全局视野的人提前规划好整条路线。这是 R6B 的对比——**贪婪选择在局部最优，全局规划在整体最优；代价是后者要解 NP-hard 的 TSP（但在线场景接受启发式近似解）。本节先实现 Yamauchi（路线 α，理解骨架），再讲 FUEL 的简化接法（路线 γ）。

代码走读：自实现 Yamauchi 前沿探索¶

下面是一个最小化 Yamauchi 前沿探索节点的核心逻辑（C++，伪框架，省略 ROS2 样板）。这是路线 α 的探索层。

// frontier_explorer.cpp —— 最简 Yamauchi 前沿探索
// 输入：占据地图  输出：下一个探索目标（geometry_msgs/PoseStamped）

#include <queue>
#include <Eigen/Core>

class FrontierExplorer {
public:
    // 主循环：每次被调用返回下一个探索目标
    bool selectNextGoal(const OccupancyMap& map,
                        const Eigen::Vector3d& cur_pos,
                        Eigen::Vector3d& goal, double& goal_yaw) {
        // 步骤 1：提取所有前沿体素
        std::vector<Eigen::Vector3d> frontiers = extractFrontiers(map);
        if (frontiers.empty()) return false;   // 没有前沿 = 探索完成

        // 步骤 2：贪婪选择——最近的前沿（Yamauchi 核心）
        double min_dist = 1e9;
        Eigen::Vector3d best;
        for (const auto& f : frontiers) {
            double d = (f - cur_pos).norm();
            if (d > min_reach_dist_ && d < min_dist) {  // 太近的跳过（已在视野内）
                min_dist = d;  best = f;
            }
        }
        goal = best;
        // 步骤 3：yaw 朝向前沿方向（让传感器看向未知区域）
        Eigen::Vector3d dir = best - cur_pos;
        goal_yaw = std::atan2(dir.y(), dir.x());
        return true;
    }

private:
    // 前沿提取：遍历地图，找"自由体素且邻居中有未知体素"的体素
    std::vector<Eigen::Vector3d> extractFrontiers(const OccupancyMap& map) {
        std::vector<Eigen::Vector3d> frontiers;
        for (const auto& voxel : map.freeVoxels()) {       // 只看自由空间
            for (const auto& nb : map.neighbors(voxel)) {  // 检查 6/26 邻域
                if (map.isUnknown(nb)) {                    // 邻居是未知
                    frontiers.push_back(map.voxelCenter(voxel));
                    break;  // 这个体素已确认是前沿，看下一个
                }
            }
        }
        return frontiers;
    }
    double min_reach_dist_ = 1.0;   // 小于此距离的前沿视为"已看到"，跳过
};

逐段解释（这是本节的核心教学）：

前沿提取（extractFrontiers）：前沿的定义直接落地为代码——遍历所有**自由**体素，检查其邻域，只要有一个邻居是**未知**体素，这个自由体素就是前沿。这正是 D7 里"已知自由与未知的边界"的代码实现。邻域用 6 邻接（面相邻）还是 26 邻接（含对角）影响前沿的连续性，6 邻接更严格。
贪婪选择（步骤 2）：Yamauchi 的全部智慧就一句——选最近的前沿。min_reach_dist_ 的过滤很关键：太近的前沿往往是当前视野边缘的噪声，去了也获取不到多少新信息，跳过它们避免无人机"原地抽搐"。
yaw 规划（步骤 3）：让 yaw 指向"从当前位置看向目标前沿"的方向。这样无人机飞过去的过程中，传感器正好朝着未知区域，边飞边探。这是利用 yaw 这个免费感知自由度的最简方式。

⚠️ 编程陷阱：前沿提取没过滤孤立噪声体素。错误做法：把所有满足"自由且邻居未知"的体素都当有效前沿，不做聚类或最小尺寸过滤。现象：无人机频繁被一两个噪声体素（地图边缘的伪前沿）吸引，飞过去发现没东西，来回横跳，探索效率极低。根本原因：传感器噪声和地图边界会产生大量孤立的"伪前沿"（一两个体素的小簇），它们不代表真正的未探索区域。正确做法：对前沿做**聚类**（如欧氏聚类），过滤掉体素数小于阈值（如 10）的小簇，只把足够大的前沿簇作为候选目标。FUEL 正是这么做的——前沿聚类是从 Yamauchi 走向实用的第一步。

接缝重点：探索层与规划层的握手¶

探索层选了目标点，但**这个目标点不一定可达**——它可能在障碍物后面，或者轨迹层算不出无碰撞路径。所以探索层和规划层之间需要"握手"：

探索层给一个候选目标。
规划层尝试生成到该目标的轨迹。
如果失败（无可行轨迹），探索层换下一个候选目标（次近的前沿）。
重试直到成功或候选耗尽。

这个握手逻辑通常放在 FSM 里（§D12.7），但探索层要提供"给我下一个候选"的接口（如返回按距离排序的前沿列表，而非只返回最近一个）。

本质洞察：探索决策**不能假设自己选的目标一定可达**。地图是不完整的（正在探索中），探索层基于不完整信息做决策，规划层基于（同样不完整的）地图验证可达性。两者必须通过"提议-验证-回退"的握手来协作，而不是探索层单方面下命令。这是 R6B 的反事实：如果探索层假设目标一定可达、不留回退路径，遇到不可达目标时整个系统就卡死（FSM 死锁的常见来源，见 §D12.7）。

FUEL 简化版的接法（路线 γ）¶

路线 γ 用 FUEL 的简化版替代 Yamauchi。FUEL（ZJU FAST Lab）的核心是三步：

前沿聚类：用增量式的前沿信息结构（FIS）维护前沿簇，避免每帧全图重算。
视点生成：每个前沿簇生成若干候选视点（位置+yaw），评估每个视点能覆盖多少前沿。
全局 ATSP + 局部精修：求解访问所有前沿簇的最短路径（ATSP），取第一段作为当前目标；局部用 B 样条精修。

简化接法：直接用 FUEL 开源实现的 exploration_manager，订阅你的地图，输出目标点。集成的关键是把你的地图格式对接到 FUEL 期望的 SDFMap 接口——这是路线 γ 探索层的主要工程量。

运行验证¶

ros2 launch your_pkg exploration.launch.py
# 探索层会发布目标点话题（如 /exploration/goal）
ros2 topic echo /exploration/goal   # 看是否输出合理的目标位置
# 在 RViz 里加 Marker 显示前沿（探索层应可视化前沿体素）和目标点

成功标志：无人机起飞后，探索层自动输出一系列目标点，每个都指向未探索区域，且随着探索进行，目标逐渐覆盖整个环境。前沿在 RViz 里可见且随建图收缩。

⚠️ 常见陷阱¶

🔧 配置陷阱：探索目标离障碍太近无法生成安全轨迹。错误做法：直接把前沿体素中心当目标点，不考虑该点到障碍的距离。现象：探索层选的目标紧贴墙面，轨迹层因安全约束无法到达，探索停滞。根本原因：前沿在已知/未知边界，常常紧邻障碍物。目标点落在膨胀区域内，规划器找不到无碰撞轨迹。正确做法：探索目标应从前沿"后退"一个安全距离（沿前沿法向往自由空间方向退），或选前沿附近 ESDF 值足够大的点作为实际目标。

💡 概念误区：以为探索就是"覆盖所有空间"。新手想法："探索的目标是飞遍每一个角落，覆盖率 100%。" 实际上：100% 覆盖往往不现实也不必要——有些区域（如桌子底下、极窄缝隙）无人机物理上进不去，强行追求会让无人机卡在某个去不了的前沿上耗尽电池。正确理解：探索的目标是**在资源（时间/电池）约束下最大化信息获取**。实用系统设"覆盖率阈值"（如 90%）和"前沿最小尺寸"，当剩余前沿都太小或不可达时，判定探索完成、返航。这是 §D12.7 FSM 里 EXPLORING→RETURN_HOME 转换条件的依据。

⚠️ 编程陷阱：探索决策频率过高导致目标频繁切换。错误做法：每收到一帧地图更新（50 Hz）就重新选目标。现象：目标点高频抖动，无人机刚朝一个目标加速又被换到另一个，轨迹支离破碎，飞行效率极低。根本原因：地图每帧都在微小变化，最近前沿也随之频繁变动。如果每帧都重选，目标永远在跳。正确做法：探索决策频率应**远低于**地图更新频率（如 1-2 Hz），或加"目标锁定"逻辑——只有当前目标已到达/不可达时才重选。目标的稳定性比新鲜度更重要。

练习¶

[管线搭建] 给 Yamauchi 前沿提取加聚类过滤（欧氏聚类 + 最小簇尺寸阈值）。对比加之前和之后：无人机"横跳"现象是否消失？探索效率（单位时间覆盖增量）提升多少？
[配置练习] 调整 min_reach_dist_ 从 0.5 到 2.0，观察对探索行为的影响。太小会怎样（被近处噪声吸引）？太大会怎样（忽略近处真前沿、漏探）？找到你场景的合适值。
[集成测试·跨章综合] 综合 D7（探索）+ D1（微分平坦的 yaw）：实现"yaw 前瞻"——不仅让 yaw 指向当前目标，还在轨迹执行中让 yaw 提前转向**下一个**目标方向。对比固定 yaw、指向当前目标 yaw、前瞻 yaw 三种策略的探索效率。解释为什么 yaw 规划能提升探索速度（提示：提前看到的区域不用再飞过去）。

§D12.5 轨迹层集成——MINCO / B 样条与重规划 ⭐⭐⭐¶

过渡：§D12.4 给了我们一个目标点，但目标点不能直接喂给控制器——控制器要的是一条**时间参数化的、平滑的、无碰撞的轨迹** \(p(t), v(t), a(t)\)。轨迹层就是把"去哪"（目标点）变成"怎么去"（轨迹）。算法你在 D3（多项式）、D4（B 样条）、D5（MINCO）学过，本节讲怎么接进系统，重点是**重规划的触发逻辑**——这是静态轨迹生成和在线自主飞行的本质区别。

模块功能与在系统中的位置¶

轨迹层吃**当前状态（起点）+ 目标点 + 地图（障碍/安全走廊），吐**一条分段轨迹（位置+速度+加速度+jerk+yaw，按时间参数化）。它是规划和控制之间的桥——把离散的"目标"变成连续的"参考信号"。

本质洞察：自主飞行的轨迹层和你在 D3 学的"给一串航点生成 min-snap 轨迹"有一个根本区别——它必须反复重规划。静态场景下生成一次轨迹就完事；自主探索中，环境在被实时建图（地图不断更新）、目标在变（探索到新区域）、可能突然出现障碍。所以轨迹层不是"算一条轨迹"，而是"以一定频率持续算新轨迹，每次都基于最新地图和当前状态"。重规划频率（5-10 Hz）直接决定系统对环境变化的反应速度——这是 R6B 的"不是 X 而是 Y"：轨迹层要的不是"一条完美轨迹"，而是"持续产出的、够用的轨迹流"。

选型：MINCO（GCOPTER）vs B 样条（EGO-Planner）（动机 → 选型）¶

维度	min-snap（D3）	EGO-Planner B 样条（D4）	GCOPTER MINCO（D5）
决策变量	多项式系数	B 样条控制点	航点 + 段时间
安全约束	走廊（硬约束 QP）	ESDF 梯度（软约束推离）	走廊（软约束 + MINCO 参数化）
时间优化	固定/启发式	固定	时空联合
计算速度	快（闭式）	快（局部支撑）	极快（带状系统 + L-BFGS）
重规划友好	中	高（增量重规划）	高
路线	α	β	γ

EGO-Planner（路线 β）的工程优势：它不需要显式构建 ESDF（用环境梯度的局部估计），重规划是增量式的（只在碰撞处局部修正），在 Jetson 这类机载计算机上验证过实时性。根据公开部署案例，EGO-Planner 在 NVIDIA Jetson Orin NX + Holybro Kakute H7 飞控的组合上跑通过自主导航，MAVROS 做机载计算机和飞控的通信桥。

GCOPTER/MINCO（路线 γ）的优势：时空联合优化，能在安全走廊里生成时间最优的轨迹，配合 ROG-Map 的走廊生成，是性能上限最高的方案。

接缝重点：重规划的触发逻辑¶

重规划"什么时候触发"是轨迹层最核心的工程决策。三类触发条件（R6E 系统分类）：

触发类型	条件	频率	目的
周期触发	固定时间间隔（如每 0.1s）	5-10 Hz	跟上地图更新和状态变化
事件触发	检测到当前轨迹即将碰撞	异步	应对新出现的障碍（紧急）
目标触发	探索层给了新目标	异步	朝新目标规划

⚠️ 编程陷阱：重规划时把当前实际状态当起点 vs 把轨迹上的预测状态当起点。错误做法：重规划时直接用 SLAM 给的当前位置/速度作为新轨迹的起点。现象：每次重规划，新旧轨迹在接缝处不连续，无人机出现周期性的速度/加速度跳变（"一顿一顿"地飞）。根本原因：从"测量当前状态"到"新轨迹算完并下发"有几十毫秒延迟。如果用重规划时刻的状态当起点，等新轨迹生效时无人机已经飞到别处了，新轨迹起点和实际位置对不上。正确做法：重规划的起点应该用**旧轨迹在"预计新轨迹生效时刻"的预测状态**（即 \(t_{now} + t_{plan}\) 时刻旧轨迹的 \(p,v,a\)），而不是当前测量状态。这样新轨迹从旧轨迹"接力"，平滑衔接。这是在线重规划的关键技巧，EGO-Planner 和 GCOPTER 都这么做。

配置详解：轨迹层关键参数（以 EGO-Planner 为例）¶

# ego_planner 关键参数
manager:
    max_vel: 3.0              # 最大速度（m/s），路线 β 用 3，γ 可到 10+
    max_acc: 6.0             # 最大加速度（m/s²）
    planning_horizon: 7.5    # 规划范围（米）——只规划前方一段，不规划到终点
    control_points_distance: 0.4  # B 样条控制点间距
optimization:
    lambda_smooth: 1.0       # 平滑项权重
    lambda_collision: 0.5    # 碰撞项权重（推离障碍）
    lambda_feasibility: 0.1  # 动力学可行性项（速度/加速度限制）
    dist0: 0.5              # 期望与障碍的安全距离（米）

参数教学深度：

参数	含义	依据与影响
`max_vel`/`max_acc`	速度/加速度上限	必须 ≤ 无人机物理极限（由推重比决定，见 D8）。设超过物理极限 → 控制器跟不上、轨迹发散。路线越激进设越大，但要先确认平台能力
`planning_horizon`	局部规划距离	7.5m——只规划"看得见"的前方一段。设太大：超出地图已知范围，规划进未知区有风险；太小：反应迟钝，看不远
`dist0`	期望安全距离	0.5m——轨迹希望离障碍多远。和地图膨胀配合：膨胀保证不碰，dist0 保证"舒适余量"
`lambda_collision`	碰撞项权重	决定避障的"激进程度"。太大：轨迹过度绕远；太小：贴障碍飞，余量不足

⚠️ 配置陷阱：max_vel/max_acc 超过平台物理极限。错误做法：为了飞得快，把 max_vel 设到 15 m/s，但平台推重比只有 2（最大加速度约 \(g\)）。现象：控制器无法跟踪激进轨迹，跟踪误差越来越大，最终发散炸机。根本原因：轨迹层生成的轨迹要求的加速度超过了推力能提供的极限（\(a_{req} > (\text{TWR}-1)g\)），物理上无法实现。正确做法：max_acc 设为 \((\text{TWR}-1)g\) 的 70-80%（留裕度）。先在 D8 测出平台真实推重比，再据此设轨迹约束。轨迹层的约束必须**反映而非超越**平台能力。

接缝重点：安全走廊的生成与对接（路线 γ MINCO）¶

MINCO/GCOPTER 需要**安全走廊**（一串凸多面体）作为输入。走廊从占据地图生成：沿着一条初始路径（如 A* 给的几何路径），在每个路径点周围"膨胀"出一个最大的无障碍凸多面体（如用 RILS 或 FIRI 算法）。这些多面体的并集就是安全飞行空间，MINCO 在其中优化轨迹。

接缝在于：走廊生成器要订阅地图，初始路径要由一个前端（kinodynamic A*）提供。所以路线 γ 的轨迹层其实是"前端路径搜索 + 走廊生成 + MINCO 后端优化"三件套，比 EGO-Planner 多了走廊这一步。这是 D5 的内容，本节负责把它们串起来。

代码走读：轨迹层的重规划主循环（伪框架）¶

// trajectory_server.cpp —— 轨迹层重规划主循环
class TrajectoryServer {
    void replanTimerCallback() {   // 周期触发，10 Hz
        // 1) 计算新轨迹的起点：旧轨迹在 (now + plan_delay) 时刻的预测状态
        double t_start = now() + estimated_plan_time_;
        State start = current_traj_.valid()
            ? current_traj_.sample(t_start)   // 从旧轨迹接力
            : getCurrentStateFromOdom();       // 首次规划用实测状态

        // 2) 检查当前轨迹是否仍然安全（事件触发的避障）
        if (current_traj_.valid() && isTrajInCollision(current_traj_, map_)) {
            RCLCPP_WARN(get_logger(), "Current traj will collide, urgent replan!");
            // 紧急重规划，可能需要先减速/悬停
        }

        // 3) 调用规划器生成新轨迹
        Trajectory new_traj;
        bool success = planner_->plan(start, goal_, map_, new_traj);

        // 4) 处理规划结果
        if (success) {
            current_traj_ = new_traj;           // 切换到新轨迹
            publishTrajectory(new_traj);        // 发给控制层
        } else {
            replan_fail_count_++;               // 失败计数
            if (replan_fail_count_ > 3) {
                triggerHover();                 // 连续失败 → 悬停（交给 FSM 处理）
            }
        }
    }
};

关键设计点逐条：

起点接力（步骤 1）：如上文陷阱所述，用 now() + plan_delay 时刻旧轨迹的预测状态当起点，保证新旧轨迹平滑衔接。estimated_plan_time_ 是规划耗时的估计（如 5-20 ms）。
碰撞自检（步骤 2）：每个周期检查当前轨迹在最新地图里是否还安全。新障碍出现时，这里会触发紧急重规划。这是"事件触发"和"周期触发"的融合。
规划失败处理（步骤 4）：规划不一定成功（目标不可达、走廊太窄）。连续失败超阈值就触发悬停，把控制权交给 FSM——轨迹层不自己处理异常，只上报状态，异常决策归 FSM（§D12.7）。这是模块职责清晰的体现。

本质洞察：轨迹层的健壮性，不在于它总能规划成功，而在于它规划失败时的行为是安全的。一个会偶尔失败但失败时干净地报告"我失败了"的轨迹层，远好于一个假装总能成功、失败时吐出垃圾轨迹的轨迹层。这呼应了 §D12.4 的握手原则——每一层都要诚实地暴露自己的失败，让上层（FSM）有机会兜底。这是分层系统鲁棒性的根基。

运行验证¶

ros2 launch your_pkg trajectory.launch.py
# 给一个目标点（手动 publish 或让探索层给），观察：
ros2 topic echo /trajectory/poly_traj   # 轨迹话题
# RViz 里加 Path/Marker 显示规划出的轨迹，应是一条平滑曲线，避开所有障碍
# 给一个障碍后面的目标，验证轨迹绕开障碍

成功标志：给定目标点，轨迹层生成平滑、无碰撞、满足速度/加速度约束的轨迹，且在 RViz 里可见。给障碍后的目标时，轨迹正确绕行。重规划频率达到设定值（5-10 Hz）。

⚠️ 常见陷阱¶

🔧 配置陷阱：planning_horizon 超出地图已知范围。错误做法：把规划范围设得很大（如 20m），但地图滑窗只有 10m。现象：规划器试图在未知区域规划，要么把未知当自由空间撞上去，要么规划失败。根本原因：规划只能在"已知"的地图里安全进行。规划范围超过地图覆盖，就在未知区域瞎规划。正确做法：planning_horizon ≤ 地图滑窗尺寸的一半左右，确保规划范围完全在已知地图内。未知区域应该靠探索逐步揭开，而不是靠规划"赌"。

⚠️ 编程陷阱：轨迹时间戳和控制层不同步。错误做法：轨迹用规划完成的墙钟时刻作为 \(t=0\)，控制层用自己的时钟采样轨迹。现象：控制层采样到的轨迹点和无人机实际应该在的位置有时间偏移，跟踪有系统性滞后或超前。根本原因：轨迹是时间参数化的，采样时刻必须和轨迹的时间基准一致。两层时钟不同步，采样的 \(t\) 就错了。正确做法：轨迹消息里带明确的"起始时间戳"（start_time），控制层用 (now - start_time) 作为采样时刻 \(t\)。全系统统一 use_sim_time。这又一次说明时间同步贯穿所有接缝。

💡 概念误区：以为轨迹生成成功就等于能飞。新手想法："规划器吐出了一条平滑无碰撞轨迹，那无人机肯定能跟着飞。" 实际上：轨迹"几何上无碰撞"不等于"动力学上可跟踪"。如果轨迹要求的加速度超过推力极限、或角速度变化太快超过电机响应，控制器根本跟不上，无人机会偏离轨迹甚至失控。正确理解：轨迹必须同时满足**几何约束**（无碰撞）和**动力学约束**（速度/加速度/jerk 在平台能力内）。EGO-Planner 的 lambda_feasibility 项、MINCO 的动力学约束就是干这个的。验收轨迹时既要看它避不避障，也要看它的峰值加速度/角速度是否在平台极限内。

练习¶

[集成测试] 实现"起点接力"重规划：对比"用实测状态当起点"和"用旧轨迹预测状态当起点"两种方式，在连续重规划下无人机的飞行平滑度（画速度/加速度曲线，看接缝处是否有跳变）。
[配置练习] 把 max_vel 从 1 逐步提到 5 m/s，记录每个速度下的实际跟踪误差（RMS）。找到"跟踪误差开始明显增大"的速度——这就是你当前平台+控制器的速度上限。解释为什么超过这个速度误差会爆炸（提示：延迟、动力学极限）。
[管线搭建·跨章综合] 综合 D5（MINCO）+ D6（ROG-Map 走廊）：路线 γ 的完整轨迹层——前端 A* 给路径 → ROG-Map 生成安全走廊 → GCOPTER MINCO 优化。验证：在一个有多个障碍的场景里，能生成时间最优的无碰撞轨迹。对比和 EGO-Planner 的轨迹质量（时间、平滑度）差异。

§D12.6 控制层与坐标系翻转——把几何控制接到 PX4 ⭐⭐⭐⭐¶

这是本章第二个、也是最凶险的"接缝重灾区"。 几何控制算法你在 D1 推导过（Lee 的 SE(3) 控制器、微分平坦逆映射），本节**不重复推导**。本节的全部内容是把它**正确地接到 PX4**——这里有两个能让无人机直接炸机的接缝：NED↔ENU/FRD↔FLU 坐标系翻转**和 **Offboard 模式心跳。这两个坑每个无人机工程师都踩过，本节让你提前踩完。

模块功能与在系统中的位置¶

控制层吃**轨迹参考**（\(p, v, a, j, \psi, \dot\psi\)）和**当前状态**（位姿、速度），吐**底层控制指令**（集体推力 + 体轴角速度，或姿态四元数 + 推力），通过 MAVLink/uORB 发给 PX4。它是软件栈和飞行硬件之间的最后一道关——这里出错，前面所有层做对了也白搭。

回顾 D1（本章逐行复用）：四旋翼微分平坦，给定平坦输出及其导数，可代数恢复推力和角速度。Lee 几何控制器在 \(SO(3)\) 上定义姿态误差 \(e_R = \frac{1}{2}(R_d^\top R - R^\top R_d)^\vee\)，避免欧拉角奇异。集体推力 \(f = m(\ddot{p}_{des} + g e_3 - K_p e_p - K_v e_v) \cdot R e_3\)。本节把这些公式接到 PX4 的接口上。

接缝重点一：NED ↔ ENU 与 FRD ↔ FLU 坐标系翻转¶

这是整个无人机栈**最容易炸机**的接缝。问题的根源：PX4 和 ROS 用不同的坐标系约定。

系统	世界坐标系	机体坐标系
PX4（航空惯例）	NED（North-East-Down，北-东-地）	FRD（Forward-Right-Down，前-右-下）
ROS（REP-103）	ENU（East-North-Up，东-北-天）	FLU（Forward-Left-Up，前-左-上）

你的几何控制器（按 D1）通常在 ROS 的 ENU/FLU 里算，但 PX4 要 NED/FRD。直接把 ENU 的指令发给 PX4，无人机会朝着错误的方向猛冲——比如你想往北飞（ENU 的 +Y），PX4 把它解释成往东（NED 的 +Y 是东），方向偏 90°；更糟的是 Z 轴，ENU 的 +Z 朝上，NED 的 +Z 朝下，期望上升会变成期望下降，无人机直接拍地。

ENU↔NED 的变换是一个固定的坐标轴重排（不是简单取负）：

\[ \begin{pmatrix} x_{NED} \\ y_{NED} \\ z_{NED} \end{pmatrix} = \begin{pmatrix} 0 & 1 & 0 \\ 1 & 0 & 0 \\ 0 & 0 & -1 \end{pmatrix} \begin{pmatrix} x_{ENU} \\ y_{ENU} \\ z_{ENU} \end{pmatrix} \]

即：NED 的北 = ENU 的北（ENU 的 y），NED 的东 = ENU 的东（ENU 的 x），NED 的地 = ENU 的天取负。这个变换矩阵是它自己的逆（对合，involutory），所以 NED→ENU 用同一个矩阵。

本质洞察：NED↔ENU 不是"把某个轴取负"那么简单，而是一次"交换 X/Y + Z 取负"的重排。新手最常见的错误是只把 Z 取负（以为只有上下反了），结果 X/Y 还是错的，无人机往垂直于期望的方向飞。理解这个变换是"轴重排"而非"符号翻转"是避免这个坑的关键。这也是 R6B 的"不是 X 而是 Y"：坐标系转换不是改符号，而是换基。

⚠️ 编程陷阱（炸机级）：把 ENU 指令直接发给 PX4 的 NED 接口。错误做法：几何控制器在 ENU 里算出期望位置/姿态，不做转换直接填进 PX4 的 setpoint 消息。现象：无人机一切 Offboard 就朝意外方向猛冲，常见是侧飞或直接向下撞地。根本原因：PX4 把收到的数据按 NED/FRD 解释，但数据是 ENU/FLU 的。X/Y 被交换、Z 被反向，期望运动方向完全错误。正确做法：在控制器输出和 PX4 接口之间加一个明确的坐标系转换层。位置/速度/加速度用上面的 ENU→NED 矩阵转换；姿态四元数要做对应的旋转复合（q_ned = q_enu_to_ned * q_enu * q_flu_to_frd）。如果用 MAVROS，它**已经帮你做了**这个转换（MAVROS 的话题都是 ENU/FLU），这是路线 α 用 MAVROS 的一大便利；如果用 uXRCE-DDS 直发 PX4 uORB，你**必须自己转换**。

这就是为什么版本兼容表里 uXRCE-DDS 那行备注"需自己翻转到 ENU"，而 MAVROS 那行"已做翻转"——这个区别直接决定了你要不要自己写坐标转换代码，是选通信路线时最实际的考量。

接缝重点二：Offboard 模式心跳¶

PX4 的 Offboard 模式有一个**致命的安全机制**：它要求外部控制器**持续证明自己活着**。具体规则（来自 PX4 官方文档）：

切入 Offboard 模式**之前**，必须已经以 >2 Hz 的频率发送 OffboardControlMode 消息**至少 1 秒**。
Offboard 运行中，如果 OffboardControlMode 消息流频率**掉到 2 Hz 以下**，PX4 在约 0.5 秒后**自动退出 Offboard**，触发 failsafe（通常是悬停或降落）。

本质洞察：Offboard 心跳的设计哲学是"默认不信任外部控制器"。PX4 是飞控，它对机载计算机发来的指令保持警惕——万一机载计算机崩溃、ROS 节点卡死、网络断了，PX4 不能傻等着无人机失控，必须能"夺回控制权"触发 failsafe。2 Hz 心跳就是这个"你还活着吗"的探测信号。理解这一点，你就知道为什么控制线程的实时性如此关键——不是为了控制精度，而是为了不被 PX4 判定为"死亡"而夺权。

⚠️ 编程陷阱（炸机级）：控制线程偶发卡顿导致 Offboard 心跳丢失。错误做法：把控制计算、日志写入、规划查询都放在同一个线程，控制频率名义上 100 Hz 但偶尔因为日志 IO 卡顿 0.6 秒。现象：无人机飞着飞着突然"夺权"——切出 Offboard、自己悬停或降落，且没有明显报错。根本原因：控制线程偶尔卡顿超过 0.5 秒，OffboardControlMode 消息流中断，PX4 判定外部控制器失效，自动退出 Offboard。正确做法：(1) OffboardControlMode 心跳用**独立的高优先级定时器**发送（如 50 Hz），和重计算解耦，保证即使主控制卡顿心跳也不断；(2) 控制热路径**零阻塞**——不在控制线程里做日志 IO、不做堆分配、不等锁；(3) 监控控制频率，掉频时报警。这正是导读"从软实时 10 Hz 到硬实时 1 ms"认知跨越的实战体现。

配置详解：Offboard 控制的发送频率¶

消息	推荐频率	最低频率	说明
OffboardControlMode（心跳）	50 Hz	>2 Hz	必须独立线程发，掉到 2 Hz 以下被夺权
TrajectorySetpoint（位置/速度参考）	50-100 Hz	与心跳同步	实际控制指令
体轴角速度 + 推力（高频控制）	200-400 Hz	100 Hz	路线 γ 直发 rate setpoint

代码走读：Offboard 控制节点（uXRCE-DDS 路线）¶

下面是接到 PX4 的 Offboard 控制节点核心，重点展示心跳和坐标转换两个接缝（伪框架，省略部分样板）：

// offboard_control.cpp —— 几何控制接 PX4（uXRCE-DDS）
class OffboardControl : public rclcpp::Node {
public:
    OffboardControl() : Node("offboard_control") {
        // PX4 uORB 话题发布器
        offboard_mode_pub_ = create_publisher<OffboardControlMode>(
            "/fmu/in/offboard_control_mode", 10);
        setpoint_pub_ = create_publisher<TrajectorySetpoint>(
            "/fmu/in/trajectory_setpoint", 10);

        // ★关键：心跳用独立的高频定时器（50 Hz），和控制解耦
        heartbeat_timer_ = create_wall_timer(20ms,
            std::bind(&OffboardControl::publishHeartbeat, this));
        // 控制主循环（100 Hz）
        control_timer_ = create_wall_timer(10ms,
            std::bind(&OffboardControl::controlLoop, this));
    }

private:
    // 心跳：声明本周期控制的是哪种 setpoint。必须持续发！
    void publishHeartbeat() {
        OffboardControlMode msg{};
        msg.position = true;        // 本例用位置控制
        msg.velocity = false;
        msg.acceleration = false;
        msg.attitude = false;
        msg.body_rate = false;
        msg.timestamp = get_clock()->now().nanoseconds() / 1000;  // 微秒
        offboard_mode_pub_->publish(msg);   // 即使控制卡顿，心跳也不停
    }

    // 控制主循环：采样轨迹 → 几何控制 → 坐标转换 → 发 PX4
    void controlLoop() {
        if (!traj_.valid()) return;
        // 1) 采样轨迹参考（ENU 系，来自 D1 微分平坦）
        double t = (now() - traj_.start_time()).seconds();
        State ref = traj_.sample(t);   // ref.pos/vel/acc 在 ENU

        // 2) ★坐标转换：ENU → NED（致命接缝！）
        Eigen::Vector3d pos_ned = enuToNed(ref.pos);
        double yaw_ned = enuYawToNed(ref.yaw);   // yaw 也要转

        // 3) 填 PX4 setpoint（NED 系）
        TrajectorySetpoint sp{};
        sp.position = {(float)pos_ned.x(), (float)pos_ned.y(), (float)pos_ned.z()};
        sp.yaw = (float)yaw_ned;
        sp.timestamp = get_clock()->now().nanoseconds() / 1000;
        setpoint_pub_->publish(sp);
    }

    // ENU → NED 的轴重排（不是取负！）
    Eigen::Vector3d enuToNed(const Eigen::Vector3d& enu) {
        return Eigen::Vector3d(enu.y(), enu.x(), -enu.z());
        //                      北=ENU.y  东=ENU.x  地=-ENU.z
    }
    double enuYawToNed(double yaw_enu) {
        // ENU 的 yaw（从东逆时针）转 NED 的 yaw（从北顺时针）
        return M_PI_2 - yaw_enu;
    }
};

逐段解释（本节最核心的代码）：

心跳定时器独立（构造函数）：heartbeat_timer_ 是 50 Hz 的独立定时器，只做一件事——发 OffboardControlMode。它和 control_timer_ 解耦，即使控制循环卡顿，心跳也照常发，避免被 PX4 夺权。这是上文炸机级陷阱的正确解法的代码落地。
心跳内容声明控制类型（publishHeartbeat）：OffboardControlMode 的 bool 字段告诉 PX4 "本周期我用哪种 setpoint"。这里 position=true 表示位置控制；路线 γ 高频控制会设 body_rate=true 直发角速度。
坐标转换是致命接缝（controlLoop 步骤 2 + enuToNed）：轨迹在 ENU（因为轨迹层/控制器按 ROS 约定），发给 PX4 前必须转 NED。enuToNed 实现上面的轴重排矩阵——注意是 (y, x, -z) 不是 (-x, -y, -z)。yaw 也要从"ENU 东起逆时针"转成"NED 北起顺时针"。这几行代码错一个符号就炸机。

本质洞察：上面的代码用**位置 setpoint**（让 PX4 内部的位置控制器跟踪），这是最简单的 Offboard 方式（路线 α/β）。但它**没有用到你 D1 学的几何控制器**——位置环在 PX4 里跑。如果要用自己的几何控制器（路线 γ），应该发**体轴角速度 + 集体推力**（设 body_rate=true，填 VehicleRatesSetpoint 和推力），让 PX4 只做最内层的角速度环。这是一个深刻的架构选择：你把控制的多少留给 PX4、自己接管多少。路线 α 全交给 PX4（最稳但最不灵活），路线 γ 只留角速度环给 PX4（最灵活，能跑 D1 的几何控制和 D8 的敏捷飞行，但要自己保证实时性和坐标正确）。

Arming 与模式切换序列¶

切入 Offboard 不是发一条指令就行，有严格的**时序**：

// Offboard 进入序列（伪代码）
// 1) 先发足够的心跳（>1 秒，>2 Hz）建立"控制器健康"证明
for (int i = 0; i < 100; i++) {   // 100 × 20ms = 2 秒
    publishHeartbeat();
    publishSetpoint(current_position);   // 先发当前位置作为 setpoint
    sleep(20ms);
}
// 2) 请求切换到 Offboard 模式
setMode("OFFBOARD");
// 3) 解锁（arm）
arm();
// 4) 之后持续发心跳 + setpoint

时序的关键：必须先发心跳和 setpoint，再切 Offboard、再 arm。如果顺序反了（先切 Offboard 但没有心跳流），PX4 会立即拒绝或退出。这是新手切 Offboard 失败的头号原因。

运行验证¶

ros2 run your_pkg offboard_control
# 观察：
# - PX4 shell 显示进入 Offboard 模式且 armed
# - 无人机起飞到 setpoint 指定位置并悬停
# - 给一个轨迹，无人机跟踪轨迹（RViz 对比参考轨迹和实际轨迹）
ros2 topic hz /fmu/in/offboard_control_mode   # 心跳应稳定 50 Hz

成功标志：无人机成功进入 Offboard 并 arm，跟踪轨迹，跟踪 RMS 误差 <0.3m（路线 α/β）。心跳频率稳定在 50 Hz 不掉。无人机朝**正确**方向飞（坐标转换对了）。

⚠️ 常见陷阱¶

🔧 配置陷阱：EKF2 没收敛就切 Offboard。错误做法：PX4 一启动就立刻切 Offboard、arm、起飞。现象：无人机起飞瞬间位置估计跳变，姿态失稳。根本原因：PX4 的 EKF2 状态估计器需要时间收敛（融合 IMU、视觉/GPS、磁力计）。没收敛时位置/姿态估计不可靠，此时控制等于"盲飞"。正确做法：起飞前检查 EKF2 的健康标志（/fmu/out/vehicle_status 或 MAVROS 的 /mavros/state），确认位置估计有效（local_position valid）再切 Offboard。这是 §D12.9 起飞 checklist 的核心项之一。

⚠️ 编程陷阱（炸机级）：yaw 坐标转换漏了或方向错。错误做法：转了位置的 ENU→NED，但忘了转 yaw，或 yaw 转换公式写错。现象：无人机位置对，但机头朝向系统性偏转，传感器看的方向不对，探索时"答非所问"；严重时 yaw 误差导致控制耦合发散。根本原因：ENU 的 yaw（从东轴逆时针为正）和 NED 的 yaw（从北轴顺时针为正）定义不同，需要 \(\psi_{NED} = \pi/2 - \psi_{ENU}\) 转换。漏转或符号错，机头方向就错。正确做法：位置、速度、加速度、yaw、yaw 速率**全部**做对应转换，逐个验证。用 MAVROS 可避免（它全转好了）；用 uXRCE-DDS 要自己仔细处理每一个量。

💡 概念误区：以为 Offboard 模式下 PX4 不管安全。新手想法："切了 Offboard，无人机就完全听我的，PX4 不插手。" 实际上：PX4 在 Offboard 下**仍然运行 failsafe**——心跳丢失、电池过低、地理围栏越界、RC 丢失（取决于配置），PX4 都会接管。Offboard 不是"完全交权"，而是"在 PX4 的安全监督下接受外部 setpoint"。正确理解：PX4 的 failsafe 是你的**安全网**，不是障碍。正确配置 failsafe（§D12.9）让它在真正危险时兜底，而不是关掉它。把 failsafe 当敌人关掉的人，第一次炸机就会后悔。

练习¶

[集成测试] 故意把 enuToNed 写成 (-x, -y, -z)（新手常见错误），在仿真里观察无人机往哪个方向飞（应该是错误方向）。然后改回正确的 (y, x, -z)，验证方向正确。这道题让你亲眼看到坐标转换错误的后果，记一辈子。（务必在仿真里做，绝不在实机上做。）
[管线搭建] 实现"心跳独立线程"：故意在控制主循环里加一个 sleep(0.6s) 模拟卡顿。对比心跳在主循环里（会被夺权）vs 心跳在独立定时器里（不被夺权）两种实现，验证后者即使主循环卡顿也不掉 Offboard。
[集成测试·跨章综合] 综合 D1（几何控制）：把位置 setpoint 模式（PX4 跑位置环）换成体轴角速度+推力模式（自己跑 D1 的几何控制器，PX4 只跑角速度环）。对比两种模式的跟踪性能和敏捷性。解释为什么高速敏捷飞行（D8）必须用后者（提示：PX4 内部位置环的带宽限制）。

§D12.7 FSM 调度与异常处理——系统的总指挥 ⭐⭐⭐¶

过渡：前面六节我们搭好了从感知到控制的全链路，但缺一个"总指挥"——谁决定什么时候起飞、什么时候开始探索、什么时候返航？谁在规划失败时叫停、在电池低时返航、在通信断时降落？这就是有限状态机（Finite State Machine, FSM）。它不产生轨迹也不做控制，但它**编排**所有层的协作，是系统从"一堆能跑的模块"变成"一个有行为的系统"的关键。

模块功能与在系统中的位置¶

FSM 是系统的"行为大脑"。它维护一个**当前状态**（INIT / TAKEOFF / EXPLORING / RETURN_HOME / EMERGENCY），根据**条件**（位姿、电池、通信、规划结果）触发**状态转换**，并在每个状态里**指挥**对应的层做事。它不在数据流的主路径上（不处理点云、不算轨迹），而是在**控制流**上——决定"现在该干什么"。

本质洞察：FSM 的价值不在于它能做什么"聪明"的决策，而在于它让系统的行为**可预测、可调试、可验证**。没有 FSM，异常处理逻辑会散落在各个节点里（探索节点判断电池、轨迹节点判断超时、控制节点判断通信），互相矛盾、难以追踪。FSM 把所有"什么时候做什么"的决策**集中到一处**，让你能画出完整的状态图、能验证每个转换、能在日志里清楚看到"现在是什么状态、为什么转换"。这是 R6B 的对比——分散的条件判断 vs 集中的状态机；前者灵活但混乱，后者受约束但可控。对安全攸关的飞行系统，可控性压倒灵活性。

状态机设计：状态与转换¶

骨架给定的核心状态流：INIT → TAKEOFF → WAIT_EXPLORE → EXPLORING → RETURN_HOME。我们把它细化成一个完整的状态图：

        ┌──────┐  起飞前检查通过   ┌─────────┐  到达悬停高度  ┌──────────────┐
        │ INIT │ ───────────────> │ TAKEOFF │ ────────────> │ WAIT_EXPLORE │
        └──────┘                   └─────────┘                └──────┬───────┘
           ▲                                                         │ 收到探索指令
           │ 降落完成                                                 ▼
        ┌──────┐  到达 home 并降落  ┌─────────────┐  探索完成/电池低  ┌───────────┐
        │ LAND │ <──────────────  │ RETURN_HOME │ <──────────────  │ EXPLORING │
        └──────┘                   └─────────────┘                  └─────┬─────┘
                                          ▲                               │
                                          │                    (循环：选前沿→规划→执行)
                                   ┌──────────────┐  任何状态下                │
                                   │  EMERGENCY   │ <────────────────────────┘
                                   └──────────────┘  通信断/严重故障 → 紧急降落

每个状态做什么、转换条件是什么（这是 FSM 的全部内容）：

状态	在这个状态做什么	转出条件 → 目标状态
INIT	等 EKF2 收敛、TF 树就绪、各节点上线	检查全通过 → TAKEOFF
TAKEOFF	发起飞指令，爬升到悬停高度	到达高度且稳定 → WAIT_EXPLORE
WAIT_EXPLORE	悬停，等探索指令/初始化探索模块	收到开始指令 → EXPLORING
EXPLORING	循环：探索层选目标 → 轨迹层规划 → 控制层执行	无前沿/覆盖达标/电池低 → RETURN_HOME；规划连续失败 → 悬停重试
RETURN_HOME	规划回 home 点的轨迹并执行	到达 home → LAND
LAND	下降、降落、上锁	降落完成 → INIT（或结束）
EMERGENCY	立即悬停或就地降落	（终止态，需人工介入）

接缝重点：三类异常的处理¶

骨架明确要求处理三类异常。这是 FSM 设计的核心，也是系统鲁棒性的体现（R6E 系统分类）：

异常	检测方式	处理策略	进入状态
规划超时/失败	轨迹层连续 N 次规划失败	先悬停等待（地图可能在更新），重试；超时后换目标或返航	EXPLORING 内悬停 → 重试
电池低电	订阅电池电压/电量	立即停止探索，规划返航	EXPLORING → RETURN_HOME
通信中断	心跳超时（与地面站/关键节点）	就地悬停或缓降，等待恢复；长时间断 → 降落	任何状态 → EMERGENCY

⚠️ 编程陷阱：FSM 死锁——卡在某状态出不来。错误做法：EXPLORING 状态等待"规划成功"才转出，但目标不可达导致永远规划失败，又没有超时退出机制。现象：无人机悬停在原地不动，FSM 卡在 EXPLORING，既不探索也不返航，直到电池耗尽。根本原因：状态转换条件设计不完备——只考虑了"正常路径"（规划成功），没考虑"卡住路径"（一直失败）的退出。正确做法：每个可能卡住的状态都要有超时退出。EXPLORING 里规划连续失败超过阈值（如 10 次或 30 秒），就放弃当前目标换下一个；所有目标都不可达，就判定探索完成转 RETURN_HOME。FSM 设计的铁律：每个状态都必须有"无论如何都能转出"的路径，绝不允许只有"理想条件满足才转出"的设计。 这是避免死锁的根本。

本质洞察：FSM 的健壮性等于**所有转换条件的完备性**。一个状态如果只定义了"成功时怎么转"，没定义"失败/超时/异常时怎么转"，它就是一个潜在的死锁点。设计 FSM 时的正确姿态是**悲观主义**——对每个状态都问"如果这里出了最坏的情况（永远等不到期望事件），它怎么逃出去？"。这呼应了 §D12.5 轨迹层"诚实暴露失败"和 §D12.4 探索层"提供回退候选"——整个分层系统的鲁棒性，建立在每一层都假设下游会失败、并为失败准备退路的基础上。

代码走读：FSM 主循环（伪框架）¶

// fsm.cpp —— 自主探索状态机
enum class State { INIT, TAKEOFF, WAIT_EXPLORE, EXPLORING, RETURN_HOME, LAND, EMERGENCY };

class ExplorationFSM : public rclcpp::Node {
    void fsmTimerCallback() {   // 10 Hz 状态机主循环
        // ★最高优先级：异常检测，任何状态都先查（抢占式）
        if (commLost() || criticalFault()) {
            transitTo(State::EMERGENCY);
        }
        if (batteryLow() && state_ != State::RETURN_HOME && state_ != State::LAND) {
            RCLCPP_WARN(get_logger(), "Battery low, returning home");
            transitTo(State::RETURN_HOME);
        }

        // 状态机主逻辑
        switch (state_) {
        case State::INIT:
            if (ekf2Ready() && tfTreeReady() && allNodesUp())
                transitTo(State::TAKEOFF);
            break;

        case State::TAKEOFF:
            commandTakeoff(hover_height_);
            if (reachedHeight(hover_height_) && isStable())
                transitTo(State::WAIT_EXPLORE);
            break;

        case State::EXPLORING: {
            // 循环：选目标 → 规划 → 执行
            if (needNewGoal()) {                    // 到达当前目标或无目标
                Eigen::Vector3d goal; double yaw;
                if (explorer_->selectNextGoal(map_, cur_pos_, goal, yaw)) {
                    if (planner_->plan(cur_state_, goal, traj_)) {
                        executeTraj(traj_);
                        replan_fail_count_ = 0;
                    } else {
                        replan_fail_count_++;       // 规划失败
                        commandHover();             // 先悬停
                        if (replan_fail_count_ > 10) // ★超时退出，防死锁
                            explorer_->blacklistGoal(goal);  // 拉黑这个目标
                    }
                } else {
                    // 没有前沿了 = 探索完成
                    transitTo(State::RETURN_HOME);
                }
            }
            break;
        }

        case State::RETURN_HOME:
            if (planner_->plan(cur_state_, home_pos_, traj_)) executeTraj(traj_);
            if (reachedPosition(home_pos_)) transitTo(State::LAND);
            break;

        case State::LAND:
            commandLand();
            if (landed()) transitTo(State::INIT);
            break;

        case State::EMERGENCY:
            commandHover();   // 或 commandLand()，取决于策略
            break;
        default: break;
        }
    }

    void transitTo(State next) {
        RCLCPP_INFO(get_logger(), "FSM: %s -> %s",
                    toStr(state_).c_str(), toStr(next).c_str());  // ★日志每次转换
        state_ = next;
    }
};

关键设计点：

抢占式异常检测（开头）：异常检查放在 switch 之前，任何状态下每个周期都先查。通信断/严重故障立刻进 EMERGENCY，电池低立刻转 RETURN_HOME——这些是"高优先级中断"，优先于正常状态逻辑。
EXPLORING 的防死锁（replan_fail_count_）：规划连续失败超 10 次就把目标拉黑（blacklistGoal），避免反复尝试同一个不可达目标。所有前沿耗尽则转 RETURN_HOME。这是上文死锁陷阱的正确解法。
每次转换都打日志（transitTo）：状态转换全部经过 transitTo 并打日志。这让你在 rosbag 或终端里能清楚回溯"系统经历了哪些状态、为什么转换"——调试 FSM 的生命线。
FSM 自己不算轨迹：它调用 explorer_->selectNextGoal 和 planner_->plan，自己只做"调度"。职责清晰：探索层负责"去哪"，轨迹层负责"怎么去"，FSM 负责"现在做哪件事"。

运行验证¶

ros2 launch your_pkg full_system.launch.py   # 拉起全部 7 层 + FSM
# 观察终端的 FSM 状态转换日志：
# INIT -> TAKEOFF -> WAIT_EXPLORE -> EXPLORING -> ... -> RETURN_HOME -> LAND
# 在 RViz 里观察无人机自主完成：起飞 → 探索（地图增长）→ 返航 → 降落

成功标志：无人机在**无人干预**下完成完整任务流——起飞、自主探索（覆盖率持续增长）、探索完成后自动返航降落。FSM 日志清晰显示每次状态转换。这是整个系统集成的"毕业时刻"。

⚠️ 常见陷阱¶

🔧 配置陷阱：异常优先级设置错误，电池低被探索逻辑覆盖。错误做法：把电池检查放在 EXPLORING 的 case 里面，而不是 switch 之前。现象：探索逻辑正忙（在规划/执行）时，电池低没被及时响应，无人机继续探索直到没电。根本原因：异常处理被埋在正常状态逻辑里，只有在特定分支才会执行，无法抢占。正确做法：所有抢占式异常（电池、通信、故障）放在 switch 之前，每个周期无条件检查。正常状态逻辑是"低优先级"，异常是"高优先级中断"。

⚠️ 编程陷阱：状态转换时没有清理上一状态的残留指令。错误做法：从 EXPLORING 转 RETURN_HOME 时，直接改状态，但 EXPLORING 时下发的旧轨迹还在被控制层执行。现象：转换瞬间无人机行为矛盾——既想返航又在执行旧探索轨迹，可能朝错误方向冲。根本原因：状态转换只改了 FSM 的状态变量，没有"中止旧状态的动作 + 初始化新状态"。正确做法：在 transitTo 里加 onExit(旧状态) 和 onEnter(新状态) 钩子——退出 EXPLORING 时停止旧轨迹执行，进入 RETURN_HOME 时清空目标、重新规划。状态转换是"原子操作"：先清理旧的，再初始化新的。

💡 概念误区：以为 FSM 越复杂越好。新手想法："多加几个状态、多分几种情况，系统就更智能更鲁棒。" 实际上：状态越多，转换关系越复杂，越容易出现未覆盖的转换组合（死锁、活锁）。一个有 20 个状态、上百条转换的 FSM 几乎不可能验证完备。正确理解：FSM 应该**尽可能简单**——状态数控制在能画在一张纸上、每个转换都能枚举验证的规模。复杂的决策应该委托给专门的模块（探索层、规划层），FSM 只做最顶层的调度。简单的 FSM 才是可验证、可信赖的 FSM。

练习¶

[管线搭建] 实现完整 FSM 并跑通"起飞→探索→返航→降落"全流程。在 EXPLORING 里制造一个不可达目标（如把目标设在障碍物里），验证防死锁逻辑：FSM 是否在 N 次失败后拉黑目标、继续探索其他区域？
[集成测试] 模拟三类异常：(a) 突然把电池电量话题设为低值，(b) 停掉一个关键节点模拟通信断，(c) 让规划器持续失败。验证 FSM 对每类异常的响应是否正确（电池低→返航、通信断→悬停/降落、规划失败→重试/换目标）。
[集成测试·跨章综合] 综合全章：给 FSM 加一个 onEnter/onExit 钩子机制，确保每次状态转换都干净地中止旧动作、初始化新动作。用 rosbag 录一次完整任务，回放并标注每次状态转换的时刻和原因，画出系统的"行为时间线"。

§D12.8 性能调优与 timeline 分析 ⭐⭐⭐¶

过渡：系统能飞了，但"能飞"和"飞得好"之间还有很大距离。路线 α 的系统可能规划只有 1 Hz、飞 1 m/s；路线 γ 要求规划 >10 Hz、飞 >10 m/s。这一节讲怎么测量瓶颈、画 timeline、把性能从"能跑"调到"跑得快"。这是工程实践教学的"性能调优"练习类型的核心。

模块功能与定位¶

性能调优不是某一层的事，而是**贯穿全栈的系统工程**。它的方法论是：先测量、再定位、后优化——不要凭感觉猜瓶颈在哪，要用数据说话。

本质洞察：自主飞行系统的性能瓶颈，几乎从不在你以为的地方。新手总以为是"算法太慢"，实际上最常见的瓶颈是：点云没降采样导致 SLAM 慢、地图分辨率过高导致更新慢、控制线程里的日志 IO 导致掉频、节点间话题序列化开销。这些都是**工程问题而非算法问题**。R6B 的反事实：如果不测量就盲目优化算法，你会花几天优化一个只占总延迟 5% 的环节，而真正的瓶颈（占 60%）纹丝不动。测量是优化的前提，没有测量的优化是赌博。

性能指标体系¶

要优化，先定义"什么是好"。自主飞行栈的关键性能指标（KPI）：

指标	目标（α/β/γ）	测量方法	影响
SLAM 里程计延迟	<20/15/10 ms	点云时间戳 → /odom 发布时刻	高速跟踪精度
SLAM 里程计频率	>50/100/100 Hz	`ros2 topic hz /odom`	控制参考的新鲜度
地图更新时间	<50/30/20 ms	节点内打点	规划能否拿到最新地图
规划频率	>1/5/10 Hz	`ros2 topic hz /trajectory`	对环境变化的反应速度
单次规划耗时	<100/20/5 ms	规划器内打点	决定规划频率上限
控制频率	>50/100/200 Hz	`ros2 topic hz /fmu/in/...`	跟踪精度、Offboard 心跳
端到端延迟	测量	感知 → 控制全链路	系统总反应时间

timeline（甘特图）分析¶

骨架要求"画甘特图展示各模块的时间占用"。这是系统性能分析的核心工具——把各模块在一个控制周期内的执行时间画成横条，一眼看出谁占用最多、是否有空闲、是否串行阻塞。

一个 100ms 周期内各模块的 timeline（理想 vs 有瓶颈）：

理想（各模块并行/流水，总延迟 < 周期）：
SLAM      [████████]                          (8ms)
Map           [██████████]                     (10ms，等 SLAM)
Plan                  [████]                    (5ms，等 Map)
Control   [█][█][█][█][█][█][█][█][█][█]        (每 10ms 一次，独立高频)
          └─ 总反应延迟 ~23ms，规划可 10Hz ─┘

有瓶颈（地图更新慢，串行阻塞）：
SLAM      [████████]                            (8ms)
Map           [██████████████████████████]      (40ms！瓶颈)
Plan                                    [████]   (5ms，被迫等 40ms)
          └─ 总延迟 ~53ms，规划掉到 ~5Hz ────┘
                        ▲ 优化目标：把 Map 从 40ms 压到 15ms

⚠️ 编程陷阱：在控制热路径里做 .cpu() 或日志 IO 导致掉频。错误做法：在 100-200 Hz 的控制循环里加 RCLCPP_INFO 打印调试信息，或对 Eigen 矩阵做调试输出。现象：控制频率名义 200 Hz 实际只有几十 Hz，jitter 大，偶尔触发 Offboard 心跳超时。根本原因：日志 IO（写终端/文件）是阻塞操作，几十微秒到毫秒级，在高频循环里累积成可观延迟。Eigen 的某些调试输出还会触发堆分配。正确做法：控制热路径**零阻塞**——调试数据用无锁队列异步传给单独的日志线程，或用 RCLCPP_INFO_THROTTLE 限频打印（如每秒一次），或离线分析 rosbag。这正是工程实践规范里"GPU 管线中 .cpu() 破坏流水线"陷阱在 CPU 控制循环上的对应版本。

配置详解：性能调优的关键旋钮¶

旋钮	在哪	调它影响什么
点云降采样体素	SLAM/地图入口（`filter_size`）	调大 → 点少、快，但精度降；性能调优第一刀
地图分辨率	地图层（`resolution`）	调粗 → 内存少、更新快；房间场景 0.15m 够
规划范围	轨迹层（`planning_horizon`）	调小 → 规划快，但看得近；权衡反应速度和规划量
规划迭代上限	优化器（L-BFGS max_iter）	设上限 → 宁可次优也保实时（关键！）
节点执行器线程数	ROS2 executor	多线程 executor 让回调并行，避免串行阻塞

本质洞察：实时系统的优化哲学是"宁可次优，不可超时"。一个偶尔超时的"最优"规划器，远不如一个总是按时返回"够用解"的规划器——因为超时意味着控制拿不到新参考，无人机在用过期轨迹飞。给所有迭代优化器（L-BFGS、QP）设**迭代上限和时间预算**，到时间就返回当前最好的解。这是 D8 敏捷飞行控制频率要求和导读"硬实时 1 ms"认知跨越的统一原则——实时性是硬约束，最优性是软目标。

代码走读：给关键环节打时间戳¶

// 性能打点的标准模式（用 steady_clock，不受系统时间调整影响）
void mapUpdateCallback(const PointCloud2& cloud) {
    auto t0 = std::chrono::steady_clock::now();

    updateMap(cloud);   // 被测量的环节

    auto t1 = std::chrono::steady_clock::now();
    double ms = std::chrono::duration<double, std::milli>(t1 - t0).count();
    // 限频打印或写入统计（不要每帧都 INFO）
    RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000,  // 每 1000ms 一次
        "Map update: %.2f ms", ms);
    map_update_time_stats_.add(ms);   // 累积统计（均值/最大/p99）
}

要点：用 steady_clock（单调时钟，不受 NTP 调整影响）测耗时；用 _THROTTLE 限频打印避免日志成为新瓶颈；累积统计关注 **p99/最大值**而非只看均值——实时系统怕的是偶发的长尾延迟（一次 100ms 的卡顿就能让无人机失控），均值漂亮但尾部糟糕的系统是危险的。

运行验证与调优流程¶

# 1) 全局测量各话题频率和延迟
ros2 topic hz /odom /rog_map/occ_inflate /trajectory /fmu/in/trajectory_setpoint
ros2 topic delay /odom

# 2) 用 ros2 的 trace 工具或节点内打点，画出 timeline
# 3) 定位最慢环节，针对性优化（降采样/降分辨率/限迭代/多线程）
# 4) 重新测量，验证瓶颈是否转移

调优的成功标志（路线 γ）：规划频率从初始的 ~5 Hz 提到 >10 Hz，单次规划 <5 ms，地图更新 <20 ms，控制稳定 200 Hz，端到端延迟 <50 ms。在林地场景能稳定飞 >10 m/s 探索。

⚠️ 常见陷阱¶

🔧 配置陷阱：单线程 executor 导致回调串行阻塞。错误做法：用默认的单线程 executor 跑所有节点的回调。现象：一个慢回调（如地图更新）阻塞了其他所有回调（包括控制），整个系统被最慢的环节拖慢。根本原因：单线程 executor 串行执行所有回调，一个卡住，全部排队等待。正确做法：用 MultiThreadedExecutor，并合理设置回调组（CallbackGroup）——把控制等实时回调放在独立的回调组（MutuallyExclusive 或 Reentrant），与慢回调隔离，让它们能并行。

💡 概念误区：以为升级硬件就能解决性能问题。新手想法："规划慢？换个更强的 Jetson/CPU 不就行了。" 实际上：很多性能问题是**软件结构问题**（串行阻塞、没降采样、没限迭代、热路径 IO），换硬件只是把问题推后，且机载计算受功耗/重量约束不能无限升级。正确理解：先用 profiler 定位是"算力不够"还是"结构不对"。如果是结构问题（最常见），优化软件比换硬件性价比高得多。换硬件是最后手段，不是第一反应。

练习¶

[性能调优] 给系统的每个关键环节（SLAM、地图、规划、控制）加 steady_clock 打点，画出一个完整控制周期的 timeline 甘特图。找出占用最多的环节。
[性能调优] 针对你找到的瓶颈做一次优化（如地图更新慢就降采样/降分辨率），重新测量。报告：瓶颈环节的耗时降低了多少？规划频率提升了多少？瓶颈是否转移到了别的环节？
[性能调优·跨章综合] 综合全栈：把系统从路线 α 的性能（规划 1 Hz、飞 1 m/s）优化到路线 β（规划 5 Hz、飞 3 m/s）。列出你做的所有优化项和各自的收益。这是一道"系统级调优"综合题，检验你对整条链路性能的理解。

§D12.9 实机部署——从仿真到真机的安全工程 ⭐⭐⭐⭐¶

过渡：仿真里飞得再好，真机才是终极考验。这一节讲怎么把仿真验证过的系统安全地搬到真机上——起飞前 checklist、failsafe 配置、从拴绳到放开的渐进测试。这一节的每一条都用炸机和受伤的教训换来的，请逐条对待。 如果你只用仿真做毕业项目，这节可以速读；如果你要上真机，这节比前面任何一节都重要。

为什么仿真到真机是一道鸿沟（动机 → 反面）¶

动机：你在仿真里把系统调到完美，自然想上真机验证。但仿真和真机之间有一道由无数细节构成的鸿沟。

反面：如果你不做渐进测试、直接把仿真参数搬上真机满油门起飞，结果几乎注定是炸机。原因（R6E 系统分类，sim-to-real gap 的四个维度，呼应工程实践规范的"仿真中跑通≠真机能用"概念误区）：

维度	仿真 vs 真机的差异	后果
动力学	真机有电机延迟（15-30ms）、桨涡干扰、电池电压跌落、未建模气动	控制器跟踪变差，激进机动易失稳
传感器	真机 IMU 有偏置漂移、LiDAR 有测距噪声、相机有运动模糊	SLAM 在起飞震动下可能发散
时间	真机各传感器时钟不同步，机载计算受功耗约束算力下降	时间同步问题放大，可能掉频
环境	真实光照变化、反光表面、动态物体、电磁干扰	感知失效、磁力计受扰

本质洞察：从仿真到真机，风险不是线性增长而是阶跃式的。仿真里最坏的后果是"重新跑一次"，真机里最坏的后果是"几千块的设备摔碎 + 伤人"。所以迁移策略的核心不是"快速验证"而是"最小化首次真飞的风险"——每一步都在可控范围内增加一点点风险，确认安全再放开下一点。这是安全工程的基本姿态，和软件开发"快速迭代"的思维完全不同。

起飞前 checklist（每次真飞必查）¶

这是一份结构化的起飞前检查清单，分硬件、软件、安全三类。每次真飞前逐项确认，不要凭记忆跳过。

硬件检查：

#	检查项	通过标准	不通过的后果
H1	电池电压	满电（如 4S 16.8V），固定牢固	电压不足飞行中掉电、电池脱落
H2	螺旋桨	朝向正确（CW/CCW 对应）、无裂纹、拧紧	装反则推力反向，立即翻机
H3	机架/线材	无松动、无磨损、传感器固定	振动导致 IMU 噪声、SLAM 发散
H4	重心	在几何中心附近	重心偏移导致控制器持续补偿、能耗增
H5	遥控器	已绑定、电量足、手动模式可控	失去人工接管能力（最危险）

软件检查：

#	检查项	通过标准	不通过的后果
S1	EKF2 收敛	位置估计 valid、方差收敛	起飞瞬间位置跳变、失稳
S2	SLAM 里程计	静止时 /odom 稳定不漂、频率达标	飞行参考错误
S3	TF 树完整	`view_frames` 无断裂	规划拿到错误坐标数据
S4	时间同步	各节点 use_sim_time=false（真机用真实时钟）、TIMESYNC 正常	时间戳错乱
S5	坐标转换	手动给小位移指令，确认无人机朝正确方向（地面测试）	朝错误方向猛冲（炸机级）
S6	Offboard 心跳	心跳频率稳定 >2 Hz	飞行中被夺权
S7	failsafe 配置	已设并测试（见下）	异常时无兜底

安全检查：

#	检查项	通过标准
A1	飞行空域	室内有防护网/室外合规、无人员在桨平面内
A2	急停手段	遥控器一键切手动/上锁，操作员手指在急停位
A3	拴绳（首飞）	首次自主飞行用安全绳限制活动范围
A4	灭火/急救	锂电池起火预案、急救箱

⚠️ 配置陷阱（炸机级）：跳过 S5 坐标转换地面验证直接起飞。错误做法：仿真里坐标转换对了，就假设真机也对，不做地面验证直接自主起飞。现象：真机一切 Offboard 朝某个方向猛冲（可能是仿真和真机的传感器朝向/外参不同导致转换链上某处不一致）。根本原因：真机的传感器安装方向、外参标定、磁力计校准都可能和仿真假设不同，坐标转换链上任一处不一致都会导致方向错误。正确做法：真飞前必做地面坐标验证——把无人机放地上（或手持），通过 Offboard 给一个小的"向前 0.5m"位置指令，观察控制器输出的推力/姿态指令方向是否朝前（不真起飞，只看指令或低油门测试）。确认前后左右上下六个方向都对，再允许起飞。这一步能拦住 90% 的坐标转换炸机。

failsafe 配置¶

PX4 的 failsafe 是你最后的安全网。关键 failsafe 参数（在 QGroundControl 或参数表里设）：

failsafe	参数	推荐设置	触发行为
RC 丢失	`NAV_RCL_ACT`	返航或降落	遥控信号丢失时自动处置
数据链丢失	`NAV_DLL_ACT`	降落	与地面站通信断
低电量	`BAT_LOW_THR` / `BAT_CRIT_THR`	低电警告/紧急降落	分级电量保护
地理围栏	`GF_ACTION`	返航	越界自动拉回
Offboard 丢失	（内置）	默认降落/悬停	心跳丢失时处置

本质洞察：配置 failsafe 的核心是**想清楚"每种故障下，什么行为最安全"，而不是套默认值。室内无 GPS 不能"返航到 GPS 点"，应设"就地降落"；室外开阔可以"返航"。同一个 failsafe 在不同场景下的最优行为不同。**failsafe 不是开关，是一组需要按场景设计的决策。把它当 checklist 项认真设计，是真飞前必须完成的工程，而非可选项。

渐进测试流程：从拴绳到放开¶

骨架和导读都强调"逐步放开"。这是把首飞风险降到最低的标准流程：

阶段 1：地面测试（不起飞）
  - 验证坐标转换方向（S5）
  - 验证 Offboard 进入、心跳、failsafe 触发
  - 低油门测试电机响应方向

阶段 2：拴绳悬停（限高 < 1m）
  - 安全绳限制活动范围，手动随时接管
  - 验证起飞、悬停、降落
  - 观察 SLAM 在真实振动下是否稳定

阶段 3：拴绳低速机动（限速 < 1 m/s）
  - 给小范围位置指令，验证跟踪
  - 验证 TF/时间同步在真机上无误

阶段 4：解绳低速自主（限速 < 2 m/s、限高、防护网内）
  - 完整自主探索小范围
  - 操作员手指在急停位

阶段 5：逐步放开速度/范围
  - 每次只放开一点，确认稳定再继续
  - 直到达到目标性能

每个阶段的铁律：只有上一阶段完全稳定，才进入下一阶段。任何异常都退回上一阶段排查。绝不跳级。

⚠️ 编程陷阱（炸机级）：仿真参数（尤其控制增益）直接搬真机。错误做法：把仿真里调好的几何控制器增益 \(K_p, K_v, K_R\) 原封不动用到真机。现象：真机要么软绵绵跟不上（增益偏小），要么高频震荡发散（增益偏大，激发了真机未建模的延迟/柔性）。根本原因：真机有电机延迟、机架柔性、气动等仿真未完全建模的因素，最优增益和仿真不同。仿真无延迟时能用的大增益，真机有延迟时会震荡。正确做法：真机增益**从仿真值的 50-70% 起调**，在拴绳阶段逐步加大到刚好不震荡。或者用 D9 的域随机化训练让策略对参数不敏感，或用 D8 的 INDI 控制器对延迟/扰动鲁棒。真机调参必须在安全（拴绳）条件下从保守值渐进。

用 rosbag 复现和分析问题¶

真飞出问题时，现场往往来不及看。全程录 rosbag，事后离线复现分析是排障的标准手段。

# 真飞全程录关键话题（注意磁盘和带宽，点云可降频录）
ros2 bag record /odom /tf /tf_static /fmu/out/vehicle_local_position \
  /fmu/out/vehicle_status /trajectory /fmu/in/trajectory_setpoint \
  /exploration/goal /fsm/state -o flight_$(date +%Y%m%d_%H%M%S)

# 事后回放分析（如分析一次异常的前因后果）
ros2 bag play flight_xxx.bag
# 在 RViz 里回放观察，对照 FSM 状态日志和控制指令，定位问题时刻

录包要点：录**决策相关**的话题（里程计、TF、状态、目标、FSM 状态、控制指令），点云这类大数据可降频或不录。出问题时，回放 rosbag + FSM 状态日志，能精确还原"哪个时刻、什么状态、为什么做了那个决策"——这是真机排障不可替代的工具。

运行验证（真机首飞）¶

成功标志（阶段 4）：无人机在防护网内、限速 2 m/s、操作员随时可接管的条件下，完成一次小范围自主探索（起飞→探索→返航→降落），全程无需人工干预，SLAM 不发散，无碰撞，failsafe 未误触发。这是真机集成的"毕业时刻"，比仿真的毕业时刻含金量高得多。

⚠️ 常见陷阱¶

🔧 配置陷阱：真机忘了关 use_sim_time。错误做法：从仿真切真机，launch 文件里 use_sim_time 还是 true。现象：所有节点等待 /clock 话题（真机没有），全部卡住不工作，或时间戳全是 0。根本原因：仿真用仿真时钟（/clock），真机用系统真实时钟。use_sim_time=true 让节点等仿真时钟，真机上没有这个话题。正确做法：真机部署时**所有节点** use_sim_time:=false（或不设，默认 false）。最佳实践是用一个 launch 参数统一切换 sim/real，避免漏改。

💡 概念误区：以为有了 failsafe 就可以激进测试。新手想法："反正有 failsafe 兜底，我可以大胆放开速度试。" 实际上：failsafe 有响应时间（检测+决策+执行），高速下 failsafe 触发时无人机可能已经撞上障碍或飞出安全区。failsafe 是"最后防线"不是"免死金牌"。正确理解：failsafe 降低**严重事故**的概率，但不能替代渐进测试。安全 = 渐进测试（降低进入危险的概率）+ failsafe（降低危险变成事故的概率）+ 人工接管（最终兜底）。三者缺一不可，没有任何一个能单独保证安全。

练习¶

[管线搭建] 把你的系统从仿真切到真机配置：写一个统一的 launch 参数 sim:=true/false，一键切换 use_sim_time、传感器话题名、通信路线（仿真 SITL vs 真机串口）。验收：同一套代码，改一个参数就能在仿真和真机间切换。
[集成测试] 在仿真里完整演练起飞前 checklist 的软件部分（S1-S7），写一个自动检查脚本：起飞前自动验证 EKF2 收敛、TF 完整、心跳正常、坐标转换方向正确，全通过才允许 arm。这是把 checklist 工程化，减少人为遗漏。
[集成测试·跨章综合] 综合 D9（sim-to-real）：设计一个完整的真机迁移计划，列出从仿真到阶段 5 的每一步、每步的验收标准、每步可能的失败模式和应对。这是一份"真机部署作战计划"，检验你对整个迁移过程风险的理解。（即使没有真机，这个计划本身就是宝贵的工程训练。）

本章常见误解汇总¶

误解	正确理解
系统集成的难点在算法	难点在接缝——TF、时间同步、坐标系、心跳、频率匹配。算法你都学过了
仿真追求视觉逼真最重要	接口一致性比视觉逼真重要一个数量级。Gazebo 接口标准 > Flightmare 视觉炫
SLAM 要建一张好看的地图	SLAM 要给控制器一个能跟的高频低延迟位姿。地图视觉质量次要
`map→base_link` 一段就够	必须 `map→odom→base_link` 两段：分离"连续漂移位姿"和"离散全局校正"
地图分辨率越高越好	分辨率匹配任务需要的最小障碍尺度即可。过高是浪费算力的过度工程
探索就是覆盖 100% 空间	探索是资源约束下最大化信息。不可达区域要果断放弃
轨迹生成成功=能飞	几何无碰撞 ≠ 动力学可跟踪。还要满足速度/加速度/jerk 在平台极限内
轨迹层要算一条完美轨迹	轨迹层要持续产出够用的轨迹流，重规划频率才是命门
ENU→NED 就是把某轴取负	是"交换 X/Y + Z 取负"的轴重排，不是符号翻转。错了直接炸机
Offboard 模式 PX4 不管安全	Offboard 下 PX4 仍运行 failsafe。它是安全网不是障碍
控制线程实时性是为了精度	更是为了不被 PX4 判定为死亡而夺权（心跳 >2 Hz）
FSM 越复杂越智能	FSM 应尽可能简单——能画在一张纸上、每个转换可验证
性能瓶颈在算法慢	几乎总在工程问题：没降采样、热路径 IO、串行阻塞。先测量再优化
升级硬件解决性能问题	多数是软件结构问题，换硬件只是推后。先 profiler 定位
仿真参数可直接搬真机	真机增益从仿真值 50-70% 起调，拴绳渐进。直接搬必震荡或发散
有 failsafe 就能激进测试	failsafe 有响应时间，高速下可能来不及。安全=渐进+failsafe+人工接管

本章小结¶

D12 是整个无人机方向的毕业验收。我们没有学新算法——前 11 章已经把每一层的算法讲透了。本章做的是**系统集成**：把这些层焊接成一个能真正起飞、自主探索、安全返航的完整系统。

贯穿全章的一条主线是：自主飞行系统的可靠性取决于最弱的接缝，而非最强的算法。 我们花了大量篇幅在那些论文里从不提、但实际会让无人机炸机的东西上——TF 树的两段式设计、时间同步的三类来源、NED↔ENU 的轴重排、Offboard 心跳的独立线程、重规划的起点接力、FSM 的防死锁、真机迁移的渐进测试。这些"接缝工程"才是从"论文能跑"到"系统能飞"之间真正的鸿沟。

另一条主线是**分层系统的鲁棒性哲学**：每一层都假设下游会失败，并为失败准备退路——探索层提供回退候选、轨迹层诚实暴露规划失败、FSM 为每个状态准备超时退出。鲁棒性不是某一层做得特别好，而是**整个系统在每个环节都为最坏情况做了准备**。

术语速查表¶

术语	英文/缩写	定义	首见
微分平坦逆映射	Differential Flatness Inverse Map	从平坦输出及导数代数恢复控制量	§D12.6
几何控制	Geometric Control (SE(3))	在旋转群上直接定义姿态误差的控制器	§D12.6
里程计	Odometry	SLAM 输出的位姿估计（带漂移）	§D12.2
占据栅格	Occupancy Grid	体素占据/空闲/未知的概率表示	§D12.3
欧氏符号距离场	ESDF	每点到最近障碍的带符号距离及梯度	§D12.3
前沿	Frontier	已知自由空间与未知空间的边界	§D12.4
安全飞行走廊	Safe Flight Corridor (SFC)	一串无障碍凸多面体，约束轨迹	§D12.5
重规划	Replanning	周期性基于最新地图重新生成轨迹	§D12.5
变换树	TF Tree	ROS2 维护坐标系间随时间变换的机制	§D12.2
北-东-地	NED (North-East-Down)	PX4 的世界坐标系约定	§D12.6
东-北-天	ENU (East-North-Up)	ROS 的世界坐标系约定（REP-103）	§D12.6
板外模式	Offboard Mode	PX4 接受外部 setpoint 的飞行模式	§D12.6
心跳	Heartbeat	证明外部控制器健康的周期消息（>2 Hz）	§D12.6
有限状态机	FSM (Finite State Machine)	编排系统行为的状态-转换机制	§D12.7
安全失效	Failsafe	异常时 PX4 自动接管的保护机制	§D12.6
微 XRCE-DDS	uXRCE-DDS	PX4 与 ROS2 的 DDS 桥接中间件	环境配置
软件在环	SITL (Software In The Loop)	飞控固件在 PC 上仿真运行	§D12.1

知识点总表¶

#	知识点	核心要点	对应节	难度
1	通信路线选择	uXRCE-DDS（低延迟、自翻坐标）vs MAVROS（便利、已翻坐标）	环境配置	⭐⭐
2	仿真层接口一致性	接口一致 > 视觉逼真；Gazebo 主力	§D12.1	⭐⭐
3	传感器配置	LiDAR/深度相机参数与 SLAM/地图选型耦合	§D12.1	⭐⭐
4	TF 树两段式	map→odom→base_link 分离漂移与全局校正	§D12.2	⭐⭐⭐
5	时间同步三来源	仿真时钟、传感器间、飞控-机载	§D12.2	⭐⭐⭐
6	FAST-LIO2 集成	高频低延迟里程计是命门；外参/线数一致	§D12.2	⭐⭐⭐
7	地图层选型	ROG-Map(LiDAR/大场景) vs voxblox(深度/ESDF)	§D12.3	⭐⭐
8	膨胀的意义	把无人机体积转移到障碍物，规划当质点	§D12.3	⭐⭐
9	前沿提取与选择	Yamauchi 贪婪 → FUEL 全局 ATSP	§D12.4	⭐⭐⭐
10	探索-规划握手	提议-验证-回退，目标不假设可达	§D12.4	⭐⭐⭐
11	重规划起点接力	用旧轨迹预测状态当起点，平滑衔接	§D12.5	⭐⭐⭐
12	轨迹约束反映平台	max_vel/acc ≤ 物理极限（推重比）	§D12.5	⭐⭐⭐
13	NED↔ENU 轴重排	(y,x,-z) 非取负；错了炸机	§D12.6	⭐⭐⭐⭐
14	Offboard 心跳	>2 Hz 独立线程，掉了被夺权	§D12.6	⭐⭐⭐⭐
15	控制权分配	留多少给 PX4（位置环 vs 仅角速度环）	§D12.6	⭐⭐⭐⭐
16	FSM 防死锁	每状态都有超时退出路径	§D12.7	⭐⭐⭐
17	抢占式异常	电池/通信/故障在 switch 前无条件查	§D12.7	⭐⭐⭐
18	先测量后优化	timeline 甘特图定位瓶颈	§D12.8	⭐⭐⭐
19	实时性优先	宁可次优不可超时；优化器设迭代上限	§D12.8	⭐⭐⭐
20	渐进真机迁移	地面→拴绳→限速→放开，绝不跳级	§D12.9	⭐⭐⭐⭐

累积项目：完整自主探索无人机（毕业项目）¶

项目定位：这是无人机方向累积项目的**终章**。前面每一章你都给项目加了一个模块（D1 的几何控制器、D3-D5 的轨迹生成器、D6 的地图、D7 的探索器）。D12 把它们全部焊接起来，再补上仿真层、FSM 和部署工程，构成一个完整的、可复现的自主探索无人机系统。

项目目录结构¶

mini_drone_ws/
├── src/
│   ├── drone_bringup/          # launch 文件、配置、参数（系统总入口）
│   │   ├── launch/
│   │   │   ├── sim_bringup.launch.py      # §D12.1 仿真层
│   │   │   ├── perception.launch.py       # §D12.2 感知层
│   │   │   ├── mapping.launch.py          # §D12.3 地图层
│   │   │   └── full_system.launch.py      # 全系统（含 sim:=true/false 切换）
│   │   └── config/                        # 各层 yaml 参数
│   ├── fast_lio/               # §D12.2 SLAM（复用 SLAM 主线）
│   ├── rog_map/                # §D12.3 地图（D6 模块）
│   ├── frontier_explorer/      # §D12.4 探索（D7 模块）
│   ├── traj_planner/           # §D12.5 轨迹（D3-D5 模块）
│   ├── geometric_controller/   # §D12.6 控制（D1 模块）
│   ├── offboard_bridge/        # §D12.6 PX4 接口 + 坐标转换
│   └── exploration_fsm/        # §D12.7 FSM 调度
├── docs/                       # §D12.9 技术文档（架构图、参数表、性能报告）
└── README.md                   # 复现说明

本章新增模块（相对前面章节的累积）¶

模块	来源	D12 新增的工作
仿真层	新增	Gazebo+PX4 SITL 配置、传感器 SDF、桥接
感知层	SLAM 主线	TF 树搭建、时间同步、frame 重映射
地图层	D6	接入 SLAM 输出、坐标系对齐
探索层	D7	接入地图、握手逻辑、yaw 规划
轨迹层	D3-D5	重规划主循环、起点接力、走廊对接
控制层	D1	坐标转换层、Offboard 心跳、PX4 接口（全新工程）
FSM	新增	状态机、三类异常处理、防死锁
部署	新增	checklist、failsafe、渐进测试、rosbag

验收标准（对应骨架三路线）¶

指标	α（入门）	β（标准）	γ（进阶）
体积覆盖率	>80%	>90%	>90%
平均飞行速度	>1 m/s	>3 m/s	>10 m/s
规划频率	>1 Hz	>5 Hz	>10 Hz
碰撞次数	≤3	0	0
探索时间（标准场景）	<300 s	<180 s	<120 s
技术文档	架构图	架构图+数据流	完整报告（含性能分析）

技术文档要求（§D12.9 产出）¶

毕业项目必须产出一份可复现的技术文档（2-3 页起），包含：

系统架构图：7 层 + 接缝（draw.io 或本章的数据流图风格）
ROS2 计算图：rqt_graph 导出的节点-话题连接图
TF 树图：view_frames 生成的 frames.pdf
关键参数表：各层的关键配置项及取值依据
性能指标表：实测的 SLAM 延迟/规划频率/控制频率/覆盖率
timeline 甘特图：一个控制周期内各模块的时间占用
遇到的问题及解决方案：你踩过的接缝坑和怎么填的（最有价值的部分）

延伸阅读¶

资源	类型	难度	说明
PX4 ROS2 User Guide（docs.px4.io/main/en/ros2/）	官方文档	⭐⭐	uXRCE-DDS、Offboard、消息定义的权威来源
PX4 Offboard Control Example	官方示例	⭐⭐	OffboardControlMode 心跳、setpoint 的标准写法
MAVROS Wiki	官方文档	⭐⭐	MAVROS 话题、坐标转换、TIMESYNC 配置
FAST_LIO ROS2 分支（hku-mars/FAST_LIO）	开源代码	⭐⭐⭐	LiDAR-惯性里程计的生产级实现
ROG-Map（hku-mars/ROG-Map, IROS 2024）	论文+代码	⭐⭐⭐	机器人中心滑窗占据图，50 Hz 大场景建图
ego-planner / ego-planner-swarm（ZJU-FAST-Lab）	开源代码	⭐⭐⭐	B 样条在线重规划，机载部署验证
GCOPTER（ZJU-FAST-Lab）	开源代码	⭐⭐⭐⭐	MINCO 时空联合优化 + 安全走廊
FUEL（ZJU-FAST-Lab, RA-L 2021）	论文+代码	⭐⭐⭐⭐	前沿聚类 + ATSP 全局探索
aerial_navigation_development_environment（CMU）	开源框架	⭐⭐⭐	系统级的自主飞行开发与部署环境
REP-103 / REP-105（ROS 坐标系/TF 规范）	规范	⭐⭐	TF 树两段式设计、坐标系约定的官方依据
ROS 2 时间同步与 use_sim_time 文档	官方文档	⭐⭐	仿真时间、时钟桥接的权威说明

本章与前序章节的关系¶

D12 是综合验收，与几乎所有前序章节都有直接的复用关系：

前序章节	本章如何复用	复用的核心知识点
D1 微分平坦/几何控制	§D12.6 控制层	微分平坦逆映射、SE(3) 控制器、推力/角速度公式
D2 MPC/自适应	§D12.6 路线 γ 选配	NMPC 作为高速控制器替代
D3 多项式轨迹	§D12.5 路线 α	min-snap 轨迹生成
D4 B 样条	§D12.5 路线 β	EGO-Planner 在线重规划
D5 MINCO/走廊	§D12.5 路线 γ	GCOPTER 时空优化、安全走廊
D6 环境表示	§D12.3 地图层	占据栅格、ESDF、ROG-Map/voxblox
D7 感知探索	§D12.4 探索层	前沿、Yamauchi/FUEL、yaw 规划
D8 敏捷飞行平台	§D12.6/§D12.9	DFBC/INDI、推重比与轨迹约束、sim-to-real
D9 RL 敏捷飞行	§D12.9 真机迁移	域随机化、sim-to-real gap 处置
v8 Ch31 ROS2 高级	全章	节点/话题/TF/launch/生命周期
SLAM 主线 FAST-LIO2	§D12.2 感知层	直接复用为感知后端

这是无人机方向的终点，也是一个新起点：搭完模块化栈后，你具备了理解和调试任何更高级架构（学习+优化混合、端到端策略）的地基。导读里讲的"2026 年六层混合架构"，现在你有能力逐层去实现和评估了。

🔧 故障排查手册¶

下表针对自主飞行栈集成中最高频的现场故障，给出"症状 → 可能原因 → 排查步骤 → 相关章节"的结构化诊断流程。本章是工程实践教学，故障排查重点覆盖**环境配置、版本兼容和接缝问题**。

场景一：ROS2 里看不到任何 `/fmu/*` 话题¶

项	内容
症状	PX4 SITL 起来了，但 `ros2 topic list` 里没有任何 `/fmu/out/` 或 `/fmu/in/` 话题
可能原因	(1) 没启动 uXRCE-DDS Agent；(2) Agent 端口和 PX4 配置不匹配；(3) px4_msgs 版本与固件不符
排查步骤	① 确认 Agent 在跑（`MicroXRCEAgent udp4 -p 8888`），看是否有 client 连接日志；② 检查 PX4 的 `uxrce_dds_client` 是否启动（pxh shell 输入 `uxrce_dds_client status`）；③ 核对 Agent 端口（8888）和 PX4 的 `XRCE_DDS_PRT` 参数一致；④ 确认 px4_msgs 分支与固件版本一致（release/1.14 对 v1.14）
相关章节	环境配置（通信路线）、Quick Start、§D12.6

场景二：话题有了但内容全是 0 或字段缺失¶

项	内容
症状	`/fmu/out/vehicle_local_position` 能 echo，但数值全 0，或报字段不存在
可能原因	px4_msgs 版本与 PX4 固件版本不匹配，消息定义对不上
排查步骤	① 核对 px4_msgs 的 git 分支（`git -C src/px4_msgs branch`）是否为 release/1.XX 且与固件一致；② 重新 `colcon build` px4_msgs；③ 检查是否混用了不同版本的 px4_msgs（工作空间里有多份）
相关章节	环境配置（版本兼容表，px4_msgs 陷阱）

场景三：TF 树有断裂，规划/地图报 LookupException¶

项	内容
症状	`view_frames` 显示 TF 树断成两截，地图/规划节点报 `Could not find a connection between 'map' and 'lidar'`
可能原因	(1) 某段变换没人发布（如 map→odom 或静态外参）；(2) frame 命名不一致；(3) 时间戳问题导致变换"过期"
排查步骤	① `view_frames` 看缺哪一段；② 确认 SLAM 发布了 odom→base_link、静态发布器发布了 base_link→lidar、map→odom 有人顶替；③ `ros2 topic echo /tf` 看 frame 名是否和约定一致；④ 检查 use_sim_time 是否全部一致
相关章节	§D12.2（TF 树两段式、frame 重映射）

场景四：FAST-LIO2 里程计起飞就发散¶

项	内容
症状	静止时 /odom 正常，一起飞（有振动）里程计就乱跳、发散，地图扭曲
可能原因	(1) `scan_line` 与雷达实际线数不符；(2) IMU 噪声协方差设得不合理；(3) LiDAR-IMU 外参错；(4) 真机振动激发未建模噪声
排查步骤	① 核对 config 的 `scan_line` 等于雷达物理线数；② 检查 `extrinsic_T/R` 与 TF/SDF 三者一致；③ 真机按 IMU datasheet 重设 acc_cov/gyr_cov；④ 检查 IMU 是否减震安装；⑤ 用静止数据验证零偏估计
相关章节	§D12.2（FAST-LIO2 参数、scan_line 陷阱）、§D12.9（真机振动）

场景五：一切 Offboard 无人机朝错误方向猛冲¶

项	内容
症状	切 Offboard 瞬间无人机朝意外方向（侧飞/向下）猛冲，仿真和/或真机
可能原因	(1) ENU→NED 坐标转换错（写成取负而非轴重排）；(2) yaw 转换漏了或方向错；(3) 真机传感器朝向/外参与假设不符
排查步骤	① 检查 enuToNed 是否为 `(y, x, -z)` 而非 `(-x,-y,-z)`；② 确认 yaw 转换 `π/2 - yaw_enu`；③ 地面给小位移指令验证六个方向（§D12.9 的 S5）；④ 用 MAVROS 排除（它已翻转）对比定位是否转换问题
相关章节	§D12.6（NED↔ENU 轴重排，炸机级陷阱）、§D12.9（S5 地面验证）

场景六：飞行中无人机突然"夺权"切出 Offboard¶

项	内容
症状	自主飞行中无人机突然自己悬停/降落，切出 Offboard，无明显报错
可能原因	(1) OffboardControlMode 心跳频率掉到 2 Hz 以下（控制线程卡顿）；(2) 触发了某个 failsafe（电池/地理围栏/RC）；(3) EKF2 估计失效
排查步骤	① `ros2 topic hz /fmu/in/offboard_control_mode` 看心跳是否稳定 >2 Hz；② 检查心跳是否在独立线程（不被主控卡顿影响）；③ 查 `/fmu/out/vehicle_status` 的 failsafe 标志和 nav_state；④ 用 rosbag 回放定位夺权时刻的前因
相关章节	§D12.6（Offboard 心跳，炸机级陷阱）、§D12.8（控制掉频）、§D12.9（rosbag 排障）

场景七：规划频率上不去/系统整体卡顿¶

项	内容
症状	规划频率远低于目标（如只有 2 Hz），系统反应迟钝，控制有 jitter
可能原因	(1) 点云没降采样/地图分辨率过高；(2) 单线程 executor 串行阻塞；(3) 热路径有日志 IO 或堆分配；(4) 优化器没设迭代上限偶发慢收敛
排查步骤	① 用 steady_clock 打点画 timeline 定位最慢环节；② 检查 executor 是否多线程、回调组是否隔离；③ 排查控制/规划热路径的 RCLCPP_INFO 和动态 Eigen 分配；④ 给 L-BFGS/QP 设 max_iter；⑤ 降采样点云、降地图分辨率
相关章节	§D12.8（timeline、热路径 IO、单线程 executor 陷阱）

场景八：FSM 卡死，无人机悬停不动也不返航¶

项	内容
症状	无人机悬停在原地，FSM 卡在某状态（常是 EXPLORING），既不探索也不返航
可能原因	(1) EXPLORING 等"规划成功"但目标不可达，无超时退出；(2) 状态转换条件不完备；(3) 探索层没提供回退候选
排查步骤	① 看 FSM 状态日志卡在哪个状态；② 检查该状态是否有"无论如何都能转出"的超时路径；③ 确认规划连续失败有计数+拉黑+换目标逻辑；④ 验证探索层返回的是候选列表而非单一目标
相关章节	§D12.7（FSM 防死锁、转换完备性）、§D12.4（探索-规划握手）

场景九：仿真完美真机发散（增益/sim-to-real）¶

项	内容
症状	仿真里系统跑得完美，真机上控制震荡或跟不上
可能原因	(1) 控制增益从仿真直接搬（真机有延迟/柔性）；(2) 真机传感器噪声远大于仿真；(3) `use_sim_time` 真机忘了关
排查步骤	① 真机增益从仿真值 50-70% 起，拴绳渐进加大；② 确认所有节点真机 use_sim_time=false；③ 仿真加噪声重新调参；④ 对照 D9 SimpleFlight 五因素核查；⑤ 严格走 §D12.9 渐进测试流程
相关章节	§D12.9（真机迁移、增益渐进、use_sim_time 陷阱）、§D12.1（仿真加噪声）、D9

版本信息速查¶

本章涉及的所有工具/库/框架的版本号汇总（与环境配置指南的版本兼容表对应）。搭栈前先对照此表锁定版本，可避免绝大多数依赖地狱问题。

组件	本章使用版本	说明
Ubuntu	22.04 LTS	对应 ROS2 Humble
ROS2	Humble Hawksbill	LTS，支持到 2027
PX4-Autopilot	v1.14.0	uXRCE-DDS 取代 microRTPS 的版本
Micro-XRCE-DDS-Agent	v2.4.2	须与 PX4 内置 client 匹配
px4_msgs	release/1.14	必须与 PX4 固件版本同分支
MAVROS	2.x (ros-humble-mavros)	路线 α 的快速接口
Gazebo	Garden / Harmonic (gz-sim)	PX4 v1.14+ 默认仿真器
Eigen	3.4.0	header-only，与 SLAM 主线一致
PCL	1.12	随 Ubuntu 22.04
FAST_LIO (ROS2)	ROS2 分支	hku-mars/FAST_LIO 或 Ericsii/FAST_LIO_ROS2
ROG-Map	master	hku-mars/ROG-Map
ego-planner	master	ZJU-FAST-Lab（B 样条路线）
GCOPTER	master	ZJU-FAST-Lab（MINCO 路线）
Livox-SDK2	1.2.x	仅实机 Livox 雷达
QGroundControl	最新稳定版	地面站、参数配置、failsafe 设置

版本锁定建议：PX4 固件、px4_msgs、Micro-XRCE-DDS-Agent 三者**精确锁定**（它们之间消息定义强耦合）；ROS2 锁定**大版本**（Humble）；SLAM/规划开源库锁定到**特定 commit**（这些库迭代快，master 可能引入不兼容改动）。把版本写进 README 和 .repos 文件，是技术文档可复现性的基础。