前面讲了一堆局部规划方法DWA、TEB、Lattice Planner、势场法。今天讲一个全局规划算法——混合AHybrid A。这个算法在自动驾驶领域非常出名Stanford的无人车Junior和Audi的自主泊车系统都用了它。混合A的核心思想在A搜索的基础上把节点从2D栅格(x,y)扩展到3D状态(x,y,theta)扩展节点时不用8方向或4方向的直线移动而是用满足车辆运动学的曲线前轮转角固定的圆弧。说白了混合A*搜索出来的路径车一定能开过去。一、混合A和标准A的区别标准A*在2D栅格上搜索每个节点是(x,y)邻居是上下左右4个或8个方向。扩展时走直线不考虑朝向。混合A*的节点是(x,y,theta)——多了一个朝向维度。扩展时不走直线而是模拟车辆运动——给定前轮转角delta和速度v仿真一小段时间dt得到新的位姿。不同的delta对应不同的圆弧轨迹——这就是运动基元。# 混合A*的节点扩展 def expand(node, steering_angles, dt0.5): successors [] for delta in steering_angles: # 比如[-30,-15,0,15,30]度 # 模拟自行车模型 x_new node.x v * cos(node.theta) * dt y_new node.y v * sin(node.theta) * dt theta_new node.theta v * tan(delta) / L * dt successors.append(State(x_new, y_new, theta_new)) return successors关键区别标准A的边是直线混合A的边是满足运动学的曲线。标准A不考虑车辆能不能转弯混合A搜索出来的路径车辆一定能走。二、启发函数的设计混合A*的效率高度依赖启发函数。好的启发函数能让搜索快速收敛差的启发函数让搜索退化成Dijkstra。混合A*常用两种启发函数无约束的2D距离忽略运动学约束直接用起点到终点的欧氏距离或A*在2D栅格上的最短路径。计算快但不够紧——因为忽略了转弯约束。有约束的启发函数考虑车辆的最小转弯半径。比如计算不考虑障碍物但考虑最小转弯半径的最短路径——这可以用Reed-Shepp曲线或Dubins曲线来算下一篇详细讲。这个启发函数更紧搜索更快但每次计算启发值的代价更大。工程上通常用两种启发函数的组合先用2D A*算一个粗略的启发值不考虑运动学再加上一个运动学修正项。三、连续性与离散化混合A*有个微妙的问题状态空间是连续的(x,y,theta)但搜索需要离散化。离散化太粗路径精度差离散化太细状态数量暴增。解决方案混合A*用了一个巧妙的技巧——节点离散化但边连续。节点(x,y,theta)被snap到离散的格子上用于判断是否访问过但扩展时的运动模拟是连续的。这样既控制了状态数量又保证了路径的运动学可行性。具体来说theta通常离散为72个方向每5度一个x和y的分辨率根据场景设定比如0.1-0.5m。同一个格子可能被多次访问——但只有当新的到达角度和之前不同时才算不同的状态。解析扩展Analytic Expansion混合A*还有一个加速技巧。每隔一定迭代次数比如每100次尝试用Reed-Shepp曲线或Dubins曲线直接从当前节点连到目标。如果这条曲线不穿过障碍物就直接返回路径——不用继续搜索了。这个技巧在开阔空间中效果极好能大幅减少搜索时间。代价函数的设计混合A*的代价函数不只是路径长度。工程上通常加入几项额外代价方向变化惩罚鼓励少换挡、转向变化惩罚鼓励平滑转向、离障碍物距离惩罚鼓励远离障碍物。这些额外代价让搜索出来的路径更好开——不只是最短还最舒适。# 混合A*的代价函数 cost path_length \ 5.0 * direction_changes # 换挡惩罚 2.0 * steering_changes # 转向变化惩罚 0.5 * obstacle_proximity # 离障碍物距离三.5、混合A*的实现要点工程上实现混合A*有几个关键细节。状态去重怎么判断一个状态是否已经访问过用三维数组visited[x_idx][y_idx][theta_idx]。x_idx和y_idx是离散化后的索引theta_idx是角度离散化后的索引。如果visited为true跳过这个状态。优先队列和标准A*一样用优先队列按f值排序。f g hg是从起点到当前节点的实际代价h是启发函数估计值。碰撞检测扩展节点时对整条运动基元做碰撞检测——不能只检查终点要检查整条弧线经过的所有点。工程上把弧线离散成若干小段逐段检查。搜索方向混合A*支持前进和后退。前轮转角为正时车往左前为负时往右前。速度为负时车往后退。后退时代价要乘以一个系数比如1.5鼓励机器人优先前进。四、面试实战Q混合A和标准A有什么区别A标准A在2D(x,y)栅格上搜索边是直线。混合A在3D(x,y,theta)状态空间中搜索边是满足车辆运动学的曲线。混合A的路径车辆一定能开过去标准A的不行。Q混合A*的启发函数怎么设计A常用两种。一种是无约束的2D距离计算快但不紧另一种是基于Reed-Shepp/Dubins曲线的距离考虑最小转弯半径更紧但计算量大。工程上通常组合使用。Q混合A*在自动驾驶中用过吗A用过。做园区无人车的全局路径规划时用的混合A*。在2D栅格地图上搜索节点分辨率0.2m/5度前轮转角采样5个值。搜索时间约100-300ms。路径质量不错车辆能直接跟踪。Q混合A*的缺点是什么A三个主要问题。一是计算量大——3D状态空间比2D大得多。二是路径可能不够平滑——运动基元是固定曲率的圆弧拼接处可能不平滑需要后处理。三是对起点/终点的朝向敏感——如果终点朝向不合理可能找不到解。Q混合A*和Lattice Planner有什么区别ALattice Planner预计算运动基元基元必须在状态格子上对齐。混合A在线计算运动基元不需要格子对齐但节点需要离散化用于去重。混合A更灵活但实现更复杂。Lattice Planner的搜索更快基元预计算好了但灵活性差。Q混合A*的解析扩展具体怎么用A每隔N次迭代比如100次取当前优先队列中f值最小的节点尝试用Reed-Shepp曲线直接连到目标。如果曲线无碰撞直接返回路径。这个操作的成功率在搜索后期很高——因为当前节点已经离目标很近了。在开阔空间中解析扩展能节省50%以上的搜索时间。Q混合A*在泊车场景中表现如何A非常好。泊车场景中车辆需要多次前进后退来调整位置混合A天然支持这个。代价函数中给后退加惩罚搜索出来的路径会尽量减少换挡次数。Audi的自主泊车系统就用了混合A的变体。小结混合A的核心在(x,y,theta)状态空间中用A搜索扩展节点时用满足车辆运动学的曲线。搜索出来的路径车辆一定能开过去。优势路径满足运动学约束适合车辆类机器人在自动驾驶中广泛使用。 劣势计算量大3D状态空间路径可能不平滑需要后处理对起止点朝向敏感。混合A是自动驾驶路径规划的基石算法之一。Stanford的Junior、Audi的自主泊车、很多园区无人车系统都用了混合A或其变体。理解混合A*之后下一篇讲Reed-Shepp曲线——车辆运动学的最短路径计算方法。如果这篇文章对你有帮助欢迎点赞、在看、转发三连。 你的支持是我持续更新的最大动力。「机器人软件开发面试·从入门到精通」连载系列上一篇第222篇 势场法——经典但仍有生命力的局部规划方法下一篇预告第224篇 Reed-Shepp曲线——车辆运动学的最短路径有任何问题欢迎评论区留言我会尽量回复。