运动规划实战案例 | 基于多源流场(Flow Field)的路径规划(附ROS C++/Python实现)

1 多源流场的构建

在机器人导航、游戏AI或自动驾驶等领域，路径规划的核心挑战在于如何在复杂环境中快速找到从起点到目标的最优路径。传统的单源路径规划算法（如A*）虽能解决单起点问题，但在多起点或多目标场景下，往往需要重复计算或难以平衡全局效率。多源流场（Multi-source Flow Field） 通过构建全局的流动信息场，将多源起点的信息融合到每个网格单元中，可以用于启发式函数设计，或提供梯度场进行轨迹优化。

形式化地，流场可表示为一个二维网格$\mathcal{F}$，其中每个单元$\mathbf{x}=(x,y)$对应一个状态向量 F ( x ) = { cost ( x ) , dir ( x ) , neighbors ( x ) } \mathcal{F}(\mathbf{x}) = \{ \text{cost}(\mathbf{x}), \text{dir}(\mathbf{x}), \text{neighbors}(\mathbf{x}) \} F(x)={cost(x),dir(x),neighbors(x)}，分别描述该单元的：

• 邻接表：可移动的相邻单元
• 代价：到达该位置的路径代价
• 流动方向：朝向代价最低节点的最佳流动方向

通过上述定义，任意单元的路径规划只需跟随流场方向即可逼近目标位置。

1.1 邻接表生成

单元格可能被障碍物阻挡，因此需要预先计算每个单元格的有效邻居（即相邻的可通行单元）。具体来说，对于单元格$\mathbf{c}=(x,y)$，遍历其周围的所有可能方向$d\in D$（如八邻域等），检查邻居$\mathbf{n}=\mathbf{c}+d$是否在地图范围内且可通行（$\text{is\_free}(\mathbf{n})=\text{true}$）。若满足条件，则将$\mathbf{n}$加入$\mathbf{c}$的邻接表$\text{neighbors}(\mathbf{c})$。这个过程可以在流场过程前预计算，实现算法加速

1.2 代价计算

代价计算的核心算法是基于广度优先搜索算法的全局成本更新，其思想是：每次从开放表中取出当前成本最小的单元格$\mathbf{c}$，遍历其所有有效邻居$\mathbf{n}$，计算从$\mathbf{c}$到$\mathbf{n}$的新成本$C(\mathbf{n}{)}_{\text{new}}$，常用的计算方式是

$$C(\mathbf{n}{)}_{\text{new}}=C(\mathbf{c})+d(\mathbf{c},\mathbf{n})=C(\mathbf{c})+\sqrt{(n_x-c_x{)}^2+(n_y-c_y{)}^2}$$

若该成本低于$\mathbf{n}$的当前成本 $C(\mathbf{n}{)}_{\text{old}}$，则更新$\mathbf{n}$的成本，并将其加入开放表(开放表本质上是一个优先队列，本文用双端队列模拟，用于后续按成本从小到大处理单元格)。

因为我们的问题是多目标点的，所以初始化阶段，需要将所有目标栅格都加入开放表，相当于为每个起点$\mathbf{s}$定义了一个初始波前，后续的成本更新将基于这些波前向外纹波扩展，最终覆盖整个地图，并确保每个单元格的成本最终收敛到从最近起点到该单元格的最短路径代价。

1.3 流动方向

代价更新完成后，每个栅格已存储了到最近起点的最短成本。接下来还需确定即从当前单元格出发，沿哪个方向移动能最快接近最近的目标点。最简单的做法是：对于单元格$\mathbf{c}$，遍历所有可能的方向$\mathbf{d}\in D$，计算沿方向$\mathbf{d}$移动后的邻居$\mathbf{n}=\mathbf{c}+\mathbf{d}$。若$\mathbf{n}$的成本$C(\mathbf{n})$低于$\mathbf{c}$的成本$C(\mathbf{c})$，则$\mathbf{d}$ 是一个候选方向。最终，选择使 $C(\mathbf{n})$最小的方向作为$\mathbf{c}$的流动方向。这一方向本质上是代价函数的负梯度方向，因此路径规划时只需沿流动方向移动，即可保证每一步都朝着成本更低的方向前进。

2 基于Flow Field的路径规划

假设流场代价矩阵$\mathbf{F}(s)$，对于任意起点$s_{start}$，总存在一条路径
$$s_{start}\to s_1\to s_2\to ...-\to s_{goal}$$

使得总代价为$d(s_{start})$，证明了最优路径的存在性

接着，对于当前栅格$s$，需选择下一栅格$s^{\prime }\in \mathrm{neighbor}(s)$使得总代价最小。根据动态规划方程

$$d\left(s\right)=\underset{s^{\prime }\in \mathrm{neighbor}\left(s\right)}{\min }\left(c\left(s,s^{\prime }\right)+d\left(s^{\prime }\right)\right)$$

因此，最优的下一栅格$s^{\ast }$应满足

$$s^{\ast }={\mathrm{argmin}}_{{\mathrm{s}}^{\prime }\in \mathrm{neighbor}(\mathrm{s})}(\mathrm{c}\left(\mathrm{s},{\mathrm{s}}^{\prime }\right)+\mathrm{d}({\mathrm{s}}^{\prime }))$$

即选择邻域中$d(s^{\prime })$最小的栅格，称为贪心拓展策略

接下来通过数学归纳法证明贪心拓展策略的最优性

• 当$s=s_{goal}$时，路径长度为 0，显然成立；
• 归纳假设：假设对于所有满足$d(s^{\prime })<d(s)$的栅格$s^{\prime }$，算法能正确找到从$s^{\prime }$到$s_{goal}$的最短路径
• 归纳步骤：对当前栅格$s$，选择$s^{\ast }={\mathrm{argmin}}_{{\mathrm{s}}^{\prime }\in \mathrm{neighbor}(\mathrm{s})}(\mathrm{c}\left(\mathrm{s},{\mathrm{s}}^{\prime }\right)+\mathrm{d}({\mathrm{s}}^{\prime }))$，则路径总代价为$d(s)=c\left(s,s^{\ast }\right)+d(s^{\ast })$

根据归纳假设，从$s^{\ast }$到$s_{goal}$的路径是最优的，因此$s\to s^{\ast }\to \cdot \cdot \cdot \to s_{goal}$的路径也为最优。所以通过流场梯度下降法即可找到最短路径，且这个路径可以是多源的。

3 算法仿真

3.1 ROS C++仿真

核心算法如下所示

void FlowField::generate(const std::vector<rmp::common::geometry::Point2i>& start_positions)
{
  // 1. calculate valid neighbors
  for (auto& col : flow_field_)
  {
    for (auto& cell : col)
    {
      cell.reset();
      updateNeighbors(cell);
    }
  }

  // 2. open table initialization
  std::deque<FlowCell::FlowCellPtr> open_set;
  for (const auto& start_position : start_positions)
  {
    if (start_position.x() >= 0 && start_position.x() < map_width_ && start_position.y() >= 0 &&
        start_position.y() < map_height_)
    {
      auto* start_cell = &flow_field_[start_position.x()][start_position.y()];
      start_cell->cost = 0.0;
      start_cell->is_open = true;
      start_cell->closest_start_positions.insert(start_cell->cell_pos);
      open_set.push_back(start_cell);
    }
  }

  // 3. flow field process
  while (!open_set.empty())
  {
    FlowCell::FlowCellPtr current = open_set.front();
    open_set.pop_front();
    current->is_open = false;

    for (FlowCell::FlowCellPtr neighbor : current->neighbor_cells)
    {
      double new_cost =
          std::hypot(neighbor->cell_pos.x() - current->cell_pos.x(), neighbor->cell_pos.y() - current->cell_pos.y()) +
          current->cost;

      if (new_cost <= neighbor->cost)
      {
        if (new_cost < neighbor->cost)
        {
          neighbor->cost = new_cost;
          neighbor->closest_start_positions.clear();
        }

		...
      }
    }
  }
}

3.2 Python仿真

核心算法如下所示

def plan(self, start: Point3d, goal: Point3d) -> Tuple[List[Point3d], List[Dict]]:
    """
    Flow field-based motion plan function.

    Parameters:
        start (Point3d): The starting point of the planning path.
        goal (Point3d): The goal point of the planning path.

    Returns:
        path (List[Point3d]): The planned path from start to goal.
        visual_info (List[Dict]): Information for visualization
    """
    start_x, start_y = round(start.x()), round(start.y())
    goal_x, goal_y = round(goal.x()), round(goal.y())
    FlowField.generate([self.flow_field[goal_x][goal_y]], self.flow_field)
    if self.flow_field[start_x][start_y].cost > 0:
        path = [start]
        cost = self.flow_field[start_x][start_y].cost
        curr_x, curr_y = start_x, start_y
        while not (curr_x == round(goal.x()) and curr_y == round(goal.y())):
            min_x, min_y = None, None
            curr_cost = self.flow_field[curr_x][curr_y].cost
            for dx, dy in FlowField.directions:
                new_x = curr_x + dx
                new_y = curr_y + dy
                if 0 <= new_x < self.env.x_range and 0 <= new_y < self.env.y_range:
                    new_cost = self.flow_field[new_x][new_y].cost
                    if new_cost >= 0 and new_cost < curr_cost:
                        curr_cost = new_cost
                        min_x, min_y = new_x, new_y
            curr_x = min_x
            curr_y = min_y
            path.append(Point3d(curr_x, curr_y))

完整工程代码请联系下方博主名片获取

🔥 更多精彩专栏：

• 《ROS从入门到精通》
• 《Pytorch深度学习实战》
• 《机器学习强基计划》
• 《运动规划实战精讲》
• …