4、横向误差定义：横向偏移误差 e_y 与航向角误差 e_psi 的数学表达
好，咱们进入正题。做车道保持，你首先得知道「车偏了没」。
说白了，横向控制的核心就两个误差：横向偏移误差 e_y 和 航向角误差 e_psi。这两个量搞不清楚，后面 PID 还是 LQR 都白搭。我刚开始做实车调试时，就吃过这个亏——传感器数据明明看着没问题，车却一直画龙。后来才发现，是误差定义的方向搞反了。
4.1 横向偏移误差 e_y
先看第一个：e_y，代表车辆当前位置与期望路径之间的垂直距离。
数学上怎么定义？
假设我们有一条期望路径，路径上离车辆最近的点叫 目标点（或者叫投影点）。从车辆质心到这个目标点做一条垂线，这条垂线的长度就是 e_y 的大小。
关键点：e_y 是有正负的。我习惯定义：
- 车辆在路径左侧 → e_y > 0
- 车辆在路径右侧 → e_y < 0
当然，也有人反过来定义。这无所谓，但整个系统必须统一。我曾经见过一个团队，代码里用正，仿真模型用负，结果调了三天参数车就是不走直线。
公式表达很简单：
e_y = (y_vehicle - y_reference) * cos(psi_ref) - (x_vehicle - x_reference) * sin(psi_ref)嗯，看着有点复杂？其实拆开看就明白了。它本质上是在做坐标系变换——把全局坐标系下的位置差，投影到路径的切向和法向上。你想想看，如果路径是直线，公式就简化成：
e_y = y_vehicle - y_reference是不是清爽多了？
4.2 航向角误差 e_psi
第二个误差：e_psi，代表车辆当前航向与期望路径方向之间的角度差。
这个更直观。车辆有个朝向角 ψ，路径在目标点也有个切线方向角 ψ_ref。两者一减：
e_psi = ψ - ψ_ref但这里有个坑——角度周期性。ψ 和 ψ_ref 都是 [-π, π] 或者 [0, 2π) 的范围。直接相减可能得到 350° 这种离谱值。所以实际代码里一定要做角度归一化：
def normalize_angle(angle):
while angle > np.pi:
angle -= 2 * np.pi
while angle < -np.pi:
angle += 2 * np.pi
return angle
e_psi = normalize_angle(psi_vehicle - psi_ref)注意：不做角度归一化，你的控制器会「抽风」。我踩过这个坑——仿真时路径曲率小，没发现问题。一上实车，遇到急弯，e_psi 直接跳变，方向盘猛打，差点把测试工程师吓出心脏病。
4.3 两个误差的关系
你可能会问：e_y 和 e_psi 是独立的吗？
不是。它们通过车辆运动学耦合在一起。简单说：
- e_psi 不为零 → 车辆会逐渐偏离路径 → e_y 增大
- e_y 不为零 → 控制器需要调整航向 → e_psi 变化
我习惯用一个表格来总结它们的特性：
| 误差项 | 物理含义 | 单位 | 理想值 |
|---|---|---|---|
| e_y | 车辆到路径的垂直距离 | 米 (m) | 0 |
| e_psi | 车辆航向与路径切线的夹角 | 弧度 (rad) | 0 |
个人经验：做 LQR 时，状态向量通常取 [e_y, e_psi, e_y_dot, e_psi_dot]。但如果你只做 PID，一般只用 e_y 和 e_psi 就够了。微分项可以通过滤波后的差分来近似。
4.4 实际代码中的实现
最后，给一段我常用的 Python 代码片段。它从车辆状态和参考路径点中提取 e_y 和 e_psi：
def compute_lateral_errors(vehicle_state, ref_point):
"""
vehicle_state: [x, y, psi, v]
ref_point: [x_ref, y_ref, psi_ref]
"""
x, y, psi = vehicle_state[0], vehicle_state[1], vehicle_state[2]
x_ref, y_ref, psi_ref = ref_point
# 横向偏移误差
dx = x - x_ref
dy = y - y_ref
e_y = dy * np.cos(psi_ref) - dx * np.sin(psi_ref)
# 航向角误差（带归一化）
e_psi = psi - psi_ref
e_psi = np.arctan2(np.sin(e_psi), np.cos(e_psi))
return e_y, e_psi这段代码我用了很多年，简单可靠。注意那个 np.arctan2 的用法——它比手动 while 循环更优雅，而且不会出现除零错误。
好了，e_y 和 e_psi 的定义就讲到这里。下一章我们会把它们塞进控制器里，看看 PID 和 LQR 分别怎么处理这两个误差。到时候你会发现——定义清楚误差，控制就成功了一半。