📚 课程目录第2章拖拉机自动导航横向偏差计算与修正

2. 拖拉机自动导航横向偏差计算与修正

任务拆解：从GPS位姿到转向角指令

拖拉机自动导航的横向控制本质是一个“几何误差 → 控制量”的映射问题。本节以一辆装备双天线RTK-GNSS的拖拉机为对象，预设一条直线作业路径，完成三件事：由定位数据算出横向偏差与航向偏差、用纯追踪（Pure Pursuit）思想设计修正控制器、输出前轮转向角指令并给出可验证的判读方法。

先明确坐标与符号约定，全节保持一致：以目标路径为参考，车辆位于路径右侧时横向偏差 e_y > 0，位于左侧时 e_y < 0；航向偏差 e_ψ = ψ_vehicle − ψ_path，车头偏向路径右侧为正。转向角 δ 以左转为正。这样“偏差为正 → 需要向左修正 → δ 为正”形成同号关系，便于检查符号错误。

横向偏差与航向偏差的计算

设目标路径由起点 A(x_a, y_a) 与终点 B(x_b, y_b) 定义（UTM 平面坐标，单位 m），车辆定位点为 P(x, y)，车辆航向由双天线基线解算得到 ψ_v（弧度，正北为 0，顺时针为正）。

路径单位方向向量为 u = (B − A)/|B − A|，路径法向量取左法向 n = (−u_y, u_x)。则横向偏差为车辆相对路径的有符号距离：

e_y = n · (P − A) = −u_y·(x − x_a) + u_x·(y − y_a)

按此定义，车辆位于路径左侧时 e_y > 0。为与本节“偏右为正”的约定一致，实际代码中取 e_y = −(n · (P − A))，即偏右为正。航向偏差为 e_ψ = wrap(ψ_v − ψ_path)，其中 ψ_path = atan2(u_x, u_y)，wrap 将角度归一化到 (−π, π]，避免 ±180° 跳变导致控制器输出突变。

常见错误：直接用 atan2 求航向差而不做角度归一化，当车辆航向从 179° 跨到 −179° 时，偏差会瞬间跳变约 358°，转向指令剧烈摆动。务必先 wrap 再送入控制器。

纯追踪修正控制器实现

控制器采用纯追踪法：在目标路径上取距车辆前视距离 L_d 的前视点，用车辆到前视点的几何关系求期望曲率，再换算为前轮转向角。前视点取路径上沿行驶方向、距车辆投影点 L_d 处的点；对直线路径可直接由投影点沿 u 前移得到。

期望曲率 κ = 2·sin(α)/L_d，其中 α 为车辆航向与前视点连线的夹角。对直线路径，α 可由 e_y 与 e_ψ 合成：α = atan2(e_y, L_d) − e_ψ。最终转向角 δ = arctan(κ·L)，L 为轴距。为抑制抖动，对 δ 做限幅（示例 ±35°）与一阶低通。

import math

WHEELBASE = 2.6          # 轴距 m
LOOKAHEAD = 6.0          # 前视距离 m
MAX_STEER = math.radians(35.0)
STEER_ALPHA = 0.35       # 一阶低通系数

def wrap_pi(a):
    while a > math.pi:  a -= 2*math.pi
    while a <= -math.pi: a += 2*math.pi
    return a

def lateral_error(px, py, ax, ay, bx, by):
    dx, dy = bx - ax, by - ay
    norm = math.hypot(dx, dy)
    ux, uy = dx/norm, dy/norm
    # 左法向点积，取负使“偏右为正”
    return -(-uy*(px-ax) + ux*(py-ay))

def heading_error(psi_v, ax, ay, bx, by):
    psi_path = math.atan2(bx-ax, by-ay)
    return wrap_pi(psi_v - psi_path)

def pure_pursuit_steer(e_y, e_psi, ld=LOOKAHEAD, L=WHEELBASE):
    alpha = math.atan2(e_y, ld) - e_psi
    kappa = 2.0 * math.sin(alpha) / ld
    delta = math.atan(kappa * L)
    return max(-MAX_STEER, min(MAX_STEER, delta))

class SteeringFilter:
    def __init__(self, alpha=STEER_ALPHA):
        self.alpha, self.y = alpha, 0.0
    def update(self, x):
        self.y += self.alpha * (x - self.y)
        return self.y

# 示例：路径 A(0,0)→B(100,0)，车在 (10, 0.8)，航向偏右 5°
e_y = lateral_error(10.0, 0.8, 0.0, 0.0, 100.0, 0.0)   # ≈ 0.8
e_psi = heading_error(math.radians(5.0), 0.0, 0.0, 100.0, 0.0)
delta = pure_pursuit_steer(e_y, e_psi)
print(round(e_y,3), round(math.degrees(e_psi),2), round(math.degrees(delta),2))

按上述示例参数，e_y ≈ 0.8 m、e_ψ = 5°，α = atan2(0.8, 6.0) − 5° ≈ 7.59° − 5° = 2.59°，κ ≈ 2·sin(2.59°)/6 ≈ 0.0151 1/m，δ ≈ arctan(0.0151×2.6) ≈ 2.25°。结果为正值，表示前轮左转，与“车偏右需左修”的约定一致——这是检查符号链是否正确的快速手段。

闭环验证与结果判读

把控制器接入 10 Hz 的定位回调，用转向执行器反馈的实际前轮角构成内环，形成“横向偏差外环 + 转向角内环”的双环结构。验证时不必依赖实车，可先用运动学模型做离线闭环仿真：ẋ = v·sin(ψ)、ẏ = v·cos(ψ)、ψ̇ = v·tan(δ)/L，以 0.1 s 步长积分，初始偏差设为 e_y = 0.8 m、e_ψ = 5°，车速 1.5 m/s。

观察量	期望现象	判读结论
e_y 收敛方向	由 0.8 m 单调减小并稳定在 ±0.05 m 内	符号与增益正确
e_ψ 过零	先减小、轻微反向后再回零	存在合理阻尼，无持续振荡
δ 幅值	初始约 2°~3°，随后回落至 0 附近	未触发限幅，前视距离合适
e_y 持续增大	偏差发散	符号取反错误或 L_d 过大
δ 高频抖动	转向指令锯齿状	未做角度 wrap 或低通不足

调参顺序建议：先固定 L_d = 6 m 验证符号与收敛性，再缩短 L_d 提升跟踪精度（过小会引起振荡），最后调低通系数 STEER_ALPHA 抑制执行器抖动。每次只改一个参数，并记录 e_y 的峰值与稳态误差。

上线前还需确认两个工程细节：一是 GPS 天线安装点与车辆后轴中心的杆臂偏移，需用 P_rear = P_antenna − R(ψ_v)·lever 换算，否则转弯时横向偏差会出现与曲率相关的系统性偏置；二是定位数据的时间戳要与控制周期对齐，延迟超过 200 ms 时应在控制器中做航向与位置的短时预测，否则高速作业下会出现“追着旧偏差修”的滞后振荡。