很多人第一次听到 Hybrid A*是在做园区无人车、仓储机器人或者自动泊车预研的时候。我也是在某个自动化仓库的项目里被逼着上手它的当时用标准 A* 规划出来的路径在仿真里怎么看都正常可一上实车问题就全冒出来了——该过的弯过不去该倒进去的车位没有任何倒车动作车辆频繁原地打转。折腾了一个多月才彻底明白问题不在路径平滑而在搜索阶段就没考虑车辆的运动学约束。后来换到 Hybrid A*这些问题才真正被解决。这篇文章就围绕 Hybrid A* 展开从它到底在搜什么、怎么实现、为什么需要 RS 曲线收尾到我在实际测试中踩过的一堆坑一次讲清楚。不管你是刚开始接触运动规划的研究生还是已经在做路径规划相关功能开发、想快速落地的工程师这篇都值得读完。我不打算给你一个改改就能用的完整源码而是把工程里真正重要的决策点和判断依据讲透让你自己也能搭出一套能跑通的实现。1. 从 A* 到 Hybrid A*栅格搜索与车辆运动学之间的那道坎1.1 标准 A* 为什么在真实车辆上不好使先回顾一个基础问题标准 A* 在栅格地图上搜索时节点是离散格子边通常只连接相邻或对角栅格走出来的路径天生就是折线。这种折线完全没有考虑车辆的前轮转角极限、轴距、最小转弯半径这些约束。你可以在后处理阶段用样条曲线、贝塞尔曲线去平滑它但根本问题在于搜索阶段根本没有产生倒车一下再前进这类动作后处理再强也变不出一段带倒车操作的轨迹。举个例子。一个普通车位停着一辆车旁边是障碍物车辆需要倒一把才进得去。A* 搜索时只会从当前格子一步步走到目标格子路径全是前进方向的你后处理再平滑车也进不了那个需要倒车调整姿态的车位。原因很简单搜索空间里没有倒车这个维度。所以后来业界才意识到路径规划不只是在可走空间里找一条路而是在可行空间里找一条路。可行空间意味着路径上任意一点的曲率都不能超过车辆实际能执行的曲率上限而且允许车辆在必要时刻倒车。Hybrid A* 正是为了解决这个离散搜索 连续运动学约束的矛盾而出现的。1.2 Hybrid到底混在哪里Hybrid A* 里的 Hybrid指的是它在同一个搜索框架里同时处理了离散和连续两种空间。核心思路是仍然使用和 A* 一样的节点扩展、代价值比较、open list / closed list 这样的搜索骨架但每个节点不再是一个单纯的格子坐标而是一个包含位置和航向的完整位姿状态。标准 A* 的节点是 (x, y)Hybrid A* 的节点是 (x, y, yaw)甚至还要加上档位方向前进或倒车。正因为状态里多了一个航向角搜索才能区分车头朝左的位姿和车头朝右的位姿——这两者在车辆执行路径时完全是两种行为。具体落地时x 和 y 仍然映射到栅格坐标系用于快速查重和判断是否访问过yaw 则需要离散化成若干个角度区间比如把 360 度分成 36 个桶每个桶代表 10 度。这样一来状态空间就变成了一个有限的离散状态集可以从起点出发用类似 A* 的方式向目标搜索。1.3 车辆运动学模型搜索的底层物理Hybrid A* 扩展节点的方式也和 A* 不一样。A* 从当前格子跳到相邻格子Hybrid A* 则是用一组控制量按照车辆运动学模型把一小段轨迹真实地开出来。最常用的模型是自行车模型车辆轴距为 L前轮转角 δ 限制在 [−δ_max, δ_max] 范围内速度 v 可以取正值前进或负值倒车航向角变化率满足 yaw_rate v * tan(δ) / L。前轮转角最大值决定了车辆的最小转弯半径也就是路径曲率的上限。这一条是后来所有事情的基础。正因为 v 可以为负搜索算法才能主动去探索倒车这类操作这也是泊车场景能用 Hybrid A* 的关键。我刚开始接触时总觉得这里很抽象后来把车想象成人在骑车就好理解了你每次蹬一小段路车头偏一点身体倾斜一个角度下一时刻的位姿完全由当前位姿和控制输入决定。Hybrid A* 就是把这种连续运动离散成一段一段的短轨迹在搜索过程中逐步推进。2. 状态、边、代价搜索核心的数据结构与启发函数设计2.1 状态定义与离散化策略先看看一个节点需要存什么信息。我实际实现时用的是这样的结构体struct Node { double x; // 连续坐标单位米 double y; double yaw; // 航向角单位弧度 bool is_back; // 是否倒车 int x_idx; int y_idx; int yaw_idx; double g; // 从起点到当前节点的实际代价 double h; // 启发式估计代价 double f; // g h Node* parent; // 父节点指针用于回溯路径 // 以下为碰撞检测轨迹缓存 std::vectorPose partial_path; };这里要注意一点x_idx、y_idx、yaw_idx 是为了在搜索过程中快速判重。如果只保存连续坐标每次比较状态是否访问过都会很麻烦。用网格索引加角度桶索引查找基本就是 O(1) 的哈希操作。关于角度桶宽度我实际测试下来的经验是24 桶每桶 15 度会导致生成的路径比较倔一些本可以斜着通过的缝隙被漏掉36 桶每桶 10 度是一个很实用的平衡点48 桶每桶 7.5 度对窄通道和复杂掉头场景有明显改善但节点扩展数量会明显上升。第一版建议用 36 桶跑通之后再根据性能数据决定要不要加细。2.2 扩展过程控制量采样与轨迹积分扩展一个节点核心逻辑是对一组控制量做采样。我的做法是取前轮转角 δ 和速度方向 v 的笛卡尔积。比如δ 取值-δ_max, -0.5δ_max, 0, 0.5δ_max, δ_max共 5 个v 方向前进 v_forward倒车 v_back组合起来就是 10 组控制量也就是每个节点最多扩展出 10 条候选边。对每组控制量使用运动学模型按 dt 积分生成一小段带中间点的轨迹def extend(node, delta, v, dt, steps): trajectory [] x, y, yaw node.x, node.y, node.yaw for i in range(steps): x v * math.cos(yaw) * dt y v * math.sin(yaw) * dt yaw v * math.tan(delta) / wheelbase * dt trajectory.append((x, y, yaw)) return trajectory每段轨迹由多个中间点组成每个中间点代表车辆在某时刻的位姿。这些中间点是后续碰撞检测的样本点。dt 和 steps 的选择直接影响搜索质量。我的经验是低速泊车场景下速度通常 1~2 m/sdt 取 0.2~0.4 秒每段扩展 3~5 个中间点这样每段轨迹长度大约 0.5~1 米。如果 dt 太大轨迹段之间的间距过大碰撞检测会漏检dt 太小节点数爆炸搜索性能急剧下降。扩展完成后的终点状态就是该边的终点节点。把终点的 x_idx、y_idx、yaw_idx 算出来作为该状态是否访问过的依据。2.3 G、H、F 代价的工程化设计Hybrid A* 的代价函数和 A* 同源但 G 代价要复杂得多。我把它拆成几个独立叠加的项惩罚项建议初始值作用说明前进距离1.0基础代价让路径尽量短后退距离1.3倒车比前进更危险、更费时适当加重档位切换8.0防止频繁前进/倒车切换否则路径会非常凌乱转向变化0.2防止方向盘来回甩提高轨迹平顺性启发值权重1.1增强搜索目的性但会损失一定的全局最优性这些权重怎么理解我一开始把档位切换惩罚设成 2.0结果规划出来的路径在空地上来回倒车看路径轨迹像在画五角星实车执行根本没有可行性。后来把档位切换惩罚提到 8.0路径立刻变得规整了很多。转向变化惩罚这个项也非常重要尤其在低速泊车时如果这一项为 0算法会频繁选择原地大幅度打方向盘的操作轨迹看着曲率变化巨大实车转向机构根本跟不上。H 启发值的选择也有讲究。经典 A* 常用欧氏距离但 Hybrid A* 里如果用欧氏距离搜索会非常盲目因为搜索状态包含航向单纯的位置距离完全体现不出这个方向能不能直接到达目标这个问题。更合理的做法是使用满足运动学约束的 RS 距离或 Dubins 距离也就是忽略障碍物的情况下从当前位姿到目标位姿的最短可驾驶路径长度。对于泊车和园区低速场景RS 距离更合适因为它同时允许前进和倒车。计算 RS 距离比欧氏距离慢不少但比完整碰撞检测快得多。工程上可以采用降低频率的策略不是每个节点都算 RS 距离而是每隔 10~20 个节点做一次精确计算中间的节点用欧氏距离乘一个修正系数近似。实测下来这种策略能把搜索时间缩短三分之一左右。2.4 open list 用哪种堆实现搜索性能的一个关键点是 open list 的数据结构。大多数教科书代码使用 std::priority_queue配合懒删除策略当某个状态的新 g 值更小时不更新原节点而是重新 push 一个新节点并用 visited 标志忽略旧节点。这个方案代码最简单实测在中等地图上完全够用。如果想追求更高性能可以用支持 decrease-key 的配对堆、斐波那契堆等。好处是 open list 中的节点数会明显减少坏处是代码复杂度和出错概率上升。我给出的建议是第一版一定用 priority_queue 跑通只有遇到实际性能瓶颈后再考虑升级结构。过早优化是运动规划实现里最容易犯的错误。3. 为什么 RS 曲线能一锤定音终点冲刺与死胡同回避的机制3.1 RS 曲线解决的核心问题RS 曲线是一套在无障碍物假设下求解两点之间满足最大曲率约束的最短路径的经典解法。它的输出是一组由圆弧段和直线段组合而成的路径最多由几个基本图元拼接而成。这套解法和 Dubins 曲线的区别在于Dubins 只允许前进RS 曲线允许前进和倒车。因此对需要倒车调整姿态的泊车场景RS 曲线是更贴合实际的那个。你可能要问路径规划不是要避障吗无碰撞假设下的曲线有什么用其实它的价值恰恰体现在两个地方一是作为搜索的启发值告诉算法如果不考虑障碍物从这个位置到目标还有多远这是非常精准的 H 代价二是用于最终的收尾当搜索到某个节点时尝试直接用 RS 曲线连接目标和当前节点如果这条曲线在障碍物检测下是安全的那就直接把它拼接到路径末端搜索立即结束。3.2 final shot提前结束搜索的加速器这是 Hybrid A* 工程实现中非常常用的一招。在每次从 open list 中弹出节点后除正常的节点扩展外额外做一次终点冲刺current pop_min_from_open() if rs_curve(current, goal) exists and collision_free(rs_curve): return combine_path(backtrack(current), rs_curve)为什么要这么做因为纯靠栅格搜索一路扩展下去最后一公里的搜索会很慢而且离散化状态下很难精确命中目标航向。RS 曲线是解析方法能精确地从任意位姿到达目标位姿曲率也严格满足约束。只要它无碰撞这段路径就完全合法而且往往比继续搜索得到的路径更短、更平滑。我实际测试下来的感受是有没有 final shot对搜索耗时的影响是数量级的。一个 100 米 × 100 米的地图没有 final shot 可能要搜索数万节点加了 final shot经常几千节点就收敛了。原因就是搜索不再需要一格一格地摸到目标而是可以从中间任意节点直接冲刺到终点。3.3 用 RS 曲线避开死胡同RS 曲线还可以用来处理搜索中的死胡同问题。Hybrid A* 的一个典型毛病是启发函数低估了某个方向的实际代价导致搜索进入一条死路。比如一个 U 型区域从外面看离目标很近但车辆进去之后必须原路倒出来这在启发值上完全没有体现。解决方法是在节点扩展时对 RS 曲线做一次碰撞检测。如果从当前节点出发的 RS 曲线虽然无碰撞却因为路径穿过障碍物被判定不可行算法可以对该节点施加一个额外的惩罚或者直接把该分支标记为无效。这就相当于提前识别出这个方向看着近实际过不去从而引导搜索绕行。在我做的园区项目里这个处理让搜索成功率提升了很多。不加这个机制时车辆经常钻进某条岔路然后卡住搜索时间飙到十几秒加上 RS 碰撞检测之后这类问题基本绝迹。3.4 RS 曲线计算时的数值稳定性RS 曲线的求解公式看起来简单但在实际代码里有一个容易翻车的细节角度差必须归一化到 [−π, π] 之间。我第一次实现时没注意这个结果在多个测试场景里出现目标明明就在正前方RS 曲线却绕一个大圈子的诡异现象。排查后发现是角度归一化不彻底导致的。另一个注意点是单位。RS 曲线的许多公式依赖曲率和弧长推导如果 x、y 坐标用像素坐标曲率单位就会乱掉。建议所有输入都使用米制坐标并在计算前把坐标平移到以目标点为中心的原点附近能有效减少浮点误差。低速场景下弧长很小数值稳定性尤其要重视。4. 实测避坑记录代价权重、碰撞检测与窄通道这些细节4.1 代价权重怎么调一次只动一个参数我见过不少人在 Hybrid A* 上花了很长时间调参最后发现是权重互相干扰导致搜索行为完全失控。这里分享一个我总结出来的调参顺序先把所有额外惩罚项设为 0只保留前进和后退距离代价在简单场景下确认搜索能产生一条可执行的路径哪怕比较绕逐步增加档位切换惩罚观察路径里倒车次数是否显著减少增加转向变化惩罚观察轨迹是否更平滑最后调启发值权重和膨胀半径。权重变化的观察方法最好是可视化。把每个被扩展的节点画到地图上能一眼看出搜索是在乱逛还是直奔目标。如果节点分布又散又乱大概率是启发权重太低或换挡惩罚不足如果节点分布过于集中到某一条直线可能是启发权重太高搜索过早贪婪地收敛到局部最优。下面是我在多个项目里沉淀下来的初始参数表可以直接作为起点使用参数我的初始值调参方向前进距离代价1.0固定为基准后退距离代价1.2~1.5后退频繁时增大档位切换惩罚8.0~12.0路径太碎时增大转向变化惩罚0.1~0.4路径抖动明显时增大启发值权重1.05~1.2搜索太慢时增大角度桶数量36路径质量差时增至 484.2 碰撞检测的漏检与过检碰撞检测是 Hybrid A* 里最容易出现隐性 bug 的地方。最简单的做法是把车辆看成一个矩形在每一小段轨迹上用矩形外轮廓去覆盖栅格地图矩形覆盖到的格子有障碍物就判定碰撞。但这个做法存在两个极端漏检和过检。漏检发生在高速采样时。如果轨迹中间点隔得太远矩形轮廓之间的栅格没有被覆盖到车辆正好从障碍物边缘擦过去检测却认为安全。解决方法是按固定弧长加密采样点比如每 0.1 米生成一个新的车辆轮廓宁可多算几次也不能漏。过检则更容易让人迷惑。车辆矩形如果画得比实际车体大不少经过窄通道时明明车身能过碰撞检测却把整个通道堵死了。我的建议是碰撞检测的矩形按真实车辆尺寸再加 5 厘米、10 厘米余量而搜索阶段的膨胀图和最终 RS 路径校验可以分开处理。前者用于高速粗筛后者用于最终确认。4.3 膨胀半径与窄通道的取舍在某个园区场景里车位两侧的立柱间距只有车宽加 0.8 米一开始我把障碍物膨胀了 0.3 米搜索结果直接失败——因为膨胀后的通道完全被堵死。后来把膨胀半径降到 0.2 米搜索才成功。这个现象反映出膨胀半径和窄通道之间的天然矛盾。我的处理方式是分两层搜索阶段使用适中的膨胀半径尽量保证通道不因膨胀而断绝但要以较高的碰撞检测密度兜底路径生成后再用真实车辆轮廓重新校验一遍最终路径。如果最终路径与障碍物的最小距离依然满足安全阈值就接受它。这个策略在多个窄通道场景中都很有效。要小心的是不要把搜索阶段的膨胀半径设得太小。过小的膨胀半径会导致搜索出来的路径贴着障碍物走后续平滑阶段一旦把路径稍微拉直就可能重新撞墙。经验值是搜索阶段膨胀半径取车辆半宽加 0.1~0.3 米具体看场景宽裕程度。4.4 死胡同与搜索时间失控还有一个常见问题是搜索时间失控。地图只有几百平方米却经常跑十几秒才出路径。我的排查套路是依次检查这几项是否启用了 final shot 机制启发值权重是否过低角度桶数量是否被无意中调高控制量集合是否包含了不必要的组合碰撞检测是否有重复计算。每一步都用日志打点验证。比如加上本次搜索扩展节点数碰撞检测调用次数RS 曲线计算次数这几个计数一眼就能看出瓶颈在哪里。多数情况是 RS 曲线被反复调用却没有起到短路作用——这种情况通常意味着地图中大量区域对当前状态不可达需要检查 RS 启发值是否为无穷大以及死胡同标记是否生效。5. 落地参考一个最小可复现的工程骨架与调参顺序5.1 模块划分与文件组织很多初学者一上来就想写一个几百行的 main.cpp最后被各种细节淹没。我推荐把工程按职责拆成独立模块至少包含以下文件gridmap.hpp栅格地图加载、膨胀、障碍查询kinematics.cpp车辆运动学模型前轮角、速度到轨迹的积分collision_checker.cpp车辆轮廓与地图的碰撞检测rs_curve.cppRS 曲线求解与启发值计算hybrid_astar.cpp搜索主流程main.cpp入口加载地图调用搜索输出路径。这种划分的好处是任何一个模块都可以单独拿出来做单元测试。尤其是碰撞检测和 RS 曲线求解这两个模块如果独立测试通过搜索主流程里的问题就只剩代价权重和启发函数逻辑排查范围会小很多。5.2 主循环伪代码搜索主流程用伪代码表示和 A* 长得几乎一样差别只在扩展方式和 final shotpush_to_open(start_node) while open_list not empty: current pop_min_f(open_list) if visited[current.id]: continue visited[current.id] true if is_goal(current): return reconstruct_path(current) rs_path rs_curve(current, goal) if rs_path exists and collision_free(rs_path): return combine(reconstruct_path(current), rs_path) for control in control_set: next_traj simulate_kinematics(current, control) if not is_valid(next_traj): continue next_node make_node(next_traj) next_node.g current.g cost(current, next_traj) next_node.h heuristic(next_node) if next_node.id not in visited and next_node.g best_g[next_node.id]: best_g[next_node.id] next_node.g next_node.parent current push_to_open(next_node) return failure这个主循环本身不复杂复杂的地方全在 cost 函数、heuristic 函数和 collision_free 函数里。实现时建议从上到下逐步填充不要一次性写完所有细节每加一块都跑个小场景验证。5.3 调参顺序与验证方法我刚上手时犯过一个错误直接拿一个很难的泊车场景调试。结果搜索失败后根本分不清是碰撞检测的问题、启发函数的问题还是代价权重的问题。后来我总结了从易到难的验证路径无障碍物场景验证 RS 曲线和运动学模型是否正确路径能否从起点到终点且曲率满足约束单障碍物场景验证 final shot 是否会在障碍物附近失效搜索是否能绕过障碍物有窄通道和柱子的园区场景验证膨胀半径与碰撞检测密度的匹配度真实泊车位场景验证倒车换挡动作和最终停车姿态是否满足要求。每个阶段都记录三组指标搜索时间、路径长度、换挡次数。换挡次数是一个很容易被忽略但非常关键的指标。哪怕路径稍微长一点如果换挡次数从 5 次降到 2 次实车执行的质量也会有本质提升。5.4 从 C 部署到 Python 原型的效率差最后说一个工程实践上的经验我强烈建议先用 Python 做原型验证再迁移到 C 部署。Python 配合可视化的路径绘制调试代价权重和启发函数的速度远快于 C。等搜索行为和路径形态都对了再翻译成 C 版本性能只是最后的工程优化问题。我见过有人一上来就用 C 写结果调一个启发函数的权重都要重新编译、再看日志整个调试周期被拉得很长。先用 Python 把算法逻辑跑通再用 C 重实现两种语言之间的代码逻辑几乎可以一一对应迁移成本非常低。我在实际测试中的体会还有一个调参时每次只动一个参数非常关键。换了权重后又改了角度桶数量出了问题很难判断是哪个改动造成的。把除目标参数外的所有变量固定住记录改动前后路径形态和性能指标才能形成可复用的调参经验。等你把换挡惩罚、启发权重、碰撞检测密度这三组关键参数摸透之后Hybrid A* 在绝大多数园区和泊车场景里都能稳定工作搜索时间通常能控制在 500 毫秒以内路径质量也足够直接给终端执行了。
