ROS2 DWB 局部规划器深度优化：零动态内存分配的 BFS 算法
ROS2 DWB 局部规划器深度优化：零动态内存分配的 BFS 算法
ROS2 DWB 局部规划器深度优化：零动态内存分配的 BFS 算法
背景：被忽视的内存杀手
在 ROS2 的 Nav2 导航框架中，DWB 局部规划器被广泛使用。然而，在长期运行或内存受限的嵌入式设备上，DWB 内部的 costmap_queue 会引发一个隐蔽的风险：极高频的动态内存分配。
在 20Hz 的控制频率下，costmap_queue 相关的内存分配次数可达百万级别。海量的小内存分配不仅带来巨大的 CPU 开销，还会产生严重的内存碎片，导致长期运行后内存水位不断上涨。
探究其本质，DWB 在评估轨迹时（MapGridCritic），需要计算地图上各个点到目标点/路径的曼哈顿距离，这本质上是一个广度优先搜索（BFS）的过程。本文将从源码剖析其性能瓶颈，并实现一套“零动态分配”的极限优化方案。
DWB 源码瓶颈分析
DWB 使用 MapGridCritic 进行距离传播，其核心逻辑如下：
1
2
3
4
5
6
7
8
9
void MapGridCritic::propagateManhattanDistances()
{
while (!queue_->isEmpty()) {
costmap_queue::CellData cell = queue_->getNextCell();
// 计算并填充曼哈顿距离
cell_values_[cell.index_] = CellData::absolute_difference(cell.src_x_, cell.x_) +
CellData::absolute_difference(cell.src_y_, cell.y_);
}
}
而在入队时（enqueueCell），算法会构建一个包含 1 个 double 和 5 个 unsigned int 的 CellData 对象，并将其压入队列中。
致命的调用链分析： 在 20Hz 的计算频率（即每 50ms 一次）下，DWB 的每次迭代都会触发：
1
2
3
4
5
6
7
8
9
prepare()
└── createMap()
└── queue_->reset() <-- 清理内部数据结构
└── queue_->enqueueCell() × N（初始路径点数量）
└── enqueue(distance, CellData)
└── std::map::insert / std::vector::push_back <-- 【高频动态内存分配点】
└── propogateManhattanDistances()
└── while(!isEmpty()) { getNextCell() }
└── front() + pop() + enqueueCell() × 4 <-- 【海量动态内存分配点】
可以看到，在寻找最近距离的过程中，底层大量调用了 std::map 的插入和 std::vector 的扩容。
为什么 DWB 原版 BFS 这么慢？
一个标准的 BFS 传播算法主要包含四个要素：队列、已探索标记、代价地图、状态重置。我们深入源码，看看 DWB 的 costmap_queue 是如何实现的：
- 优先级队列过度设计： DWB 底层竟然使用了
std::map<double, std::vector<CellData>> item_bins_;来做优先级队列。每次插入元素都会触发红黑树节点的动态内存分配。 - 笨重的数据结构：
CellData携带了大量不必要的坐标信息，体积臃肿。 - 代价地图： 使用
std::vector<double>存储，占用空间大（8 字节），对 CPU 缓存（Cache）不友好。 - 重置开销： 每次规划前，都需要清除
map，并用std::fill重置一维数组，开销不容小觑。
零动态分配的 BFS 优化方案
针对上述痛点，我们可以实现一个零动态分配（Zero Dynamic Allocation）的 BFS 算法。
所谓“零动态分配”，即在初始化时一次性分配好算法所需的最大连续内存块。运行时只改变指针和数值，彻底杜绝运行时的 new/delete 或 malloc/free。
1. 摒弃 map，回归朴素的 Ring Buffer
核心认知： 在网格地图上计算曼哈顿距离，每一步的代价都是 1。根据图搜索理论，对于权重恒定为 1 的图，普通的先进先出队列（FIFO）天然就能保证距离的单调递增，根本不需要用 map 来做按权重排序！
因此，我们直接预分配一个与地图栅格总数相同的一维数组作为循环队列（Ring Buffer）：
1
2
3
4
5
6
7
8
std::vector<uint32_t> queue_buf_; // 预先分配好全图大小，永不扩容
uint32_t qHead_ = 0, qTail_ = 0;
// 入队 (极为轻量)
queue_buf_[qTail_++] = idx;
// 出队
const uint32_t cur = queue_buf_[qHead_++];
由于是指针自增，不涉及任何对象创建，入队/出队速度提升几个数量级。
2. 压缩代价地图，利用哨兵值合并标记
原版使用 std::vector<bool> 做访问标记，std::vector<double> 存距离。 其实，我们可以将 double 压缩为 int16_t（从 8 字节降至 2 字节）。这不仅节省了 75% 的内存，更重要的是极大地提高了 CPU Cache 命中率！
同时，我们可以利用负数作为未访问哨兵，将“已探索标记”和“代价地图”合二为一：
cost_[nIdx] < 0：未访问cost_[nIdx] >= 0：已访问且记录了距离值
3. 重置优化：Memset 魔法
因为队列的内存是固定的，重置队列只需要把头尾指针归零即可（qHead_ = 0, qTail_ = 0），队列内部遗留的脏数据在下一轮会直接被覆盖，完全无需清理。
对于代价数组，我们需要用哨兵值（未访问）填充。这里有一个极其高效的技巧：
1
2
3
// 方案：使用 -1 作为哨兵（int16_t 下两字节都是 0xFF）
constexpr int16_t UNREACHABLE = -1;
std::memset(cost_.data(), 0xFF, total_ * sizeof(int16_t));
为什么用 memset 0xFF？ 因为 int16_t{-1} 的二进制表示为 0xFFFF，每个字节都是 0xFF。直接使用底层的 memset 按字节写入，比 std::fill 快得多。
进阶探讨：版本号机制与脏格记录
在重置地图时，如果当前只探索了一小片区域，用 memset 刷写整张大地图会不会太亏了？这里引出另一种优化思路——版本号机制配合脏格记录。
我们额外维护两个数组：
version_：记录每个格子最后更新时的“规划轮次数”。dirty_：记录上一轮修改过的格子索引。
逻辑流转：
1
2
3
4
5
6
7
8
9
10
11
12
13
// 1. 重置阶段：只清理上一轮弄脏的格子
for (uint32_t i = 0; i < dirty_count_; ++i) {
cost_[dirty_[i]] = int16_t{-1};
}
dirty_count_ = 0; // 重置脏桶数量
// 2. 探索更新阶段：对比版本号
if (version_[nIdx] != cur_version_) {
version_[nIdx] = cur_version_; // 更新版本号
cost_[nIdx] = curCost + 1; // 写入距离
dirty_[dirty_count_++] = nIdx; // 记录为脏格
queue_buf_[qTail_++] = nIdx; // 入队
}
性能测算与抉择： 这种方案看起来很美，但实际表现取决于 BFS 的扩散范围：
- 场景 A：扩散范围极小（N < 200格，< 5%）
- 脏格清理：200次随机写，耗时约 0.05-0.1μs。
- memset：8KB（假设）连续顺序写，耗时也在 0.1μs 量级。两者差距不大。
- 场景 B：扩散范围中等（N > 500格，> 12%）
- 脏格清理：大量随机地址写入，产生严重的 CPU Cache Miss。
- memset：大块内存顺序写，SIMD 自动优化，速度极快。memset 胜出。
- 场景 C：全图扩散
- memset 完胜。
结论： 在实际运行中，局部规划器往往需要评估到地图边缘的代价。此时全量重置的 memset 因为是严格的连续内存顺序写，反而能跑到最高性能。因此，直接采用“Ring Buffer + memset”的组合，是工程上收益最高、代码最简洁的最优解。
总结
通过摒弃过度设计的 std::map 优先队列，改用预分配的一维数组 Ring Buffer，并将状态压缩至 int16_t 并配合 memset，我们将 DWB 中的 BFS 传播变成了一个内存零分配、对 CPU 缓存极度友好的高性能模块。这对于提升机器人在复杂环境下的导航稳定性，具有立竿见影的效果。