1 算法整体思路

一种结合了就是Lattice Planner（格点路径规划）运动学约束（kinematic constraints）和离散搜索（discrete search）思想的路径规划方法。

它通过 预先生成一组满足车辆动力学约束的轨迹“模板库（motion rimitives）”，并将这些轨迹连接成一个格点（lattice）结构的图，在其中进行搜索（通常是 A* 或 D*），找到从起点到目标点的最优路径。

2 算法运行流程

2.1 生成状态格点

状态格点（state lattice）是一个在状态空间（例如 x, y, θ）中离散化的网格结构，每个格点代表体系的一个可能状态。不同于普通的栅格地图（只考虑位置），Lattice Planner 同时离散化了姿态角或其他维度的状态，例如：

  • x ∈ [0, 100]，步长 Δx = 1 m
  • y ∈ [0, 100]，步长 Δy = 1 m
  • θ ∈ {0°, 45°, 90°, 135°, 180°}

这样，每一个格点状态为：

这些状态节点构成搜索图的顶点，覆盖规划所需的空间范围（如机器人当前位置到目标点的区域）。具体状态维度取决于系统模型，例如，无人车常用状态为 。

2.2 生成运动原语

运动原语（Motion Primitives）是车辆从一个格点状态到另一个格点状态的可行轨迹段，它必须符合车辆的运动学模型（如 Ackermann 转向约束、加速度限制），确保轨迹可执行。

转向角：就是常用车辆模型，其中 L 是轴距，δ

生成方式：基于机器人的运动模型（如自行车模型、动力学模型），从某个状态格点出发，通过预设的控制量（如转向角、油门 / 刹车）在短时间内生成轨迹，最终终止于另一个状态格点。

  • 离线采样若干速度、转向角；
  • 对每种输入组合，积分得到一段轨迹；
  • 将起点归一化为 (0, 0, 0)，得到模板轨迹；
  • 离线保存为运动原语库。

运行时，只需将这些模板轨迹变换到当前格点坐标系下即可使用。

原语属性：每个运动原语包括：

  • 起点和终点状态；
  • 轨迹的几何路径（位置序列）；
  • 代价（如路径长度、时间、能耗、平滑度等）；
  • 可行性标记（是否避障、是否满足约束）。

2.3 构建搜索图

把所有格点作为节点，运动原语作为边，构建一个有向加权图，用于后续搜索。

仅保留规划场景中高效的状态格点（如在可行驶区域内、未碰撞的格点），排除位于障碍物或禁行区的节点，对每个有效节点，凭借查询预计算的运动原语，连接所有可达的其他奏效节点，边的权重为运动原语的代价（如路径长度 + 启发式代价，用于 A * 搜索）。

每条边（轨迹）的代价一般与以下因素有关：

  • 路径长度；
  • 曲率变化；
  • 是否靠近障碍物（加惩罚）；
  • 是否倒车（若允许，可加权）。

代价函数示例：

其中 L为轨迹长度，K为曲率，d obs为离障碍物的距离。

若环境存在动态障碍物（如行人、其他车辆），需在线更新节点的有效性（标记被占据的节点为不可达），并调整边的连接关系。

2.4 使用搜索算法寻找最优路径

在构建好的图上进行路径搜索。

常用算法：

  • A*（最优但计算量较大）
  • D*（适合动态环境）
  • Anytime Repairing A*（适合实时规划）

启发式函数（Heuristic）可采用：

  • 欧氏距离；
  • Dubins 距离（前进单向约束）；
  • Reeds-Shepp 距离（前后双向约束）。

搜索时，每个节点会扩展到多个后继节点（由 motion primitives 提供），最终找到到目标状态的最小总代价路径。

2.5 路径平滑与连接

搜索得到的路径由运动原语拼接而成，可能存在折线或突变（如航向角跳变），需凭借后处理优化，提升轨迹的平滑性和执行效率。

可进行以下处理：

  • 轨迹拼接与平滑（如Cubic spline平滑）
  • 曲率连续化（确保可控）
  • 时标分配（加入速度剖面）
  • 轨迹优化（如用梯度下降最小化代价函数）

最终输出可直接供控制模块（如 MPC、Pure Pursuit）跟踪。

本文对lattice算法的理解还是不够深入的，只能说把握了下大体的框架和概念，仅供参考，后序要用到软件代码时也会进一步完善博客。欢迎大家一起交流学习，共同进步！