【Scan-Planner源码解读（上篇）】双圆柱碰撞检测 + Project-A* 绕障：SCAN 避障链路逐行拆

m0_73800387

295人浏览 · 2026-08-17 16:36:17

m0_73800387 · 2026-08-17 16:36:17 发布

前言

最近我们解读了不少关于3D世界的路径规划器
- 全局路径规划器：
  - 【基于OctoMap与地面约束的三维A*路径规划方法实现】26邻域查找与八叉树离散化映射实现
  - 【PCT-Planner 源码解读】从点云断层扫描到 GPMP2 轨迹优化 — 公式与代码逐行对照
- 局部路径规划器：
  - 【10天速通ROS2-PX4无人机】(三) EGO-Planner 自主避障飞行，飞机终于会自己找路了
- 自主探索规划器：
  - 【FUEL 源码解读】未知环境自主探索 — 从 Frontier 到 B-spline 的公式与代码逐行对照
之前我们解读的局部规划器 EGO-Planner，是给无人机设计的——它的机器人模型是一个球，碰撞检测和地图膨胀都是按「单球体」来做的
但机器人不是只有无人机，还有四足机器人。四足的身体是一个长长的、有头有尾的长条形，走路还要分前后，用「一个圆」去描述它，怎么看怎么别扭
刚好，2026 年出来了一篇专门解决这个问题的论文：Spatial Collision-Aware Local Planning for Route-Guided Long-Range Quadruped Navigation（空间碰撞感知的、路由引导的长距离四足导航局部规划）
它背后的系统叫 SCAN-Planner，本质上是在 EGO-Planner 的框架上，把「球」换成了双圆柱体，还加入了一条全局参考路由来引导局部规划器——这就是本期的主角
代码仓库：https://github.com/wuyi2121/SCAN-Planner
论文：Spatial Collision-Aware Local Planning for Route-Guided Long-Range Quadruped Navigation（arXiv 2026）
老规矩，我们仍然是「先讲直觉、再上公式、最后逐行对代码」，把这篇论文里最硬核的三块——双圆柱体空间碰撞模型、滑动窗口占据栅格地图、B-spline 轨迹优化——讲深讲透

目录（本文为上篇，覆盖第 1-5 章）

文章目录

1 局部规划器

1-1 常用算法

请添加图片描述

在讲 SCAN-Planner 之前，我们还是先花一小节，把「局部路径规划器」的生态捋一遍，看看它是站在谁的肩上
上一期解读 PCT-Planner 时我们说过：
- 全局规划器拿着完整先验地图，算一条从 A 到 B 的大致路线；
- 局部规划器则边走边看，用传感器实时感知的信息，在毫秒级的时间内高频规划出可以执行的轨迹
局部规划器的常用算法，主流可以分成「反应式」、「图/优化式」、「采样式」三大类：

类别	代表算法	核心思想	优点	缺点
反应式	DWA	直接对速度空间 (v, w) 采样，打分选最优	简单、快、鲁棒	短视，容易陷入局部最小值
图/优化式	TEB	把轨迹离散成位姿序列，用图优化求平滑	轨迹平滑、可加约束	参数多，高维时慢
优化式	EGO-Planner	B-spline 控制点 + 梯度优化	平滑、安全、无 ESDF 依赖	依赖好初值，可能陷入局部极值
采样/MPC	MPPI、MPC	随机采样候选轨迹或滚动优化	能处理非线性动力学	计算量大，需调协方差

本期的主角 SCAN-Planner 属于 优化式，而且它的骨架几乎就是 EGO-Planner 本尊：B-spline 表示轨迹 + L-BFGS 优化 + A* 绕障反弹

1-2 之前的机器人膨胀

请添加图片描述

机器人在栅格地图里做碰撞检测，不会真的拿它复杂的几何外形去逐点比对——那太慢了
工程上的标准做法是：先把障碍物按机器人半径膨胀一圈，然后把机器人当成一个「点」来查碰撞。这一步叫膨胀（Inflation）
我们用公式表达一下这个过程：假设机器人可以被一个半径为 $r$ 的圆（二维）或球（三维）包住，那么对地图里的任意一个被占据的体素 $g$ ，我们要把它周围半径 $r$ 以内的体素全部标记为「膨胀占用」：
$\forall\, g_o \in \mathcal{O}:\quad g\ \text{is inflated} \iff \|g - g_o\|_2 < r$
这样碰撞检测就从「图形 vs 图形」降维成了「点 vs 查询表」，一次 getOccupancy 就完成了
EGO-Planner 就是这么做的，它的参数里有一个 robot_radius，把地图按这个半径膨胀，然后整个规划全程把无人机当成一个质点来处理
我们在 SCAN-Planner 的源码里也能看到同样的思路，只是它把「一个半径」换成了「半径 + 双中心」，这个我们后面 3-4、3-5 节重点讲

说人话：膨胀就是在障碍物周围画一圈半径等于机器人半径的安全区。安全区画大了，机器人离障碍远远的（安全但无法通过窄缝）；画小了，机器人会擦到障碍（灵活但危险）。

1-3 之前算法的问题？

单球/单圆膨胀对无人机非常合适，因为无人机就是近似轴对称的，前后左右都一样
但四足机器人（比如宇树 Go2）的身体是长条形的，大概有 0.7 m 长、0.35 m 宽，而且它有明确的前后之分：
- 单圆半径取小（按宽度）：前脚后脚都会伸出圆外，转弯或贴边时直接蹭到墙
- 单圆半径取大（按长度）：中间的腰身被过度膨胀，一条 0.5 m 宽的走廊本来钻得过去，结果被「大圆」挡住死活过不去
更麻烦的是，单圆膨胀是旋转不变的——它根本不知道机器人面朝哪里。而四足行走时，前身、后身扫过的空间是随 yaw 角变化的
还有一点：四足要上楼梯、过窄门，头顶和脚下各有一段净空（obstacles_inflation_z_up/down），这在球形模型里也只能用一个固定高度带近似
于是论文提出了核心问题：能不能让碰撞模型「认得机器人的朝向」？能不能用两个小圆柱去贴合四足的真实外形？

说人话：之前的算法把机器人当成一个固定半径的球——对无人机正合适，对长条形的四足却要么包不住头尾、要么把腰身撑得过宽，而且不随朝向变化。SCAN-Planner 改用两个贴合机身前后部的小圆柱，半径、间距都按四足身形配置，并随航向角一起旋转。

2 SCAN-Planner 总览

请添加图片描述

2-1 介绍

SCAN-Planner 的全称是 Spatial Collision-Aware Local Planning，空间碰撞感知局部规划，目标是解决四足机器人（quadruped）在长距离、多楼层环境里的局部导航问题
它的承继关系非常清晰：底层沿用 EGO-Planner 那一套「B-spline 表示轨迹 + L-BFGS 优化 + A* 绕障」的优化式局部规划框架，只把最关键的碰撞模型换掉了——论文（arXiv 2606.19555）用双圆柱体取代 EGO 的单球，并用一条全局参考轨迹引导局部规划，使四足在长距离走廊、窄门、楼梯场景下安全高效地导航

说人话：SCAN-Planner 没去重造局部规划的轮子，而是把 EGO-Planner 的「球」换成了「会随航向转的两个圆柱」——机身是长条的，用两个小圆柱贴合前后，比一个圆准得多。

论文：Spatial Collision-Aware Local Planning for Route-Guided Long-Range Quadruped Navigation
官方仓库：https://github.com/wuyi2121/SCAN-Planner（本文以 ROS1 主分支为准）

2-2 系统架构总览

我们先从一张数据流图看整体，再逐个模块拆解：

拆开看，这个系统由四个层次组成：

层次	模块	文件	作用
感知	`GridMap` 滑动占据栅格	`plan_env/grid_map.cpp`	点云/深度图 → 计数式占据更新 → 双圆柱膨胀
全局引导	`PolynomialTraj` min-snap 全局轨迹	`traj_utils/polynomial_traj.cpp`	路点 → 一条平滑的全局参考轨迹（路由）
局部规划	`SCANReplanFSM` 状态机	`plan_manage/scan_replan_fsm.cpp`	沿路由取局部目标，调度绕障与优化
优化	`BsplineOptimizer` + `AStar`	`bspline_opt` + `path_searching`	B-spline 优化 + A* 反弹绕障

论文的核心创新集中在感知层的地图膨胀和优化层的碰撞代价这两处，其余骨架与 EGO-Planner 一脉相承
所以我们后面的顺序是：先讲地图与膨胀（第 3 章），再讲全局路由（第 4 章）；接着先讲怎么从全局路径取点、连出初始轨迹（第 5 章），再按局部规划的实际处理顺序依次讲 Projected A* 绕障搜索（第 6 章）、B-spline 轨迹优化（第 7 章）与 L-BFGS 优化（第 8 章），最后收束到调度这一切的局部重规划状态机 FSM（第 9 章）

2-3 三种导航模式

论文里 route-guided（路由引导）有三种落地方式，对应 fsm/navi_mode 这个参数：

navi_mode	名称	目标来源	适用场景
1	MANUAL_TARGET	RViz 手动点击 `/move_base_simple/goal`	单目标点
2	PRESET_TARGET	`tools/keypoint.yaml` 预设路点序列	多楼层、多路点巡航
3	REFERENCE_PATH	订阅 `/initial_path`（全局路径）	上层规划器给出完整路由

源码里对应 scan_replan_fsm.cpp 的初始化：

// 对应 2-3 节：三种导航模式
if (navi_mode_ == NAVI_MODE::MANUAL_TARGET)
  goal_sub_ = nh.subscribe("/move_base_simple/goal", 1, &SCANReplanFSM::rvizGoalCallback, this);   // 模式 1：RViz 点目标
else if (navi_mode_ == NAVI_MODE::PRESET_TARGET)
{
  ...
  planGlobalTrajbyGivenWps();               // 模式 2：预设路点 → 全局轨迹
}
else if (navi_mode_ == NAVI_MODE::REFERENCE_PATH)
  path_sub_ = nh.subscribe("/initial_path", 1, &SCANReplanFSM::pathCallback, this);                // 模式 3：订阅全局路径

无论是哪种模式，最终都会统一变成一条全局参考轨迹 global_data_.global_traj_，局部规划器只负责沿它取目标、避障、规划

2-4 核心贡献：双圆柱体空间碰撞感知

在进入代码之前，我们先把这篇论文的灵魂——双圆柱体模型——用一张图和几个公式讲清楚
假设机器人在位置 $\mathbf{p}$ ，当前航向角为 $\theta$ ，那么它的前进方向（heading）是：
$\mathbf{h}(\theta) = \begin{bmatrix}\cos\theta\\[2pt] \sin\theta\\[2pt] 0\end{bmatrix}$
我们不把整个机身当成一个圆柱，而是用两个小圆柱去包住机身的前部和后部，两个圆柱的中心分别在：
$\mathbf{p}_{\text{front}} = \mathbf{p} + d_c\,\mathbf{h}(\theta), \qquad \mathbf{p}_{\text{rear}} = \mathbf{p} - d_c\,\mathbf{h}(\theta)$
其中 $d_c$ 就是 double_cylinder_offset（默认 0.18 m），两个圆柱半径都是 $double_cylinder_radius R=\text{double\_cylinder\_radius}$ （默认 0.25 m）
于是「位置 $\mathbf{p}$ 、航向 $\theta$ 处是否碰撞」就等价于「前圆柱或后圆柱的膨胀占用至少有一个为真」：
$\mathrm{collide}(\mathbf{p}, \theta) = \mathcal{O}_{\mathrm{inflate}}(\mathbf{p}_{\text{front}}) \;\vee\; \mathcal{O}_{\mathrm{inflate}}(\mathbf{p}_{\text{rear}})$
后面第 3 章我们会看到，这个公式在代码里就对应 GridMap::getInflateOccupancy(pos, yaw)，短短几行，却彻底改变了碰撞模型对四足的适配性
下图直观对比了「单圆」与「双圆柱」的覆盖范围：单圆要么包不住头尾、要么把腰身撑胖，双圆柱则严丝合缝地贴合四足的长条形机身（橙色矩形为机身示意）：

说人话：机器人在栅格地图上不是「一个圆」，而是「机身前端一个圆 + 机身后端一个圆」，两个圆都随航向角旋转。前进时前端的圆顶在最前；原地转弯时两个圆绕着机身中心转。这样腰身窄、头尾宽的真实机身就被精确地描述出来了。

3 滑动窗口占据栅格地图

局部规划器最核心的输入，就是一张实时更新的占据栅格地图。SCAN 的地图直接继承了 ROG-Map 的设计：机器人坐标系 + 滑动窗口 + 计数式占据更新
这一章我们按「地图怎么滑动 → 占据怎么更新 → 障碍怎么膨胀 → 碰撞怎么查询」的顺序，把 grid_map.cpp 从头到尾走一遍

3-1 机器人坐标系滑动窗口

全局地图在长距离导航里会无限增长，局部规划器并不需要——它只需要机器人周围一小块
SCAN 的做法是：固定一块 10 m × 10 m × 5 m 的地图，分辨率 0.05 m，中心始终跟随机器人，机器人走远了就平移重定位（滑动窗口）
地图原点 map_origin_ 记录的是窗口最小角点坐标（初始 $(-x/2,\,-y/2,\,\text{ground})$ ），窗口中心由 map_origin_idx_ 表示（初始在世界原点），尺寸 map_size_ 决定体素总数：

参数	默认值	含义
`sliding_map_size_x/y/z`	10 / 10 / 5	滑动窗口尺寸（m）
`resolution`	0.05	分辨率（m）
`map_sliding_thresh`	0.2	机器人偏离窗口中心超过该距离就滑动
`max_ray_length`	5.0	射光线最大长度（m）

初始化代码在 grid_map.cpp 的 initMap：

// 对应 3-1 节：地图尺寸与分辨率
mp_.map_origin_ = Eigen::Vector3d(-x_size / 2.0, -y_size / 2.0, mp_.ground_height_);
mp_.map_size_   = Eigen::Vector3d(x_size, y_size, z_size);
...
for (int i = 0; i < 3; ++i)
  mp_.map_voxel_num_(i) = ceil(mp_.map_size_(i) / mp_.resolution_);  // 各维体素个数
mp_.map_origin_idx_ = mp_.map_bound_min_idx_ + mp_.map_voxel_num_ / 2; // 窗口中心体素

滑动逻辑在 updateSlidingMap(center)：当机器人偏离中心超过阈值 map_sliding_thresh_vox_ 时，把窗口往新中心平移，只清理滑出窗口的那几层窄条（层数 = 位移量 shift，首次触发通常一次清 4 层），其余体素全部保留（这就是「滑动」而不是「重建」的关键）：

// 对应 3-1 节：滑动窗口平移
const Eigen::Vector3i shift_num = new_origin_idx - mp_.map_origin_idx_;
if (shift_num.cwiseAbs().maxCoeff() < mp_.map_sliding_thresh_vox_)
  return;                                      // 没走远，不滑
...
// 按维度把滑出去的体素标记到 clear_mask，其余全部复用
for (int dim = 0; dim < 3; ++dim) {
  const int shift = shift_num(dim);
  ...
  add_clear_addr(id_l);                        // 记录需要清除的地址
}

说人话：地图是块跟随机器人平移的滑动窗口，机器人往前走 0.2 m，窗口就往前挪 0.2 m，只把身后移出窗口的那一窄条清掉，其余体素全部保留。比起「每次重建整张地图」，既省时间又不丢历史信息。

3-2 计数式占据概率更新

我们曾经在【Navigation2进阶】(十三）：手写概率地图!从log-odds推导到MI互信息探索与Nav2插件实现讲过概率地图的梳理，这里就不进行详细概念说明
占据栅格地图的经典做法是贝叶斯占据概率更新：对每个体素维护一个概率 $p$ ，用 log-odds（对数几率）表示：
$\mathrm{logit}(p) = \ln\frac{p}{1-p}$
每次观测到「命中」（ray end 落在该体素）就加一个正的增量，观测到「未命中」（射线穿过该体素）就加一个负的增量：
$L_{t+1} = \mathrm{clamp}\big(L_t + \Delta L,\; L_{\min},\; L_{\max}\big)$
SCAN 沿用了 ROG-Map 的 计数式（counting）融合：它不逐体素立刻更新，而是先在 count_hit_ / count_hit_and_miss_ 里计数，攒够一轮后批量融合
一次观测里，某个体素既可能被多条射线穿过（miss），又可能被某些射线命中（hit），于是：
$\Delta L = \begin{cases} L_{\text{hit}} , & n_{\text{hit}} \ge n_{\text{miss}} \\[4pt] L_{\text{miss}}, & n_{\text{hit}} < n_{\text{miss}} \end{cases}$
其中 $n_{\text{hit}}$ 是命中计数， $hit_and_miss − n hit n_{\text{miss}}=n_{\text{hit\_and\_miss}} - n_{\text{hit}}$ 是穿过计数。默认参数：

参数	值	logit 值	含义
`p_hit`	0.85	$L_{\text{hit}}\approx+1.73$	命中增量
`p_miss`	0.30	$L_{\text{miss}}\approx-0.85$	穿过增量
`p_occ`	0.80	$L_{\text{occ}}\approx+1.39$	视为「占用」的阈值
`p_min`	0.12	$L_{\min}\approx-1.99$	概率下限
`p_max`	0.98	$L_{\max}\approx+3.89$	概率上限

代码里 initMap 直接把概率转成 log-odds 存下来，后续所有更新都在 log-odds 空间进行：

// 对应 3-2 节：概率 → log-odds 转换
mp_.prob_hit_log_  = logit(mp_.p_hit_);     // +1.73
mp_.prob_miss_log_ = logit(mp_.p_miss_);    // -0.85
mp_.clamp_min_log_ = logit(mp_.p_min_);     // -1.99
mp_.clamp_max_log_ = logit(mp_.p_max_);     // +3.89
mp_.min_occupancy_log_ = logit(mp_.p_occ_); // +1.39

计数阶段在 setCacheOccupancy：命中 count_hit_+1，只要被观测过（无论命中还是穿过）count_hit_and_miss_+1，并把体素塞进 cache_voxel_ 队列：

// 对应 3-2 节：计数缓存
int GridMap::setCacheOccupancy(Eigen::Vector3d pos, int occ) {
  ...
  md_.count_hit_and_miss_[idx_ctns] += 1;    // 只要被观测到就 +1
  if (md_.count_hit_and_miss_[idx_ctns] == 1)
    md_.cache_voxel_.push(id);               // 第一次被观测，入队
  if (occ == 1)
    md_.count_hit_[idx_ctns] += 1;           // 命中才 +1
  return idx_ctns;
}

融合阶段在 raycastProcess 末尾批量执行，先用计数比较决定增量正负，再 clamp 到上下限：

// 对应 3-2 节：批量融合（log-odds 累加 + clamp）
double log_odds_update =
    md_.count_hit_[idx_ctns] >= md_.count_hit_and_miss_[idx_ctns] - md_.count_hit_[idx_ctns]
        ? mp_.prob_hit_log_ : mp_.prob_miss_log_;      // 命中多 → 加 hit，否则加 miss
...
const double new_log_odds =
    std::min(std::max(md_.occupancy_buffer_[idx_ctns] + log_odds_update,
                      mp_.clamp_min_log_), mp_.clamp_max_log_);  // clamp 到 [Lmin, Lmax]
applyOccupancyUpdate(idx, new_log_odds);

说人话：地图更新不是「每来一帧点云就把所有体素重新算一遍」，而是先累计观测（哪些体素被射线穿过、哪些被激光打中），攒一批再统一算。命中多就向「占用」方向累加，穿过多就向「空闲」方向累加，累加结果还限制在上下限里，防止一次误观测让占据概率突变。

下面这张图模拟了一个体素的 log-odds 随观测次数变化的曲线：前 4 次观测命中，log-odds 快速爬升越过 L_occ 判定为占用；后 6 次观测穿过，逐步回落，并始终被 clamp 在 L_min 与 L_max 之间：

3-3 射光线融合

光有端点还不够——激光打在障碍物上，激光和障碍物之间的一切都应该是空闲的，所以必须对每条射线做raycasting，把中间穿过的体素全部记成 miss
SCAN 用的是 RayCaster（3D-DDA 体素遍历），射线起点是雷达位置 ray_pos_，终点是反投影出来的世界点 pt_w：

// 对应 3-3 节：射光线融合
raycaster.setInput(pt_w / mp_.resolution_, md_.ray_pos_ / mp_.resolution_);
while (raycaster.step(ray_pt))
{
  Eigen::Vector3d tmp = (ray_pt + half) * mp_.resolution_;
  length = (tmp - md_.ray_pos_).norm();
  vox_idx = setCacheOccupancy(tmp, 0);     // 射线穿过的体素 → miss 计数
  ...
}

注意两个标志的分工：flag_rayend_ 保证同一终点体素在一帧内只做一次 DDA 遍历——多条射线打到同一个端点体素时，只有第一条执行中间体素的 miss 标记，后续直接 continue（端点体素的命中计数不受影响，照样累加）；flag_traverse_ 保证每条射线穿过体素时一旦遇到已处理体素就提前 break。至于「入队去重」，由 setCacheOccupancy 里的 count_hit_and_miss_ == 1 决定——第一次命中该体素才 cache_voxel_.push(id)
这就是缓存计数能保持高效的原因：一帧点云几百上千条射线，但最终只有有限的一小撮体素会被推入更新队列

3-4 双圆柱体膨胀

现在到了全文的正题——地图膨胀。上一章提到按障碍物尺寸对障碍膨胀（在障碍周围留出安全间距），在 SCAN 里膨胀的半径不是一圈均匀的圆，而是双圆柱
先看膨胀偏移量的生成 rebuildInflationOffsets：XY 平面内，偏移 $(x, y)$ 只要落在半径 $R$ 的圆内（ $double_cylinder_radius = 0.25 R=\text{double\_cylinder\_radius}=0.25$ m）就保留，Z 方向则在 $[-z_{\text{down}}, z_{\text{up}}]$ 内取：

$\underbrace{x^2 + y^2 < R^2}_{\text{XY 平面圆内}}, \qquad z \in [-z_{\text{down}},\; z_{\text{up}}]$

参数	默认值	含义
`double_cylinder_radius`	0.25	单个圆柱的 XY 半径（m）
`double_cylinder_offset`	0.18	前/后圆柱中心离机身中心的距离（m）
`obstacles_inflation_z_up`	0.1	膨胀上界（m）
`obstacles_inflation_z_down`	0.1	膨胀下界（m）
`body_height`	0.4	机身离地高度（m）

下面用一张立体图把「双圆柱模型」和「圆柱膨胀」的关系画出来：机器人被建模成前后两个圆柱（中心偏移 $\pm d_c=0.18$ m、半径 $R = 0.25$ m、高度带 $h_{body}-z_{down},\; h_{body}+z_{up}]$ ），而地图里每个障碍体素也会膨胀成同半径、同高度带的圆柱区域。因为膨胀半径等于圆柱半径，所以碰撞检测只需检查「前圆柱中心」和「后圆柱中心」这两个点有没有落进膨胀区即可：![[Pasted image 20260816112152.png]]
代码：

// 对应 3-4 节：生成膨胀偏移集合
void GridMap::rebuildInflationOffsets() {
  const double double_radius = std::max(0.0, mp_.double_cylinder_radius_);   // R = 0.25
  const int inf_step_xy   = ceil(double_radius / mp_.resolution_);           // XY 步数
  const int inf_step_z_up = ceil(mp_.obstacles_inflation_z_up  / mp_.resolution_);
  const int inf_step_z_down = ceil(mp_.obstacles_inflation_z_down / mp_.resolution_);
  ...
  for (int x = -inf_step_xy; x <= inf_step_xy; ++x)
    for (int y = -inf_step_xy; y <= inf_step_xy; ++y) {
      Eigen::Vector2d offset_xy(x * mp_.resolution_, y * mp_.resolution_);
      if (offset_xy.norm() >= double_radius)
        continue;                                    // 只保留圆内的偏移
      for (int z = -inf_step_z_down; z <= inf_step_z_up; ++z)
        md_.inflate_offsets_.push_back(Eigen::Vector3i(x, y, z));
    }
}

有了偏移集合，当一个体素从空闲变成占用（或相反）时，applyOccupancyUpdate 会把「占用/空闲」增量 ±1 广播到它周围的全部偏移体素，用计数 occupancy_buffer_inflate_cnt_ 记录：

// 对应 3-4 节：状态变化 → 更新膨胀层
void GridMap::applyOccupancyUpdate(const Eigen::Vector3i& id, double new_log_odds) {
  ...
  const bool was_occ = md_.occupancy_buffer_[addr] > mp_.min_occupancy_log_;
  const bool now_occ = new_log_odds > mp_.min_occupancy_log_;
  md_.occupancy_buffer_[addr] = new_log_odds;
  if (was_occ != now_occ)
    updateInflation(id, now_occ ? 1 : -1);    // 变为占用 +1，变为空闲 -1
}

updateInflation 把 inflate_offsets_ 逐个叠加到邻居上：膨胀计数为正的体素，膨胀标志 occupancy_buffer_inflate_ 记为 1（即「膨胀占用」）

说人话：每个真正的障碍体素，会把自己的「占用」状态沿着一整套预先算好的偏移向量，洒到周围半径 0.25 m 的圆柱范围里。这样碰撞查询时只需查「点的膨胀占用」这一个标志，而不用每次算距离。

这个「洒」的过程，用一张俯视图看最直观：(a) rebuildInflationOffsets 先把以体素为中心、半径 $R$ 圆内的所有格点坐标预存成偏移集合；(b) 当某个体素从空闲变成占用时，updateInflation 沿着这些偏移向量把计数 $+ 1$ 广播出去，计数大于 0 的体素就被打上「膨胀占用」标记：

3-5 空间碰撞检测 getInflateOccupancy

现在把 2-4 节的公式落到代码——这是 SCAN-Planner 全仓库最核心的函数，在 grid_map.h 里以内联函数形式给出
常规的单球/单圆模型只查一个点就够，而 SCAN-Planner（双圆柱）要查两个点——front 和 rear，因为膨胀半径 = 圆柱半径 $R$ ，「圆柱中心落在膨胀区」等价于「圆柱体碰到障碍」
先用一张俯视图理解「yaw 是关键输入」：机器人停在同一个位置，前方 $(0.25,\ 0)$ 处有一个障碍体素（已画出它的膨胀区，半径 $R = 0.25$ m）。左图机器人航向 $0^\circ$ ，front 点恰好落进膨胀区 → 判定碰撞；右图原地转到 $90^\circ$ ，front/rear 两个查询点都转开了 → 判定安全。同一个位置、同一片障碍，只是换个朝向，碰撞结论就完全不同——这就是「空间碰撞感知」的名字由来：

// 对应 3-5 节 / 2-4 节公式：双圆柱体空间碰撞检测（官方源码，已加中文注释）
inline int GridMap::getInflateOccupancy(Eigen::Vector3d pos, double yaw) {
  // 由航向角得到前进方向单位向量 h(theta) = [cos, sin, 0]
  Eigen::Vector3d heading(std::cos(yaw), std::sin(yaw), 0.0);
  // 前圆柱中心：pos + offset * heading
  Eigen::Vector3d front = pos + mp_.double_cylinder_offset_ * heading;
  // 后圆柱中心：pos - offset * heading
  Eigen::Vector3d rear  = pos - mp_.double_cylinder_offset_ * heading;

  // 先查前圆柱的膨胀占用
  int front_occ = getInflateOccupancyFromBuffer(front, md_.occupancy_buffer_inflate_);
  if (front_occ != 0) return front_occ;   // 前圆柱被占 → 立即判定碰撞

  // 前圆柱空闲，再查后圆柱
  return getInflateOccupancyFromBuffer(rear, md_.occupancy_buffer_inflate_);
}

对应的 getInflateOccupancyFromBuffer 只是一个「位置 → 体素 → 查表」的过程：

// 对应 3-5 节：单点膨胀占用查询
inline int GridMap::getInflateOccupancyFromBuffer(Eigen::Vector3d pos,
                                                  const std::vector<char>& buffer) {
  if (!isInMap(pos)) return -1;          // 出界 → 视为未知
  Eigen::Vector3i id;
  posToIndex(pos, id);                   // 连续坐标 → 体素索引
  return int(buffer[toAddress(id)]);     // 查膨胀标志
}

从这个函数我们能提炼出三个要点：
- yaw 是关键输入：同样的位置 $\mathbf{p}$ ，航向不同，前/后圆柱扫到的体素就不同，碰撞结果就不同。这就是「空间碰撞感知」的名字由来
- 前后独立：前圆柱空闲不代表后圆柱空闲，必须两个都查（front_occ == 0 才继续查 rear）
- 处处复用：地图膨胀、A* 搜索、B-spline 优化、FSM 安全检测，全仓库所有碰撞查询都走这一个函数

说人话：别的规划器问地图「这个点能不能站」，SCAN 问的是「这个点、这个朝向，机身前部和后部会不会碰到墙」。多问了「朝向」这一项，四足的窄门和走廊问题就解决了。

4 路由引导：全局参考轨迹

4-1 从「目标点」到「路由」

传统局部规划器（比如 EGO-Planner）拿到一个目标点后，从当前位置直接往目标点规划。这在开阔地没问题，但在长走廊、多楼层的场景里，局部规划器「只见树木不见森林」，容易钻进死胡同

论文的思路是：上面先给一条全局参考轨迹（route），局部规划器只要沿着这条路由往前走一小段就行

那问题来了：上层给的不就是一条全局路径（一串路点/折线）吗？为什么还要用 minimum-snap 拆成一条多项式轨迹，直接拿折线当路由不行吗？——不行。折线只是「一串离散点」，而局部规划需要的是一条随时可求值、运动学连续的曲线 $p (t)$ ：
- 折线没有「时间维度」：getLocalTarget 先在轨迹上按时间投影找最近点，再沿轨迹走到 planning_horizon_ 取局部目标点——折线只有几何位置，算不出「几秒后机器人该在哪」
- 折线运动学不连续：速度、加速度在路点处发生阶跃，跟随行进时存在明显抖动；四足的 yaw 参考取轨迹切线方向，折线拐角处切线方向突变，双圆柱的 yaw 碰撞检测（3-5 节）也随之波动
所以上层三种导航模式（2-3 节）给的输入，最终都整理成「一串路点」，统一走 4-3 节 min-snap 变成一条平滑多项式轨迹（这条轨迹如何作为局部优化的初始解，见 5-3 / 5-4 节）：

navi_mode	目标来源	生成入口	轨迹形态
1 MANUAL_TARGET	RViz 点 `/move_base_simple/goal`	`planGlobalTraj`	起点 → 单目标点，一整段
2 PRESET_TARGET	`tools/keypoint.yaml` 路点序列	`planGlobalTraj`（逐段）	当前位置 → 下一个路点，到点再规划下一段
3 REFERENCE_PATH	订阅 `/initial_path`	`planGlobalTrajWaypoints`	整条路径一次性 min-snap

落到 global_data_.global_traj_ 后都是同一条「随时可求值、运动学连续」的多项式轨迹，局部规划器只认它，从不关心它从哪来

4-2 时间分配

先说为什么需要这一步：每段轨迹 $p_k(t)$ 定义在 $0, T_k]$ 上，是时间函数。min-snap 的目标是对时间积分，QP 里「段末位置/速度/加速度等于指定值」的等式约束，也要靠 $T_k$ 才能把「系数」映射成「段末状态」——4-3 节 A 矩阵里就有 $T_k^{\,j-i}$ 这一项，没有 $T_k$ ，整个 QP 都写不出来
更重要的是：min-snap 只保证几何平滑，不约束速度。同一段路，时间给短了机器人就得高速超调，给长了又走走停停。所以时间分配就是给每段轨迹定一个符合物理极限的「节奏」——距离除以最大速度：
$T_k = \frac{\big\|\mathbf{p}_{k+1} - \mathbf{p}_k\big\|}{v_{\max}}$
在标准 min-snap 闭式解法里， $T_k$ 是先于求解给定的固定参数，只有多项式系数是优化变量，所以时间分配必须发生在求解之前（4-4 节的代码正是先 time(i) = dist / vmax，再调 minSnapTraj）
首段和末段的时间再翻倍——起、止阶段的加减速需要额外余量，否则一出发就要立刻冲到 $v_{\max}$ ：
$T_0 \leftarrow 2T_0,\qquad T_{m-1} \leftarrow 2T_{m-1}$

4-3 min-snap 全局轨迹

生成全局轨迹用的是经典的 minimum-snap 方法：把轨迹写成逐段多项式，然后最小化三阶导数（jerk）的能量——注意：官方函数名沿用 minSnapTraj，但代码里 Q 矩阵实际最小化的是 jerk（三阶导），是历史遗留误称，公式上我们以代码为准（下面马上证明）
先用一张散点图看全局轨迹生成的全过程：输入是一串路点（红色大圆），超过阈值的远段会被先等间距插密成灰色小圆（对应 4-4 节代码），min-snap 再生成一条平滑穿过所有点的多项式轨迹（蓝色曲线，拐角处自然圆滑）；图中还标出了 4-2 节的首末段时间翻倍：
![[scanplanner_global_traj.png]]
假设有 $m$ 段、每段轨迹是 5 阶多项式（6 个系数），位置、速度、加速度在段与段之间连续，路点位置作为等式约束：
$p_k(t) = \sum_{i=0}^{5} c_{k,i}\,t^i,\qquad k=0,\dots,m-1$
优化目标是最小化 jerk（三阶导）的积分：
$\min_{p(t)}\; \sum_{k}\int_{0}^{T_k} \big\|p_k^{(3)}(t)\big\|^2 dt$
为什么是 jerk 而不是 snap？看 polynomial_traj.cpp 里 Q 矩阵的元素 $i(i-1)(i-2)\,j(j-1)(j-2)\,\frac{T^{i+j-5}}{i+j-5}$ ——系数 $i (i - 1) (i - 2)$ 正是三阶导的系数（四阶导应是 $i (i - 1) (i - 2) (i - 3)$ ），且 $i$ 从 3 起才非零。5 阶多项式 + 端点位置/速度/加速度六个约束，恰好是 min-jerk 的最小可解阶数；若真最小化 snap 得用 6 阶多项式
整理成二次规划（QP）形式： $A$ 是「系数 → 各阶导数」的映射矩阵， $C$ 是 Mellinger 式「固定/自由变量选择矩阵」， $Q$ 是 jerk（三阶导）内积的 Hessian：

$\min_{\mathbf{p}}\; \mathbf{p}^T Q\,\mathbf{p},\qquad \text{s.t.}\;\; A\,\mathbf{p}=\mathbf{d}$

polynomial_traj.cpp 里正是这么搭的：

// 对应 4-3 节：min-snap QP 的映射矩阵 A（每个段 6 系数，两端位置/速度/加速度）
for (int k = 0; k < seg_num; k++) {
  Ab = Eigen::MatrixXd::Zero(6, 6);
  for (int i = 0; i < 3; i++) {
    Ab(2 * i, i) = Factorial(i);                                  // 段首：p^(i)(0)
    for (int j = i; j < 6; j++)
      Ab(2 * i + 1, j) = Factorial(j) / Factorial(j - i) * pow(Time(k), j - i); // 段尾：p^(i)(T)
  }
  A.block(k * 6, k * 6, 6, 6) = Ab;
}
...
/* ---------- minimum snap matrix ---------- */
Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(seg_num * 6, seg_num * 6);
for (int k = 0; k < seg_num; k++)
  ...   // Q = 系数空间的 jerk 内积矩阵（对应上面 ∫ p^(3) p^(3) dt）——官方注释误写为 snap，看元素可知是三阶导

4-4 代码实现

路点中间还会做一步插点：任意两路点之间距离超过阈值（max(total_len/8, 4.0) m）就等间距插入中间点，防止长段轨迹让 min-snap 产生过大的中间波动：

// 对应 4-4 节：路点过远时插入中间点
double dist_thresh = max(total_len / 8, 4.0);
for (size_t i = 0; i < points.size() - 1; ++i) {
  inter_points.push_back(points.at(i));
  double dist = (points.at(i + 1) - points.at(i)).norm();
  if (dist > dist_thresh) {
    int id_num = floor(dist / dist_thresh) + 1;
    for (int j = 1; j < id_num; ++j)
      inter_points.push_back(points.at(i) * (1.0 - double(j)/id_num)
                           + points.at(i + 1) * double(j)/id_num);   // 线性插点
  }
}
inter_points.push_back(points.back());   // 官方原文：补上最后一个路点

然后写位置矩阵、按 4-2 节分配时间、调 minSnapTraj：

// 对应 4-2 / 4-4 节：时间分配 + 生成全局轨迹
Eigen::VectorXd time(pt_num - 1);
for (int i = 0; i < pt_num - 1; ++i)
  time(i) = (pos.col(i + 1) - pos.col(i)).norm() / (pp_.max_vel_);   // T_k = 距离 / vmax
time(0) *= 2.0;                    // 首段时间翻倍
time(time.rows() - 1) *= 2.0;      // 末段时间翻倍

PolynomialTraj gl_traj;
if (pos.cols() >= 3)
  gl_traj = PolynomialTraj::minSnapTraj(pos, start_vel, end_vel, start_acc, end_acc, time);
else if (pos.cols() == 2)
  gl_traj = PolynomialTraj::one_segment_traj_gen(start_pos, start_vel, start_acc,
                                                 pos.col(1), end_vel, end_acc, time(0));
else
  return false;                          // 点数异常 → 放弃本次全局规划
auto time_now = ros::Time::now();        // 官方原文：取当前时刻（供 setGlobalTraj 记录时间戳）
global_data_.setGlobalTraj(gl_traj, time_now);   // 存入全局参考轨迹

至此，「路由」就绪。接下来局部规划器就进入「沿着这条路由不断取目标、规划」的循环——先讲第 5 章怎么从全局路径取局部目标点、连出初始轨迹，再讲第 6 章的 Projected A* 绕障搜索，调度这一切的状态机 FSM 放在第 9 章

说人话：全局路由就像手机地图给你规划的一条「路线」，局部规划器就是司机——司机不需要一次背完整条路线，只需要知道「再往前开 3.5 米到哪」，到了就再往前看 3.5 米。这段「3.5 米」就是 planning_horizon。

5 从全局路径取点：局部目标点与初始轨迹

5-1 初始轨迹：一次局部优化的初始种子

局部规划器的输入不是整条全局轨迹，而是当前这一小段路——从机器人当前位置 $s_0$ 连到「沿全局轨迹往前 planning_horizon（默认 3.5 m）处的局部目标点」（取点方法见 5-2 节）。这条两点之间的连线，就是初始轨迹
为什么叫「初始」：它只是本次局部优化的种子。后面的 A* 绕障（第 6 章）、反弹方向（6-3 节）、碰撞代价（第 7 章）都作用在这条线上，最后被 B-spline + L-BFGS 优化成真正可执行的局部轨迹（第 8 章）
这里最容易踩的坑，是把这条初始轨迹和第 4 章的全局轨迹混为一谈——两条轨迹都是 min-snap 生成的，但不是同一条：

	4-3 节全局轨迹	5-3 / 5-4 节初始轨迹
生成函数	`planGlobalTrajWaypoints`（第 4-4 节）	`reboundReplan` 的 STEP 1（min-snap 采样见 5-3 节、重采样见 5-4 节）
输入	一串路点（起点 + 途经点）	只有两个点：当前起点 $s_0$ + 局部目标点
穿过	所有路点（平滑穿过全局路由的每一个途经点）	只从 $s_0$ 连到局部目标点那一个点
一次生成 / 反复生成	收到导航指令时生成一次	约 10 ms 一次调度、按需重新生成（FSM 按 `replan_thresh` 等条件触发，非每个 10 ms 都重规划）
作用	提供「往哪走」的大方向 + 供取局部目标点	作为本次局部优化的初始种子，等待 B-spline + L-BFGS 优化

把上面这张表画成图，关系就一目了然——全局路径（灰）穿所有路点、一次生成；机器人当前点 $s_0$ 并不在路径上，先把当前位置投影到全局轨迹（5-2 节的第一步），再沿路径往前 3.5 m 取局部目标点；然后从当前点另画一条初始轨迹（红）连到那个点，全局路径只贡献终点那一个坐标，其余形状就此丢掉：

请添加图片描述

两者的关系：全局轨迹只贡献一个坐标——局部目标点。局部规划器在全局轨迹上取「再往前 planning_horizon」处的点，然后丢掉全局轨迹的其余形状，另起炉灶生成一条只连两个点的初始轨迹（或上一段轨迹截断），再交给优化器

说人话：全局轨迹是「长途路线图」，从起点标到终点、穿过沿途所有路点；局部规划器每次重规划都从路线图上取「前方 3.5 米那一个路标」当临时终点，然后自己画一条「当前点 → 那个路标」的短线作为初始猜测，再交给优化器打磨。短线画得好不好没关系，反正最后要交给 B-spline + L-BFGS 优化。

5-2 取点：getLocalTarget 从全局路径取局部目标点

全局路由（一串路点/折线）本身不直接进优化器——它对局部规划只做一件事：提供局部目标点。局部规划器沿路由取「再往前 planning_horizon（部署配置 advanced_param.xml 默认 3.5 m；代码层兜底是 5.0）」处的点作为这次规划的终点（getLocalTarget()）。它分三步：
第一步：投影。把机器人当前位置投影到全局轨迹上（找轨迹上离当前最近的点 $t_{\text{proj}}$ ）：

// 对应 5-2 节：把当前位置投影到全局轨迹
for (double t = 0.0; t < duration; t += t_step) {
  Eigen::Vector3d pos_t = planner_manager_->global_data_.getPosition(t);
  double dist_to_start = (pos_t - start_pt_).norm();
  if (dist_to_start < min_dist_to_start) { min_dist_to_start = dist_to_start; t_proj = t; }
}

第二步：向前取 horizon。从 $t_{\text{proj}}$ 开始沿轨迹累加弧长，走到总长达到 planning_horizon_（默认 3.5 m）的位置，就是局部目标：

// 对应 5-2 节：向前取 planning_horizon 距离的点
for (double t = t_proj; t < duration; t += t_step) {
  Eigen::Vector3d pos_t = planner_manager_->global_data_.getPosition(t);
  total_dist += (pos_t - prev_pos).norm();
  if (total_dist >= planning_horizon_) { local_target_pt_ = pos_t; target_t = t; target_found = true; break; }   // 官方原文：置 target_found
  prev_pos = pos_t;
}

第三步：目标碰撞避难。如果取到的目标点本身落在障碍里，就在它前后各搜一段，找一个空闲点替代：

// 对应 5-2 节：目标点在障碍中则前后搜索空闲点
auto targetOccupancy = [&](const Eigen::Vector3d &pt) {
  return planner_manager_->grid_map_->getInflateOccupancy(pt, estimateYawFromSegment(odom_pos_, pt));
};
if (targetOccupancy(local_target_pt_) != 0) {
  bool found_free_target = false;
  double adjusted_t = target_t;
  for (double dt = 0.0; dt <= planner_manager_->global_data_.global_duration_; dt += t_step) {
    double t_forward = target_t + dt;                       // 先往前找
    if (t_forward <= planner_manager_->global_data_.global_duration_) {
      Eigen::Vector3d pt = planner_manager_->global_data_.getPosition(t_forward);
      if (targetOccupancy(pt) == 0) { local_target_pt_ = pt; adjusted_t = t_forward; found_free_target = true; break; }
    }
    double t_backward = target_t - dt;                      // 再往后找
    if (t_backward >= std::max(0.0, t_proj)) {
      Eigen::Vector3d pt = planner_manager_->global_data_.getPosition(t_backward);
      if (targetOccupancy(pt) == 0) { local_target_pt_ = pt; adjusted_t = t_backward; found_free_target = true; break; }
    }
  }
  if (found_free_target) target_t = adjusted_t;   // 目标被调整 → 更新 target_t（后续按它取速度）
}

局部目标速度 local_target_vel_ 取 global_data_.getVelocity(target_t)——注意 getVelocity 有三个分支，目标时刻若落在本地轨迹时间窗内会返回本地轨迹的速度，正常在 horizon 之外走全局分支；接近终点时（ $\|\mathrm{end}\_\mathrm{pt}-\mathrm{local\_target\_pt}\| < v_{\max}^2/(2a_{\max})$ ）清零，保证到点能刹住

5-3 连线：生成初始轨迹（STEP 1）

有了「当前起点 $s_0$ + 局部目标点」，reboundReplan 的 STEP 1 先连一条初始轨迹：首次从 min-snap 多项式采样路径点（本节代码块），后续从上一段已执行轨迹按伪弧长重采样（第 5-4 节）
于是 reboundReplan 的输入是「当前起点 $s_0$ + 局部目标点」——要绕障的那条「初始轨迹」，本质就是这两点之间连出来的一条线。后面的 A* 绕障（6-1、6-2）、反弹方向（6-3）、第 7 章碰撞代价里的控制点偏移，全都发生在这条线上——全局路径只贡献终点那一个坐标，线形是局部另画的
采出的路径点还要反算成 B-spline 控制点（parameterizeToBspline），供后续优化使用：

// 对应 5-3 节：连线（STEP 1 INIT）——min-snap 多项式采样路径点（官方源码节选，已加中文注释）
double dist = (start_pt - local_target_pt).norm();
// 时间按梯形速度曲线定：距离够则加速到 v_max 再匀速；不够就三角（匀加匀减），时间 = 2*sqrt(dist/a_max)
double time = pow(pp_.max_vel_, 2) / pp_.max_acc_ > dist ? sqrt(dist / pp_.max_acc_)
                                                       : (dist - pow(pp_.max_vel_, 2) / pp_.max_acc_) / pp_.max_vel_ + 2 * pp_.max_vel_ / pp_.max_acc_;
gl_traj = PolynomialTraj::one_segment_traj_gen(start_pt, start_vel, start_acc,
                                               local_target_pt, local_target_vel,
                                               Eigen::Vector3d::Zero(), time);   // 起点→目标 一条多项式
// （官方此处外层有 flag_first_call||flag_polyInit||flag_force_polynomial 分支，内层还有 !flag_randomPolyTraj 的 else 随机插点分支，均省略）
double t;
bool flag_too_far;
ts *= 1.5;  // 官方原文：先 ×1.5，下面 do 循环首轮 ÷1.5 抵消，保证首次采样用原始 ts
do {                                   // 自适应细化：ts 逐次 ÷1.5，直到相邻点距足够近、点数足够多
  ts /= 1.5;  point_set.clear();  flag_too_far = false;
  Eigen::Vector3d last_pt = gl_traj.evaluate(0);
  for (t = 0; t < time; t += ts) {
    Eigen::Vector3d pt = gl_traj.evaluate(t);                     // 采样成路径点
    if ((last_pt - pt).norm() > pp_.ctrl_pt_dist * 1.5) { flag_too_far = true; break; }
    last_pt = pt;  point_set.push_back(pt);
  }
} while (flag_too_far || point_set.size() < 7);   // 保证点数足够、控制点间距合理
t -= ts;                                        // 官方原文：退回末次采样时刻
start_end_derivatives.push_back(gl_traj.evaluateVel(0));          // 首端速度
start_end_derivatives.push_back(local_target_vel);                // 末端速度
start_end_derivatives.push_back(gl_traj.evaluateAcc(0));          // 首端加速度
start_end_derivatives.push_back(gl_traj.evaluateAcc(t));          // 末端加速度：取末次采样点处
UniformBspline::parameterizeToBspline(ts, point_set, start_end_derivatives, ctrl_pts); // 反算控制点

连线完成后，紧接着 initControlPoints(ctrl_pts, true) 会沿这条初始轨迹做碰撞分段（第 6 章细讲）——撞上了就用第 6 章的 Projected A* 绕障，没撞就直接进优化

5-4 重采样：上一段轨迹 → 新初始轨迹

第 5-3 节讲的初始轨迹生成，是首次规划的路径：从零用 min-snap 连一条「当前起点 → 局部目标点」的多项式，再采样成路径点——起点状态是多项式首端「算」出来的
但进入重规划后，这条初始轨迹的生成逻辑会不一样：机器人正在执行一段轨迹，随时可能因前方碰撞、轨迹失效而要换一条新轨迹（触发时机见第 9 章），但它不会停下来等——新轨迹的起点必须是当前正在执行的状态（位置、速度、加速度）。若还照搬首次的 min-snap 从零生成，起点状态是算出来的，与正在走的状态对不上，接缝处速度、加速度阶跃，运动就会突变。所以初始轨迹不能重新生成，只能「接着没走完的路」续：从上一段已执行轨迹、当前时刻之后截取
reboundReplan 的 STEP 1 按 FSM 状态走两条采样分支：
- 首次 / 换目标（GEN_NEW_TRAJ）：由「当前起点 $s_0$ → 局部目标点」生成 min-snap 多项式，按时间均匀采样（5-3 节）
- 中途重规划（REPLAN_TRAJ）：把「当前时刻 $t_{\mathrm{cur}}$ 之后的剩余部分」按伪弧长均匀重采样，得到新的路径点序列（planner_manager.cpp reboundReplan 的 STEP 1 else 分支）

请添加图片描述

伪弧长指的是：把「走过的距离」当作标定轨迹位置的尺子——从 $t_{\mathrm{cur}}$ 起沿剩余轨迹逐点累计相邻采样点间的距离，得到弧长表 pseudo_arc_length；之后以固定弧长步长在这张表上等间隔取点，控制点间距就均匀了

说人话：原本的路径起点改变了，剩下的点如果还是用以前的，可能会出现数量不够的情况，这时候我们要借助 B-spline 的特性，直接取伪弧长，保证剩下的控制点数量是足够的，且等弧长取出的点间距是均匀的

// 对应 5-4 节：伪弧长重采样（planner_manager.cpp，reboundReplan STEP 1 else 分支，官方源码节选）
double t_cur = (ros::Time::now() - local_data_.start_time_).toSec();  // 当前执行时刻（相对本段起点）

vector<double> pseudo_arc_length;   // 累计弧长表：segment_point[i] 处的累计弧长
vector<Eigen::Vector3d> segment_point;   // 沿剩余轨迹逐点采出的原始路径点
pseudo_arc_length.push_back(0.0);   // 官方在后续尾巴采样处还包了 !pseudo_arc_length.empty() 空值守卫（此处因首元素已 push 0.0 恒非空，省略）
for (t = t_cur; t < local_data_.duration_ + 1e-3; t += ts)   // 从 t_cur 起，按 ts 扫完剩余轨迹
{
  segment_point.push_back(local_data_.position_traj_.evaluateDeBoorT(t));  // 取轨迹上的点
  if (t > t_cur)
    pseudo_arc_length.push_back((segment_point.back() - segment_point[segment_point.size() - 2]).norm()
                                + pseudo_arc_length.back());  // 弧长逐段累计
}
t -= ts;

double poly_time = (local_data_.position_traj_.evaluateDeBoorT(t) - local_target_pt).norm() / pp_.max_vel_ * 2;
if (poly_time > ts)   // 剩余轨迹终点离局部目标点还远：再接一段 min-snap 尾巴（5-3 节）平滑过去
{
  PolynomialTraj gl_traj = PolynomialTraj::one_segment_traj_gen(local_data_.position_traj_.evaluateDeBoorT(t),
                                                                local_data_.velocity_traj_.evaluateDeBoorT(t),
                                                                local_data_.acceleration_traj_.evaluateDeBoorT(t),
                                                                local_target_pt, local_target_vel,
                                                                Eigen::Vector3d::Zero(), poly_time);
  for (t = ts; t < poly_time; t += ts)
  {
    segment_point.push_back(gl_traj.evaluate(t));            // 尾巴上的点也追加进采样池
    pseudo_arc_length.push_back((segment_point.back() - segment_point[segment_point.size() - 2]).norm()
                                + pseudo_arc_length.back());
  }
}

double sample_length = 0;
double cps_dist = pp_.ctrl_pt_dist * 1.5;   // 目标间距；随后 ÷1.5 自适应细化
size_t id = 0;
do
{
  cps_dist /= 1.5;  point_set.clear();  sample_length = 0;  id = 0;
  while ((id <= pseudo_arc_length.size() - 2) && sample_length <= pseudo_arc_length.back())
  {
    if (sample_length >= pseudo_arc_length[id] && sample_length < pseudo_arc_length[id + 1])
    {
      // 在弧长表相邻两行之间线性插值，取「等弧长间隔」的点
      point_set.push_back((sample_length - pseudo_arc_length[id]) / (pseudo_arc_length[id + 1] - pseudo_arc_length[id]) * segment_point[id + 1]
                        + (pseudo_arc_length[id + 1] - sample_length) / (pseudo_arc_length[id + 1] - pseudo_arc_length[id]) * segment_point[id]);
      sample_length += cps_dist;
    }
    else
      id++;
  }
  point_set.push_back(local_target_pt);   // 末尾补上局部目标点
} while (point_set.size() < 7);           // 点数不足则继续细化（cps_dist ÷1.5）

start_end_derivatives.push_back(local_data_.velocity_traj_.evaluateDeBoorT(t_cur));      // 首端速度：直接继承执行轨迹
start_end_derivatives.push_back(local_target_vel);                                        // 末端速度
start_end_derivatives.push_back(local_data_.acceleration_traj_.evaluateDeBoorT(t_cur));  // 首端加速度：直接继承执行轨迹
start_end_derivatives.push_back(Eigen::Vector3d::Zero());

说人话：重规划要「接着没走完的路」重新算。直接从旧轨迹上截取一段下来，控制点疏密不匀，均匀 B-spline 不满足等间距前提；所以先量出每段的弧长，再按「每走多远取一个点」的固定步长重新取样，点距均匀、数量可控，起点还能和正在执行的轨迹平滑衔接。

附录

附录一：双圆柱体碰撞模型示意图生成代码

生成 2-4 节「双圆柱 vs 单圆」对比图（已嵌入 2-4 节），直观看出单圆与双圆柱覆盖范围的差异

请添加图片描述

import matplotlib
matplotlib.use('Agg')
import matplotlib.pyplot as plt
import numpy as np

fig, axes = plt.subplots(1, 2, figsize=(11, 4.6))

# 左图：单圆（按机身体长取大圆）——腰身被过度膨胀
ax = axes[0]
circle = plt.Circle((0, 0), 0.43, color='skyblue', alpha=0.45, ec='#3b6fa0')
ax.add_patch(circle)
# 机身示意（长条形）
ax.add_patch(plt.Rectangle((-0.43, -0.175), 0.86, 0.35, fc='#e8a33d', alpha=0.85, ec='k'))
ax.set_xlim(-0.6, 0.6); ax.set_ylim(-0.6, 0.6)
ax.set_aspect('equal'); ax.set_title('single circle R=0.43 (over-conservative)')

# 右图：双圆柱（前后各一个半径 0.25、中心 ±0.18）
ax = axes[1]
R, dc = 0.25, 0.18
for s in (+1, -1):
    ax.add_patch(plt.Circle((s * dc, 0), R, color='#7ec98e', alpha=0.55, ec='#2f7d3a'))
ax.add_patch(plt.Rectangle((-0.43, -0.175), 0.86, 0.35, fc='#e8a33d', alpha=0.85, ec='k'))
# 前圆柱中心
ax.plot([dc], [0], marker='o', color='k')
ax.annotate('front', (dc, 0), textcoords='offset points', xytext=(4, 8))
ax.plot([-dc], [0], marker='o', color='k')
ax.annotate('rear', (-dc, 0), textcoords='offset points', xytext=(4, 8))
ax.annotate(r'$d_c=0.18$', (0, 0), textcoords='offset points', xytext=(0, -20))
ax.set_xlim(-0.6, 0.6); ax.set_ylim(-0.6, 0.6)
ax.set_aspect('equal'); ax.set_title('double cylinder R=0.25, dc=0.18')

plt.tight_layout()
plt.savefig('scanplanner_double_cylinder.png', dpi=160)
print('saved: scanplanner_double_cylinder.png')

附录二：占据概率 logit 更新曲线生成代码

生成 3-2 节占据概率随观测次数累积的曲线（已嵌入 3-2 节）：命中加 hit 增量、穿过加 miss 增量，并展示 clamp 效果

请添加图片描述

import matplotlib
matplotlib.use('Agg')
import matplotlib.pyplot as plt
import numpy as np

def logit(p): return np.log(p / (1 - p))

p_hit, p_miss = 0.85, 0.30
p_min, p_max, p_occ = 0.12, 0.98, 0.80
L_hit, L_miss = logit(p_hit), logit(p_miss)
L_min, L_max = logit(p_min), logit(p_max)
L_occ = logit(p_occ)

# 模拟一个体素：前 4 次观测命中，后 6 次穿过
L = L_min - 0.01          # 初始未知
hits = [L_min]
seq = [1, 1, 1, 1, 0, 0, 0, 0, 0, 0]
for m in seq:
    dL = L_hit if m == 1 else L_miss
    L = np.clip(L + dL, L_min, L_max)
    hits.append(L)

steps = np.arange(len(hits))
plt.figure(figsize=(6.8, 4.2))
plt.plot(steps, hits, 'o-', color='#3b6fa0', lw=1.8, label='log-odds L')
plt.axhline(L_occ, color='#b03a2e', ls='--', alpha=0.8, label='L_occ (occupied)')
plt.axhline(L_min, color='gray', ls=':', alpha=0.8, label='L_min')
plt.axhline(L_max, color='gray', ls=':', alpha=0.8, label='L_max')
plt.axvline(4.5, color='green', ls='--', alpha=0.5)
plt.text(4.6, 1.2, 'hit -> miss', fontsize=9)
plt.xlabel('observation count'); plt.ylabel('log-odds')
plt.title('Counting-based occupancy update (p_hit=0.85, p_miss=0.30)')
plt.legend(); plt.grid(alpha=0.3); plt.tight_layout()
plt.savefig('scanplanner_logit_update.png', dpi=160)
print('saved: scanplanner_logit_update.png')

附录三：双圆柱体膨胀广播俯视图生成代码

生成 3-4 节「偏移集合 + 广播膨胀」的俯视图（已嵌入 3-4 节）：左图 rebuildInflationOffsets 画出半径 $R$ 圆内的全部偏移向量，右图 updateInflation 展示障碍体素把 $+ 1$ 广播到圆内所有体素、计数大于 0 即标记为「膨胀占用」

请添加图片描述

# -*- coding: utf-8 -*-
# 生成 3-4 节双圆柱体膨胀广播步骤俯视图（已嵌入 3-4 节）
import matplotlib
matplotlib.use('Agg')
import matplotlib.pyplot as plt
import numpy as np

R, res = 0.25, 0.05        # double_cylinder_radius、resolution（格数 n = 5）
n = int(np.ceil(R / res))

fig, axes = plt.subplots(1, 2, figsize=(11, 5.2))

# ---------- 左图：偏移集合 rebuildInflationOffsets ----------
ax = axes[0]
for ix in range(-6, 7):
    for iy in range(-6, 7):
        ax.add_patch(plt.Rectangle((ix*res-0.5*res, iy*res-0.5*res), res, res,
                     fc='white', ec='#dddddd', lw=0.5))
off = [(ix, iy) for ix in range(-n, n+1) for iy in range(-n, n+1)
       if (ix*res)**2 + (iy*res)**2 < R**2]        # 圆内偏移（x^2+y^2 < R^2）
for (ix, iy) in off:
    ax.add_patch(plt.Rectangle((ix*res-0.5*res, iy*res-0.5*res), res, res,
                 fc='#7ec98e', ec='#2f7d3a', lw=0.6, alpha=0.7))
    ax.annotate('', xy=(ix*res, iy*res), xytext=(0, 0),
                arrowprops=dict(arrowstyle='->', color='#2f7d3a', lw=0.7, alpha=0.7))
ax.add_patch(plt.Rectangle((-0.5*res, -0.5*res), res, res, fc='#b03a2e', ec='k'))
theta = np.linspace(0, 2*np.pi, 100)
ax.plot(R*np.cos(theta), R*np.sin(theta), color='#b03a2e', lw=1.6, ls='--')
ax.annotate(r'$R = 0.25$ m', xy=(R, 0), xytext=(R+0.06, 0.04),
            fontsize=10, color='#b03a2e')
ax.text(0, -R-0.16, 'offset set (green cells)', ha='center', fontsize=9, color='#2f7d3a')
ax.set_title('(a) rebuildInflationOffsets: offsets with $x^2+y^2<R^2$', fontsize=10)
ax.set_xlim(-0.32, 0.32); ax.set_ylim(-0.34, 0.34)
ax.set_aspect('equal'); ax.set_xticks([]); ax.set_yticks([])

# ---------- 右图：广播 +1 → 膨胀占用 updateInflation ----------
ax = axes[1]
for ix in range(-6, 7):
    for iy in range(-6, 7):
        ax.add_patch(plt.Rectangle((ix*res-0.5*res, iy*res-0.5*res), res, res,
                     fc='white', ec='#dddddd', lw=0.5))
for (ix, iy) in off:
    ax.add_patch(plt.Rectangle((ix*res-0.5*res, iy*res-0.5*res), res, res,
                 fc='#f4a261', ec='#d9802c', lw=0.7))   # 收到 +1 → 膨胀占用
ax.add_patch(plt.Rectangle((-0.5*res, -0.5*res), res, res, fc='#b03a2e', ec='k'))
ax.text(0, 0, '+1', color='white', ha='center', va='center', fontsize=8, weight='bold')
ax.plot(R*np.cos(theta), R*np.sin(theta), color='#b03a2e', lw=1.6, ls='--')
ax.annotate(r'$R = 0.25$ m', xy=(R, 0), xytext=(R+0.06, 0.04),
            fontsize=10, color='#b03a2e')
ax.text(0, -R-0.16, 'inflated (cnt>0 -> flag=1)', ha='center', fontsize=9, color='#d9802c')
ax.set_title('(b) updateInflation: broadcast +1 to all offsets', fontsize=10)
ax.set_xlim(-0.32, 0.32); ax.set_ylim(-0.34, 0.34)
ax.set_aspect('equal'); ax.set_xticks([]); ax.set_yticks([])

plt.tight_layout()
plt.savefig('scanplanner_inflation_broadcast.png', dpi=160)
print('saved: scanplanner_inflation_broadcast.png')

附录四：航向 yaw 影响碰撞检测俯视图生成代码

生成 3-5 节「航向不同 → 碰撞结论不同」的俯视图（已嵌入 3-5 节）：机器人停在同一位置，前方一个障碍体素，左图 yaw $=0^\circ$ 时 front 点落进膨胀区判定碰撞，右图 yaw $=90^\circ$ 时两个查询点都转开判定安全

请添加图片描述

# -*- coding: utf-8 -*-
# 生成 3-5 节：航向 yaw 如何影响双圆柱碰撞检测俯视图（已嵌入 3-5 节）
import matplotlib
matplotlib.use('Agg')
import matplotlib.pyplot as plt
import numpy as np

R, dc = 0.25, 0.18        # double_cylinder_radius / double_cylinder_offset
res = 0.05
ox, oy = 0.25, 0.0        # 障碍体素中心（膨胀半径 R）

fig, axes = plt.subplots(1, 2, figsize=(11, 5.0))

def draw_scene(ax, yaw):
    """公共场景：网格 + 障碍体素 + 膨胀区 + 随 yaw 旋转的机身。"""
    for ix in range(-10, 11):
        for iy in range(-8, 9):
            ax.add_patch(plt.Rectangle((ix*res-0.5*res, iy*res-0.5*res), res, res,
                         fc='white', ec='#e8e8e8', lw=0.4))
    theta = np.linspace(0, 2*np.pi, 100)
    ax.fill(ox + R*np.cos(theta), oy + R*np.sin(theta),
            color='#f4a261', alpha=0.40, zorder=1)          # 障碍的膨胀区
    ax.plot(ox + R*np.cos(theta), oy + R*np.sin(theta),
            color='#d9802c', lw=1.4, ls='--', zorder=2)
    ax.annotate(r'inflated radius $R=0.25$', xy=(ox+R, oy), xytext=(ox+R+0.05, 0.04),
                fontsize=9, color='#d9802c', zorder=5)
    ax.add_patch(plt.Rectangle((ox-0.5*res, oy-0.5*res), res, res,
                 fc='#555555', ec='k', zorder=3))           # 障碍体素
    ax.text(ox, oy-0.10, 'obstacle', ha='center', fontsize=9,
            color='#333333', zorder=5)
    # 机身绕机器人中心 (0,0) 旋转 yaw 度
    body = plt.Rectangle((-0.35, -0.175), 0.70, 0.35,
                         angle=np.degrees(yaw), rotation_point=(0, 0),
                         fc='#e8a33d', alpha=0.95, ec='k', lw=0.8, zorder=4)
    ax.add_patch(body)

def draw_query(ax, yaw):
    """front/rear 两个查询点：落在膨胀区内→红(碰撞)，区外→绿(安全)。"""
    heading = np.array([np.cos(yaw), np.sin(yaw)])
    front = dc * heading                       # pos + dc * heading
    rear  = -dc * heading                      # pos - dc * heading
    for name, pt in (('front', front), ('rear', rear)):
        d = np.hypot(pt[0]-ox, pt[1]-oy)
        if d < R:
            color, verdict = '#b03a2e', 'inside'      # 碰撞
        else:
            color, verdict = '#2f7d3a', 'outside'     # 安全
        ax.plot([pt[0]], [pt[1]], marker='o', ms=11, color=color,
                mec='k', mew=0.6, zorder=6)
        ax.annotate(f'{name} ({verdict})', (pt[0], pt[1]),
                    textcoords='offset points', xytext=(7, 7),
                    fontsize=9, color=color, zorder=7)
    # 航向箭头
    ax.annotate('', xy=(0.30*heading[0], 0.30*heading[1]), xytext=(0, 0),
                arrowprops=dict(arrowstyle='->', color='#3b6fa0', lw=2.2), zorder=5)
    ax.text(0.30*heading[0]+0.04, 0.30*heading[1]+0.04, 'heading',
            color='#3b6fa0', fontsize=9, zorder=5)

# 左图：yaw = 0° → front 点在膨胀区内，碰撞
ax = axes[0]
draw_scene(ax, 0.0)
draw_query(ax, 0.0)
ax.set_title(r'(a) yaw $= 0^\circ$: front inside inflated $\rightarrow$ collision',
             fontsize=10, color='#b03a2e')

# 右图：yaw = 90° → 两个查询点都在膨胀区外，安全
ax = axes[1]
draw_scene(ax, np.pi/2)
draw_query(ax, np.pi/2)
ax.set_title(r'(b) yaw $= 90^\circ$: both outside $\rightarrow$ safe',
             fontsize=10, color='#2f7d3a')

for ax in axes:
    ax.set_xlim(-0.5, 0.75); ax.set_ylim(-0.45, 0.45)
    ax.set_aspect('equal'); ax.set_xticks([]); ax.set_yticks([])

plt.tight_layout()
plt.savefig('scanplanner_yaw_collision.png', dpi=160)
print('saved: scanplanner_yaw_collision.png')

附录五：全局轨迹生成散点图生成代码

生成 4-3 节「路点 → 插点 → min-snap 平滑轨迹」的散点图（已嵌入 4-3 节）：红色大圆是输入路点，灰色小圆是长段插密点，蓝色曲线是 min-snap 多项式轨迹；min-snap 求解器直接复刻仓库 polynomial_traj.cpp 的 Q 矩阵，轨迹验证穿过所有点

请添加图片描述

# -*- coding: utf-8 -*-
# 生成第 4 章：全局轨迹生成散点图（路点 -> 插点 -> min-snap 平滑轨迹）
# min-snap 求解器与仓库 polynomial_traj.cpp 的 Q 矩阵一致
import math
import numpy as np
import matplotlib
matplotlib.use('Agg')
import matplotlib.pyplot as plt


def min_snap_1d(P, T, v0=0.0, a0=0.0, vf=0.0, af=0.0):
    """一维 min-snap（Q 与仓库代码一致：最小化三阶导能量）。P: m+1 个路点，T: m 段时间"""
    m = len(T)
    nd = 6 * m

    # ---- Q：逐段分块，∫(p''')² dt ----
    Q = np.zeros((nd, nd))
    for k in range(m):
        t = T[k]
        for i in range(3, 6):
            for j in range(3, 6):
                Q[6 * k + i, 6 * k + j] = i * (i - 1) * (i - 2) * j * (j - 1) * (j - 2) / (i + j - 5) * t ** (i + j - 5)

    # ---- 约束行：phi(k, deriv, t) 取 c 的第 6k+i 项系数 = i!/(i-deriv)! t^(i-deriv) ----
    def phi(k, deriv, t):
        row = np.zeros(nd)
        for i in range(deriv, 6):
            row[6 * k + i] = math.factorial(i) / math.factorial(i - deriv) * t ** (i - deriv)
        return row

    rows, d = [], []
    rows.append(phi(0, 0, 0.0));   d.append(P[0])          # 起点位置
    rows.append(phi(0, 1, 0.0));   d.append(v0)            # 起点速度
    rows.append(phi(0, 2, 0.0));   d.append(a0)            # 起点加速度
    for k in range(m - 1):                                  # 每个连接点
        rows.append(phi(k, 0, T[k]));       d.append(P[k + 1])   # 段尾位置 = 路点
        rows.append(phi(k + 1, 0, 0.0));    d.append(P[k + 1])   # 段首位置 = 路点
        rows.append(phi(k, 1, T[k]) - phi(k + 1, 1, 0.0)); d.append(0.0)  # 速度连续
        rows.append(phi(k, 2, T[k]) - phi(k + 1, 2, 0.0)); d.append(0.0)  # 加速度连续
    rows.append(phi(m - 1, 0, T[-1])); d.append(P[-1])      # 终点位置
    rows.append(phi(m - 1, 1, T[-1])); d.append(vf)         # 终点速度
    rows.append(phi(m - 1, 2, T[-1])); d.append(af)         # 终点加速度
    A = np.array(rows)
    d = np.array(d)

    # ---- KKT 求解 ----
    K = np.block([[Q, A.T], [A, np.zeros((A.shape[0], A.shape[0]))]])
    rhs = np.concatenate([np.zeros(nd), d])
    sol = np.linalg.solve(K, rhs)
    return sol[:nd]


def eval_traj(c, T, n_per_seg=200):
    """把系数向量求值成密集点（含时间参数 t 和弧长）"""
    m = len(T)
    segs = []
    ts = []
    for k in range(m):
        ck = c[6 * k:6 * k + 6]
        tt = np.linspace(0.0, T[k], n_per_seg)
        # p(t) = Σ c_i t^i
        p = sum(ck[i] * tt ** i for i in range(6))
        segs.append(p)
        ts.append(tt)
    return segs, ts
# ---- 路点（米）：一段先爬升、拐弯、再进 9 米长走廊、最后拐出 ----
waypoints = [(0.0, 0.0), (1.0, 2.0), (3.0, 1.5), (12.0, 1.5), (14.0, 0.0)]
vmax = 2.0

# ---- 插点（对应 4-4 代码：距离 > max(total_len/8, 4.0) 则等间距插入）----
pts = [np.array(waypoints[0])]
total_len = sum(np.linalg.norm(np.array(waypoints[i + 1]) - np.array(waypoints[i]))
                for i in range(len(waypoints) - 1))
dist_thresh = max(total_len / 8.0, 4.0)
for i in range(len(waypoints) - 1):
    a, b = np.array(waypoints[i]), np.array(waypoints[i + 1])
    dist = np.linalg.norm(b - a)
    if dist > dist_thresh:
        id_num = math.floor(dist / dist_thresh) + 1
        for j in range(1, id_num):
            pts.append(a * (1.0 - j / id_num) + b * (j / id_num))
    pts.append(b)
print(f"dist_thresh={dist_thresh:.2f} m, 插点后点数: {len(pts)}")

# ---- 时间分配（对应 4-2/4-4 代码：T_k=dist/vmax，首末段翻倍）----
T = []
for i in range(len(pts) - 1):
    T.append(np.linalg.norm(pts[i + 1] - pts[i]) / vmax)
T[0] *= 2.0
T[-1] *= 2.0
print("各段时间:", [round(t, 2) for t in T])

# ---- 分别对 X、Y 求 min-snap ----
Px = [p[0] for p in pts]
Py = [p[1] for p in pts]
cx = min_snap_1d(Px, T)
cy = min_snap_1d(Py, T)

# ---- 验证轨迹确实穿过每个点 ----
ok = True
for i, (px, py) in enumerate(zip(Px, Py)):
    # 该点位于段 k=i-1 的末尾 / 段 i 的开头
    if i == 0:
        k = 0; tt = 0.0
    elif i == len(pts) - 1:
        k = len(T) - 1; tt = T[k]
    else:
        k = i - 1; tt = T[k]
    ck_x = cx[6 * k:6 * k + 6]
    ck_y = cy[6 * k:6 * k + 6]
    ex = sum(ck_x[j] * tt ** j for j in range(6))
    ey = sum(ck_y[j] * tt ** j for j in range(6))
    err = np.linalg.norm([ex - px, ey - py])
    if err > 1e-8:
        ok = False
    print(f"点{i} 轨迹({ex:.3f},{ey:.3f}) vs 路点({px:.3f},{py:.3f}) 误差={err:.2e}")
print("穿过所有点:", "OK" if ok else "FAIL")

# ---- 求值 + 绘图 ----
segx, tss = eval_traj(cx, T)
segy, _ = eval_traj(cy, T)
X = np.concatenate(segx)
Y = np.concatenate(segy)

fig, ax = plt.subplots(figsize=(9.5, 4.4))
# min-snap 平滑轨迹
ax.plot(X, Y, '-', color='#2f7ed8', lw=2.2, zorder=3, label='min-snap trajectory')
# 插点（灰色小圆）
inserted = [p for p in pts[1:-1]]
ins_arr = np.array(inserted) if inserted else np.empty((0, 2))
ax.scatter(ins_arr[:, 0], ins_arr[:, 1], s=42, c='#9aa0a6', marker='o',
           edgecolors='#5f6368', linewidths=0.8, zorder=4, label='inserted points')
# 路点（红色大圆）
wp = np.array(waypoints)
ax.scatter(wp[:, 0], wp[:, 1], s=130, c='#e0453a', marker='o',
           edgecolors='#7c1008', linewidths=1.2, zorder=5, label='waypoints')

ax.annotate('waypoint $P_0$', (0, 0), xytext=(0.15, 0.35), fontsize=10, color='#7c1008')
ax.annotate('waypoint $P_4$', (14, 0), xytext=(10.7, -0.55), fontsize=10, color='#7c1008')
ax.annotate('inserted points\n(split long segment)', (6, 1.5), xytext=(5.0, 2.35),
            fontsize=9, color='#5f6368', ha='center',
            arrowprops=dict(arrowstyle='->', color='#5f6368', lw=0.8))
ax.annotate('min-snap trajectory\n(passes every point, smooth at corners)',
            (8.5, 1.15), xytext=(6.6, -1.15), fontsize=9, color='#1b5ba6', ha='center',
            arrowprops=dict(arrowstyle='->', color='#1b5ba6', lw=0.8))
ax.text(0.4, 2.6, '$T_0 \\leftarrow 2T_0$ (accel.)', fontsize=10, color='#b3541e')
ax.text(13.35, 0.7, '$T_{m-1} \\leftarrow 2T_{m-1}$', fontsize=10, color='#b3541e')

ax.set_xlim(-0.8, 15.2)
ax.set_ylim(-1.7, 3.3)
ax.set_aspect('equal')
ax.set_xlabel('x (m)')
ax.set_ylabel('y (m)')
ax.set_title('Global trajectory generation: waypoints → inserted points → min-snap')
ax.grid(True, ls='--', alpha=0.35)
ax.legend(loc='lower left', fontsize=9, framealpha=0.9)

plt.tight_layout()
plt.savefig('scanplanner_global_traj.png', dpi=170)
print('saved: scanplanner_global_traj.png')

附录六：全局路由驱动局部规划示意图生成代码

生成第 6 章「全局路由 → 局部目标点 → 初始轨迹 → 碰撞区间 → A* 绕障」的链路示意图（已嵌入 6 章开头）：灰色虚线路点是全局路由（提供方向的粗路由），路由上 3.5 m 处取局部目标点；红色虚线是 min-snap 初始轨迹（沿路由方向、穿过障碍）， $P_1$ … $P_3$ 为碰撞控制点、in_id/out_id 为区间两端自由控制点；蓝色折线是 A* 只对碰撞段 in→out 搜索的绕障路径，终点是碰撞段出口 out_id 而不是局部目标点；图中各环节用分散标签直接标注

# -*- coding: utf-8 -*-
# 第 6 章：A* 绕障搜索 —— 局部目标点 vs A* 搜索区间
# 链路：全局路由(灰色路点) → 取局部目标点(planning_horizon=3.5m) → 初始轨迹(min-snap, 红虚线)
#       → 碰撞检测标出 [in_id, out_id] → A* 只搜 in→out(蓝折线，终点=out_id，不是局部目标点)
import sys
import matplotlib
if 'mpl_toolkits' in sys.modules:
    del sys.modules['mpl_toolkits']
for k in list(sys.modules):
    if k.startswith('mpl_toolkits.mplot3d'):
        del sys.modules[k]
import numpy as np
import matplotlib.font_manager as fm
fm.fontManager.addfont('/usr/share/fonts/opentype/noto/NotoSansCJK-Regular.ttc')
matplotlib.rcParams['font.sans-serif'] = ['Noto Sans CJK JP', 'DejaVu Sans']
matplotlib.rcParams['axes.unicode_minus'] = False
matplotlib.use('Agg')
import matplotlib.pyplot as plt
from matplotlib.patches import Ellipse, Polygon

C_ROUTE = '#5f6368'
C_TRAJ = '#e0453a'
C_ASTAR = '#1b5ba6'
C_OBST = '#f4a261'
C_OBST_EDGE = '#d9802c'
C_INK = '#111111'
C_WARN = '#7c2f00'
C_TARGET = '#ff8c00'
C_ROBOT = '#b3541e'

y = 1.0
cx, cy, rx, ry = 4.4, y, 0.9, 0.6            # 膨胀障碍
waypts = [(1.2, y), (3.0, y), (4.4, y), (6.2, y), (8.0, y)]  # 全局路由路点
robot = (2.0, y)
target = (6.2, y)                             # 局部目标点（路由上 3.5m 处）
ctrl_x = [3.0, 3.7, 4.4, 5.1, 5.8]            # P_0..P_4（初始轨迹上的控制点）
IN_ID, OUT_ID = 0, 4                          # in_id=P_0(最后一个自由点) out_id=P_4(第一个自由点)
A_PATH = [(3.0, 1.0), (3.2, 1.9), (4.4, 2.3), (5.6, 1.9), (5.8, 1.0)]

fig, ax = plt.subplots(figsize=(10.2, 4.4))

# 全局路由（灰色虚线路点）——先画，让障碍盖住中间一段（示意「路由不知道这里有障碍」）
R = np.array(waypts)
ax.plot(R[:, 0], R[:, 1], '--', color=C_ROUTE, lw=1.6, alpha=0.9, zorder=1)
ax.scatter(R[:, 0], R[:, 1], s=34, marker='s', c=C_ROUTE, zorder=2)
ax.text(8.15, y + 0.09, '全局路由', fontsize=9.5, color=C_ROUTE, ha='left', va='bottom')

# 膨胀障碍
ax.add_patch(Ellipse((cx, cy), 2 * rx, 2 * ry, fc=C_OBST, alpha=0.45,
                     ec=C_OBST_EDGE, lw=1.4, ls='--', zorder=4))
ax.text(cx, cy, '膨胀障碍', ha='center', va='center', fontsize=9, color=C_OBST_EDGE, zorder=6)

# 初始轨迹（红虚线：min-snap 起点→局部目标点），覆盖在路由上方
ax.plot([robot[0], target[0]], [y, y], '--', color=C_TRAJ, lw=2.2, zorder=3)
ax.text(1.9, y - 0.30, '初始轨迹（min-snap：起点 → 局部目标点）', fontsize=9.5,
        color=C_TRAJ, ha='left', va='top', zorder=6)

# 控制点：碰撞的 P_1..P_3 红实心，两端 in/out 空心
for k, x in enumerate(ctrl_x):
    if k == IN_ID or k == OUT_ID:
        ax.scatter([x], [y], s=150, facecolors='none', edgecolors=C_WARN,
                   linewidths=1.8, zorder=7)
    else:
        ax.scatter([x], [y], s=120, c=C_TRAJ, marker='o',
                   edgecolors='#7c1008', linewidths=1.1, zorder=7)
    ax.text(x, y - 0.13, f'$P_{k}$', ha='center', fontsize=8.5, color=C_INK, zorder=7)
ax.text(ctrl_x[IN_ID], y + 0.13, 'in_id', ha='center', fontsize=9, color=C_WARN)
ax.text(ctrl_x[OUT_ID], y + 0.13, 'out_id', ha='center', fontsize=9, color=C_WARN)

# 机器人当前位姿（三角）
tri = Polygon([(robot[0] - 0.16, y - 0.14), (robot[0] - 0.16, y + 0.14), (robot[0] + 0.20, y)],
              closed=True, fc=C_ROBOT, ec=C_INK, lw=0.8, zorder=8)
ax.add_patch(tri)
ax.text(robot[0] - 0.14, y - 0.42, '机器人\n（当前起点）', fontsize=9, color=C_ROBOT,
        ha='center', va='top', zorder=8)

# 局部目标点（路由上 planning_horizon 处）
ax.scatter(*target, s=260, facecolors='none', edgecolors=C_TARGET, linewidths=2.2, zorder=8)
ax.text(target[0], y + 0.40, '局部目标点\n（路由上 3.5 m 处）', fontsize=9.5,
        color=C_TARGET, ha='center', va='bottom', zorder=8)
# 起点→局部目标点 的间距标注
ax.annotate('', xy=target, xytext=robot,
            arrowprops=dict(arrowstyle='<->', color=C_TARGET, lw=1.4, alpha=0.8,
                            shrinkA=24, shrinkB=32), zorder=5)
ax.text((robot[0] + target[0]) / 2, y - 0.30, 'planning_horizon = 3.5 m',
        fontsize=9, color=C_TARGET, ha='center', va='top', zorder=6)

# 碰撞区间括号 [in_id, out_id]
ax.plot([ctrl_x[IN_ID], ctrl_x[OUT_ID]], [0.74, 0.74], '-', color=C_WARN, lw=1.5)
ax.plot([ctrl_x[IN_ID], ctrl_x[IN_ID]], [0.70, 0.78], '-', color=C_WARN, lw=1.5)
ax.plot([ctrl_x[OUT_ID], ctrl_x[OUT_ID]], [0.70, 0.78], '-', color=C_WARN, lw=1.5)
ax.text((ctrl_x[IN_ID] + ctrl_x[OUT_ID]) / 2, 0.62, '碰撞区间 $[\\mathrm{in\\_id}, \\mathrm{out\\_id}]$',
        ha='center', fontsize=9.5, color=C_WARN, zorder=6)

# A* 绕障路径：in → out（蓝折线，只搜这一小段，终点不到局部目标点）
A = np.array(A_PATH)
ax.plot(A[:, 0], A[:, 1], '-', color=C_ASTAR, lw=2.6, marker='s', ms=6,
        mfc=C_ASTAR, mec='white', mew=0.7, zorder=6)
ax.text(5.95, 2.42, 'A* 绕障路径（in → out）', fontsize=9.5, color=C_ASTAR,
        ha='left', va='bottom', zorder=6)


ax.set_xlim(1.0, 8.7)
ax.set_ylim(0.1, 3.15)
ax.set_aspect('equal')
ax.set_xlabel('x (m)')
ax.set_ylabel('y (m)')
ax.set_title('第 6 章：全局路由 → 局部目标点 → 初始轨迹 → 碰撞区间 → A* 只搜 in→out',
             fontsize=11)

plt.tight_layout()
plt.savefig('scanplanner_global_to_local.png', dpi=160)
print('saved: scanplanner_global_to_local.png')

附录七：全局路径与初始轨迹关系示意图生成代码

生成 5-1 节「全局轨迹 vs 初始轨迹」示意图（已嵌入 5-1 节）：灰色实线为全局轨迹（第 4-3 节生成、一次穿过所有路点）；机器人当前点 $s_0$ 不在路径上，先把当前位置投影到全局轨迹（ $t_{proj}$ ），再沿路径往前 planning_horizon=3.5 m 取局部目标点（橙色圈）；红色曲线为初始轨迹（第 5-3 节），从当前点另画、连到局部目标点；目标点之后全局轨迹画成浅灰虚线，示意「其余形状丢掉」

请添加图片描述

# -*- coding: utf-8 -*-
# 第 5-1 节：全局轨迹 vs 初始轨迹（同一张俯视图里看关系）
# 全局路径(灰)穿过所有路点、一次生成；机器人当前点 s0 不在路径上，先把当前位置投影到
# 全局轨迹(t_proj)，再沿路径往前 planning_horizon=3.5 m 取局部目标点；初始轨迹(红)从当前点
# 另画、连到局部目标点——全局路径只贡献终点那一个坐标，目标点之后其余形状丢掉
import sys
import matplotlib
if 'mpl_toolkits' in sys.modules:
    del sys.modules['mpl_toolkits']
for k in list(sys.modules):
    if k.startswith('mpl_toolkits.mplot3d'):
        del sys.modules[k]
import numpy as np
import matplotlib.font_manager as fm
fm.fontManager.addfont('/usr/share/fonts/opentype/noto/NotoSansCJK-Regular.ttc')
matplotlib.rcParams['font.sans-serif'] = ['Noto Sans CJK JP', 'DejaVu Sans']
matplotlib.rcParams['axes.unicode_minus'] = False
matplotlib.use('Agg')
import matplotlib.pyplot as plt
from matplotlib.patches import Polygon

C_ROUTE = '#5f6368'
C_ROUTE_DIM = '#9aa0a6'
C_TRAJ = '#e0453a'
C_TARGET = '#ff8c00'
C_PROJ = '#6a737d'
C_ROBOT = '#b3541e'
C_INK = '#111111'

YG = 1.6                      # 全局路径所在高度
waypts = [(1.2, YG), (3.0, YG), (4.7, YG), (6.5, YG), (8.5, YG)]  # 全局路径路点
s0 = (3.2, 0.85)              # 当前点（机器人实际位置，不在全局路径上）
tproj = (3.0, YG)             # 当前位置投影到全局轨迹的点 t_proj
target = (6.5, YG)            # 局部目标点（从 t_proj 沿路径往前 planning_horizon=3.5 m）

# 初始轨迹：从当前点 s0 出发、连到局部目标点（微弯曲线，区别于全局路径）
B0, B1, B2 = np.array(s0), np.array((4.9, 0.72)), np.array(target)
tt = np.linspace(0, 1, 100)
bezi = (1 - tt) ** 2 * B0[:, None] + 2 * (1 - tt) * tt * B1[:, None] + tt ** 2 * B2[:, None]

fig, ax = plt.subplots(figsize=(10.2, 4.8))

# 全局路径：目标点之前实线；目标点之后浅灰虚线 = 丢掉
R = np.array(waypts)
ax.plot(R[:, 0], R[:, 1], '-', color=C_ROUTE, lw=2.0, zorder=3)
ax.plot([6.5, 8.5], [YG, YG], '--', color=C_ROUTE_DIM, lw=1.6, zorder=2)
ax.scatter([p[0] for p in waypts], [YG] * 5, s=46, marker='s',
           c=[C_ROUTE] * 4 + [C_ROUTE_DIM], edgecolors='white', linewidths=0.8, zorder=4)
ax.text(0.95, YG + 0.33, '全局路径（第 4-3 节）', fontsize=10, color=C_ROUTE,
        ha='left', va='center', zorder=5)
ax.text(7.5, YG - 0.16, '丢掉', fontsize=8.5, color=C_ROUTE_DIM,
        ha='center', va='top', zorder=5)

# 投影：当前点 → 全局轨迹上的 t_proj
ax.plot([s0[0], tproj[0]], [s0[1], tproj[1]], ':', color=C_PROJ, lw=1.4, zorder=4)
ax.text(2.2, 1.18, '投影到全局轨迹', fontsize=8.5, color=C_PROJ,
        ha='right', va='center', zorder=5)

# 当前点 s0：机器人三角（实际位置）
tri = Polygon([(s0[0] - 0.18, s0[1] - 0.16), (s0[0] - 0.18, s0[1] + 0.16), (s0[0] + 0.22, s0[1])],
              closed=True, fc=C_ROBOT, ec=C_INK, lw=0.8, zorder=7)
ax.add_patch(tri)
ax.text(s0[0], s0[1] - 0.40, '当前点 $s_0$', fontsize=10, color=C_ROBOT,
        ha='center', va='top', zorder=7)

# 局部目标点
ax.scatter(*target, s=260, facecolors='none', edgecolors=C_TARGET, linewidths=2.2, zorder=7)
ax.text(6.6, YG + 0.26, '局部目标点', fontsize=10, color=C_TARGET,
        ha='left', va='center', zorder=7)

# planning_horizon 括号标注（全局路径上方，避让下方初始轨迹）
ax.plot([tproj[0], target[0]], [1.72, 1.72], '-', color=C_TARGET, lw=1.5, zorder=6)
ax.plot([tproj[0], tproj[0]], [YG, 1.72], '-', color=C_TARGET, lw=1.5, zorder=6)
ax.plot([target[0], target[0]], [YG, 1.72], '-', color=C_TARGET, lw=1.5, zorder=6)
ax.text(4.75, 1.86, 'planning_horizon = 3.5 m', fontsize=9, color=C_TARGET,
        ha='center', va='bottom', zorder=6)

# 初始轨迹（红粗线：当前点 s0 → 局部目标点）
ax.plot(bezi[0], bezi[1], '-', color=C_TRAJ, lw=3.2, zorder=5)
ax.text(4.9, 1.38, '初始轨迹（第 5-3 节）', fontsize=10, color=C_TRAJ,
        ha='left', va='center', zorder=6)

ax.set_xlim(0.75, 9.3)
ax.set_ylim(0.30, 2.35)
ax.set_aspect('equal')
ax.set_xlabel('x (m)')
ax.set_ylabel('y (m)')
ax.set_title('5-1：全局轨迹 vs 初始轨迹 —— 全局轨迹只贡献局部目标点那一个坐标', fontsize=11)

plt.tight_layout()
plt.savefig('scanplanner_global_local_init.png', dpi=160)
print('saved: scanplanner_global_local_init.png')

附录八：伪弧长重采样示意图生成代码

生成 5-4 节「伪弧长重采样」示意图（已嵌入 5-4 节）：上、下两格画同一段变速的「上一段已执行轨迹」——按时间戳等间隔采样（红点）点距随速度疏密不匀，破坏均匀 B-spline 的等间距前提；按伪弧长等间隔重采样（绿点）点距完全一致，顶部弧长轴上均匀刻度竖线示意「在弧长参数域均匀取点」

请添加图片描述

# -*- coding: utf-8 -*-
# 第 5-4 节：伪弧长重采样 —— 为什么不能按时间戳直接截断
# 上一段已执行轨迹(灰)是一条变速曲线：等时间戳采样 → 空间点距随速度变化、疏密不匀，
# 均匀 B-spline 的等间距前提被破坏；伪弧长重采样 → 在弧长参数域均匀取点，曲线上点距完全一致。
# 上：按时间戳等间隔采样(红点)；下：按伪弧长等间隔重采样(绿点) + 弧长轴示意「在弧长域均匀取点」。
import sys
import matplotlib
if 'mpl_toolkits' in sys.modules:
    del sys.modules['mpl_toolkits']
for k in list(sys.modules):
    if k.startswith('mpl_toolkits.mplot3d'):
        del sys.modules[k]
import numpy as np
import matplotlib.font_manager as fm
fm.fontManager.addfont('/usr/share/fonts/opentype/noto/NotoSansCJK-Regular.ttc')
matplotlib.rcParams['font.sans-serif'] = ['Noto Sans CJK JP', 'DejaVu Sans']
matplotlib.rcParams['axes.unicode_minus'] = False
matplotlib.use('Agg')
import matplotlib.pyplot as plt

C_TRAJ   = '#e0453a'   # 红：等时间戳采样点（疏密不匀）
C_RES    = '#1e8449'   # 绿：等弧长重采样点（均匀）
C_ROUTE  = '#5f6368'   # 灰：上一段已执行轨迹
C_ROUTE_DIM = '#9aa0a6'
C_INK    = '#111111'
C_ASTAR  = '#1a56a0'   # 蓝：弧长轴刻度线

# ---------- 构造一条「变速」的已执行轨迹，并做弧长参数化 ----------
t0 = np.linspace(0, 1, 2000)
x  = 0.6 + 8.8 * t0                                 # x 单调推进
y  = 1.05 + 0.85 * np.sin(1.65 * t0 * np.pi) + 0.12 * t0   # 波形：速度在弯道处快慢不一
dx, dy = np.diff(x), np.diff(y)
arc = np.concatenate([[0], np.cumsum(np.hypot(dx, dy))])   # 累计弧长表（同官方 pseudo_arc_length）
L   = arc[-1]

# 弧长均匀重采样成 1200 个高密度点，作为曲线的「弧长等距表示」
N_HI = 1200
s_hi = np.linspace(0, L, N_HI)
xh   = np.interp(s_hi, arc, x)
yh   = np.interp(s_hi, arc, y)

# ---- 采样方式 1：等时间戳（红）。时间均匀，弧长增量 ∝ 速度 → 点距不匀 ----
K = 9
tau = np.linspace(0, 1, K)
g   = tau ** 1.55                                   # 时间→弧长比例：下凸 → 前段点疏、后段点密
s_time = L * g
i_time = np.round(s_time / L * (N_HI - 1)).astype(int)
xt_, yt_ = xh[i_time], yh[i_time]

# ---- 采样方式 2：等弧长（绿）。弧长均匀 → 曲线上点距完全一致 ----
s_uni = np.linspace(0, L, K)
i_uni = np.round(s_uni / L * (N_HI - 1)).astype(int)
xu_, yu_ = xh[i_uni], yh[i_uni]

fig, (ax1, ax2) = plt.subplots(2, 1, figsize=(10.4, 7.0))

# ================= 上：按时间戳采样 =================
ax1.plot(xh, yh, '-', color=C_ROUTE, lw=2.2, alpha=0.9, zorder=2)
ax1.plot(xt_, yt_, 'o', color=C_TRAJ, ms=9, mec='#7c1008', mew=1.0, zorder=4)
for i in range(K - 1):                             # 相邻点间连线，凸显间距差异
    ax1.plot([xt_[i], xt_[i + 1]], [yt_[i], yt_[i + 1]], '-', color=C_TRAJ,
             lw=1.2, alpha=0.45, zorder=3)
ax1.text(0.7, 2.28, '按时间戳等间隔采样：$t_k = t_{\\mathrm{cur}} + k\\,\\Delta t$', fontsize=10,
         color=C_TRAJ, ha='left', va='bottom')
ax1.text(0.7, 0.45, '点距随速度变化（前疏后密），不满足均匀 B-spline 等间距前提',
         fontsize=9.5, color=C_INK, ha='left', va='bottom')
ax1.text(9.0, 2.28, '点距 $\\propto$ 速度', fontsize=9, color=C_TRAJ,
         ha='right', va='bottom')
ax1.set_ylim(0.15, 2.5)

# ================= 下：按弧长重采样 =================
# 弧长轴（顶部）：把弧长比例映射到曲线 x 区间，刻度均匀 = 弧长均匀
sx0, sx1 = 0.6, 9.4
ax2.plot([sx0, sx1], [2.62, 2.62], '-', color=C_ROUTE_DIM, lw=1.6, zorder=1)
ax2.text(sx1 + 0.06, 2.62, '弧长 $s$', fontsize=9.5, color=C_ROUTE_DIM,
         ha='left', va='center')
# 红点（时间采样）在弧长轴上的投影位置：不匀
sx_r = sx0 + s_time / L * (sx1 - sx0)
ax2.scatter(sx_r, [2.62] * K, marker='|', s=90, color=C_TRAJ, zorder=3)
# 绿点（弧长采样）在弧长轴上的投影位置：均匀
sx_g = sx0 + s_uni / L * (sx1 - sx0)
ax2.scatter(sx_g, [2.62] * K, marker='|', s=120, color=C_RES, zorder=3)
ax2.text((sx0 + sx1) / 2, 2.78, '弧长轴上均匀取点（绿）↔ 时间均匀投影（红）',
         fontsize=9.5, color=C_INK, ha='center', va='bottom')

ax2.plot(xh, yh, '-', color=C_ROUTE, lw=2.2, alpha=0.55, zorder=2)   # 原轨迹作淡参照
ax2.plot(xu_, yu_, 'o', color=C_RES, ms=9, mec='#0f4d24', mew=1.0, zorder=4)
# 均匀刻度竖线：弧长轴 → 曲线上绿点
for k in range(K):
    ax2.plot([sx_g[k], xu_[k]], [2.62, yu_[k]], ':', color=C_ASTAR, lw=1.0, alpha=0.7, zorder=1)
# 等距双箭头：任意两相邻绿点间距都是 Δs
ax2.annotate('', xy=(xu_[1], yu_[1]), xytext=(xu_[0], yu_[0]),
             arrowprops=dict(arrowstyle='<->', color=C_RES, lw=1.6, shrinkA=4, shrinkB=4), zorder=5)
ax2.text((xu_[0] + xu_[1]) / 2, (yu_[0] + yu_[1]) / 2 - 0.22, '$\\Delta s$ = 目标间距',
         fontsize=9.5, color=C_RES, ha='center', va='top', zorder=6)
ax2.text(9.0, 2.28, '等弧长点距', fontsize=9.5, color=C_RES, ha='right', va='bottom')
ax2.set_ylim(0.15, 2.95)

for ax, tag in [(ax1, '① 按时间戳截断（不推荐）'), (ax2, '② 按伪弧长重采样（官方做法）')]:
    ax.set_xlim(0.4, 10.2)
    ax.set_aspect('equal')
    ax.set_xticks([])
    ax.set_yticks([])
    ax.text(0.012, 0.03, tag, transform=ax.transAxes, fontsize=10.5, color=C_INK,
            ha='left', va='bottom',
            bbox=dict(boxstyle='round,pad=0.3', fc='white', ec=C_ROUTE_DIM, lw=0.8))

fig.suptitle('5-4：伪弧长重采样 —— 在弧长参数域均匀取点，控制点间距一致',
             fontsize=12, y=0.98)
plt.tight_layout(rect=[0, 0, 1, 0.96])
plt.savefig('scanplanner_resample.png', dpi=160)
print('saved: scanplanner_resample.png')

附录九：首次规划 vs 重规划初始轨迹生成示意图生成代码

生成 5-4 节「首次规划 vs 重规划」示意图（已嵌入 5-4 节）：上、下两格对照初始轨迹的起点状态来源不同——① 首次规划（第 5-3 节 GEN_NEW_TRAJ）从零用 min-snap 连一条「当前起点 → 局部目标点」的多项式，没有旧轨迹可续；② 重规划（第 5-4 节 REPLAN_TRAJ）机器人正在执行上一段轨迹（灰色，已走完部分实线、剩余部分虚线），新轨迹起点必须是当前正在执行的状态（位置/速度/加速度，蓝箭头 = 继承的起点速度），只能「接着没走完的路」续、绕开障碍连到局部目标点；若照搬首次从零 min-snap（棕红点线），起点方向与执行轨迹对不上，接缝处速度、加速度阶跃

请添加图片描述

# -*- coding: utf-8 -*-
# 第 5-4 节：首次规划 vs 重规划 —— 初始轨迹的起点状态来源不同
# 上：首次规划（第 5-3 节 GEN_NEW_TRAJ）—— 从零用 min-snap 连一条「当前起点 → 局部目标点」
#     的多项式，起点状态是多项式首端「算」出来的，没有旧轨迹可续。
# 下：重规划（第 5-4 节 REPLAN_TRAJ）—— 机器人正在执行上一段轨迹，随时可能因碰撞要换新轨迹
#     但它不停下等：新轨迹起点必须是当前正在执行的状态（位置/速度/加速度），只能「接着没走完
#     的路」续。画：原来的路径（灰，已走完实线 + 剩余虚线）、现在的点（三角）、已重规划的路径
#     （红，从当前状态出发、起点切线 = 旧轨迹切线，绕开障碍连到局部目标点）。若照搬首次从零
#     min-snap，起点方向与执行轨迹对不上 → 接缝处速度、加速度阶跃（棕红虚线 + 阶跃标注）。
import sys
import matplotlib
if 'mpl_toolkits' in sys.modules:
    del sys.modules['mpl_toolkits']
for k in list(sys.modules):
    if k.startswith('mpl_toolkits.mplot3d'):
        del sys.modules[k]
import numpy as np
import matplotlib.font_manager as fm
fm.fontManager.addfont('/usr/share/fonts/opentype/noto/NotoSansCJK-Regular.ttc')
matplotlib.rcParams['font.sans-serif'] = ['Noto Sans CJK JP', 'DejaVu Sans']
matplotlib.rcParams['axes.unicode_minus'] = False
matplotlib.use('Agg')
import matplotlib.pyplot as plt
from matplotlib.patches import Polygon, FancyBboxPatch

C_ROUTE   = '#5f6368'   # 灰：上一段已执行轨迹（已走完）
C_ROUTE_DIM = '#9aa0a6' # 浅灰：剩余部分（将截断丢弃）
C_TRAJ    = '#e0453a'   # 红：首次 / 重规划初始轨迹
C_TARGET  = '#ff8c00'   # 橙：局部目标点
C_ROBOT   = '#b3541e'   # 机器人三角
C_INK     = '#111111'
C_OBST    = '#cfcfcf'   # 障碍（浅色，让剩余虚线可见）
C_TAN     = '#1a56a0'   # 蓝：接缝处继承的切线方向
C_WRONG   = '#7a5230'   # 棕红：错误做法（从零 min-snap 的起点方向）

# ---------- 三次贝塞尔工具 ----------
def cubic(B, N=600):
    B = np.asarray(B, float)
    tt = np.linspace(0, 1, N)
    return ((1-tt)**3 * B[0][:,None] + 3*(1-tt)**2*tt * B[1][:,None]
            + 3*(1-tt)*tt**2 * B[2][:,None] + tt**3 * B[3][:,None])

def cubic_deriv(B, t):
    B = np.asarray(B, float)
    return (3*(1-t)**2 * (B[1]-B[0]) + 6*(1-t)*t * (B[2]-B[1]) + 3*t**2 * (B[3]-B[2]))

def at_x(B, x0, N=800):
    p = cubic(B, N)
    i = int(np.argmin(np.abs(p[0] - x0)))
    return p[:, i], i

def norm(v):
    v = np.asarray(v, float)
    return v / np.hypot(v[0], v[1])

# ---------- 面板 ① 首次规划 ----------
S0    = np.array([1.2, 0.95])                       # 当前起点（多项式首端）
FIRST = np.array([S0, [4.1, 1.30], [6.5, 0.55], [8.8, 0.95]])   # 初始轨迹（微弯）
TGT   = np.array([8.8, 0.95])                       # 局部目标点

# ---------- 面板 ② 重规划 ----------
PREV  = np.array([[0.9, 0.75], [3.4, 1.50], [6.2, 0.40], [8.8, 0.95]])  # 上一段已执行轨迹
X_CUR = 5.0
P_CUR, _ = at_x(PREV, X_CUR)
t_cur = X_CUR / (PREV[3,0] - PREV[0,0]) * 0 + (np.clip((X_CUR - PREV[0,0]) / (PREV[3,0] - PREV[0,0]), 0, 1))
# 用数值法求切线更稳：找曲线上最近 t
tts = np.linspace(0, 1, 800)
pxs = cubic(PREV, 800)[0]
i_t = int(np.argmin(np.abs(pxs - X_CUR)))
tan = norm(cubic_deriv(PREV, tts[i_t]))
# 重规划路径：起点=当前点，初始方向=旧轨迹切线（继承速度），绕开障碍连到局部目标点
REPLAN = np.array([P_CUR,
                   P_CUR + tan * 0.95,
                   [6.35, 0.05],
                   TGT])
# 错误做法：若照搬首次从零 min-snap，起点方向 = 从当前点直连目标（与执行轨迹方向对不上）
WRONG  = np.array([P_CUR,
                   P_CUR + 0.55 * (TGT - P_CUR),
                   P_CUR + 0.9 * (TGT - P_CUR),
                   TGT])
OBST = (6.2, 0.6, 1.2, 0.9)   # (x0, y0, w, h)：挡在剩余路径上，逼着重规划绕行

prev_all = cubic(PREV)
i_split  = int(np.argmin(np.abs(prev_all[0] - X_CUR)))

fig, (ax1, ax2) = plt.subplots(2, 1, figsize=(10.6, 8.2))

# ================= ① 首次规划 =================
b1 = cubic(FIRST)
ax1.plot(b1[0], b1[1], '-', color=C_TRAJ, lw=3.2, zorder=5)
ax1.scatter(*TGT, s=260, facecolors='none', edgecolors=C_TARGET, linewidths=2.2, zorder=7)
tri1 = Polygon([(S0[0]-0.18, S0[1]-0.16), (S0[0]-0.18, S0[1]+0.16), (S0[0]+0.22, S0[1])],
               closed=True, fc=C_ROBOT, ec=C_INK, lw=0.8, zorder=7)
ax1.add_patch(tri1)
ax1.text(1.15, 0.28, '当前起点 $s_0$', fontsize=10, color=C_ROBOT, ha='center', va='top', zorder=7)
ax1.text(8.82, 1.38, '局部目标点', fontsize=10, color=C_TARGET, ha='right', va='center', zorder=7)
ax1.text(4.95, 1.74, '初始轨迹：min-snap 多项式（第 5-3 节）', fontsize=10, color=C_TRAJ,
         ha='center', va='bottom', zorder=6)
ax1.text(5.35, 0.28, '从零连一条「当前起点 → 局部目标点」的多项式，起点状态是多项式首端「算」出来的',
         fontsize=9.5, color=C_INK, ha='center', va='top', zorder=6)
ax1.set_xlim(0.7, 9.6); ax1.set_ylim(0.10, 2.05)

# ================= ② 重规划 =================
# 上一段已执行轨迹：已走完部分（实线灰）
ax2.plot(prev_all[0][:i_split+1], prev_all[1][:i_split+1], '-', color=C_ROUTE, lw=2.2, zorder=3)
# 剩余部分（浅灰虚线：继续走会撞障碍，将被截断丢弃）
ax2.plot(prev_all[0][i_split:], prev_all[1][i_split:], '--', color=C_ROUTE_DIM, lw=1.8, zorder=3)
# 障碍
ax2.add_patch(FancyBboxPatch((OBST[0], OBST[1]), OBST[2], OBST[3],
                             boxstyle='round,pad=0.03,rounding_size=0.12',
                             fc=C_OBST, ec='#8a8a8a', lw=1.0, zorder=2))
ax2.text(OBST[0]+OBST[2]/2, OBST[1]+OBST[3]/2, '障碍', fontsize=10, color=C_INK,
         ha='center', va='center', zorder=4)

# 当前点：机器人三角 + 继承的切线方向（蓝箭头）
tri2 = Polygon([(P_CUR[0]-0.18, P_CUR[1]-0.16), (P_CUR[0]-0.18, P_CUR[1]+0.16), (P_CUR[0]+0.22, P_CUR[1])],
               closed=True, fc=C_ROBOT, ec=C_INK, lw=0.8, zorder=8)
ax2.add_patch(tri2)
ax2.annotate('', xy=P_CUR + tan*1.0, xytext=P_CUR,
             arrowprops=dict(arrowstyle='-|>', color=C_TAN, lw=2.4, shrinkA=2, shrinkB=2), zorder=8)

# 错误做法（棕红点线：照搬首次从零 min-snap，起点方向与执行轨迹不一致）
bw = cubic(WRONG)
ax2.plot(bw[0], bw[1], ':', color=C_WRONG, lw=2.2, zorder=4)

# 重规划路径（红粗线：从当前状态接着续，绕开障碍）
b2 = cubic(REPLAN)
ax2.plot(b2[0], b2[1], '-', color=C_TRAJ, lw=3.2, zorder=6)
ax2.scatter(*TGT, s=260, facecolors='none', edgecolors=C_TARGET, linewidths=2.2, zorder=8)

# 标注
ax2.text(0.95, 1.88, '上一段已执行轨迹', fontsize=10, color=C_ROUTE, ha='left', va='center', zorder=7)
ax2.text(1.05, 1.30, '已走完', fontsize=9, color=C_ROUTE, ha='left', va='center', zorder=7)
ax2.text(7.75, 1.70, '剩余部分（将截断丢弃）', fontsize=8.5, color=C_ROUTE_DIM,
         ha='center', va='bottom', zorder=7)
ax2.text(5.0, 0.32, '当前点 $t_{\\mathrm{cur}}$', fontsize=9.5,
         color=C_ROBOT, ha='center', va='top', zorder=7)
ax2.text(7.62, 0.12, '重规划路径（第 5-4 节）',
         fontsize=9.5, color=C_TRAJ, ha='left', va='bottom', zorder=7)
ax2.text(2.35, 0.55, '若照搬首次从零 min-snap：\n起点方向与执行轨迹对不上 → 阶跃',
         fontsize=8.5, color=C_WRONG, ha='right', va='center', zorder=7)
ax2.text(3.5, 1.90, '新轨迹起点 = 当前执行状态\n（位置/速度/加速度，蓝箭头 = 继承的起点速度）',
         fontsize=9.5, color=C_INK, ha='center', va='center', zorder=7,
         bbox=dict(boxstyle='round,pad=0.35', fc='white', ec=C_ROUTE_DIM, lw=0.8))
ax2.set_xlim(0.7, 9.6); ax2.set_ylim(-0.05, 2.2)

for ax, tag in [(ax1, '① 首次规划（第 5-3 节）：从零 min-snap'),
                (ax2, '② 重规划（第 5-4 节）：接着上一段轨迹续')]:
    ax.set_aspect('equal')
    ax.set_xticks([])
    ax.set_yticks([])
    ax.text(0.985, 0.97, tag, transform=ax.transAxes, fontsize=10.5, color=C_INK,
            ha='right', va='top',
            bbox=dict(boxstyle='round,pad=0.3', fc='white', ec=C_ROUTE_DIM, lw=0.8))

fig.suptitle('5-4：首次规划从零连、重规划接着续 —— 新轨迹起点必须是当前正在执行的状态',
             fontsize=12, y=0.985)
plt.tight_layout(rect=[0, 0, 1, 0.97])
plt.savefig('scanplanner_first_vs_replan.png', dpi=160)
print('saved: scanplanner_first_vs_replan.png')

下篇

Projected A* 绕障搜索、B-spline 轨迹优化、L-BFGS 优化与反弹重启、FSM 状态机调度、完整流程串讲，详见下篇：【Scan-Planner源码解读（下篇）】双圆柱碰撞检测 + Project-A* 绕障：SCAN 避障链路逐行拆。双圆柱判断「撞没撞」，Projected A* 决定「往哪躲」，B-spline + L-BFGS 决定「怎么走」。

DAMO开发者矩阵

DAMO开发者矩阵，由阿里巴巴达摩院和中国互联网协会联合发起，致力于探讨最前沿的技术趋势与应用成果，搭建高质量的交流与分享平台，推动技术创新与产业应用链接，围绕“人工智能与新型计算”构建开放共享的开发者生态。

更多推荐

2026 具身智能：让机器人“长眼睛、会动手”，MonkeyCode 免费上手

2026 年 AI 热点“具身智能”科普：让 AI 从聊天框走进现实，长眼睛、会动手。文章从深夜机械臂抓箱失败的真实故事切入，讲清具身智能“感知—决策—执行”闭环，并深入拆解当下最热的 VLA（视觉-语言-动作）模型与世界模型两大知识点。最后推荐 MonkeyCode 平台免费上手：浏览器即开即用、云端环境跑仿真、GLM/Kimi/MiniMax/Qwen/DeepSeek 主流大模型一键切换、完