横向控制：模型、算法与调参

横向控制目标是让车辆稳定跟踪参考路径并维持姿态稳定，直接影响行车安全与乘坐舒适性。

---

1. 车辆运动学与动力学模型

1.1 运动学自行车模型

简化假设：忽略轮胎侧偏角，前轴为转向轴，后轴固定。状态方程：

\[\dot{x} = v \cos\psi\]

\[\dot{y} = v \sin\psi\]

\[\dot{\psi} = \frac{v}{L} \tan\delta\]

其中 \(L\) 为轴距，\(\delta\) 为前轮转角，\(\psi\) 为航向角。

路径跟踪误差模型（Frenet 坐标系）：

\[\dot{e}_y = v \sin(e_\psi) \approx v \cdot e_\psi \quad (\text{小角度近似})\]

\[\dot{e}_\psi = \frac{v}{L}\tan\delta - \kappa v\]

其中 \(e_y\) 为横向位置误差，\(e_\psi\) 为航向误差，\(\kappa\) 为参考路径曲率。

1.2 动力学自行车模型

考虑轮胎侧偏力，适用于中高速场景：

\[m\dot{v}_y = F_{yf} + F_{yr} - mv_x \dot{\psi}\]

\[I_z \ddot{\psi} = l_f F_{yf} - l_r F_{yr}\]

轮胎侧偏力线性化：\(F_{yf} = C_{\alpha f} \alpha_f\)，\(F_{yr} = C_{\alpha r} \alpha_r\)

其中 \(C_{\alpha f/r}\) 为前/后轮侧偏刚度，\(\alpha\) 为轮胎侧偏角。

模型选择原则

低速（< 30 km/h）泊车场景：运动学模型足够；高速（> 80 km/h）或需精细控制：使用动力学模型。

---

2. 误差定义

常用误差状态：

误差量	符号	说明
横向位置误差	\(e_y\)	车辆重心到参考线的垂直距离
航向误差	\(e_\psi\)	车辆航向与参考路径切线方向之差
位置误差变化率	\(\dot{e}_y\)	横向误差的时间导数
航向误差变化率	\(\dot{e}_\psi\)	航向误差的时间导数

工程注意

误差定义应与规划轨迹坐标系保持一致，避免坐标转换引入抖动。在曲率较大的路段，需用弧长参数化而非欧氏距离计算投影点。

---

3. 经典横向控制算法

3.1 Pure Pursuit（纯追踪）

算法思路：找到参考路径上距前视距离 \(l_d\) 的预瞄点，计算转角：

\[\delta = \arctan\left(\frac{2L\sin\alpha}{l_d}\right)\]

其中 \(\alpha\) 为预瞄点相对车辆的方位角，\(L\) 为轴距，\(l_d\) 为前视距离。

前视距离自适应：

\[l_d = k \cdot v + l_{d,\min}\]

• 车速越高，前视距离越大，避免高速抖动
• \(l_{d,\min}\) 保证低速时仍有最小前视

优缺点：

优点	缺点
实现简单、鲁棒性强	高速场景欠稳，曲率大时超调
对噪声不敏感	仅消除横向偏差，不直接控制航向

3.2 Stanley 控制器

Stanley 控制律同时考虑横向误差和航向误差：

\[\delta = e_\psi + \arctan\left(\frac{k \cdot e_y}{v + \epsilon}\right)\]

其中 \(k\) 为增益参数，\(\epsilon\) 为防止低速除零的小正数。

特点：

• 前轴参考点，低速效果好
• 参数对噪声敏感，高速需要附加滤波
• \(k\) 值过大会导致转向振荡

3.3 LQR（线性二次调节器）

设计流程：

1. 线性化车辆模型（运动学或动力学）
2. 定义状态向量：\(\mathbf{x} = [e_y, \dot{e}_y, e_\psi, \dot{e}_\psi]^T\)
3. 定义代价函数：

\[J = \int_0^\infty \left(\mathbf{x}^T Q \mathbf{x} + \delta^T R \delta \right) dt\]

1. 求解代数 Riccati 方程（ARE）得到最优增益矩阵 \(K\)：

\[\mathbf{u} = -K\mathbf{x}\]

增益矩阵（示例）：

\[K = [k_{e_y}, k_{\dot{e}_y}, k_{e_\psi}, k_{\dot{e}_\psi}]\]

Q/R 矩阵调参含义：

参数	增大效果	减小效果
\(Q_{e_y}\)（横向误差权重）	更快收敛，可能抖动	响应慢
\(Q_{e_\psi}\)（航向误差权重）	更快修正航向	航向误差容忍度高
\(R_\delta\)（转角惩罚权重）	转向更平缓	响应更快但可能激进

Gain Scheduling（增益调度）：

不同车速下需使用不同增益，避免高速增益不足或低速增益过大：

# 伪代码：按车速插值增益
def get_lqr_gain(v):
    if v < 20:
        return K_low   # 低速增益
    elif v < 60:
        return interpolate(K_low, K_mid, v, 20, 60)
    else:
        return K_high  # 高速增益

3.4 MPC（模型预测控制）

MPC 在有限时域内在线求解最优控制序列，每步仅执行第一个控制量。

预测模型（离散化，步长 \(\Delta t\)）：

\[\mathbf{x}_{k+1} = A\mathbf{x}_k + B\mathbf{u}_k\]

代价函数：

\[J = \sum_{k=0}^{N-1} \left( \mathbf{x}_k^T Q \mathbf{x}_k + u_k^T R u_k \right) + \mathbf{x}_N^T P_f \mathbf{x}_N\]

约束：

\[|\delta| \leq \delta_{\max}, \quad |\dot{\delta}| \leq \dot{\delta}_{\max}, \quad |a_y| \leq a_{y,\max}\]

MPC vs LQR：

维度	LQR	MPC
约束处理	难（需后处理限幅）	直接建模
算力需求	低（离线求解 \(K\)）	高（在线 QP/NLP）
性能上限	较低（线性假设）	高（可非线性）
工程难度	低	高（调参/求解器选型）

---

4. 执行器延迟补偿

控制指令下发后，转向执行器存在 50–200 ms 的延迟。不补偿会导致高速场景稳定性下降。

方法一：状态预测

将延迟期间的状态向前预测后再进行控制计算：

\[\mathbf{x}_{\text{predict}} = f(\mathbf{x}_{\text{current}}, u_{\text{prev}}, \Delta t_{\text{delay}})\]

方法二：Smith 预估器

在控制回路中引入延迟模型，提前补偿延迟影响。

方法三：MPC 中建模延迟

在 MPC 预测模型中将延迟步数作为额外状态处理：

\[u_k^{\text{actual}} = u_{k-d}^{\text{cmd}} \quad (d \text{ 为延迟步数})\]

---

5. 常见问题诊断

现象	可能原因	排查方向
直道持续抖动	增益过高或定位噪声过大	降低 \(Q_{e_y}\)，增加定位输出滤波
弯道切角（内侧偏离）	预瞄距离过大或曲率前馈不足	减小 \(l_d\)，增加前馈曲率补偿
高速横摆发散	动力学参数偏差或 gain scheduling 不当	检查轮胎侧偏刚度估计，重新设计高速增益
低速跟踪迟缓	增益过低或执行器死区未补偿	提高低速增益，添加死区前馈
路口横向误差大	轨迹曲率变化快，前馈项不足	增加曲率前馈，减小预瞄距离

---

6. 调参流程建议

1. 先跑直道验证定位质量：分析 \(e_y\) 和 \(e_\psi\) 的噪声水平
2. 低速弯道验证基础跟踪：从 Pure Pursuit 开始，确认基本跟踪可行
3. 引入 LQR/MPC，先保稳定：从保守增益开始，逐步提高
4. 分速度段回归：低速/城区/高速分别测试，建立 Gain Schedule
5. 压力测试：急切入、大曲率弯道、颠簸路面验证鲁棒性

---

7. 评估指标

指标	说明	参考目标
RMS 横向误差	整段轨迹误差均方根	< 10 cm（城区）
95 分位横向误差	长尾误差控制	< 20 cm
RMS 航向误差	航向跟踪精度	< 1°（城区）
峰值横向加速度	乘坐舒适性	< 3 m/s²
峰值横向 jerk	舒适性细指标	< 5 m/s³
转向动作频率	方向盘抖动程度	与主观评价结合