路径跟踪与闭环飞行

本文介绍空域载具八叉树三维寻路项目中的闭环环节：把前面几节规划出的路径 逐点交给控制器，让载具真的飞过去。

对应总纲第 7 节。

1. 运行节点与话题关系

本节新增一个节点 path_follower，其余节点沿用前几节。 roslaunch octree_uav_3d_pathfinding full_demo.launch 会启动下图除模拟器以外的全部节点：

1.1 各节点的职责

1.2 话题

2. 需要先解决的一个结构性问题

/uav/goal 被两个节点同时订阅：规划器订阅它来规划，桥接节点订阅它来飞。 如果跟踪节点也往 /uav/goal 逐点发航点，就会出现：

path_follower 发航点 → /uav/goal → astar_planner 重新规划 → 发布新路径
                                → airsim_bridge 飞向该航点
path_follower 收到新路径 → 从第 0 点重新开始 → 又发航点 → …

每次重规划都要跑一遍 A*，路径还会因为起点变化而抖动，载具飞不出去。

解决办法是把两种语义拆成两个话题：

实现上只改了桥接节点的一处 —— 话题名从硬编码改成参数，默认值保持 /uav/goal：

self.goal_topic = rospy.get_param('~goal_topic', '/uav/goal')
rospy.Subscriber(self.goal_topic, Point, self.goal_cb, queue_size=1)

full_demo.launch 把它设成 /uav/goal_cmd。默认值不变，前三节启动方式与行为都不受影响。

3. 状态机

跟踪逻辑全部放在 include/octree_uav_3d_pathfinding/path_follow_core.h， 是不依赖 ROS 的纯函数，因此可以离线单测；src/path_follower.cpp 只做消息转换与发布。

        ┌──────────────────────────────────────────┐
        │                                          │
   ┌────▼────┐  收到路径(pts>=2)  ┌───────────┐   │
   │  IDLE   ├──────────────────►│ FOLLOWING │   │
   └─────────┘                   └─────┬─────┘   │ 收到新路径：
        ▲                              │         │ 复位到新路径
        │                  到达终点     │         │（不是重规划）
        │                              ▼         │
        └───────────────────────── ┌─────────┐   │
                                   │  DONE   │───┘
                                   └─────────┘

为什么收到新路径要从头开始：规划器每规划一次都会发一条新路径（latched）， 旧路径可能已经因为地图更新而失效，继续按旧下标追没有意义。

3.1 判定规则

dist = ‖当前位置 − path[index]‖

若 index 是最后一个点：
    若 dist < finish_tolerance  → 到达，转 DONE
否则若 dist < lookahead         → index++，下发 path[index]
否则                            → 不动

若 距上次前进 > per_point_timeout：
    若 index 是最后一个点      → 直接结束（不跳过，否则永远到不了终点）
    否则                       → index++，计入 skipped，下发新航点

三个阈值的关系是本节的关键：

4. 两个取舍

4.1 前瞻跳点，而不是停稳再走

桥接节点只有位置控制（moveToPositionAsync：飞到一个点并减速停住）， 没有速度或轨迹跟踪接口。所以"连续飞行"只能靠提前改写目标实现：

代价：实际轨迹会比规划路径切内角（没到点就转向）。 这个代价是量化的 —— 节点统计 max_dev：实际轨迹到规划折线的最大距离。 注意是"到线段"而不是"到航点"：航点稀疏，到航点的距离会把正常切内角误判成大幅偏离。

这个做法依赖一个前提：后发的目标覆盖先发的目标，而不是排队执行。 这一点在 AirSim 1.8.1 上实测确认过（2026-10-26）：先发 A(60, 0, 8)，在载具 仍在飞往 A 的途中改发 B(−60, 0, 8)，记录 /ground_truth/odom 得到的距离曲线是：

到 A 的距离在 41.28 m 处由减小转为增大，同期到 B 的距离单调收敛 —— 载具 没有飞到 A 就掉头了。所以 moveToPositionAsync 是覆盖语义，前瞻跳点成立。 若换成排队语义，这套设计就得改成"每个航点停稳后再发下一个"，max_dev 也会 退化成接近 0（没有切内角），代价是每段都要启停。

4.2 不做飞行中重规划

飞行中地图持续生长，每张新地图都可能产出不同路径，跟着换会让载具抖动； 真正的重规划还需要"路径变化超过阈值才换"的一致性判据。本节只在收到新路径时复位， 不做主动重规划 —— 见 §7。

5. 参数

config/params.yaml 的 path_follower 组：

airsim_bridge 组新增：

飞行速度仍由既有参数 goal_velocity（默认 3.0 m/s）决定。

6. 运行方法

# 编译
cd ~/uav_ws && catkin_make
source ~/uav_ws/devel/setup.bash

# 闭环飞行：完整链路（桥接 + 建图 + 规划 + 跟踪 + 自检 + RViz）
roslaunch octree_uav_3d_pathfinding full_demo.launch

6.1 用 mock 场景离线验证（不起模拟器）

bridge:=false 只是不启动桥接节点，它不提供点云与位姿。所以离线验证要另外起两个节点， 各占一个终端；否则没有 /cloud_in、没有 /ground_truth/odom，规划器和跟踪节点都不会动：

# 终端 1：roscore
roscore

# 终端 2：点云（巡游 40 秒填满地图，之后停在 (-7.5, 7.5)）
rosrun octree_uav_3d_pathfinding mock_cloud_publisher.py

# 终端 3：假载具（位姿来源）。起点对齐路径起点，避免多一段"飞向起点"
rosrun octree_uav_3d_pathfinding mock_uav_mover.py \
    _start_x:=-7.5 _start_y:=7.5 _start_z:=0.0

# 终端 4：链路本身
roslaunch octree_uav_3d_pathfinding full_demo.launch \
    bridge:=false check:=false body_frame:=lidar_link

若不传 _start_x/_start_y，假载具从 (0, 0, 0) 起飞，会先飞 22.5 m 到路径起点。 那段"接近段"不计入 flown 与 duration（见下表的 approach 字段），但会拉长总耗时。

6.2 下发目标点

飞行器起飞、扫描一段时间地图建起来之后，下发一次目标点即可，后续由跟踪节点接管：

rostopic pub -1 /uav/goal geometry_msgs/Point "{x: 7.5, y: -7.5, z: 2.0}"

到达终点后节点会打印统计：

follow: DONE | waypoints=73 reached=73 skipped=0 final_timed_out=0 | path=33.52 m flown=31.95 m
        approach=0.00 m/0.00s | duration=10.65s | mean_speed=3.00 m/s | max_dev=0.41 m
  corner cutting: flown is 4.7% shorter than the path

两个口径必须分清：

• mean_speed 的分子是 flown 而不是 path。前瞻跳点会切内角，实际路程比规划路径短； 用 path / duration 算出来的是"路径推进速率"，会超过 goal_velocity （实测出现过 37.22 / 10.90 = 3.41 m/s，而设定速度是 3.0 m/s，物理上不可能）。
• max_dev 只统计贴上路径之后的轨迹。index = 0 表示载具还在飞向路径起点， 那一段是"接近段"，本来就不在路径上；计入的话，一次从上一趟终点飞回新起点的 22.5 m 直线会把 max_dev 拉到 5 m 以上，指标就失去意义。

reached 的口径是 waypoints - skipped - final_timed_out：终点若是超时收尾 （没进 finish_tolerance），它不再算作到达，日志里也会把 final_timed_out 标成 1。

7. 验证

7.1 离线单元测试

tests/test_path_follower.cpp，只依赖 STL —— 不需要 ROS 运行时、不需要 octomap、不需要模拟器。 被测对象是 path_follow_core.h 里的纯函数。实测 59 项检查全部通过。

运行方式：

~/uav_ws/devel/lib/octree_uav_3d_pathfinding/test_path_follower
# 退出码 0 = 全部通过

7.2 离线整链路（mock 场景）

场景：scripts/mock_cloud_publisher.py 造的 20×20×6 m 房间 + 5 个障碍（两道错开的墙 + 三根柱子）， 传感器巡游 40 秒填满地图后停在 (-7.5, 7.5)。目标点取其对角 (7.5, -7.5, 2.0)。

位姿由 scripts/mock_uav_mover.py 提供 —— 它是只有运动学的桩：匀速直线、到达即停、 不做碰撞判定。它替代的是桥接节点的位姿发布，不是飞行物理，所以这组数据只反映跟踪逻辑， 不反映真实飞行品质。

测量条件：

起点必须对齐：假载具默认起点是 (0, 0, 0)，此时它会先飞 22.5 m 到路径起点。 日志里 approach 字段单独给出这段，flown 与 duration 都只算跟踪阶段。

航点数取决于样条是否通过复检

同一套参数跑出来的结果分两种形态，差别不在跟踪逻辑，而在 P5 的平滑结果：

样条是拟合，必须穿过每个航点，拐角处的航点贴在安全边界上时会被复检拒绝 （见 P5 文档 §6.3）。它是否通过取决于地图几何的细节，因此不同次的运行可能落在不同形态。

所以下面把两种形态的数据都列出来，并只用形态内部的数字做比较。

稳定的是判定性的量：skipped = 0（全部到达）、mean_speed 正好等于 goal_velocity、 max_dev 小于 lookahead。这三条在两种形态、多次运行里都成立。

浮动的是路径形态相关的量：航点数、path、max_dev。它们由"样条是否通过复检" 和"地图自由叶数"（实测 17945 与 19366）共同决定，复现时不必追求逐位相同 —— 按同形态的量对比即可。

max_dev 与航点密度直接相关：73 点的样条路径几乎逐点走到了，偏离 0.41 m； 10 点的折线只剩拐角，前瞻跳点会斜穿，偏离升到 1.25 m。

跟折线 vs 跟曲线

同一场景、同一目标点，只改 path_topic。下表是另一次运行的对照 （同一张地图、同一个规划结果的两条路径；与该次运行同属"样条被拒绝"的形态， 见上面的两种形态表）：

方向与预期一致：跟折线时 max_dev 更小（航点密，切角少），路径略长、耗时略久。

但这组对照低估了平滑的收益，原因在测量手段：mock_uav_mover.py 只有运动学 （匀速直线、无加减速、无转向耗时），所以"48 次转向"和"11 次转向"对它几乎没有区别 —— 耗时只差 0.35 s。真实载具有加速度与姿态响应，转向次数直接决定耗时与轨迹抖动， 差距会明显大于这里的读数。这组数据能说明"max_dev 与航点密度的关系"， 不能用来量化"平滑在真实飞行中省多少时间"。

7.3 闭环飞行动图

下面这段录自 §7.2 里"样条被复检拒绝、11 航点"的那次运行（不是 73 点那组）， 属于离线场景（mock 点云 + 运动学假载具），不是 AirSim 仿真：

能连续看到：地图建满 → 绿色/青色路径出现 → 黄色轨迹边飞边长 → 到达终点。

黄线与青线的间距就是该次运行的 max_dev（1.26 m）的直观形态： 前瞻跳点斜穿拐角，轨迹因此偏在路径内侧。

AirSim 仿真与离线 mock 场景的差别在于动力学：假载具没有加减速与转向耗时， 所以上面这段只能说明跟踪逻辑跑通，不能说明真实飞行品质。带动力学的仿真见 §7.4。

7.4 AirSim 上的闭环飞行

2026-10-27 在 AirSim（OpenHUTB 发行版，Town10HD 场景）上跑通完整闭环： 桥接 → 建图 → A* → 安全裕度与平滑 → 跟踪 → 飞行。飞机从 (-31.65, -8.55, 7.95) 飞到 (-45.15, 1.35, 7.65)，两点直线 16.74 m。

这份动图的视角偏远（录制时 RViz 的 Distance 约 120 m），路径只占画面中部的一小块， 点云与展开节点也在同一屏里，读图时需要先找到那条折线。录制时把 SensorCloud 与 AStarExplored 关掉、Distance 收到 40~60 m、路径线宽加到 0.08 m，绕行会更清楚。 这一版保留的是首次录制的原始视角，没有为了好看而重录。

三点需要如实说明：

1. 12.30 s 短于 §8 第 5 条建议的 15~25 s。 这次两点直线只有 16.74 m，行程长度是靠 绕行（1.67 倍）撑起来的。要更长就得有更大的已扫描范围，而"未知 = 障碍" 把可规划区域限制在雷达量程以内。
2. 样条又一次被复检拒绝，与 §7.2 里第二种形态同因：仿真里的地图比 mock 场景更稀疏， 平滑曲线更容易擦到障碍。退回折线是复检在起作用的证据，不是平滑失败。
3. 细枝、细杆进不了八叉树（0.3 m 分辨率 + 稀疏采样），规划器会把它们当成可通行。 这一轮实测里载具撞到过树枝，靠物理引擎自行脱困，但轨迹会短暂偏离。 这是感知分辨率与碰撞模型之间的固有缺口，见 §8 第 6 条。

8. 实现中要注意的几点

9. 进一步工作

10. 参考

• nav_msgs/Path 消息定义
• nav_msgs/Odometry 消息定义
• AirSim 多旋翼 API 文档（moveToPositionAsync 等）
• 模块源码：src/air/octree_uav_3d_pathfinding/

查看实际运行的节点与话题关系（第 1 节的图是静态版本）：

rosrun rqt_graph rqt_graph        # 节点—话题图
rostopic info /uav/goal           # 确认真实订阅者只有规划器
rostopic info /uav/goal_cmd       # 确认真实订阅者只有桥接节点
rosrun rqt_tf_tree rqt_tf_tree    # TF 树