企业官网建设流程全解析

最近把 EGO-Planner 这篇论文逐行读了一遍，越读越觉得它解决的是很多做无人机规划的人都踩过的痛点：ESDF 的构建和更新成本太高。EGO-Planner 是一套 ESDF-free 的 gradient-based local planner for quadrotors，也就是不依赖欧氏距离场，直接用梯度优化做四旋翼局部轨迹规划。它最大的贡献不是又调快了一个规划器，而是把“梯度从哪来”这个问题换了个解法，从此不需要维护一张全局距离场，也不需要做耗时的增量更新。如果你在做无人机避障、自主飞行、甚至机械臂运动规划，这篇文章都值得花时间啃一啃。

1. 从ESDF的“刚需”到被替代：EGO-Planner为什么要绕开距离场

1.1 过去基于梯度的规划器为什么离不开ESDF

在无人机局部规划里，基于优化的方法长期占据主流。优化需要一个目标函数，而避障项几乎都依赖轨迹到最近障碍物的距离。传统的做法是把环境编码成 ESDF，也就是每个体素都存一个数值：该体素到最近障碍物表面的距离，以及对应的梯度方向。

有了这个场，优化器就能飞快地查询任意轨迹采样点附近的“推斥力”。比如轨迹上一个点在障碍物内部，距离值为负数，梯度指向最近表面；点在障碍物外部，距离值为正，梯度同样指向表面方向。然后把这个距离插进惩罚函数，求导后得到梯度，轨迹自然会被推離障碍物。

这种思路很直观，但代价也很直接：构建 ESDF 相当于给整张地图“焊”了一张连续的势场，体素越多，场的计算和维护就越贵。静态环境还可以预先算一次，动态环境一旦有障碍物移动，就必须局部更新，更新时还要保证距离场的一致性和光滑性。做真机实验的朋友应该都有印象，光是把 ESDF 的更新频率提上去，就够处理器吃点力。

1.2 ESDF的三大痛点：内存、计算、更新延迟

我把 ESDF 方案在工程上的问题归纳成三点：

第一是内存。假设一个 100m × 100m × 5m 的飞行区域，分辨率 0.1m，体素数量就有足足五千万个。每个体素只存 4 字节浮点数加 3 个方向的梯度，内存占用轻松超过一个 G。尽管工程上可以用截断距离场（Truncated ESDF），只保存靠近障碍物表面的截断距离，但内存压力依然不小。

第二是计算量。构建一张完整的距离场，哪怕用快速巡游算法，也需要对整个地图做多轮扫描。更麻烦的是增量更新：当激光雷达或视觉地图发现障碍物发生变化，即使只是很小一块区域，也需要在受影响范围内重新传播距离值。很多团队的做法是把这个更新放到单独线程里跑，规划线程等更新完成后再优化，导致系统反应延迟。

第三是更新延迟带来的安全问题。一个典型的场景：一个障碍物刚刚出现在轨迹旁边，ESDF 还没更新完，优化器拿到的还是旧距离，轨迹自然不避让。对于高速飞行的四旋翼，哪怕延迟只有几十毫秒，也可能撞上障碍物。

EGO-Planner 的出发点很直接：能不能不做 ESDF，只在优化需要梯度的时候，从原始地图里“现算”出所需要的障碍物信息？这样内存省了，更新延迟的问题也自然被绕过，因为每次规划都读取当前最新的地图数据。

1.3 EGO-Planner的思路转变：从建场到按需索取

EGO-Planner 的核心转变，是把“距离场”这个全局数据结构，变成了“采样点级别的距离近似”。它仍然需要地图，但地图可以是普通的占据栅格、体素地图或八叉树，不需要预计算距离值。优化器在计算碰撞代价时，只针对轨迹上的采样点，在局部范围内找到最近的障碍物占据单元，然后基于这个最近障碍点构造一个局部惩罚函数。

这个思路有点像你晚上在房间里走动，不需要知道房间每个角落到墙体的精确距离，只需要在手或脚即将碰到墙的那一刻，感知到墙在哪个方向、离自己多近，然后往反方向避让一下。EGO-Planner 就是把这种“触觉式”的避障逻辑搬进了轨迹优化里。

这种按需索取的方式，让规划器的每次迭代都直接面对当前地图，地图变了梯度也跟着变，天然适合动态环境。当然，它也引入了一个新的难题：如何保证这种局部近似出来的梯度方向足够可靠、不会让优化发散。下一节专门拆这个。

2. 无ESDF的梯度从哪来：碰撞惩罚的构造逻辑

2.1 一个直觉：软管顶到墙的斥力模型

要理解 EGO-Planner 的梯度构造，可以先想象一根柔软的胶管，被什么东西推着向一堵墙靠近。墙不会预先把“离墙1米内有斥力”这种信息广播到整个房间，你只会在胶管触到墙的那一刻，在接触点感受到一个把管头向外推的力。力的大小取决于胶管压进墙的深度，方向则沿着墙面的法向。

EGO-Planner 的碰撞惩罚就是类似思路。它检查的是 B 样条轨迹的采样点，以及轨迹控制点（具体机制稍后说）与障碍物表面的“接触深度”。问题来了：没有距离场，怎么知道接触深度和方向？答案是从占据地图里找采样点附近最近的障碍物占据单元，用这个最近点和采样点之间的相对位置，当作“穿透方向”。

2.2 最近障碍点查询与惩罚方向的计算

具体做法可以简化成两步。第一步，对轨迹上的某个采样点 p，以 p 为中心在给定半径内搜索地图体素，找到最近的一个被占据的体素中心，记为 p_obs。第二步，计算方向向量 n = (p - p_obs) / ||p - p_obs||，这个 n 就可以近似看作障碍物表面在 p 这一侧的法线方向。

有了 n，惩罚就可以定义成一个关于 p 到障碍物表面距离 d 的函数。注意这里 d 并不是精确的欧氏距离，而是基于最近体素中心算出来的近似距离。论文里用了一种截断式的惩罚：

• 如果采样点 p 已经处于“侵入”状态（比如离障碍物中心小于半径 R，或者接近轨迹允许的最小距离），惩罚函数取一个较大的二次项；
• 如果 p 在安全范围内但不够远，惩罚则按距离的指数或二次形式衰减；
• 如果 p 离障碍物足够远，惩罚直接为 0。

这样构造出的碰撞代价是一个分段光滑函数，对 p 求梯度后，梯度方向基本就是 n 的方向，大小由侵入深度决定。优化器沿着负梯度方向调整控制点，轨迹就会像被墙顶回来的软管一样，逐渐滑到安全位置。

这里有个细节：梯度方向只是“基本”指向 n。因为实际最近障碍点可能在离散体素网格上跳变，所以梯度会有轻微的不连续。EGO-Planner 的应对办法是使用近似但光滑的惩罚函数，并且在优化迭代中允许步长控制，让这种不连续不至于把优化逼到发散。

2.3 为什么这种近似梯度在优化中不会跑偏

很多人第一反应是：只靠局部最近障碍点算梯度，不会把轨迹引到错误的角落吗？这个问题确实存在，但 EGO-Planner 用两个机制把它控制住了。

第一，B 样条本身的局部支撑性。轨迹上每个采样点只受相邻几个控制点影响，控制点移动一点点，只会影响局部轨迹。这天然限制了单次梯度更新对轨迹全局的改动，不容易被某个错误的局部梯度带跑。

第二，优化是一个多轮迭代的过程。第一轮梯度可能有点偏，但轨迹被推动后，采样点改变了位置，下一次迭代会重新查询最近障碍点，重新计算梯度方向。只要障碍物分布不是极端病态，梯度方向会随着轨迹远离障碍物而逐渐收敛到合理的避让路径。

换句话说，EGO-Planner 用“动态重查询”替代了“静态距离场”，用迭代逼近替代了全局精确解。它不是在建立精确势场，而是在每个迭代步里构造一个“够用的”势场。

3. B样条轨迹优化：目标函数与时间重分配任务

3.1 均匀B样条的参数化与凸包性质

EGO-Planner 把轨迹表示成均匀 B 样条。均匀的意思是控制点之间的时间间隔 Δt 是一个固定值。一条由 N 个控制点 Q0 到 QN-1 定义的 B 样条，任意时刻的轨迹位置都可以写成控制点的加权组合，权重由 B 样条基函数决定。

均匀 B 样条最让规划者喜欢的性质是凸包性：整条轨迹必定落在控制点张成的凸包内。也就是说，如果所有控制点都位于某个无碰撞的凸区域里，那这整条 B 样条就一定不会碰到障碍物。

不过真实环境很少是凸的，所以 EGO-Planner 并不严格要求控制点全部在自由空间里，而是把碰撞惩罚施加到轨迹的采样点和控制点上。通过优化，让控制点整体离开障碍物。B 样条的保凸性让这个优化过程不容易出现“控制点安全但轨迹穿墙”的诡异情况。

3.2 目标函数的长相：碰撞、速度、加速度、平滑度

整个轨迹优化目标可以写成几个代价项的加权和：

• 碰撞代价：根据前面说的障碍物近距惩罚，对控制点或采样点计算，并把惩罚叠加到轨迹上；
• 速度代价：包含最大速度约束的软惩罚，超限部分会产生梯度；
• 加速度代价：对应机体的机动能力，四旋翼加速度超限会明显让轨迹不可行；
• 平滑度代价：鼓励控制点分布平滑，减少不必要的抖动；
• 距离代价：在可行的情况下，尽量让轨迹不要距离障碍物太近，给控制不确定性留余量。

最终的优化问题就是调整控制点位置，让这串加权代价之和最小。实际代码里权重不是随便设的，通常是碰撞权重最高，其次是加速度约束，然后才是平滑度和距离代价。权重关系调对了，轨迹才能在安全与平滑之间找到平衡。

3.3 时间重分配：不改几何也能救回动力学可行性

B 样条优化有个容易忽略的问题：控制点位置优化好了，但轨迹的速度和加速度可能不满足动力学限制。因为速度和加速度本质上等于控制点差分除以时间间隔 Δt：ΔQ / Δt 和 Δ²Q / Δt²。几何路径已经确定的时候，想降低速度、加速度，最直接的办法不是重新挪控制点，而是把 Δt 增大。

EGO-Planner 里专门做了时间重分配。它会检测当前轨迹的速度和加速度峰值，如果超过限制，就相应增大对应轨迹段的时间间隔，然后重新优化控制点。这个步骤可以迭代几次，直到动力学约束满足。

从直觉上理解，这相当于把一段已经画好的路径“放慢”或者“局部放慢”。几何形状基本不变，但时间曲线被拉开，速度和加速度就降下来了。这个策略很像你在高速上遇到弯道时松油门，而不是重新选择一条路线。它省掉了二次规划的大量计算，也让优化器的收敛更稳。

4. 从论文到工程：复现时需要想清楚的细节

4.1 地图数据结构与最近点查询的选型

ESDF-free 听起来轻松，但“找最近障碍点”并不是免费的。地图数据结构和最近点查询的速度，直接决定规划器的实时性。我整理了一下常见选型：

真机场景里，很多团队用局部体素地图配合固定半径的搜索窗口。因为采样点附近的障碍物通常不会太多，小半径内暴力遍历几万个体素也够快。相比之下，全局 KD-Tree 在障碍物数量大时查询很快，但重建成本偏高。

我的建议是：如果你的地图本身是八叉树结构，那就直接用八叉树做最近障碍点搜索，省去额外的数据结构；如果你只有稠密点云，用 KD-Tree 会更省心。核心是把查询半径限制在轨迹膨胀半径的 1.5 到 2 倍内，既保证梯度来源，又控制运算量。

4.2 初始轨迹与前端路径：优化器不是万能的

EGO-Planner 是一个局部规划器，它需要一条初始轨迹才能开始优化。初始轨迹可以从全局规划器给出的路径离散采样，再用 B 样条拟合得到，也可以直接用上一时刻的规划结果续传。

我踩过的一个坑是：初始轨迹如果严重穿过障碍物核心，优化器需要很多次迭代才能把它推出来，而且过程中容易陷入奇怪的局部形状。更稳的做法是先用 A* 或 RRT* 跑出一条大致无碰的路径，再把它剪枝成少量关键点，然后用 B 样条插值拟合。这样初始轨迹离最终解不远，优化的迭代次数能少一个数量级。

如果只做论文复现，可以先用一个简单的路径生成器产生直线或圆弧轨迹，然后故意让它穿过一个障碍物，观察碰撞代价如何把轨迹推开。这会让你更直观地理解前端路径质量和优化收敛速度的关系。

4.3 调参心得：权重顺序比权重绝对值更重要

复现论文时，不少人一上来就对着代码里的权重较劲，试图找到“最优参数”。我的经验是，先抓权重数量级关系，再抓具体数值。EGO-Planner 类的梯度优化，碰撞代价权重通常要比平滑度权重大 1 到 2 个数量级。如果平滑度权重太大，轨迹会变得非常保守，甚至绕过没必要绕的障碍；如果碰撞权重太大，轨迹又可能扭来扭去，加速度超限。

还有一个关键参数是惩罚距离 ρ，也就是轨迹允许离障碍物多近。这个值应该大于飞行器半径加感知不确定度。比如四旋翼半径 0.3m，传感器噪声 0.1m，那惩罚距离至少设到 0.4m 以上。设太小，轨迹看着贴障碍物飞，实际飞行器一抖就撞；设太大，狭窄环境根本找不到可行路径。

时间间隔 Δt 也要配合速度上限来调。如果已知最大巡航速度是 5m/s，采样间隔 Δt=0.2s 意味着每米只有 5 个采样点，密集障碍物下漏检风险升高。可以把 Δt 降到 0.1s，代价是控制点更多、优化更慢。这个平衡只能由实际场景决定。

4.4 优化器与迭代次数：别迷信“越多越好”

EGO-Planner 的论文通常只用少量迭代就能收敛，因为局部梯度的目的是“把轨迹推出危险区”，而不是像全局搜索那样找精确最优解。跑实验时我习惯设置 3 到 5 轮迭代，每轮只做几十次 L-BFGS 步。迭代太多，轨迹形状已经基本不变，纯粹浪费算力。

另外，优化器本身也值得检查。用自动微分框架的话，要确认梯度的计算是正确的，尤其是在查询最近障碍点这种非连续环节。这一步最容易出错，一旦梯度方向反了，优化器会把轨迹往障碍物里怼。我的调试技巧是：固定一个障碍物，打印轨迹采样点的碰撞代价和梯度，人工检查梯度是否指向障碍物外侧。这个检查花不了五分钟，却能省下一整天的排查时间。

5. 边界条件与常见误区：什么场景下EGO-Planner会翻车

5.1 窄通道与局部最优：梯度法也不是万能的

ESDF-free 的近似梯度，本质上是“近视眼”，只关心眼前最近的障碍物。当飞行器处在一个很窄的通道里，两侧都有障碍物，前后又有障碍物时，局部梯度可能让轨迹在通道内左右乱顶，迟迟找不到一个顺畅穿过的解。

这种现象在经典梯度优化里叫局部最优。解决思路有几个方向：一是给前端路径提供更干净、更居中的初始轨迹；二是在目标函数里加入“离障碍物距离尽量远”的均匀性约束，让轨迹倾向于待在通道中央；三是增强采样密度，让轨迹对两侧障碍物都敏感。但无论如何，纯局部优化在某些极端场景下依然无解。论文里也提到类似机制的不足，读者不要把它当成万能避障器。

5.2 高速飞行时采样点漏检：一个隐蔽的安全隐患

B 样条轨迹在时间上是连续参数化的，但优化代价只能在离散采样点上计算。如果速度很高而采样点数量不够，就可能出现两个相邻采样点分别位于障碍物两侧、中间轨迹段却穿过障碍物的情况。采样点本身没有碰撞代价，优化器自然认为轨迹安全，结果真机一飞就撞。

应对方式有两种：一是根据轨迹速度动态调整采样点密度，高速段多采样；二是在代价函数里额外检查控制点之间的线段与障碍物的相交情况，把这种“漏检”显式加入罚款。做真机实验前，一定要在仿真里做几次高速穿透测试，别让采样密度问题到飞行那天才暴露。

5.3 ESDF-free 不等于免地图维护，障碍物更新延迟依然存在

有些同学误以为用 EGO-Planner 就能完全摆脱地图维护成本。实际上，它只是不用维护距离场，原始占据地图依然需要实时更新。激光雷达或视觉 SLAM 建出来的地图，如果障碍物移除或新增，地图更新不及时，EGO-Planner 查询到的仍然是旧障碍物位置，同样会导致碰撞。

换句话说，ESDF-free 解决的是“距离信息转化为梯度”的计算开销，而不是“传感器信息转化为地图”的感知开销。在动态环境中，地图的更新频率和延迟才是真正的安全瓶颈。我在实际测试中发现，即使规划器再轻快，感知模块如果只能提供 5Hz 的障碍物更新，飞行速度也得限制在 2m/s 以内，否则反应根本来不及。

5.4 与其他规划器的配合：局部规划器不能脱离全局规划单独存在

EGO-Planner 是局部规划器，这意味着它只负责“接下来几秒钟”的轨迹。如果全局路径完全错误，比如指向一个死胡同或大障碍物后方，局部优化只能沿着错误大方向躲来躲去，最终还是会卡住。

所以实际系统通常安排一个低频全局规划器，比如 1Hz 到 2Hz 在拓扑地图或稀疏图搜索上提供航路点，EGO-Planner 再以 10Hz 到 20Hz 的频率在航路点之间做局部优化。两个频率差一个数量级是常见配置。全局路径的大尺度安全由搜索算法负责，局部轨迹的精细避障和动力学可行由 EGO-Planner 负责。

5.5 对初始化时间和控制点数量的敏感性

虽然论文里的例子看起来都很顺畅，但我的复现经验是：EGO-Planner 对控制点数量比较敏感。控制点太少，轨迹的表示能力不足，无法绕开复杂障碍；控制点太多，优化变量维度高，收敛变慢，还可能出现蛇形抖动。

控制点数量应该根据规划时域和速度估算。比如规划时域 3s，Δt=0.15s，轨迹段长度约 20 个间隔，控制点数量至少需要 23 到 25 个。少于这个数，轨迹可能连一个完整的绕障弯都画不出来；多于这个数，又没有足够梯度信息约束这么多自由度。论文里的实验通常是在精心挑选的控制点数量下完成的，换成自己的场景时要重新校准。

另外，初始控制点的位置也很重要。有些人直接全部初始化为起点或终点，优化器要花很多轮才能把轨迹“拉”出来。更好的做法是用全局路径的离散点作为初始控制点，并保证时间间隔和路径点间距匹配。这一步对收敛速度的影响，可能比调任何权重都大。

我对 EGO-Planner 的整体印象是：它用一种很聪明的工程取舍，把“建场”的成本变成了“查询”的成本，避免了距离场更新带来的实时性痛点，同时保留了梯度优化规划器优雅的轨迹质量。不过它并没有降低对地图更新、前端路径、调参和采样的要求。学习这篇论文时，建议一边读一边在仿真里改参数、看轨迹变化，你会比只看公式更快找到手感。