企业官网建设流程全解析

深夜调车的时候，那种状态很容易让人上瘾：整个停车场只剩你一个人,屏幕上的轨迹线亮着冷光，一圈圈扫过车位。我那天本来只想把泊车功能的老问题复现一遍，结果盯着轨迹优化模块的日志看了两个小时，越看越觉得这套东西里藏着不少“骚操作”。它不像高速场景那样讲究全局最优和长时间博弈，而是在“窄车位+低速+精准停正”这组苛刻条件下，把一个看似简单的轨迹求解问题玩出了花。今天不聊虚的，直接扒开代码，看看泊车轨迹优化里三个核心模块到底怎么打架、又怎么配合的。

先说清楚这东西是干嘛的。市面上主流开源方案里，泊车那段轨迹规划通常不是一个小模块，而是一条完整链路：先有感知给出的车位位姿，再有决策决定是前进还是倒车，最后落到轨迹优化，算出一条能让车“一把进”或“多把进”的光滑轨迹。这篇文章适合两类人：一类是刚接触规划控制、想在实车或仿真里复现泊车逻辑的开发者，另一类是已经在做轨迹优化、但被各种参数调到怀疑人生的工程师。我会直接按模块拆，讲清楚每一步为什么这么做，以及我踩过的坑。

1. 整体链路拆解：三个模块怎么分工，又怎么“打架”

泊车轨迹优化听着好像就是一个函数“求解出一条路径”，但实际工程里，它被拆成三个边界非常清晰的模块：方向搜索模块、轨迹平滑模块、速度绑定模块。这三个模块各有各的脾气：搜索模块要快，恨不能几百毫秒内从状态空间里捞出一条可行路径；平滑模块要稳，恨不得把每个点上的曲率变化都限制得死死的；速度绑定模块要准，必须让车在终点附近稳稳停住，不能带着速度冲进车位。三者目标不一样，接口一对接，自然就“打架”。

我最早接触这套代码的时候犯过一个错：把这当成一个整体黑盒，遇到问题就瞎调参数，结果越调越乱。后来我把链路彻底拆开看，才明白每个模块的输入输出到底是什么关系：

• 搜索模块的输入是车位角点、自车起始位姿、障碍物边界；输出是一条由离散位姿点组成的粗路径，每个点包含(x, y, 航向角)，但不保证曲率连续，甚至可能带着尖角。
• 平滑模块吃下这条粗路径，在S-L坐标系里做优化，输出是一条曲率连续、曲率变化率受控的光滑曲线，但此时它依然没有时间信息。
• 速度绑定模块拿到光滑路径后，在S-T空间里规划速度曲线，给每个路径点附上时间戳，最终输出一条完整的、带速度和加速度约束的轨迹。

为什么不能一个模块直接干完？因为把“几何可行”和“动态可行”揉在一起，求解难度会爆炸。搜索算法在离散栅格上跑，天然适合找“有没有路”；平滑算法在连续空间里优化，天然适合“把路修整”；速度规划关心的是“什么时候到哪、以多快速度到”。三者解耦之后，每个模块都可以用自己最擅长的数学工具，这才是工程上真正可落地的方案。

但“打架”恰恰出现在解耦的接缝处。举个例子：搜索模块为了快点找到路，可能走了一条贴着障碍物的窄缝，平滑模块一上采样点，曲率约束一拉，轨迹就可能被推回障碍物里；速度绑定模块看到一条很弯的路径，为了不超侧向加速度，只能把速度压到极低，结果搜索模块算出能过、速度模块却让车在弯心几乎停下来。所以真正调好东西，不是在单个模块里自嗨，而是要站在接口处看数据流，理解每个模块的“脾气边界”。

2. 搜索模块：如何在离散状态空间里先捞出一条“能走”的路径

2.1 为什么泊车搜索不能直接套A*或RRT

最简单的想法是栅格化地图，用普通A找一条从起点到目标点的路径。但泊车场景有个硬约束：车辆不能原地转向，航向角不能突变。普通A搜出来的路径经常是“先横着走两步、再竖着走两步”，在车看来根本没法执行。RRT虽然随机性强，但产生的轨迹抖动大，后期平滑模块要花很大力气才能修回来。

所以泊车场景的标准做法是混合A*（Hybrid A*）：它在扩展节点时直接考虑车辆运动学约束，每走一步都按“最大转角”“最小转弯半径”来生成下一段圆弧或直线，让搜出来的路径天生就是车能跟的路径。这个思路听着简单，实际上有两个关键细节：

• 状态空间不是(x, y)二维，而是(x, y, 航向角)三维。航向角要离散化，通常按6度或10度一个分辨率。分辨率太大，路径粗糙；分辨率太小，状态爆炸，搜索变慢。
• 每扩展一个节点，都要做一次碰撞检测，但不是检测一个点，而是检测整个车体矩形在当前位置、当前航向下的轮廓是否与膨胀后的障碍物相交。

2.2 启发式函数的“双通道”设计

混合A*最容易翻车的地方是终点收敛。停车场环境里，车位往往在角落，普通欧氏距离启发式会把搜索引向一条“看起来近、实际绕远”的路。我见过有的实现只用一个简单的Reeds-Shepp距离做启发式，结果在通道复杂时搜索节点发散，耗时飘到几秒甚至更久。

好的做法是双通道启发式：一路计算无障碍物时的RS曲线长度，快速指向终点；另一路跑一个带障碍物代价的Dijkstra，生成每个栅格到终点的最短距离图。两路取最大值作为启发值，兼顾“快速收敛”和“绕障能力”。这块代码删起来很诱人，因为双通道会多占内存和初始化时间，但实测下来，在窄车位场景里它能省掉一半以上的无效节点扩展，该保留。

2.3 搜索完的“最后一步”处理

搜索模块还有个容易忽略的细节：当扩展节点距离终点足够近时，不要再靠栅格一步步磨了，而是直接用RS曲线解析连接当前节点到目标位姿。这是因为栅格搜索的航向角是离散的，最后几米可能因为航向角差一个分辨率，导致永远无法精确终止。RS曲线是一系列“圆弧+直线”的解析解，可以精确到达指定位置和航向。

伪代码逻辑大致是这样：

def search(start_pose, target_pose, map): while not open_set.empty(): current = open_set.pop_lowest_cost() if distance(current, target_pose) < step_length: candidate = rs_curve(current, target_pose) if is_collision_free(candidate, map): return reconstruct_path(candidate) for next_pose in expand_by_motion_primitive(current): if not is_collision_free(next_pose, map): continue update_cost_and_push(next_pose)

这段代码最值得注意的点是：判定“接近终点”的阈值不能太小，我试过设成半个车长，结果在搜索末尾反复横跳；设成一个步长长度就稳很多，RS曲线连接的成功率也高。

3. 平滑模块：把“能走”的粗路径，修成“好走”的光滑曲线

3.1 平滑的目标不只是“不碰到障碍物”

很多人以为平滑就是把折线变成曲线，视觉上好看就行。实际上，平滑模块要同时满足三个目标：距离障碍物足够远、曲率不超过车辆极限、曲率变化率不超过转向执行器转速。第三个最容易被忽略，但它恰恰是实车上“方向盘猛打”和“方向盘顺滑”的区别。

我把这三个目标套进生活场景来解释：一条粗路径就像山路上的车道线，贴着悬崖、弯很急；平滑模块像是“把车道线重新描一遍”，但要保证：不越过路边护栏（避障）、每个弯的角度不能超过车的转向极限（曲率约束）、方向盘转动速度不能像甩方向盘一样瞬间拉满（曲率变化率约束）。三件事同时满足，路径才能真正交到控制模块手里。

3.2 QP优化问题怎么建

工程里最常见的建模方式是二次规划（QP）：把路径在S方向上均匀采样成N个点，每个点的横向偏移量作为优化变量，构造一个带约束的优化问题：

• 目标函数：最小化横向偏移变化率（让路径平滑）、最小化与参考粗路径的偏差（别跑太远）、最小化曲率变化率（让转向柔顺）。
• 约束条件：每个采样点到障碍物的距离大于安全阈值；每个点的曲率小于最大曲率（由最小转弯半径决定）；相邻点曲率差值小于最大曲率变化率；起点和终点的位姿固定。

这个优化问题的关键陷阱是参考线质量。如果搜索模块给的粗路径本身质量差，优化器为了满足碰撞约束，会把路径“顶”到障碍物旁边，看着每帧都安全，实际上安全裕度已经被吃光了。所以我在实际使用中，会对粗路径先做一次低通滤波，把高频抖动抹掉，再喂给QP。这一步成本极低，收益非常明显。

3.3 障碍物约束是靠“硬编码”还是“软惩罚”

泊车环境障碍物复杂，车位两侧可能停着车，后方可能有矮桩，如果所有障碍物约束都写成硬约束，QP大概率无解——因为搜索出来的粗路径本身可能就是擦着边过的。更稳妥的做法是分两层：硬约束只保留车辆极限参数（曲率、曲率变化率）和不与关键障碍物相交；软惩罚覆盖所有普通障碍物，用罚函数让优化器尽量远离，但允许在极端情况下小幅靠近。

软惩罚里有个衡量系数要格外小心：太大，轨迹会扭曲到奇怪的方向去绕障；太小，平滑模块等于没看到障碍物。我一般的起步值是让障碍物惩罚项在总代价里占3成左右，再根据实车表现微调。视图化调试时，直接打印每个采样点到最近障碍物的净距，比看轨迹形状直观得多。

4. 速度绑定模块：没有时间戳的轨迹，控制模块根本没法执行

4.1 从路径到轨迹：“什么时候、以多快”到达每个点

平滑模块输出的只是一条几何曲线。控制模块拿到一个路径点，只知道“这里有路”，但不知道“该什么时候到这里、速度是多少”。要真正执行，必须做速度规划，把路径变成轨迹——每个点都带有期望速度、加速度甚至加加速度。

泊车速度规划有两个特殊性：一是速度本身就低，通常不会超过5km/h，所以“安全跟车”不是核心矛盾，“精准停位”才是；二是终点速度必须严格为0，而且最好以非常小的速度逼近终点，避免到达时猛跺一脚刹车。

常见的速度曲线是S-T图里的梯形或三角曲线：先匀加速到一个巡航速度（泊车时一般是1~2m/s），再匀速走一段，最后匀减速到0。泊车场景里，我更推荐在末端加一段“低速逼近段”：从巡航速度先减速到一个接近速度（比如0.3m/s），保持一小段距离，再减到0。这样做的好处是，即使位姿估计有误差，车也有机会在低速下修正，不会带着明显初速度冲进车位。

4.2 速度和曲率的关系，是速度绑定的隐藏噩梦

几何路径再光滑，也拗不过物理：速度越高，允许的曲率越小。因为侧向加速度和速度平方乘以曲率成正比。在泊车这种低速场景，速度低到一定程度后，曲率约束几乎不会触发。但如果路径里有一段很小半径的转弯（比如车位前后夹击需要原地多打几把），速度又没来得及提前降下来，就会出现一个很尴尬的现象：路径上某点的曲率明明是车辆极限内，但是按当前速度算出来的侧向加速度已经超了。

解决起来不复杂，但一定要做：速度规划完成后，逐点反算曲率×速度平方，校验侧向加速度。超了就局部降低速度，再重新平滑一段速度曲线。我在项目里吃过这个亏：第一次实车测试，车在车头快接近障碍物时突然横向漂了一下，数据回放看到曲率只有0.8，没问题，但速度是1.8m/s，侧向加速度已经逼近极限，轮胎抓地力在车库环氧地坪上根本扛不住。从那以后，侧向加速度校验就成了必跑项。

4.3 验证安全靠“全路径碰撞再检查”

轨迹最终出炉前，还有一道保险：把带时间戳的轨迹离散成更密的点集，对每个点重新做一次全车体轮廓碰撞检测。为什么已经做过了还要再做？因为平滑和速度模块都可能“微调”过轨迹，哪怕只偏移了2厘米，在窄车位里也可能是生与死的差别。我通常设置每10厘米一个校验点，比平滑采样密5倍，扫一遍不到10毫秒，非常划算。

5. 实车调参心得与Bug排查实录

5.1 典型问题一：平滑后轨迹反而侵入障碍物

我遇到过好几次：搜索出来的粗路径明明是安全的，平滑模块一跑，轨迹却往障碍物方向“鼓”。排查后发现是QP的采样分辨率不够密，相邻采样点之间间隔太大，障碍物约束只保证了采样点本身安全，两点的连线却穿了障碍物。解决方法很简单：加密采样间隔，同时在优化后检查相邻点连线与障碍物是否相交。采样点从1米加密到0.2米后问题立刻消失，代价是求解时间多了几十毫秒，泊车场景完全扛得住。

5.2 典型问题二：轨迹末端反复“甩头”

方向盘在终点附近老是来回校正，看起来车像在“犹豫”。问题根源是搜索模块的粗路径在末端有航向角残差，平滑模块为了让终点的航向角严格满足目标位姿，强行憋了一个大曲率弧度。看代码会发现，终端航向角约束被写成了硬约束，但距离终点的最后一段采样点太少，优化器只能用一个大曲率来补足角度差。

我的处理是先调高末端采样密度，确保终点的角度变化摊到更多点上；如果还不行，就放宽终点的航向角容差——比如允许5度以内的误差，交给控制模块去补偿，而不是轨迹算法硬扛。实测下来，控制模块纠偏5度远比轨迹憋一个大弯要稳。

5.3 典型问题三：速度规划导致停车点头

这个最玄学。明明速度曲线很平滑，终点速度是0，但实车上每次到位都“咯噔”一下。查了很久才发现，是速度规划里终点的减速度段设计得太硬：从匀速段直接以最大减速度降到0，加速度在终点处发生阶跃。控制模块为了跟踪这个阶跃，不得不让刹车猛一踩。解决方式是加一个“加加速度（jerk）约束”，让减速段的减速度线性变化，而不是恒定最大。车一下子就顺了，连续测试二十次都不点头。这个参数值得认真调，即便泊车速度低，jerk约束对乘坐体验的提升也非常明显。

5.4 常见问题速查表

表格之外，还有一个我自己常用的“土办法”：把每条轨迹的曲率变化画成热力图，颜色越红代表曲率变化率越高，这样一眼就能看出哪段轨迹在“折磨转向机构”，比盯着密密麻麻的坐标点直观太多了。

6. 一点个人经验和最后的技巧

这套泊车轨迹优化改到现在，最大的体会是模块配合甚至比单模块优化更重要。搜索模块多绕一点路，平滑模块就多一点调整空间；平滑模块多留2厘米安全裕度，速度模块就不用把车压得太慢；速度模块末端温和一点，控制模块就不会在最后关头手忙脚乱。

最后分享一个我一直在用的技巧：调试时别只盯着成功率和耗时，多看几个“边缘样本”——车位特别窄、旁边柱子特别近、地面有坡度的场景。问题往往藏在那些“再差一点点就不行”的边界条件里。把边缘样本跑顺了，正常场景基本不会出大幺蛾子。这套代码改到现在，我依然觉得它还能再“骚”下去，但先把基础配合打好，比堆一堆花哨算法实用得多。