第221篇 Lattice Planner——基于状态格子的局部规划

郑伟强dev

144人浏览 · 2026-08-20 05:25:31

郑伟强dev · 2026-08-20 05:25:31 发布

局部规划系列讲了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篇势场法——经典但仍有生命力的局部规划方法

有任何问题欢迎评论区留言，我会尽量回复。