深夜调车的时候突然发现，Apollo的泊车轨迹优化藏着不少“骚操作“。咱们今天不聊虚的，直接扒开代码看三个核心模块怎么打架...哦不，怎么配合的

apollo 泊车轨迹优化代码 hybridastar+iaps平滑优化+obca平滑优化 第一个图是matlab绘制 后面的图是程序用sdl库绘制

先看Hybrid A*这个愣头青。这货生成的轨迹就像刚拿驾照的新手，能避开障碍物但轨迹拧巴得很。看看它扩展节点的代码片段：

Node3D* expand(const Node3D& current, int direction) { // 方向盘打满时的转弯半径 const float delta = direction * steering_angle_ * M_PI / 180; float x = current.x + step_size_ * cos(current.t); float y = current.y + step_size_ * sin(current.t); float theta = mod2pi(current.t + delta); return new Node3D(x, y, theta, current); }

这代码把车辆运动拆解成离散的"碎片"，每步前进固定距离+固定转向角。虽然生成路径像折线，但在狭窄车位里确实能找到解。Matlab绘制的初始轨迹图上那些锐角转折，就是这哥们儿的杰作。

接下来出场的IAPS（Iterative Adaptive Polynomial Smoothing）像个强迫症患者。看它处理曲率的这段：

def ips_smoothing(path, max_iter=50): for _ in range(max_iter): # 曲率突变检测 curvature_grad = np.abs(np.gradient(curvature(path))) violation_idx = np.where(curvature_grad > max_curvature_rate)[0] # 在突变点插入控制点 new_ctl_pts = insert_control_points(path, violation_idx) # 重新拟合B样条 smoothed = bspline_fit(new_ctl_pts) return smoothed

这个迭代过程就像在折线上不断"打补丁"，SDL可视化里那些突然变平滑的弯道，就是它一次次插入控制点的结果。不过有时候会矫枉过正，轨迹离障碍物太近...

apollo 泊车轨迹优化代码 hybridastar+iaps平滑优化+obca平滑优化 第一个图是matlab绘制 后面的图是程序用sdl库绘制

这时候OBCA（Optimization-Based Collision Avoidance）带着数学家的严谨进场了。看这段障碍物约束的处理：

// 构建安全边界 for (const auto& obs : obstacles) { // 把障碍物膨胀成凸多边形 Polygon safe_zone = inflate_polygon(obs, vehicle_width); // 添加分离超平面约束 for (int i=0; i<traj.size(); ++i) { auto h = compute_separating_hyperplane(traj[i], safe_zone); problem.AddConstraint(h.dot(traj[i]) >= 1); // 数学意义上的安全距离 } }

这代码把碰撞检测变成了凸优化问题，SDL界面里那些红色安全边界就是它的手笔。有意思的是，当Hybrid A*和IAPS的轨迹被OBCA重构后，会发现原本贴着障碍物的路径突然变得"守规矩"了。

三种算法在可视化中的表现差异明显：Matlab的初始路径充满棱角，SDL的动态演示里能看到轨迹像被无形的手逐步捋直。实测中发现，当车位两侧误差小于10cm时，OBCA会把轨迹往车位中线拽，这过程在SDL的实时渲染里看得特别清楚——轨迹线像橡皮筋一样被物理引擎拉扯。

调参时有个邪门现象：把IAPS的迭代次数设太高反而容易让OBCA报无解。后来发现是过度平滑导致优化时没有足够的调整空间，这俩模块得像说相声似的互相找平衡。现在看Apollo的泊车轨迹，那丝滑的入库动作背后，其实是三个算法在掐架又合作的结果。