局部规划系列讲了DWA和TEB。今天讲一个思路不太一样的方法——Lattice Planner（状态格规划器）。说白了，Lattice Planner把连续的空间离散化成一组"状态格子"，然后在这些格子上用A*搜索路径。和DWA的区别是：DWA在速度空间中采样，Lattice Planner在位姿空间中搜索。
Lattice Planner最早用于自动驾驶领域——Stanford在DARPA挑战赛中的无人车Junior就用了Lattice Planner。后来被广泛应用到各种移动机器人和自动驾驶系统中。
一、Lattice Planner的原理
Lattice Planner的核心是"状态格子"。什么是状态格子？就是在(x, y, theta)空间中均匀取一组离散点。每个格子代表机器人的一个可能位姿。
但和普通的2D栅格不同，Lattice Planner的格子包含了朝向信息theta。这意味着同一个(x, y)位置，不同朝向是不同的状态。这让Lattice Planner能处理非完整约束（比如车不能横向移动）。
规划流程：
- 预计算运动基元：离线计算一组从标准起点出发能到达的轨迹片段（motion primitives）。每个基元连接两个状态格子。
- 在线搜索：收到起点和终点后，在状态格子上用A*搜索——从起点格子开始，沿着运动基元跳到相邻格子，直到到达终点格子。
- 碰撞检测：搜索时对每条运动基元做碰撞检测——检查基元经过的所有点是否在障碍物中。
class LatticePlanner: def __init__(self, motion_primitives): self.primitives = motion_primitives # 预计算的运动基元 self.lattice = StateLattice(resolution_xy=0.5, resolution_theta=15) def plan(self, start, goal, occupancy_grid): open_set = PriorityQueue() open_set.put(start, heuristic(start, goal)) while not open_set.empty(): state = open_set.get() if state == goal: return self.extract_path(state) for primitive in self.primitives: next_state = apply(state, primitive) if not collision_check(primitive, occupancy_grid): cost = state.cost + primitive.cost if cost < next_state.cost: next_state.cost = cost open_set.put(next_state, cost + heuristic(next_state, goal)) return None二、运动基元的设计
运动基元是Lattice Planner的核心。基元的质量直接决定规划效果。
怎么生成运动基元？常用方法是模拟机器人的运动学模型。比如对差速驱动机器人，从标准起点(0,0,0)出发，给不同的(v, omega)组合仿真固定时间T，得到一组终点。每条轨迹就是一个运动基元。
基元的数量和分辨率：基元太少，搜索空间受限，可能找不到好的路径。基元太多，搜索慢。工程上一般每个状态格子有6-12个运动基元（前左、前右、直走、原地左转、原地右转等）。
基元的连接性：基元要满足"格子对齐"——基元的终点必须恰好落在状态格子上。如果不巧落在两个格子之间，需要snap到最近的格子。这个snap操作可能引入误差——分辨率越粗，误差越大。
# 典型的运动基元（差速驱动） # 直线前进: v=1.0, omega=0, T=1s # 左转弯: v=0.8, omega=30deg/s, T=1s # 右转弯: v=0.8, omega=-30deg/s, T=1s # 原地左转: v=0, omega=45deg/s, T=1s # 原地右转: v=0, omega=-45deg/s, T=1s # 倒车: v=-0.5, omega=0, T=1s三、Lattice Planner的优劣
Lattice Planner的优势在于运动学约束天然满足。因为运动基元本身就是根据机器人运动学模型生成的，所以搜索出来的路径一定满足运动学约束——不需要额外处理。
另一个优势是可预测性。搜索空间是离散的，A*保证找到最优解（在离散空间中）。计算时间有上界——取决于状态格子数量和基元数量。
劣势也很明显。离散化误差——状态格子是离散的，真实路径可能不在格子上。分辨率越粗，误差越大。但分辨率太细，状态数量暴增（x * y * theta），搜索变慢。
维度问题：加了theta维度后，状态数量是2D栅格的几十倍（theta通常离散为24-72个方向）。在大规模地图中，搜索时间可能不可接受。
预计算依赖：运动基元需要离线预计算。如果机器人的运动学参数变了（比如换了底盘），基元需要重新计算。
四、面试实战
Q：Lattice Planner和DWA有什么区别？A：DWA在速度空间中采样，前向仿真后选最优速度。Lattice Planner在位姿空间中搜索，用A*在离散的状态格子上找最优路径。DWA更灵活（速度连续），Lattice Planner更有保证（离散最优）。
Q：Lattice Planner和A*在栅格地图上搜索有什么区别？A：栅格A在2D(x,y)空间中搜索，不考虑朝向。Lattice Planner在3D(x,y,theta)空间中搜索，考虑了朝向。Lattice Planner的运动基元满足运动学约束，栅格A的8方向或4方向移动不满足。
Q：实际项目中用过Lattice Planner吗？A：用过。做园区无人车低速导航时用的Lattice Planner。全局用A*在2D栅格上规划粗略路径，局部用Lattice Planner在状态格子中搜索满足Ackermann运动学的轨迹。状态分辨率0.5m/15度，运动基元12个。搜索时间约20-50ms。
Q：Lattice Planner在自动驾驶中还在用吗？A：还在用，但通常作为备选方案。主流的自动驾驶系统更倾向于用优化方法（比如QP、EM Planner）做局部规划。但Lattice Planner在低速场景（园区、港口）中仍然很常见——实现简单，行为可预测。
Q：Lattice Planner怎么处理倒车？A：在运动基元中包含倒车基元就行。但要注意：倒车时朝向和运动方向相反，运动学模型不同。工程上需要分别为前进和倒车生成不同的基元集合。搜索时A*的启发函数也要考虑——倒车代价通常设得比前进高，鼓励机器人优先前进。
小结
Lattice Planner的核心：把(x,y,theta)空间离散成状态格子，预计算运动基元，用A*在状态格子上搜索满足运动学约束的路径。
优势：运动学约束天然满足，可预测性好，离散最优。 劣势：离散化误差，维度问题（加了theta后状态数量暴增），预计算依赖。
Lattice Planner是自动驾驶和移动机器人中经典的局部规划方法之一。Stanford的Junior、Google早期的无人车、很多港口AGV系统都用过Lattice Planner。虽然现在优化方法越来越流行，但Lattice Planner因为实现简单、行为可预测，在工业界仍然有很强的生命力。理解Lattice Planner之后，下一篇讲势场法——另一种经典的局部规划方法。
如果这篇文章对你有帮助，欢迎点赞、在看、转发三连。 你的支持是我持续更新的最大动力。
「机器人软件开发面试·从入门到精通」连载系列
上一篇：第220篇 TEB局部规划器——用图优化生成局部轨迹
下一篇预告：第222篇 势场法——经典但仍有生命力的局部规划方法
有任何问题欢迎评论区留言，我会尽量回复。
