EgoPlanner轨迹优化实战：从3D无人机到2D地面机器人的算法移植与性能调优

1. 从天空到地面：为什么要把EgoPlanner移植到2D机器人？

大家好，我是老张，一个在机器人圈子里摸爬滚打了十来年的工程师。这些年，我见过不少优秀的轨迹规划算法，从无人机上飞得风生水起，但一到地面机器人项目里就“水土不服”。今天我想跟你聊聊的，就是这么一个典型的例子：EgoPlanner。

你可能听说过EgoPlanner，它是浙江大学FAST-Lab高飞老师团队开源的一个明星算法，在无人机领域，尤其是复杂动态环境下的实时避障，表现非常亮眼。它的核心优势在于，放弃了传统上计算开销巨大的ESDF（欧几里得有符号距离场）地图，转而采用一种更“聪明”的碰撞检测和斥力场方法，把轨迹优化的计算时间压缩到了毫秒级。这对于高速飞行的无人机来说，简直是救命稻草。

但问题来了。我手头最近的项目，是一个在仓库里搬运货物的2D地面移动机器人（AGV）。我们试过TEB（Time Elastic Band），也折腾过FastPlanner，总感觉在密集的货架环境里，要么实时性不够，要么轨迹不够平滑，偶尔还会出现奇怪的“抖动”。这时候，我自然就把目光投向了以高效著称的EgoPlanner。

然而，直接把无人机那套代码拿过来用，立刻就撞墙了。最直观的问题就是：无人机是3D的，它可以向上飞，越过障碍物；但地面机器人是2D的，它只能绕着走。在仿真里，我眼睁睁看着规划出的轨迹试图从障碍物“头顶”（Z轴方向）穿过去，这显然不现实。所以，移植的核心，不是简单的复制粘贴，而是一场从三维空间思维到二维平面思维的“降维”改造。这个过程涉及到地图处理、碰撞力方向、动力学约束等一系列调整，也正是这次实战分享想要解决的核心问题。

我猜，正在看这篇文章的你，可能也遇到了类似的困境：手里有一个在论文或开源社区里评价很高的算法，但它的应用场景和自己的项目对不上。别担心，接下来我会把我从3D无人机代码“魔改”到2D地面机器人可用的全过程，包括踩过的坑、关键的参数，以及最终的代码封装，毫无保留地分享出来。我们的目标很明确：不只是看懂原理，而是真正能让算法在你的工程里跑起来。

2. 核心改造一：锁死Z轴，让地图“扁平化”

移植工作的第一步，也是最关键的一步，就是处理环境表示。在三维的EgoPlanner中，环境通常由点云或体素网格表示，算法会在X、Y、Z三个维度上自由地搜索和优化轨迹。但对于地面机器人，世界被简化成了二维平面（X, Y），Z轴的高度信息要么固定，要么无关紧要。

2.1 地图数据的预处理：从3D点云到2.5D膨胀

原版算法处理障碍物时，默认障碍物是三维空间中的实体。当地面机器人前方出现一个矮柱（比如一个纸箱）时，3D算法可能会计算出一条从上方绕过的轨迹，这显然不行。我们的改造思路是：在数据输入阶段，就强制让所有障碍物在Z轴方向上“膨胀”，模拟出一堵无限高的墙。

具体怎么做呢？在初始化地图（比如ESDF地图或直接处理点云）时，我们需要对每一个障碍物点进行操作。假设我们有一个障碍物点坐标为 (x, y, z)。在原始的3D处理中，这个点只影响它所在的高度层。为了适应2D地面机器人，我会在代码里，将这个点沿着Z轴正负方向进行扩展。

例如，设置一个z_inflate参数（比如5.0米）。那么，这个障碍物点就不再是一个点，而是代表从 z = -z_inflate 到 z = +z_inflate 的一根“柱子”。在构建距离场或进行碰撞检测时，算法会认为这个位置在任何高度都存在障碍物。这样，当轨迹优化器试图在Z方向寻找出路时，会发现此路不通，从而被迫在X-Y平面内寻找绕行路径。

在我的实现里，这个修改主要发生在 SDFMap（距离场地图）的初始化或障碍物设置函数中。下面是关键代码逻辑的示意：

// 假设原始障碍物点云输入为 obstacle_cloud (3D点)