【Navigation2进阶】(十三）：手写概率地图!从log-odds推导到MI互信息探索与Nav2插件实现

前言

• 上一期我们把 RIGGoalSelector 引入了探索系统——通过 RRT 偏向高信息前沿来评估路径代价，比单纯的 Nearest 和 Utility 已经有了明显提升
• 往期内容：
  • 第一期：【10天速通Navigation2】(一) 框架总览和概念解释
  • 第二期：【10天速通Navigation2】(二) ：ROS2gazebo阿克曼小车模型搭建-gazebo_ackermann_drive等插件的配置和说明
  • 第三期：【10天速通Navigation2】(三) ：Cartographer建图算法配置：从仿真到实车，从原理到实现
  • 第四期：【10天速通Navigation2】(四) ：ORB-SLAM3的ROS2 humble编译和配置
  • 第五期：【10天速通Navigation2】(五) ：基于gazebo仿真的复杂地形的ORB-SLAM3配置
  • 第六期：【10天速通Navigation2】(六) ：Navigation2基础配置与参数解析
  • 第七期：【10天速通Navigation2】(七) ：Hybrid-A*全局规划器的原理推导与Nav2插件实现
  • 第八期：【10天速通Navigation2】(八）：RRT与RRT*采样规划器的原理推导与Nav2插件实现
  • 第九期：【10天速通Navigation2】(九）：LQR最优控制器的原理推导与Nav2插件实现
  • 第十期：【10天速通Navigation2】(十）：MPC模型预测控制器的原理推导与Nav2插件实现
  • 第十一期：【Navigation2进阶】(十一）：自主探索的起点——Nearest 与 Utility Frontier 算法推导与 Nav2 插件实现
  • 第十二期：【Navigation2进阶】(十二）：自主探索性能优化与 RIG 算法推导与 Nav2 插件实现
• 但往期三种探索策略有一个共同缺陷："信息增益"的计算本质上只是在数 UNKNOWN 邻居格子——在 OccupancyGrid 的三值（-1 / 0 / 100）地图上，这已经是能做到的最好了
• 说人话就是：我们的地图只知道"这里有没有东西"，不知道"这里有多不确定"，所以"信息增益"只能近似地认为——前沿挨着的 UNKNOWN 格子越多，去那里探索"应该"越值
• 本期我们就来解决这个问题：
  • 先手写一个概率地图发布节点 prob_map，用 log-odds 递推 + 逆传感器模型 维护每 cell 的连续占据概率 P ∈ [0, 1]
  • 再实现 MI（Mutual Information，互信息）目标选择器，从候选前沿质心发射虚拟光束，累加可见 cell 的香农熵作为真正的信息增益
  • 最终在仿真中完成 MI 探索的验证

文章目录

• • 前言
  • 1 问题：三值地图为什么不够
  • • • 1-1 往期探索策略的信息增益回顾
      • 1-2 缺失的关键维度
  • 2 概率占据网格
  • • • 2-1 贝叶斯递推与 log-odds 更新
      • • 2-1-1 log-odds 到底是什么
        • 2-1-2 贝叶斯更新在 log-odds 空间 = 加法
      • 2-2 逆传感器模型（Inverse Sensor Model）
      • 2-3 Bresenham 光线追踪
      • 2-4 代码实现：ProbabilityMapper 完整类
      • 2-5 双 Topic 发布：三值地图 + 概率地图
  • 3 基于互信息的前沿选择
  • • • 3-1 香农熵
      • 3-2 互信息 = 预期熵减
      • 3-3 虚拟光束模拟 MI
      • 3-4 代码实现：MIGoalSelector
      • 3-5 效用函数与路径代价
  • 4 系统集成
  • • • 4-1 项目结构
      • 4-2 探索节点的修改
      • 4-3 启动流程
      • 4-4 RViz 可视化
      • 4-5 完整源码清单
      • • probability_mapper.hpp
        • probability_mapper.cpp
        • mapper_main.cpp
        • prob_mapper.launch.py
        • slam_prob_mapper.launch.py
        • mi_goal_selector.hpp
        • mi_goal_selector.cpp
        • exploration_node.hpp（完整文件，含 MI 扩展）
        • exploration_node.cpp（完整文件，含 MI 扩展）
        • 5_prob_map.sh
        • 7_mi_explore.sh
  • 5 完整参数与对比
  • • • 5-1 prob_mapper 参数表
      • 5-2 exploration_node MI 相关参数
      • 5-3 四种探索策略对比
  • 6 Python 可视化代码
  • • • 6-1 三值 vs 概率 vs 熵 对比图（1-2 节）
      • 6-2 log-odds 函数曲线（2-1-1 节）
      • 6-3 MI 虚拟光束模拟（3-2 / 3-3 节）
  • 总结

1 问题：三值地图为什么不够

1-1 往期探索策略的信息增益回顾

• 往期三种探索策略的 computeInfoGain() 实现本质上都是同一个函数——数某个前沿聚类周围有多少个 UNKNOWN 邻居：

// UtilityGoalSelector 和 RIGGoalSelector 的 computeInfoGain()
double computeInfoGain(const FrontierCluster & cluster,
const nav_msgs::msg::OccupancyGrid & map)
{
int unknown_count = 0;
for (const auto & cell : cluster.cells) {
for (int d = 0; d < 4; ++d) {
int nx = cell.first + dx[d];
int ny = cell.second + dy[d];
if (map.data[ny * w + nx] == UNKNOWN) ++unknown_count;
}
}
return static_cast<double>(unknown_count);
}

• 说人话就是：前沿的 4 邻域里有多少个 -1，这个前沿的"信息量"就约等于这个数
• 这个方法有两个致命问题：

1-2 缺失的关键维度

• 核心缺失的是 不确定性量化。OccupancyGrid 的 -1 / 0 / 100 是"定性"的地图，它告诉机器人在哪里有什么东西，但不告诉机器人"我有多确定"
• 如果我们能维护每 cell 的 连续占据概率 P ∈ [0, 1]，我们就能定义 香农熵：

H

(

c

e

l

l

)

=

−

P

log

⁡

2

P

−

(

1

−

P

)

log

⁡

2

(

1

−

P

)

H(cell) = -P\\log_2 P – (1-P)\\log_2(1-P)

H(cell)=−Plog2​P−(1−P)log2​(1−P)

• 举个例子你就懂了：

• 有了熵，“信息增益"就不再是"数格子”，而是 “消除多少不确定性”。这正是 MI 探索的数学基础
• 三值地图和概率地图的直观对比如下图所示——同一个场景，三种视角：

• 从这张对比图可以清楚看到：
  • 左图（三值地图）：只有三种颜色——白色（FREE）、黑色（OCCUPIED）、灰色（UNKNOWN）。前沿边界是硬边缘，一格之内从"已知"跳到"未知"，没有任何过渡
  • 中图（概率地图）：灰度连续渐变。已扫描区域呈浅灰色（概率趋近 0），障碍物呈深黑色（概率趋近 1），前沿附近保留 5~10 格宽的灰色过渡带
  • 右图（熵地图）：亮色 = 高熵 = 不确定性大。已探索区域和障碍物内部都是暗的（已经确定了），只有前沿边缘呈明亮环带——这就是 MI 算法要去"消除"的地方

2 概率占据网格

2-1 贝叶斯递推与 log-odds 更新

2-1-1 log-odds 到底是什么

• log-odds 的中文叫 “对数几率”，它把 [0, 1] 的概率映到 (-∞, +∞) 的实数轴：

• $p=0.5$
• 代码实现就是这两行互相转换：

double logOddsToProb(double l) { return 1.0 / (1.0 + std::exp(–l)); }
double probToLogOdds(double p) { return std::log(p / (1.0 – p)); }

• 两条曲线的形状如下——左图是 log-odds → 概率（sigmoid / logistic），右图是概率 → log-odds（logit）：

• 几个关键点看一眼就明白：
  • l=0 始终对应 p=0.5（完全未知，log-odds 为 0）
  • l 在 ±4 附近钳位，对应的概率在 0.018 ~ 0.982 之间，防止极端值
  • 概率在 0.3~0.7 之间的灰色过渡带，对应 l 在 [-0.85, +0.85] 之间——这正是保守传感器模型希望保留的区域

2-1-2 贝叶斯更新在 log-odds 空间 = 加法

• 假设每个 cell 的占据事件 $m$
• 直接使用贝叶斯公式：

P

(

m

∣

z

1

:

t

)

=

P

(

z

t

∣

m

)

⋅

P

(

m

∣

z

1

:

t

−

1

)

P

(

z

t

∣

z

1

:

t

−

1

)

P(m \\mid z_{1:t}) = \\frac{P(z_t \\mid m) \\cdot P(m \\mid z_{1:t-1})}{P(z_t \\mid z_{1:t-1})}

P(m∣z1:t​)=P(zt​∣z1:t−1​)P(zt​∣m)⋅P(m∣z1:t−1​)​

• 这个形式每次都要算归一化项 P(z_t | z_{1:t-1})，不好算
• 但对两边取 log-odds 后，贝叶斯递推 退化成加法：

l

t

=

l

t

−

1

+

ln

⁡

P

(

z

t

∣

m

=

1

)

P

(

z

t

∣

m

=

0

)

l_t = l_{t-1} + \\ln\\frac{P(z_t \\mid m=1)}{P(z_t \\mid m=0)}

lt​=lt−1​+lnP(zt​∣m=0)P(zt​∣m=1)​

• 本质上跟"取对数把 $a\times b$
• 落实到代码，每条激光束穿过或命中的 cell 只需要一行加法：

log_odds_[idx] = std::clamp(log_odds_[idx] + l_free_, log_odds_min_, log_odds_max_);

• 做一次加法 = 完成一次贝叶斯更新。一整帧雷达几千条射线，全是加法——这也是 OctoMap 底层用同一套公式的原因

2-2 逆传感器模型（Inverse Sensor Model）

• 传感器模型分两种——问的问题恰好相反：

• 我们这个场景显然是后者——已知每束激光是"打到了东西（hit）“还是"穿过了（miss）”，要反过来推断每格被占据的概率
• 回到 2-1-2 的公式，$l_t=l_{t-1}+\ln \frac{P(z_t\mid m=1)}{P(z_t\mid m=0)}$

• 这里选择了 保守参数 P(hit)=0.55 / P(miss)=0.45，目的是让概率 缓慢收敛，在前沿附近保留 5~10 格宽的灰色渐变带
• 对比一下不同参数的收敛速度：

• 说人话：激进参数让概率"秒变黑白"，前沿附近几乎没有灰度过渡，MI 算法无从区分；保守参数让概率慢慢收敛，灰色渐变带足够宽，MI 才能在不同前沿之间拉开差距

2-3 Bresenham 光线追踪

• 每条激光束从传感器原点出发，在网格中穿过的所有 cell 都需要被更新——命中点（l_occ）+ 穿过点（l_free）
• 这里使用经典的 Bresenham 2D 直线算法，它可以高效地枚举两个网格坐标之间的所有 cell，时间复杂度 $O(\max (|\Delta x|,|\Delta y|))$

// 对应 2-3 节：Bresenham 2D 直线遍历（整数网格，含两端点）
void bresenhamLine(int x0, int y0, int x1, int y1,
std::vector<std::pair<int, int>> & out)
{
int dx = std::abs(x1 – x0);
int dy = –std::abs(y1 – y0); // 负值（算法约定）
int sx = (x0 < x1) ? 1 : –1;
int sy = (y0 < y1) ? 1 : –1;
int err = dx + dy;

out.clear();
while (true) {
out.emplace_back(x0, y0);
if (x0 == x1 && y0 == y1) break;
int e2 = 2 * err;
if (e2 >= dy) { err += dy; x0 += sx; }
if (e2 <= dx) { err += dx; y0 += sy; }
}
}

• 每条激光束（共约 1080 条）都调用一次 Bresenham，然后：

  • 遍历 cell（0 ~ len-2）：log_odds += l_free（标记为 空闲）
  • 命中 cell（len-1）：log_odds += l_occ（标记为 占据）

• 注意：如果光束读数到达 max_range（无回波），只更新遍历 cell，不更新命中 cell——合理的，你看不到回波意味着光束没遇到东西

2-4 代码实现：ProbabilityMapper 完整类

• 先看头文件——类对外暴露的接口：

// prob_map/probability_mapper.hpp —— 对应 2-4 节
namespace prob_map {

class ProbabilityMapper {
public:
explicit ProbabilityMapper(rclcpp::Node * node);

private:
// 激光回调：TF 查到传感器位姿 → updateMap()
void laserCallback(sensor_msgs::msg::LaserScan::SharedPtr msg);
void publishTimerCallback();

// 核心建图：逐束射线 Bresenham 光线追踪，log-odds 递推更新
void updateMap(const sensor_msgs::msg::LaserScan & scan,
const geometry_msgs::msg::Pose & sensor_pose,
double sensor_yaw);

void initGrid();
static void bresenhamLine(int x0, int y0, int x1, int y1,
std::vector<std::pair<int, int>> & out);

// log-odds ↔ 概率 ↔ 熵 的互转
static double logOddsToProb(double l);
static double probToLogOdds(double p);
static double entropy(double p);

// 双 Topic 发布
void publishMaps(const rclcpp::Time & stamp, const std::string & frame_id);

// 核心数据：log-odds 网格（不再存三值！）
std::vector<double> log_odds_;
int width_{0}, height_{0};

// 传感器模型参数
double prob_hit_{0.55}; // P(occ | hit)
double prob_miss_{0.45}; // P(occ | miss)
double l_occ_{0.0}; // ln(0.55/0.45) ≈ 0.20
double l_free_{0.0}; // ln(0.45/0.55) ≈ -0.20
double log_odds_min_{–4.0}; // P ≈ 0.018
double log_odds_max_{4.0}; // P ≈ 0.982
double max_range_{30.0}; // 激光最大使用距离

// Publishers — 注意这里是三个 publisher
rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr prob_map_pub_;
rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr thresh_map_pub_;
rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr entropy_map_pub_;
};

} // namespace prob_map

• 核心的 updateMap() 逻辑——逐束处理全部激光：

// 对应 2-4 节：updateMap —— 逐束射线处理
void ProbabilityMapper::updateMap(
const sensor_msgs::msg::LaserScan & scan,
const geometry_msgs::msg::Pose & sensor_pose, double sensor_yaw)
{
const double sx = sensor_pose.position.x;
const double sy = sensor_pose.position.y;

// 传感器网格坐标（所有射线的起点）
int sx_g, sy_g;
if (!worldToGrid(sx, sy, sx_g, sy_g)) return;

std::vector<std::pair<int, int>> line;
line.reserve(static_cast<size_t>(max_range_ / resolution_) + 10);

for (size_t i = 0; i < scan.ranges.size(); ++i) {
float range = scan.ranges[i];
if (std::isnan(range) || std::isinf(range)) continue;
if (range < min_range_) continue;

const float world_angle = sensor_yaw + scan.angle_min + i * scan.angle_increment;
const bool hit_valid = (range < scan.range_max && range <= max_range_);
const float effective_range = hit_valid ? range : max_range_;

// 终点世界坐标
double ex = sx + effective_range * std::cos(world_angle);
double ey = sy + effective_range * std::sin(world_angle);

int ex_g, ey_g;
worldToGrid(ex, ey, ex_g, ey_g);
ex_g = std::clamp(ex_g, 0, width_ – 1);
ey_g = std::clamp(ey_g, 0, height_ – 1);

// Bresenham 光线追踪
bresenhamLine(sx_g, sy_g, ex_g, ey_g, line);

// 穿过点 → FREE 更新（不含终点）
for (size_t j = 0; j + 1 < line.size(); ++j) {
int idx = gridIndex(line[j].first, line[j].second);
log_odds_[idx] = std::clamp(log_odds_[idx] + l_free_,
log_odds_min_, log_odds_max_);
}

// 命中点 → OCCUPIED 更新（仅当光束实际击中，非 max_range）
if (hit_valid && !line.empty()) {
auto & ep = line.back();
int idx = gridIndex(ep.first, ep.second);
log_odds_[idx] = std::clamp(log_odds_[idx] + l_occ_,
log_odds_min_, log_odds_max_);
}
}
}

• 说人话就是：对每一帧 LaserScan，用 TF 查到传感器在 map 系下的位姿，然后每条射线 → Bresenham 画线 → 穿过 cell 加 l_free（空闲证据）→ 命中 cell 加 l_occ（占据证据）→ 钳位在 [-4, 4] 防止极端值

2-5 双 Topic 发布：三值地图 + 概率地图

• ProbabilityMapper 同时发布 三个 Topic：

• 全局三值地图

• 概率地图（灰度色板 map）： * 白色 / 浅灰：概率趋近 0，确定空闲——已被多次激光穿过，几乎没有不确定性 * 深黑：概率趋近 1，确定占据——激光反复命中，确定为障碍物 * 灰色渐变带：概率在 0.3~0.7 之间，不确定——前沿边缘，只有少量激光扫过，正是 MI 算法要去"消除"的地方

• 熵地图（costmap 色板，推荐）：

  • 蓝色 / 深蓝：熵趋近 0，低不确定性——已探索区域或障碍物内部，已充分收敛
  • 青色 / 绿色：熵 0.3~0.7，中等不确定性——前沿后方，激光扫过的次数还不够多
  • 黄色 / 橙红：熵趋近 1，高不确定性——前沿边缘和未知区域交界处，MI 探索的核心目标
  • 切换方式：RViz 左侧面板 → /entropy_map 的 Map Display → Color Scheme 从 map 改为 costmap

• 发布代码：

// 对应 2-5 节：三张地图的发布
void ProbabilityMapper::publishMaps(
const rclcpp::Time & stamp, const std::string & frame_id)
{
const int total = width_ * height_;

// 1. 阈值化地图（给 Nav2 用）
for (int i = 0; i < total; ++i) {
double p = logOddsToProb(log_odds_[i]);
if (p > 0.70) msg.data[i] = 100; // 占据
else if (p < 0.30) msg.data[i] = 0; // 空闲
else msg.data[i] = –1; // 未知
}

// 2. 连续概率地图（给 MI 用）
for (int i = 0; i < total; ++i) {
double p = logOddsToProb(log_odds_[i]);
msg.data[i] = static_cast<int8_t>(std::round(p * 100.0)); // 0~100
}

// 3. 香农熵地图（给 RViz 看）
for (int i = 0; i < total; ++i) {
double p = logOddsToProb(log_odds_[i]);
double h = entropy(p); // max = 1.0 at p = 0.5
msg.data[i] = static_cast<int8_t>(std::round(h * 100.0));
}
}

• 这里用 transient_local QoS（类似 ROS1 的 latching），确保 RViz 和 Nav2 延迟订阅时也能拿到最新一帧

3 基于互信息的前沿选择

3-1 香农熵

• 有了概率地图，每个 cell 的不确定性可以用 香农熵（Shannon Entropy）来量化：

H

(

c

e

l

l

)

=

−

P

(

m

)

⋅

log

⁡

2

P

(

m

)

−

(

1

−

P

(

m

)

)

⋅

log

⁡

2

(

1

−

P

(

m

)

)

H(cell) = -P(m) \\cdot \\log_2 P(m) – (1 – P(m)) \\cdot \\log_2(1 – P(m))

H(cell)=−P(m)⋅log2​P(m)−(1−P(m))⋅log2​(1−P(m))

• 代码实现：

// 对应 3-1 节：香农熵
static double entropy(double p) {
if (p <= 1e-6 || p >= 1.0 – 1e-6) return 0.0;
return –(p * std::log2(p) + (1.0 – p) * std::log2(1.0 – p));
}

3-2 互信息 = 预期熵减

• MI 探索的核心思路：如果我去位置 x，我预期能消除多少不确定性？

I

(

M

;

Z

∣

x

)

=

H

(

M

)

−

H

(

M

∣

Z

,

x

)

I(M; Z \\mid x) = H(M) – H(M \\mid Z, x)

I(M;Z∣x)=H(M)−H(M∣Z,x)

• 其中 $H(M)$
• 直接精确计算这个值非常昂贵（需要对所有可能的观测做积分），但我们可以用一个 实用近似：

在候选位姿 x 处模拟一套虚拟激光扫描，累加所有"可见 cell"的熵，作为 MI 的近似

• 这个近似的直觉是合理的：如果从 x 处能看到大量高熵 cell，那么去 x 处"应该"能获得大量信息

3-3 虚拟光束模拟 MI

• 对于每个前沿的质心 (cx, cy)，放置一台虚拟 360 传感器，发射 N=60 束射线，每条方向均匀分布：

θ

i

=

2

π

i

N

,

i

=

0

,

1

,

…

,

N

−

1

\\theta_i = \\frac{2\\pi i}{N}, \\quad i = 0, 1, \\dots, N-1

θi​=N2πi​,i=0,1,…,N−1

• 每束射线执行一次 Bresenham 遍历：
  • 从质心出发，沿方向 (cos θ, sin θ) 前进
  • 遇到 P(occ) > 0.7 → 停止（被障碍物遮挡）
  • 否则 → 累加当前 cell 的熵 H(cell)
  • 到达 max_range（8m）→ 停止
• 将所有束的熵累加后再平均：

MI

(

c

a

n

d

i

d

a

t

e

)

=

1

N

∑

i

=

0

N

−

1

∑

c

e

l

l

∈

b

e

a

m

i

H

(

c

e

l

l

)

\\text{MI}(candidate) = \\frac{1}{N} \\sum_{i=0}^{N-1} \\sum_{cell \\in beam_i} H(cell)

MI(candidate)=N1​i=0∑N−1​cell∈beami​∑​H(cell)

说人话：从候选位姿"看一圈"，看见的所有 cell 的熵加起来 ÷ 射线数。如果某个方向大片都是灰色的不确定区域，那 MI 就高；如果四面都已经被扫描过（黑白分明），那 MI 就低。

• 下面这张图把 3-2 和 3-3 的概念放在了一起：

• 从图中可以看到：
  • 左图（概率 + 射线）：候选位姿位于前沿交界处，射线穿入右上未知区域（红/橙色，高熵 → 高 MI），射线穿入左下已探索区域（蓝色，低熵 → 低 MI），射线撞到墙壁提前终止
  • 中图（熵 + 射线）：同组射线覆在熵热力图上，亮色（高熵）区域恰好是 MI 想要消除的地方
  • 右图（熵函数曲线）：$H(p)$

3-4 代码实现：MIGoalSelector

• 头文件——继承 GoalSelector，额外缓存概率地图：

// exploration/mi_goal_selector.hpp —— 对应 3-4 节
class MIGoalSelector : public GoalSelector {
public:
MIGoalSelector() = default;
void setProbMap(const nav_msgs::msg::OccupancyGrid & prob_map);

FrontierCluster select(
const std::vector<FrontierCluster> & frontiers,
const geometry_msgs::msg::Pose & robot_pose,
const nav_msgs::msg::OccupancyGrid & map) override;

private:
// 从 (wx, wy) 模拟 360° 扫描，累加可见 cell 熵
double computeMI(double wx, double wy) const;

// 单束射线遍历，累加沿途熵（遇到占据 cell 停止）
double beamEntropy(int sx_g, int sy_g, double dx_w, double dy_w) const;

nav_msgs::msg::OccupancyGrid prob_map_;
bool prob_map_received_{false};

int num_beams_{60}; // 模拟扫描的射线数
double mi_max_range_{8.0}; // 每条射线最大距离
double occ_threshold_{0.7}; // P > 此值 → 视为占据，射线停止
};

• 核心计算—— computeMI() 和 beamEntropy()：

// 对应 3-4 节：单束射线的熵累加
double MIGoalSelector::beamEntropy(
int sx_g, int sy_g, double dx_w, double dy_w) const
{
const double res = prob_map_.info.resolution;
const double ox = prob_map_.info.origin.position.x;
const double oy = prob_map_.info.origin.position.y;

double start_wx = ox + (sx_g + 0.5) * res;
double start_wy = oy + (sy_g + 0.5) * res;
double end_wx = start_wx + dx_w * mi_max_range_;
double end_wy = start_wy + dy_w * mi_max_range_;

int ex_g = static_cast<int>((end_wx – ox) / res);
int ey_g = static_cast<int>((end_wy – oy) / res);
ex_g = std::clamp(ex_g, 0, static_cast<int>(prob_map_.info.width) – 1);
ey_g = std::clamp(ey_g, 0, static_cast<int>(prob_map_.info.height) – 1);

std::vector<std::pair<int, int>> cells;
bresenhamLine(sx_g, sy_g, ex_g, ey_g, cells);

double total = 0.0;
for (const auto & [mx, my] : cells) {
double p = probAt(mx, my) / 100.0;
if (p > occ_threshold_) break; // 遮挡 → 停止
total += entropy(p); // 累加熵
}
return total;
}

// 对应 3-4 节：360° 模拟扫描 → 平均 MI
double MIGoalSelector::computeMI(double wx, double wy) const
{
const double res = prob_map_.info.resolution;
const double ox = prob_map_.info.origin.position.x;
const double oy = prob_map_.info.origin.position.y;

int sx_g = static_cast<int>((wx – ox) / res);
int sy_g = static_cast<int>((wy – oy) / res);
if (!inBoundsProb(sx_g, sy_g)) return 0.0;

double mi = 0.0;
for (int i = 0; i < num_beams_; ++i) {
double angle = 2.0 * M_PI * static_cast<double>(i) / num_beams_;
mi += beamEntropy(sx_g, sy_g, std::cos(angle), std::sin(angle));
}
return mi / static_cast<double>(num_beams_); // 归一化
}

• 对比往期的 computeInfoGain()：

3-5 效用函数与路径代价

• MI 本身只衡量"到达那个位置后能看到多少不确定区域"，不考虑"开过去要花多少代价"
• 沿用 Burgard 式效用函数：

U

(

f

)

=

M

I

(

f

)

c

o

s

t

(

r

o

b

o

t

→

f

)

U(f) = \\frac{MI(f)}{cost(robot \\to f)}

U(f)=cost(robot→f)MI(f)​

• 路径代价用 Dijkstra 波前传播 在阈值化地图上计算（8 连通，允许穿越 FREE 和 UNKNOWN cell），与 UtilityGoalSelector 完全一致：

// 对应 3-5 节：select() 主循环
for (size_t i = 0; i < frontiers.size(); ++i) {
double mi = computeMI(frontiers[i].centroid.x, frontiers[i].centroid.y);
double nav_cost = /* Dijkstra 距离 */;
double utility = (nav_cost > 1e-6) ? mi / nav_cost : mi;

if (utility > best_utility) {
best_utility = utility;
best_idx = i;
}
}

4 系统集成

4-1 项目结构

• 本期新增了 prob_map 包，并在 exploration 包中扩展了 MI 选择器。两个包的完整文件树如下：

ros2_ws/src/
├── prob_map/ # 新建：概率地图包
│ ├── CMakeLists.txt
│ ├── package.xml
│ ├── include/prob_map/
│ │ └── probability_mapper.hpp # ProbabilityMapper 类声明
│ ├── src/
│ │ ├── probability_mapper.cpp # 核心：log-odds 更新 + 光线追踪 + 三 topic 发布
│ │ └── mapper_main.cpp # 入口
│ └── launch/
│ ├── prob_mapper.launch.py # 单独启动 prob_mapper
│ └── slam_prob_mapper.launch.py # SLAM + prob_mapper + RViz 联合启动
│
└── exploration/ # 扩展现有包
├── CMakeLists.txt # 新增 mi_goal_selector.cpp 编译
├── package.xml
├── include/exploration/
│ ├── frontier_detector.hpp # （不变）前沿检测
│ ├── goal_selector.hpp # （不变）选择器基类 + Nearest/Utility/RIG
│ ├── mi_goal_selector.hpp # 新增：MIGoalSelector 类声明
│ └── exploration_node.hpp # 新增 prob_map 订阅 + mi 分支
└── src/
├── frontier_detector.cpp # （不变）
├── goal_selector.cpp # （不变）
├── mi_goal_selector.cpp # 新增：虚拟光束 MI 计算 + select()
├── exploration_node.cpp # 新增 probMapCallback + mi 选择器集成
└── explorer_main.cpp # （不变）入口

• 新增文件一览：

• 以上所有文件的完整源码见 4-5 完整源码清单，可直接复制粘贴到对应位置后编译运行

4-2 探索节点的修改

• 在 ExplorationNode 中新增：

  • 一个 /prob_map 订阅器，独立于 /map 订阅器
  • 一个 MIGoalSelector 的实例化分支（goal_selector_type == "mi"）
  • 在 selectAndSendGoal() 中，调用 select() 前将最新概率地图注入 MI 选择器

• 构造器中声明参数：

// exploration_node.cpp —— 对应 4-2 节
ExplorationNode::ExplorationNode(const rclcpp::NodeOptions & options)
: rclcpp_lifecycle::LifecycleNode("exploration_node", options)
{
declare_parameter("map_topic", "/map");
declare_parameter("prob_map_topic", "/prob_map"); // 新增
declare_parameter("goal_selector_type", "nearest");
}

• on_configure() 中注册 MI 选择器：

// 对应 4-2 节
} else if (goal_selector_type_ == "mi") {
goal_selector_ = std::make_unique<MIGoalSelector>();
RCLCPP_INFO(get_logger(), "Using MIGoalSelector (mutual information)");
}

• on_activate() 中订阅概率地图：

// 对应 4-2 节：独立订阅概率地图，transient_local QoS
prob_map_sub_ = create_subscription<nav_msgs::msg::OccupancyGrid>(
prob_map_topic_,
rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(),
std::bind(&ExplorationNode::probMapCallback, this, std::placeholders::_1));

• selectAndSendGoal() 中将概率地图注入再调用：

// 对应 4-2 节：select 前注入概率地图
if (goal_selector_type_ == "mi") {
auto * mi_sel = dynamic_cast<MIGoalSelector *>(goal_selector_.get());
if (mi_sel) {
mi_sel->setProbMap(last_prob_map_);
}
}
auto selected = goal_selector_->select(frontiers, current_pose_, last_map_);

4-3 启动流程

• 三个终端，顺序启动：

# 终端 1：Gazebo 仿真 + 机器人
./1_bringup.sh

# 终端 2：SLAM + 概率地图 + Nav2 + RViz
./5_prob_map.sh

# 终端 3：MI 互信息探索
./7_mi_explore.sh

• 5_prob_map.sh 内部启动的节点拓扑：

• prob_mapper 依赖 slam_toolbox 提供的 map→laser TF 来做传感器定位，自己不做 SLAM
• prob_map_size=100.0（100m × 100m），max_range=30.0（全量程），prob_hit=0.55 / prob_miss=0.45（保守传感器模型）

4-4 RViz 可视化

• RViz 中可添加 3 个 Map Display 分别查看：

• 熵热力图用 costmap 色板比灰度更直观——前沿边缘呈红/橙色环带，表明那里有大量不确定性等待消除

4-5 完整源码清单

• 以下为本文涉及的全部源码文件，按文件路径列出，可直接复制粘贴到对应位置后编译运行

probability_mapper.hpp

// 文件: prob_map/include/prob_map/probability_mapper.hpp
#ifndef PROB_MAP__PROBABILITY_MAPPER_HPP_
#define PROB_MAP__PROBABILITY_MAPPER_HPP_

#include <rclcpp/rclcpp.hpp>
#include <nav_msgs/msg/occupancy_grid.hpp>
#include <sensor_msgs/msg/laser_scan.hpp>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>

#include <string>
#include <vector>
#include <utility>

namespace prob_map
{

class ProbabilityMapper
{
public:
explicit ProbabilityMapper(rclcpp::Node * node);
~ProbabilityMapper() = default;

private:
void laserCallback(sensor_msgs::msg::LaserScan::SharedPtr msg);
void publishTimerCallback();
void initGrid();
void updateMap(const sensor_msgs::msg::LaserScan & scan,
const geometry_msgs::msg::Pose & sensor_pose,
double sensor_yaw);

static void bresenhamLine(int x0, int y0, int x1, int y1,
std::vector<std::pair<int, int>> & out);

bool worldToGrid(double wx, double wy, int & mx, int & my) const;
void gridToWorld(int mx, int my, double & wx, double & wy) const;
int gridIndex(int mx, int my) const { return my * width_ + mx; }
bool inBounds(int mx, int my) const;

static double logOddsToProb(double l);
static double probToLogOdds(double p);
static double entropy(double p);

void populateOccupancyGrid(nav_msgs::msg::OccupancyGrid & grid,
const rclcpp::Time & stamp,
const std::string & frame_id);
void publishMaps(const rclcpp::Time & stamp, const std::string & frame_id);

rclcpp::Node * node_;
std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
std::shared_ptr<tf2_ros::TransformListener> tf_listener_;

rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr laser_sub_;
rclcpp::TimerBase::SharedPtr publish_timer_;
rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr prob_map_pub_;
rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr thresh_map_pub_;
rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr entropy_map_pub_;

std::vector<double> log_odds_;
int width_{0}, height_{0};

double map_width_m_{100.0}, map_height_m_{100.0}, resolution_{0.05};
double origin_x_{0.0}, origin_y_{0.0};
double prob_hit_{0.55}, prob_miss_{0.45};
double l_occ_{0.0}, l_free_{0.0};
double log_odds_min_{–4.0}, log_odds_max_{4.0};
double max_range_{30.0}, min_range_{0.15};
double publish_rate_{5.0};
bool publish_entropy_{true};

std::string laser_topic_{"/scan"}, map_frame_{"map"};
std::string prob_topic_{"/prob_map"}, thresh_topic_{"/thresholded_map"};
std::string entropy_topic_{"/entropy_map"};

bool initialized_{false};
int scan_count_{0};
};

} // namespace prob_map
#endif // PROB_MAP__PROBABILITY_MAPPER_HPP_

probability_mapper.cpp

// 文件: prob_map/src/probability_mapper.cpp
#include "prob_map/probability_mapper.hpp"
#include <algorithm>
#include <cmath>
#include <limits>
#include <stdexcept>

namespace prob_map
{

double ProbabilityMapper::logOddsToProb(double l)
{
return 1.0 / (1.0 + std::exp(–l));
}

double ProbabilityMapper::probToLogOdds(double p)
{
return std::log(p / (1.0 – p));
}

double ProbabilityMapper::entropy(double p)
{
if (p <= 0.0 || p >= 1.0) return 0.0;
return –(p * std::log2(p) + (1.0 – p) * std::log2(1.0 – p));
}

void ProbabilityMapper::bresenhamLine(
int x0, int y0, int x1, int y1,
std::vector<std::pair<int, int>> & out)
{
int dx = std::abs(x1 – x0);
int dy = –std::abs(y1 – y0);
int sx = (x0 < x1) ? 1 : –1;
int sy = (y0 < y1) ? 1 : –1;
int err = dx + dy;
out.clear();
while (true) {
out.emplace_back(x0, y0);
if (x0 == x1 && y0 == y1) break;
int e2 = 2 * err;
if (e2 >= dy) { err += dy; x0 += sx; }
if (e2 <= dx) { err += dx; y0 += sy; }
}
}

ProbabilityMapper::ProbabilityMapper(rclcpp::Node * node)
: node_(node)
{
node_->declare_parameter("map_width_m", 100.0);
node_->declare_parameter("map_height_m", 100.0);
node_->declare_parameter("resolution", 0.05);
node_->declare_parameter("prob_hit", 0.55);
node_->declare_parameter("prob_miss", 0.45);
node_->declare_parameter("max_range", 30.0);
node_->declare_parameter("min_range", 0.15);
node_->declare_parameter("publish_rate", 5.0);
node_->declare_parameter("publish_entropy", true);
node_->declare_parameter("laser_topic", "/scan");
node_->declare_parameter("map_frame", "map");
node_->declare_parameter("prob_topic", "/prob_map");
node_->declare_parameter("thresh_topic", "/thresholded_map");
node_->declare_parameter("entropy_topic", "/entropy_map");

map_width_m_ = node_->get_parameter("map_width_m").as_double();
map_height_m_ = node_->get_parameter("map_height_m").as_double();
resolution_ = node_->get_parameter("resolution").as_double();
prob_hit_ = node_->get_parameter("prob_hit").as_double();
prob_miss_ = node_->get_parameter("prob_miss").as_double();
max_range_ = node_->get_parameter("max_range").as_double();
min_range_ = node_->get_parameter("min_range").as_double();
publish_rate_ = node_->get_parameter("publish_rate").as_double();
publish_entropy_ = node_->get_parameter("publish_entropy").as_bool();
laser_topic_ = node_->get_parameter("laser_topic").as_string();
map_frame_ = node_->get_parameter("map_frame").as_string();
prob_topic_ = node_->get_parameter("prob_topic").as_string();
thresh_topic_ = node_->get_parameter("thresh_topic").as_string();
entropy_topic_ = node_->get_parameter("entropy_topic").as_string();

l_occ_ = probToLogOdds(prob_hit_);
l_free_ = probToLogOdds(prob_miss_);

initGrid();

tf_buffer_ = std::make_shared<tf2_ros::Buffer>(node_->get_clock());
tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);

auto latch_qos = rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable();
prob_map_pub_ = node_->create_publisher<nav_msgs::msg::OccupancyGrid>(
prob_topic_, latch_qos);
thresh_map_pub_ = node_->create_publisher<nav_msgs::msg::OccupancyGrid>(
thresh_topic_, latch_qos);
if (publish_entropy_) {
entropy_map_pub_ = node_->create_publisher<nav_msgs::msg::OccupancyGrid>(
entropy_topic_, latch_qos);
}

laser_sub_ = node_->create_subscription<sensor_msgs::msg::LaserScan>(
laser_topic_, rclcpp::SensorDataQoS(),
std::bind(&ProbabilityMapper::laserCallback, this, std::placeholders::_1));

auto period = std::chrono::duration<double>(1.0 / publish_rate_);
publish_timer_ = node_->create_wall_timer(
period, std::bind(&ProbabilityMapper::publishTimerCallback, this));

RCLCPP_INFO(node_->get_logger(),
"ProbabilityMapper: %dx%d (%.2fm x %.2fm) @ %.3fm/cell",
width_, height_, map_width_m_, map_height_m_, resolution_);
}

void ProbabilityMapper::initGrid()
{
width_ = static_cast<int>(std::ceil(map_width_m_ / resolution_));
height_ = static_cast<int>(std::ceil(map_height_m_ / resolution_));
double total_w = width_ * resolution_;
double total_h = height_ * resolution_;
origin_x_ = –total_w / 2.0;
origin_y_ = –total_h / 2.0;
log_odds_.assign(width_ * height_, 0.0);
}

bool ProbabilityMapper::worldToGrid(double wx, double wy, int & mx, int & my) const
{
mx = static_cast<int>((wx – origin_x_) / resolution_);
my = static_cast<int>((wy – origin_y_) / resolution_);
return inBounds(mx, my);
}

void ProbabilityMapper::gridToWorld(int mx, int my, double & wx, double & wy) const
{
wx = origin_x_ + (mx + 0.5) * resolution_;
wy = origin_y_ + (my + 0.5) * resolution_;
}

bool ProbabilityMapper::inBounds(int mx, int my) const
{
return mx >= 0 && mx < width_ && my >= 0 && my < height_;
}

void ProbabilityMapper::laserCallback(sensor_msgs::msg::LaserScan::SharedPtr msg)
{
geometry_msgs::msg::TransformStamped tf;
try {
tf = tf_buffer_->lookupTransform(
map_frame_, msg->header.frame_id, msg->header.stamp,
tf2::durationFromSec(0.05));
} catch (const tf2::TransformException & e) {
RCLCPP_WARN_THROTTLE(node_->get_logger(), *node_->get_clock(), 5000,
"ProbMapper TF lookup failed: %s", e.what());
return;
}

geometry_msgs::msg::Pose sensor_pose;
sensor_pose.position.x = tf.transform.translation.x;
sensor_pose.position.y = tf.transform.translation.y;
sensor_pose.position.z = tf.transform.translation.z;
sensor_pose.orientation = tf.transform.rotation;

double qx = sensor_pose.orientation.x, qy = sensor_pose.orientation.y;
double qz = sensor_pose.orientation.z, qw = sensor_pose.orientation.w;
double siny = 2.0 * (qw * qz + qx * qy);
double cosy = 1.0 – 2.0 * (qy * qy + qz * qz);
double sensor_yaw = std::atan2(siny, cosy);

updateMap(*msg, sensor_pose, sensor_yaw);
++scan_count_;
}

void ProbabilityMapper::updateMap(
const sensor_msgs::msg::LaserScan & scan,
const geometry_msgs::msg::Pose & sensor_pose, double sensor_yaw)
{
const double sx = sensor_pose.position.x;
const double sy = sensor_pose.position.y;

int sx_g, sy_g;
if (!worldToGrid(sx, sy, sx_g, sy_g)) return;

std::vector<std::pair<int, int>> line;
line.reserve(static_cast<size_t>(max_range_ / resolution_) + 10);

for (size_t i = 0; i < scan.ranges.size(); ++i) {
float range = scan.ranges[i];
if (std::isnan(range) || std::isinf(range)) continue;
if (range < min_range_) continue;

const float beam_angle = scan.angle_min + static_cast<float>(i) * scan.angle_increment;
const float world_angle = static_cast<float>(sensor_yaw) + beam_angle;
const bool hit_valid = (range < scan.range_max && range <= max_range_);
const float effective_range = hit_valid ? range : max_range_;

double ex = sx + effective_range * std::cos(world_angle);
double ey = sy + effective_range * std::sin(world_angle);

int ex_g, ey_g;
worldToGrid(ex, ey, ex_g, ey_g);
ex_g = std::clamp(ex_g, 0, width_ – 1);
ey_g = std::clamp(ey_g, 0, height_ – 1);

bresenhamLine(sx_g, sy_g, ex_g, ey_g, line);

for (size_t j = 0; j + 1 < line.size(); ++j) {
int idx = gridIndex(line[j].first, line[j].second);
log_odds_[idx] = std::clamp(log_odds_[idx] + l_free_,
log_odds_min_, log_odds_max_);
}

if (hit_valid && !line.empty()) {
auto & ep = line.back();
int idx = gridIndex(ep.first, ep.second);
log_odds_[idx] = std::clamp(log_odds_[idx] + l_occ_,
log_odds_min_, log_odds_max_);
}
}
}

void ProbabilityMapper::populateOccupancyGrid(
nav_msgs::msg::OccupancyGrid & grid,
const rclcpp::Time & stamp, const std::string & frame_id)
{
grid.header.stamp = stamp;
grid.header.frame_id = frame_id;
grid.info.width = width_;
grid.info.height = height_;
grid.info.resolution = resolution_;
grid.info.origin.position.x = origin_x_;
grid.info.origin.position.y = origin_y_;
grid.info.origin.position.z = 0.0;
grid.info.origin.orientation.w = 1.0;
}

void ProbabilityMapper::publishMaps(
const rclcpp::Time & stamp, const std::string & frame_id)
{
const int total = width_ * height_;

// thresholded map for Nav2
{
nav_msgs::msg::OccupancyGrid msg;
populateOccupancyGrid(msg, stamp, frame_id);
msg.data.resize(total);
for (int i = 0; i < total; ++i) {
double p = logOddsToProb(log_odds_[i]);
if (p > 0.70) msg.data[i] = 100;
else if (p < 0.30) msg.data[i] = 0;
else msg.data[i] = –1;
}
thresh_map_pub_->publish(msg);
}

// continuous probability map for MI
{
nav_msgs::msg::OccupancyGrid msg;
populateOccupancyGrid(msg, stamp, frame_id);
msg.data.resize(total);
for (int i = 0; i < total; ++i) {
double p = logOddsToProb(log_odds_[i]);
msg.data[i] = static_cast<int8_t>(std::round(p * 100.0));
}
prob_map_pub_->publish(msg);
}

// entropy map for RViz
if (publish_entropy_ && entropy_map_pub_) {
nav_msgs::msg::OccupancyGrid msg;
populateOccupancyGrid(msg, stamp, frame_id);
msg.data.resize(total);
for (int i = 0; i < total; ++i) {
double p = logOddsToProb(log_odds_[i]);
msg.data[i] = static_cast<int8_t>(std::round(entropy(p) * 100.0));
}
entropy_map_pub_->publish(msg);
}
}

void ProbabilityMapper::publishTimerCallback()
{
auto stamp = node_->get_clock()->now();
publishMaps(stamp, map_frame_);
}

} // namespace prob_map

mapper_main.cpp

// 文件: prob_map/src/mapper_main.cpp
#include "prob_map/probability_mapper.hpp"

int main(int argc, char ** argv)
{
rclcpp::init(argc, argv);
auto node = std::make_shared<rclcpp::Node>("prob_mapper");
prob_map::ProbabilityMapper mapper(node.get());
RCLCPP_INFO(node->get_logger(), "Probability mapper started. Spinning…");
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}

prob_mapper.launch.py

# 文件: prob_map/launch/prob_mapper.launch.py
# 概率地图建图 — prob_mapper 独立启动 + 可选 RViz

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.conditions import IfCondition
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node

def generate_launch_description():
declare_rviz = DeclareLaunchArgument(
'rviz', default_value='true',
description='Launch RViz')

declare_map_width = DeclareLaunchArgument(
'map_width_m', default_value='100.0',
description='Map width in meters')

declare_map_height = DeclareLaunchArgument(
'map_height_m', default_value='100.0',
description='Map height in meters')

declare_resolution = DeclareLaunchArgument(
'resolution', default_value='0.05',
description='Grid resolution (m/cell)')

declare_prob_hit = DeclareLaunchArgument(
'prob_hit', default_value='0.55',
description='P(occ | hit)')

declare_prob_miss = DeclareLaunchArgument(
'prob_miss', default_value='0.45',
description='P(occ | miss)')

declare_max_range = DeclareLaunchArgument(
'max_range', default_value='30.0',
description='Maximum laser range to use (m)')

declare_min_range = DeclareLaunchArgument(
'min_range', default_value='0.15',
description='Minimum laser range (m)')

declare_laser_topic = DeclareLaunchArgument(
'laser_topic', default_value='/scan',
description='LaserScan topic')

declare_map_frame = DeclareLaunchArgument(
'map_frame', default_value='map',
description='Map TF frame')

declare_publish_rate = DeclareLaunchArgument(
'publish_rate', default_value='5.0',
description='Map publish rate (Hz)')

declare_publish_entropy = DeclareLaunchArgument(
'publish_entropy', default_value='true',
description='Also publish /entropy_map')

declare_use_sim_time = DeclareLaunchArgument(
'use_sim_time', default_value='false',
description='Use simulation /clock time')

prob_mapper = Node(
package='prob_map',
executable='prob_mapper',
name='prob_mapper',
output='screen',
parameters=[{
'use_sim_time': LaunchConfiguration('use_sim_time'),
'map_width_m': LaunchConfiguration('map_width_m'),
'map_height_m': LaunchConfiguration('map_height_m'),
'resolution': LaunchConfiguration('resolution'),
'prob_hit': LaunchConfiguration('prob_hit'),
'prob_miss': LaunchConfiguration('prob_miss'),
'max_range': LaunchConfiguration('max_range'),
'min_range': LaunchConfiguration('min_range'),
'laser_topic': LaunchConfiguration('laser_topic'),
'map_frame': LaunchConfiguration('map_frame'),
'publish_rate': LaunchConfiguration('publish_rate'),
'publish_entropy': LaunchConfiguration('publish_entropy'),
}],
)

return LaunchDescription([
declare_rviz, declare_map_width, declare_map_height,
declare_resolution, declare_prob_hit, declare_prob_miss,
declare_max_range, declare_min_range, declare_laser_topic,
declare_map_frame, declare_publish_rate, declare_publish_entropy,
declare_use_sim_time, prob_mapper,
])

slam_prob_mapper.launch.py

# 文件: prob_map/launch/slam_prob_mapper.launch.py
# slam_toolbox + 概率地图 + RViz 联合启动

from launch import LaunchDescription, LaunchContext
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.conditions import IfCondition
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch.substitutions import EnvironmentVariable
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.substitutions import FindPackageShare
from launch_ros.actions import Node

def generate_launch_description():
slam_launch_path = PathJoinSubstitution(
[FindPackageShare('slam_toolbox'), 'launch', 'online_async_launch.py'])
slam_config_path = PathJoinSubstitution(
[FindPackageShare('linorobot2_navigation'), 'config', 'slam.yaml'])

lc = LaunchContext()
ros_distro = EnvironmentVariable('ROS_DISTRO')
slam_param_name = 'slam_params_file'
if ros_distro.perform(lc) == 'foxy':
slam_param_name = 'params_file'

navigation_launch_path = PathJoinSubstitution(
[FindPackageShare('nav2_bringup'), 'launch', 'navigation_launch.py'])
nav2_config_path = PathJoinSubstitution(
[FindPackageShare('linorobot2_navigation'), 'config', 'navigation.yaml'])

rviz_config_path = PathJoinSubstitution(
[FindPackageShare('linorobot2_navigation'), 'rviz', 'linorobot2_slam.rviz'])

declare_sim = DeclareLaunchArgument('sim', default_value='false')
declare_rviz = DeclareLaunchArgument('rviz', default_value='true')
declare_map_size = DeclareLaunchArgument('prob_map_size', default_value='100.0')
declare_prob_hit = DeclareLaunchArgument('prob_hit', default_value='0.55')
declare_prob_miss = DeclareLaunchArgument('prob_miss', default_value='0.45')
declare_max_range = DeclareLaunchArgument('max_range', default_value='30.0')
declare_publish_entropy = DeclareLaunchArgument('publish_entropy', default_value='true')

return LaunchDescription([
declare_sim, declare_rviz, declare_map_size,
declare_prob_hit, declare_prob_miss, declare_max_range,
declare_publish_entropy,

IncludeLaunchDescription(
PythonLaunchDescriptionSource(navigation_launch_path),
launch_arguments={
'use_sim_time': LaunchConfiguration('sim'),
'params_file': nav2_config_path,
}.items()
),

IncludeLaunchDescription(
PythonLaunchDescriptionSource(slam_launch_path),
launch_arguments={
'use_sim_time': LaunchConfiguration('sim'),
slam_param_name: slam_config_path,
}.items()
),

Node(
package='prob_map', executable='prob_mapper',
name='prob_mapper', output='screen',
parameters=[{
'use_sim_time': LaunchConfiguration('sim'),
'map_width_m': LaunchConfiguration('prob_map_size'),
'map_height_m': LaunchConfiguration('prob_map_size'),
'resolution': 0.05,
'prob_hit': LaunchConfiguration('prob_hit'),
'prob_miss': LaunchConfiguration('prob_miss'),
'max_range': LaunchConfiguration('max_range'),
'min_range': 0.15,
'laser_topic': '/scan',
'map_frame': 'map',
'publish_rate': 5.0,
'publish_entropy': LaunchConfiguration('publish_entropy'),
}],
),

Node(
package='rviz2', executable='rviz2', name='rviz2',
output='screen',
condition=IfCondition(LaunchConfiguration('rviz')),
arguments=['-d', rviz_config_path],
parameters=[{'use_sim_time': LaunchConfiguration('sim')}],
),
])

mi_goal_selector.hpp

// 文件: exploration/include/exploration/mi_goal_selector.hpp
#ifndef EXPLORATION__MI_GOAL_SELECTOR_HPP_
#define EXPLORATION__MI_GOAL_SELECTOR_HPP_

#include <vector>
#include "rclcpp/rclcpp.hpp"
#include "exploration/goal_selector.hpp"
#include "exploration/frontier_detector.hpp"
#include "nav_msgs/msg/occupancy_grid.hpp"
#include "geometry_msgs/msg/pose.hpp"

namespace exploration
{

class MIGoalSelector : public GoalSelector
{
public:
MIGoalSelector() = default;

void setProbMap(const nav_msgs::msg::OccupancyGrid & prob_map);

FrontierCluster select(
const std::vector<FrontierCluster> & frontiers,
const geometry_msgs::msg::Pose & robot_pose,
const nav_msgs::msg::OccupancyGrid & map) override;

private:
double computeMI(double wx, double wy) const;
double beamEntropy(int sx_g, int sy_g, double dx_w, double dy_w) const;
static double entropy(double p);

bool worldToGridProb(double wx, double wy, int & mx, int & my) const;
int gridIndexProb(int mx, int my) const;
bool inBoundsProb(int mx, int my) const;
int8_t probAt(int mx, int my) const;

nav_msgs::msg::OccupancyGrid prob_map_;
bool prob_map_received_{false};

int num_beams_{60};
double mi_max_range_{8.0};
double occ_threshold_{0.7};
};

} // namespace exploration
#endif // EXPLORATION__MI_GOAL_SELECTOR_HPP_

mi_goal_selector.cpp

// 文件: exploration/src/mi_goal_selector.cpp
#include "exploration/mi_goal_selector.hpp"
#include <algorithm>
#include <cmath>
#include <limits>
#include <queue>
#include <utility>

namespace exploration
{

namespace {

void bresenhamLine(int x0, int y0, int x1, int y1,
std::vector<std::pair<int, int>> & out)
{
out.clear();
int dx = std::abs(x1 – x0), dy = –std::abs(y1 – y0);
int sx = (x0 < x1) ? 1 : –1, sy = (y0 < y1) ? 1 : –1;
int err = dx + dy;
while (true) {
out.emplace_back(x0, y0);
if (x0 == x1 && y0 == y1) break;
int e2 = 2 * err;
if (e2 >= dy) { err += dy; x0 += sx; }
if (e2 <= dx) { err += dx; y0 += sy; }
}
}

} // anonymous namespace

double MIGoalSelector::entropy(double p)
{
if (p <= 1e-6 || p >= 1.0 – 1e-6) return 0.0;
return –(p * std::log2(p) + (1.0 – p) * std::log2(1.0 – p));
}

void MIGoalSelector::setProbMap(const nav_msgs::msg::OccupancyGrid & prob_map)
{
prob_map_ = prob_map;
prob_map_received_ = true;
}

bool MIGoalSelector::worldToGridProb(double wx, double wy, int & mx, int & my) const
{
double ox = prob_map_.info.origin.position.x;
double oy = prob_map_.info.origin.position.y;
double res = prob_map_.info.resolution;
mx = static_cast<int>((wx – ox) / res);
my = static_cast<int>((wy – oy) / res);
return inBoundsProb(mx, my);
}

int MIGoalSelector::gridIndexProb(int mx, int my) const
{ return my * static_cast<int>(prob_map_.info.width) + mx; }

bool MIGoalSelector::inBoundsProb(int mx, int my) const
{
return mx >= 0 && mx < static_cast<int>(prob_map_.info.width) &&
my >= 0 && my < static_cast<int>(prob_map_.info.height);
}

int8_t MIGoalSelector::probAt(int mx, int my) const
{ return prob_map_.data[gridIndexProb(mx, my)]; }

double MIGoalSelector::beamEntropy(
int sx_g, int sy_g, double dx_w, double dy_w) const
{
const double ox = prob_map_.info.origin.position.x;
const double oy = prob_map_.info.origin.position.y;
const double res = prob_map_.info.resolution;
const int w = static_cast<int>(prob_map_.info.width);
const int h = static_cast<int>(prob_map_.info.height);

double start_wx = ox + (sx_g + 0.5) * res;
double start_wy = oy + (sy_g + 0.5) * res;
double end_wx = start_wx + dx_w * mi_max_range_;
double end_wy = start_wy + dy_w * mi_max_range_;

int ex_g = static_cast<int>((end_wx – ox) / res);
int ey_g = static_cast<int>((end_wy – oy) / res);
ex_g = std::clamp(ex_g, 0, w – 1);
ey_g = std::clamp(ey_g, 0, h – 1);

std::vector<std::pair<int, int>> cells;
cells.reserve(static_cast<size_t>(mi_max_range_ / res) + 10);
bresenhamLine(sx_g, sy_g, ex_g, ey_g, cells);

double total = 0.0;
for (const auto & [mx, my] : cells) {
if (!inBoundsProb(mx, my)) break;
double p = probAt(mx, my) / 100.0;
if (p > occ_threshold_) break;
total += entropy(p);
}
return total;
}

double MIGoalSelector::computeMI(double wx, double wy) const
{
const double ox = prob_map_.info.origin.position.x;
const double oy = prob_map_.info.origin.position.y;
const double res = prob_map_.info.resolution;

int sx_g = static_cast<int>((wx – ox) / res);
int sy_g = static_cast<int>((wy – oy) / res);
if (!inBoundsProb(sx_g, sy_g)) return 0.0;

double mi = 0.0;
for (int i = 0; i < num_beams_; ++i) {
double angle = 2.0 * M_PI * static_cast<double>(i) / num_beams_;
mi += beamEntropy(sx_g, sy_g, std::cos(angle), std::sin(angle));
}
return mi / static_cast<double>(num_beams_);
}

static std::vector<double> dijkstraDistanceMap(
const nav_msgs::msg::OccupancyGrid & map, int sx, int sy)
{
const int w = static_cast<int>(map.info.width);
const int h = static_cast<int>(map.info.height);
std::vector<double> dist(w * h, std::numeric_limits<double>::infinity());
using Node = std::pair<double, int>;
std::priority_queue<Node, std::vector<Node>, std::greater<Node>> pq;

dist[sy * w + sx] = 0.0;
pq.emplace(0.0, sy * w + sx);

const int dx8[8] = {1, 1, 0, –1, –1, –1, 0, 1};
const int dy8[8] = {0, 1, 1, 1, 0, –1, –1, –1};
const double diag = std::sqrt(2.0);

while (!pq.empty()) {
auto [d, k] = pq.top(); pq.pop();
if (d > dist[k] + 1e-6) continue;
int cx = k % w, cy = k / w;
for (int dir = 0; dir < 8; ++dir) {
int nx = cx + dx8[dir], ny = cy + dy8[dir];
if (nx < 0 || nx >= w || ny < 0 || ny >= h) continue;
if (map.data[ny * w + nx] > 0) continue;
double step = (dir % 2 == 0) ? 1.0 : diag;
double nd = d + step;
int nk = ny * w + nx;
if (nd < dist[nk]) { dist[nk] = nd; pq.emplace(nd, nk); }
}
}
return dist;
}