导航补充教程 • ZeroHour's Site

导航培训 Appendix：规划、地图与动态避障进阶#

本文是导航教程的进阶附录，建议先了解重定位、里程计、全局规划器和局部规划器的基本分工。

零.#

这些内容涉及许多算法，比较详细一些，也很难一次性写清楚，我基本都是写了个大概一些比较重要的点，真正要详细讲的话每一个算法实现起来都有诸多细节，目前 agent 比较屌的情况下，掌握算法的基本原理和功能比如何写出来更重要。另外，我写这份附录的时候也使用 GPT-5.6 帮我补充了近 6 成内容，所以有啥不懂的大家后面慢慢查 GPT 吧

层级	建议内容
基础	占据栅格、Dijkstra、A*、LOS、footprint、静态避障
工程相关	距离场、底盘模型、时间戳、轨迹与路径区别、碰撞检查
规划	RRT、RRT、PRM、JPS、State Lattice、Hybrid/Kinodynamic A
优化	B 样条、安全走廊、QP、L-BFGS、MINCO
动态避障	点云分割、检测、跟踪、预测、时空规划、MPC/MPPI
了解	语义地图

一. 路径和轨迹有什么区别#

1. 路径#

路径 path 主要描述从哪里经过，通常是一串没有严格时间含义的几何点：

P=\{\mathbf p_0,\mathbf p_1,\ldots,\mathbf p_N\}

只要这些点和点之间的连线不碰撞，就可以说得到了一条几何可行路径。但它没有回答机器人在每个点的速度、加速度是多少，也没有保证底盘能够拐过每一个弯。

2. 轨迹#

轨迹 trajectory 描述“什么时刻到哪里、以什么状态运动”，是关于时间的连续函数：

\mathbf p(t),\quad \mathbf v(t)=\dot{\mathbf p}(t),\quad \mathbf a(t)=\ddot{\mathbf p}(t)

根据机器人模型，还可能需要朝向 $\theta(t)$ 、角速度 $\omega(t)$ 、转角和控制输入。

因此一条常见规划流水线是：

离散地图 → 搜索得到无碰路径 → 路径抽稀/平滑
         → 分配时间 → 轨迹优化 → 控制器跟踪

text

搜索负责尽快找到一条可行的“种子”，轨迹优化负责把种子变成更光滑、更安全、更符合动力学约束的运动。两者不是互相替代的关系。

二. 图搜索#

0. 把地图变成图#

图由节点和边组成。栅格地图中，每个可通行栅格可以作为节点，相邻栅格之间建立边；边的代价可以由距离、障碍物代价、转向代价等组成

需要先约定：

使用四邻域、八邻域还是三维邻域
斜向移动的代价是否为 $\sqrt 2$
能否穿过两个对角障碍物之间的缝隙
未知区域是禁止通过还是增加代价
碰撞检查按一个点、圆形半径还是完整 footprint 进行

约定不同，即使算法都叫 A*，结果也可能不同

1. Dijkstra#

Dijkstra 按当前已知的最小累计代价 $g(n)$ 向外扩展。只要边权非负，它能找到最小代价路径

它不知道目标在哪个方向，因此可能向四周扩展大量与目标无关的节点。可以把它理解为 A* 在启发函数 $h(n)=0$ 时的特殊情况

2. A*#

A* 使用：

f(n)=g(n)+h(n)

$g(n)$ ：从起点到当前节点已经付出的代价
$h(n)$ ：从当前节点到终点的剩余代价估计
$f(n)$ ：经过当前节点到达目标的总代价估计

启发函数决定搜索是否既快又保持最优性。

可采纳性 admissibility：

h(n)\le h^*(n)

其中 $h^*(n)$ 是真实最小剩余代价。可采纳启发不能高估真实代价；在标准条件下，A* 因此能够保持最优性

一致性 consistency：

h(n)\le c(n,n')+h(n')

它相当于启发函数满足三角不等式。一致性通常蕴含可采纳性，并使沿路径的 $f$ 值不下降；在常见的 graph-search A* 中，这能避免或减少节点重新打开

在四邻域等代价栅格中常用曼哈顿距离，在允许对角移动时可使用 octile distance。直接使用与运动模型不匹配的启发函数，可能失去效率甚至最优性保证

3. JPS#

JPS（Jump Point Search）是规则栅格上对 A* 的对称性剪枝。空旷栅格中，大量不同的节点展开顺序实际对应同一类路径；JPS 沿一个方向“跳过”中间节点，只保留跳点和强制邻居，从而减少展开节点数

适用前提：

经典 JPS 针对规则、均匀代价栅格
邻域、对角通行和碰撞规则必须与剪枝规则一致
存在复杂非均匀代价、朝向状态或运动学约束时，不能直接套用经典 JPS 的最优性结论
JPS 优化的是搜索过程，不会自动解决路径平滑和动力学可行性

三. 采样式规划#

图搜索先离散出节点和边，再在图上找路；采样式规划不枚举完整规则栅格，而是在连续状态空间中抽样并逐渐建立连通结构。

1. RRT#

RRT（Rapidly-exploring Random Tree）从起点开始生长一棵树：

在状态空间随机采样
找到树中离采样点最近的节点
朝采样点延伸一小步
通过碰撞检查后把新节点加入树

它倾向于快速探索尚未覆盖的区域，适合高维、非凸以及带复杂约束的空间。基础 RRT 更关心尽快找到可行解，不保证第一次得到的路径质量好。

2. RRT*#

RRT* 在加入新节点时选择更优父节点，并对附近节点重新连接。采样数趋于无穷时，它具有渐近最优性；但有限时间内的速度和路径质量仍受采样、距离度量、扩展方法和碰撞检测影响。

3. PRM#

PRM（Probabilistic Roadmap）先在自由空间采样一批节点，连接可直接到达的邻居形成路线图，再在查询时把起终点接入图中。

RRT/RRT* 更常用于单次查询和在线探索
PRM 更适合在同一较稳定环境中反复查询不同起终点
狭窄通道对采样效率是典型挑战

四. 从折线路径到可执行轨迹#

1. LOS 可见性平滑#

栅格 A* 的结果常包含很多相邻格点和锯齿。LOS（Line of Sight）平滑的基本思路是：

从当前保留点开始，尝试直接连接更远的路径点
检查这条线段是否穿过障碍物
若无遮挡，就删除中间冗余点
若有碰撞，则保留上一个仍可见的点并继续

LOS 只改善几何折线，不自动保证曲率、速度和加速度连续。

2. B 样条#

B 样条用一组控制点、节点向量和基函数表示参数曲线。它的优点包括局部支撑、阶次和连续性可控，修改一个控制点通常只影响附近曲线。

但“把 A* 点直接拟合成 B 样条”不一定安全：平滑后的曲线可能切进障碍物。需要通过安全走廊、控制点约束、距离场代价或连续碰撞检查保证安全。

3. Hybrid A* 与 Kinodynamic A*#

Hybrid A* 通常在连续位姿空间中传播满足运动学约束的短运动，并把状态映射到离散索引进行去重。它比只搜索 $(x,y)$ 的 A* 更容易生成车辆能转过去的路径。

Kinodynamic A* 进一步把速度等动态状态和控制输入放进搜索。例如：

\mathbf x=[\mathbf p,\mathbf v],\qquad \mathbf u=\mathbf a

每条边不再是走到相邻格，而是在一段时间内施加某个控制输入，积分动力学到达新状态，因此它能处理更多情况：

起点具有非零速度
速度、加速度和转向速率限制
几何上能通过但来不及刹车的情况
到达同一位置但速度/朝向不同，后续可行性也不同的情况

五. 轨迹优化与 MINCO#

1. 分段多项式#

常见轨迹把时间轴分成 $M$ 段，每段使用多项式：

\mathbf p_i(t)=\sum_{k=0}^{N}\mathbf c_{i,k}t^k

优化变量可以是全部多项式系数，也可以是中间点、各段时间和边界导数。段与段之间要满足位置、速度、加速度等连续性

常见光滑代价是速度、加速度、jerk 或 snap 的平方积分。例如最小 snap：

J=\int_0^T\left\|\mathbf p^{(4)}(t)\right\|^2dt

2. 微分平坦#

若系统是微分平坦的，可以选取一组平坦输出，使系统状态和控制输入都能由这些输出及其有限阶导数代数地恢复，这样可以在较低维的输出空间中设计轨迹，而不必在优化的每一步都显式积分完整动力学

不过这不等于任何机器人的朝向都能随意独立，例如：

差速或非完整约束的底盘，朝向往往与平面速度方向耦合
全向底盘，平移和朝向可能具有更强的独立性，但仍受执行器能力限制

是否独立优化朝向，应从底盘模型、任务需求和可跟踪性出发，而不是因为使用了多项式或 MINCO 就默认可以独立。

3. MINCO#

MINCO（Minimum Control）是一类最小控制量分段多项式轨迹表示。它利用多段最小控制代价问题的最优性结构，把大量多项式系数消去，使用较稀疏的中间点、边界状态和分段时间来参数化轨迹。

需要区分：

MINCO 是轨迹表示和高效求导/参数化框架，不是“输入路径就必然成功”的完整规划器
搜索结果或安全走廊可给它提供初值和空间约束
外层优化还要定义时间、障碍物、速度、加速度、控制量等代价或约束
优化结果仍需做连续碰撞检查和动力学可行性验证

搜索种子很差、分段时间不合理或障碍代价不光滑时，MINCO 外层优化同样可能陷入局部最优或失败。

4. 凸走廊与 QP#

安全走廊通常用一串位于自由空间中的凸集合包住初始路径。轨迹被约束在这些凸区域内，从而把复杂非凸自由空间拆成局部较容易处理的问题。

若目标是多项式导数平方积分，且连续性、边界条件和走廊约束能写成线性等式/不等式，问题常能形成 QP（二次规划）：

\min_{\mathbf x}\ \frac12\mathbf x^TQ\mathbf x+\mathbf q^T\mathbf x \quad \text{s.t. } A\mathbf x=\mathbf b,\ G\mathbf x\le\mathbf h

凸 QP 的局部最优也是全局最优；但原始的避障、时间分配、非线性动力学或非凸地图并不会因为写了一个 QP 就自动变成凸问题。安全走廊本身通常依赖前面的搜索种子。

5. L-BFGS 与线搜索#

L-BFGS 是有限内存拟牛顿法。它不保存完整 Hessian，而是用最近若干次变量和梯度变化近似逆 Hessian 作用，适合变量较多的光滑无约束优化。

若原问题有约束，常见做法是变量重参数化、障碍/可行性罚函数或增广拉格朗日等；具体做法会改变优化性质。

线搜索为当前下降方向选择步长：

Armijo 条件要求新点获得足够的函数下降
Wolfe 条件在充分下降之外增加曲率条件，避免步长过小并改善拟牛顿更新

轨迹优化“不收敛”时优先检查：

初始轨迹是否已经严重穿障碍或违反动力学
目标函数和梯度是否实现一致，可用有限差分检查
代价是否连续可导，最近障碍物索引跳变是否造成梯度突变
不同代价项的量纲和数量级是否失衡
时间变量是否接近零或产生数值病态
非凸问题是否陷入由初值决定的局部极小

六. 地图表示#

1. 占据栅格与 log-odds#

占据栅格为每个格子维护“被障碍占据”的概率。直接反复相乘概率不方便，因此常转换为 log-odds：

l(m)=\log\frac{p(m)}{1-p(m)}

在常见独立观测假设下，可用加法增量更新：

l_t(m)=l_{t-1}(m)+l(m\mid z_t)-l_0(m)

$z_t$ 是当前传感器观测， $l(m\mid z_t)$ 来自反传感器模型。以激光雷达为例，一条射线终点附近可增加占据证据，射线经过区域增加空闲证据。

工程上通常还要：

对 log-odds 上下限裁剪，避免一次错误长期无法纠正
区分未知、空闲和占据
处理传感器盲区、最大量程和动态物体
使用测量时刻的位姿进行射线更新

2. ROG-Map#

ROG-Map（Robocentric Occupancy Grid Map）是面向 LiDAR 运动规划的机器人中心局部占据栅格地图。它不是 SLAM 或里程计算法：输入是已经配准到正确位置的点云和机器人位姿，输出是供碰撞检查、障碍物膨胀和路径规划查询的局部地图。

它主要解决高分辨率三维占据栅格在大场景中的两个问题：如果保存整个场景，内存占用会随范围迅速增加；如果每帧重新计算障碍物膨胀，计算量又会很大。

ROG-Map 的核心思路包括：

机器人中心局部地图：只维护机器人附近固定尺寸的高分辨率区域，机器人移动到一定距离后，局部地图随机器人滑动
固定内存复用：通过全局栅格索引到局部内存索引的映射复用已经分配的数组；滑出局部范围的区域被清空，用来存储新进入的区域
概率占据更新：通过 ray casting 区分射线经过的空闲栅格与命中的占据栅格，并使用 log-odds 累积观测
增量障碍物膨胀：只处理占据状态发生变化的栅格。论文将“非占据变成占据”称为 rising grid，将“占据变成非占据”称为 falling grid，并通过邻域计数更新膨胀层

它的优势是局部范围内查询快、内存有界，适合使用高频 LiDAR 点云进行实时局部规划。但也要注意：

地图滑动后，离开局部范围的占据信息会被遗忘，因此它不是用于长期保存整个场景的全局地图
地图中心跟随机器人，不代表地图坐标系必须跟着机器人旋转；应区分“存储窗口滑动”和“坐标表达变化”
它依赖正确的点云时间戳、位姿和 LiDAR 外参；上游定位错了，地图仍会被错误更新
原论文主要在三维 LiDAR 无人机规划中验证，移植到 RoboMaster 底盘时仍需重新确定地图高度范围、分辨率、局部尺寸、膨胀距离和机器人 footprint
ROG-Map 只提供环境表示与查询，A*、RRT*、MINCO、MPPI 等规划或控制算法仍是它的下游使用者

更适合使用 ROG-Map 的情况是“只需要机器人附近高分辨率地图做实时避障”；如果任务要求保存完整全局地图、全局探索覆盖率或返回很久以前经过的区域，则还需要单独的持久化全局地图。

3. 距离变换、SDF 与 ESDF#

二值障碍地图只告诉你“碰撞/不碰撞”。距离变换为每个栅格计算到最近障碍物的距离。

SDF：带符号距离场，用正负号区分障碍内外
ESDF：欧氏带符号距离场，数值对应到最近表面的欧氏距离

距离场的作用包括：

快速查询安全距离
通过距离梯度把轨迹推出障碍物
生成随距离连续变化的膨胀代价
给轨迹优化提供较平滑的障碍物代价

需要注意，离散 ESDF 在最近障碍物切换处不一定处处光滑；地图更新延迟也会使优化器使用过期距离。

4. 体素地图#

三维体素地图把空间划分成立方体单元。常见表示各自解决的问题不同：

OctoMap：基于八叉树的概率占据地图，能表达占据、空闲和未知，并支持多分辨率存储
TSDF/ESDF 体素地图：TSDF 适合融合表面和生成网格，ESDF 适合查询到障碍物的距离和梯度
iVox 一类增量体素结构：常用于高频点云近邻查询和局部地图维护；它不天然等同于带占据概率的 OctoMap，也不天然提供 ESDF

选择地图结构前先问规划器需要什么查询：只要碰撞、需要概率、需要最近邻，还是需要连续距离和梯度。

5. 语义地图#

占据栅格回答“这里能不能走”，语义地图还可以回答“这里是什么”。语义信息可以附在栅格、体素、实例对象或拓扑节点上，例如：

对手、队友、裁判系统设施
禁行区、补给区、坡道等区域类别
可移动障碍物和永久障碍物
不同区域的速度限制或通行代价

语义地图不应直接覆盖几何安全判断。类别识别会出错，因此应同时保留几何观测、置信度、时间戳和信息有效期。

七. 动态障碍物感知、跟踪与预测#

1. 点云分割#

原始点云是一堆空间点，避障系统通常需要先去除无效点和机器人自身，再提取有意义的结构。

地面提取：基于高度阈值、平面拟合、法向或分区模型区分地面与非地面点
欧式聚类：按点间距离把相邻点聚成簇，实现简单、依赖点云密度和距离阈值
区域生长：依据法向、曲率或其他局部相似性合并邻域

聚类结果不一定等于真实物体：一个物体可能被分裂，多辆相邻机器人可能被合并。后续跟踪需要面对这些误差。

2. 动态障碍物检测#

“当前看到一个点云簇”和“它正在运动”是两个问题。常见动态检测思路包括：

背景地图差分：当前观测与静态地图不一致
多帧点云差分或占据变化：观察空间占据随时间的变化
场景流、光流或法向/几何一致性分析
语义分割或目标检测：利用类别先验寻找可能运动的对象
定位补偿后的聚类速度估计

必须先补偿机器人自身运动，否则机器人一动，静态墙面也会表现得像动态物体。单帧语义只能说明“它可能会动”，不能证明当前正在运动。

3. 多目标跟踪#

多目标跟踪把每帧检测连接成随时间连续的航迹，典型流程是：

点云/图像 → 检测与聚类 → 状态预测 → 数据关联
          → 滤波更新 → 航迹创建/确认/删除

text

数据关联可先构造检测与航迹之间的代价矩阵，再使用门控排除明显不可能的匹配，最后用匈牙利算法等方法求一对一匹配。

卡尔曼滤波适合线性高斯的恒速/恒加速度模型；EKF、UKF 或其他方法可处理非线性。无论使用哪种滤波器，都要设计航迹管理：

连续命中多少帧后确认新目标
丢失多少帧后删除
遮挡时只预测多久
目标合并、分裂和身份交换怎样处理

4. 运动预测#

最简单的恒速模型为：

\mathbf p(t+\Delta t)=\mathbf p(t)+\mathbf v(t)\Delta t

还可以使用恒加速度、转弯模型、速度衰减模型、交互多模型或学习式预测。模型越复杂不一定越可靠，尤其在观测短、碰撞频繁的比赛场景中。

预测结果应携带不确定度。预测时间越远，障碍物可能出现的区域通常越大；只发布一条没有置信范围的“确定未来轨迹”会让规划器过度自信。

八. 动态避障#

0. 静态避障和动态避障的区别#

把动态障碍物当前位置直接写进普通代价地图，只能让机器人避开“它现在在哪”，不能回答双方未来是否会同时到达同一位置。

动态避障需要同时考虑空间和时间。两个机器人经过同一个位置，只要时间不同就可能安全；两条几何路径没有交叉，也可能因为机器人轮廓和预测误差发生碰撞。

1. 三类基本路线#

**反应式方法：**根据当前局部障碍和速度快速输出控制，例如 DWA/DWB、速度障碍 VO/ORCA 一类思想。响应快，但长远行为和复杂交互可能不足。

**预测式局部优化：**把障碍物预测轨迹加入 MPC、MPPI 或轨迹优化的代价/约束，在有限时域内联合考虑跟踪、避障和控制平滑。

**时空搜索：**把时间加入状态，例如 $(x,y,t)$ 或 $(x,y,\theta,v,t)$ ，搜索时检查机器人与动态障碍物在同一时刻是否碰撞。表达直接，但状态维度和计算量会上升。

2. MPPI#

MPPI（Model Predictive Path Integral Control，模型预测路径积分控制）是一种基于采样的随机最优控制方法。它与前面 RRT 的“在状态空间采样节点”不同：MPPI 在一段有限预测时域内，对控制序列加入随机扰动，通过动力学模型展开出大量候选轨迹，再根据每条轨迹的总代价加权更新控制序列。

设当前控制序列为：

U=\{\mathbf u_0,\mathbf u_1,\ldots,\mathbf u_{N-1}\}

第 $k$ 条样本给每个控制量加入噪声 $\boldsymbol\epsilon_{k,t}$ ，并使用模型向前展开：

\mathbf x_{t+1}^{k}=f(\mathbf x_t^{k},\mathbf u_t+\boldsymbol\epsilon_{k,t})

计算每条样本轨迹的总代价 $S_k$ 后，代价低的样本获得更大权重：

w_k=\frac{\exp\left(-\frac{S_k-\rho}{\lambda}\right)} {\sum_j\exp\left(-\frac{S_j-\rho}{\lambda}\right)}, \qquad \rho=\min_k S_k

再用加权噪声修正控制序列：

\mathbf u_t\leftarrow \mathbf u_t+\sum_k w_k\boldsymbol\epsilon_{k,t}

每个控制周期只执行优化序列的第一个控制量，然后把预测窗口向前移动、读取新状态并重新采样，这就是模型预测控制的 receding horizon 思路。

一条 MPPI 的代价函数通常需要同时考虑：

与全局路径或参考轨迹的偏差
到目标点的距离和目标姿态
障碍物碰撞、机器人 footprint 和安全距离
速度、角速度、加速度和底盘运动学限制
控制变化量与轨迹平滑性
倒车、横移或旋转等行为偏好

MPPI 的几个关键量：

预测时域 $N\Delta t$ ：太短看不到远处风险，太长则计算量增大且模型误差累积
样本数：更多样本通常覆盖更充分，但计算成本近似随样本数和时间步数增加
采样噪声方差：太小跳不出当前控制附近，太大会产生大量明显不可行样本
温度 $\lambda$ ：控制不同代价样本的权重集中程度
运动模型：必须与差速、全向、阿克曼等实际底盘匹配
代价权重：不同项量纲相差很大时，权重会让机器人只顾跟路径或只顾躲障碍

和其他方法相比：

DWA/DWB 常采样速度或较简单的短时控制，MPPI 采样整段控制序列，能表达更丰富的未来动作
梯度轨迹优化依赖目标函数梯度，MPPI 主要依靠随机 rollout 和代价加权，不要求对完整系统显式求梯度
MINCO 更偏向连续轨迹表示与优化，MPPI 更常作为滚动时域局部控制器；二者可以位于不同层，而不是必须二选一

MPPI 并不会自动获得动态障碍预测能力。如果 rollout 只查询“当前时刻”的静态代价地图，它主要是对障碍进行反应式避让；只有把障碍物未来位置或随时间变化的占据代价加入每个预测时刻，才是在显式利用动态预测。

实车还要关注模型误差、里程计延迟、计算超时和随机采样波动。所有样本均碰撞或控制计算超时时，应触发停车或明确的降级逻辑，不能继续发送旧指令。

此外，给碰撞设置一个很大的软代价不等于获得形式化安全保证。MPPI 的安全性仍取决于地图是否及时、rollout 是否使用完整 footprint、预测时域是否覆盖制动过程，以及底层是否另有碰撞监测和急停。

3. 动态障碍代价#

一个简单的预测避障代价可以根据机器人轨迹 $\mathbf p_r(t)$ 与第 $j$ 个障碍预测 $\mathbf p_j(t)$ 的距离构造：

d_j(t)=\|\mathbf p_r(t)-\mathbf p_j(t)\|

当 $d_j(t)$ 小于安全距离时增加代价或判为不可行。实际还要加入：

双方 footprint，而不是把双方都当成质点
障碍预测协方差和定位误差
感知、规划、控制和通信延迟
机器人制动距离
传感器视野外的未知风险