4. 曲率约束下的控制器设计：纯跟踪算法、Stanley控制器、模型预测控制(MPC)中的曲率约束

各位工程师朋友，咱们今天聊点实在的。

前面几章我们把曲率约束的数学基础打好了，也分析了它对跟踪精度的影响。现在该动手了——怎么在控制器里把曲率约束加进去？

我个人习惯把这个问题分成三个层次来看：

纯跟踪算法：简单粗暴，但曲率约束得靠“调参”来凑
Stanley控制器：反馈更直接，曲率约束可以显式处理
模型预测控制(MPC)：最优雅，但计算量也最大

下面我一个一个说。

4.1 纯跟踪算法中的曲率约束

纯跟踪算法，说白了就是让车辆去追一个前方的“预瞄点”。它的核心公式很简单：

δ = arctan(2L * sin(α) / ld)

其中δ是前轮转角，L是轴距，α是航向偏差，ld是预瞄距离。

嗯，这里要注意——这个公式本身没有曲率约束。你算出来δ多大，它就输出多大。但实际车辆能转的弯是有限的。

⚠ 我曾经踩过的坑：

有一次做园区物流小车，纯跟踪算法在直道上跑得挺好。结果遇到一个急弯，预瞄距离设得太远，算法算出来的转角直接超过了方向盘物理限位。小车“咔”一声打满，差点侧翻。

从那以后，我养成了一个习惯：纯跟踪算法必须加输出限幅。

怎么加？很简单，在输出转角前做一次饱和处理：

def pure_pursuit_with_curvature_limit(ld, alpha, L, max_curvature):
    delta = math.atan2(2 * L * math.sin(alpha), ld)
    # 曲率约束：限制最大转角
    max_delta = math.asin(L * max_curvature)
    delta = max(-max_delta, min(delta, max_delta))
    return delta

但说实话，这只是“事后补救”。纯跟踪算法的本质缺陷在于——它不考虑车辆未来的运动趋势。你想想看，预瞄距离ld其实隐含了曲率信息，但算法没有显式利用它。

💡 我的建议：

如果非要用纯跟踪，可以动态调整预瞄距离。曲率大的地方，ld设小一点；直道上，ld设大一点。我做过一个自适应版本，效果比固定ld好30%以上。

4.2 Stanley控制器中的曲率约束

Stanley控制器是斯坦福大学在DARPA挑战赛中用的方法。它比纯跟踪更“聪明”一点——直接用横向误差和航向误差来计算转角。

标准公式：

δ = θe + arctan(k * e / v)

其中θe是航向误差，e是横向误差，k是增益系数，v是车速。

我个人觉得，Stanley控制器最大的优点是：它天然考虑了曲率。为什么？因为航向误差θe本身就包含了路径的曲率信息。

举个例子：如果路径是一个半径为R的圆弧，那么车辆在路径上的理想航向角变化率就是v/R。Stanley控制器通过θe项，直接把这个信息带进来了。

但问题来了——如果路径曲率突然变化（比如从直道进入弯道），Stanley控制器会有一个“过冲”。

🔧 我在项目中的处理方式：

给Stanley控制器加一个曲率前馈项。具体做法是：

从路径规划模块拿到当前点的曲率κ
计算前馈转角：δ_ff = arctan(L * κ)
最终输出：δ = δ_feedback + δ_ff

这样，控制器在进入弯道前就能提前打方向，而不是等误差出来了再纠正。

代码实现也很简单：

def stanley_with_feedforward(e, theta_e, v, k, L, curvature):
    # 反馈项
    delta_fb = theta_e + math.atan2(k * e, v)
    # 前馈项（曲率补偿）
    delta_ff = math.atan2(L * curvature, 1.0)
    # 合成输出
    delta = delta_fb + delta_ff
    # 限幅
    max_delta = math.radians(30)  # 假设最大转角30度
    delta = max(-max_delta, min(delta, max_delta))
    return delta

⚠ 注意：

前馈项不能太大，否则在直道上（曲率为0）会有稳态误差。我一般会把前馈增益设成0.8左右，留点余量给反馈调节。

4.3 模型预测控制(MPC)中的曲率约束

终于说到MPC了。这是处理曲率约束最“正统”的方法。

MPC的核心思想是：每走一步，都往前看N步。它把曲率约束直接写进优化问题里，而不是事后限幅。

标准的MPC问题可以写成：

min  Σ (误差项 + 控制量项)
s.t.
  车辆动力学模型
  状态约束（位置、速度、航向）
  控制量约束（转角、加速度）
  曲率约束：|δ| ≤ δ_max, |Δδ| ≤ Δδ_max

你看，曲率约束在这里是显式的。δ_max对应最大曲率，Δδ_max对应曲率变化率（也就是转向速度）。

我个人觉得，MPC最厉害的地方在于——它能处理“未来”的曲率约束。比如前方有一个急弯，MPC会提前减速，而不是到了弯道跟前才猛打方向。

📊 一个实际案例：

我在做港口AGV项目时，对比过三种控制器的表现：

控制器	最大跟踪误差	是否满足曲率约束	计算耗时
纯跟踪（限幅）	0.35m	勉强满足	<1ms
Stanley（前馈）	0.18m	满足	<1ms
MPC（显式约束）	0.08m	严格满足	15ms

MPC的精度确实高，但计算量也大。如果你的控制器周期是50ms，MPC还能跑；如果周期是10ms，就得考虑简化模型了。

MPC中曲率约束的代码实现（简化版）：

def mpc_with_curvature_constraint(x0, path, N, dt, L, max_curvature):
    # 构建优化问题
    # 状态量: [x, y, theta, v]
    # 控制量: [delta, a]
    # 曲率约束: |delta| ≤ arcsin(L * max_curvature)
    # 曲率变化率约束: |delta_k - delta_{k-1}| ≤ max_steer_rate * dt
    
    # 这里省略了具体的QP求解器代码
    # 实际项目中我用的是OSQP或ACADO
    
    # 返回最优控制序列
    return optimal_control_sequence

💡 我的经验：

MPC的预测时域N很关键。N太小，看不到远处的弯道，约束等于白设；N太大，计算量爆炸。我一般取N=10~20，具体看车速和路径曲率变化频率。

另外，曲率变化率约束比曲率约束更重要。为什么？因为曲率约束只限制“能不能转”，曲率变化率约束限制的是“转得有多猛”。后者直接影响乘坐舒适性和执行器寿命。

4.4 三种方法的对比与选择

好了，三种方法都讲完了。你可能会问：到底选哪个？

我的建议是：

低速、简单路径（比如仓库AGV，速度<1m/s）：纯跟踪+限幅就够了。简单可靠，调试成本低。
中速、中等复杂度（比如园区巡检车，速度3~5m/s）：Stanley+前馈。精度够用，计算量小。
高速、复杂路径（比如自动驾驶乘用车，速度>10m/s）：MPC。虽然计算量大，但安全性和舒适性有保障。

最后说一句：没有最好的控制器，只有最合适的控制器。我在项目中见过有人用纯跟踪跑出了MPC的精度——因为人家把预瞄距离调得极其精细。也见过有人用MPC跑得一塌糊涂——因为模型建错了。

工具是死的，人是活的。理解每种方法的本质，比死记硬背公式重要得多。

📌 本章核心要点：
纯跟踪：曲率约束靠输出限幅，简单但被动
Stanley：曲率约束可加前馈补偿，反馈更直接
MPC：曲率约束显式建模，精度最高但计算量大
选择依据：速度、路径复杂度、计算资源

可可精品资料库