背景介绍：路径规划 + 控制的解耦 vs 联合

在移动机器人导航中，路径规划（如基于地图构建的搜索）与轨迹跟踪（如模型预测控制 MPC）通常被拆解为两个阶段：

  • 规划模块：找到一条从起点到目标的无碰路径
  • 控制模块：跟随这条路径，使机器人平稳、可控地到达目标

但在复杂环境中，尤其是动态障碍物、曲折通道等场景中，如果不能做好两者的耦合与接口设计，系统效果会受到很大限制。

本项目采用 四叉树（QuadTree）划分安全区域 + MPC 控制器跟踪轨迹 的结构，成功实现了一个典型的路径→控制联合流程。

第一步：通过 QuadTree 提取可行路径

我们使用 connective_quadtree 构建了一个基于图的四叉树分区系统，它可以：

  • 在任意障碍物地图上生成 QuadTree 结构
  • 快速查询从 start_pos 到 goal_pos 的路径节点
  • 每个叶子节点有 center()，表示其几何中心
path_nodes = self.quadtree.find_path(start_pos[:2], goal_pos[:2])
path_points = [node.center() for node_id in path_nodes]

这里输出的是一个 [(x0, y0), (x1, y1), …] 的路径点序列，尚不能直接用于控制器输入。

第二步：MPC 控制器的输入格式要求

为了让 MPC 能正常运行，需要如下格式：

initial_state = [x, y, yaw, v]  # 当前状态
reference_trajectory = np.array([[x0, y0], [x1, y1], ...])

控制器内部会基于参考轨迹进行插值、预测、代价优化。

第三步：桥接逻辑实现（完整整合流程）

我们在 trajectory_generator.py 中实现了完整的桥接类 TrajectoryGenerator，封装逻辑如下：

路径转为 MPC 输入轨迹

ref_path = PathNodeList([PathNode(p[0], p[1]) for p in path_points])
self.set_ref_trajectory(ref_path)

这一过程自动插值生成 TrajectoryNodeList（x, y, theta），最终用于 MPC 滑窗轨迹。

生成 local horizon 轨迹片段

ref_traj_local, idx_next = self.get_local_ref_traj(...)

用于 MPC 的每一步优化。

运行 MPC 控制器求解动作

actions, pred_states, cost = self.run_step(
    stc_constraints, dyn_constraints,
    other_robot_states, ref_traj_local
)

通过组装参数列表（包含当前状态、目标点、ref轨迹、惩罚项、障碍信息等），将数据传入 Rust 优化器执行实时轨迹优化。

求解结果包括：

  • 当前采取的控制动作actions

  • 预测的未来状态序列pred_states

  • 当前步代价和求解时间cost

附：完整调用逻辑回顾

self.load_quadtree(quadtree)
self.extract_path_from_quadtree(start_pos, goal_pos)
self.set_reference_from_quadtree(start_pos, goal_pos)
self.run_step(...)

总结

这套逻辑通过合理的数据结构设计（PathNodeList → TrajectoryNodeList）与模块封装（TrajectoryGenerator 类），将一个抽象的地图路径输出有效连接到高性能的 MPC 控制器中，实现了路径规划 + 控制融合的完整链路。

如果你也在做类似任务，不妨参考本项目结构，探索如何将地图表达与控制决策更自然地融合起来。

欢迎访问项目代码仓库 👉 GitHub: safe_area_navigation_for_robots