局部规划系列讲了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_xy0.5, resolution_theta15) 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操作可能引入误差——分辨率越粗误差越大。# 典型的运动基元差速驱动 # 直线前进: v1.0, omega0, T1s # 左转弯: v0.8, omega30deg/s, T1s # 右转弯: v0.8, omega-30deg/s, T1s # 原地左转: v0, omega45deg/s, T1s # 原地右转: v0, omega-45deg/s, T1s # 倒车: v-0.5, omega0, T1s三、Lattice Planner的优劣Lattice Planner的优势在于运动学约束天然满足。因为运动基元本身就是根据机器人运动学模型生成的所以搜索出来的路径一定满足运动学约束——不需要额外处理。另一个优势是可预测性。搜索空间是离散的A*保证找到最优解在离散空间中。计算时间有上界——取决于状态格子数量和基元数量。劣势也很明显。离散化误差——状态格子是离散的真实路径可能不在格子上。分辨率越粗误差越大。但分辨率太细状态数量暴增x * y * theta搜索变慢。维度问题加了theta维度后状态数量是2D栅格的几十倍theta通常离散为24-72个方向。在大规模地图中搜索时间可能不可接受。预计算依赖运动基元需要离线预计算。如果机器人的运动学参数变了比如换了底盘基元需要重新计算。四、面试实战QLattice Planner和DWA有什么区别ADWA在速度空间中采样前向仿真后选最优速度。Lattice Planner在位姿空间中搜索用A*在离散的状态格子上找最优路径。DWA更灵活速度连续Lattice Planner更有保证离散最优。QLattice 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。QLattice Planner在自动驾驶中还在用吗A还在用但通常作为备选方案。主流的自动驾驶系统更倾向于用优化方法比如QP、EM Planner做局部规划。但Lattice Planner在低速场景园区、港口中仍然很常见——实现简单行为可预测。QLattice 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篇 势场法——经典但仍有生命力的局部规划方法有任何问题欢迎评论区留言我会尽量回复。
