从A*到Hybrid A*：泊车规划这条路，我走过的弯路和正道
做泊车规划的第一年，我觉得 A* 就是答案。
那时候刚从学校出来，脑子里全是算法课上的东西：栅格地图、f=g+h、open list、closed list。把停车场建模成网格，起点终点一设，A* 一跑，路径就出来了。干净，漂亮，教科书级别。
然后我把这条路径发给控制同事，他看了一眼说：“这车怎么原地转90度？”
那一刻我才意识到，A* 规划出来的路径，是给一个没有体积、没有转弯半径、可以瞬移的”点”走的。车不是点。
A* 本身没问题，问题是你拿它干什么
先把 A* 的核心逻辑说清楚，因为后面 Hybrid A* 的骨架还是它。
A* 做的事情很简单：在一张栅格地图上，找从起点到终点的最优路径。每个格子有一个评估分数：
f(n) = g(n) + h(n)g(n)：从起点走到当前格子，实际花了多少代价（走过的距离）h(n)：从当前格子到终点，估计还要花多少代价（启发函数）
每次从待探索的格子里挑 f 值最小的那个展开，直到摸到终点。
h(n) 怎么选？这个问题我当时纠结了挺久。欧氏距离最简单，但它假设你能直线飞过去——车又不是无人机。曼哈顿距离只适合上下左右四方向走格子。在泊车场景里这俩都不靠谱，因为车有最小转弯半径，你离车位直线2米，实际可能要绕8米才能到。
A* 在泊车里能干什么？老实说，能干的事不多，但有一件很关键：作为全局粗规划器。在大型停车场里，从入口到目标车位可能隔着好几排车，中间要绕行。这种”找大方向”的活，A* 在二维栅格上跑一遍就够了，不需要考虑车辆姿态。
但从”车位附近”到”停进车位”这最后几米，A* 就彻底不行了。
为什么必须用 Hybrid A*
标准 A* 的搜索空间是离散的二维网格 (x, y)。车辆的真实状态是连续的三维空间 (x, y, θ)——位置加朝向。一辆车停在同一个格子里，朝北和朝南是完全不同的两个状态，但 A* 分不出来。
Hybrid A* 的核心改动就一句话：节点不再是格子，而是车辆的真实姿态 (x, y, θ)。展开节点时套用一组运动基元——左转、直行、右转，各带前进和倒车，每个都遵守最小转弯半径。我们用5度一档的转向角离散化，这个粒度是试出来的折中。
启发函数换成了 Reeds-Shepp 曲线——算满足转弯半径约束下的最短路径长度，比欧氏距离靠谱得多。碰撞检测是性能瓶颈，一开始老实做多边形碰撞，一帧200ms，后来改成障碍物膨胀掩码只查中心点，快了一个数量级。
理论到工程之间，隔着一个停车场的距离
算法跑通了，我一度觉得问题解决了。然后实车测试给了我当头一棒。
第一个坑是车位在”漂”。 感知同事给我的车位坐标，每一帧都在微微抖动。视觉检测本身有误差，车辆自身的定位（航位推算）也有累积漂移。我拿着一个不断跳动的终点去规划，Hybrid A* 每次算出来的轨迹都不一样，车开起来一会儿往左修一会儿往右修，乘客体验极差。
第二个坑是远近不一致。 车在10米外识别到车位时，角度偏、分辨率低，解算出来的车位位置可能偏了20厘米。等车开到3米外，摄像头看得更清楚了，车位位置突然”跳”了一下。如果不处理这个跳变，规划出来的轨迹会突然拐弯。
我们最后的方案分两步走。
先用卡尔曼滤波锁定车位。车位是静态的，但我们对它的观测是带噪声的，这正好是卡尔曼滤波最擅长的场景。
滤波器设计很直白：
- 状态向量：车位四个角点的坐标，
x = [x1, y1, x2, y2, x3, y3, x4, y4]，8个量 - 状态转移矩阵 F：单位矩阵。车位不动，下一帧的真实位置等于上一帧
- 过程噪声 Q：不是说车位会动，而是我们的定位在漂，这个漂移会被错误地”映射”成车位在动。Q 就是用来对冲这个定位误差的
- 观测矩阵 H：单位矩阵。我们直接观测角点坐标
- 观测噪声 R：传感器测量的不确定性——像素识别误差、相机标定误差、地面不平导致的投影误差，全在这里面
每来一帧新的车位检测结果，滤波器就做一次”预测-更新”。几帧之后，车位坐标就收敛到一个稳定值，不再跳了。
然后是粗规划 + 精规划。不再指望一次规划搞定全程。
车在远处时，用初始感知的车位做一次粗规划，先把车引导到车位附近。等车开近了，摄像头看得更清楚，卡尔曼滤波也积累了足够多的观测，这时候在轨迹上找一个合适的”泊车切入点”，用更新后的高精度车位坐标重新跑一次 Hybrid A*。
这个”切入点”的选择也有讲究——不是随便找个近的点就行，要保证车辆在该点的姿态适合直接入库，否则还得多倒一把。
回头看
现在回想，A* 那段时间也不算白费。它把搜索的基本框架——open list、启发函数、最优性保证——刻进了我的肌肉记忆，这些概念在 Hybrid A* 里一个没少用。只是当时太天真，以为算法对了，事情就成了。
实际上，Hybrid A* 本身的代码量并不大，我大概两周就写完了。但围绕它的工程适配——卡尔曼滤波、粗精规划切换、碰撞检测加速、和感知同事反复对齐接口——前前后后折腾了三个月。这可能就是做工程和做算法题最大的区别：题目里的世界是干净的，现实里的停车场不是。