多Agent路径规划 LaCAM for multi-agent path finding (MAPF)

一、项目概述

想象一个仓库,里面有很多机器人(比如 400 个),每个机器人需要从自己的起点走到自己的终点,而且所有机器人同时移动,彼此之间不能相撞、不能穿越。这就是多智能体路径规划(Multi-Agent Path Finding, MAPF)问题。

LaCAM 是解决这个问题的一种算法,由 Keisuke Okumura 提出。这个仓库是 LaCAM 的基础实现版本。

1.2 核心思想(通俗版)

TEXT 复制代码
高层搜索(High-Level):尝试找到一个"好的"机器人布局方案(配置)
    ↓
低层搜索(Low-Level):用 PIBT 算法快速生成机器人下一步的具体位置

可以类比为:

  • 高层像"指挥官",决定每个机器人应该大致往哪个方向走
  • 低层像"执行者",具体安排每个机器人在当前时刻移动到哪个格子

二、项目结构总览

TEXT 复制代码
lacam0/
├── main.cpp                    # 程序入口
├── CMakeLists.txt              # 顶层构建配置
├── lacam/                      # 核心算法库
│   ├── CMakeLists.txt
│   ├── include/                # 头文件(接口声明)
│   │   ├── utils.hpp           # 工具函数(时间、随机数、日志)
│   │   ├── graph.hpp           # 图/地图定义
│   │   ├── instance.hpp        # 问题实例定义
│   │   ├── dist_table.hpp      # 距离表(启发式信息)
│   │   ├── pibt.hpp            # PIBT 算法(底层移动规划)
│   │   ├── lacam.hpp           # LaCAM 算法(主搜索框架)
│   │   ├── planner.hpp         # 规划器入口
│   │   ├── post_processing.hpp # 结果验证与输出
│   │   └── metrics.hpp         # 解的质量指标
│   └── src/                    # 源文件(具体实现)
│       ├── utils.cpp
│       ├── graph.cpp
│       ├── instance.cpp
│       ├── dist_table.cpp
│       ├── pibt.cpp
│       ├── lacam.cpp
│       ├── planner.cpp
│       ├── post_processing.cpp
│       └── metrics.cpp
├── tests/                      # 测试文件
├── assets/                     # 地图和场景文件
└── third_party/                # 第三方库(命令行解析)

三、基础数据结构(从底层到顶层)

3.1 工具层 --- utils.hpp/cpp

这是整个项目的基础工具箱。

cpp 复制代码
// 时间管理器
struct Deadline {
  const Time::time_point t_s;      // 开始时间
  const double time_limit_ms;       // 时间限制(毫秒)

  Deadline(double _time_limit_ms = 0);
  double elapsed_ms() const;        // 已过时间
};

// 判断是否超时
bool is_expired(const Deadline *deadline);

关键点:整个搜索过程有时间限制(默认 3 秒),Deadline 负责计时。搜索循环中会反复检查 is_expired(),超时就停止。

随机数工具:

cpp 复制代码
// 获取 [0,1) 之间的随机浮点数
float get_random_float(std::mt19937 &MT, float from = 0, float to = 1);
// 获取 [from, to] 之间的随机整数
int get_random_int(std::mt19937 &MT, int from = 0, int to = 1);

日志输出工具:

cpp 复制代码
// level: 日志级别, verbose: 用户设置的详细程度
// 只有 level <= verbose 时才输出
template <typename... Body>
void info(const int level, const int verbose, Body &&...body);

3.2 图/地图层 --- graph.hpp/cpp

Vertex(顶点 = 地图上的一个可通行格子)

cpp 复制代码
struct Vertex {
  const int id;      // 在 V 数组中的索引(只计可通行格子)
  const int index;   // 在 U 数组中的索引(width * y + x,包含障碍物占位)
  const int x;       // 地图上的 x 坐标
  const int y;       // 地图上的 y 坐标

  std::vector<Vertex *> neighbors;  // 相邻可通行格子(上下左右)
  std::vector<Vertex *> actions;    // neighbors + 自身(可选择不动)
};

理解要点:

neighbors:只能移动到的地方(不含自身)

actions:可以"选择"的地方(含自身,因为机器人可以选择原地等待)

Graph(图 = 整张地图)

cpp 复制代码
struct Graph {
  Vertices V;     // 所有可通行格子(不含 nullptr)
  Vertices U;     // 所有格子(含 nullptr 表示障碍物),|U| = width * height
  int width;      // 地图宽度
  int height;     // 地图高度
};

地图文件格式(如 random-32-32-10.map):

cpp 复制代码
type octile
height 32
width 32
map
.@@@@@@@.@@@@@@@@.@@@@.@@@@@@@@..
...(32行,每行32个字符)

其中 . 表示可通行,@ 或 T 表示障碍物。

图的构建过程(Graph::Graph(const std::string &filename)):

cpp 复制代码
步骤1: 读取 height, width
步骤2: 逐行读取地图,遇到 '.' 就创建 Vertex,遇到 '@' 跳过
步骤3: 遍历所有格子,为每个 Vertex 建立与邻居的连接(上下左右)

可视化理解:

cpp 复制代码
地图:           对应的 U 数组:
. @ .           [V0] [nullptr] [V1]
. . .           [V2] [V3]      [V4]
@ . .           [nullptr] [V5] [V6]

V0 的 neighbors = {V2}     (只有下方是通的)
V0 的 actions   = {V2, V0} (可以向下走,也可以不动)

3.3 问题实例层 --- instance.hpp/cpp

cpp 复制代码
struct Instance {
  Graph G;          // 地图
  Config starts;    // 所有机器人的起始位置
  Config goals;     // 所有机器人的目标位置
  const uint N;     // 机器人数量
};

其中 Config 是 std::vector<Vertex *>,即每个机器人当前位置的列表。

三种构造方式:

cpp 复制代码
// 方式1: 从场景文件加载(基准测试用)
// 场景文件格式: 每行包含 map名, 起点坐标, 终点坐标
Instance(scen_filename, map_filename, N);

// 方式2: 随机生成(给定种子)
Instance(map_filename, N, seed);

// 方式3: 直接指定起止索引
Instance(map_filename, start_indexes, goal_indexes);

场景文件(.scen)格式解读:

cpp 复制代码
version 1
3   random-32-32-10.map   32   32   11   6   7   18   13.65685425
│   │                      │    │    │    │   │   │    └─ 最优路径长度
│   │                      │    │    │    │   │   └─ 终点 y
│   │                      │    │    │    │   └─ 终点 x
│   │                      │    │    │    └─ 起点 y
│   │                      │    │    └─ 起点 x
│   │                      │    └─ 地图高度
│   │                      └─ 地图宽度
│   └─ 地图文件名
└─ 难度等级

3.4 距离表层 --- dist_table.hpp/cpp

目的:快速回答"机器人 i 到达格子 v 需要多少步?"

cpp 复制代码
struct DistTable {
  const int K;                          // 格子总数
  std::vector<std::vector<int>> table;  // table[i][v] = 机器人i的目标到格子v的距离
  std::vector<std::queue<Vertex *>> OPEN; // BFS 搜索队列(懒加载模式)
};

两种初始化模式:

cpp 复制代码
// 模式1: 多线程预计算(默认,速度快)
//   对每个机器人启动一个线程,BFS 从目标格子出发,计算到所有格子的距离
if (MULTI_THREAD_INIT) {
    auto bfs = [&](const int i) {
        // 从 goals[i] 开始 BFS
        // table[i][v] = 从 goals[i] 到 v 的最短距离
    };
    // 每个机器人一个线程并行计算
    for (size_t i = 0; i < ins->N; ++i) {
        pool.emplace_back(std::async(std::launch::async, bfs, i));
    }
}

// 模式2: 懒加载 BFS(按需计算)
//   只初始化起点,查询时如果没算过就继续 BFS
//   适合只需查询少量距离的场景

懒加载查询过程:

cpp 复制代码
int DistTable::get(const int i, const int v_id) {
  // 如果已经算过了,直接返回
  if (table[i][v_id] < K) return table[i][v_id];

  // 否则继续 BFS,直到算出 v_id 的距离
  while (!OPEN[i].empty()) {
    auto n = OPEN[i].front();
    OPEN[i].pop();
    // 对 n 的每个邻居做松弛操作
    for (auto m : n->neighbors) {
      if (d_n + 1 < table[i][m->id]) {
        table[i][m->id] = d_n + 1;
        OPEN[i].push(m);
      }
    }
    if (n->id == v_id) return d_n;  // 找到了!
  }
}

为什么需要距离表? 它是启发式搜索的核心。距离表告诉算法每个机器人离目标还有多远,用于决定移动的优先级。

四、核心算法

4.1 PIBT 算法 --- pibt.hpp/cpp

PIBT(Priority Inheritance with Backtracking)是 LaCAM 的"底层引擎"。给定当前所有机器人的位置和一个优先级顺序,PIBT 为每个机器人规划一步移动。

核心数据结构

cpp 复制代码
struct PIBT {
  const int N;           // 机器人数量
  const int V_size;      // 格子数量
  DistTable *D;          // 距离表

  const int NO_AGENT;    // 表示"没有机器人"的哨兵值 = N
  std::vector<int> occupied_now;   // occupied_now[v] = 当前在格子v上的机器人编号
  std::vector<int> occupied_next;  // occupied_next[v] = 下一步将在格子v上的机器人编号
};

主函数 set_new_config

cpp 复制代码
bool PIBT::set_new_config(
    const Config &Q_from,   // 当前配置(所有机器人当前位置)
    Config &Q_to,           // 输出:下一步配置
    const std::vector<int> &order  // 机器人处理顺序(按优先级从高到低)
) {
  // 步骤1: 初始化占用表
  for (auto i = 0; i < N; ++i) {
    occupied_now[Q_from[i]->id] = i;
    if (Q_to[i] != nullptr) {
      // 检查是否有冲突(高层搜索已预设了部分机器人的位置)
      occupied_next[Q_to[i]->id] = i;
    }
  }

  // 步骤2: 按优先级顺序处理每个未决定的机器人
  for (auto i : order) {
    if (Q_to[i] == nullptr) {
      funcPIBT(i, Q_from, Q_to);  // 递归规划
    }
  }

  // 步骤3: 清理
  // ...
}

核心递归函数 funcPIBT

这是 PIBT 的精髓。以机器人 i 为例:

cpp 复制代码
bool PIBT::funcPIBT(const int i, const Config &Q_from, Config &Q_to) {
  // 第一步: 为机器人i计算每个可能动作的代价
  //   动作 = {邻居1, 邻居2, ..., 原地等待}
  //   代价 = (到目标距离, 阻碍程度, 随机数)

  // 第二步: 按代价从小到大排序动作

  // 第三步: 检查是否需要交换(swap)

  // 第四步: 依次尝试每个动作
  for (每个候选动作 u,按代价从低到高) {
    if (u 已被其他机器人占用) continue;
    if (u 会导致交换冲突) continue;

    // 占领格子 u
    occupied_next[u->id] = i;
    Q_to[i] = u;

    // 如果 u 上原来有个机器人 j,j 需要让路
    if (j 在 u 上 && j 还没决定去哪) {
      // 优先级继承:递归调用 funcPIBT 帮 j 规划
      if (!funcPIBT(j, Q_from, Q_to)) {
        continue;  // j 让不了路,尝试下一个动作
      }
    }

    return true;  // 成功!
  }

  // 所有动作都失败了,机器人i原地不动
  Q_to[i] = Q_from[i];
  return false;
}

图解 PIBT 的工作过程:

cpp 复制代码
场景: 机器人A想去右边,但右边是机器人B

  当前:  [A][B][ ]     A 的目标 →
  优先级: A > B

  步骤1: A 尝试向右移动到 B 的位置
  步骤2: B 被"踢出",递归调用 funcPIBT(B)
  步骤3: B 发现右边是空的,可以向右移动
  步骤4: 成功!

  结果:  [ ][A][B]

Hindrance(阻碍)启发式

cpp 复制代码
// 如果开启 HINDRANCE,计算移动到格子 u 会阻碍多少其他机器人
int hindrance = 0;
for (每个邻居机器人 k) {
  // 如果 k 的目标是 u,但 k 当前不在 u 上,
  // 而且移动到 u 会让 k 离目标更远
  if (Q_from[k] != u && D->get(k, u) < D->get(k, Q_from[k])) {
    hindrance += 1;  // 这个移动会阻碍 k
  }
}

这个启发式让机器人优先选择不会阻碍其他机器人的动作。

Swap(交换)机制

cpp 复制代码
// 当机器人 i 想移动到机器人 j 的位置时
// 检查是否应该执行"交换"操作(两个机器人互换位置)

bool is_swap_required(pusher, puller, ...) {
  // 模拟:pusher 想去 puller 的位置
  // 沿着 puller 的方向检查,如果 puller 后面是一条"死胡同"
  // (只有一个邻居可以去),就标记为需要交换
}

bool is_swap_possible(v_pusher, v_puller) {
  // 模拟 puller 被推着往后退的过程
  // 检查这条链上是否有足够的空间
}

4.2 LaCAM 搜索框架 --- lacam.hpp/cpp

高层搜索节点 HNode

cpp 复制代码
struct HNode {
  const Config Q;      // 这个节点对应的机器人配置
  HNode *parent;       // 父节点(用于回溯路径)
  std::set<HNode *> neighbors;  // 相邻节点(用于树重连)

  int g;   // 从起点到此节点的实际代价
  int h;   // 启发式估计(到目标的估计距离)
  int f;   // f = g + h
  int depth; // 搜索深度

  std::vector<float> priorities;  // 每个机器人的优先级
  std::vector<int> order;         // 按优先级排序的机器人顺序
  std::queue<LNode *> search_tree; // 低层搜索树
};

低层搜索节点 LNode

cpp 复制代码
struct LNode {
  std::vector<int> who;      // 已经固定了哪些机器人的位置
  Vertices where;            // 这些机器人被固定到了哪里
  const uint depth;          // 当前深度(= who.size())
};

低层搜索是一个约束生成过程:逐步固定某些机器人的位置,剩下的机器人由 PIBT 来规划。

主搜索循环

cpp 复制代码
Solution LaCAM::solve() {
  // 初始化
  auto H_init = new HNode(ins->starts, D);  // 起始节点
  OPEN.push_front(H_init);                    // 放入开放列表

  while (!OPEN.empty() && !is_expired(deadline)) {
    ++loop_cnt;

    // === 随机重插(多样性策略)===
    if (H_goal != nullptr) {
      // 有一定概率把某个节点重新放到队列前端
      // 这有助于跳出局部最优
    }

    auto H = OPEN.front();  // 取出队首节点

    // === 剪枝 ===
    if (H_goal != nullptr && H->f >= H_goal->g) {
      // 已有解且当前节点不可能更优,跳过
      continue;
    }

    // === 目标检测 ===
    if (H_goal == nullptr && is_same_config(H->Q, ins->goals)) {
      H_goal = H;  // 找到解了!
      if (!ANYTIME) break;  // 非随时模式直接结束
      continue;
    }

    // === 低层搜索:生成约束 ===
    if (H->search_tree.empty()) {
      OPEN.pop_front();
      continue;
    }
    auto L = H->search_tree.front();
    H->search_tree.pop();

    // 扩展低层搜索树
    if (L->depth < H->Q.size()) {
      const auto i = H->order[L->depth];  // 当前要固定的机器人
      for (auto u : H->Q[i]->actions) {    // 它可以去的位置
        H->search_tree.push(new LNode(L, i, u));
      }
    }

    // === 用 PIBT 生成新配置 ===
    auto Q_to = Config(ins->N, nullptr);
    auto res = set_new_config(H, L, Q_to);
    // set_new_config: 先填入 L 中固定的机器人位置,再用 PIBT 规划其余

    // === 检查是否已探索过 ===
    auto iter = EXPLORED.find(Q_to);
    if (iter == EXPLORED.end()) {
      // 新配置:创建新节点加入 OPEN
      auto H_new = new HNode(Q_to, D, H, g_val, h_val);
      OPEN.push_front(H_new);
      EXPLORED[Q_to] = H_new;
    } else {
      // 已知配置:执行树重连(优化)
      rewrite(H, iter->second);
      OPEN.push_front(iter->second);
    }
  }

  // 从 H_goal 回溯,得到完整解
  Solution solution;
  auto H = H_goal;
  while (H != nullptr) {
    solution.push_back(H->Q);
    H = H->parent;
  }
  std::reverse(solution.begin(), solution.end());
  return solution;
}

代价计算

cpp 复制代码
// h值:所有机器人到各自目标的最短距离之和
int LaCAM::get_h_val(const Config &Q) {
  auto c = 0;
  for (size_t i = 0; i < ins->N; ++i)
    c += D->get(i, Q[i]);
  return c;
}

// 边代价:有多少机器人还没到达目标且发生了移动
int LaCAM::get_edge_cost(const Config &Q1, const Config &Q2) {
  auto cost = 0;
  for (size_t i = 0; i < ins->N; ++i) {
    if (Q1[i] != ins->goals[i] || Q2[i] != ins->goals[i]) {
      cost += 1;  // 这个机器人还在路上
    }
  }
  return cost;
}

树重连(Tree Rewiring)--- LaCAM*

cpp 复制代码
void LaCAM::rewrite(HNode *H_from, HNode *H_to) {
  if (!ANYTIME) return;

  // 更新邻居关系
  H_from->neighbors.insert(H_to);

  // Dijkstra 式的传播
  std::queue<HNode *> Q({H_from});
  while (!Q.empty()) {
    auto n_from = Q.front();
    Q.pop();
    for (auto n_to : n_from->neighbors) {
      auto g_val = n_from->g + get_edge_cost(n_from->Q, n_to->Q);
      if (g_val < n_to->g) {
        // 找到更短路径!更新
        n_to->g = g_val;
        n_to->parent = n_from;
        Q.push(n_to);
        // 如果比当前最优解更好,加入 OPEN 重新搜索
        if (H_goal != nullptr && n_to->f < H_goal->f) {
          OPEN.push_front(n_to);
        }
      }
    }
  }
}

树重连的作用:当发现到某个已知配置的更短路径时,更新其父节点关系。这使得最终解的质量可以不断改进(anytime 特性)。

优先级机制

cpp 复制代码
HNode::HNode(Config _Q, DistTable *D, HNode *_parent, ...) {
  for (int i = 0; i < N; ++i) {
    if (parent == nullptr) {
      // 初始节点:优先级 = 到目标距离 / 10000
      priorities[i] = (float)D->get(i, Q[i]) / 10000;
    } else {
      // 动态优先级(类似 PIBT 的累积优先级)
      if (D->get(i, Q[i]) != 0) {
        // 还没到目标:优先级逐渐增加(越等越急)
        priorities[i] = parent->priorities[i] + 1;
      } else {
        // 到了目标:优先级重置
        priorities[i] = parent->priorities[i] - (int)parent->priorities[i];
      }
    }
  }
  // 按优先级从高到低排序
  std::sort(order.begin(), order.end(),
            [&](int i, int j) { return priorities[i] > priorities[j]; });
}

五、程序入口与流程 --- main.cpp + planner.cpp

5.1 命令行参数

cpp 复制代码
// 必需参数
-m, --map           地图文件
-N, --num           机器人数量

// 可选参数
-i, --scen          场景文件(不指定则随机生成起止点)
-s, --seed          随机种子(默认 0)
-v, --verbose       日志详细程度(0=最少,3=很详细)
-t, --time_limit_sec 时间限制(默认 3 秒)
-o, --output        输出文件路径
-l, --log_short     简短日志

// 算法开关
--anytime           启用随时优化(树重连)
--no_dist_table_init 禁用多线程距离表初始化
--no_pibt_swap      禁用 PIBT 的交换操作
--no_pibt_hindrance 禁用阻碍启发式

5.2 主流程

cpp 复制代码
int main(int argc, char *argv[]) {
  // 1. 解析命令行参数
  // 2. 创建问题实例
  auto ins = Instance(scen_name, map_name, N);

  // 3. 设置算法参数
  DistTable::MULTI_THREAD_INIT = !program.get<bool>("no_dist_table_init");
  LaCAM::ANYTIME = program.get<bool>("anytime");
  PIBT::SWAP = !program.get<bool>("no_pibt_swap");
  PIBT::HINDRANCE = !program.get<bool>("no_pibt_hindrance");

  // 4. 求解
  const auto deadline = Deadline(time_limit_sec * 1000);
  const auto solution = solve(ins, verbose - 1, &deadline, seed);

  // 5. 验证解的可行性
  if (!is_feasible_solution(ins, solution, verbose)) {
    return 1;  // 无效解
  }

  // 6. 输出统计信息和结果文件
  print_stats(verbose, &deadline, ins, solution, comp_time_ms);
  make_log(ins, solution, output_name, ...);
}

5.3 solve 函数(planner.cpp)

cpp 复制代码
Solution solve(const Instance &ins, int verbose, const Deadline *deadline, int seed) {
  // 1. 构建距离表
  auto D = DistTable(ins);

  // 2. 创建 LaCAM 求解器并求解
  auto lacam = LaCAM(&ins, &D, verbose, deadline, seed);
  return lacam.solve();
}

六、解的验证与评估 --- post_processing.cpp + metrics.cpp

6.1 可行性检查

cpp 复制代码
bool is_feasible_solution(const Instance &ins, const Solution &solution) {
  // 检查1: 起始位置是否正确
  if (!is_same_config(solution.front(), ins.starts)) return false;

  // 检查2: 终止位置是否正确
  if (!is_same_config(solution.back(), ins.goals)) return false;

  for (每个时间步 t) {
    for (每对机器人 i, j) {
      // 检查3: 移动合法性(只能移动到相邻格子或原地不动)
      if (v_i_from != v_i_to && v_i_to 不是 v_i_from 的邻居) return false;

      // 检查4: 顶点冲突(两个机器人不能在同一格子)
      if (v_j_to == v_i_to) return false;

      // 检查5: 交换冲突(两个机器人不能互换位置)
      if (v_j_to == v_i_from && v_j_from == v_i_to) return false;
    }
  }
  return true;
}

6.2 解的质量指标

cpp 复制代码
// Makespan(完工时间)= 解的时间步数 - 1
// 即最晚到达目标的机器人的步数
int get_makespan(const Solution &solution) {
  return solution.size() - 1;
}

// Sum of Costs(总代价)= 所有机器人路径代价之和
// 机器人i的代价 = 它实际移动的步数(到达目标后停下不再计入)
int get_sum_of_costs(const Solution &solution) {
  int c = 0;
  for (size_t i = 0; i < N; ++i)
    c += get_path_cost(solution, i);  // 机器人i到达目标前的步数
  return c;
}

// Sum of Loss(总损失)= 所有机器人"未在目标"的时间步之和
// 与 Sum of Costs 的区别:机器人在等待时也算损失
int get_sum_of_loss(const Solution &solution) {
  // 对每个机器人,统计有多少时间步它不在目标位置
}

指标对比图示:

cpp 复制代码
机器人A: S → · → · → G → G    代价=3, 损失=3
机器人B: S → · → G → G → G    代价=2, 损失=2

Makespan = 4(总步数-1)
Sum of Costs = 3 + 2 = 5
Sum of Loss = 3 + 2 = 5(这里恰好相等)

如果A在第3步到达后还在第4步移动了:
机器人A: S → · → · → G → ·    代价=3, 损失=4
Makespan = 4
Sum of Costs = 3 + 2 = 5
Sum of Loss = 4 + 2 = 6(损失更大了)

七、完整算法流程图

cpp 复制代码
开始
  │
  ├─ 读取地图和场景文件,构建 Instance
  ├─ 构建距离表 DistTable(多线程 BFS)
  │
  ├─ 初始化 LaCAM 求解器
  │    │
  │    ├─ 将起点配置作为初始 HNode 加入 OPEN
  │    │
  │    └─ 搜索循环 (直到找到解或超时):
  │         │
  │         ├─ 取出 OPEN 队首节点 H
  │         │
  │         ├─ [剪枝] 如果已有解且 H 不可能更优,跳过
  │         │
  │         ├─ [目标检测] 如果 H 的配置等于目标,记录为 H_goal
  │         │
  │         ├─ [低层搜索] 从 H 的搜索树中取出一个约束 L
  │         │    │
  │         │    ├─ L 中固定了某些机器人的位置
  │         │    ├─ 扩展搜索树(为下一个机器人添加约束)
  │         │    │
  │         │    └─ 调用 PIBT 生成新配置 Q_to
  │         │         │
  │         │         ├─ 高优先级机器人先选位置
  │         │         ├─ 如果位置被占,递归让低优先级机器人让路
  │         │         ├─ 支持交换(swap)和阻碍(hindrance)启发式
  │         │         └─ 返回是否成功生成有效配置
  │         │
  │         ├─ [检查重复] Q_to 是否已在 EXPLORED 中?
  │         │    │
  │         │    ├─ 不在 → 创建新 HNode,加入 OPEN
  │         │    └─ 已在 → 执行树重连(rewrite),更新最短路径
  │         │
  │         └─ 继续循环...
  │
  ├─ 从 H_goal 回溯得到完整解(配置序列)
  │
  ├─ 验证解的可行性
  │
  └─ 输出结果

八、关键设计思想总结

为什么用两层搜索?

层级 负责什么 关键特点

高层 (LaCAM) 决定"哪些机器人该移动" 全局视角,搜索配置空间

低层 (PIBT) 决定"具体怎么移动" 局部视角,快速生成一步

8.2 PIBT 的优先级继承

cpp 复制代码
普通规划: A 优先 → A 想去 B 的位置 → B 没人管了 → 失败
PIBT:     A 优先 → A 想去 B 的位置 → 递归让 B 先规划 → B 让路 → 成功

这就像排队:高优先级的人可以"插队",被插队的人自动获得次高优先级。

8.3 LaCAM 的随机性

LaCAM 有多处随机操作:

1.PIBT 中的动作随机打乱:std::shuffle(C.begin(), C.end(), MT)

2.低层搜索树的随机扩展:动作顺序随机

3.随机重插:以小概率将某些节点重新放回队首

4.随机重启:以极小概率回到起点重新开始

这些随机性帮助算法跳出局部最优,探索更多解空间。

8.4 Anytime 特性

当 --anytime 开启时:

  • 找到第一个解后不会停止
  • 继续搜索并通过树重连不断改进解的质量
  • 直到时间耗尽

九、构建与运行指南

9.1 编译

cpp 复制代码
# 克隆(含子模块)
git clone --recursive <repo-url>
cd lacam0

# 构建
cmake -B build && make -C build -j4

9.2 运行示例

cpp 复制代码
# 基本运行:400 个机器人,详细日志
build/main -i assets/random-32-32-10-random-1.scen \
           -m assets/random-32-32-10.map \
           -N 400 -v 3

# 随机实例(不指定场景文件)
build/main -m assets/random-32-32-10.map -N 100 -s 42

# 开启 anytime 模式
build/main -m assets/random-32-32-10.map \
           -i assets/random-32-32-10-random-1.scen \
           -N 100 --anytime -t 10

9.3 运行测试

cpp 复制代码
ctest --test-dir ./build

9.4 可视化

bash 复制代码
# 使用配套可视化工具
mapf-visualizer assets/random-32-32-10.map build/result.txt

本文项目地址

https://kei18.github.io/lacam-project/

相关推荐
一切皆是因缘际会1 小时前
可落地数字生命工程:从记忆厮杀到自我意识觉醒全链路,AGI内生智能硅基生命心智建模
人工智能·深度学习·算法·机器学习·ai·系统架构·agi
nlpming2 小时前
opencode Agent 详解
算法
江南十四行2 小时前
排序算法进阶:直接插入排序(简单排序)与希尔排序
数据结构·算法·排序算法
nlpming2 小时前
opencode System Prompt 构建机制 & AGENTS.md注入机制
算法
nlpming2 小时前
opencode - 安装和配置
算法
nlpming2 小时前
opencode 内置工具
算法
nlpming2 小时前
opencode - 常用命令&自定义命令
算法
CoderCodingNo2 小时前
【CSP】CSP-J 2021真题 | 插入排序 luogu-P7910 (适合GESP四-六级及以上考生练习)
数据结构·算法·排序算法
艺术电影节2 小时前
祝贺电影《撤离》《悼念词》《水草长生》 荣获亚洲艺术电影节提名
算法·推荐算法·电视