在Apollo自动驾驶系统中，Planning（规划）和Control（控制）模块中的曲率平滑（Curvature Smoothing）是确保车辆轨迹平顺、舒适且符合车辆动力学约束的关键步骤。

---

1. 曲率平滑的核心目标

• 减少横向加速度突变：避免方向盘的频繁剧烈转动，提升舒适性。
• 满足车辆运动学约束：确保曲率连续且符合最大转向角限制（如阿克曼转向几何）。
• 路径跟踪可行性：控制模块（如MPC）需要平滑的曲率输入以生成合理的转向指令。

---

2. Planning模块中的曲率平滑

(1) 输入与输出

• 输入：规划模块生成的初始路径（离散点列，含坐标 $((x,y))$ 和航向角 $(\theta )$）。
• 输出：平滑后的路径点，附带曲率 $(\kappa )$ 信息。

(2) 平滑算法：基于优化与样条曲线

Apollo主要采用以下两种方法：

方法一：二次规划（QP） + 曲率约束

• 优化变量：路径点的横向偏移量 $(l_i)$（相对于参考线）。
• 代价函数：平衡曲率变化率和平滑性：
  $$\min \sum _i{\left({\kappa }_{i+1}-2{\kappa }_i+{\kappa }_{i-1}\right)}^2+w\cdot l_i^2$$
• 第一项最小化曲率的二阶差分（曲率变化率）。
• 第二项防止偏离原始路径过远（权重 $(w)$ 调节）。
• 约束条件：
• 曲率连续性：$({\kappa }_i=\frac{{\theta }_{i+1}-{\theta }_i}{\Delta s})$（$(\Delta s)$ 为弧长）。
• 最大曲率限制：$(|{\kappa }_i|\le {\kappa }_{\text{max}})$（由车辆最小转弯半径决定）。

方法二：螺旋曲线（Clothoid/Euler Spiral）

• 原理：螺旋曲线的曲率随弧长线性变化（$(\kappa (s)={\kappa }_0+c\cdot s)$），天然适合平滑过渡。
• Apollo实现：
• 将路径分段拟合为螺旋曲线，连接处曲率和曲率导数连续。
• 使用Fresnel积分数值计算曲线坐标：
  $$x(s)={\int }_0^s\cos \left(\frac{c}{2}{t}^2+{\kappa }_{0}t\right)dt,y(s)={\int }_0^s\sin \left(\frac{c}{2}{t}^2+{\kappa }_{0}t\right)dt$$

(3) 工程优化技巧

• 预处理：对原始路径进行等弧长重采样（apollo::common::PathApproximation）。
• 曲率估算：使用五点差分法计算离散曲率：
  $${\kappa }_i\approx \frac{\Delta \theta }{\Delta s}=\frac{({\theta }_{i+2}-{\theta }_{i-2})}{2\cdot (s_{i+2}-s_{i-2})}$$
• 热启动（Warm Start）：用上一帧平滑结果初始化QP变量，加速收敛。

---

3. Control模块中的曲率跟踪

(1) 输入与输出

• 输入：规划模块提供的平滑路径（含曲率 $(\kappa )$ 和速度 $(v)$）。
• 输出：方向盘转角 $(\delta )$ 和加速度指令。

(2) 模型预测控制（MPC）中的曲率处理

• 车辆模型：使用自行车模型（Bicycle Model）关联曲率与转向角：
  $$\kappa =\frac{\tan \delta }{L}(L\text{为轴距})$$
• MPC代价函数：惩罚曲率跟踪误差：
  $$J=\sum _{k=0}^N{\left({\kappa }_{\text{ref},k}-{\kappa }_{\text{pred},k}\right)}^2$$
• $({\kappa }_{\text{ref},k})$：参考路径在第$(k)$步的曲率。
• $({\kappa }_{\text{pred},k})$：模型预测的曲率。

(3) 前馈-反馈控制（Feedforward + Feedback）

• 前馈控制：根据曲率直接计算理论转向角：
  $${\delta }_{\text{ff}}=\arctan (\kappa \cdot L)$$
• 反馈控制：PID或LQR纠正横向误差（如路径偏移量 $(e)$）：
  $${\delta }_{\text{fb}}=K_{p}e+K_d\frac{de}{dt}$$
• 最终转向指令：$(\delta ={\delta }_{\text{ff}}+{\delta }_{\text{fb}})$.

---

4. 实际案例与调参

(1) 曲率突变处理

• 问题场景：路径中存在急转弯（曲率从0.1突变为0.5 m⁻¹）。
• 解决方案：
• 在QP中增加曲率变化率约束：$(\left|\frac{d\kappa }{ds}\right|\le 0.1{\text{ m}}^{-2})$.
• 使用螺旋曲线过渡，确保曲率线性变化。

(2) 速度-曲率协同优化

• 动态调整：根据曲率限制速度，满足横向加速度约束：
  $$v_{\text{max}}=\sqrt{\frac{a_{\text{lat,max}}}{\kappa }},a_{\text{lat,max}}\approx 2.0\text{ m/s²}$$

---

5. 与其他模块的交互

• 与Routing模块：全局路径的粗略曲率用于早期速度规划。
• 与Perception模块：障碍物避让时，局部路径曲率需动态调整（如绕行障碍物后重新平滑）。

---

总结

Apollo中的曲率平滑通过优化算法（QP）和几何曲线（螺旋曲线）在Planning模块生成动力学可行的路径，再通过Control模块的MPC和前馈控制实现精准跟踪。其核心思想是：

1. 规划层：以数学优化保证曲率连续性和舒适性；
2. 控制层：结合模型动力学实现物理可执行性。
   这种分层处理显著提升了自动驾驶的平顺性和安全性。